Welcome aboard, aspiring ROS 2 navigator! In this tutorial, we’ll embark on an exciting journey through the seas of robotics, transforming our marine craft simulator from the previous tutorial into a full-fledged ROS 2 vessel. In this tutorial we shall learn the art of ROS 2 communication, package creation, odometry publishing, odometry subscribing, and more.
After completing this tutorial, you will be able to:
Publish and subscribe to ROS 2 topics from the command line
Create a ROS 2 package in a workspace
Write a ROS 2 node that publishes odometry data
Write a ROS 2 node that subscribes to odometry data
Launch a ROS 2 node
15.1 Getting Started
We will be using the same repository as the previous tutorial. Once the previous tutorial deadline is over, your instructor will update the upstream repository with the necessary files for this tutorial and raise a pull request on your team repository. You will need to pull in the changes from the upstream repository to your team repository to start this tutorial. The instructions to pull in the changes are given in Appendix B.
After you complete the steps shown in Appendix B to synchronize your repository with the upstream repository, navigate to the root directory of your repository. Pull the latest changes made to your team repository onto your local machine.
cd <your-repository-name>
git pull origin main
The relevant folder structure of the repository for this tutorial is shown below.
All your group members can work independently on their own branches. However, you will have to merge your branches back to the main branch before the end of the tutorial. We learnt how to do this in Chapter 13 using pull requests.
15.2 Understanding ROS 2 Topics
ROS 2 can be imagined as a vast ocean of information, with topics as the currents carrying messages between different parts of your robotic system. These topics are named channels that allow nodes (our program’s functional units) to exchange data.
Let’s start by exploring these currents using the command line:
Open a new terminal and navigate to the root directory of your repository.
cd<your-repository-name>
Start the docker container with the following command:
./ros2_run_devdocker.sh
Source your ROS 2 installation:
source /opt/ros/humble/setup.bash
List all active topics:
ros2 topic list
Publish a message to a topic:
ros2 topic pub /sea_conditions std_msgs/String "data: 'Calm seas ahead'"
Now let’s open a new terminal and connect to our docker container
docker exec -it oe3036 bash
Source your ROS 2 installation:
source /opt/ros/humble/setup.bash
Now let’s subscribe to the topic that we published to in the previous terminal:
ros2 topic echo /sea_conditions
You’ll see the message appear in your subscriber terminal. Congratulations, you’ve just sent your first message across the ROS 2 seas!
Open a new terminal and connect to our docker container:
docker exec -it oe3036 bash
Source your ROS 2 installation:
source /opt/ros/humble/setup.bash
Now let’s get ROS2 to tell us what it knows about the topic:
ros2 topic info /sea_conditions --verbose
This should give you some information about the topic, including the type of message it carries and the number of publishers and subscribers. You should see something like this:
We can see that there are two nodes - 1 publisher and 1 subscriber to the topic. We can also see that the publisher and subscriber nodes are named _ros2cli_354 and _ros2cli_834 respectively in this output (on your docker container, you will see different node names). These nodes have been created by the ros2 topic command that we used earlier for publishing and subscribing to the topic. Nodes are the entities that can publish and subscribe to topics.
15.3 Creating a ROS 2 Workspace and Package
Now that we understand the basics of communication, let’s construct a ROS 2 package for our marine craft simulator that we developed in the previous tutorial. Here we will create our own nodes using Python to publish and subscribe to the topic. For organizing our code we will create a workspace and a package.
15.4 Setting Up the Python Node to Publish Odometry Data
Now, let’s adapt our existing marine craft simulator to work with ROS 2. We’ll create a new Python script that will act as our ROS 2 node. The node will be responsible for simulating the vessel and publishing the vessel’s odometry data over the /mav_odom topic.
Create a new file mav_simulate.py in the /workspaces/mavlab/ros2_ws/src/student/tut_04/mav_simulator/mav_simulator directory:
cd /workspaces/mavlab/ros2_ws/src/student/tut_04/mav_simulator/mav_simulatortouch mav_simulate.py
Copy the following content into the file:
Code
import rclpyfrom rclpy.node import Nodefrom nav_msgs.msg import Odometryfrom geometry_msgs.msg import Quaternionfrom std_msgs.msg import Float64import syssys.path.append('/workspaces/mavlab/')from ros2_ws.src.student.tut_03.class_vessel import Vesselfrom ros2_ws.src.student.tut_03.module_kinematics import eul_to_quatfrom ros2_ws.src.student.tut_03.read_input import read_inputimport numpy as npclass MavNode(Node):def__init__(self, vessel_params, hydrodynamic_data):# Initialize the nodesuper().__init__('mav_node')# Initialize the vesselself.vessel = Vessel(vessel_params, hydrodynamic_data, ros_flag=True)# Create a publisher to publish the vessel odometryself.publisher =self.create_publisher(Odometry, 'mav_odom', 10)# Create a publisher to publish the rudder angleself.rudder_publisher =self.create_publisher(Float64, 'mav_rudder', 10)# Create a subscriber to receive the rudder angleself.rudder_subscriber =self.create_subscription(Float64, 'mav_rudder_command', self.rudder_callback, 10)# Create a timer to call the timer_callback function at the vessel time stepself.timer =self.create_timer(self.vessel.dt, self.timer_callback) def timer_callback(self):# Step the vessel forward in timeself.vessel.step()# Create an odometry message odom_msg = Odometry() odom_msg.header.stamp =self.get_clock().now().to_msg() odom_msg.header.frame_id ='odom' odom_msg.child_frame_id ='base_link'# Set position odom_msg.pose.pose.position.x =self.vessel.current_state[6] odom_msg.pose.pose.position.y =self.vessel.current_state[7] odom_msg.pose.pose.position.z =self.vessel.current_state[8]# Set orientation q = eul_to_quat(self.vessel.current_state[9:12], order='ZYX', deg=False) odom_msg.pose.pose.orientation = Quaternion(x=q[1], y=q[2], z=q[3], w=q[0])# Set linear velocity odom_msg.twist.twist.linear.x =self.vessel.current_state[0] odom_msg.twist.twist.linear.y =self.vessel.current_state[1] odom_msg.twist.twist.linear.z =self.vessel.current_state[2]# Set angular velocity odom_msg.twist.twist.angular.x =self.vessel.current_state[3] odom_msg.twist.twist.angular.y =self.vessel.current_state[4] odom_msg.twist.twist.angular.z =self.vessel.current_state[5]# Publish the odometry messageself.publisher.publish(odom_msg)# Publish rudder angle rudder_msg = Float64() rudder_msg.data =self.vessel.current_state[12] *180/ np.piself.rudder_publisher.publish(rudder_msg)# Log infoself.get_logger().info(f'\nTime: {self.vessel.t:.2f}s\n'f'Position (x,y,z): [{self.vessel.current_state[6]:.2f}, {self.vessel.current_state[7]:.2f}, {self.vessel.current_state[8]:.2f}]\n'f'Euler angles (phi,theta,psi): [{self.vessel.current_state[9]*180/np.pi:.2f}, {self.vessel.current_state[10]*180/np.pi:.2f}, {self.vessel.current_state[11]*180/np.pi:.2f}]\n'f'Velocity (u,v,w): [{self.vessel.current_state[0]:.2f}, {self.vessel.current_state[1]:.2f}, {self.vessel.current_state[2]:.2f}]\n'f'Angular velocity (p,q,r): [{self.vessel.current_state[3]:.2f}, {self.vessel.current_state[4]:.2f}, {self.vessel.current_state[5]:.2f}]\n'f'Rudder Command: {self.vessel.current_state[12] *180/ np.pi:.2f} deg\n' )def rudder_callback(self, msg):# Set the commanded rudder angleself.vessel.delta_c = msg.datadef main(args=None):try:# Initialize the ROS 2 node rclpy.init(args=args) vessel_params, hydrodynamic_data = read_input() vessel_node = MavNode(vessel_params, hydrodynamic_data) rclpy.spin(vessel_node)exceptKeyboardInterrupt:# Destroy the node and shutdown ROS 2 vessel_node.destroy_node() rclpy.shutdown()finally:# Destroy the node and shutdown ROS 2 vessel_node.destroy_node() rclpy.shutdown()if__name__=='__main__': main()
Let’s break down the important parts of the code:
The file contains a class MavNode that inherits from the Node class of ROS2. The __init__ method initializes the node, the vessel, and the publishers and subscribers. The timer_callback method is called at the vessel time step and publishes the odometry data. The rudder_callback method is called when a rudder command is received. Note that here we make a distinction between the commanded rudder angle and the actual rudder angle. The commanded rudder angle is the angle that the node will try to achieve, while the actual rudder angle is the angle that the vessel is currently at.
The main function initializes the ROS 2 node, reads the vessel parameters and hydrodynamic data, and spins the node. Spinning the node means that the node will run continuously and handle the communication between the node and the ROS 2 system. Unlike a regular Python script that runs to completion, a spun node will continue to run until the node is stopped.
Note that the MavNode class wraps a Vessel object that is defined in the class_vessel.py file. This Vessel object is the same as the one we defined in the previous tutorial. Instead of looping through the time steps as we did in the previous tutorial, here we use a timer to call the timer_callback method at the vessel time step. This ensures that the vessel is simulated at the correct time step and simulates the vessel in real-time. The node will publish the vessel’s odometry data to the /mav_odom topic and the actual rudder angle to the /mav_rudder topic every time the timer_callback is called.
15.5 Running Our ROS 2 Package
Now that our ship is built and rigged, it’s time to set sail! Let’s update our package configuration and create a launch file to run our node.
cd /workspaces/mavlab/ros2_wscolcon buildsource install/setup.bash
Launch the marine craft simulator:
ros2 launch mav_simulator mav_launch.py
This should start the vessel simulator and you should see the vessel’s odometry data being published to the /mav_odom topic. The node will also log the vessel’s current state to the terminal. The terminal should look like this:
[INFO] [launch]: All log files can be found below /home/mavlab/.ros/log/2025-02-10-12-59-45-647703-iit-m-2558
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [mav_simulate-1]: process started with pid [2559]
[mav_simulate-1] [INFO] [1739192386.338351042] [mav_simulate]:
[mav_simulate-1] Time: 0.10s
[mav_simulate-1] Position (x,y,z): [0.05, 0.00, 0.00]
[mav_simulate-1] Euler angles (phi,theta,psi): [0.00, 0.00, 0.00]
[mav_simulate-1] Velocity (u,v,w): [0.50, 0.00, 0.00]
[mav_simulate-1] Angular velocity (p,q,r): [0.00, 0.00, 0.00]
[mav_simulate-1] Rudder Command: 0.00 deg
[mav_simulate-1]
[mav_simulate-1] [INFO] [1739192386.402897739] [mav_simulate]:
[mav_simulate-1] Time: 0.20s
[mav_simulate-1] Position (x,y,z): [0.10, 0.00, 0.00]
[mav_simulate-1] Euler angles (phi,theta,psi): [0.00, 0.00, 0.00]
[mav_simulate-1] Velocity (u,v,w): [0.50, 0.00, 0.00]
[mav_simulate-1] Angular velocity (p,q,r): [0.00, 0.00, 0.00]
[mav_simulate-1] Rudder Command: 0.00 deg
[mav_simulate-1]
15.6 Understanding Odometry Messages
As our marine craft sails through the ROS 2 seas, it’s constantly reporting its position and velocity through Odometry messages. These messages are crucial for navigation and localization.
Let’s examine the Odometry message our node is publishing:
In a new terminal connected to our docker container, subscribe to the odometry topic:
You’ll see a stream of Odometry messages containing:
Header: Timestamp and frame information
Pose: Position and orientation of the marine craft and their covariance matrices
Twist: Linear and angular velocities and their covariance matrices
This data allows other nodes in our system to track the marine craft’s movement and make decisions based on its current state. Notice that the covariance matrices are zero, which means that we have no uncertainty in our measurements. This is so as we are simulating the vessel in a perfect environment. In reality, the vessel’s position and velocity will need to be estimated from noisy measurements from sensors. The odometry estimate will be obtained by fusing the noisy measurements from the sensors along with the vessel dynamics model through a process called state estimation. In this course we will get a glimpse of this process when we learn about state observers and Kalman filters. However, we will not be implementing Kalman filters in this course.
Check the rate of publication of the odometry messages:
This means that the node is publishing the odometry messages at a rate of 10 Hz.
15.7 Setting up the Python Node to Subscribe to the Odometry Topic
So far, we have the simulator publishing the odometry data to the mav_odom topic. The simulator also subscribes to the rudder command from the mav_rudder_command topic. Let’s now create a new node that will subscribe to the odometry topic and publish a rudder command to the mav_rudder_command topic.
Create a new file mav_control.py in the mav_simulator package:
cd /workspaces/mavlab/ros2_ws/src/student/tut_04/mav_simulator/mav_simulatortouch mav_control.py
The MAVController class inherits from the Node class of ROS2. The __init__ method initializes the node, the subscriber and publisher for the odometry and rudder command topics respectively. The odom_callback method is called when a new odometry message is received. The main function initializes the ROS 2 node, reads the vessel parameters and control type, and spins the node.
The odom_callback method extracts the current time, the vessel’s state from the odometry message, and the control command from the control mode function. The control command is then published to the rudder command topic.
The main function initializes the ROS 2 node, reads the vessel parameters and control type, and spins the node.
Notice that the control_mode is a function that takes the current time and the vessel’s state as arguments and returns the control command. This function is defined in the module_control.py file from the previous tutorial.
Update setup.py in the mav_simulator package to include the new node:
Launch the marine craft simulator node and the control node:
ros2 launch mav_simulator mav_launch.py
You should see the rudder command being published to the mav_rudder_command topic and vessel simulator showing the response of the vessel to the rudder command through the odometry data published to the mav_odom topic. The terminal should be showing the logs from both nodes and should look similar to this:
Congratulations! You’ve successfully transformed your marine craft simulator into a full-fledged ROS 2 node. You’ve mastered the arts of topic communication, package creation, node programming, odometry publishing, and odometry subscribing. Remember that this is just the beginning of your ROS 2 journey. There are many more aspects of ROS 2 to explore, from services and actions to more complex navigation and control algorithms.
15.8 Testing the Code
To test the code, commit your changes and push them to the remote repository. This will trigger the GitHub Actions workflow to run the tests. You can view the results of the tests by navigating to the “Actions” tab in the GitHub repository. If the tests pass, the workflow will be marked as successful. If the tests fail, you can view the detailed logs to see which tests failed and why. The GitHub workflow will also show the difference between the expected output and the output generated by your code.
15.9 Evaluation
The evaluation of the tutorial will be performed in the classroom. Notice that this is a group activity. You will be working in a group of 4 students. The evaluation will be performed on the group level. In order to pass the tutorial, the group must pass all the tests in the GitHub Actions workflow and also be able to answer all the questions during the evaluation in the classroom. Those who are not present in the classroom on the day of evaluation will not be able to participate in the evaluation and will automatically fail the tutorial.
While one of the objectives of these tutorials is to give you hands on experience with coding and practical implementation, the more important goal is to help you relate what you are doing in your tutorials to the underlying concepts discussed in the class. Therefore, while you are working on the implementation tasks, make sure you are also thinking about the underlying concepts and how they are being applied in the code.
Some questions to think about while you are working on the implementation tasks:
Is your code able to pass all the tests in the GitHub Actions workflow?
Are you able to show running of the ROS2 wrapped simulator live during the evaluation?
What are the topics and nodes that are being published and subscribed to in the simulation?
How is a node different from a topic?
What is type of ROS2 message used for publishing and subscribing to the odometry data?
What is type of ROS2 message used for publishing and subscribing to the rudder command?
How did the code from the previous tutorial change in this tutorial to allow for ROS2 communication?
How was the simulation code separated into two nodes in this tutorial? What does each node do?
15.10 Instructor Feedback through Pull Requests
Commit your changes to the remote repository often. This will help you to track your progress and also help you to revert to a working version if you make a mistake. In addition, the instructor and TA will be able to provide you with feedback on your code through the Feeback pull request available in your repository. Please do not close this pull request.