Creating nodes

Chapter description

In this chapter, you will create your first ROS program. You will use what you learned in the previous chapter about starting the camera. First, you'll learn how to create a workspace. Then you'll create your first node and learn how to build and run it. Based on the original code you will learn how to create with publisher, subscriber, serviceClient and serviceServer.

You can run this tutorial on:

Repository containing the final effect after doing all the ROS Tutorials you can find here

Workspace setup

To begin developing your own nodes, you need to do some workspace configuration first. Workspace is the place where all your source files, libraries and compiled nodes will be stored.

First you need to create a folder, where your workspace will be located. You can do this by typing in:

husarion@husarion:~$

mkdir -p ~/ros_ws/src

This will create folder named ros_ws and folder src inside it. All of the packages with source files will be stored in folder src.

Creating new package

As you should already know, in ROS, nodes are distributed in packages, so in order to create a node you need to create a package. Packages are created with command catkin_create_pkg and it must be executed in src folder in your workspace.

Syntax of catkin_create_pkg is:

catkin_create_pkg package_name [required packages]

where package_name is desired package name and argument required packages is optional and contain names of packages that are used by newly created packages.

For our tutorial we will create package named tutorial_pkg which depends on package roscpp. Package roscpp is a basic ROS library for C++.

husarion@husarion:~$
cd ~/ros_ws/src 
catkin_create_pkg tutorial_pkg roscpp

After typing in this command you should get output like this:

This will create folder named tutorial_pkg and some files in it. Your workspace file structure should now look like like below:

Created files are:

CMakeLists.txt - these are build instructions for your nodes, you need to edit this file if you want to compile any node, we will do it later.
package.xml - this file contains package metadata like author, description, version or required packages. Package can be built without changing it, but you should adjust this file if you want to publish your package to others.

Write your first node

Now that we have a workspace created and our own package inside that workspace, we can create our first source file.

Code of your first node

Let’s create C++ file for your node, name it my_first_node.cpp and place it in src folder under tutorial_pkg:

husarion@husarion:~$

touch ~/ros_ws/src/tutorial_pkg/src/my_first_node.cpp

Open file in your favourite text editor and paste:

~/ros_ws/src/tutorial_pkg/src/my_first_node.cpp
#include <ros/ros.h>

int main(int argc, char **argv)
{
    ros::init(argc, argv, "my_first_node");
    ros::NodeHandle n("~");
    ros::Rate loop_rate(30);
    
    ROS_INFO("Node started!");
    while (ros::ok())
    {
        ros::spinOnce();
        loop_rate.sleep();
    }
}

The code explained

#include <ros/ros.h>

Add header files for basic ROS libraries.

int main(int argc, char **argv) {

Beginning of node main function.

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

Initialization of ROS node, this function contacts with ROS master and registers node in the system.

ros::NodeHandle n("~");

Get the handle for node, this handle is required for interactions with system e.g. subscribing to topic.

ros::Rate loop_rate(30);

Define rate for repeatable operations.

ROS_INFO("Node started!");

ROS_INFO and similar logging functions are our replacement for printf/cout. It's part of rosconsole, which supports hierarchical loggers, verbosity levels and configuration-files.

while (ros::ok()) {

Check if ROS is working. E.g. if ROS master is stopped or there was sent signal to stop the system, ros::ok() will return false.

ros::spinOnce();

Process all incoming messages.

loop_rate.sleep();

Wait until defined time passes.

You can save the C++ file.

Building your node

Before you build the node, you need to edit CMakeLists.txt from tutorial_pkg directory. Open file in your favourite text editor and paste:

cmake_minimum_required(VERSION 2.8.3)
project(tutorial_pkg)

add_compile_options(-std=c++11)

find_package(catkin REQUIRED COMPONENTS
  roscpp
)

catkin_package(
  CATKIN_DEPENDS
)

include_directories(${catkin_INCLUDE_DIRS})

add_executable(my_first_node src/my_first_node.cpp)

target_link_libraries(my_first_node
  ${catkin_LIBRARIES}
)

The code explained

add_compile_options(-std=c++11)

This will allow to use C++11 standard of C++.

add_executable(my_first_node src/my_first_node.cpp)

This tells the compiler that it should create an executable file named my_first_node from source src/my_first_node.cpp.

target_link_libraries(my_first_node
  ${catkin_LIBRARIES}
)

This will cause compiler to link libraries required by your node. Save the changes and close editor.

Build code

Now you can move to your workspace main directory and compile it. Open terminal, move to workspace main directory and build your project with command catkin_make:

husarion@husarion:~$
cd ~/ros_ws 
catkin_make

After this command you should get similar output.

After this operation you should have two new folders in your workspace: build for storing files that are used during compilation and devel for storing output files.

note

Remember to build your package every time when you make a changes.

Running your node

Sourcing files

Your node is built and ready for running, but before you run it, you need to load some environment variables:

husarion@husarion:~$

source ~/ros_ws/devel/setup.sh

These environment variables allow you to run node regardless of directory you are working in. You have to load it every time you open new terminal, so it's recommended to add following line to your .bashrc file, to automaticly load your ROS workspace.

husarion@husarion:~$
echo 'source ~/ros_ws/devel/setup.sh' >> ~/.bashrc

Running node - rosrun

To run the code, we execute the rosrun command we learned in the previous chapter.

Task 1

Run your node with rosrun command. Then use rosnode to examine system and check if your node is visible in the system. Remember that new created node is named tutorial_pkg.

Hint

As a reminder, you can start ROS node by typing:

rosrun package_name node_type [options]

After proper startup, you should see text Node started! and you should be able to see new node with rosnode tool. For more details go to rosrun and examining nodes and see prepared examples.

Running node - roslaunch

Now lets execute our node with the help of roslauch. If you want to use .launch files associated with your custom package you will have to create launch file in launch directory. To do this you can type:

husarion@husarion:~$
mkdir ~/ros_ws/src/tutorial_pkg/launch
nano ~/ros_ws/src/tutorial_pkg/launch/my_launch_file.launch

A simple .launch file to run only one my_first_node might look like this:

~/ros_ws/src/tutorial_pkg/launch/my_launch_file.launch
<launch>

    <node pkg="tutorial_pkg" type="my_first_node" name="my_first_node" output="screen"/>

</launch>

Task 2

Run your node using .launch file. Then check if your node is visible in rqt_graph tool.

Hint

As a reminder, the `roslaunch` command is analogous to the `rosrun` command.

roslaunch package_name launch_file.launch [options]

For more details go to roslaunch and examining system with rqt_graph and see prepared examples.

Create Subscriber

As you can see, your first knot is pretty boring. It does nothing but output one line of text to the terminal. Let's modify previous code to subscribe image from /camera/color/image_raw topic and calculate average brightness of recieved image.

Open again previous my_first_node.cpp

~/ros_ws/src/tutorial_pkg/src/my_first_node.cpp
#include <ros/ros.h>
#include <sensor_msgs/Image.h>

void imageCallback(const sensor_msgs::ImageConstPtr &image)
{
    long long sum = 0;
    for (int value : image->data)
    {
        sum += value;
    }
    int avg = sum / image->data.size();
    ROS_INFO("Brightness: %d", avg);
}

int main(int argc, char **argv)
{
   ros::init(argc, argv, "my_first_node");
   ros::NodeHandle n("~");
   ros::Subscriber sub = n.subscribe("/camera/color/image_raw", 10, imageCallback);
   ros::Rate loop_rate(30);

   ROS_INFO("Node started!");
   while (ros::ok())
   {
      ros::spinOnce();
      loop_rate.sleep();
   }
}

The code explained

#include <sensor_msgs/Image.h>

To process message received from the camera you need a header file with sensor_msgs/Image message type definition. Image message is an object consisting of following fields:

std_msgs/Header header - header with message metedata
uint32 height - image height in pixels
uint32 width - image width in pixels
string encoding - pixel encoding definition
uint8 is_bigendian - is data expressed in bigendian manner
uint32 step - length of data for one row
std::vector<uint8_t> data - actual image data

void imageCallback(const sensor_msgs::ImageConstPtr &image)
{
   ...
}

Then you need a function for processing received message (argument is pointer to incoming message).

long long sum = 0;

Variable for storing sum of all pixel values.

for (int value : image->data)
{
   sum += value;
}

Iteration through every pixel and colour and add current pixel value to sum.

int avg = sum/image->data.size();

Calculate average value.

ROS_INFO("Brightness: %d", avg);

Print brightness value to screen.

ros::Subscriber sub = n.subscribe("/camera/color/image_raw", 10, imageCallback);

Last thing to do is defining topic to subscribe. Here we use method subscribe of NodeHandle object. Arguments of method are:

/camera/color/image_raw - name of topic to subscribe.
10 - message queue size. Messages are processed in order they come in. In the case that node receives, in short time, more messages than this value, excessive messages will be dropped.
imageCallback - function to process incoming messages.

Running your code

Now you can build and run this node along with astra.launch or Gazebo simulator rosbot.launch.

You can also edit the lauch file to make easier to run. Let's copy following code and place it in my_launch_file.launch.

~/ros_ws/src/tutorial_pkg/launch/my_launch_file.launch
<launch>

   <arg name="use_gazebo" default="false"/>

   <include unless="$(arg use_gazebo)" file="$(find astra_camera)/launch/astra.launch"/>
   <include if="$(arg use_gazebo)" file="$(find rosbot_bringup)/launch/rosbot_tutorial.launch"/>

   <node pkg="tutorial_pkg" type="my_first_node" name="my_first_node" output="screen"/>

</launch>

Save above file and launch it:

ROSbot
Gazebo simulator

husarion@husarion:~$

roslaunch tutorial_pkg my_launch_file.launch

user@mylaptop:~$
roslaunch tutorial_pkg my_launch_file.launch use_gazebo:=true

After running this launcher in the terminal, you should see the brightness information. Cover the camera and see if the information has changed.

Parameters

Your node can receive parameters, these are used to customize the behavior of the node, e.g. subscribed topic name, device name or baud rate for the serial port. In the example below, you will modify the node to receive a boolean parameter that specifies whether the node should print the brightness of the image to the screen or not.

~/ros_ws/src/tutorial_pkg/src/my_first_node.cpp
#include <ros/ros.h>
#include <sensor_msgs/Image.h>

bool print_b;

void imageCallback(const sensor_msgs::ImageConstPtr &image)
{
   long long sum = 0;
   for (int value : image->data)
   {
      sum += value;
   }
   int avg = sum / image->data.size();
   if (print_b)
   {
      std::cout << "Brightness: " << avg << std::endl;
   }
}

int main(int argc, char **argv)
{
   ros::init(argc, argv, "my_first_node");
   ros::NodeHandle n("~");
   ros::Subscriber sub = n.subscribe("/camera/color/image_raw", 10, imageCallback);
   n.param<bool>("print_brightness", print_b, false);
   ros::Rate loop_rate(30);

   ROS_INFO("Node started!");
   while (ros::ok())
   {
      ros::spinOnce();
      loop_rate.sleep();
   }
}

The code explained

bool print_b;

To receive the parameter you need a variable to store its value, in this example variable should have a global scope:

n.param<bool>("print_brightness", print_b, false);

Receive parameter value is done using functions param of NodeHandle object. Arguments of method are:

print_brightness - name of parameter to receive.
print_b - variable to store parameter value.
false - parameter default value.

if (print_b)
{
   std::cout << "Brightness: " << avg << std::endl;
}

The last thing is to set the print depending on the parameter value.

Task 3

Run your node with parameter print_brightness set to false and again set to true.

Hint

For rosrun add option:

_param:=value

For roslaunch add parameter for node, you will need to add <param> tag inside <node> tag in my_first_node.launch file.

<param name="name" value="value"/>

For more details go to rosrun setting parameters and roslauch setting parameter.

Properly created program should work in an intuitive way.

Create publisher

Now let's modify node to publish brightness value to a new topic with message of type std_msgs/UInt8.

~/ros_ws/src/tutorial_pkg/src/my_first_node.cpp
#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <std_msgs/UInt8.h>

bool print_b;
ros::Publisher brightness_pub;

void imageCallback(const sensor_msgs::ImageConstPtr &image)
{
   long long sum = 0;
   for (int value : image->data)
   {
      sum += value;
   }
   int avg = sum / image->data.size();
   if (print_b)
   {
      std::cout << "Brightness: " << avg << std::endl;
   }
   std_msgs::UInt8 brightness_value;
   brightness_value.data = avg;
   brightness_pub.publish(brightness_value);
}

int main(int argc, char **argv)
{
   ros::init(argc, argv, "my_first_node");
   ros::NodeHandle n("~");
   ros::Subscriber sub = n.subscribe("/camera/color/image_raw", 10, imageCallback);
   n.param<bool>("print_brightness", print_b, false);
   brightness_pub = n.advertise<std_msgs::UInt8>("brightness", 1);
   ros::Rate loop_rate(30);

   ROS_INFO("Node started!");
   while (ros::ok())
   {
      ros::spinOnce();
      loop_rate.sleep();
   }
}

The code explained

#include <std_msgs/UInt8.h>

Add header file to obtain new msg type std_msgs/UInt8. Message std_msgs/UInt8 is object with only one field:

uint8_t data - contain from 0 to 255 number data.

ros::Publisher brightness_pub;

Next define publisher object with global scope:

brightness_pub = n.advertise<std_msgs::UInt8>("brightness" , 1);

Then register in the system to publish to a specific topic. Here we use method advertise of NodeHandle object. Arguments of method are:

brightness - topic name.
1 - message queue size.

You also need to declare type of message which will be published, in this case it is std_msgs::UInt8.

std_msgs::UInt8 brightness_value;
brightness_value.data=avg;
brightness_pub.publish(brightness_value);

Last thing is to put some data into message and send it to topic for each recieved image.

Task 4

Build your node and run it along with camera node. Use rostopic tools to read brightness of the image from the camera.

Hint

As a reminder, you should type:

rostopic echo [topic_name]   

For more details go to rostopic .

Calling the service

The next functionality will be calling the service. A service provider we will use image_saver node from already know image_view package. image_saver have one service named save. every time it is called, one frame from subscribed image topic is saved to hard drive. Desired node behaviour is to count incoming frames and call service once per given number of frames.

Let's add new line into code.

~/ros_ws/src/tutorial_pkg/src/my_first_node.cpp
#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <std_msgs/UInt8.h>
#include <std_srvs/Empty.h>

bool print_b;
ros::Publisher brightness_pub;
int frames_passed = 0;

void imageCallback(const sensor_msgs::ImageConstPtr &image)
{
   long long sum = 0;
   for (int value : image->data)
   {
      sum += value;
   }
   int avg = sum / image->data.size();
   if (print_b)
   {
      std::cout << "Brightness: " << avg << std::endl;
   }
   std_msgs::UInt8 brightness_value;
   brightness_value.data = avg;
   brightness_pub.publish(brightness_value);
   frames_passed++;
}

int main(int argc, char **argv)
{
   ros::init(argc, argv, "my_first_node");
   ros::NodeHandle n("~");
   ros::Subscriber sub = n.subscribe("/camera/color/image_raw", 10, imageCallback);
   n.param<bool>("print_brightness", print_b, false);
   brightness_pub = n.advertise<std_msgs::UInt8>("brightness", 1);
   ros::ServiceClient client = n.serviceClient<std_srvs::Empty>("/image_saver/save");
   std_srvs::Empty srv;
   ros::Rate loop_rate(30);

   ROS_INFO("Node started!");
   while (ros::ok())
   {
      ros::spinOnce();
      if (frames_passed > 100)
      {
         client.call(srv);
         frames_passed = 0;
      }
      loop_rate.sleep();
   }
}

The code explained

#include <std_srvs/Empty.h>

Begin with importing required header for std_srvs/Empty message type. This type of message has no field and can not carry any data, it can be used only for invoking action in another node and getting reply when its done.

int frames_passed = 0;

We need one variable for counting passed frames.

frames_passed++;

In imageCallback function increment counter with every incomingmessage.

ros::ServiceClient client = n.serviceClient<std_srvs::Empty>("/image_saver/save");

Create a client which will be caling to service. Here we use method serviceClient of NodeHandle object. Method has only one argument

name of service.

You also need to determine message type for service: std_srvs::Empty.

std_srvs::Empty srv;

Instantiate message object.

if (frames_passed > 100)
{
   client.call(srv);
   frames_passed = 0;
}

Check if required number of frames passed, call the service and reset counter.

Running your code

After building your node we can modify a my_launch_file.launch to run also image_saver node.

~/ros_ws/src/tutorial_pkg/launch/my_launch_file.launch
<launch>

    <arg name="use_gazebo" default="false"/>

    <include unless="$(arg use_gazebo)" file="$(find astra_camera)/launch/astra.launch"/>
    <include if="$(arg use_gazebo)" file="$(find rosbot_bringup)/launch/rosbot_tutorial.launch"/>

    <include file="$(find tutorial_pkg)/launch/image_saver.launch"/>

    <node pkg="tutorial_pkg" type="my_first_node" name="my_first_node" output="screen"/>

    <node pkg="image_view" type="image_saver" name="image_saver">
        <param name="save_all_image" value="false" />
        <param name="filename_format" value="$(env HOME)/ros_ws/image%04d.%s"/>
        <remap from="/image" to="/camera/color/image_raw"/>
    </node>

</launch>

note

Note the filename_format parameter, which defines where the images will be saved.

After launching this file you can observe as new frames are being saved to your workspace directory.

To delete image files created by this example run following command in your ros_ws directory.

husarion@husarion:~/ros_ws$
rm $(find image*)

Providing the service

The final modification in the node will be the provision of service. Service will return information regarding how many images were saved.

~/ros_ws/src/tutorial_pkg/src/my_first_node.cpp

#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <std_msgs/UInt8.h>
#include <std_srvs/Empty.h>
#include <std_srvs/Trigger.h>

bool print_b;
ros::Publisher brightness_pub;
int frames_passed = 0;
int saved_imgs = 0;

void imageCallback(const sensor_msgs::ImageConstPtr &image)
{
   long long sum = 0;
   for (int value : image->data)
   {
      sum += value;
   }
   int avg = sum / image->data.size();
   if (print_b)
   {
      std::cout << "Brightness: " << avg << std::endl;
   }
   std_msgs::UInt8 brightness_value();
   brightness_value.data = avg;
   brightness_pub.publish(brightness_value);
   frames_passed++;
}

bool saved_img(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
{
   res.success = 1;
   res.message = "Saved images: " + std::to_string(saved_imgs);
   return true;
}

int main(int argc, char **argv)
{
   ros::init(argc, argv, "my_first_node");
   ros::NodeHandle n("~");
   ros::Subscriber sub = n.subscribe("/camera/color/image_raw", 10, imageCallback);
   n.param<bool>("print_brightness", print_b, false);
   brightness_pub = n.advertise<std_msgs::UInt8>("brightness", 1);
   ros::ServiceClient client = n.serviceClient<std_srvs::Empty>("/image_saver/save");
   std_srvs::Empty srv;
   ros::ServiceServer service = n.advertiseService("saved_images", saved_img);
   ros::Rate loop_rate(30);

   ROS_INFO("Node started!");
   while (ros::ok())
   {
      ros::spinOnce();
      if (frames_passed > 100)
      {
         client.call(srv);
         frames_passed = 0;
         saved_imgs++;
      }
      loop_rate.sleep();
   }
}

#include <std_srvs/Trigger.h>

Start with including required header for std_srvs/Trigger message type. It has no field for request and two fields for response:

bool success - indicate successful run of triggered service
string message - informational

int saved_imgs = 0;

Variable for storing number of saved images:

bool saved_img(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
{
   res.success = 1;
   res.message = "Saved images: " + std::to_string(saved_imgs);
   return true;
}

Function to execute when service is called. Inside the function, we complete the response fields according to the std_srvs/Trigger message. Arguments for this function are pointers to request and response data. All services are called the same way, even if it does not carry any data, in that case these are pointer of void type.

return true;

The service returns true when it is complete.

saved_imgs++;

Increment image counter after saving frame.

ros::ServiceServer service = n.advertiseService("saved_images", saved_img);

Last thing to do is to register provided service in the system. Here we use method advertiseService of NodeHandle object. Arguments of method are:

service name ("saved_images").
method to execute (saved_img).

Running your code

Build your node and run my_launch_file.launch. Then call service using rosservice call command. Usage of rosservice is analogical to rostopic. To call service type

husarion@husarion:~$

rosservice call /my_first_node/saved_images

As a response you should get something like this:

Message types

In ROS there are many message types defined, they are grouped in packages accordingly to their application. Some of the most used are:

std_msgs - messages with basic data types like integer or float
geometry_msgs - for handling information regarding object position, orientation, velocity or acceleration, also for defining points or polygons
nav_msgs - for handling maps or robot localization data
sensor_msgs - for handling data from sensors

Each of these packages contains many types of messages, which have their uses in different applications. While developing nodes for mobile robots you will probably you will encounter some of this messages:

geometry_msgs/Twist - message for defining linear and angular object velocity. Could be also used for setting desired velocities.
nav_msgs/OccupancyGrid - message for handling flat occupancy grid based maps.
nav_msgs/Odometry - message for defining robot position and orientation based on odometry measurements.
nav_msgs/Path - message for defining robot path as a set of positions
sensor_msgs/JointState - it has fields for determining position , velocity and effort for a set of joints.
sensor_msgs/LaserScan - message for handling scans of planar laser scanners like RpLidar or Hokuyo.
sensor_msgs/Range - message for one dimensional distance measurement

It is also possible to implement user defined messages if existing ones are not sufficient.

Summary

After completing this tutorial you should be able to write code for your own node, build and run it in the same way as it can be done with nodes provided with ROS.

Your node may be configured with parameters and it can:

subscribe topics published by other nodes
publish new topic
provide service
call service provided by other nodes

by Łukasz Mitka & Rafał Górecki, Husarion

Do you need any support with completing this tutorial or have any difficulties with software or hardware? Feel free to describe your thoughts on our community forum: https://community.husarion.com/ or to contact with our support: support@husarion.com

Chapter description​

Workspace setup​

Write your first node​

Code of your first node​

Building your node​

Build code​

Running your node​

Sourcing files​

Running node - rosrun​

Running node - roslaunch​

Create Subscriber​

Parameters​

Create publisher​

Calling the service​

Providing the service​

Message types​

Summary​

Chapter description

Workspace setup

Write your first node

Code of your first node

Building your node

Build code

Running your node

Sourcing files

Running node - rosrun

Running node - roslaunch

Create Subscriber

Parameters

Create publisher

Calling the service

Providing the service

Message types

Summary