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:
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++.
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
:
touch ~/ros_ws/src/tutorial_pkg/src/my_first_node.cpp
Open file in your favourite text editor and paste:
#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
:
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.
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:
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.
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
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:
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:
<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
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
#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 metedatauint32 height
- image height in pixelsuint32 width
- image width in pixelsstring encoding
- pixel encoding definitionuint8 is_bigendian
- is data expressed in bigendian manneruint32 step
- length of data for one rowstd::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
.
<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
roslaunch tutorial_pkg my_launch_file.launch
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.
#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
.
#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
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.
#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.
<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 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.
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.
#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 servicestring 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
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 floatgeometry_msgs
- for handling information regarding object position, orientation, velocity or acceleration, also for defining points or polygonsnav_msgs
- for handling maps or robot localization datasensor_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 positionssensor_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