[Bug]: robot_torque_enable(self, cmd_type: str, name: str, enable: bool) does not appear to return from self.robot_spin_until_future_complete(future_torque_enable)

[tshumay]

What happened?

I noticed that when using some code derived from the interbotix_xsarm_joy joystick example, it seemed that I could torque off the motors, but following that, no further commands were processed by the xsarm_robot node following that. I dug in a bit deeper to see if I could reproduce this using only CLI messages to isolate it, and it turned out that I could.

I reported an issue with the arm.py script a couple weeks back, and I believe that caused some changes to core.py as well, and I think they may be related to the problem here. Sorry for causing trouble! I figured I'd report this one because it may indirectly be as a result of those changes, so hopefully it can be fixed.

Robot Model

Was testing against a custom robot, but reproduced using wx200 in the sim as well.

Operating System

Ubuntu 22.04

ROS Distro

ROS 2 Humble

Steps To Reproduce

1. Launch the joystick launch file (note I am using the sim, but this seems to happen with both the real arm and sim, but using sim to test)

ros2 launch interbotix_xsarm_joy xsarm_joy.launch.py robot_model:=wx200 use_sim:=true

2. Test that things are working by issuing some pose commands (I did home and then sleep to make sure commands were going to the joystick node)

ros2 topic pub /wx200/commands/joy_processed interbotix_xs_msgs/msg/ArmJoy "{pose_cmd: 15}" --once
ros2 topic pub /wx200/commands/joy_processed interbotix_xs_msgs/msg/ArmJoy "{pose_cmd: 16}" --once

3. Use CLI to issue a torque on or off command (I used torque on here)

ros2 topic pub /wx200/commands/joy_processed interbotix_xs_msgs/msg/ArmJoy "{torque_cmd: 23}" --once

4. Following that - the node will be stuck and will no longer respond to commands. For example. trying to issue a sleep command will have no result:

ros2 topic pub /wx200/commands/joy_processed interbotix_xs_msgs/msg/ArmJoy "{pose_cmd: 16}" --once

I tracked this down a bit and it looks like the culprit is this function from core.py (I added log messages before and after the call to the robot_spin_until_future_complete() call). The first message fires, the second does not indicating that it may be stuck waiting for the future?

def robot_torque_enable(self, cmd_type: str, name: str, enable: bool) -> None:
        """
        Torque a single motor or a group of motors to be on or off.

        :param cmd_type: can be "group" for a group of motors or "single" for a single motor
        :param name: group name if `cmd_type` is 'group' or the motor name if `cmd_type` is
            'single'
        :param enable: `True` to torque on or `False` to torque off
        """
        future_torque_enable = self.srv_torque.call_async(
            TorqueEnable.Request(cmd_type=cmd_type, name=name, enable=enable)
        )
        self.get_logger().info("Waiting for future_torque_enable to complete...")
        self.robot_spin_until_future_complete(future_torque_enable)
        self.get_logger().info("future_torque_enable completed!")

Relevant log output

[xsarm_robot.py-6] [INFO] [1699932996.767920819] [wx200.robot_manipulation]: Initialized InterbotixArmXSInterface!
[xsarm_robot.py-6] [INFO] [1699932997.272614087] [wx200.robot_manipulation]: 
[xsarm_robot.py-6] 	Gripper Name: gripper
[xsarm_robot.py-6] 	Gripper Pressure: 50.0%
[xsarm_robot.py-6] [INFO] [1699932997.274050527] [wx200.robot_manipulation]: Initialized InterbotixGripperXSInterface!
[xsarm_robot.py-6] [INFO] [1699932997.791897461] [wx200.robot_manipulation]: Ready to receive processed joystick commands.
[xs_sdk_sim.py-1] [INFO] [1699933183.155520273] [interbotix_xs_sdk.xs_sdk_sim]: The 'arm' group was torqued on.
[xsarm_robot.py-6] [INFO] [1699933183.156182587] [wx200.robot_manipulation]: Waiting for future_torque_enable to complete...

Additional Info

No response