ROS Development
The Robot Operating System 2 (ROS 2) is a flexible framework for writing robot software. It is a collection of tools, libraries, and conventions that aim to simplify the task of creating complex and robust robot behavior across a wide variety of robotic platforms
ROS 2 provides the capability to write and run different processes (called nodes) that communicate with each other by sending and receiving messages on named buses (called topics), by calling remote procedures (called services) or by using actions. Please read the ROS 2 Basic Concepts to get a more clear overview of the concepts related to ROS 2.
This tutorial will describe some basic ROS 2 functionality that can be accomplished with Raph Rover.
Prerequisites​
Introspecting the ROS 2 graph​
ROS 2 comes with command line tools that can help to introspect the current
network of running nodes. The ROS 2 CLI uses a unified command structure with
the format ros2 <command> <verb>. Some of the available commands are:
- ros2 node - printing information about currently running nodes, testing connectivity,
- ros2 topic - listing and printing information about topics currently in use, printing published messages, publishing data to topics, finding the type of published messages
- ros2 service - listing and printing information about available services, calling the service with provided arguments,
- ros2 interface - displaying the fields of a specified ROS 2 message, service or action type.
Let's try running some examples. Before that, connect to Raph Rover via SSH.
Start by reading currently running nodes:
ros2 node list
You should see the nodes running on your Raph Rover. A graph of the running ROS nodes can be found in the ROS nodes section of the specification. Among them, you'll find nodes that communicate with the hardware and provide the base functionality.
Let's get more information about a specific node. For example, if you have a
node called /controller:
ros2 node info /controller
You should see all the subscribed and published topics, as well as available services that the node provides. Full topic list and their descriptions can be found in the ROS API section of the documentation.
Among published topics, you might see the /controller/odom topic. Let's read
the published values using ros2 topic command:
ros2 topic echo /controller/odom
Now, let's look at the /controller/cmd_ackermann topic. This topic is used by
the firmware to receive drive commands. We can look at its type:
ros2 topic type /controller/cmd_ackermann
You should get ackermann_msgs/msg/AckermannDrive. This is a ROS 2 message for
commanding Ackermann steering vehicles. You can look up the message description
using ros2 interface command:
ros2 interface show ackermann_msgs/msg/AckermannDrive
The output should look like this:
float32 steering_angle
float32 steering_angle_velocity
float32 speed
float32 acceleration
float32 jerk
The steering_angle field represents the desired steering angle in radians,
where steering_angle_velocity is the rate of change of the steering angle in
radians per second. The speed field indicates the desired speed of the vehicle
in meters per second, while acceleration and jerk represent the desired
acceleration and jerk (rate of change of acceleration) in meters per second
squared and meters per second cubed, respectively.
We can use ros2 topic pub command to publish messages to the
/controller/cmd_ackermann topic and control the rover:
ros2 topic pub --rate 20 /controller/cmd_ackermann ackermann_msgs/msg/AckermannDrive "{speed: 0.2, steering_angle: 0.0, steering_angle_velocity: 0.0, acceleration: 0.2, jerk: 0.2}"
The rover should start moving forward with a speed of 0.2 m/s. To stop message publishing, simply type Ctrl+C.
The --rate 20 argument tells the ros2 topic tool to publish the message repeatedly 20 times per second instead of publishing only one message. This is necessary because the firmware implements a timeout that will stop the rover if it doesn't receive the next command after some time.
Using ROS 2 client library to publish messages​
ROS 2 provides several client libraries that let you write ROS 2 nodes in different programming languages. The most common ones are rclcpp for C++ and rclpy for Python.
Here is a simple Python node that commands the rover by publishing to
/controller/cmd_ackermann topic:
#!/usr/bin/env python3
import time
import rclpy
from ackermann_msgs.msg import AckermannDrive
from rclpy.node import Node
class AckermannDemo(Node):
def __init__(self) -> None:
super().__init__('ackermann_demo')
# Create ROS 2 publisher
self.publisher = self.create_publisher(AckermannDrive, 'controller/cmd_ackermann', 10)
# Function that drives the rover with specified speed and
# steering angle for 2 seconds
def drive(self, speed: float, steering_angle: float) -> None:
msg = AckermannDrive()
msg.speed = speed
msg.steering_angle = steering_angle
msg.steering_angle_velocity = 1.0
msg.acceleration = 0.2
msg.jerk = 0.2
# Fields that are not set will default to zero
for _ in range(20): # Repeat 20 times
self.publisher.publish(msg) # Publish message
time.sleep(0.1) # Sleep for 100 ms
def main() -> None:
rclpy.init()
# Create node instance
node = AckermannDemo()
node.drive(0.2, 0.0) # forward
node.drive(0.0, 0.0) # stop
node.drive(0.2, 0.4) # gentle left arc
node.drive(0.0, 0.0) # stop
node.drive(0.2, -0.4) # gentle right arc
node.drive(0.0, 0.0) # stop
node.drive(-0.2, 0.0) # backward
node.drive(0.0, 0.0) # stop
# Cleanup
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
Copy this scirpt to rover's onboard computer.
You can paste this to new file when using a terminal. Copy the script to clipboard, then type:
cat > test_drive.py
Type Ctrl+Shift+V if using Linux terminal. Then type Ctrl+D to end the file.
Add execute permission to the file:
chmod +x test_drive.py
And execute it by typing:
python3 test_drive.py
The rover should drive forward, turn left, turn right, and then reverse. You can edit the script to change the speed and steering angles.
Ensure that the rover has a clear path around it and that you don't have a Web UI running as it may interfere with the control commands.
Adding additional functionality to the rover​
RaphOS provides an easy mechanism for adding new functionalities without building any of the base packages. The whole process of starting the ROS nodes at boot can be summarized by the following files:
- /etc/ros/robot.launch - a main launch file that starts the robot's functionality. It includes the launch file from the raph_bringup package which starts the base functionality of the rover, but also allows to add additional nodes to be started or parameters to be set.
A launch file in ROS 2 describes a set of nodes to be started with specified parameters. ROS 2 supports Python, XML, and YAML launch files, with Python being the most flexible format and XML being the simplest.
- /etc/ros/setup.bash - the environment setup file that sets all the
environment variables necessary for the successful start of the ROS nodes. It
sources the environment setup file from the target ROS distribution (by
default
/opt/ros/jazzy/setup.bash) and sets additional environment variables. - /etc/ros/urdf/robot.urdf.xacro - the URDF description (in
xacro format) that is uploaded to the
Parameter Server by the
raph_bringup.launch.xmlfile. It includes the robot's model from the raph_description package, but also allows to add additional links or joints to the model. - /etc/ros/aliases - a file that defines some convenient bash aliases for
system services. Most importantly, it defines
ros-restartalias for restarting ROS nodes andros-stopfor stopping them.
Adding custom nodes to be started at boot​
To add additional ROS nodes, you can create a custom launch file. You can learn how to create files in the ROS 2 Launch tutorial.
To add your custom functionality to the rover, include your launch file in
/etc/ros/robot.launch file by adding the following line inside the <launch>
tag:
<include file="$(find-pkg-share your_package_name)/launch/your_custom_launch_file.launch.xml"/>
After including your launch file, you can restart the ROS nodes by typing:
ros-restart
Now, your custom nodes should be started along with the base functionality of the rover and will start on boot.
Expanding the URDF model​
When integrating a sensor or other device to your rover, you might sometimes want to extend the robot's URDF model to:
- visualize the device attached to the rover in RViz 2
- make the robot aware of device's collision geometry
- provide additional reference frames (for example for the sensor readings)
You can create a separate URDF file for your attached device like this one:
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<!-- a link representing visual and collision
properties of the sensor -->
<link name="sensor_base_link">
<visual>
<origin xyz="0 0 0.05"/>
<geometry>
<box size="0.05 0.05 0.1"/>
</geometry>
<material name="red">
<color rgba="1 0 0 0.7"/>
</material>
</visual>
<collision>
<origin xyz="0 0 0.05"/>
<geometry>
<box size="0.05 0.05 0.1"/>
</geometry>
</collision>
</link>
<!-- fixed joint that attaches
the sensor to the rover's body -->
<joint name="sensor_base_joint" type="fixed">
<origin xyz="0.08 0 0"/>
<parent link="base_link"/>
<child link="sensor_base_link"/>
</joint>
<!-- reference frame for sensor readings -->
<link name="sensor_frame"/>
<!-- fixed joint that sets the origin
of the reference frame -->
<joint name="sensor_joint" type="fixed">
<origin xyz="0 0 0.06"/>
<parent link="sensor_base_link"/>
<child link="sensor_frame"/>
</joint>
</robot>
And include it in main xacro file that is located at
/etc/ros/urdf/robot.urdf.xacro by adding the following line in between
<robot> tags:
<xacro:include filename="$(find your_package)/urdf/sensor.urdf.xacro"/>
Now, when you restart the nodes with ros-restart command, the new URDF model
will be published and you should be able to view the new model in RViz 2.
You can use base_link as a reference frame for other links in the model. The
exact position of the base_link origin is defined as the center point in
between the front wheels.
The distance can be easily measured in CAD programs or even using physical measuring tools.
Building custom ROS 2 packages on Raph Rover​
To build custom ROS 2 packages on Raph Rover, you need to first
create a ROS 2 workspace.
The recommended location for the workspace on the rover is home directory
(~/ros2_ws or /home/raph/ros_ws). You can learn how to create a custom
package in the
Creating a ROS 2 Package tutorial.
When building your packages on the rover using colcon, we recommend using the following flags:
--symlink-install- to avoid copying install files during active development. Note: C/C++ sources still require rebuilds.--cmake-args -DCMAKE_BUILD_TYPE=Release- to build C++ packages in Release mode for better performance.
An example build command can look like this:
colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release
If UpBoard runs out of memory during the build, you can limit the number of
parallel workers using --parallel-workers flag. For example, to use only one
worker:
colcon build --parallel-workers 1
After a successful build, source the workspace install space in the system setup
so the robot uses your overlay on boot. Edit /etc/ros/setup.bash and add or
replace the sourced file with your workspace install setup, for example:
# comment out system ROS setup
# source /opt/ros/jazzy/setup.bash
source /home/raph/ros_ws/install/setup.bash
Connecting your computer to Raph Rover ROS 2 network​
ROS 2 is designed with distributed computing in mind. Unlike ROS 1, ROS 2 uses DDS (Data Distribution Service) which provides automatic discovery of nodes on the network. This makes it easier to run nodes across multiple machines without extensive configuration.
To install ROS 2 on your computer, you can follow the official installation guides.
In this section, we will assume you run Ubuntu 24.04 with ROS 2 Jazzy or later.
First, connect your computer to the same network your rover is connected to. It
can be either the Rover's Access Point (Raph-XXXX-5G or Raph-XXXX-2G by
default) or an external router (if you connected your rover to it).
ROS 2 nodes on the same network will discover each other automatically if they
use the same Domain ID. By default, ROS 2 uses Domain ID 0, but you can change
this by modifying the ROS_DOMAIN_ID environment variable:
export ROS_DOMAIN_ID=1
Make sure both your computer and the rover use the same Domain ID. If that's the case you should be able to see the rover's nodes in a terminal on your computer:
ros2 node list
ROS 2 graphical tools​
With ROS 2 properly configured on your computer, you can now run graphical tools to introspect and visualize the rover's state.
RViz 2​
RViz 2 is the 3D visualization tool for ROS 2. You can use it to visualize the robot model, sensor data, and more. You can find more information about RViz 2 in the RViz 2 User Guide.
rqt​
rqt provides various GUI plugins for ROS 2.
Some useful rqt plugins include:
rqt_graph- Visualize the ROS 2 computation graphrqt_console- View log messagesrqt_plot- Plot topic data in real-timerqt_image_view- View image topics
You can find more information about rqt in the rqt User Guide.