Just a quick note when I was learning this ROS 2 tutorial, Creating custom ROS 2 msg and srv files. Using C++ template to save time typing all the types and can use lambdas as well.
#include "rclcpp/rclcpp.hpp"
#include "tutorial_interfaces/srv/add_three_ints.hpp"
#include <memory>
#include <tuple>
template <typename T, typename F, typename S = typename rclcpp::Service<T>::SharedPtr>
auto make_service(const char *node_name, const char * service_name, F && func)
-> std::tuple<std::shared_ptr<rclcpp::Node>, S>
{
std::shared_ptr<rclcpp::Node> node = rclcpp::Node::make_shared(node_name);
S service = node->create_service<T>(service_name, std::forward<F>(func));
return std::make_tuple(node, service);
}
int main(int argc, char **argv) {
rclcpp::init(argc, argv);
using Service = tutorial_interfaces::srv::AddThreeInts;
auto [node, service] = make_service<Service>(
"add_three_ints_server", "add_three_ints",
[](const std::shared_ptr<Service::Request> request, std::shared_ptr<Service::Response> response) {
response->sum = request->a + request->b + request->c;
RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Incoming request\na: %ld" " b: %ld" " c: %ld",
request->a, request->b, request->c);
RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "sending back response: [%ld]", (long int)response->sum);
});
RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Ready to add three ints");
rclcpp::spin(node);
rclcpp::shutdown();
}