Robot Operating System——自定义Service/Client通信消息结构

最新推荐文章于 2024-10-01 22:54:40 发布

breaksoftware

最新推荐文章于 2024-10-01 22:54:40 发布

阅读量710

点赞数 31

分类专栏： Robot Operating System 文章标签： linux c++ 机器人

本文链接：https://blog.csdn.net/breaksoftware/article/details/141298403

版权

Robot Operating System 专栏收录该内容

88 篇文章 7 订阅

订阅专栏

大纲

初始化环境
生成自定义服务的工程
使用自定义服务的工程
工程地址
参考资料

在《Robot Operating System——自定义订阅/发布的消息结构》一文中，我们讲解了如何自定义消息结构。这个消息是发布者向订阅者发送的消息，具有单向性。但是Service/Client类型的消息是具有双向性的。Client向Service发送的是Request消息，Service向Client发送的是Response消息。

初始化环境

在《Robot Operating System——深度解析自动隐式加载动态库的运行模式》一文中，我们展现了ROS2可执行文件的链接指令。可以看到它依赖了很多ROS2环境相关的动态库，所以我们在创建工程之前也要初始化环境。

source /opt/ros/jazzy/setup.bash

关于环境的安装可以参见《Robot Operating System——Ubuntu上以二进制形式安装环境》。

生成自定义服务的工程

创建包

ros2 pkg create --build-type ament_cmake --license Apache-2.0 custom_service

在这里插入图片描述
其目录结构如下

自定义消息

我们创建一个目录srv，然后在其下新建一个文件NavSatPose.srv，填入下面的内容

sensor_msgs/NavSatStatus nav
---
geometry_msgs/PoseWithCovariance pose
std_msgs/Bool boolvalue

nav是Request消息结构体中的内容；pose和boolvalue是Response消息结构体中的内容。
在这里插入图片描述

package.xml

在该文件中新增如下内容

  <depend>sensor_msgs</depend>
  <depend>geometry_msgs</depend>
  <depend>std_msgs</depend>
  <buildtool_depend>rosidl_default_generators</buildtool_depend>
  <exec_depend>rosidl_default_runtime</exec_depend>
  <member_of_group>rosidl_interface_packages</member_of_group>

sensor_msgs、geometry_msgs和std_msgs是消息体中用的三种ROS2提供的基础消息类型；
rosidl_default_generators用于将上述msg文件生成代码；
rosidl_default_runtime是运行时依赖；
后三者是一定要加的，前面的三个根据自定义消息的类型酌情添加。
此时的目录结构如下

完整文件

<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
  <name>custom_service</name>
  <version>0.0.0</version>
  <description>TODO: Package description</description>
  <maintainer email="f304646673@gmail.com">fangliang</maintainer>
  <license>Apache-2.0</license>

  <buildtool_depend>ament_cmake</buildtool_depend>
  
  <depend>sensor_msgs</depend>
  <depend>geometry_msgs</depend>
  <depend>std_msgs</depend>
  <buildtool_depend>rosidl_default_generators</buildtool_depend>
  <exec_depend>rosidl_default_runtime</exec_depend>
  <member_of_group>rosidl_interface_packages</member_of_group>

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

  <export>
    <build_type>ament_cmake</build_type>
  </export>
</package>

CMakeLists.txt

新增如下内容

find_package(sensor_msgs REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(std_msgs REQUIRED)
find_package(rosidl_default_generators REQUIRED)

rosidl_generate_interfaces(${PROJECT_NAME}
  "srv/NavSatPoseBool.srv"
  DEPENDENCIES sensor_msgs geometry_msgs std_msgs 
)

DEPENDENCIES 中添加的是我们自定义消息的基础类型。

完整文件

cmake_minimum_required(VERSION 3.8)
project(custom_service)

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)
# uncomment the following section in order to fill in
# further dependencies manually.
# find_package(<dependency> REQUIRED)

find_package(sensor_msgs REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(std_msgs REQUIRED)
find_package(rosidl_default_generators REQUIRED)

rosidl_generate_interfaces(${PROJECT_NAME}
  "srv/NavSatPoseBool.srv"
  DEPENDENCIES sensor_msgs geometry_msgs std_msgs 
)

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()

ament_package()

编译

colcon build --packages-select custom_service

在这里插入图片描述

注册

source install/setup.sh

经过注册后，我们可以在ROS2中查看这个自定义服务

ros2 interface show custom_service/srv/NavSatPoseBool

在这里插入图片描述

使用自定义服务的工程

创建包

ros2 pkg create --build-type ament_cmake --license Apache-2.0 custom_service_nodes --dependencies rclcpp custom_service

在这里插入图片描述
–dependencies参数可以帮我们在package.xml和CMakeLists.txt中自动添加相应依赖。此时我们添加上一步创建的custom_service。
此时目录结构如下

代码

在src目录下新建server.cpp和client.cpp，分别填入以下内容

#include "rclcpp/rclcpp.hpp"
#include "custom_service/srv/nav_sat_pose_bool.hpp"

#include <memory>
#include <string>
#include <sstream>


std::string serializeRequest(const custom_service::srv::NavSatPoseBool::Request &request) {
    std::ostringstream oss;
    oss << request.nav.service << " " 
        << request.nav.status << " ";

    return oss.str();
}

std::string serializeResponse(const custom_service::srv::NavSatPoseBool::Response &response) {
    std::ostringstream oss;
    oss << response.pose.pose.position.x << " " 
        << response.pose.pose.position.y << " " 
        << response.pose.pose.position.z << " " 
        << response.pose.pose.orientation.x << " " 
        << response.pose.pose.orientation.y << " " 
        << response.pose.pose.orientation.z << " " 
        << response.pose.pose.orientation.w << " " 
        << response.boolvalue.data;

    return oss.str();
}

void handle(const std::shared_ptr<custom_service::srv::NavSatPoseBool::Request> request,     
          std::shared_ptr<custom_service::srv::NavSatPoseBool::Response> response)  
{
  response->pose.pose.position.x = 1.0;
  response->pose.pose.position.y = 2.0;
  response->pose.pose.position.z = 3.0;
  response->pose.pose.orientation.x = 4.0;
  response->pose.pose.orientation.y = 5.0;
  response->pose.pose.orientation.z = 6.0;
  response->pose.pose.orientation.w = 7.0;
  response->boolvalue.data = true;

  RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Incoming request\na: %s", serializeRequest(*request).c_str()); 
  RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "sending back response: [%s]", serializeResponse(*response).c_str());
}

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

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

  rclcpp::Service<custom_service::srv::NavSatPoseBool>::SharedPtr service =               
    node->create_service<custom_service::srv::NavSatPoseBool>("handle",  &handle);   

  RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Ready to start service.");                     

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

#include "rclcpp/rclcpp.hpp"
#include "custom_service/srv/nav_sat_pose_bool.hpp"

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

using namespace std::chrono_literals;

std::string serializeRequest(const custom_service::srv::NavSatPoseBool::Request &request) {
    std::ostringstream oss;
    oss << request.nav.service << " " 
        << request.nav.status << " ";

    return oss.str();
}

std::string serializeResponse(const custom_service::srv::NavSatPoseBool::Response &response) {
    std::ostringstream oss;
    oss << response.pose.pose.position.x << " " 
        << response.pose.pose.position.y << " " 
        << response.pose.pose.position.z << " " 
        << response.pose.pose.orientation.x << " " 
        << response.pose.pose.orientation.y << " " 
        << response.pose.pose.orientation.z << " " 
        << response.pose.pose.orientation.w << " " 
        << response.boolvalue.data;

    return oss.str();
}

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

  std::shared_ptr<rclcpp::Node> node = rclcpp::Node::make_shared("custom_client");  
  rclcpp::Client<custom_service::srv::NavSatPoseBool>::SharedPtr client =                
    node->create_client<custom_service::srv::NavSatPoseBool>("handle");          

  auto request = std::make_shared<custom_service::srv::NavSatPoseBool::Request>();
  request->nav.service = 1;
  request->nav.status = 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);
  RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "sending request: [%s]", serializeRequest(*request).c_str());
  // Wait for the result.
  if (rclcpp::spin_until_future_complete(node, result) ==
    rclcpp::FutureReturnCode::SUCCESS)
  {
    RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "receiving back response: [%s]", serializeResponse(*result.get()).c_str());
  } else {
    RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Failed to call service custom_server");    
  }

  rclcpp::shutdown();
  return 0;
}

CMakeLists.txt

然后分别编译这两个文件成为可自行文件。

add_executable(server src/server.cpp)
ament_target_dependencies(server rclcpp custom_service)

add_executable(client src/client.cpp)
ament_target_dependencies(client rclcpp custom_service)

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

编译

 colcon build --packages-select custom_service_nodes

在这里插入图片描述

运行

在新的终端中，需要先初始化环境

source custom_service_nodes/install/setup.sh

然后执行server和client
在这里插入图片描述

工程地址

参考资料

https://docs.ros.org/en/jazzy/Tutorials/Beginner-Client-Libraries/Custom-ROS2-Interfaces.html

breaksoftware

关注

31
点赞
踩
26

收藏

觉得还不错? 一键收藏
打赏
0
评论
复制链接

分享到 QQ

分享到新浪微博

扫一扫

专栏目录