Session 15: Implementing the High Five Behavior

Week: 15 Element: ICTPRG430 Element 2.1 Duration: 4 hours Phase: GO2 Capstone Project

---

Session Introduction

In Session 14, you built a GO2Controller class using ROS2 Services — quick request-response calls like sit() and stand(). But some robot behaviors are long-running: they take several seconds, provide feedback as they proceed, and can be cancelled mid-execution.

This session introduces ROS2 Actions — the right tool for long-running behaviors — by implementing a high_five action server. You'll also explore a core concept in professional robotics: sim-to-real polymorphism, where the same ROS2 node works in simulation or on a real robot simply by remapping topic names.

Official Tutorial Reference

This session follows the same pattern as the official ROS2 tutorials: Writing an Action Server and Client (Python)

Complete that tutorial first if you haven't. This session applies the same concepts to a real robot behavior.

---

Portfolio 2 Preparation - Part B

Building on Session 14's GO2Controller wrapper, this session focuses on implementing the high five action using ROS2 Actions — a critical component of Portfolio 2. Your implementation will be evaluated on code quality, design patterns, and integration with existing components.

Learning Objectives

By the end of this session, you will be able to:

• Explain the difference between Services and Actions and when to use each
• Define a custom ROS2 Action (Goal, Feedback, Result)
• Implement an Action Server for a multi-step robot behavior
• Implement an Action Client that sends goals and monitors feedback
• Apply topic remapping to make the same node work in simulation and on real hardware

---

Session Structure

1. Services vs Actions — Why actions exist
2. Define the High Five Action — Create a custom .action file
3. Build the Action Server — Implement high_five_server.py
4. Build the Action Client — Send goals and receive feedback
5. Sim-to-Real Polymorphism — Topic remapping on real hardware

---

Part 1: Services vs Actions

In Session 14, you used the RobotBehaviorCommand service to trigger sit and stand. Services are ideal for fast, atomic operations.

But what about high five?

Property	Service	Action
Duration	Milliseconds	Seconds
Feedback during execution	❌ No	✅ Yes
Cancellable mid-flight	❌ No	✅ Yes
Example	sit(), stand()	high_five(), navigate_to()

A high five takes ~3 seconds, has distinct phases (raising → ready → completing), and you might want to cancel it if the robot is about to fall. An action server handles all of this cleanly.

sequenceDiagram
    participant Client as Action Client
    participant Server as Action Server (robot)
    Client->>Server: Send Goal (target_height)
    Server-->>Client: Goal Accepted ✓
    Server-->>Client: Feedback: "raising leg" (0%)
    Server-->>Client: Feedback: "ready" (50%)
    Server-->>Client: Feedback: "completing" (90%)
    Server-->>Client: Result: success=True

---

Part 2: Define the High Five Action

2.1 Create the Action File

ROS2 actions are defined in .action files with three sections separated by ---:

Goal       # What you ask the robot to do
---
Result     # What you get back when it finishes
---
Feedback   # What the robot reports while it's doing it

Create the file go2_high_five/action/HighFive.action:

# Goal: how to perform the high five
float32 target_height    # How high to raise the front leg (metres)
int32 hold_seconds       # How long to hold the raised position
---
# Result: outcome when complete
bool success
string message
---
# Feedback: reported during execution
string phase             # "raising", "ready", "lowering", "complete"
float32 progress         # 0.0 → 1.0

2.2 Register the Action in Your Package

In CMakeLists.txt or setup.py (depending on your package type), you need to declare the action so ROS2 can generate the Python bindings.

For an ament_python package, add the action directory to package.xml:

<buildtool_depend>rosidl_default_generators</buildtool_depend>
<exec_depend>rosidl_default_runtime</exec_depend>
<member_of_group>rosidl_interface_packages</member_of_group>

Discovery Task

Look at how quadropted_msgs declares its service in package.xml and CMakeLists.txt. Your action needs the same pattern. Check: ROS2 Creating Custom Action

---

2.2 Action Server/Client Pattern - Interaction Diagram

The action pattern allows the client to monitor long-running tasks. Here's how the high five action flows:

sequenceDiagram
    participant Client as HighFiveClient
    participant Server as HighFiveServer  
    participant Sim as Simulation

    Client->>Server: send_goal(HighFiveGoal)
    Server->>Sim: Call sit/walk behaviors
    Sim-->>Server: Success
    Server-->>Client: ACCEPTED
    Server->>Sim: Execute leg lift sequence
    Server-->>Client: Feedback (40% complete)
    Server-->>Client: Feedback (70% complete)
    Sim-->>Server: Complete
    Server-->>Client: SUCCEEDED with result
    Client->>Client: Process result

OOP Principle: Polymorphism

This action server can be swapped with a real robot server without changing client code. Both implement the same interface: - Same goal message format - Same feedback message format - Same result message format

This is design to interface, not implementation—a core OOP principle enabling sim-to-real robotics.

Part 3: Implement the Action Server

3.1 What the Server Needs to Do

Before writing any code, plan the execution sequence:

1. Accept or reject the goal (is the robot in a state to high-five?)
2. Stand the robot if not already standing
3. Raise the front-right leg to target_height while publishing feedback
4. Hold for hold_seconds
5. Lower the leg and return to standing
6. Return the result

3.2 Action Server Skeleton

Create go2_high_five/high_five_server.py. The pattern mirrors the official ROS2 tutorial:

import rclpy
from rclpy.action import ActionServer
from rclpy.node import Node
from sensor_msgs.msg import JointState
from go2_high_five.action import HighFive
# Add your other imports here


class HighFiveServer(Node):
    """Action server that executes a high five behavior on the GO2 robot.

    Subscribes to /robot1/joint_states to track current leg position.
    Publishes to /robot1/joint_command to control leg joints.
    Uses /robot1/robot_behavior_command service to stand the robot.

    Tip: These topic names match the simulator. See Part 5 for how
    to use this same node on the real robot via topic remapping.
    """

    def __init__(self):
        super().__init__('high_five_server')

        self._action_server = ActionServer(
            self,
            HighFive,
            'high_five',
            self._execute_callback
        )

        # TODO: set up your joint publisher, joint state subscriber,
        # and behavior service client here
        # Hint: look at what GO2Controller sets up in Session 14

    def _execute_callback(self, goal_handle):
        """Execute the high five behavior.

        This runs when a client sends a goal. You must:
        - Publish feedback at each phase
        - Call goal_handle.succeed() or goal_handle.abort() at the end
        - Return a HighFive.Result()
        """
        self.get_logger().info(
            f'Executing high five: height={goal_handle.request.target_height}'
        )

        feedback_msg = HighFive.Feedback()

        # TODO: implement the behavior sequence here
        # Phase 1: raise leg → publish feedback phase="raising", progress=0.0→0.5
        # Phase 2: hold     → publish feedback phase="ready",   progress=0.5
        # Phase 3: lower    → publish feedback phase="lowering", progress=0.5→1.0

        result = HighFive.Result()
        result.success = True
        result.message = 'High five complete!'
        goal_handle.succeed()
        return result


def main(args=None):
    rclpy.init(args=args)
    server = HighFiveServer()
    rclpy.spin(server)

Discovery Questions

Before writing the joint commands, explore the simulator:

1. Start the simulation and run ros2 topic list | grep robot1
2. What topic accepts joint position commands?
3. Run ros2 topic info <that topic> -v — what message type does it use?
4. Which joints belong to the front-right (FR) leg?

Hint: ros2 topic echo /robot1/joint_states --once shows all joint names.

3.3 Publishing Feedback

Inside _execute_callback, publish feedback like this:

feedback_msg.phase = "raising"
feedback_msg.progress = 0.2
goal_handle.publish_feedback(feedback_msg)

Publish at each phase transition so the client can track progress.

---

Part 4: Implement the Action Client

4.1 Action Client Skeleton

Create go2_high_five/high_five_client.py:

import rclpy
from rclpy.action import ActionClient
from rclpy.node import Node
from go2_high_five.action import HighFive


class HighFiveClient(Node):

    def __init__(self):
        super().__init__('high_five_client')
        self._action_client = ActionClient(self, HighFive, 'high_five')

    def send_goal(self, target_height: float, hold_seconds: int = 2):
        goal_msg = HighFive.Goal()
        goal_msg.target_height = target_height
        goal_msg.hold_seconds = hold_seconds

        self._action_client.wait_for_server()

        self._send_goal_future = self._action_client.send_goal_async(
            goal_msg,
            feedback_callback=self._feedback_callback
        )
        self._send_goal_future.add_done_callback(self._goal_response_callback)

    def _feedback_callback(self, feedback):
        phase = feedback.feedback.phase
        progress = feedback.feedback.progress
        self.get_logger().info(f'Feedback: {phase} ({progress:.0%})')

    def _goal_response_callback(self, future):
        goal_handle = future.result()
        if not goal_handle.accepted:
            self.get_logger().error('Goal rejected')
            return
        self._result_future = goal_handle.get_result_async()
        self._result_future.add_done_callback(self._result_callback)

    def _result_callback(self, future):
        result = future.result().result
        self.get_logger().info(f'Result: {result.message}')
        rclpy.shutdown()


def main(args=None):
    rclpy.init(args=args)
    client = HighFiveClient()
    client.send_goal(target_height=0.4, hold_seconds=2)
    rclpy.spin(client)

4.2 Test in Simulation

Terminal 1: Launch simulation

source ~/go_sim/go2_sim.env
ros2 launch gazebo_sim launch.py

Terminal 2: Start the action server

source ~/go_sim/go2_sim.env
ros2 run go2_high_five high_five_server

Terminal 3: Send a goal

# Option A: Use your client
ros2 run go2_high_five high_five_client

# Option B: Use the CLI directly
ros2 action send_goal /high_five go2_high_five/action/HighFive \
  "{target_height: 0.4, hold_seconds: 2}"

Watch the feedback print in Terminal 2 as the robot executes.

---

Part 5: Sim-to-Real Polymorphism

5.1 The Problem

Your HighFiveServer publishes to /robot1/joint_command — a topic that exists in the simulator. But the real GO2 dog (accessed via ~/go2_sdk/) may publish joint data on different topic names.

How do you use the same code on both? Topic remapping.

5.2 ROS2 Topic Remapping

ROS2 lets you rename topics at runtime, without changing any code:

# Run your action server, but remap sim topic names to real robot topics
ros2 run go2_high_five high_five_server \
  --ros-args \
  --remap /robot1/joint_states:=/go2/joint_states \
  --remap /robot1/joint_command:=/go2/joint_command

Your node still thinks it's talking to /robot1/joint_states — but ROS2 transparently redirects it.

5.3 Republishing Topics

If the real robot publishes in a different message format or with incompatible types, you can write a bridge node that subscribes to the real robot's topic and republishes on the standard name:

# bridge_node.py (pseudocode)
# Subscribe to real dog's topic
self.create_subscription(RealDogMsg, '/go2/joints', self._bridge_callback, 10)
# Republish in standard format
self._pub = self.create_publisher(JointState, '/robot1/joint_states', 10)

def _bridge_callback(self, msg):
    # Convert RealDogMsg → JointState, publish on standard topic
    ...

This is Polymorphism

Your HighFiveServer doesn't know or care whether it's talking to Gazebo or a real GO2. The interface is the contract — topic names and message types. The implementation behind those topics can be a simulator, a real robot, or a bridge node. Same code, different hardware.

5.4 Discovery Task

Explore the real dog SDK topics:

source ~/go2_sdk/go2_sdk.env
ros2 topic list

Compare the topic names to your simulator. Which topics need bridging or remapping to use HighFiveServer on the real dog?

---

Check Your Understanding

Q1: When should you use an Action instead of a Service?

Answer

When the behavior is long-running (seconds), needs to provide progress updates (feedback), or must be cancellable mid-execution. Services are fire-and-forget; actions are goal-track-complete.

Q2: What are the three parts of a .action file?

Answer

Goal (what you ask for), Result (what you get back when done), Feedback (intermediate updates during execution). Separated by ---.

Q3: How does topic remapping enable sim-to-real transfer?

Answer

Your node uses fixed topic names in code. At runtime, --remap old:=new makes ROS2 transparently redirect those names to match the real hardware. No code changes needed.

---

Session Summary

You have: 1. Understood when to use Actions vs Services (long-running, feedback, cancellable) 2. Defined a custom action type with Goal, Feedback, and Result 3. Implemented an action server for a multi-step robot behavior 4. Implemented an action client and tested it in Gazebo 5. Applied topic remapping to demonstrate sim-to-real polymorphism

---

Homework (4 hours)

1. Tune the high five — experiment with joint positions until the motion looks natural
2. Add cancellation handling — what should happen if the client cancels mid-execution?
3. Write unit tests for the action server using mocked joint publishers (apply Session 14 patterns)
4. Try the remap — start the SDK environment and list what topics exist on the real dog. Which ones need bridging?
5. ICTPRG430 evidence collection:
6. Screenshot the action server running and feedback printing
7. Screenshot your .action file definition
8. Write a brief explanation of how topic remapping enables polymorphism

---

Navigation

← Week 14 - Wrapper Class | Learning Plan | Week 16 - Integration →