Session 14: GO2Controller Wrapper Class

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

---

Session Introduction

Building on your exploration of the GO2 simulation in Session 13, you'll now create your own ROS2 Python package called go2_high_five. This package will contain a GO2Controller class that wraps the simulation's service calls in a clean, object-oriented interface.

This session applies core OOP concepts from earlier in the course:

• Encapsulation - Hide ROS2 complexity behind simple methods
• Methods - sit(), stand(), walk(), move()
• Documentation - Complete docstrings and type hints

By the end of this session, you'll have a professional Python package that provides a simple interface for controlling the GO2 robot.

---

Portfolio 2 Preparation - Part A

This session begins Portfolio 2: GO2 Capstone Project. In Sessions 14-16, you'll build the GO2Controller wrapper class and implement the high five action. This evidence will be compiled and submitted as part of your capstone portfolio in Session 17.

Focus on: - Clean, professional code with comprehensive docstrings - Git commits with clear messages showing your development process - Documentation of your design decisions

Learning Objectives

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

• Create a ROS2 Python package from scratch
• Implement a controller class with proper encapsulation
• Wrap ROS2 service clients in Python methods
• Write docstrings and type hints for all public methods

---

Session Structure

1. Package Creation - Set up ROS2 Python package structure
2. GO2Controller Class - Implement the wrapper class
3. Demo Script - Run your controller against the simulation
4. Build and Run - See the robot respond to your code

---

Part 1: Creating the ROS2 Package

1.1 Package Structure

Navigate to your workspace and create the package:

cd ~/go_sim/src

# Create ROS2 Python package
ros2 pkg create --build-type ament_python go2_high_five \
  --dependencies rclpy std_msgs geometry_msgs

# View the created structure
tree go2_high_five/

Expected structure:

go2_high_five/
├── go2_high_five/
│   └── __init__.py
├── resource/
│   └── go2_high_five
├── test/
│   ├── test_copyright.py
│   ├── test_flake8.py
│   └── test_pep257.py
├── package.xml
├── setup.cfg
└── setup.py

1.2 Add Dependency on quadropted_msgs

Edit package.xml to add the custom message dependency:

<!-- Add after other <depend> tags -->
<depend>quadropted_msgs</depend>

1.3 Create Module Structure

cd ~/go_sim/src/go2_high_five/go2_high_five

# Create controller module
touch controller.py

---

Part 2: Implementing GO2Controller

2.1 The Controller Class

Create go2_high_five/controller.py with the following structure:

"""GO2 Robot Controller Module.

This module provides a high-level interface for controlling the 
Unitree GO2 robot via ROS2 services and topics.

!!! info "OOP Principle: Encapsulation"
    This module **hides ROS2 complexity** behind simple methods like sit(), walk(), move().
    Students only need to call these methods—they don't need to understand service clients,
    async futures, or Twist messages.

Example:
    >>> controller = GO2Controller('robot1')
    >>> controller.sit()  # Simple interface!
    True
"""

import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist
from quadropted_msgs.srv import RobotBehaviorCommand


class GO2Controller(Node):
    """High-level controller for GO2 robot.

    **Inheritance**: This class extends `rclpy.Node`, which provides:
    - Service client creation (`create_client()`)
    - Topic publishing (`create_publisher()`)
    - Logging (`get_logger()`)

    By inheriting from Node, we get all ROS2 infrastructure automatically.

    Attributes:
        namespace: Robot namespace for topics/services (e.g., 'robot1')
    """

    def __init__(self, namespace: str = "robot1"):
        """Initialize the GO2 controller.

        Args:
            namespace: Robot namespace for ROS2 topics/services.
                      Default is 'robot1'.
        """
        super().__init__('go2_controller')
        self._namespace = namespace

        # Create service client for behavior commands
        self._behavior_client = self.create_client(
            RobotBehaviorCommand,
            f'/{namespace}/robot_behavior_command'
        )

        # Create publisher for velocity commands
        self._cmd_vel_pub = self.create_publisher(
            Twist,
            f'/{namespace}/cmd_vel',
            10
        )

        # Wait for service to be available
        self._wait_for_service()

    def _wait_for_service(self, timeout_sec: float = 5.0) -> bool:
        """Wait for behavior service to become available.

        Args:
            timeout_sec: Maximum time to wait in seconds.

        Returns:
            True if service is available, False otherwise.
        """
        self.get_logger().info(
            f'Waiting for behavior service on /{self._namespace}...'
        )
        return self._behavior_client.wait_for_service(timeout_sec)

    def _call_behavior(self, command: str) -> bool:
        """Call the robot behavior service.

        This is a private method (starts with `_`) that encapsulates
        the complexity of making async ROS2 service calls.

        Args:
            command: Behavior command ('sit', 'walk', 'up').

        Returns:
            True if command was accepted, False otherwise.
        """
        request = RobotBehaviorCommand.Request()
        request.command = command

        future = self._behavior_client.call_async(request)
        rclpy.spin_until_future_complete(self, future, timeout_sec=5.0)

        if future.result() is not None:
            self.get_logger().info(f'Behavior "{command}" executed')
            return True
        else:
            self.get_logger().error(f'Behavior "{command}" failed')
            return False

    # --- Student Tasks: implement the four methods below ---
    # sit() is provided as a worked example.

    def sit(self) -> bool:
        """Make the robot sit down.

        Returns:
            True if successful, False otherwise.
        """
        return self._call_behavior('sit')

    def stand(self) -> bool:
        """Make the robot stand up (locked position, no walking).

        Returns:
            True if successful, False otherwise.
        """
        # TODO: Implement
        pass

    def walk(self) -> bool:
        """Enable walking mode (TROT).

        After calling this, use move() to control the robot.

        Returns:
            True if successful, False otherwise.
        """
        # TODO: Implement
        pass

    def move(self, linear_x: float = 0.0, angular_z: float = 0.0) -> None:
        """Send a velocity command to the robot.

        Robot must be in walk mode first.

        Args:
            linear_x: Forward/backward speed in m/s (positive = forward).
            angular_z: Rotation rate in rad/s (positive = left).
        """
        # TODO: Implement
        pass

    def stop(self) -> None:
        """Bring the robot to a complete stop."""
        # TODO: Implement
        pass

    def high_five(self) -> bool:
        """Execute high five action.

        Makes the robot lift its front-right leg and high five.

        This method will be fully implemented in Session 15 as an Action.
        For now, just return False.

        Returns:
            False (not yet implemented).
        """
        self.get_logger().warn('high_five() not yet implemented')
        return False

2.2 Method Specification

Use the UML class diagram in section 2.3 and the information below to guide your implementation. The diagram and table describe what each method does at the ROS2 level — the Python design is yours to work out.

sequenceDiagram
    participant C as GO2Controller
    participant BS as behavior_server<br/>/robot1/robot_behavior_command
    participant VC as velocity_controller<br/>/robot1/cmd_vel

    Note over C,BS: Behavior methods — ROS2 service
    C->>BS: stand() → command string
    BS-->>C: success: bool
    C->>BS: walk() → command string
    BS-->>C: success: bool

    Note over C,VC: Velocity methods — ROS2 topic
    C-)VC: move(x, z) → Twist message
    C-)VC: stop() → Twist message

Method	ROS2 interface	Message type	Key detail
stand()	Service call	RobotBehaviorCommand	Command string: "up"
walk()	Service call	RobotBehaviorCommand	Command string: "walk"
move(x, z)	Topic publish	geometry_msgs/Twist	Two fields to set: forward speed and rotation rate
stop()	Topic publish	geometry_msgs/Twist	Zero velocity

Part 3: Creating a Demo Script

3.1 Add Entry Point Script

Create scripts/high_five_demo.py:

mkdir -p ~/go_sim/src/go2_high_five/scripts
touch ~/go_sim/src/go2_high_five/scripts/high_five_demo.py
chmod +x ~/go_sim/src/go2_high_five/scripts/high_five_demo.py

Add content to scripts/high_five_demo.py:

#!/usr/bin/env python3
"""Demo script for GO2Controller.

Demonstrates basic robot control using the GO2Controller class.
"""

import rclpy
import time
from go2_high_five.controller import GO2Controller


def main():
    """Run the GO2 controller demo."""
    rclpy.init()

    try:
        # Create controller
        controller = GO2Controller('robot1')
        print("Controller initialized!")

        # Demo sequence
        print("\n--- Standing up ---")
        controller.stand()
        time.sleep(2)

        print("\n--- Sitting down ---")
        controller.sit()
        time.sleep(2)

        print("\n--- Enabling walk mode ---")
        controller.walk()
        time.sleep(1)

        print("\n--- Moving forward ---")
        controller.move(linear_x=0.2)
        time.sleep(2)

        print("\n--- Stopping ---")
        controller.stop()
        time.sleep(1)

        print("\n--- Sitting down ---")
        controller.sit()

        print("\nDemo complete!")

    except KeyboardInterrupt:
        print("\nDemo interrupted")
    finally:
        controller.destroy_node()
        rclpy.shutdown()


if __name__ == '__main__':
    main()

3.2 Register Entry Point

Edit setup.py to add the entry point:

entry_points={
    'console_scripts': [
        'high_five_demo = scripts.high_five_demo:main',
    ],
},

2.3 Class Design - UML Diagram

The GO2Controller class demonstrates key OOP principles through its structure:

classDiagram
    class Node {
        +create_client()
        +create_publisher()
        +get_logger()
        +destroy_node()
    }

    class GO2Controller {
        -_namespace: str
        -_behavior_client: Client
        -_cmd_vel_pub: Publisher
        +__init__(namespace)
        +sit() bool
        +stand() bool
        +walk() bool
        +move(linear, angular)
        +stop()
        +high_five() bool
        -_wait_for_service() bool
        -_call_behavior(command) bool
    }

    class RobotBehaviorCommand {
        <<service>>
        Request command: str
        Response success: bool
    }

    class Twist {
        <<message>>
        linear: Vector3
        angular: Vector3
    }

    Node <|-- GO2Controller
    GO2Controller --> RobotBehaviorCommand : calls
    GO2Controller --> Twist : publishes

OOP Principles Highlighted:

• Inheritance (is-a): GO2Controller is a Node—inherits all ROS2 capabilities
• Composition (has-a): GO2Controller has a service client and publisher—encapsulates ROS2 complexity
• Encapsulation: Private methods (_wait_for_service, _call_behavior) hide async ROS2 details from users
• Abstraction: Public methods provide simple interface (sit(), walk(), move()) hiding service complexities

---

Part 4: Build and Run

Understanding @patch — unittest.mock

@patch is a decorator from Python's built-in unittest.mock library. It temporarily replaces a named object (a class, method, or function) with a MagicMock for the duration of a single test, then restores the original automatically.

In these tests it's used to replace _wait_for_service and _call_behavior so tests run without a live ROS2 simulation. The mock object records what it was called with, letting you assert correct behaviour without the real infrastructure.

Simple example — from familiar to new:

You already know how to assert a return value:

# Session 1–3 style: test a pure function
def test_add(self):
    self.assertEqual(add(2, 3), 5)   # no side-effects, no hardware

With @patch you do the same thing, but swap out a dependency so the function under test thinks it's talking to real hardware:

from unittest.mock import patch

# Patch replaces `send_command` with a fake for this test only
@patch('my_robot.driver.send_command')
def test_stand(self, mock_send):
    mock_send.return_value = True        # fake: pretend command succeeded
    result = robot.stand()
    self.assertTrue(result)                          # same assertEqual style
    mock_send.assert_called_once_with('stand')  # also check what was sent

The mock_send argument is injected automatically by @patch — you don't create it yourself.

Decorators in context: @patch uses the same decorator mechanism as @property and @setter you've already used. A decorator is just a function that wraps another function to add behaviour. Other common examples include @staticmethod, @classmethod, and @deprecated (from typing_extensions) — which prints a warning when old/outdated code is called. Once you know the pattern, any decorator is readable.

Note: When stacking multiple @patch decorators, they are applied bottom-up but injected into the function top-down — so the parameter order is reversed from the decorator order.

Why this matters for robotics: In professional robotics development you can't always have hardware on your desk — the robot may be shared, expensive, or simply not available. Mocking lets you write and run the full test suite on a laptop with no robot connected. CI/CD pipelines (GitHub Actions, etc.) also have no hardware access, so mock-based tests are the only way to get automated test coverage. The same principle applies to any hardware-dependent system: spacecraft, medical devices, industrial controllers. Write tests that don't need the hardware; use the hardware only to validate that the real integration works.

Real Python — Understanding the Python Mock Library

4.1 Build the Package

cd ~/go_sim
colcon build --packages-select go2_high_five --symlink-install
source install/setup.bash

4.2 Integration Test

Terminal 1: Start the simulation

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

Terminal 2: Run the demo

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

You should see the robot:

1. Stand up
2. Sit down
3. Enable walk mode
4. Move forward
5. Stop
6. Sit down

---

Summary

Today you:

1. Created a ROS2 Python package from scratch
2. Implemented GO2Controller with proper encapsulation
3. Wrapped ROS2 service calls in clean Python methods
4. Wrote docstrings and type hints for all public methods
5. Ran the demo and watched the robot respond

Next session: You'll implement the high five() method to make the robot high five!

---

Homework (4 hours)

1. Extend GO2Controller with additional methods:

2. turn_left(angle: float) - Turn left by specified radians

3. turn_right(angle: float) - Turn right by specified radians

4. move_forward(distance: float) - Move forward by approximate distance

5. Research how the existing behaviors in quadropted_controller work:

6. How do they modify leg positions?

7. What messages/topics do they use?

8. Design your high five action (pseudocode):

9. What's the sequence of operations?
10. What ROS2 interfaces will you need?

---

Navigation

← Week 13 - GO2 Setup | Learning Plan | Week 15 - High Five Behavior →