ROS2 编写Service和Client

https://blog.csdn.net/wei242425445/article/details/115278114
按照上文的描述编写自定义消息后，我们就可以开始编写服务了

创建包

$ ros2 pkg create --build-type ament_cmake cpp_srvcli

编写service

具体代码如下

#include "rclcpp/rclcpp.hpp"
#include "message_interfaces/srv/add.hpp"

#include <memory>

void add(const std::shared_ptr<message_interfaces::srv::Add::Request> request,
          std::shared_ptr<message_interfaces::srv::Add::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_server");

  rclcpp::Service<message_interfaces::srv::Add>::SharedPtr service =
    node->create_service<message_interfaces::srv::Add>("add", &add);

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

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

编写Client

具体代码如下

#include "rclcpp/rclcpp.hpp"
#include "message_interfaces/srv/add.hpp"

#include <chrono>
#include <cstdlib>
#include <memory>

using namespace std::chrono_literals;

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

  if (argc != 3) {
      RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "usage: add X Y");
      return 1;
  }

  std::shared_ptr<rclcpp::Node> node = rclcpp::Node::make_shared("client");
  rclcpp::Client<message_interfaces::srv::Add>::SharedPtr client =
    node->create_client<message_interfaces::srv::Add>("add");

  auto request = std::make_shared<message_interfaces::srv::Add::Request>();
  request->a = atoll(argv[1]);
  request->b = atoll(argv[2]);

  while (!client->wait_for_service(1s)) {
    if (!rclcpp::ok()) {
      RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Interrupted while waiting for the service. Exiting.");
      return 0;
    }
    RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "service not available, waiting again...");
  }

  auto result = client->async_send_request(request);
  // Wait for the result.
  if (rclcpp::spin_until_future_complete(node, result) ==
    rclcpp::FutureReturnCode::SUCCESS)
  {
    RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Sum: %ld", result.get()->sum);
  } else {
    RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Failed to call service add_two_ints");
  }

  rclcpp::shutdown();
  return 0;
}

修改CMakeLists.txt

修改的代码如下

# 查找依赖
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)
find_package(message_interfaces REQUIRED)

# 生成add_server的可执行文件
add_executable(add_server src/add_server.cpp)
ament_target_dependencies(add_server rclcpp std_msgs message_interfaces)

# 生成add的可执行文件
add_executable(add src/client.cpp)
ament_target_dependencies(add rclcpp std_msgs message_interfaces)

# 安装
install(TARGETS
  add_server
  add
  DESTINATION lib/${PROJECT_NAME})

修改package.xml文件

修改的代码如下

  <buildtool_depend>ament_cmake</buildtool_depend>

  <depend>rclcpp</depend>
  <depend>std_msgs</depend>
  <depend>message_interfaces</depend>

  <test_depend>ament_lint_auto</test_depend>
  <test_depend>ament_lint_common</test_depend>

编译

使用colcon build命令编译

~/code/ros-demo
$ colcon build --packages-select cpp_srvcli
Starting >>> cpp_srvcli
Finished <<< cpp_srvcli [0.22s]                     

Summary: 1 package finished [0.35s]

运行service

~/code/ros-demo$ . install/setup.bash
~/code/ros-demo$ ros2 run cpp_srvcli add_server 
[INFO] [1616912707.268216614] [rclcpp]: Ready to add two ints.
[INFO] [1616912746.016196170] [rclcpp]: Incoming request
a: 1 b: 2
[INFO] [1616912746.016278798] [rclcpp]: sending back response: [3]

运行client

~/code/ros-demo$ . install/setup.bash
~/code/ros-demo$ ros2 run cpp_srvcli add 1 2 
[INFO] [1616912746.017254917] [rclcpp]: Sum: 3