Section 1 ROS Nodes in Cpp - ika-rwth-aachen/acdc GitHub Wiki

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

• Contents
• Introduction to C++
• Code elements of a ROS node in C++
  • Code outside functions
    • Includes
    • Publisher and subscriber objects
    • Vehicle controller object
  • Callback function of the subscriber
    • Execution of control algorithms at VehicleController interface
    • Conversion of native C++ variables to ROS messages
    • Publish commands to topic
  • Main function
    • Initialize ROS node
    • Access the ROS parameter server
    • Log messages
    • Allocate dynamic memory
    • Configure publisher and subscriber
    • Event loop
• Task 1
• Task 2
• Wrap-up

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!

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.

#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

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.

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.

/**
 * @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).

  // 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

  // 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 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.

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.

  // 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

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.

  // 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.

  // 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.

  // 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.

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.

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.

Hints:

• the input message msg includes fiveLidar measurement msg->ranges[0], ..., msg->ranges[4]
• you can directly use the ROS function ROS_INFO

Do not forget to save your source files and build your source code after making changes:

catkin build

• You learned basic concepts of C++ ROS nodes.
• You learned how to setup a ROS subscriber and publisher.
• You learned how to modify the spinning event-loop and how to add logging messages.