Support full goals in HelloNode.move_to_pose

hello_helpers/src/hello_helpers/hello_misc.py

@@ -5,6 +5,7 @@ import os
 import sys
 import glob
 import math
+import numbers
 
 import rospy
 import tf2_ros
@@ -75,34 +76,44 @@ class HelloNode:
     def point_cloud_callback(self, point_cloud):
         self.point_cloud = point_cloud
     
-    def move_to_pose(self, pose, return_before_done=False, custom_contact_thresholds=False):
-        joint_names = [key for key in pose]
+    def move_to_pose(self, pose, return_before_done=False, custom_full_goal=False, custom_contact_thresholds=False):
         point = JointTrajectoryPoint()
         point.time_from_start = rospy.Duration(0.0)
-
         trajectory_goal = FollowJointTrajectoryGoal()
         trajectory_goal.goal_time_tolerance = rospy.Time(1.0)
-        trajectory_goal.trajectory.joint_names = joint_names
-        if not custom_contact_thresholds: 
-            joint_positions = [pose[key] for key in joint_names]
-            point.positions = joint_positions
-            trajectory_goal.trajectory.points = [point]
+        trajectory_goal.trajectory.header.stamp = rospy.Time.now()
+        trajectory_goal.trajectory.joint_names = list(pose.keys())
+        trajectory_goal.trajectory.points = [point]
+
+        # populate goal
+        if custom_full_goal:
+            pose_correct = all([len(joint_goal)==4 for joint_goal in pose.values()])
+            if not pose_correct:
+                rospy.logerr(f"{self.node_name}(HelloNode).move_to_pose: Not sending trajectory due to improper pose. The 'custom_full_goal' option requires tuple with 4 values (pose_target, velocity, acceleration, contact_threshold_effort) for each joint name, but pose = {pose}")
+                return
+            point.positions = [joint_goal[0] for joint_goal in pose.values()]
+            point.velocities = [joint_goal[1] for joint_goal in pose.values()]
+            point.accelerations = [joint_goal[2] for joint_goal in pose.values()]
+            point.effort = [joint_goal[3] for joint_goal in pose.values()]
+        elif custom_contact_thresholds:
+            pose_correct = all([len(joint_goal)==2 for joint_goal in pose.values()])
+            if not pose_correct:
+                rospy.logerr(f"{self.node_name}(HelloNode).move_to_pose: Not sending trajectory due to improper pose. The 'custom_contact_thresholds' option requires tuple with 2 values (pose_target, contact_threshold_effort) for each joint name, but pose = {pose}")
+                return
+            point.positions = [joint_goal[0] for joint_goal in pose.values()]
+            point.effort = [joint_goal[1] for joint_goal in pose.values()]
         else:
-            pose_correct = all([len(pose[key])==2 for key in joint_names])
+            pose_correct = all([isinstance(joint_position, numbers.Real) for joint_position in pose.values()])
             if not pose_correct:
-                rospy.logerr("HelloNode.move_to_pose: Not sending trajectory due to improper pose. custom_contact_thresholds requires 2 values (pose_target, contact_threshold_effort) for each joint name, but pose = {0}".format(pose))
+                rospy.logerr(f"{self.node_name}(HelloNode).move_to_pose: Not sending trajectory due to improper pose. The default option requires 1 value, pose_target as integer, for each joint name, but pose = {pose}")
                 return
-            joint_positions = [pose[key][0] for key in joint_names]
-            joint_efforts = [pose[key][1] for key in joint_names]
-            point.positions = joint_positions
-            point.effort = joint_efforts
-            trajectory_goal.trajectory.points = [point]
-        trajectory_goal.trajectory.header.stamp = rospy.Time.now()
+            point.positions = [joint_position for joint_position in pose.values()]
+
+        # send goal
         self.trajectory_client.send_goal(trajectory_goal)
-        if not return_before_done: 
+        if not return_before_done:
             self.trajectory_client.wait_for_result()
-            #print('Received the following result:')
-            #print(self.trajectory_client.get_result())
+            rospy.logdebug(f"{self.node_name}(HelloNode).move_to_pose: got result {self.trajectory_client.get_result()}")
 
     def get_robot_floor_pose_xya(self, floor_frame='odom'):
         # Returns the current estimated x, y position and angle of the