Mobile Base Command Group exception thrown when commanding base movements with 0 values

[hello-lamsey]

Description

When the stretch_driver is in position mode, the mobile base command group throws an exception after receiving position move commands with values of 0.

@hello-amal may have some insights into why this is happening.

To recreate:

1. In one terminal, run ros2 launch stretch_core stretch_driver.launch.py
2. In a second terminal, first run ros2 service call /switch_to_position_mode std_srvs/srv/Trigger {}
3. In the second terminal, then run ros2 action send_goal /stretch_controller/follow_joint_trajectory control_msgs/action/FollowJointTrajectory "{trajectory: {joint_names: ['rotate_mobile_base'], points: [{positions: [0.0], time_from_start: {sec: 1, nanosec: 0}}]}}"
4. You should see that the goal is rejected, and error output will appear in the terminal running stretch_driver.

Example Terminal Output

[stretch_driver-8] [INFO] [1721923884.306860885] [stretch_driver]: stretch_driver joint_traj action: New trajectory received with joint_names = ['rotate_mobile_base', 'joint_lift']
[stretch_driver-8] [ERROR] [1721923884.314657521] [stretch_driver]: Error raised in execute callback: cannot unpack non-iterable NoneType object
[stretch_driver-8] Traceback (most recent call last):
[stretch_driver-8]   File "/opt/ros/humble/local/lib/python3.10/dist-packages/rclpy/action/server.py", line 332, in _execute_goal
[stretch_driver-8]     execute_result = await await_or_execute(execute_callback, goal_handle)
[stretch_driver-8]   File "/opt/ros/humble/local/lib/python3.10/dist-packages/rclpy/executors.py", line 107, in await_or_execute
[stretch_driver-8]     return callback(*args)
[stretch_driver-8]   File "/home/hello-robot/ament_ws/install/stretch_core/lib/python3.10/site-packages/stretch_core/joint_trajectory_server.py", line 182, in execute_cb
[stretch_driver-8]     c.init_execution(self.node.robot, robot_status)
[stretch_driver-8]   File "/home/hello-robot/ament_ws/install/stretch_core/lib/python3.10/site-packages/stretch_core/command_groups.py", line 649, in init_execution
[stretch_driver-8]     (_, mobile_base_error_m), (_, mobile_base_error_rad) = self.update_execution(robot_status)
[stretch_driver-8] TypeError: cannot unpack non-iterable NoneType object
[stretch_driver-8] [WARN] [1721923884.316403181] [stretch_driver]: Goal state not set, assuming aborted. Goal ID: [197 252 125 113 109 217  77 217 137  54 104  86  12 210  62 127]

[hello-amal]

I have been able to re-create this. I believe it is a race condition to do with the setting of self.active (code here). I believe when init_execution gets called, self.active is True, but by the time it calls update_execution, self.active has been set to False, thus returning a None type.

As a further improvement, given that it is possible for update_execution to return None, all pieces of code that call it should handle that case. mypy can help us identify such cases (e.g., we would label the retval as Optional[None] and it will then preemptively warn us when we are trying to access an attribute of a possibly-None variable).