Section 1 ROS Nodes in Cpp

ROS1

header

This tutorial focuses on how to implement your own ROS node in C++. It is strongly based on the official ROS tutorial about writing a simple publisher and subscriber.

In this exercise you will learn

some basic concepts about C++
how to setup a ROS node with includes, initialization, and NodeHandle
the usage of ROS subscribers, publishers, and callback functions
how to access the ROS parameter server
the usage of the ROS logging functionality
the concept behind ROS spinning

Introduction to C++

It is required to have a solid understanding of a high-level programming language in advance! If you have not used C++ itself before, you should still be able to understand the concepts. Actual programming is limited to small tasks, for which it should be enough to individually look up the C++ syntax.

The C++ links in External Documentation can help you understand the basic concepts of the language. It can be useful to check out C++ tutorials that assume knowledge of another language, like e.g. this C++ tutorial for Python programmers.

The most important differences between C++ and many other languages are (incomplete list):

There exist pointer variables that only contain the memory address of an actual variable or data structure.
- At the declaration of a variable, the * operator makes a variable a pointer variable.
- When using pointer variable, the * operator gives us the contents of the memory location that is pointed to.
You traditionally have to allocate / deallocate dynamic memory yourself, e.g. using new/delete or other, more modern syntax.

Some practical examples of C++ from the the file ~/ws/catkin_workspace/src/workshops/section_1/racing/src/vehicle_controller_node.cpp are:

#include "VehicleController.h"

Include code from another file into your current file, similar to importing a package in Python.

/**
 * @brief Callback function that is automatically triggered when a new Lidar scan is available
 * @param msg A pointer to message object that contains the new Lidar scan
 */

A multi-line comment that is formatted according to Doxygen, which allows automatic generation of a code documentation.

    distances[i] = msg->ranges[i];

The [] operator is used for accessing array elements. The -> operator used to get get the member ranges of a class instance, like e.g. with the dot operator in msg.ranges[i]. However, -> has to be used when msg is only a pointer variable to the class instance and not the instance itself.

ros::spin();

The spin() function in defined within the ros namespace. We can only access it by using the :: operator to enter this namespace.

If you want to write C++ code in an actual ROS project besides university lectures, we strongly recommend learning C++ from scratch!

Code elements of a ROS node in C++

In this section, we look at all ROS-specific code parts of the vehicle controller node, which is implemented in the file ~/ws/catkin_workspace/src/workshops/section_1/racing/src/vehicle_controller_node.cpp.

Open this file in an editor of your choice and locate the following highlighted code snippets to understand their context.

Code outside functions

Includes

#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>
#include <geometry_msgs/Twist.h>

We need to make the ROS-specific code, which is located in the roscpp package, available in our current source file. Therefore, we include:

ros/ros.h, which contains a number of commonly used ROS functions, and
individual message definitions for the messages exchanged between the flatland simulation and this controller node

Publisher and subscriber objects

Usually, it is bad programming style to have global variables outside of functions. In this example, declaring the Publisher globally allows us to access it both from the main function and from the callback function.

ros::Publisher *publisher_actions = nullptr;
ros::Subscriber *subscriber_sensor_data = nullptr;

Here, we declare pointer variables to ros::Publisher and ros::Subscriber objects. Right now, they point to the empty memory location NULL. Later on in the code, they will point to dynamically allocated memory.

Vehicle controller object

VehicleController *vehicle_controller = nullptr;

Declaration of a pointer variable to a VehicleController class object. The actual control algorithm will be implemented inside this VehicleController class, not directly in the present file.

Callback function of the subscriber

/**
 * @brief Callback function that is automatically triggered when a new Lidar scan is available
 * @param msg A pointer to message object that contains the new Lidar scan
 */
void callbackLaserSensor(const sensor_msgs::LaserScanPtr &msg) {
  // function body removed
}

We can implement our custom code inside this function. The automatic execution upon new Lidar scan data only works if

the subscriber object has specified the correct topic to subscribe to, and
the name of this function is registered as the corresponding callback function (covered later).

Execution of control algorithms at VehicleController interface

  // Interface calls to the VehicleController instance
  vehicle_controller->overwriteLidarDistances(distances);
  vehicle_controller->computeTargetValues();
  double linear_velocity = vehicle_controller->getTargetVelocity();
  double steering_angle = vehicle_controller->getTargetSteeringAngle();

The VehicleController class object vehicle_controller (in fact a pointer to the object) provides us public functions to inform it about the latest Lidar Scanner distances, to compute new target values / actuator commands, and to actually access them. We intentionally outsource this functionality into another class to keep this source file modular:

ROS-specific code in vehicle_controller_node.cpp
Algorithm, which is independent of ROS and thus reusable in other frameworks, in VehicleController.h / VehicleController.cpp

Conversion of native C++ variables to ROS messages

  // Convert local variables to a geometry_msgs::Twist message for publishing.
  geometry_msgs::Twist new_action;
  geometry_msgs::Vector3 steering;
  geometry_msgs::Vector3 velocity;
  steering.z = steering_angle;
  velocity.x = linear_velocity;
  new_action.linear = velocity;
  new_action.angular = steering;

We need to convert the target values for the actuators to the agreed message format (geometry_msgs::Twist, like covered before), such that they can be published on the topic later.

Publish commands to topic

  // Publish the newly computed actuator command to the topic
  publisher_actions->publish(new_action);

The publisher_actions object is the same as declared before. It is used to publish the message object new_action of type geometry_msgs::Twist. The topic to publish to is specified down below in the main function.

Main function

Initialize ROS node

ros::init(argc, argv, "vehicle_controller");

You must call the ros::init function before using any other part of the ROS system (from official tutorial). This version of the function takes the command line arguments as counter argc and vector argv, and a string that specifies the name of the node.

ros::NodeHandle node_handle;

From official tutorial: NodeHandle is the main access point to communications with the ROS system. The first NodeHandle constructed will fully initialize this node, and the last NodeHandle destructed will close down the node.

Access the ROS parameter server

  // Declare local variables for subscribe and publish topics
  std::string subscribe_topic_sensors;
  std::string publish_topic_actuators;

  // Write publish and subscribe topics from parameter server into local variables
  node_handle.getParam("vehicle/sensor_topic", subscribe_topic_sensors);
  node_handle.getParam("vehicle/actuator_topic", publish_topic_actuators);

In our launch-file, we set the parameter vehicle/sensor_topic to the topic name that the simulation node and the controller node agree on for communication. Now that we are inside the controller node, we retrieve this topic name such that we can later on tell our subscriber to listen to that topic. Therefore, the NodeHandle object offers us the getParam function:

the first argument specifies the name of the parameter on the ROS parameter server
the second argument provides a local variable which will have the parameter value assigned to it

Log messages

ROS_INFO("Vehicle controller subscribes to: %s", subscribe_topic_sensors.c_str());

These are the log messages that appear as outputs in the terminal or in rqt_console. You can output contents of variables, like e.g. output a string using the %s syntax and the corresponding string variable as an appended argument subscribe_topic_sensors.c_str(). This syntax is similar to the popular printing syntax of C.

Allocate dynamic memory

  // Initialize / allocate dynamic memory
  vehicle_controller = new VehicleController;
  subscriber_sensor_data = new ros::Subscriber;
  publisher_actions = new ros::Publisher;

These pointer variables had been declared earlier, however, only now they get corresponding data structures allocated to point to. In particular, the constructors of VehicleController, ros::Subscriber, and ros::Publisher are called now.

Configure publisher and subscriber

  // Connect subscriber and publisher to their respective topics and callback function
  *subscriber_sensor_data = node_handle.subscribe(subscribe_topic_sensors, 10, callbackLaserSensor);

From official ROS tutorial: The subscribe() call is how you tell ROS that you want to receive messages on a given topic. This invokes a call to the ROS master node, which keeps a registry of who is publishing and who is subscribing. Messages are passed to a callback function, here called callbackLaserSensor. subscribe() returns a Subscriber object that you must hold on to until you want to unsubscribe. [...]

The second parameter to the subscribe() function is the size of the message queue. If messages are arriving faster than they are being processed, this is the number of messages that will be buffered up before beginning to throw away the oldest ones.

  *publisher_actions = node_handle.advertise<geometry_msgs::Twist>(publish_topic_actuators, 10);

From official ROS tutorial: The advertise() function is how you tell ROS that you want to publish on a given topic name. This invokes a call to the ROS master node, which keeps a registry of who is publishing and who is subscribing. After this advertise() call is made, the master node will notify anyone who is trying to subscribe to this topic name, and they will in turn negotiate a peer-to-peer connection with this node. advertise() returns a Publisher object which allows you to publish messages on that topic through a call to publish() [like done at the end of the callbackLaserSensor function].

The second parameter to advertise() is the size of the message queue used for publishing messages. If messages are published more quickly than we can send them, the number here specifies how many messages to buffer up before throwing some away.

Event loop

  // Enter a loop to keep the node running while looking for messages on the subscribed topic [...]
  ros::spin();

The loop inside the spin() can go on for unlimited times, but is typically terminated by a Ctrl+C hit in the corresponding terminal window. You can also implement this behavior manually as a while loop inside the main function.

Task 1

Adjust the ros::spin() event loop in the main function of vehicle_controller_node.cpp code such that incoming messages are looked for exactly every 20ms.

Hints:

Instead of the ros::spin() statement, you could use the ros::spinOnce() function in combination with a while-loop.
Consider the ros::Rate expression for adding a sleeping phase within in the while-loop for exactly 20ms.

Task 2

Improve the log messages within the callbackLaserSensor function of vehicle_controller_node.cpp. Add a log message each time a message msg is actually received and add the measured Lidar distances in that log message.