3 ROS System

EE3305/ME3243 Robotic System Design

© Lai Yan Kai, National University of Singapore

The Robot Operating System (ROS) is a suite of software tools and libraries to standardize robotic software development. Standardization is important so that software can be easily run and troubleshooted, especially when code from different industrial or academic groups are used.

In this chapter, we go through the basic elements of ROS, and learn about the basic syntax to allow the different parts of a robot system communicate with one another.

You are allowed to use either Python or C++ for the project. C++ will be used in EE4308 because robotic software development for high-performing systems are all in C++. To use Python, go to the ee3305_py package and find the .py files. To use C++, go to the ee3305_cpp package and find the .cpp files.

2 Python and C++ Templates

2.1 Subscriber Template in Node Constructor

2.2 Publisher Template in Node Constructor

2.3 Subscriber Callback

3 Behavior Node

3.1 Relevant File

3.2 Initialize Goal Pose Subscriber

3.3 Initialize Odometry Subscriber

3.4 Initialize Path Request Publisher

3.5 Program Goal Pose Subscriber Callback

3.6 Program Odometry Subscriber Callback

3.7 Publish Path Request

4 Controller Node

4.1 Relevant File

4.2 Initialize Path Subscriber

4.3 Initialize Odometry Subscriber

4.4 Initialize Command Velocity Publisher

4.5 Initialize Lookahead Point Publisher

4.6 Program Path Subscriber Callback

4.7 Program Odometry Subscriber Callback

4.8 Publish Command Velocity

4.9 Publish Lookahead Point

5 Planner Node

5.1 Relevant File

5.2 Initialize Global Costmap Subscriber

5.3 Initialize Path Request Subscriber

5.4 Initialize Path Publisher

5.5 Program Path Request Subscriber Callback

5.6 Program Global Costmap Subscriber Callback

5.7 Publish Path

6 Building

7 Testing

1 The Project Design

The image below depicts the nodes and topics in the project's ROS system.

1.1 Nodes

Nodes are processes in the ROS system which are run simultaneously. They implement one component of a robotic system. In this project, the Behavior, Planner, and Controller nodes will be implemented.

The behavior node is the decision maker that controls how often paths should be planned and directs the robot to the clicked goal pose.
The planner node finds a path around obstacles based on the start and goal coordinates provided by the behavior node. The path is found using Dijkstra.
The controller node follows the path found by the planner node. Pure pursuit is used to follow the path.

Other nodes exist in the system but they need not be implemented. The nodes implementing the simulation are lumped together into Simulated Robot in the diagram below. The map server provides the map found using SLAM, and RViz visualizes the robot and topics.

1.2 Topics

Nodes communicate with each other using topics. Every topic contains a message that is a certain type. A topic subscriber receives messages, while a topic publisher transmits messages. Every node can have multiple subscribers and publishers on different topics.

1.3 Callbacks

In programming, callbacks are used to asynchronously receive messages and run instructions at fixed intervals of time. The focus of the project would be to program these callbacks so that the robotic system can run as intended.

When programming callbacks, keep in mind that callbacks must run and terminate quickly. In the default implementation, callbacks within a node are run one after another and not simultaneously. If a callback is run for too long or waits indefinitely, other callbacks are delayed or not run, and the node may not run as intended.

Keeping callbacks short is particularly when it comes to topics. Messages are processed by subscriber callbacks. Delaying subscriber callbacks because of another slow callback can prevent the node from running on the most updated information. A planner or controller that acts on outdated information can cause the robot to crash.

1.4 Timer

The timer controls how frequently a callback is run. This is useful for implementing controllers that need to be run at a fixed frequency, and logic that needs to be run in a loop without blocking other callbacks. The timer callback is just like any other callback and must run and terminate quickly.

2 Python and C++ Templates

In the subsequent sections, the topic subscribers, publishers, and subscriber callbacks are implemented. The templates and in this section should be used when implementing them.

2.1 Subscriber Template in Node Constructor

A subscriber should be initialized in its node's class constructor. In Python, the constructor is the function __init__(). In C++, the constructor function has the same name as the class and begins with the explicit keyword.

Node Class	`NodeA`. C++ only.
Handle	`sub_topic_a_`
Message Type	`MsgTypeA`. For C++, use `lib_a::msg::MsgTypeA`.
Topic	`"topic_a"`
Callback	`callbackSubTopicA_`
QoS	`qos_a`

Match the syntax above to the template below, depending on the language your team has chosen for this project.

Python

self.sub_topic_a_ = self.create_subscription(
    MsgTypeA,
    "topic_a",
    self.callbackSubTopicA_,
    qos_a,
)

C++

this->sub_topic_a_ = this->create_subscription<lib_a::msg::MsgTypeA>(
    "topic_a", 
    qos_a, 
    std::bind(&NodeA::callbackSubTopicA_, this, std::placeholders::_1)
);

2.2 Publisher Template in Node Constructor

A publisher should be initialized in its node's class constructor. While subscribers use callbacks to process received messages, publishers rely on function calls to publish messages, and do not require callbacks.

Handle	`pub_topic_a_`
Message Type	`MsgTypeA`. For C++, use `lib_a::msg::MsgTypeA`.
Topic	`"topic_a"`
QoS	`qos_a`

Match the syntax above to the template below, depending on the language your team has chosen for this project.

Python	self.pub_topic_a_ = self.create_publisher( MsgTypeA, "topic_a", qos_a )
C++	this->pub_topic_a_ = this->create_publisher<lib_a::msg::MsgTypeA>( "topic_a", qos_a );

2.3 Subscriber Callback

A subscriber callback creates a local copy of a received message. The copied data is stored in the node instance's properties (variables). The subscriber callback is run once for every message.

In the callback CallbackSubTopicA_, copy the message contents into the node instance. The message is of type lib_a::msg::MsgTypeA. By googling or using VSCode Intellisense, find the matching message property and copy the data into the corresponding node instance's variable.


Callback	`CallbackSubTopicA_`
Message Type	`lib_a::msg::MsgTypeA`
`instance_property_a_`	Description of the property's purpose. Find the message property that corresponds to the description.
`instance_property_b_`
`instance_property_c_`

In the callback, match the syntax to the template below.

Python

self.instance_property_a_ = msg.p.q.r # find the message property
self.instance_property_b_ = msg.p.s   # find the message property
self.instance_property_c_ = msg.p.t   # find the message property

C++

this->instance_property_a_ = msg->p.q.r // find the message property
this->instance_property_b_ = msg->p.s   // find the message property
this->instance_property_c_ = msg->p.t   // find the message property

3 Behavior Node

The behavior node is the decision-maker of the robotic navigation system. In this section, the subscribers, publishers, and subscriber callbacks for the behavior node, will be implemented.

3.1 Relevant File

The following file will be relevant for this task. Choose either the Python or C++ file, depending on your chosen language.

Name	Where	Description
Behavior code	Python file located in `src/ee3305_py/ee3305_py/behavior.py`	Program the ROS components in this file.
Behavior code	C++ file located in `src/ee3305_cpp/src/behavior.cpp`	Program the ROS components in this file.

3.2 Initialize Goal Pose Subscriber

Subscribes to the goal pose sent by RViz. The table below shows the function and variable names to implement the topic subscriber.

Goal Pose Subscriber
Node Class	`Behavior`. C++ only.
Handle	`sub_goal_pose_`
Message Type	`PoseStamped`. For C++, use `geometry_msgs::msg::PoseStamped`.
Topic	`"goal_pose"`
Callback	`callbackSubGoalPose_`
QoS	`10`

3.3 Initialize Odometry Subscriber

Subscribes to the odometry messages, which contains the robot's pose and twist. The robot's pose is its position and orientation. The robot's twist is its linear and angular velocities.

Odometry Subscriber
Node Class	`Behavior`. C++ only.
Handle	`sub_odom_`
Message Type	`Odometry`. For C++, use `nav_msgs::msg::Odometry`.
Topic	`"odom"`
Callback	`callbackSubOdom_`
QoS	`10`

3.4 Initialize Path Request Publisher

Publishes the request to plan a path between the robot's pose and goal pose.

Keep in mind that using topics for such requests is not standard design. ROS2 services should be used when a relatively long running operation is requested. In this case, path planning may last a few hundreds of milliseconds for long paths.

Path Request Publisher
Handle	`pub_path_request_`
Message Type	`Path`. For C++, use `nav_msgs::msg::Path`
Topic	`"path_request"`
QoS	`10`

3.5 Program Goal Pose Subscriber Callback

The callback copies the 2D coordinates of the clicked goal pose into the node.

Goal Pose Subscriber Callback
Callback	`callbackSubGoalPose_`
Message Type	`geometry_msgs::msg::PoseStamped`

`goal_x_`	The goal's x-coordinate.
`goal_y_`	The goal's y-coordinate.

3.6 Program Odometry Subscriber Callback

The callback copies the 2D coordinates of the robot into the node. There is no need for orientation in this node.

Odometry Subscriber Callback
Callback	`callbackSubOdom_`
Message Type	`nav_msgs::msg::Odometry`

`rbt_x_`	The robot's x-coordinate.
`rbt_y_`	The robot's y-coordinate.

3.7 Publish Path Request

The timer callback callbackTimerPlan_() publishes a path request based on the robot's current pose and the last known goal pose at regular intervals of time. As mentioned, this is best implemented as a ROS2 service.

Look for callbackTimerPlan_() in the code.
Write the message rbt_pose with the robot's 2D coordinates.
1. Copy the node instance's rbt_x_ property into the message.
2. Copy the node instance's rbt_y_ property into the message.
Write the message goal_pose with the last known goal's 2D coordinates.
1. Copy the node instance's goal_x_ property into the message.
2. Copy the node instance's goal_y_ property into the message.
The message to publish is msg_path_request. The message property poses is a list (Python) / vector (C++).
1. rbt_pose should be the first element in poses.
2. goal_pose should be the second element in poses.
Publish the message using the pub_path_request handle. The code is already provided.

4 Controller Node

The controller node implements pure pursuit to follow the path returned by the planner node.

4.1 Relevant File

The following file will be relevant for this task. Choose either the Python or C++ file, depending on your chosen language.

Name	Where	Description
Controller code	Python file located in `src/ee3305_py/ee3305_py/controller.py`	Program the ROS components in this file.
Controller code	C++ file located in `src/ee3305_cpp/src/controller.cpp`	Program the ROS components in this file.

4.2 Initialize Path Subscriber

The path subscriber receives the path found by the path planner node. The received path message contains the individual poses of points along the path, which will be subsequently used by the pure pursuit algorithm to find a lookahead point.

Path Subscriber
Node Class	`Controller`. C++ only.
Handle	`sub_path_`
Message Type	`Path`. For C++, use `nav_msgs::msg::Path`
Topic	`"path"`
Callback	`callbackSubPath_`
QoS	`10`

4.3 Initialize Odometry Subscriber

Subscribes to the robot's current pose and twist.

Odometry Subscriber
Node Class	`Controller`. C++ only.
Handle	`sub_odom_`
Message Type	`Odometry`. For C++, use `nav_msgs::msg::Odometry`
Topic	`"odom"`
Callback	`callbackSubOdom_`
QoS	`10`

4.4 Initialize Command Velocity Publisher

Publishes the command velocities using a twist (linear and angular velocities) message. The velocities move (command) the robot.

Command Velocity Publisher
Handle	`pub_cmd_vel_`
Message Type	`TwistStamped`. For C++, use `geometry_msgs::msg::TwistStamped`
Topic	`"cmd_vel"`
QoS	`10`

4.5 Initialize Lookahead Point Publisher

Publishes the pose (position and orientation) of the lookahead point found by the controller. Only the position is used. Primarily for visualizing the lookahead point in RViz.

Lookahead Point Publisher
Handle	`pub_cmd_vel_`
Message Type	`PoseStamped`. For C++, use `geometry_msgs::msg::PoseStamped`
Topic	`"lookahead"`
QoS	`10`

4.6 Program Path Subscriber Callback

The callback copies the path found by the path planner node into the controller node.

Path Subscriber Callback
Callback	`callbackSubPath_`
Message Type	`nav_msgs::msg::Path`

`path_poses_`	The array / vector containing the poses (`PoseStamped`) of the path points.

4.7 Program Odometry Subscriber Callback

The callback copies the robot's pose (position and orientation) into the node.

Odometry Subscriber Callback
Callback	`callbackSubOdom_`
Message Type	`nav_msgs::msg::Odometry`

`rbt_x_`	The robot's x-coordinate.
`rbt_y_`	The robot's y-coordinate.
`rbt_yaw_`	The robot's yaw heading.

To obtain the yaw heading, the quaternion in the message has to be converted to Euler angles (roll, pitch, yaw). Suppose the quaternion (implemented as q) is:

$$\mathbf{q} = \left[ q_x , q_y , q_z , q_w \right]^\top$$

The yaw $\varphi$ is calculated as:

$$\begin{align*} \Delta y &= 2 (q_w q_z + q_x q_y) \\\ \Delta x &= 1 - 2 (q_y^2 + q_z^2)\\\ \varphi &= \mathrm{atan2} ( \Delta y, \Delta x ) \end{align*}$$

The function atan2() can be used.

4.8 Publish Command Velocity

This section can be deferred until the pure pursuit controller is completed in 04_Controller.md.

The callbackTimer_() function runs the pure pursuit controller loop, which determine*s the linear (forward / backwards) and angular velocity (yaw steering) that the robot should move. The pub_cmd_vel_ handle should be used to publish the velocities in the msg_cmd_vel message.

4.9 Publish Lookahead Point

This section can be deferred until the pure pursuit controller is completed in 04_Controller.md.

The getLookaheadPoint_() function finds the lookahead point from the path planner's path. The pub_lookahead_ handle should be used to publish the velocities in the msg_lookahead message.

5 Planner Node

The planner node uses the inflation costmap (global costmap) to plan a path around non-convex obstacles, while keeping a safe distance from obstacles.

5.1 Relevant File

The following file will be relevant for this task. Choose either the Python or C++ file, depending on your chosen language.

Name	Where	Description
Planner code	Python file located in `src/ee3305_py/ee3305_py/planner.py`	Program the ROS components in this file.
Planner code	C++ file located in `src/ee3305_cpp/src/planner.cpp`	Program the ROS components in this file.

5.2 Initialize Global Costmap Subscriber

This subscriber requires a custom Quality of Service (QoS) to receive the global costmap. Specifically, a latching QoS is so that the global costmap, which is published once, is always available for late-joining subscribers.

The code is already given and nothing needs to be done here.

Global Costmap Subscriber
Node Class	`Planner`. C++ only.
Handle	`sub_global_costmap_`
Message Type	`OccupancyGrid`. For C++, use `nav_msgs::msg::OccupancyGrid`
Topic	`"global_costmap"`
Callback	`callbackSubGlobalCostmap_`
QoS	A latching QoS, by setting the durability policy to transient local.

5.3 Initialize Path Request Subscriber

Subscribes to the path request sent by the behavior node. The path request contains the robot's pose and goal pose.

As mentioned, it is more appropriate to use ROS services to implement a path planning request.

Path Request Subscriber
Node Class	`Planner`. C++ only.
Handle	`sub_path_request_`
Message Type	`Path`. For C++, use `nav_msgs::msg::Path`
Topic	`"path_request"`
Callback	`callbackSubPathRequest_`
QoS	`10`

5.4 Initialize Path Publisher

Publishes the path found by the path planner. The path is used by the RViz (visualization) and the controller for path following.

Path Publisher
Handle	`pub_path_`
Message Type	`Path`. For C++, use `nav_msgs::msg::Path`
Topic	`"path"`
QoS	`10`

5.5 Program Path Request Subscriber Callback

The callback copies the 2D coordinates of the robot and goal into the node.

Path Request Subscriber Callback
Callback	`callbackSubPathRequest_`
Message Type	`nav_msgs::msg::Path`

`rbt_x_`	The robot's x-coordinate.
`rbt_y_`	The robot's y-coordinate.
`goal_x_`	The goal's x-coordinate.
`goal_y_`	The goal's y-coordinate.

5.6 Program Global Costmap Subscriber Callback

The callback copies the global costmap and its dimensions into the node. In this project, the callback is only run once, after the node is initialized. The message type is nav_msgs::msg::OccupancyGrid.

Global Costmap Subscriber Callback
Callback	`callbackSubGlobalCostmap_`
Message Type	`nav_msgs::msg::OccupancyGrid`

`costmap_`	The array / vector containing the costmap's cost values.
`costmap_resolution_`	The length of each cell in meters.
`costmap_origin_x_`	The x-coordinate of the costmap's bottom-left corner.
`costmap_origin_y_`	The y-coordinate of the costmap's bottom-left corner.
`costmap_rows_`	The height of the costmap.
`costmap_cols_`	The width of the costmap.

5.7 Publish Path

This section can be deferred until the Dijkstra algorithm is completed in 05_Planner.md

The dijkstra_() function runs the Dijkstra path planning algorithm. The path is published once the path is found, which is when the goal is reached by the algorithm. The pub_path_ handle should be used to publish the msg_path path message.

6 Building

If modifications are made to the code, re-building is necessary in C++, but not needed for Python.

Python	The `--symlink-install` argument in `colcon build` links the Python scripts into the installation directory. Therefore, once the workspace has been built, rebuilding is not needed after the Python scripts are modified. Fix any IntelliSense warnings or errors in VSCode instead.
C++	Rebuilding is required if modifications are made to the C++ files. Fix any IntelliSense warnings or errors in VSCode before building. To build, cd ~/ee3305 # if terminal is not in workspace colcon build --symlink-install

7 Testing

After completing the tasks in this chapter, only the controller and planner algorithms need to be implemented (excluding publishing tasks that require the algorithms to be implemented). To test the code,

Build the workspace if required.

Run the simulation using the ros2 launch command with the following arguments.

Launch Argument	Launch Value
`cpp`	`False`. Default. Runs the Python scripts from the Python package.
`cpp`	`True`. Runs the C++ executables from the C++ package.
`headless`	`False`. Default. Opens the 3D graphic user interface for Gazebo.
`headless`	`True`. Runs the simulation in the background without opening the interface. This option is easier on the computer.
`libgl`	`False`. Default. Meant for single/dual boot users. VirtualBox users will not be able to launch the simulation with this value.
`libgl`	`True`. Meant for VirtualBox users.

Take note of the cpp and libgl arguments.

cd ~/ee3305                 # if terminal is not in workspace
source install/setup.bash   # if not already sourced
ros2 launch ee3305_bringup run.launch.py cpp:=False headless:=False libgl:=False

Select a goal pose by pressing 2D Goal Pose on RViz and clicking a location on the map.
If there are no runtime errors, a straight line path will now be generated between the robot and the goal selected from 2D Goal Pose. The path does not consider obstacles and the robot will remain stationary.
If there are runtime errors, examine the output of the terminal and fix the errors.
In any case, use Ctrl+C on the terminal to stop the simulation. To properly kill Gazebo,
```
pkill -9 ruby
```

FilesExpand file tree

03_ROS_System.md

Latest commit

History

03_ROS_System.md

File metadata and controls

3 ROS System

Table of Contents

1 The Project Design

1.1 Nodes

1.2 Topics

1.3 Callbacks

1.4 Timer

2 Python and C++ Templates

2.1 Subscriber Template in Node Constructor

2.2 Publisher Template in Node Constructor

2.3 Subscriber Callback

3 Behavior Node

3.1 Relevant File

3.2 Initialize Goal Pose Subscriber

3.3 Initialize Odometry Subscriber

3.4 Initialize Path Request Publisher

3.5 Program Goal Pose Subscriber Callback

3.6 Program Odometry Subscriber Callback

3.7 Publish Path Request

4 Controller Node

4.1 Relevant File

4.2 Initialize Path Subscriber

4.3 Initialize Odometry Subscriber

4.4 Initialize Command Velocity Publisher

4.5 Initialize Lookahead Point Publisher

4.6 Program Path Subscriber Callback

4.7 Program Odometry Subscriber Callback

4.8 Publish Command Velocity

4.9 Publish Lookahead Point

5 Planner Node

5.1 Relevant File

5.2 Initialize Global Costmap Subscriber

5.3 Initialize Path Request Subscriber

5.4 Initialize Path Publisher

5.5 Program Path Request Subscriber Callback

5.6 Program Global Costmap Subscriber Callback

5.7 Publish Path

6 Building

7 Testing