Trajectory Components with ToolNone

[Artificys]

So, as far as I can tell, JointTrajectoryAction will always throw an error if there is no tool on the Stretch. When trying to execute the callback, on line 285 it'll clear each joint's trajectory manager. However, when it gets to the GripperComponent, since there is no 'device' named 'stretch_gripper' on the robot, it returns a NoneType with no trajectory_manager and will fail the trajectory.

I don't have the error anymore because I fixed it, but if needed I can undo it and get the error again.

My fix for it was to add another check in the trajectory_components.py:

def get_trajectory_components(robot):
    robot_status = robot.get_status()
    if 'wrist_pitch' in robot_status['end_of_arm']:
        return {component.name: component for component in [HeadPanComponent(robot),
                                                            HeadTiltComponent(robot),
                                                            WristYawComponent(robot),
                                                            WristPitchComponent(robot),
                                                            WristRollComponent(robot),
                                                            GripperComponent(robot),
                                                            ArmComponent(robot),
                                                            LiftComponent(robot),
                                                            BaseComponent(robot),
                                                            ]}
    elif 'stretch_gripper' in robot_status['end_of_arm']:
        return {component.name: component for component in [HeadPanComponent(robot),
                                                            HeadTiltComponent(robot),
                                                            WristYawComponent(robot),
                                                            GripperComponent(robot),
                                                            ArmComponent(robot),
                                                            LiftComponent(robot),
                                                            BaseComponent(robot),
                                                            ]}
    else:
        return {component.name: component for component in [HeadPanComponent(robot),
                                                            HeadTiltComponent(robot),
                                                            WristYawComponent(robot),
                                                            ArmComponent(robot),
                                                            LiftComponent(robot),
                                                            BaseComponent(robot),
                                                            ]}

Not sure if this is the correct way of doing this, but for now this fixes the error and lets the trajectory continue.

[Artificys]

This was also found while following the Align To Aruco example - which I know isn't in the new V0.3 docs, but the tutorial code is still there. Not sure if that matters, but worth noting.