机器人零拷贝数据传输编程开发

编程开发

为了方便大家使用，TogetherROS针对零拷贝功能进行了封装，风格类似ROS2中话题通信的接口，还是话题通信一样的流程，我们只需要修改几个函数就可以实现啦。

运行例程

$ source /opt/tros/local_setup.bash
$ source install/local_setup.bash
$ ros2 run hbmem_pubsub talker
$ ros2 run hbmem_pubsub listener

代码解析

发布者publisher_hbmem.cpp：

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


#include "rclcpp/rclcpp.hpp"
#include "hbmem_pubsub/msg/sample_message.hpp"


using namespace std::chrono_literals;




class MinimalHbmemPublisher  : public rclcpp::Node {
 public:
  MinimalHbmemPublisher () : Node("minimal_hbmem_publisher"), count_(0) {
    // 创建publisher_hbmem，topic为"topic"，QOS为KEEPLAST(10)，以及默认的可靠传输
    publisher_ = this- >create_publisher_hbmem< hbmem_pubsub::msg::SampleMessage >(
        "topic", 10);


    // 定时器，每隔40毫秒调用一次timer_callback进行消息发送
    timer_ = this- >create_wall_timer(
        40ms, std::bind(&MinimalHbmemPublisher ::timer_callback, this));
  }


 private:
  // 定时器回调函数
  void timer_callback() {
    // 获取要发送的消息
    auto loanedMsg = publisher_- >borrow_loaned_message();
    // 判断消息是否可用，可能出现获取消息失败导致消息不可用的情况
    if (loanedMsg.is_valid()) {
      // 引用方式获取实际的消息
      auto& msg = loanedMsg.get();


      // 获取当前时间，单位为us
      auto time_now =
          std::chrono::duration_cast< std::chrono::microseconds >(
              std::chrono::steady_clock::now().time_since_epoch()).count();


      // 对消息的index和time_stamp进行赋值
      msg.index = count_;
      msg.time_stamp = time_now;


      // 打印发送消息
      RCLCPP_INFO(this- >get_logger(), "message: %d", msg.index);
      publisher_- >publish(std::move(loanedMsg));
      // 注意，发送后，loanedMsg已不可用
      // 计数器加一
      count_++;
    } else {
      // 获取消息失败，丢弃该消息
      RCLCPP_INFO(this- >get_logger(), "Failed to get LoanMessage!");
    }
  }


  // 定时器
  rclcpp::TimerBase::SharedPtr timer_;


  // hbmem publisher
  rclcpp::PublisherHbmem< hbmem_pubsub::msg::SampleMessage >::SharedPtr publisher_;


  // 计数器
  size_t count_;
};


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

订阅者subscriber_hbmem.cpp：

#include < memory >


#include "rclcpp/rclcpp.hpp"
#include "hbmem_pubsub/msg/sample_message.hpp"




class MinimalHbmemSubscriber  : public rclcpp::Node {
 public:
  MinimalHbmemSubscriber () : Node("minimal_hbmem_subscriber") {
    // 创建subscription_hbmem，topic为"sample"，QOS为KEEPLAST(10)，以及默认的可靠传输
    // 消息回调函数为topic_callback
    subscription_ =
        this- >create_subscription_hbmem< hbmem_pubsub::msg::SampleMessage >(
            "topic", 10,
            std::bind(&MinimalHbmemSubscriber ::topic_callback, this,
                      std::placeholders::_1));
  }


 private:
  // 消息回调函数
  void topic_callback(
      const hbmem_pubsub::msg::SampleMessage::SharedPtr msg) const {
    // 注意，msg只能在回调函数中使用，回调函数返回后，该消息就会被释放
    // 获取当前时间
    auto time_now =
        std::chrono::duration_cast< std::chrono::microseconds >(
            std::chrono::steady_clock::now().time_since_epoch())
            .count();
    // 计算延时并打印出来
    RCLCPP_INFO(this- >get_logger(), "msg %d, time cost %dus", msg- >index,
                time_now - msg- >time_stamp);
  }


  // hbmem subscription
  rclcpp::SubscriptionHbmem< hbmem_pubsub::msg::SampleMessage >::SharedPtr
      subscription_;
};




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