Skip to content

Session 16: Integration & Testing

Week: 16 Element: ICTPRG439 Element 3.3-3.5 Duration: 4 hours Phase: GO2 Capstone Project

Session Introduction

With your GO2Controller class complete and high_five action server working, this session focuses on integration testing, edge case handling, and sim-to-real preparation. You'll ensure your code is robust, well-documented, and ready for deployment to the real GO2 robot.

This session bridges simulation and reality - the same code you've written should work on the physical robot with minimal changes.

Portfolio 2 Preparation - Part C

This session completes the development phase of Portfolio 2. Integration testing, edge case handling, and sim-to-real preparation are essential components of your capstone portfolio submission. Ensure your code is production-ready before moving to documentation (Session 17).

Learning Objectives

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

  • Test integration between multiple ROS2 components
  • Handle edge cases and failure modes gracefully
  • Refactor code for improved reliability
  • Prepare code for sim-to-real deployment
  • Document testing procedures and results

Session Structure

  1. Integration Testing - Test full system interactions
  2. Edge Case Handling - Add robustness to your code
  3. Sim-to-Real Preparation - Parameterize for hardware
  4. Code Review - Clean up and document
  5. Progress Check - Verify portfolio readiness

Part 1: Integration Testing

1.1 What to Test

Your system now has two interacting components:

  • GO2Controller — wraps the robot behavior service (sit/stand/walk)
  • HighFiveServer — action server that uses GO2Controller internally and publishes joint commands

Integration testing verifies these components work together, not just in isolation.

Test Checklist:

Test What It Verifies
Stand → High Five → Sit Full behavior sequence works end-to-end
High Five goal accepted Action server starts correctly
Feedback received Server publishes phase/progress updates
Result returned Action completes with success=True
Action cancellation Server handles cancel gracefully

1.2 Write the Integration Test

Create scripts/integration_test.py. Your script should:

  1. Start GO2Controller and verify the robot can stand/sit
  2. Start an ActionClient and send a high five goal
  3. Assert feedback messages arrive (at least one per phase)
  4. Assert the final result is success=True
  5. Test cancellation: send a goal, cancel it mid-execution, verify the robot returns to standing

Discovery: How do you send a goal and then cancel it using ActionClient?

# Hint: check the ROS2 action client documentation
ros2 action send_goal --feedback /high_five go2_high_five/action/HighFive \
  "{target_height: 0.4, hold_seconds: 3}"

1.3 Run Integration Tests

Terminal 1: Launch simulation 

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

Terminal 2: Start action server 

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

Terminal 3: Run your integration test 

source ~/go_sim/go2_sim.env
python3 src/go2_high_five/scripts/integration_test.py

1.2 Component Integration Flow - State Machine Diagram

The following diagram shows how components integrate during high five execution:

graph TB
    Start["Client sends goal<br/>(high five request)"]
    Start --> CheckStand{"Robot<br/>standing?"}

    CheckStand -->|No| Walk["Enable walk mode<br/>(stand/walk behavior)"]
    CheckStand -->|Yes| CheckLeg
    Walk --> CheckLeg{"Leg available<br/>for lift?"}

    CheckLeg -->|No| Fail["Return FAILED<br/>(constraint violation)"]
    CheckLeg -->|Yes| Lift["Lift front-right leg<br/>(position control)"]

    Lift --> Osc["Oscillate leg<br/>(publish cmd_vel)"]
    Osc --> Feedback["Send feedback<br/>(20%, 50%, 80%complete)"]
    Feedback --> Lower["Lower leg to rest"]
    Lower --> Success["Return SUCCEEDED<br/>(high five complete)"]

    Fail --> Report["Report error to client"]
    Success --> Report
    Report --> End["End"]

    style CheckStand fill:#fff3cd
    style CheckLeg fill:#fff3cd
    style Fail fill:#f8d7da
    style Success fill:#d4edda

OOP Principle: Defensive Programming

This flow demonstrates state validation before each major action: - Check if robot is standing before executing leg lift - Validate leg constraints before attempting oscillation - Return clear error states if preconditions fail

Edge case handling is not an afterthought—it's part of the design pattern.

Part 2: Edge Case Handling

2.1 Service Timeout Handling

Update _call_behavior in controller.py:

def _call_behavior(self, command: str, timeout_sec: float = 5.0) -> bool:
    """Call the robot behavior service with timeout.

    Args:
        command: Behavior command ('sit', 'walk', 'up').
        timeout_sec: Maximum time to wait for response.

    Returns:
        True if command was accepted, False otherwise.
    """
    if not self._behavior_client.service_is_ready():
        self.get_logger().warning(
            f'Service not ready, waiting {timeout_sec}s...'
        )
        if not self._wait_for_service(timeout_sec):
            self.get_logger().error('Service still not available')
            return False

    request = RobotBehaviorCommand.Request()
    request.command = command

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

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

    except Exception as e:
        self.get_logger().error(f'Behavior "{command}" failed: {e}')
        return False

2.2 Action Cancellation & Recovery

Your HighFiveServer from Session 15 needs to handle the case where the client cancels mid-execution. In the execute callback, regularly check if the goal has been cancelled:

# Inside _execute_callback, after each phase:
if goal_handle.is_cancel_requested:
    goal_handle.canceled()
    self.get_logger().info('High five cancelled — returning to standing')
    # TODO: add recovery logic here (stand the robot safely)
    return HighFive.Result()

Questions to answer: - What should happen to the robot's leg if cancelled mid-raise? - How do you ensure the robot always returns to a safe standing position? - Where should you add cancel checks — every phase, or every joint command?

2.3 Add Status Property

Add a property to check controller readiness:

@property
def is_ready(self) -> bool:
    """Check if controller is ready for commands.

    Returns:
        True if services are available and joint states received.
    """
    service_ready = self._behavior_client.service_is_ready()
    joints_received = len(self._current_joints) > 0
    return service_ready and joints_received

Use this in HighFiveServer before accepting a goal — reject if the controller isn't ready.

Part 3: Sim-to-Real Preparation

3.1 Parameter Configuration

The action server's behavior (how high to raise the leg, how long to hold) should be configurable — not hardcoded. Create config/go2_params.yaml:

high_five_server:
  ros__parameters:
    # Robot namespace
    namespace: "robot1"

    # High five behavior parameters
    high_five:
      default_target_height: 0.4
      default_hold_seconds: 2
      raise_duration: 1.0     # seconds to raise leg
      lower_duration: 0.8     # seconds to lower leg

    # Real robot adjustments (uncomment for hardware):
    # high_five:
    #   default_target_height: 0.35
    #   raise_duration: 1.2

3.2 Load Parameters in Action Server

Add declare_parameter calls in HighFiveServer.__init__ and use them in _execute_callback:

self.declare_parameter('high_five.default_target_height', 0.4)
self.declare_parameter('high_five.raise_duration', 1.0)
# ... etc

3.3 Create a Launch File with Remap Support

Create launch/high_five.launch.py. The launch file should:

  • Start the high_five_server node
  • Load the params file
  • Accept an optional argument use_sim (default: True)
  • When use_sim:=False, add topic remaps for the real GO2 hardware
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory
import os


def generate_launch_description():
    config = os.path.join(
        get_package_share_directory('go2_high_five'),
        'config', 'go2_params.yaml'
    )

    # TODO: add DeclareLaunchArgument for 'use_sim'
    # TODO: use a ConditionalSubstitution or IfCondition to
    #       add --remap args when use_sim:=False
    #       Hint: see Session 15 Part 5 for the remap approach

    return LaunchDescription([
        Node(
            package='go2_high_five',
            executable='high_five_server',
            name='high_five_server',
            parameters=[config],
            output='screen',
        ),
    ])

Part 4: Code Review Checklist

4.1 Self-Review Your Code

Go through this checklist:

Category Check Status
Docstrings All public methods have docstrings
Type hints All parameters and returns typed
Error handling All external calls have try/except
Logging All state changes logged
Tests All methods have unit tests
Parameters Magic numbers moved to config
Recovery Failed states have recovery logic

4.2 Run Final Test Suite

# Run all tests
cd ~/go_sim
colcon test --packages-select go2_high_five
colcon test-result --verbose

# Check code style
flake8 src/go2_high_five/go2_high_five/

Part 5: Portfolio Evidence Collection

5.1 ICTPRG430 Evidence (OOP)

Document these with screenshots:

  1. Class definition - GO2Controller with encapsulation
  2. Methods - Public API (sit, stand, walk, move)
  3. Action Server - HighFiveServer class with Goal/Feedback/Result
  4. Documentation - Docstrings with Args, Returns, Examples
  5. Unit tests - Test file with passing results

5.2 ICTPRG439 Evidence (Component)

Document these:

  1. Component selection - Why you chose go2_ros2_sim_py
  2. Integration - How you connected your action server to the robot's ROS2 interface
  3. Testing - Integration test results with feedback output
  4. Sim-to-real - How topic remapping enables the same code on real hardware

5.3 Create Evidence Document

Create docs/PORTFOLIO-EVIDENCE.md:

# GO2 Capstone Portfolio Evidence

## Student: [Your Name]
## Date: [Date]

---

## ICTPRG430 - Object-Oriented Programming

### Element 2.1: Create Classes
- Screenshot: GO2Controller class definition
- Screenshot: HighFiveServer class definition

### Element 2.2: Implement Methods  
- Screenshot: HighFive.action definition (Goal/Feedback/Result)
- Screenshot: _execute_callback implementation

### Element 2.3: Test and Debug
- Screenshot: Unit test file
- Screenshot: Test results (all passing)
- Screenshot: Integration test showing feedback phases

---

## ICTPRG439 - Component Integration

### Element 1.1: Identify Requirements
- Component evaluated: go2_ros2_sim_py (prgrobots fork)
- Requirements: [List what you needed]

### Element 2.1: Integrate Component
- Screenshot: Package dependency in package.xml (quadropted_msgs)
- Screenshot: Topic connections (ros2 topic list with robot1/*)

### Element 3.2: Test Integrated System
- Screenshot: Simulation running with high_five_server
- Screenshot: High five action executing (feedback phases visible)
- Screenshot: Integration test results

---

## Demonstration Video

[Link to video of high five behavior in simulation]

---

## Reflection

[Your reflection on the project]

Summary

Today you: 1. Tested the full system (GO2Controller + HighFiveServer integration) 2. Added action cancellation handling and recovery logic 3. Parameterized the action server for sim-to-real deployment 4. Created a launch file with remap support for real hardware 5. Prepared portfolio evidence documentation

Next session: Final documentation, reflection, and portfolio submission.

Homework

  1. Complete integration tests and fix any failures
  2. Add cancel handling to HighFiveServer — test it with ros2 action cancel
  3. Fill in portfolio evidence document
  4. Record demonstration video (1-2 minutes showing high five in simulation)
  5. Prepare questions for final session

← Week 15 - High Five Action | Learning Plan | Week 17 - Documentation →