< >
Home » ROS2与C++入门教程 » ROS2与C++入门教程-服务组件演示

ROS2与C++入门教程-服务组件演示

ROS2与C++入门教程-服务组件演示

说明：

  • 介绍如何实现服务组件，利用服务组件发布服务和请求服务

步骤：

  • 新建一个包cpp_component_service
cd ~/dev_ws/src
ros2 pkg create --build-type ament_cmake cpp_component_service
  • 进入include目录，新建文件server_component.hpp
cd ~/dev_ws/src/cpp_component_service/include/cpp_component_service
touch server_component.hpp
  • 内容如下：
#ifndef COMPOSITION__SERVER_COMPONENT_HPP_
#define COMPOSITION__SERVER_COMPONENT_HPP_

#include "cpp_component_service/visibility_control.h"
#include "example_interfaces/srv/add_two_ints.hpp"
#include "rclcpp/rclcpp.hpp"

namespace composition
{

class Server : public rclcpp::Node
{
public:
  COMPOSITION_PUBLIC
  explicit Server(const rclcpp::NodeOptions & options);

private:
  rclcpp::Service<example_interfaces::srv::AddTwoInts>::SharedPtr srv_;
};

}  // namespace composition

#endif  // COMPOSITION__SERVER_COMPONENT_HPP_
  • 进入include目录，新建文件client_component.hpp
cd ~/dev_ws/src/cpp_component_service/include/cpp_component_service
touch client_component.hpp
  • 内容如下：
#ifndef COMPOSITION__CLIENT_COMPONENT_HPP_
#define COMPOSITION__CLIENT_COMPONENT_HPP_

#include "cpp_component_service/visibility_control.h"
#include "example_interfaces/srv/add_two_ints.hpp"
#include "rclcpp/rclcpp.hpp"

namespace composition
{

class Client : public rclcpp::Node
{
public:
  COMPOSITION_PUBLIC
  explicit Client(const rclcpp::NodeOptions & options);

protected:
  void on_timer();

private:
  rclcpp::Client<example_interfaces::srv::AddTwoInts>::SharedPtr client_;
  rclcpp::TimerBase::SharedPtr timer_;
};

}  // namespace composition

#endif  // COMPOSITION__CLIENT_COMPONENT_HPP_
  • 进入include目录，新建文件visibility_control.h
cd ~/dev_ws/src/cpp_component_service/include/cpp_component_service
touch visibility_control.h
  • 内容如下：
#ifndef COMPOSITION__VISIBILITY_CONTROL_H_
#define COMPOSITION__VISIBILITY_CONTROL_H_

#ifdef __cplusplus
extern "C"
{
#endif

// This logic was borrowed (then namespaced) from the examples on the gcc wiki:
//     https://gcc.gnu.org/wiki/Visibility

#if defined _WIN32 || defined __CYGWIN__
  #ifdef __GNUC__
    #define COMPOSITION_EXPORT __attribute__ ((dllexport))
    #define COMPOSITION_IMPORT __attribute__ ((dllimport))
  #else
    #define COMPOSITION_EXPORT __declspec(dllexport)
    #define COMPOSITION_IMPORT __declspec(dllimport)
  #endif
  #ifdef COMPOSITION_BUILDING_DLL
    #define COMPOSITION_PUBLIC COMPOSITION_EXPORT
  #else
    #define COMPOSITION_PUBLIC COMPOSITION_IMPORT
  #endif
  #define COMPOSITION_PUBLIC_TYPE COMPOSITION_PUBLIC
  #define COMPOSITION_LOCAL
#else
  #define COMPOSITION_EXPORT __attribute__ ((visibility("default")))
  #define COMPOSITION_IMPORT
  #if __GNUC__ >= 4
    #define COMPOSITION_PUBLIC __attribute__ ((visibility("default")))
    #define COMPOSITION_LOCAL  __attribute__ ((visibility("hidden")))
  #else
    #define COMPOSITION_PUBLIC
    #define COMPOSITION_LOCAL
  #endif
  #define COMPOSITION_PUBLIC_TYPE
#endif

#ifdef __cplusplus
}
#endif

#endif  // COMPOSITION__VISIBILITY_CONTROL_H_
  • 进入src目录，新建文件server_component.cpp
cd ~/dev_ws/src/cpp_component_service/src
touch server_component.cpp
  • 内容如下：
#include "cpp_component_service/server_component.hpp"

#include <cinttypes>
#include <iostream>
#include <memory>

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

namespace composition
{

Server::Server(const rclcpp::NodeOptions & options)
: Node("Server", options)
{
  auto handle_add_two_ints =
    [this](
    const std::shared_ptr<example_interfaces::srv::AddTwoInts::Request> request,
    std::shared_ptr<example_interfaces::srv::AddTwoInts::Response> response
    ) -> void
    {
      RCLCPP_INFO(
        this->get_logger(), "Incoming request: [a: %" PRId64 ", b: %" PRId64 "]",
        request->a, request->b);
      std::flush(std::cout);
      response->sum = request->a + request->b;
    };

  srv_ = create_service<example_interfaces::srv::AddTwoInts>("add_two_ints", handle_add_two_ints);
}

}  // namespace composition

#include "rclcpp_components/register_node_macro.hpp"

// Register the component with class_loader.
// This acts as a sort of entry point, allowing the component to be discoverable when its library
// is being loaded into a running process.
RCLCPP_COMPONENTS_REGISTER_NODE(composition::Server)
  • 进入src目录，新建文件client_component.cpp
cd ~/dev_ws/src/cpp_component_service/src
touch client_component.cpp
  • 内容如下：
#include "cpp_component_service/client_component.hpp"

#include <cinttypes>
#include <iostream>
#include <memory>

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

using namespace std::chrono_literals;

namespace composition
{

Client::Client(const rclcpp::NodeOptions & options)
: Node("Client", options)
{
  client_ = create_client<example_interfaces::srv::AddTwoInts>("add_two_ints");
  // Note(dhood): The timer period must be greater than the duration of the timer callback.
  // Otherwise, the timer can starve a single-threaded executor.
  // See https://github.com/ros2/rclcpp/issues/392 for updates.
  timer_ = create_wall_timer(2s, std::bind(&Client::on_timer, this));
}

void Client::on_timer()
{
  if (!client_->wait_for_service(1s)) {
    if (!rclcpp::ok()) {
      RCLCPP_ERROR(
        this->get_logger(),
        "Interrupted while waiting for the service. Exiting.");
      return;
    }
    RCLCPP_INFO(this->get_logger(), "Service not available after waiting");
    return;
  }

  auto request = std::make_shared<example_interfaces::srv::AddTwoInts::Request>();
  request->a = 2;
  request->b = 3;

  // In order to wait for a response to arrive, we need to spin().
  // However, this function is already being called from within another spin().
  // Unfortunately, the current version of spin() is not recursive and so we
  // cannot call spin() from within another spin().
  // Therefore, we cannot wait for a response to the request we just made here
  // within this callback, because it was executed by some other spin function.
  // The workaround for this is to give the async_send_request() method another
  // argument which is a callback that gets executed once the future is ready.
  // We then return from this callback so that the existing spin() function can
  // continue and our callback will get called once the response is received.
  using ServiceResponseFuture =
    rclcpp::Client<example_interfaces::srv::AddTwoInts>::SharedFuture;
  auto response_received_callback = [this](ServiceResponseFuture future) {
      RCLCPP_INFO(this->get_logger(), "Got result: [%" PRId64 "]", future.get()->sum);
    };
  auto future_result = client_->async_send_request(request, response_received_callback);
}

}  // namespace composition

#include "rclcpp_components/register_node_macro.hpp"

// Register the component with class_loader.
// This acts as a sort of entry point, allowing the component to be discoverable when its library
// is being loaded into a running process.
RCLCPP_COMPONENTS_REGISTER_NODE(composition::Client)
  • 编译package.xml
  • 在<buildtool_depend>ament_cmake</buildtool_depend>后增加
  <depend>rclcpp</depend>
  <depend>rclcpp_components</depend>
  <depend>example_interfaces</depend>
  • 编译CMakelist.txt
  • 在find_package(ament_cmake REQUIRED)后增加
find_package(rclcpp REQUIRED)
find_package(rclcpp_components REQUIRED)
find_package(example_interfaces REQUIRED)
  • 编译为共享库并注册server_component和client_component组件
include_directories(include)
# create ament index resource which references the libraries in the binary dir
set(node_plugins "")

add_library(server_component SHARED
  src/server_component.cpp)
target_compile_definitions(server_component
  PRIVATE "COMPOSITION_BUILDING_DLL")
ament_target_dependencies(server_component
  "example_interfaces"
  "rclcpp"
  "rclcpp_components")
rclcpp_components_register_nodes(server_component "composition::Server")
set(node_plugins "${node_plugins}composition::Server;$<TARGET_FILE:server_component>\n")

add_library(client_component SHARED
  src/client_component.cpp)
target_compile_definitions(client_component
  PRIVATE "COMPOSITION_BUILDING_DLL")
ament_target_dependencies(client_component
  "example_interfaces"
  "rclcpp"
  "rclcpp_components")
rclcpp_components_register_nodes(client_component "composition::Client")
set(node_plugins "${node_plugins}composition::Client;$<TARGET_FILE:client_component>\n")
  • 生成和安装相关库文件和执行文件
install(TARGETS
  server_component 
  client_component 
  ARCHIVE DESTINATION lib
  LIBRARY DESTINATION lib
  RUNTIME DESTINATION bin)
  • 安装相关依赖
cd ~/dev_ws/
rosdep install -i --from-path src --rosdistro galactic -y
  • 编译包
colcon build --symlink-install --packages-select cpp_component_service
  • 加载工作空间
. install/setup.bash
  • 查看工作区中已注册和可用的组件，
  • 执行以下命令：
 ros2 component types
  • 效果如下：
$ ros2 component types
cpp_component_service
  composition::Server
  composition::Client
  • 启动组件容器
ros2 run rclcpp_components component_container
  • 查看组件容器名称
$ ros2 component list
/ComponentManager
  • 加载话题发布组件到容器/ComponentManager
ros2 component load /ComponentManager cpp_component_service composition::Server
  • 加载话题订阅组件到容器/ComponentManager
ros2 component load /ComponentManager cpp_component_service composition::Client
  • 在运行启动容器组件的终端下显示
  • 效果如下:
$ ros2 run rclcpp_components component_container
[INFO] [1651832414.188548964] [ComponentManager]: Load Library: /home/ubuntu/dev_ws/install/cpp_component_service/lib/libserver_component.so
[INFO] [1651832414.190521123] [ComponentManager]: Found class: rclcpp_components::NodeFactoryTemplate<composition::Server>
[INFO] [1651832414.190605964] [ComponentManager]: Instantiate class: rclcpp_components::NodeFactoryTemplate<composition::Server>
[INFO] [1651832438.779271026] [ComponentManager]: Load Library: /home/ubuntu/dev_ws/install/cpp_component_service/lib/libclient_component.so
[INFO] [1651832438.780623487] [ComponentManager]: Found class: rclcpp_components::NodeFactoryTemplate<composition::Client>
[INFO] [1651832438.780716759] [ComponentManager]: Instantiate class: rclcpp_components::NodeFactoryTemplate<composition::Client>
[INFO] [1651832440.791329377] [Server]: Incoming request: [a: 2, b: 3]
[INFO] [1651832440.792103664] [Client]: Got result: [5]
[INFO] [1651832442.789992720] [Server]: Incoming request: [a: 2, b: 3]
[INFO] [1651832442.790243601] [Client]: Got result: [5]
[INFO] [1651832444.789996372] [Server]: Incoming request: [a: 2, b: 3]
[INFO] [1651832444.790298262] [Client]: Got result: [5]
[INFO] [1651832446.789964760] [Server]: Incoming request: [a: 2, b: 3]
[INFO] [1651832446.790329183] [Client]: Got result: [5]
[INFO] [1651832448.789917991] [Server]: Incoming request: [a: 2, b: 3]
[INFO] [1651832448.790179199] [Client]: Got result: [5]

纠错,疑问,交流: 请进入讨论区或 点击加入Q群

获取最新文章: 扫一扫右上角的二维码加入“创客智造”公众号


标签: ROS2与C++入门教程