Writing a Service Server (C++)

Create a package

First of all, you will have to create a new package, where you will place the code for your Publisher node. For this, you need to be in your ROS2 workspace src folder.

cd ~/ros2_ws/src

Now, execute the following command to create the package:

ros2 pkg create --build-type ament_cmake service_server_cpp --dependencies rclcpp example_interfaces

You will get an output similar to this one:

This will automatically generate your package and all its necessary files and folders. Now, navigate to the ~/ros2_ws/src folder in order to create your program file.

Write the code

Inside the ros2_ws/src folder, create a new file named service_server.cpp. Into this file, copy the following code:

#include "rclcpp/rclcpp.hpp"
#include "example_interfaces/srv/add_two_ints.hpp"

#include <memory>

void add(const std::shared_ptr<example_interfaces::srv::AddTwoInts::Request> request,
std::shared_ptr<example_interfaces::srv::AddTwoInts::Response>      response)
{
response->sum = request->a + request->b;
RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Incoming request\na: %ld" " b: %ld",
request->a, request->b);
RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "sending back response: [%ld]", (long int)response->sum);
}

int main(int argc, char **argv)
{
rclcpp::init(argc, argv);

std::shared_ptr<rclcpp::Node> node = rclcpp::Node::make_shared("add_two_ints_server");

rclcpp::Service<example_interfaces::srv::AddTwoInts>::SharedPtr service =
node->create_service<example_interfaces::srv::AddTwoInts>("add_two_ints", &add);

RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Ready to add two ints.");

rclcpp::spin(node);
rclcpp::shutdown();
}

Review the code

The first two #include statements are your package dependencies.

#include "rclcpp/rclcpp.hpp"
#include "example_interfaces/srv/add_two_ints.hpp"

The add function adds two integers from the request and gives the sum to the response, while notifying the console of its status using logs.

void add(const std::shared_ptr<example_interfaces::srv::AddTwoInts::Request> request,
std::shared_ptr<example_interfaces::srv::AddTwoInts::Response>      response)
{
response->sum = request->a + request->b;
RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Incoming request\na: %ld" " b: %ld",
request->a, request->b);
RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "sending back response: [%ld]", (long int)response->sum);
}

The main function accomplishes the following, line by line:

Initializes ROS 2 C++ client library:

rclcpp::init(argc, argv);

Creates a node named add_two_ints_server:

std::shared_ptr<rclcpp::Node> node = rclcpp::Node::make_shared("add_two_ints_server");

Creates a service named add_two_ints for that node and automatically advertises it over the networks with the &add method:

rclcpp::Service<example_interfaces::srv::AddTwoInts>::SharedPtr service =
node->create_service<example_interfaces::srv::AddTwoInts>("add_two_ints", &add);

Prints a log message when it’s ready:

RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Ready to add two ints.");

Spins the node, making the service available.

rclcpp::spin(node);

Modify the CMakeLists.txt file

In the Build section of your CMakeLists.txt file, add the following lines to your CMakeLists.txt file, right above the ament_package() line.

add_executable(service_server_node src/service_server.cpp)
ament_target_dependencies(service_server_node rclcpp example_interfaces)

install(TARGETS
service_server_node
DESTINATION lib/${PROJECT_NAME}
)

Compile the package

First of all, let's go the ros2_ws folder in order to be able to compile.

cd ~/ros2_ws;

Let's now execute the colcon command in order to build our node.

colcon build --symlink-install

You will get an output like the following one:

Finally, remember to source the workspace.

source ~/ros2_ws/install/setup.bash

Testing the Service Server

Let's run our service server with the following command:

ros2 run service_server_cpp service_server_node

Now, in order to call the service server, let's execute in a different shell the following command:

ros2 service call /add_two_ints example_interfaces/AddTwoInts '{a: 2, b: 3}'

From the Shell where you executed the Server, you will get the following:

And from the Shell where you called the Server, you wll get the following:

Practice Online

Also, you can test this tutorial in ROSDS, using a ROSject which already contains all the code described in it. You can get the ROSject by clicking on the button below:

Above: Get ROSDS ROSject

The link above will take yo a page like the below one:

Now, you just need to Sign In (or Sign Up if you don't have an account yet) to ROSDS in order to launch the ROSject.