Commanding trajectory cancellation results in wrist pitch drop

[hello-amal]

Description

When we command the joint trajectory server to cancel a trajectory, it sometimes drops the wrist pitch. This issue is particularly exacerbated when trajectories are cancelled several times in quick succession (e.g., operators are clicking a button rapidly).

Steps to re-create the issue

1. Copy the below code into stretch_ros2/stretch_core/stretch_core, add it to the setup.py, and re-compile the package.
2. Run the stretch drivers: ros2 launch stretch_core stretch_driver.launch.py
3. Run the node. Note this requires display access (e.g., Moonlight; X-11 forwarding doesn't work). ros2 run stretch_core keyboard_button_event_teleop
4. Press any key to see the key mapping. Then, rapidly click the keys corresponding to some direction (see notes below).
   1. For wrist pitch up (w), yaw in both directions (a and d), and roll in both directions (q and e), I have reliably seen pitch dropping.
   2. I have sometimes seen it for arm lift (8 and 2) as well, but less reliably.

Note that the issue goes away if you comment out the line trajectory_cancel_service.call(cancel_msg), which indicates that it is a bug with how the joint trajectory server processes cancellations. Perhaps related to #139 , another bug with joint trajectory server cancellations?

keyboard_button_event_teleop.py

#! /usr/bin/env python3

import rclpy
from sensor_msgs.msg import JointState
from rclpy.action import ActionClient
from control_msgs.action import FollowJointTrajectory
from geometry_msgs.msg import Transform
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
from pynput import keyboard
from rclpy.duration import Duration
from rclpy.executors import SingleThreadedExecutor
from action_msgs.msg import GoalStatus
from action_msgs.srv import CancelGoal
from unique_identifier_msgs.msg import UUID

# Declare the global variables
joint_states = None
node = None
trajectory_client = None
trajectory_cancel_service = None
cancel_msg = CancelGoal.Request()
cancel_msg.goal_info.goal_id = UUID(uuid=[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0])
cancel_msg.goal_info.stamp.nanosec = 0
cancel_msg.goal_info.stamp.sec = 0

class KeyPressHandler:
    def __init__(self):
        self.key_methods = {}
        self.listener = keyboard.Listener(on_press=self.on_key_press)
        

    def register_key(self, key, method):
        """Register a key with a method to be executed on key press."""
        self.key_methods[key] = method

    def on_key_press(self, key):
        """Internal method to handle key press events."""
        global node
        try:
            if key in self.key_methods:
                self.key_methods[key]()
            elif key.char in self.key_methods:
                self.key_methods[key.char]()
        except AttributeError as e:
            # Handle special keys (like Ctrl, Alt, etc.)
            if node is not None:
                node.get_logger().info(f"{e}")
            pass
        self.pretty_print_key_mapping()

    def pretty_print_key_mapping(self):
        """Print the key mapping in a pretty format."""
        print("\n" + "-" * 20)
        print("Key mappings:")
        for key, method in self.key_methods.items():
            print(f"Key: {key} -> Method: {method.__name__}")
        print("\n" + "-" * 20)

    def start_listening(self):
        """Start the keyboard listener."""
        self.listener.start()

    def stop_listening(self):
        """Stop the keyboard listener."""
        self.listener.stop()

def joint_states_callback(msg):
    global joint_states
    joint_states = msg
    # print(joint_states)

def move_by(joint_name, increment):
    # move a joint by an increment
    global joint_states, node, trajectory_client, trajectory_cancel_service, cancel_msg
    trajectory_cancel_service.call(cancel_msg)
    joint_positions = joint_states.position
    joint_names = joint_states.name
    joint_idx = joint_names.index(joint_name)
    joint_positions[joint_idx] += increment
    joint_names = [joint_name]
    
    point1 = JointTrajectoryPoint()
    trajectory_goal = FollowJointTrajectory.Goal()
    trajectory_goal.goal_time_tolerance = Duration(seconds=1.0).to_msg()
    trajectory_goal.trajectory.joint_names = joint_names

    point1.time_from_start = Duration(seconds=10).to_msg()
    point1.positions = [joint_positions[joint_idx]]
    trajectory_goal.trajectory.points = [point1]
    trajectory_client.send_goal_async(trajectory_goal)
    print(f"Moving {joint_name} by {increment}")

def move_pitch_up():
    move_by('joint_wrist_pitch', 0.15)

def move_pitch_down():
    move_by('joint_wrist_pitch', -0.15)

def move_yaw_left():
    move_by('joint_wrist_yaw', 0.15)

def move_yaw_right():
    move_by('joint_wrist_yaw', -0.15)

def move_roll_left():
    move_by('joint_wrist_roll', 0.15)

def move_roll_right():
    move_by('joint_wrist_roll', -0.15)

def move_lift_up():
    move_by('joint_lift', 0.02)

def move_lift_down():
    move_by('joint_lift', -0.02)

def move_arm_out():
    move_by('wrist_extension', 0.1)

def move_arm_in():
    move_by('wrist_extension', -0.1)

def move_gripper_open():
    move_by('joint_gripper_finger_left', 0.1)
    move_by('joint_gripper_finger_right', 0.1)

def move_gripper_close():
    move_by('joint_gripper_finger_left', -0.1)
    move_by('joint_gripper_finger_right', -0.1)

def main():
    global joint_states, node, trajectory_client, trajectory_cancel_service
    rclpy.init()
    node = rclpy.create_node('joint_states_subscriber')
    trajectory_client = ActionClient(node, 
                                     FollowJointTrajectory, 
                                     '/stretch_controller/follow_joint_trajectory'
    )
    trajectory_cancel_service = node.create_client(
        CancelGoal, "/stretch_controller/follow_joint_trajectory/_action/cancel_goal"
    )
    node.get_logger().info("Waiting for the follow joint trajectory controller...")
    server_reached = trajectory_client.wait_for_server(timeout_sec=60.0)
    # Create a subscriber to the joint states topic
    subscriber = node.create_subscription(
        JointState,
        '/stretch/joint_states',
        joint_states_callback,
        1  # QoS profile depth
    )
    key_handle = KeyPressHandler()
    key_handle.register_key('w', move_pitch_up)
    key_handle.register_key('s', move_pitch_down)
    key_handle.register_key('a', move_yaw_left)
    key_handle.register_key('d', move_yaw_right)
    key_handle.register_key('8', move_lift_up)
    key_handle.register_key('2', move_lift_down)
    key_handle.register_key('6', move_arm_out)
    key_handle.register_key('4', move_arm_in)
    key_handle.register_key('9', move_gripper_open)
    key_handle.register_key('7', move_gripper_close)
    key_handle.register_key('q', move_roll_left)
    key_handle.register_key('e', move_roll_right)
    key_handle.start_listening()

    # Spin the node to receive messages
    node.get_logger().info("Ready to receive keyboard commands.")
    executor = SingleThreadedExecutor()
    rclpy.spin(node, executor=executor)

    # Clean up
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

[hello-vinitha]

@hello-binit This issue is still occurring in stretch_web_teleop and are still relying on the temp fix where we don't send stop commands.

[hello-binit]

Thanks, is there a branch in Web Teleop where the temp fix is disabled? Or can you should me how to patch the code to reproduce this bug in Web Teleop?

[hello-vinitha]

There is no branch where the temp fix is disabled. You can reproduce the bug by changing false to true in the following two lines:

https://github.com/hello-robot/stretch_web_teleop/blob/master/src/pages/operator/tsx/function_providers/FunctionProvider.tsx#L83

https://github.com/hello-robot/stretch_web_teleop/blob/master/src/pages/robot/tsx/robot.tsx#L1093

Thanks! More details about the temp fix here: hello-robot/stretch_web_teleop#63