《ROS2智能机器人开发与实践》——ROS核心（节点、话题、服务、DDS通信协议等）

herowuxun

《ROS2智能机器人开发与实践》——ROS核心（节点、话题、服务、DDS通信协议等） [复制链接]

 

# 《ROS2智能机器人开发与实践》——ROS核心（节点、话题、服务、DDS通信协议等）

---

今天主要是分享一下ROS2的核心包括节点、话题、服务和DDS通信协议的内容

其中会对官方和本书的相关细节进行比对

本文章都以C++为例

一、节点

1、ROS2中的节点具有以下几个特点

1. 每个节点是一个进程
2. 每个节点都是一个可以独立运行的可执行文件
3. 每个节点可使用不同的编程语言
4. 每个节点可分布式运行在不同的主机中
5. 每个节点需要唯一的名称

2、节点编程方法（C++）

a.C++程序编写

#include "rclcpp/rclcpp.hpp"


/***
创建一个HelloWorld节点, 初始化时输出“hello world”日志
***/
class HelloWorldNode : public rclcpp::Node
{
  public:
  HelloWorldNode()
  : Node("node_helloworld_class")        // ROS2节点父类初始化
  {
    while(rclcpp::ok())            // ROS2系统是否正常运行
    {
      RCLCPP_INFO(this->get_logger(), "Hello World");// ROS2日志输出
      sleep(1);              // 休眠控制循环时间
    }
  }
};

// ROS2节点主入口main函数
int main(int argc, char * argv[])           
{

  // ROS2 C++接口初始化
  rclcpp::init(argc, argv);  

  // 创建ROS2节点对象并进行初始化     
  rclcpp::spin(std::make_shared<HelloWorldNode>()); 

  // 关闭ROS2 C++接口
  rclcpp::shutdown();           

  return 0;
}

** 可以看的出来ROS2对节点的编程推荐面向对象编程的编程方法**
同时ROS2的编程难度更大、C++的语法更新、所以使用python也不失为一种方法

b.设置编译选项（编译接口）

find_package(rclcpp REQUIRED)

add_executable(node_helloworld_class src/node_helloworld_class.cpp)

ament_target_dependencies(node_helloworld_class rclcpp)

install(TARGETS
node_helloworld_class
DESTINATION lib/${PROJECT_NAME})

前三行相对于ROS1没什么太大区别，主要是多了最后这个

将编译生成的可执行文件拷贝到install安装空间的lib文件夹下
我们知道ROS2相对于ROS1的工作空间构建方式变了，其中多了install文件夹 所以要注意最后一行

c.编译运行

#进入工作空间后
colcon build #编译工作空间
ros2 run learning_node_cpp node_helloworld_class # (功能包 可执行文件)
ros2 node list     # 查看节点列表
ros2 node info <node_name> # 查看节点信息

二、话题

1.rqt_graph

ros2 run turtlesim turtlesim_node

ros2 run turtlesim turtle_teleop_key

rqt_graph

我们将使用 rqt_graph 来可视化不断变化的节点和主题，以及它们之间的联系。

话题的特点：

• 多对多通信
• 异步通信
• 消息接口

话题通信的一个特性，就是异步通信所谓异步，只要是指发布者发出数据后，并不知道订阅者什么时候可以收到。所以ROS2的后台会有一个轮询机制，当发现有数据进入消息队列时，就会触发回调函数

2.发布话题

/***
@作者: 古月居(www.guyuehome.com)
@说明: ROS2话题示例-发布图像话题
***/

#include <chrono>
#include <functional>
#include <memory>
#include <string>

#include "rclcpp/rclcpp.hpp"    // ROS2 C++接口库
#include "std_msgs/msg/string.hpp"  // 字符串消息类型

using namespace std::chrono_literals;


class PublisherNode : public rclcpp::Node
{
  public:
  PublisherNode()
  : Node("topic_helloworld_pub") // ROS2节点父类初始化
  {
    // 创建发布者对象（消息类型、话题名、队列长度）
    publisher_ = this->create_publisher<std_msgs::msg::String>("chatter", 10); 
    // 创建一个定时器,定时执行回调函数
    timer_ = this->create_wall_timer(
      500ms, std::bind(&PublisherNode::timer_callback, this));    
  }

  private:
  // 创建定时器周期执行的回调函数
  void timer_callback()                   
  {
    // 创建一个String类型的消息对象
    auto msg = std_msgs::msg::String(); 
    // 填充消息对象中的消息数据            
    msg.data = "Hello World";
    // 发布话题消息                 
    RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", msg.data.c_str()); 
    // 输出日志信息，提示已经完成话题发布 
    publisher_->publish(msg);                
  }

  rclcpp::TimerBase::SharedPtr timer_;         // 定时器指针
  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;// 发布者指针
};

// ROS2节点主入口main函数
int main(int argc, char * argv[])        
{
  // ROS2 C++接口初始化
  rclcpp::init(argc, argv);      

  // 创建ROS2节点对象并进行初始化    
  rclcpp::spin(std::make_shared<PublisherNode>()); 

  // 关闭ROS2 C++接口
  rclcpp::shutdown();          

  return 0;
}

都是面向对象编程

3.订阅话题

/***
@作者: 古月居(www.guyuehome.com)
@说明: ROS2话题示例-订阅“Hello World”话题消息
***/

#include <memory>

#include "rclcpp/rclcpp.hpp"      // ROS2 C++接口库
#include "std_msgs/msg/string.hpp"    // 字符串消息类型
using std::placeholders::_1;

class SubscriberNode : public rclcpp::Node
{
  public:
  SubscriberNode()
  : Node("topic_helloworld_sub")  // ROS2节点父类初始化
  {
    subscription_ = this->create_subscription<std_msgs::msg::String>(   
      "chatter", 10, std::bind(&SubscriberNode::topic_callback, this, _1)); // 创建订阅者对象（消息类型、话题名、订阅者回调函数、队列长度）
  }

  private:
  void topic_callback(const std_msgs::msg::String & msg) const      // 创建回调函数，执行收到话题消息后对数据的处理
  {
    RCLCPP_INFO(this->get_logger(), "I heard: '%s'", msg.data.c_str());   // 输出日志信息，提示订阅收到的话题消息
  }

  rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscription_;   // 订阅者指针
};

// ROS2节点主入口main函数
int main(int argc, char * argv[])         
{
  // ROS2 C++接口初始化
  rclcpp::init(argc, argv);     

  // 创建ROS2节点对象并进行初始化    
  rclcpp::spin(std::make_shared<SubscriberNode>()); 

  // 关闭ROS2 C++接口
  rclcpp::shutdown();            

  return 0;
}

3.服务

服务可以实现类似你问我答的同步通信效果

a.服务的特点

服务的特点：

• 同步通信
• 一对多通信（服务端唯一）
• 服务接口

b.客户端

/***
@作者: 古月居(www.guyuehome.com)
@说明: ROS2服务示例-发送两个加数，请求加法器计算
***/

#include "rclcpp/rclcpp.hpp"        // ROS2 C++接口库
#include "learning_interface/srv/add_two_ints.hpp"  // 自定义的服务接口

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

using namespace std::chrono_literals;

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

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

  // 创建ROS2节点对象并进行初始化
  std::shared_ptr<rclcpp::Node> node = rclcpp::Node::make_shared("service_adder_client");
  // 创建服务客户端对象（服务接口类型，服务名） 
  rclcpp::Client<learning_interface::srv::AddTwoInts>::SharedPtr client =
  node->create_client<learning_interface::srv::AddTwoInts>("add_two_ints");     

  // 创建服务接口数据
  auto request = std::make_shared<learning_interface::srv::AddTwoInts::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);              

  // 接收服务器端的反馈数据
  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 C++接口
  rclcpp::shutdown();
  return 0;
}

c. 服务端

/***
@作者: 古月居(www.guyuehome.com)
@说明: ROS2服务示例-提供加法器的服务器处理功能
***/

#include "rclcpp/rclcpp.hpp"         // ROS2 C++接口库
#include "learning_interface/srv/add_two_ints.hpp" // 自定义的服务接口

#include <memory>

// 创建回调函数，执行收到请求后对数据的处理
void adderServer(const std::shared_ptr<learning_interface::srv::AddTwoInts::Request> request, 
    std::shared_ptr<learning_interface::srv::AddTwoInts::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);
}

// ROS2节点主入口main函数
int main(int argc, char **argv)                     
{
  // ROS2 C++接口初始化
  rclcpp::init(argc, argv);
  // 创建ROS2节点对象并进行初始化                       
  std::shared_ptr<rclcpp::Node> node = rclcpp::Node::make_shared("service_adder_server"); 
  // 创建服务器对象（接口类型、服务名、服务器回调函数）
  rclcpp::Service<learning_interface::srv::AddTwoInts>::SharedPtr service =
  node->create_service<learning_interface::srv::AddTwoInts>("add_two_ints", &adderServer);

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

  // 循环等待ROS2退出
  rclcpp::spin(node);  
  // 关闭ROS2 C++接口                       
  rclcpp::shutdown();                         
}

d.命令行操作

ros2 service list      # 查看服务列表
ros2 service type <service_name> # 查看服务数据类型
ros2 service call <service_name> <service_type> <service_data> # 发送服务请求

4.通信接口

同ROS1一样ROS2同样有的可自定义的通信接口，这些通信接口大大拓展了ROS的应用场景
具体的自定义接口的方法可以看官方文档或者

5.动作

动作是一种应用层的通信机制，其底层是基于话题和服务实现的

a. 服务端

/***
@作者: 古月居(www.guyuehome.com)
@说明: ROS2动作示例-负责执行圆周运动动作的服务端
***/

#include <iostream>

#include "rclcpp/rclcpp.hpp"        // ROS2 C++接口库
#include "rclcpp_action/rclcpp_action.hpp"    // ROS2 动作类
#include "learning_interface/action/move_circle.hpp"// 自定义的圆周运动接口

using namespace std;

class MoveCircleActionServer : public rclcpp::Node
{
  public:
  // 定义一个自定义的动作接口类，便于后续使用
  using CustomAction = learning_interface::action::MoveCircle;
  // 定义一个处理动作请求、取消、执行的服务器端
  using GoalHandle = rclcpp_action::ServerGoalHandle<CustomAction>;

  explicit MoveCircleActionServer(const rclcpp::NodeOptions & action_server_options = rclcpp::NodeOptions())
  : Node("action_move_server", action_server_options)             // ROS2节点父类初始化
  {
    using namespace std::placeholders;

    this->action_server_ = rclcpp_action::create_server<CustomAction>(      // 创建动作服务器（接口类型、动作名、回调函数）
      this->get_node_base_interface(),
      this->get_node_clock_interface(),
      this->get_node_logging_interface(),
      this->get_node_waitables_interface(),
      "move_circle",
      std::bind(&MoveCircleActionServer::handle_goal, this, _1, _2),
      std::bind(&MoveCircleActionServer::handle_cancel, this, _1),
      std::bind(&MoveCircleActionServer::handle_accepted, this, _1));
  }

  private:
  rclcpp_action::Server<CustomAction>::SharedPtr action_server_;// 动作服务器

  // 响应动作目标的请求
  rclcpp_action::GoalResponse handle_goal(
    const rclcpp_action::GoalUUID & uuid,
    std::shared_ptr<const CustomAction::Goal> goal_request)         
  {
    RCLCPP_INFO(this->get_logger(), "Server: Received goal request: %d", goal_request->enable);
    (void)uuid;

    // 如请求为enable则接受运动请求，否则就拒绝
    if (goal_request->enable)                   
    {
      return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
    }
    else
    {
      return rclcpp_action::GoalResponse::REJECT;
    }
  }

  // 响应动作取消的请求
  rclcpp_action::CancelResponse handle_cancel(
    const std::shared_ptr<GoalHandle> goal_handle_canceled_)          
  {
    RCLCPP_INFO(this->get_logger(), "Server: Received request to cancel action");
    (void) goal_handle_canceled_; 
    return rclcpp_action::CancelResponse::ACCEPT;
  }

  // 处理动作接受后具体执行的过程
  void handle_accepted(const std::shared_ptr<GoalHandle> goal_handle_accepted_)    
  {
    using namespace std::placeholders;
    // 在线程中执行动作过程
    std::thread{std::bind(&MoveCircleActionServer::execute, this, _1), goal_handle_accepted_}.detach(); 
  }


  void execute(const std::shared_ptr<GoalHandle> goal_handle_)
  {
    const auto requested_goal = goal_handle_->get_goal();   // 动作目标
    auto feedback = std::make_shared<CustomAction::Feedback>(); // 动作反馈
    auto result = std::make_shared<CustomAction::Result>(); // 动作结果

    RCLCPP_INFO(this->get_logger(), "Server: Executing goal");
    rclcpp::Rate loop_rate(1);

    // 动作执行的过程
    for (int i = 0; (i < 361) && rclcpp::ok(); i=i+30)
    {
      // 检查是否取消动作
      if (goal_handle_->is_canceling())
      {
      result->finish = false;
      goal_handle_->canceled(result);
      RCLCPP_INFO(this->get_logger(), "Server: Goal canceled");
      return;
      }

      // 更新反馈状态
      feedback->state = i;
      // 发布反馈状态
      goal_handle_->publish_feedback(feedback);
      RCLCPP_INFO(this->get_logger(), "Server: Publish feedback");

      loop_rate.sleep();
    }

    // 动作执行完成
    if (rclcpp::ok())
    {
      result->finish = true;
      goal_handle_->succeed(result);
      RCLCPP_INFO(this->get_logger(), "Server: Goal succeeded");
    }
  }
};

// ROS2节点主入口main函数
int main(int argc, char * argv[])          
{
  // ROS2 C++接口初始化
  rclcpp::init(argc, argv);        

  // 创建ROS2节点对象并进行初始化     
  rclcpp::spin(std::make_shared<MoveCircleActionServer>()); 

  // 关闭ROS2 C++接口
  rclcpp::shutdown();               

  return 0;
}

b. 客户端

/***
@作者: 古月居(www.guyuehome.com)
@说明: ROS2动作示例-请求执行圆周运动动作的客户端
***/

#include <iostream>

#include "rclcpp/rclcpp.hpp"        // ROS2 C++接口库
#include "rclcpp_action/rclcpp_action.hpp"    // ROS2 动作类
#include "learning_interface/action/move_circle.hpp"// 自定义的圆周运动接口

using namespace std;

class MoveCircleActionClient : public rclcpp::Node
{
  public:
  // 定义一个自定义的动作接口类，便于后续使用
  using CustomAction = learning_interface::action::MoveCircle;
  // 定义一个处理动作请求、取消、执行的客户端类
  using GoalHandle = rclcpp_action::ClientGoalHandle<CustomAction>;

  explicit MoveCircleActionClient(const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions())
  : Node("action_move_client", node_options)          // ROS2节点父类初始化
  {
    this->client_ptr_ = rclcpp_action::create_client<CustomAction>( // 创建动作客户端（接口类型、动作名）
      this->get_node_base_interface(),
      this->get_node_graph_interface(),
      this->get_node_logging_interface(),
      this->get_node_waitables_interface(),
      "move_circle");
  }

  // 创建一个发送动作目标的函数
  void send_goal(bool enable)
  {
    // 检查动作服务器是否可以使用
    if (!this->client_ptr_->wait_for_action_server(std::chrono::seconds(10))) 
    {
      RCLCPP_ERROR(this->get_logger(), "Client: Action server not available after waiting");
      rclcpp::shutdown();
      return;
    }

    // 绑定动作请求、取消、执行的回调函数
    auto send_goal_options = rclcpp_action::Client<CustomAction>::SendGoalOptions();
    using namespace std::placeholders;
    send_goal_options.goal_response_callback =
      std::bind(&MoveCircleActionClient::goal_response_callback, this, _1);
    send_goal_options.feedback_callback =
      std::bind(&MoveCircleActionClient::feedback_callback, this, _1, _2);
    send_goal_options.result_callback =
      std::bind(&MoveCircleActionClient::result_callback, this, _1);

    // 创建一个动作目标的消息
    auto goal_msg = CustomAction::Goal();
    goal_msg.enable = enable;
    // 异步方式发送动作的目标
    RCLCPP_INFO(this->get_logger(), "Client: Sending goal");
      this->client_ptr_->async_send_goal(goal_msg, send_goal_options);
  }

  private:
  rclcpp_action::Client<CustomAction>::SharedPtr client_ptr_;

  // 创建一个服务器收到目标之后反馈时的回调函数
  void goal_response_callback(GoalHandle::SharedPtr goal_message)
  {
    if (!goal_message)
    {
      RCLCPP_ERROR(this->get_logger(), "Client: Goal was rejected by server");
      rclcpp::shutdown(); // Shut down client node
    }
    else
    {
      RCLCPP_INFO(this->get_logger(), "Client: Goal accepted by server, waiting for result");
    }
  }

  // 创建处理周期反馈消息的回调函数
  void feedback_callback(
    GoalHandle::SharedPtr,
    const std::shared_ptr<const CustomAction::Feedback> feedback_message)
  {
    std::stringstream ss;
    ss << "Client: Received feedback: "<< feedback_message->state;
    RCLCPP_INFO(this->get_logger(), "%s", ss.str().c_str());
  }

  // 创建一个收到最终结果的回调函数
  void result_callback(const GoalHandle::WrappedResult & result_message)
  {
    switch (result_message.code)
    {
      case rclcpp_action::ResultCode::SUCCEEDED:
      break;
      case rclcpp_action::ResultCode::ABORTED:
      RCLCPP_ERROR(this->get_logger(), "Client: Goal was aborted");
      rclcpp::shutdown(); // 关闭客户端节点
      return;
      case rclcpp_action::ResultCode::CANCELED:
      RCLCPP_ERROR(this->get_logger(), "Client: Goal was canceled");
      rclcpp::shutdown(); // 关闭客户端节点
      return;
      default:
      RCLCPP_ERROR(this->get_logger(), "Client: Unknown result code");
      rclcpp::shutdown(); // 关闭客户端节点
      return;
    }
    RCLCPP_INFO(this->get_logger(), "Client: Result received: %s", (result_message.result->finish ? "true" : "false"));
    rclcpp::shutdown();   // 关闭客户端节点
  }
};

// ROS2节点主入口main函数
int main(int argc, char * argv[])            
{
  // ROS2 C++接口初始化
  rclcpp::init(argc, argv); 

  // 创建一个客户端指针             
  auto action_client = std::make_shared<MoveCircleActionClient>(); 

  // 发送动作目标
  action_client->send_goal(true); 

  // 创建ROS2节点对象并进行初始化          
  rclcpp::spin(action_client);

  // 关闭ROS2 C++接口           
  rclcpp::shutdown();                

  return 0;
}

c. 命令行操作

$ ros2 action list      # 查看服务列表
$ ros2 action info <action_name>  # 查看服务数据类型
$ ros2 action send_goal <action_name> <action_type> <action_data> # 发送服务请求

6.参数

参数是ROS2模块化封装的重要功能，可以利用全局参数可视化显示以及动态调整
默认情况下，某一个参数被修改后，使用它的节点并不会立刻被同步，ROS2提供了动态配置的机制，需要在程序中进行设置，通过回调函数的方法，动态调整参数

关于参数，具体可以看https://docs.ros.org/en/jazzy/Concepts/Basic/About-Parameters.html#parameters

7.DDS

DS的全称是Data Distribution Service，也就是数据分发服务，2004年由对象管理组织OMG发布和维护，是一套专门为实时系统设计的数据分发/订阅标准，最早应用于美国海军， 解决舰船复杂网络环境中大量软件升级的兼容性问题，现在已经成为强制标准。

DDS强调以数据为中心，可以提供丰富的服务质量策略，以保障数据进行实时、高效、灵活地分发，可满足各种分布式实时通信应用需求。

在命令行中配置DDS

$ ros2 topic pub /chatter std_msgs/msg/Int32 "data: 42" --qos-reliability best_effort 
$ ros2 topic echo /chatter --qos-reliability reliable
$ ros2 topic echo /chatter --qos-reliability best_effort
$ ros2 topic info /chatter --verbose #查看节点的Qos策略

8.分布式通信

分布式通信是解决使用一台计算机算力不足的情况
有几个实用的例子，也是我自己在使用的：
一台电脑运行执行器节点另一台电脑读取数据并使用QT可视化数据
一台电脑运行节点 另一台使用rviz等等

查看精华帖全部内容，请登录或者注册

Jacktang

分布式通信是解决使用一台计算机算力不足的情况，这个可以