ROS 2 Simulation Integration Exercise

Objective

Integrate ROS 2 nodes with a Gazebo simulation environment to control a virtual robot. This exercise demonstrates the sim-to-real transfer methodology by implementing robot control in simulation before real-world deployment.

Prerequisites

• ROS 2 Humble Hawksbill installed
• Gazebo installed with ROS 2 plugins
• Basic understanding of robot URDF models
• Completed ROS 2 basic publisher-subscriber exercise

Learning Goals

By completing this exercise, you will:

• Launch a robot simulation in Gazebo
• Control a simulated robot using ROS 2 topics
• Implement robot navigation in a simulated environment
• Understand the relationship between simulation and real-world robotics
• Use ROS 2 tools to monitor and debug simulated robot behavior

Exercise Steps

Step 1: Verify Gazebo Installation

First, ensure Gazebo is properly installed with ROS 2 support:

# Check Gazebo version
gazebo --version

# Check for ROS 2 Gazebo packages
apt list --installed | grep ros-humble-gazebo

Step 2: Create a Simple Robot Model

Create a basic differential drive robot URDF for simulation:

1. Create the robot description package:

cd ~/ros2_ws/src
ros2 pkg create --build-type ament_python robot_simulation
cd robot_simulation

2. Create the URDF directory and file robot_simulation/urdf/simple_robot.urdf:

<?xml version="1.0"?>
<robot name="simple_robot" xmlns:xacro="http://www.ros.org/wiki/xacro">

  <!-- Base Link -->
  <link name="base_link">
    <visual>
      <geometry>
        <box size="0.5 0.3 0.15"/>
      </geometry>
      <material name="blue">
        <color rgba="0 0 1 0.8"/>
      </material>
    </visual>
    <collision>
      <geometry>
        <box size="0.5 0.3 0.15"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="1.0"/>
      <inertia ixx="0.1" ixy="0.0" ixz="0.0" iyy="0.1" iyz="0.0" izz="0.1"/>
    </inertial>
  </link>

  <!-- Left Wheel -->
  <joint name="left_wheel_joint" type="continuous">
    <parent link="base_link"/>
    <child link="left_wheel"/>
    <origin xyz="0.0 0.2 -0.05" rpy="0 0 0"/>
    <axis xyz="0 1 0"/>
  </joint>

  <link name="left_wheel">
    <visual>
      <geometry>
        <cylinder radius="0.1" length="0.05"/>
      </geometry>
      <material name="black">
        <color rgba="0 0 0 1"/>
      </material>
    </visual>
    <collision>
      <geometry>
        <cylinder radius="0.1" length="0.05"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.2"/>
      <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
    </inertial>
  </link>

  <!-- Right Wheel -->
  <joint name="right_wheel_joint" type="continuous">
    <parent link="base_link"/>
    <child link="right_wheel"/>
    <origin xyz="0.0 -0.2 -0.05" rpy="0 0 0"/>
    <axis xyz="0 1 0"/>
  </joint>

  <link name="right_wheel">
    <visual>
      <geometry>
        <cylinder radius="0.1" length="0.05"/>
      </geometry>
      <material name="black">
        <color rgba="0 0 0 1"/>
      </material>
    </visual>
    <collision>
      <geometry>
        <cylinder radius="0.1" length="0.05"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.2"/>
      <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
    </inertial>
  </link>

  <!-- Robot Footprint -->
  <link name="base_footprint">
    <visual>
      <origin xyz="0 0 0.075"/>
      <geometry>
        <box size="0.001 0.001 0.001"/>
      </geometry>
    </visual>
  </link>

  <joint name="base_joint" type="fixed">
    <parent link="base_footprint"/>
    <child link="base_link"/>
    <origin xyz="0 0 0.075" rpy="0 0 0"/>
  </joint>

  <!-- Gazebo Plugins -->
  <gazebo>
    <plugin name="diff_drive" filename="libgazebo_ros_diff_drive.so">
      <left_joint>left_wheel_joint</left_joint>
      <right_joint>right_wheel_joint</right_joint>
      <wheel_separation>0.4</wheel_separation>
      <wheel_diameter>0.2</wheel_diameter>
      <max_wheel_torque>20</max_wheel_torque>
      <max_wheel_acceleration>1.0</max_wheel_acceleration>
      <publish_odom>true</publish_odom>
      <publish_odom_tf>true</publish_odom_tf>
      <publish_wheel_tf>true</publish_wheel_tf>
      <odometry_frame>odom</odometry_frame>
      <robot_base_frame>base_footprint</robot_base_frame>
    </plugin>
  </gazebo>

</robot>

Step 3: Create Robot Spawn Launch File

Create a launch file to spawn the robot in Gazebo:

1. Create the launch directory:

mkdir -p robot_simulation/launch

2. Create the launch file robot_simulation/launch/spawn_robot.launch.py:

from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import PathJoinSubstitution
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare


def generate_launch_description():
    # Launch Gazebo
    gazebo = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([
            PathJoinSubstitution([
                FindPackageShare('gazebo_ros'),
                'launch',
                'empty_world.launch.py'
            ])
        ]),
        launch_arguments={
            'world': PathJoinSubstitution([
                FindPackageShare('gazebo_ros'),
                'worlds',
                'empty_world.world'
            ])
        }.items()
    )

    # Spawn robot in Gazebo
    spawn_entity = Node(
        package='gazebo_ros',
        executable='spawn_entity.py',
        arguments=[
            '-topic', 'robot_description',
            '-entity', 'simple_robot',
            '-x', '0', '-y', '0', '-z', '0.1'
        ],
        output='screen'
    )

    # Robot State Publisher
    robot_state_publisher = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        parameters=[{
            'robot_description': PathJoinSubstitution([
                FindPackageShare('robot_simulation'),
                'urdf',
                'simple_robot.urdf'
            ])
        }]
    )

    return LaunchDescription([
        gazebo,
        robot_state_publisher,
        spawn_entity
    ])

Step 4: Create Robot Controller Node

Create a node to control the simulated robot:

1. Create the controller directory:

mkdir -p robot_simulation/robot_controller

2. Create the controller script robot_simulation/robot_controller/robot_controller.py:

#!/usr/bin/env python3

"""
Robot controller node for simulation
"""
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist
from nav_msgs.msg import Odometry
from std_msgs.msg import String
import math


class RobotController(Node):
    def __init__(self):
        super().__init__('robot_controller')

        # Create publisher for velocity commands
        self.cmd_vel_publisher = self.create_publisher(Twist, '/cmd_vel', 10)

        # Create subscriber for odometry
        self.odom_subscriber = self.create_subscription(
            Odometry,
            '/odom',
            self.odom_callback,
            10)

        # Create publisher for robot status
        self.status_publisher = self.create_publisher(String, '/robot_status', 10)

        # Timer for sending commands
        self.timer = self.create_timer(0.1, self.timer_callback)

        # Robot state
        self.current_x = 0.0
        self.current_y = 0.0
        self.current_theta = 0.0
        self.is_moving = False

        # Navigation goals
        self.goals = [
            (2.0, 0.0),    # Move 2m forward
            (2.0, 2.0),    # Move 2m right
            (0.0, 2.0),    # Move 2m back
            (0.0, 0.0),    # Return to origin
        ]
        self.current_goal_index = 0
        self.reached_goal = False

        self.get_logger().info('Robot controller initialized')

    def odom_callback(self, msg):
        """Update robot position from odometry"""
        self.current_x = msg.pose.pose.position.x
        self.current_y = msg.pose.pose.position.y

        # Extract orientation (simplified - assuming only z-rotation)
        quat = msg.pose.pose.orientation
        self.current_theta = math.atan2(2.0 * (quat.w * quat.z + quat.x * quat.y),
                                       1.0 - 2.0 * (quat.y * quat.y + quat.z * quat.z))

    def timer_callback(self):
        """Send movement commands based on navigation goals"""
        if self.current_goal_index >= len(self.goals):
            # All goals reached, stop the robot
            self.stop_robot()
            return

        current_goal = self.goals[self.current_goal_index]
        goal_x, goal_y = current_goal

        # Calculate distance to goal
        dist_to_goal = math.sqrt((goal_x - self.current_x)**2 + (goal_y - self.current_y)**2)

        # Check if we've reached the current goal
        if dist_to_goal < 0.1:  # 10cm tolerance
            if not self.reached_goal:
                self.get_logger().info(f'Reached goal {self.current_goal_index}: ({goal_x}, {goal_y})')
                self.reached_goal = True
                self.stop_robot()

                # Schedule next goal after a short pause
                self.get_timer = self.create_timer(2.0, self.next_goal)
        else:
            # Move toward the goal
            self.reached_goal = False
            self.move_to_goal(goal_x, goal_y)

    def move_to_goal(self, goal_x, goal_y):
        """Move the robot toward the specified goal"""
        # Calculate direction to goal
        dx = goal_x - self.current_x
        dy = goal_y - self.current_y
        goal_angle = math.atan2(dy, dx)

        # Calculate angle difference
        angle_diff = goal_angle - self.current_theta
        # Normalize angle to [-pi, pi]
        while angle_diff > math.pi:
            angle_diff -= 2 * math.pi
        while angle_diff < -math.pi:
            angle_diff += 2 * math.pi

        # Create twist message
        twist = Twist()

        # Set angular velocity to turn toward goal
        if abs(angle_diff) > 0.1:  # 0.1 rad tolerance
            twist.angular.z = max(min(angle_diff * 1.0, 1.0), -1.0)  # Limit angular velocity
        else:
            # Move forward when facing the right direction
            twist.linear.x = min(dist_to_goal * 0.5, 0.5)  # Limit linear velocity

        self.cmd_vel_publisher.publish(twist)

        # Publish status
        status_msg = String()
        status_msg.data = f'Navigating to ({goal_x:.2f}, {goal_y:.2f}), dist: {dist_to_goal:.2f}m'
        self.status_publisher.publish(status_msg)

    def next_goal(self):
        """Move to the next navigation goal"""
        self.current_goal_index += 1
        self.destroy_timer()  # Destroy the temporary timer

    def stop_robot(self):
        """Stop the robot by sending zero velocities"""
        twist = Twist()
        self.cmd_vel_publisher.publish(twist)

        status_msg = String()
        status_msg.data = 'Robot stopped'
        self.status_publisher.publish(status_msg)

    def get_timer(self):
        """Placeholder for temporary timer"""
        pass


def main(args=None):
    rclpy.init(args=args)

    robot_controller = RobotController()

    try:
        rclpy.spin(robot_controller)
    except KeyboardInterrupt:
        robot_controller.get_logger().info('Shutting down robot controller...')
    finally:
        robot_controller.stop_robot()
        robot_controller.destroy_node()
        rclpy.shutdown()


if __name__ == '__main__':
    main()

Step 5: Create Visualization Node

Create a node to visualize robot path and goals:

1. Create the visualization directory:

mkdir -p robot_simulation/path_visualizer

2. Create the visualization script robot_simulation/path_visualizer/path_visualizer.py:

#!/usr/bin/env python3

"""
Path visualizer for robot navigation
"""
import rclpy
from rclpy.node import Node
from visualization_msgs.msg import Marker, MarkerArray
from geometry_msgs.msg import Point
from nav_msgs.msg import Odometry
import math


class PathVisualizer(Node):
    def __init__(self):
        super().__init__('path_visualizer')

        # Create publisher for visualization markers
        self.marker_publisher = self.create_publisher(MarkerArray, '/visualization_marker_array', 10)

        # Create subscriber for odometry
        self.odom_subscriber = self.create_subscription(
            Odometry,
            '/odom',
            self.odom_callback,
            10)

        # Timer for publishing markers
        self.timer = self.create_timer(0.5, self.publish_markers)

        # Store robot path
        self.path_points = []
        self.max_path_length = 100  # Limit path history

        # Navigation goals
        self.goals = [
            (2.0, 0.0),
            (2.0, 2.0),
            (0.0, 2.0),
            (0.0, 0.0),
        ]

        self.get_logger().info('Path visualizer initialized')

    def odom_callback(self, msg):
        """Add current position to path"""
        point = Point()
        point.x = msg.pose.pose.position.x
        point.y = msg.pose.pose.position.y
        point.z = 0.05  # Slightly above ground for visibility

        self.path_points.append(point)

        # Limit path length
        if len(self.path_points) > self.max_path_length:
            self.path_points.pop(0)

    def publish_markers(self):
        """Publish visualization markers"""
        marker_array = MarkerArray()

        # Create path marker
        path_marker = Marker()
        path_marker.header.frame_id = "odom"
        path_marker.header.stamp = self.get_clock().now().to_msg()
        path_marker.ns = "path"
        path_marker.id = 0
        path_marker.type = Marker.LINE_STRIP
        path_marker.action = Marker.ADD

        # Set path marker properties
        path_marker.pose.orientation.w = 1.0
        path_marker.scale.x = 0.02  # Line width
        path_marker.color.r = 1.0
        path_marker.color.g = 0.0
        path_marker.color.b = 0.0
        path_marker.color.a = 1.0

        path_marker.points = self.path_points

        marker_array.markers.append(path_marker)

        # Create goal markers
        for i, (goal_x, goal_y) in enumerate(self.goals):
            goal_marker = Marker()
            goal_marker.header.frame_id = "odom"
            goal_marker.header.stamp = self.get_clock().now().to_msg()
            goal_marker.ns = "goals"
            goal_marker.id = i + 1
            goal_marker.type = Marker.CYLINDER
            goal_marker.action = Marker.ADD

            # Set goal marker properties
            goal_marker.pose.position.x = goal_x
            goal_marker.pose.position.y = goal_y
            goal_marker.pose.position.z = 0.0
            goal_marker.pose.orientation.w = 1.0
            goal_marker.scale.x = 0.3  # Diameter
            goal_marker.scale.y = 0.3  # Diameter
            goal_marker.scale.z = 0.1  # Height
            goal_marker.color.r = 0.0
            goal_marker.color.g = 1.0
            goal_marker.color.b = 0.0
            goal_marker.color.a = 0.7

            marker_array.markers.append(goal_marker)

        self.marker_publisher.publish(marker_array)


def main(args=None):
    rclpy.init(args=args)

    path_visualizer = PathVisualizer()

    try:
        rclpy.spin(path_visualizer)
    except KeyboardInterrupt:
        pass
    finally:
        path_visualizer.destroy_node()
        rclpy.shutdown()


if __name__ == '__main__':
    main()

Step 6: Update Package Configuration

Update the setup.py file to make your nodes executable:

from setuptools import find_packages, setup

package_name = 'robot_simulation'

setup(
    name=package_name,
    version='0.0.0',
    packages=find_packages(exclude=['test']),
    data_files=[
        ('share/ament_index/resource_index/packages',
            ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),
        ('share/' + package_name + '/urdf', ['urdf/simple_robot.urdf']),
        ('share/' + package_name + '/launch', ['launch/spawn_robot.launch.py']),
    ],
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='your_name',
    maintainer_email='your_email@example.com',
    description='Robot simulation package',
    license='TODO: License declaration',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
            'robot_controller = robot_simulation.robot_controller.robot_controller:main',
            'path_visualizer = robot_simulation.path_visualizer.path_visualizer:main',
        ],
    },
)

Step 7: Build and Run the Simulation

1. Build your package:

cd ~/ros2_ws
colcon build --packages-select robot_simulation
source install/setup.bash

2. Launch the robot in Gazebo:

ros2 launch robot_simulation spawn_robot.launch.py

3. In a new terminal, run the robot controller:

source ~/ros2_ws/install/setup.bash
ros2 run robot_simulation robot_controller

4. In another terminal, run the path visualizer (optional):

source ~/ros2_ws/install/setup.bash
ros2 run robot_simulation path_visualizer

Validation Criteria

Your implementation is successful if:

• The robot spawns correctly in Gazebo
• The robot navigates to all specified goals in sequence
• The robot stops when it reaches each goal before proceeding to the next
• The path visualization shows the robot's trajectory
• The robot returns to the origin at the end of the navigation sequence
• All nodes run without errors

Advanced Challenge

Extend the exercise by:

1. Adding obstacle avoidance using simulated laser scan data
2. Implementing a more sophisticated path planning algorithm (A*, Dijkstra)
3. Adding a camera sensor to the robot and processing visual data
4. Creating a custom Gazebo world with obstacles and landmarks

Monitoring and Debugging

Use these ROS 2 tools to monitor your simulation:

• ros2 topic list - List all active topics
• ros2 topic echo /odom - Monitor robot odometry
• ros2 topic echo /cmd_vel - Monitor velocity commands
• rqt_graph - Visualize the node graph
• rviz2 - Visualize robot state and sensor data

Troubleshooting

• If the robot doesn't move, check that /cmd_vel commands are being published
• If Gazebo doesn't start, verify that the Gazebo ROS packages are installed
• If the robot falls through the ground, check the URDF inertial properties
• If the simulation runs too fast/slow, adjust the Gazebo real-time factor
• Use gz topic -l to list Gazebo topics and verify communication