機器人零拷貝數據傳輸編程開發(fā)

編程開發(fā)

為了方便大家使用，TogetherROS針對零拷貝功能進行了封裝，風格類似ROS2中話題通信的接口，還是話題通信一樣的流程，我們只需要修改幾個函數就可以實現(xiàn)啦。

運行例程

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

代碼解析

發(fā)布者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) {
    // 創(chuàng)建publisher_hbmem，topic為"topic"，QOS為KEEPLAST(10)，以及默認的可靠傳輸
    publisher_ = this- >create_publisher_hbmem< hbmem_pubsub::msg::SampleMessage >(
        "topic", 10);


    // 定時器，每隔40毫秒調用一次timer_callback進行消息發(fā)送
    timer_ = this- >create_wall_timer(
        40ms, std::bind(&MinimalHbmemPublisher ::timer_callback, this));
  }


 private:
  // 定時器回調函數
  void timer_callback() {
    // 獲取要發(fā)送的消息
    auto loanedMsg = publisher_- >borrow_loaned_message();
    // 判斷消息是否可用，可能出現(xiàn)獲取消息失敗導致消息不可用的情況
    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;


      // 打印發(fā)送消息
      RCLCPP_INFO(this- >get_logger(), "message: %d", msg.index);
      publisher_- >publish(std::move(loanedMsg));
      // 注意，發(fā)送后，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") {
    // 創(chuàng)建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;
}