ROS2 编写一个更复杂的发布者和订阅者 (C++)

导言

在之前的文章中，我们学习了如何编写一个基本的 ROS2 发布者和订阅者。在本文中，我们将进一步探索更复杂的用例，包括使用消息类型、服务和动作。

消息类型

消息类型是 ROS2 中数据传输的基础。它允许发布者和订阅者在特定主题上交换特定类型的数据。要创建消息类型，需要创建一个包含数据成员的 .msg 文件。

# my_pkg/msg/CustomMessage.msg
float32 value
string message

发布者

要发布消息，我们需要使用 rclcpp::Publisher。它可以将消息发布到指定的主题上。

#include <rclcpp/rclcpp.hpp>
#include "my_pkg/msg/CustomMessage.msg"

class CustomPublisher : public rclcpp::Node
{
public:
  CustomPublisher() : Node("custom_publisher")
  {
    publisher_ = create_publisher<CustomMessage>("my_topic", 10);
  }

  void publish()
  {
    auto message = CustomMessage();
    message.value = 1.23;
    message.message = "Hello, world!";
    publisher_.publish(message);
  }

private:
  rclcpp::Publisher<CustomMessage> publisher_;
};

订阅者

要订阅主题，我们需要使用 rclcpp::Subscriber。它允许订阅者从指定的主题中获取消息。

#include <rclcpp/rclcpp.hpp>
#include "my_pkg/msg/CustomMessage.msg"

class CustomSubscriber : public rclcpp::Node
{
public:
  CustomSubscriber() : Node("custom_subscribers")
  {
    subscribe_ = create_subscirber<CustomMessage>("my_topic", 10, std::bind(&CustomSubscriber::callback, this, std::placeholders::_1));
  }

private:
  void callback(const CustomMessage::SharedPtr msg)
  {
    std::cout << "Value: " << msg->value << std::endl;
    std::cout << "Message: " << msg->message << std::endl;
  }

  rclcpp::Subscriber<CustomMessage> subscriber_;
};

动作

动作是一种请求-响应通信模式。它允许客户端向服务端发送请求并等待响应。

要创建动作，需要创建一个包含请求和响应消息类型的 .action 文件。

# my_pkg/action/CustomAction.action
# Request message
float32 value
string message
# Response message
bool success
string result

客户端

要调用动作，我们需要使用 rclcpp_action::ActionClient。

#include <rclcpp/rclcpp.hpp>
#include <rclcpp_action/rclcpp_action.hpp>
#include "my_pkg/action/CustomAction.action.hpp"

class CustomActionClient : public rclcpp::Node
{
public:
  CustomActionClient() : Node("custom_action_client")
  {
    action_client_ = rclcpp_action::create_client<CustomAction>(this, "my_action");
  }

  void send_goal()
  {
    auto goal = CustomAction::Goal();
    goal.value = 1.23;
    goal.message = "Hello, world!";
    action_client_.async_send_goal(goal, std::bind(&CustomActionClient::goal_response_callback, this, std::placeholders::_1));
  }

private:
  void goal_response_callback(rclcpp_action::ClientGoalHandle<CustomAction> goal_handle)
  {
    if (goal_handle.get_result()->success)
    {
      std::cout << "Action succeeded!" << std::endl;
    }
    else
    {
      std::cout << "Action failed!" << std::endl;
    }
  }

  rclcpp_action::Client<CustomAction> action_client_;
};

服务端

要提供服务，我们需要使用 rclcpp_action::ActionServer。

#include <rclcpp/rclcpp.hpp>
#include <rclcpp_action/rclcpp_action.hpp>
#include "my_pkg/action/CustomAction.action.hpp"

class CustomActionServer : public rclcpp::Node
{
public:
  CustomActionServer() : Node("custom_action_server")
  {
    action_server_ = rclcpp_action::create_server<CustomAction>(this, "my_action", std::bind(&CustomActionServer::handle_goal, this, std::placeholders::_1, std::placeholders::_2), std::bind(&CustomActionServer::handle_cancel, this, std::placeholders::_1));
  }

private:
  rclcpp_action::Server<CustomAction> action_server_;

  void handle_goal(const rclcpp_action::GoalConstSharedPtr<CustomAction::Goal>& goal_handle, std::shared_ptr<rclcpp_action::ServerGoalHandle<CustomAction>> goal_callback_handle)
  {
    // Process the request
    auto result = CustomAction::Result();
    result.success = true;
    result.result = "Hello, world!";
    goal_callback_handle->accepted();
    goal_callback_handle->set_result(result);
  }

  void handle_cancel(const std::shared_ptr<rclcpp_action::ServerGoalHandle<CustomAction>> goal_handle)
  {
    // Handle cancellation requests
  }
};

编写一个更复杂的发布者和订阅者

现在，我们已经了解了消息类型、服务和动作的基础，我们编写一个更复杂的发布者和订阅者。

发布者

#include <rclcpp/rclcpp.hpp>
#include "my_pkg/msg/CustomMessage.msg"
#include "my_pkg/action/CustomAction.action.hpp"

class ComplexPublisher : public rclcpp::Node
{
public:
  CustomPublisher() : Node("custom_publisher")
  {
    publisher_ = create_publisher<CustomMessage>("my_topic", 10);
    action_client_ = rclcpp_action::create_client<CustomAction>(this, "my_action");
  }

  void publish_and_call_action()
  {
    auto message = CustomMessage();
    message.value = 1.23;
    message.message = "Hello, world!";
    publisher_.publish(message);

    auto goal = CustomAction::Goal();
    goal.value = 1.23;
    goal.message = "Hello, world!";
    action_client_.async_send_goal(goal, std::bind(&CustomPublisher::goal_response_callback, this, std::placeholders::_1));
  }

private:
  rclcpp::Publisher<CustomMessage> publisher_;
  rclcpp_action::Client<CustomAction> action_client_;
};

订阅者

#include <rclcpp/rclcpp.hpp>
#include "my_pkg/msg/CustomMessage.msg"
#include "my_pkg/action/CustomAction.action.hpp"

class ComplexSubscriber : public rclcpp::Node
{
public:
  CustomSubscriber() : Node("custom_subscribers")
  {
    subscribe_ = create_subscirber<CustomMessage>("my_topic", 10, std::bind(&CustomSubscriber::callback, this, std::placeholders::_1));
    action_server_ = rclcpp_action::create_server<CustomAction>(this, "my_action", std::bind(&CustomSubscriber::handle_goal, this, std::placeholders::_1, std::placeholders::_2), std::bind(&CustomSubscriber::handle_cancel, this, std::placeholders::_1));
  }

private:
  void callback(const CustomMessage::SharedPtr msg)
  {
    std::cout << "Value: " << msg->value << std::endl;
    std::cout << "Message: " << msg->message << std::endl;
  }

  void handle_goal(const rclcpp_action::GoalConstSharedPtr<CustomAction::Goal>& goal_handle, std::shared_ptr<rclcpp_action::ServerGoalHandle<CustomAction>> goal_callback_handle)
  {
    // Process the request
    auto result = CustomAction::Result();