【ROS】ROS2编程示例：服务和客户端-C++版

【ROS】郭老二博文之：ROS目录

1、准备

1）安装ROS2
【ROS】Ubuntu22.04安装ROS2（Humble Hawksbill）

2）ROS2命令
【ROS】ROS2命令行工具详解

3）配置工作空间
【ROS】ROS2中的概念和名词解释中第一节：工作空间 workspace

4）ROS2源码及官方示例：

https://github.com/ros2
https://github.com/ros2/examples

2、编辑

2.1 创建功能包

1）ros2 pkg create

cd ~/ros/src
mkdir laoerExample
cd laoerExample
ros2 pkg create --build-type ament_cmake cSrvCli --dependencies rclcpp example_interfaces

注意：创建功能包时一定要先创建好目录，进入相应的目录后创建，不要使用下面的方法，否则会在编译时报错

cd ~/ros/src
ros2 pkg create --build-type ament_cmake laoerExample/cSrvCli

2）–dependencies
–dependencies将会自动在package.xml和CMakeLists.txt中添加必要的依赖项
例如：–dependencies rclcpp example_interfaces，将会在package.xml中自动添加：

<depend>rclcppdepend>
<depend>example_interfacesdepend>

CMakeLists.txt中自动添加：

find_package(rclcpp REQUIRED)
find_package(example_interfaces REQUIRED)

3）example_interfaces
example_interfaces功能包是ROS2中自带的功能包，不需要自己创建
功能包目录：/opt/ros/humble/share/example_interfaces
这个博文的示例中将会用到其中的一个服务接口，两个数相加后，将结果返回，接口文件路径及内容如下：

/opt/ros/humble/share/example_interfaces/srv$ cat AddTwoInts.srv 
int64 a
int64 b
---
int64 sum

2.2 编辑源码

2.2.1 服务端

~/ros/src/laoerExample/cSrvCli$ vi srv.cpp

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

#include 

/* 求和函数 */
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：ROS2自带的log输出，分等级，带颜色，输出格式与printf相同，需要标明数据的类型。*/
  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)
{
  /* 初始化ROS2 */
  rclcpp::init(argc, argv);

  /* 定义服务端节点add_two_ints_server */
  std::shared_ptr<rclcpp::Node> node = rclcpp::Node::make_shared("add_two_ints_server");

  /* 创建服务名为add_two_ints，服务函数为add的service服务端 */
  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);

  /* 退出ROS2 */
  rclcpp::shutdown();
}

2.2.2 客户端

~/ros/src/laoerExample/cSrvCli$ vi cli.cpp

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

#include 
#include 
#include 

using namespace std::chrono_literals;

int main(int argc, char **argv)
{
  /* 初始化ROS2 */
  rclcpp::init(argc, argv);

  /* 需要两个命令行参数：x和y */
  if (argc != 3) {
      RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "usage: add_two_ints_client X Y");
      return 1;
  }

  /* 定义客户端节点add_two_ints_client */
  std::shared_ptr<rclcpp::Node> node = rclcpp::Node::make_shared("add_two_ints_client");

  /* 创建服务名为add_two_ints的client客户端 */
  rclcpp::Client<example_interfaces::srv::AddTwoInts>::SharedPtr client =
    node->create_client<example_interfaces::srv::AddTwoInts>("add_two_ints");

  /* 创建请求request */
  auto request = std::make_shared<example_interfaces::srv::AddTwoInts::Request>();
  request->a = atoll(argv[1]);
  request->b = atoll(argv[2]);

  /* 搜索服务节点，间隔1s */
  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");
  }

  /* 退出ROS2 */
  rclcpp::shutdown();
  return 0;
}

2.3 添加依赖配置 package.xml

1）自动添加依赖
在创建功能包时，已经通过参数–dependencies指定了依赖

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

2）手动添加依赖
依赖已自动添加在package.xml中，如果没有则需要手动修改添加即可

<depend>rclcppdepend>
<depend>example_interfacesdepend>

3）完整的依赖 package.xml 如下：

~/ros/src/laoerExample/cSrvCli$ cat package.xml 

<package format="3">
  <name>cSrvCliname>
  <version>0.0.0version>
  <description>TODO: Package descriptiondescription>
  <maintainer email="[email protected]">lesenmaintainer>
  <license>TODO: License declarationlicense>

  <buildtool_depend>ament_cmakebuildtool_depend>

  <depend>rclcppdepend>
  <depend>example_interfacesdepend>

  <test_depend>ament_lint_autotest_depend>
  <test_depend>ament_lint_commontest_depend>

  <export>
    <build_type>ament_cmakebuild_type>
  export>
package>

2.4 修改编译配置

1）依赖相关的编译配置
在创建功能包时，已经通过参数–dependencies指定了依赖，CMakeLists.txt中也会自动添加依赖相关的编译配置，如果没有则手动添加

find_package(rclcpp REQUIRED)
find_package(example_interfaces REQUIRED)

2）添加需要编译的源码文件

add_executable(server src/srv.cpp)
ament_target_dependencies(server rclcpp example_interfaces)

add_executable(client src/cli.cpp)
ament_target_dependencies(client rclcpp example_interfaces)

3）生成安装规则

install(TARGETS
  server
  client
  DESTINATION lib/${PROJECT_NAME})

4）完整 CMakeLists.txt 内容如下：

~/ros/src/laoerExample/cSrvCli$ cat CMakeLists.txt 

cmake_minimum_required(VERSION 3.8)
project(cSrvCli)

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(example_interfaces REQUIRED)

if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  # the following line skips the linter which checks for copyrights
  # comment the line when a copyright and license is added to all source files
  set(ament_cmake_copyright_FOUND TRUE)
  # the following line skips cpplint (only works in a git repo)
  # comment the line when this package is in a git repo and when
  # a copyright and license is added to all source files
  set(ament_cmake_cpplint_FOUND TRUE)
  ament_lint_auto_find_test_dependencies()
endif()

add_executable(server src/srv.cpp)
ament_target_dependencies(server rclcpp example_interfaces)

add_executable(client src/cli.cpp)
ament_target_dependencies(client rclcpp example_interfaces)

install(TARGETS
  server
  client
  DESTINATION lib/${PROJECT_NAME})
  
ament_package()

3、编译

在工作空间根目录中编译：

cd ~/ros
~/ros$ colcon build

4、运行

在终端1中运行服务端：

~/ros$ source install/setup.sh 
~/ros$ ros2 run  cSrvCli server 

在终端2中运行客户端：

~/ros$ source install/setup.sh 
~/ros$ ros2 run cSrvCli client 123 456
[INFO] [1685671664.664459306] [rclcpp]: Sum: 579

注意：运行客户端时，需要传输两个参数值，否则会报错

[INFO] [1685671646.305281649] [rclcpp]: usage: add_two_ints_client X Y
[ros2run]: Process exited with failure 1

终端2输出打印信息：

[INFO] [1685671664.664459306] [rclcpp]: Sum: 579

终端1输出打印信息：

[INFO] [1685671604.946255679] [rclcpp]: Ready to add two ints.
[INFO] [1685671664.664030229] [rclcpp]: Incoming request
a: 123 b: 456
[INFO] [1685671664.664096508] [rclcpp]: sending back response: [579]

5、测试

1）查看节点信息
在终端3中查看节点信息，运行命令：ros2 node list

~$ ros2 node list 
/add_two_ints_server

对应服务端源码中定义服务节点时：

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

2）查看服务信息
在终端3中查看话题信息，运行命令：ros2 service list

~$ ros2 service list
/add_two_ints
/add_two_ints_server/describe_parameters
……
/add_two_ints_server/set_parameters_atomically

对应接口定义：

/opt/ros/humble/share/example_interfaces/srv/AddTwoInts.srv

以及服务端和客户端中定义节点时，使用的接口：

node->create_service("add_two_ints", &add);
node->create_client("add_two_ints");