Replaying ROS2 bag file on real robot

[ToncyChen]

I have recorded a ROS2 bag file with the Webots https://github.com/MASKOR/webots_ros2_spot using nav2, and I want to replay the bag file on the real robot (BD Spot) using ROS2 interface of your repo. Could you please guide me on how to achieve this?

[khughes-bdai]

Hello,
What information or topics in the bag file do you want to replay on the robot?

[ToncyChen]

Hello,

I remember I recorded the topics: "cmd_vel", "tf", "/Spot/odometry", "/Spot/imu" in my bag file.

[khughes-bdai]

cmd_vel should be relatively straightforward to replay on the robot. When you start the driver, there is a cmd_vel topic, when you publish to it it will move the robot. You may just have to remap the name of the topic to make sure that it matches the name of the topic from the bag.
There is currently not a way to replay tf, odometry, or imu data on the robot from a bag.

[ToncyChen]

Hello,
I want to record another ros2 bag file for the robotic arm with Webots https://github.com/MASKOR/webots_ros2_spot/tree/main?tab=readme-ov-file using Moveit2.
Is it possible for me to also replay this bag file with the real robot (BD Spot) using ROS2 interface of your repo?
If so, which topic should I record with ros2 bag file?

[khughes-bdai]

There is not currently a built in way to do this with the ROS driver. A way to do this could be

1. record joint states topic in your bag file
2. create a node that subscribes to joint states, and use this to construct a robot command protobuf with BD's robot command builder (filling in the arm joint angles)

trajectory_point = RobotCommandBuilder.create_arm_joint_trajectory_point(
            sh0=...,
            sh1=...,
            el0=...,
            el1=...,
            wr0=...,
            wr1=...,
 )
robot_command = robot_command_pb2.RobotCommand()
arm_command = robot_command.synchronized_command.arm_command
arm_command.arm_joint_move_command.trajectory.points.append(trajectory_point)

3. Convert the protobuf to a robot command ROS message using from bosdyn_api_msgs.conversions import convert
4. Send this command to the robot either using the robot_command action or the robot_command service brought up by the ROS driver

[khughes-bdai]

There is a better way to replay Spot's arm joint angles on the robot now -- record the joint states topic, and replay it onto the topic /arm_joint_commands (added in #474) @ToncyChen

[ToncyChen]

Hello,
So, in this case, I don not need to create a node to subscribe joint states and convert the message into ros message, I just have to create another ros2 bag file recording the topic /joint_states and replay onto the topic /arm_joint_command, is that right?

[khughes-bdai]

Yes, this should work! Please let me know if there are any issues using this topic.

[tcappellari-bdai]

Closing this issue as it appears to be resolved. Please re-open if you still need help

[ToncyChen]

Hi @khughes-bdai ,
I have replayed my ros2 bag file with command ros2 bag play traj8 --remap /joint_states:=/arm_joint_commands today. However, the spot robot didn't move its arm.
And there is an error message showed when bag file was playing:

I think the number of joints in topic /joint_states is not match with the number in topic `arm_joint_commands.
Could you please guide me how to solve this problem?

[tcappellari-bdai]

@ToncyChen the joint_states topic includes joints for the whole body, not just the arm, but you are trying to remap it onto a topic that can only take the 6 arm joints

[ToncyChen]

Hi @khughes-bdai ,
I have recorded another ros2 bag file with gripper movement using Webots https://github.com/bdaiinstitute/spot_ros2 using Moveit2.
Is it possible for me to also replay same gripper movement as my bag file recorded with the real robot (BD Spot) using ROS2 interface of your repo?
If so, which topic should I replay onto to make the gripper move?

[khughes-bdai]

Hi @ToncyChen , the driver does not have a topic for streaming gripper commands. There is a service for gripper commands that takes the desired gripper angle that you might be able to use -- set_gripper_angle

[ToncyChen]

Hi @khughes-bdai,
Today, I have tried to replay the recorded arm trajectories on the Spot using command ros2 bag play [name_of_file] --remap /joint_states:=/arm_joint_commands with a remapping python file run in another terminal. However, the arm trajectories could not be replayed. This is strange because I have successfully replayed the arm trajectories before. I don't know why this time cannot work.
And, because the number, name, and angle range of joints recorded my simulation environment is different to physical spot. So, I wrote a python file to remap the joint for topic /arm_joint_commands. Is there anything that I should modify in this python code? Could you please guide me how to solve this issue?
Here is the complete remapping node I wrote:

from rclpy.node import Node
from sensor_msgs.msg import JointState
import math

class JointStateRemapper(Node):
    def __init__(self):
        super().__init__('joint_state_remapper')

        self.subscription = self.create_subscription(
            JointState,
            '/joint_states',
            self.joint_states_callback,
            10
        )
        self.publisher_ = self.create_publisher(
            JointState,
            '/arm_joint_commands',
            10
        )

        self.joint_mapping = {
            'spotarm_1_joint': 'sh0',
            'spotarm_4_joint': 'el1',
            'spotarm_5_joint': 'wr0',
            'spotarm_3_joint': 'el0',
            'spotarm_6_joint': 'wr1',
            'spotarm_2_joint': 'sh1'
        }

        initial_positions_degrees = [0, -179, 170, 0, 11, 0]
        initial_positions_radians = [math.radians(deg) for deg in initial_positions_degrees]
        self.joint_command_msg = JointState()
        self.joint_command_msg.name = ['sh0', 'sh1', 'el0', 'el1', 'wr0', 'wr1']
        self.joint_command_msg.position = initial_positions_radians

        self.get_logger().info(f"Initial joint positions set: {self.joint_command_msg.position}")

    def joint_states_callback(self, msg):
        # Create a dictionary for the incoming joint states
        joint_positions = dict(zip(msg.name, msg.position))
        
        # Remap the joints based on the mapping and assign the positions to the correct command order
        for i, command_joint in enumerate(self.joint_command_msg.name):
            if command_joint in self.joint_mapping.values():
                # Find the corresponding joint in /joint_states and map the position
                state_joint = list(self.joint_mapping.keys())[list(self.joint_mapping.values()).index(command_joint)]
                if state_joint in joint_positions:
                    # Handle the special case for "spotarm_2_joint"
                    if state_joint == 'spotarm_2_joint':
                        self.joint_command_msg.position[i] = -joint_positions[state_joint]
                    else:
                        self.joint_command_msg.position[i] = joint_positions[state_joint]

        
        # Publish the remapped joint positions to /arm_joint_commands
        self.publisher_.publish(self.joint_command_msg)

def main(args=None):
    rclpy.init(args=args)
    node = JointStateRemapper()
    
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()```