Spot ROS 2

When migrating from the Spot SDK to ROS 2, direct Protobuf-based API calls are replaced by ROS 2 service calls. The logic involves constructing an InverseKinematicsRequest (using Protobuf structures) and then converting that request into a ROS 2 service request for the GetInverseKinematicSolutions service.

Workflow:

1. Construct the InverseKinematicsRequest using Protobuf messages (e.g., scene_tform_task, wrist_mounted_tool, and tool_pose_task).
2. Create a GetInverseKinematicSolutions.Request object.
3. Use a convert function to map the Protobuf request into the ROS 2 request.
4. Call the service via the ROS 2 client.
5. Convert the resulting ROS 2 response back into a Protobuf InverseKinematicsResponse for easier handling.

    def _send_ik_request(
        self, odom_T_task: SE3Pose, wr1_T_tool: SE3Pose, task_T_desired_tool: SE3Pose
    ) -> GetInverseKinematicSolutions.Request:
        """
        Send an IK request for a given task frame, tool and tool desired pose.
        Returns:
            An IK solution, if any.
        """
        ik_request = inverse_kinematics_pb2.InverseKinematicsRequest(
            root_frame_name=ODOM_FRAME_NAME,
            scene_tform_task=odom_T_task.to_proto(),
            wrist_mounted_tool=inverse_kinematics_pb2.InverseKinematicsRequest.WristMountedTool(
                wrist_tform_tool=wr1_T_tool.to_proto()
            ),
            tool_pose_task=inverse_kinematics_pb2.InverseKinematicsRequest.ToolPoseTask(
                task_tform_desired_tool=task_T_desired_tool.to_proto()
            ),
        )
        request = GetInverseKinematicSolutions.Request()
        convert(ik_request, request.request)
        ik_reponse = self._ik_client.call(request)

        proto = inverse_kinematics_pb2.InverseKinematicsResponse()
        convert(ik_reponse.response, proto)
        return proto

The wasd example (implemented via the WasdInterface class) uses two primary ROS 2 communication patterns to control the robot:

1. Service Calls for Discrete Actions

Discrete actions like standing, sitting, or stowing the arm are performed by calling ROS 2 services. The example uses service clients for the Trigger message type.

• Stand: calls the stand service.
• Sit: calls the sit service.
• Stow Arm: calls the arm_stow service.
• Power: calls the power_on service.

2. Topic Publishing for Locomotion

Continuous movement (translation and rotation) is handled by publishing geometry_msgs/msg/Twist messages to the /cmd_vel topic.

To prevent the robot from moving indefinitely, the example uses a helper function _velocity_cmd_helper() that:

1. Constructs a Twist message with the requested linear (x, y) and angular (z) velocities.
2. Continuously publishes the message for a set VELOCITY_CMD_DURATION.
3. Publishes a zeroed-out Twist message (all fields at 0.0) once the duration expires to stop the robot.

3. Subscriptions for State Monitoring

The interface monitors robot state (such as battery levels) by subscribing to relevant topics. For example, it subscribes to /<robot_name>/status/battery_states using the BatteryStateArray message type to update the latest_battery_status member.

# Example of how a service call is triggered via a keybind mapping
self._command_dictionary = {
    ord("v"): self._sit,
}

# Example of the service call implementation
def _sit(self) -> None:
    self.cli_sit.call_async(Trigger.Request())

# Example of velocity command publishing pattern
def _velocity_cmd_helper(self, desc: str = "", v_x: float = 0.0, v_y: float = 0.0, v_rot: float = 0.0) -> None:
    twist = Twist()
    twist.linear.x = v_x
    twist.linear.y = v_y
    twist.angular.z = v_rot
    start_time = time.time()
    while time.time() - start_time < VELOCITY_CMD_DURATION:
        self.pub_cmd_vel.publish(twist)
        time.sleep(0.01)
    self.pub_cmd_vel.publish(Twist())

The SpotInCircle implementation coordinates Spot's base movement and arm gaze commands simultaneously using the synchros2 ROS 2 interface.

Trajectory Logic

The script uses get_me_a_circle(radius, steps) to approximate a circular path using a series of chords. Each segment consists of a forward motion (dx) and a fixed rotation (dyaw) relative to the robot's current position.

Gaze Logic

If an arm is detected and enabled, the script:

1. Defines a gaze point (0, radius, 0.1) in the body frame.
2. Transforms this gaze point to the odom frame using TFListenerWrapper.
3. Sends an arm_gaze_command to keep the arm pointing at the center of the circle.

Movement Execution

The script iterates through the generated (dx, dy, dyaw) triplets, sending synchro_se2_trajectory_point_command for base movement at each step. After each movement segment, the robot is commanded to stop to ensure controlled transitions.

The package provides several controller options via the robot_controllers launch argument.

Warning: For the forward_position_controller and forward_state_controller, the command array ordering must strictly match the configuration file. Incorrect ordering can cause dangerous, unpredictable robot motion.

The simple_sub implementation demonstrates how to bridge ROS 2 topic subscriptions with robot actions. This pattern is useful when you want to send pose commands from a local planner to the Spot robot without losing commands during execution.

Workflow

1. Subscription: The script subscribes to the /pose_commands topic using the Pose message type.
2. Buffering: To prevent command loss, the script maintains a latest_message buffer and an is_busy flag. If the robot is currently executing an action, incoming messages are stored in the buffer.
3. Sequential Execution: When the robot is no longer busy, the script processes the buffered message. It uses robot_commander.walk_forward_with_vision_frame_goal(message) to convert the Pose message into a movement action.
4. Completion: The script waits for each action to complete before resetting the is_busy flag and checking for the next buffered command, ensuring sequential movement.