micro_ros笔记——best_effort订阅者程序编写

测试环境：

• vmware虚拟机+ubuntu20.04+ros2 foxy
• esp32 (micro_ros_arduino)

0. 写在前面

• micro_ros中的QoS有default和best_effort，其中使用best_effort发布数据能够最大化发布频率，但是接收端也需要使用best_effort的subscription
• 本文编写一个best_effort的subscription

1. best_effort的subscription

1.1 下位机完整代码

• 打开micro_ros中的示例代码：micro-ros_publisher.ino
• 修改timer为5，即数据发布频率为200Hz
• 使用rclc_publisher_init_best_effort替换rclc_publisher_init_default
• 注释loop中的delay(100);

#include 

#include 
#include 
#include 
#include 
#include 

#include 

rcl_publisher_t publisher;
std_msgs__msg__Int32 msg;
rclc_executor_t executor;
rclc_support_t support;
rcl_allocator_t allocator;
rcl_node_t node;
rcl_timer_t timer;

#define LED_PIN 13

#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){
    digitalWrite(LED_PIN, !digitalRead(LED_PIN));
    delay(100);
  }
}

void timer_callback(rcl_timer_t * timer, int64_t last_call_time)
{  
  RCLC_UNUSED(last_call_time);
  if (timer != NULL) {
    RCSOFTCHECK(rcl_publish(&publisher, &msg, NULL));
    msg.data++;
  }
}

void setup() {
  set_microros_transports();
  
  pinMode(LED_PIN, OUTPUT);
  digitalWrite(LED_PIN, HIGH);  
  
  delay(2000);

  allocator = rcl_get_default_allocator();

  //create init_options
  RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));

  // create node
  RCCHECK(rclc_node_init_default(&node, "micro_ros_arduino_node", "", &support));

  // create publisher
  RCCHECK(rclc_publisher_init_best_effort(
    &publisher,
    &node,
    ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
    "micro_ros_arduino_node_publisher"));

  // create timer,
  const unsigned int timer_timeout = 5;
  RCCHECK(rclc_timer_init_default(
    &timer,
    &support,
    RCL_MS_TO_NS(timer_timeout),
    timer_callback));

  // create executor
  RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator));
  RCCHECK(rclc_executor_add_timer(&executor, &timer));

  msg.data = 0;
}

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

1.2 上位机完整代码

• 可从github仓库拉取代码
• 除了cpp代码外，还需要建立ros2 package，修改CMakeLists和package.xml文件，不在此赘述；
• 可参考官方文档

#include 
#include 

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/int32.hpp"

using std::placeholders::_1;

class MinimalSubscriber : public rclcpp::Node
{
public:
  MinimalSubscriber()
  : Node("minimal_subscriber")
  {
    subscription_ = this->create_subscription<std_msgs::msg::Int32>("micro_ros_arduino_node_publisher", rclcpp::SensorDataQoS(), std::bind(&MinimalSubscriber::topic_callback, this, _1));
  }

private:
  void topic_callback(const std_msgs::msg::Int32::SharedPtr msg) const
  {
    RCLCPP_INFO(this->get_logger(), "The int32 data is [%d] ", msg->data);
  }
  rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr subscription_;
};

int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<MinimalSubscriber>());
  rclcpp::shutdown();
  return 0;
}

1.3 代码解析

• 使用create_subscription创建订阅者，topic名字为"micro_ros_arduino_node_publisher"，这一名字与下位机代码一致；
• create_subscription中第二个参数使用 rclcpp::SensorDataQoS()；
• 在回调函数中显示接收到的topic数据

2. 实测

• 正确烧录下位机代码
• 将下位机连接ros2,并给串口提权限

sudo chmod -R 777 /dev/ttyUSB0

• 开启micro_ros_agent

  ros2 run micro_ros_agent micro_ros_agent serial --dev /dev/ttyUSB0

• 使用colcon build编译工作空间，并进行source
• 开启subscription节点

ros2 run sub_best_effort listener 

• 可以看到终端进行了输出：
  
• 使用rqt-graph查看节点关系：