精选旭日X3派开发｜ micro-ROS C API详解(二)

张珊珊

分类：ROS2

发布时间 2023.05.15阅读数 1979 评论数 0

上一篇已经讲述了如何创建一个micro-ROS节点，那么之后便是需要使用Topic、Service等进行节点间的通讯。下面将会对Publisher- Subscriber，Server-Client的详细用法进行讲解。

Publisher

若想使用 Publisher 进行通信，那么需要进行初始化 Publisher，之后便可以使用 Publisher 进行消息的发送。使用 Publisher 进行话题的发送的步骤如下：
创建并初始化 Node -> 创建并初始化 Publisher(包括消息格式、话题名、绑定节点) -> 发送消息

Initialization

以下示例代码的前提是已经有初始化 rcl 并创建 micro-ROS Node 的代码(Node 篇)，根据所需的 QoS(Quantity of Service 服务质量)配置，有三种方法初始化发布服务器：

Reliable (default):

// Publisher对象
rcl_publisher_t publisher;
const char * topic_name = "test_topic";

// 创建消息格式支持
const rosidl_message_type_support_t * type_support =
  ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32);

// 在node上创建并初始化一个reliable的publisher
rcl_ret_t rc = rclc_publisher_init_default(
  &publisher, &node,
  type_support, topic_name);

if (RCL_RET_OK != rc) {
  ...  // 错误处理
  return -1;
}

Best effort:

// Publisher对象
rcl_publisher_t publisher;
const char * topic_name = "test_topic";

// 创建消息格式支持
const rosidl_message_type_support_t * type_support =
  ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32);

// 在node上创建并初始化一个best effort的publisher
rcl_ret_t rc = rclc_publisher_init_best_effort(
  &publisher, &node,
  type_support, topic_name);

if (RCL_RET_OK != rc) {
  ...  // 错误处理
  return -1;
}

Custom QoS:

// Publisher对象
rcl_publisher_t publisher;
const char * topic_name = "test_topic";

// 创建消息格式支持
const rosidl_message_type_support_t * type_support =
  ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32);

// 设置publisher QoS
const rmw_qos_profile_t * qos_profile = &rmw_qos_profile_default;

// Creates a rcl publisher with customized quality-of-service options
rcl_ret_t rc = rclc_publisher_init(
  &publisher, &node,
  type_support, topic_name, qos_profile);

if (RCL_RET_OK != rc) {
  ...  // 错误处理
  return -1;
}

发布消息

若想发送消息，需要使用上面创建的 publisher

// Int32 消息对象
std_msgs__msg__Int32 msg;

// 设置消息对象的内容
msg.data = 0;

// 发布消息
rcl_ret_t rc = rcl_publish(&publisher, &msg, NULL);

if (rc != RCL_RET_OK) {
  ...  // 错误处理
  return -1;
}

rcl_publish 是线程安全的，可以从多个线程调用

Cleaning Up

// Destroy publisher
rcl_publisher_fini(&publisher, &node);

example

利用 Node 篇的 Lifecycle 回调函数并使用 publisher 发送消息

Subscriber

已经学会了发布消息，那么接下来就学习如何使用 Subscriber 订阅消息。使用 Subscriber 订阅消息在创建并初始化阶段与 Publisher 相同。

Initialization

根据 QoS，同样有三种初始化方法：

Reliable (default):

// Subscription object
rcl_subscription_t subscriber;
const char * topic_name = "test_topic";

// Get message type support
const rosidl_message_type_support_t * type_support =
  ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32);

// Initialize a reliable subscriber
rcl_ret_t rc = rclc_subscription_init_default(
  &subscriber, &node,
  type_support, topic_name);

if (RCL_RET_OK != rc) {
  ...  // Handle error
  return -1;
}

Best effort:

// Subscription object
rcl_subscription_t subscriber;
const char * topic_name = "test_topic";

// Get message type support
const rosidl_message_type_support_t * type_support =
  ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32);

// Initialize best effort subscriber
rcl_ret_t rc = rclc_subscription_init_best_effort(
  &subscriber, &node,
  type_support, topic_name);

if (RCL_RET_OK != rc) {
  ...  // Handle error
  return -1;
}

Custom QoS:

// Subscription object
rcl_subscription_t subscriber;
const char * topic_name = "test_topic";

// Get message type support
const rosidl_message_type_support_t * type_support =
  ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32);

// Set client QoS
const rmw_qos_profile_t * qos_profile = &rmw_qos_profile_default;

// Initialize a subscriber with customized quality-of-service options
rcl_ret_t rc = rclc_subscription_init(
  &subscriber, &node,
  type_support, topic_name, qos_profile);

if (RCL_RET_OK != rc) {
  ...  // Handle error
  return -1;
}

订阅消息

Subscriber 订阅消息需要在 executor 上配置一个订阅回调函数，该函数参数只有一个，那就是订阅的话题的消息类型，并且包含 publisher 所发送的信息内容。

定义订阅回调函数

// Function prototype:
void (* rclc_subscription_callback_t)(const void *);

// Implementation example:
void subscription_callback(const void * msgin)
{
  // Cast received message to used type
  const std_msgs__msg__Int32 * msg = (const std_msgs__msg__Int32 *)msgin;

  // Process message
  printf("Received: %d\n", msg->data);
}

在已经初始化 executor 和 subscriber 后，将回调配置到 executor 中。在 executor spin 后就会进行订阅。

// Message object to receive publisher data
std_msgs__msg__Int32 msg;

// Add subscription to the executor
rcl_ret_t rc = rclc_executor_add_subscription(
  &executor, &subscriber, &msg,
  &subscription_callback, ON_NEW_DATA);

if (RCL_RET_OK != rc) {
  ...  // Handle error
  return -1;
}

// Spin executor to receive messages
rclc_executor_spin(&executor);

Cleaning Up

// Destroy subscriber
rcl_subscription_fini(&subscriber, &node);

example

使用 Subscriber 订阅一个控制灯光的 topic，通过 topic 控制灯光的亮灭

订阅 /led_topic 话题，若接受到数字 0 则关闭板载 led 灯，若接受到数字 1 则打开 led 灯。

#define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){error_loop();}}
#define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){}}

void error_loop() {
    while(1) {
        delay(100);
    }
}

#include <Arduino.h>
#include <micro_ros_platformio.h>
#include <WiFi.h>
#include <rcl/rcl.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>
#include <std_msgs/msg/int32.h>

rclc_executor_t executor;
rclc_support_t support;
rcl_allocator_t allocator;
rcl_node_t node;
std_msgs__msg__Int32 msg;
rcl_subscription_t subscriber;

void subscription_callback(const void * msgin)
{
    const std_msgs__msg__Int32 * msg = (const std_msgs__msg__Int32 *)msgin;
    Serial.printf("Received: %d\n", msg->data);
    if(msg->data == 1) {
        digitalWrite(2, HIGH);
    } else digitalWrite(2, LOW);
}

void setup()
{
    Serial.begin(115200);
    // Set WiFi
    IPAddress agent_ip;
    agent_ip.fromString("IP");
    set_microros_wifi_transports("SSID", "PASSWD", agent_ip, PORT);

    delay(2000);

    allocator = rcl_get_default_allocator();

    RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));
    RCCHECK(rclc_node_init_default(&node, "Test_Node", "", &support));
    RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator));

    // Initialize a reliable subscriber
    RCCHECK(rclc_subscription_init_default(
        &subscriber, 
        &node,
        ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
        "led_topic"));
    // Add subscriber to executor
    RCCHECK(rclc_executor_add_subscription(
        &executor, 
        &subscriber, 
        &msg,
        &subscription_callback, 
        ON_NEW_DATA));

    pinMode(2, OUTPUT);
}

void loop()
{
    delay(100);
    RCCHECK(rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100)));
}

注意:
所有的教程中都是直接在声明并初始化，而上述例子中，node、executor、support 等都是在函数外声明的。
这是因为若在 Arduino 的 setup 函数直接声明则是局部变量，在 loop 函数中执行时就会发生错误

Services

介绍完使用 Topic 进行通信，接下来介绍如何使用 Service 来进行节点间通信。C/S 架构由 Client 和 Server 组成。所以我们也需要对 Server 和 Client 分别进行学习。

Server

一样是由初始化开始，Server 负责对 Client 的请求进行处理，并将结果返回给 Client。

Initialization

根据 QoS 由三种初始化方法

Reliable (default):

// Service server object
rcl_service_t service;
const char * service_name = "/addtwoints";

// Get message type support
const rosidl_message_type_support_t * type_support =
  ROSIDL_GET_SRV_TYPE_SUPPORT(example_interfaces, srv, AddTwoInts);

// Initialize server with default configuration
rcl_ret_t rc = rclc_service_init_default(
  &service, &node,
  type_support, service_name);

if (rc != RCL_RET_OK) {
  ...  // Handle error
  return -1;
}

Best effort:

// Service server object
rcl_service_t service;
const char * service_name = "/addtwoints";

// Get message type support
const rosidl_message_type_support_t * type_support =
  ROSIDL_GET_SRV_TYPE_SUPPORT(example_interfaces, srv, AddTwoInts);

// Initialize server with default configuration
rcl_ret_t rc = rclc_service_init_best_effort(
  &service, &node,
  type_support, service_name);

if (rc != RCL_RET_OK) {
  ...  // Handle error
  return -1;
}

Custom QoS:

// Service server object
rcl_service_t service;
const char * service_name = "/addtwoints";

// Get message type support
const rosidl_message_type_support_t * type_support =
  ROSIDL_GET_SRV_TYPE_SUPPORT(example_interfaces, srv, AddTwoInts);

// Set service QoS
const rmw_qos_profile_t * qos_profile = &rmw_qos_profile_services_default;

// Initialize server with customized quality-of-service options
rcl_ret_t rc = rclc_service_init(
  &service, &node, type_support,
  service_name, qos_profile);

if (rc != RCL_RET_OK) {
  ...  // Handle error
  return -1;
}

之后便是需要注册 Server 的回调函数，用于处理 Client 发送的请求

Callback

当请求发送到 Server 时，executor 将会调用已经注册的回调函数，其参数包含 request 和 response 的消息指针。request_msg 包含 Client 发送的消息，response_msg 则需要在函数里进行修改赋值，在函数结束后就会将消息发送给请求的 Client。

使用 AddTwoInts.srv 消息类型进行讲解

int64 a
int64 b
---
int64 sum

Client 的请求包含 a,b 两个值，sum 是 Server 接受到请求处理后返回的响应结果。

// Function prototype:
void (* rclc_service_callback_t)(const void *, void *);

// Implementation example:
void service_callback(const void * request_msg, void * response_msg){
  // Cast messages to expected types
  example_interfaces__srv__AddTwoInts_Request * req_in =
    (example_interfaces__srv__AddTwoInts_Request *) request_msg;
  example_interfaces__srv__AddTwoInts_Response * res_in =
    (example_interfaces__srv__AddTwoInts_Response *) response_msg;

  // Handle request message and set the response message values
  printf("Client requested sum of %d and %d.\n", (int) req_in->a, (int) req_in->b);
  res_in->sum = req_in->a + req_in->b;
}

回调函数定义之后，便需要将回调函数进行注册，需要在 executor 中进行添加注册。下面的示例代码中的一些对象的创建在上一步已经完成。

// Service message objects
example_interfaces__srv__AddTwoInts_Response response_msg;
example_interfaces__srv__AddTwoInts_Request request_msg;

// Add server callback to the executor
rc = rclc_executor_add_service(
  &executor, &service, &request_msg,
  &response_msg, service_callback);

if (rc != RCL_RET_OK) {
  ...  // Handle error
  return -1;
}

// Spin executor to receive requests
rclc_executor_spin(&executor);

Client

Client 用于向 Server 发送 request。Server 也会根据 request 返回 response。

Initialization

Reliable (default):

// Service client object
rcl_client_t client;
const char * service_name = "/addtwoints";

// Get message type support
const rosidl_message_type_support_t * type_support =
  ROSIDL_GET_SRV_TYPE_SUPPORT(example_interfaces, srv, AddTwoInts);

// Initialize client with default configuration
rcl_ret_t rc = rclc_client_init_default(
  &client, &node,
  type_support, service_name);

if (rc != RCL_RET_OK) {
  ...  // Handle error
  return -1;
}

Best effort:

// Service client object
rcl_client_t client;
const char * service_name = "/addtwoints";

// Get message type support
const rosidl_message_type_support_t * type_support =
  ROSIDL_GET_SRV_TYPE_SUPPORT(example_interfaces, srv, AddTwoInts);

// Initialize client with default configuration
rcl_ret_t rc = rclc_client_init_best_effort(
  &client, &node,
  type_support, service_name);

if (rc != RCL_RET_OK) {
  ...  // Handle error
  return -1;
}

Custom QoS:

// Service client object
rcl_client_t client;
const char * service_name = "/addtwoints";

// Get message type support
const rosidl_message_type_support_t * type_support =
  ROSIDL_GET_SRV_TYPE_SUPPORT(example_interfaces, srv, AddTwoInts);

// Set client QoS
const rmw_qos_profile_t * qos_profile = &rmw_qos_profile_services_default;

// Initialize server with customized quality-of-service options
rcl_ret_t rc = rclc_client_init(
  &client, &node, type_support,
  service_name, qos_profile);

if (rc != RCL_RET_OK) {
  ...  // Handle error
  return -1;
}

Client 相比 Server，需要主动发送一个请求到 Server。所以 Client 具有一个发送消息的功能，同时还需要具有接收到 Server 发送的 response 并进行处理的回调功能。

发送 Request

使用 AddTwoInts.srv 消息格式向 Server 发送一个请求。
rcl_send_request 函数用于发送请求

// Request message object (Must match initialized client type support)
example_interfaces__srv__AddTwoInts_Request request_msg;

// Initialize request message memory and set its values
example_interfaces__srv__AddTwoInts_Request__init(&request_msg);
request_msg.a = 24;
request_msg.b = 42;

// Sequence number of the request (Populated in rcl_send_request)
int64_t sequence_number;

// Send request
rcl_send_request(&client, &request_msg, &sequence_number);

// Spin the executor to get the response
rclc_executor_spin(&executor);

Callback

定义回调函数，函数参数为 response_msg 的指针

// Function prototype:
void (* rclc_client_callback_t)(const void *);

// Implementation example:
void client_callback(const void * response_msg){
  // Cast response message to expected type
  example_interfaces__srv__AddTwoInts_Response * msgin =
    (example_interfaces__srv__AddTwoInts_Response * ) response_msg;

  // Handle response message
  printf("Received service response %ld + %ld = %ld\n", req.a, req.b, msgin->sum);
}

将回调函数添加到 executor 中

// Response message object
example_interfaces__srv__AddTwoInts_Response res;

// Add client callback to the executor
rcl_ret_t rc = rclc_executor_add_client(&executor, &client, &res, client_callback);

if (rc != RCL_RET_OK) {
  ...  // Handle error
  return -1;
}

// Spin executor to receive requests
rclc_executor_spin(&executor);

Cleaning Up

如果想删除已经注册的 server 和 client，执行下面函数

// Destroy service server and client
rcl_service_fini(&service, &node);
rcl_client_fini(&client, &node);

这将删除 agent 上任何自动创建的运行结构，并在客户端释放已使用的内存。

ROS2 客户端服务端订阅/发布

原创文章作者：张珊珊。如若转载，请注明出处：古月居 https://www.guyuehome.com/43109

打赏 0

上一篇：旭日X3派开发｜ micro-ROS C API详解 (一)

下一篇：旭日X3派开发｜项目实战

精选旭日X3派开发 ｜ micro-ROS C API详解(二)

张珊珊

Publisher

Initialization

发布消息

Cleaning Up

example

Subscriber

Initialization

订阅消息

Cleaning Up

example

Services

Server

Initialization

Callback

Client

Initialization

发送 Request

Callback

Cleaning Up

为你推荐

当旭日X3派遇上ChatGpt

精选【图像与点云融合教程（四）】海康相机 ROS2 功能包

精选【ROS2入门】理解 ROS 2 节点

闲鱼卖出一个吃灰板子|ROS2安装Gazebo11并运行两轮差速样例程序

ROS 2 | 主题 -topics-

ROS 2最新版Dashing，你尝鲜了么？

评论（0）

关于作者

张珊珊

5

0

1

2

旭日X3派开发 ｜ MacOS搭建开发环境

精选旭日X3派开发 ｜ micro-ROS C API详解(二)

精选旭日X3派开发 ｜ 项目实战

相关推荐

ROS2中的行为树 BehaviorTree

[ROS2] 查看Action 信息的常用命令

精选ros2控制ABB机械臂写字绘图

Ubuntu 22.04 LTS安装ROS2 (ros-humble-desktop)

ROS2学习笔记27--实现自定义内存分配器

[ros2学习]-CLI Tools - 配置环境

热门泡泡

30积分 失眠，聊聊自己搞ROS的心得体会吧

ros学习路线

30积分 TF_REPEATED_DATA ignoring data错误

各位大佬，有什么ROS定位算法推荐吗

5积分 想买能用ROS2的开发套件。或者开发板

5积分 ros中启动gazebo时报错

TA的专栏

旭日X3派实战

专栏导航

给作者打赏

忘记密码

修改头像

添加你感兴趣的标签

举报类型（必选）

举报详情（选填）

精选旭日X3派开发｜ micro-ROS C API详解(二)

旭日X3派开发｜ MacOS搭建开发环境

精选旭日X3派开发｜ micro-ROS C API详解(二)

精选旭日X3派开发｜项目实战

30积分失眠，聊聊自己搞ROS的心得体会吧

5积分想买能用ROS2的开发套件。或者开发板