command group set_goal ignores position in velocity mode

hello_helpers/src/hello_helpers/simple_command_group.py

@@ -103,21 +103,28 @@ class SimpleCommandGroup:
         bool
             False if commanded goal invalid, else True
         """
+        robot_mode = kwargs['robot_mode']
         self.goal = {"position": None, "velocity": None, "acceleration": None, "contact_threshold": None}
         if self.active:
             goal_pos = point.positions[self.index] if len(point.positions) > self.index else None
-            if goal_pos is None:
+            if robot_mode == 'position' and goal_pos is None:
                 err_str = ("Received goal point with positions array length={0}. "
                            "This joint ({1})'s index is {2}. Length of array must cover all joints listed "
                            "in commanded_joint_names.").format(len(point.positions), self.name, self.index)
                 invalid_goal_callback(err_str)
                 return False
+            elif robot_mode == 'velocity' and goal_pos is not None:
+                err_str = (f"Received goal point with position for joint {self.name} (index {self.index}) "
+                           "during velocity mode, which is not allowed.")
+                invalid_goal_callback(err_str)
+                return False
 
-            self.goal['position'] = hm.bound_ros_command(self.range, goal_pos, fail_out_of_range_goal)
+            if goal_pos is not None:
+                self.goal['position'] = hm.bound_ros_command(self.range, goal_pos, fail_out_of_range_goal)
             self.goal['velocity'] = point.velocities[self.index] if len(point.velocities) > self.index else None
             self.goal['acceleration'] = point.accelerations[self.index] if len(point.accelerations) > self.index else None
             self.goal['contact_threshold'] = point.effort[self.index] if len(point.effort) > self.index else None
-            if self.goal['position'] is None:
+            if robot_mode == 'position' and self.goal['position'] is None:
                 err_str = ("Received {0} goal point that is out of bounds. "
                             "Range = {1}, but goal point = {2}.").format(self.name, self.range, goal_pos)
                 invalid_goal_callback(err_str)

stretch_core/nodes/joint_trajectory_server.py

@@ -97,7 +97,8 @@ class JointTrajectoryAction:
             rospy.logdebug(("{0} joint_traj action: "
                             "target point #{1} = <{2}>").format(self.node.node_name, pointi, point))
 
-            valid_goals = [c.set_goal(point, self.invalid_goal_callback, self.node.fail_out_of_range_goal)
+            valid_goals = [c.set_goal(point, self.invalid_goal_callback, self.node.fail_out_of_range_goal,
+                           robot_mode=self.node.robot_mode)
                            for c in self.command_groups]
             if not all(valid_goals):
                 # At least one of the goals violated the requirements
@@ -116,7 +117,7 @@ class JointTrajectoryAction:
                 self.node.robot.push_command()
 
             for c in self.command_groups:
-                c.init_execution(self.node.robot, robot_status)
+                c.init_execution(self.node.robot, robot_status, robot_mode=self.node.robot_mode)
 
             #self.node.robot.push_command() #Moved to an asynchronous call in stretch_driver
             self.node.dirty_command=True
@@ -145,7 +146,8 @@ class JointTrajectoryAction:
                         return
 
                 robot_status = self.node.robot.get_status()
-                named_errors = [c.update_execution(robot_status, success_callback=self.success_callback)
+                named_errors = [c.update_execution(robot_status, success_callback=self.success_callback,
+                                robot_mode=self.node.robot_mode)
                                 for c in self.command_groups]
                 # It's not clear how this could ever happen. The
                 # groups in command_groups.py seem to return