First, we are going to learn how to create a service and how to use it in ROS. Our service will calculate the sum of three numbers. We need two nodes: a server and a client.
In the chapter2_tutorials package, create two new nodes with the following names: example2_a.cpp and example2_b.cpp. Remember to put the files in thesrc folder.
In the first file, example2_a.cpp, add the following code:
#include "ros/ros.h" #include "chapter2_tutorials/chapter2_srv1.h" bool add(chapter2_tutorials::chapter2_srv1::Request &req, chapter2_tutorials::chapter2_srv1::Response &res) { res.sum = req.A + req.B + req.C; ROS_INFO("request: A=%ld, B=%ld C=%ld", (int)req.A, (int)req.B,
(int)req.C); ROS_INFO("sending back response: [%ld]", (int)res.sum); return true; } int main(int argc, char **argv) { ros::init(argc, argv, "add_3_ints_server"); ros::NodeHandle n; ros::ServiceServer service = n.advertiseService("add_3_ints",
add); ROS_INFO("Ready to add 3 ints."); ros::spin(); return 0; }
Let's explain the code. These lines include the necessary headers and the srv file that we created:
#include "ros/ros.h" #include "chapter2_tutorials/chapter2_srv1.h"
This function will add three variables and send the result to the other node:
bool add(chapter2_tutorials::chapter2_srv1::Request &req, chapter2_tutorials::chapter2_srv1::Response &res)
Here, the service is created and advertised over ROS:
ros::ServiceServer service = n.advertiseService("add_3_ints",
add);
In the second file, example2_b.cpp, add this code:
#include "ros/ros.h" #include "chapter2_tutorials/chapter2_srv1.h" #include <cstdlib> int main(int argc, char **argv) { ros::init(argc, argv, "add_3_ints_client"); if (argc != 4) { ROS_INFO("usage: add_3_ints_client A B C "); return 1; } ros::NodeHandle n; ros::ServiceClient client =
n.serviceClient<chapter2_tutorials::chapter2_srv1>("add_3_ints"); chapter2_tutorials::chapter2_srv1 srv; srv.request.A = atoll(argv[1]); srv.request.B = atoll(argv[2]); srv.request.C = atoll(argv[3]); if (client.call(srv)) { ROS_INFO("Sum: %ld", (long int)srv.response.sum); } else { ROS_ERROR("Failed to call service add_3_ints"); return 1; } return 0; }
As usual, let's explain the code. Create a client for the service with the nameadd_3_ints:
ros::ServiceClient client =
n.serviceClient<chapter2_tutorials::chapter2_srv1>("add_3_ints");
Here, we create an instance of our srv request type and fill all the values to be sent.
If you remember, the message has three fields:
chapter2_tutorials::chapter2_srv1 srv; srv.request.A = atoll(argv[1]); srv.request.B = atoll(argv[2]); srv.request.C = atoll(argv[3]);
With this line, the service is called and the data is sent. If the call succeeds, call() will return true, and if not, call() will return false:
if (client.call(srv))
To build the new nodes, edit CMakeList.txt and add the following lines:
add_executable(example2_a src/example2_a.cpp) add_executable(example2_b src/example2_b.cpp) add_dependencies(example2_a
chapter2_tutorials_generate_messages_cpp)add_dependencies(example2_b
chapter2_tutorials_generate_messages_cpp) target_link_libraries(example2_a ${catkin_LIBRARIES}) target_link_libraries(example2_b ${catkin_LIBRARIES})
Now execute the following command:
$ cd ~/dev/catkin_ws $ catkin_make
To start the nodes, execute the following command lines:
$ rosrun chapter2_tutorials example2_a $ rosrun chapter2_tutorials example2_b 1 2 3
You should see something similar to this output:
Node example2_a [ INFO] [1355256113.014539262]: Ready to add 3 ints. [ INFO] [1355256115.792442091]: request: A=1, B=2 C=3 [ INFO] [1355256115.792607196]: sending back response: [6] Node example2_b [ INFO] [1355256115.794134975]: Sum: 6
Now we are going to create nodes with our custom msg file. The example is the same, that is, example1_a.cpp and example1_b.cpp, but with the new message, chapter2_msg1.msg.
The following code snippet is present in the example3_a.cpp file:
#include "ros/ros.h" #include "chapter2_tutorials/chapter2_msg1.h" #include <sstream> int main(int argc, char **argv) { ros::init(argc, argv, "example3_a"); ros::NodeHandle n; ros::Publisher pub =
n.advertise<chapter2_tutorials::chapter2_msg1>("message", 1000); ros::Rate loop_rate(10); while (ros::ok()) { chapter2_tutorials::chapter2_msg1 msg; msg.A = 1; msg.B = 2; msg.C = 3; pub.publish(msg); ros::spinOnce(); loop_rate.sleep(); } return 0; }
The following code snippet is present in the example3_b.cpp file:
#include "ros/ros.h" #include "chapter2_tutorials/chapter2_msg1.h" void messageCallback(const
chapter2_tutorials::chapter2_msg1::ConstPtr& msg) { ROS_INFO("I heard: [%d] [%d] [%d]", msg->A, msg->B, msg->C); } int main(int argc, char **argv) { ros::init(argc, argv, "example3_b"); ros::NodeHandle n; ros::Subscriber sub = n.subscribe("message", 1000,
messageCallback); ros::spin(); return 0; }
If we run both nodes now, we will see something similar to the following output:
... [ INFO] [1355270835.920368620]: I heard: [1] [2] [3] [ INFO] [1355270836.020326372]: I heard: [1] [2] [3] [ INFO] [1355270836.120367449]: I heard: [1] [2] [3] [ INFO] [1355270836.220266466]: I heard: [1] [2] [3] ...