< >
Home » ROS2与C++入门教程 » ROS2与C++入门教程-多线程演示

ROS2与C++入门教程-多线程演示

ROS2与C++入门教程-多线程演示

说明：

  • 介绍如何在ros2的rclcpp中使用多线程。演示一个线程用于发布话题，一个线程的订阅话题

步骤：

  • 新建一个包cpp_threads
cd ~/dev_ws/src
ros2 pkg create --build-type ament_cmake cpp_threads
  • 进入src目录，新建文件multithreaded_executor.cpp
cd dev_ws/src/cpp_threads/src
touch multithreaded_executor.cpp
  • 内容如下：
#include <chrono>
#include <functional>
#include <memory>
#include <string>
#include <thread>

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

using namespace std::chrono_literals;

/**
 * A small convenience function for converting a thread ID to a string
 **/
std::string string_thread_id()
{
  auto hashed = std::hash<std::thread::id>()(std::this_thread::get_id());
  return std::to_string(hashed);
}

/* For this example, we will be creating a publishing node like the one in minimal_publisher.
 * We will have a single subscriber node running 2 threads. Each thread loops at different speeds, and
 * just repeats what it sees from the publisher to the screen.
 */

class PublisherNode : public rclcpp::Node
{
public:
  PublisherNode()
  : Node("PublisherNode"), count_(0)
  {
    publisher_ = this->create_publisher<std_msgs::msg::String>("topic", 10);
    auto timer_callback =
      [this]() -> void {
        auto message = std_msgs::msg::String();
        message.data = "Hello World! " + std::to_string(this->count_++);

        // Extract current thread
        auto curr_thread = string_thread_id();

        // Prep display message
        RCLCPP_INFO(
          this->get_logger(), "\n<<THREAD %s>> Publishing '%s'",
          curr_thread.c_str(), message.data.c_str());
        this->publisher_->publish(message);
      };
    timer_ = this->create_wall_timer(500ms, timer_callback);
  }

private:
  rclcpp::TimerBase::SharedPtr timer_;
  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
  size_t count_;
};

class DualThreadedNode : public rclcpp::Node
{
public:
  DualThreadedNode()
  : Node("DualThreadedNode")
  {
    /* These define the callback groups
     * They don't really do much on their own, but they have to exist in order to
     * assign callbacks to them. They're also what the executor looks for when trying to run multiple threads
     */
    callback_group_subscriber1_ = this->create_callback_group(
      rclcpp::CallbackGroupType::MutuallyExclusive);
    callback_group_subscriber2_ = this->create_callback_group(
      rclcpp::CallbackGroupType::MutuallyExclusive);

    // Each of these callback groups is basically a thread
    // Everything assigned to one of them gets bundled into the same thread
    auto sub1_opt = rclcpp::SubscriptionOptions();
    sub1_opt.callback_group = callback_group_subscriber1_;
    auto sub2_opt = rclcpp::SubscriptionOptions();
    sub2_opt.callback_group = callback_group_subscriber2_;

    subscription1_ = this->create_subscription<std_msgs::msg::String>(
      "topic",
      rclcpp::QoS(10),
      // std::bind is sort of C++'s way of passing a function
      // If you're used to function-passing, skip these comments
      std::bind(
        &DualThreadedNode::subscriber1_cb,  // First parameter is a reference to the function
        this,                               // What the function should be bound to
        std::placeholders::_1),             // At this point we're not positive of all the
                                            // parameters being passed
                                            // So we just put a generic placeholder
                                            // into the binder
                                            // (since we know we need ONE parameter)
      sub1_opt);                  // This is where we set the callback group.
                                  // This subscription will run with callback group subscriber1

    subscription2_ = this->create_subscription<std_msgs::msg::String>(
      "topic",
      rclcpp::QoS(10),
      std::bind(
        &DualThreadedNode::subscriber2_cb,
        this,
        std::placeholders::_1),
      sub2_opt);
  }

private:
  /**
   * Simple function for generating a timestamp
   * Used for somewhat ineffectually demonstrating that the multithreading doesn't cripple performace
   */
  std::string timing_string()
  {
    rclcpp::Time time = this->now();
    return std::to_string(time.nanoseconds());
  }

  /**
   * Every time the Publisher publishes something, all subscribers to the topic get poked
   * This function gets called when Subscriber1 is poked (due to the std::bind we used when defining it)
   */
  void subscriber1_cb(const std_msgs::msg::String::ConstSharedPtr msg)
  {
    auto message_received_at = timing_string();

    // Extract current thread
    RCLCPP_INFO(
      this->get_logger(), "THREAD %s => Heard '%s' at %s",
      string_thread_id().c_str(), msg->data.c_str(), message_received_at.c_str());
  }

  /**
   * This function gets called when Subscriber2 is poked
   * Since it's running on a separate thread than Subscriber 1, it will run at (more-or-less) the same time!
   */
  void subscriber2_cb(const std_msgs::msg::String::ConstSharedPtr msg)
  {
    auto message_received_at = timing_string();

    // Prep display message
    RCLCPP_INFO(
      this->get_logger(), "THREAD %s => Heard '%s' at %s",
      string_thread_id().c_str(), msg->data.c_str(), message_received_at.c_str());
  }

  rclcpp::CallbackGroup::SharedPtr callback_group_subscriber1_;
  rclcpp::CallbackGroup::SharedPtr callback_group_subscriber2_;
  rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscription1_;
  rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscription2_;
};

int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);

  // You MUST use the MultiThreadedExecutor to use, well, multiple threads
  rclcpp::executors::MultiThreadedExecutor executor;
  auto pubnode = std::make_shared<PublisherNode>();
  auto subnode = std::make_shared<DualThreadedNode>();  // This contains BOTH subscriber callbacks.
                                                        // They will still run on different threads
                                                        // One Node. Two callbacks. Two Threads
  executor.add_node(pubnode);
  executor.add_node(subnode);
  executor.spin();
  rclcpp::shutdown();
  return 0;
}
  • 编译package.xml
  • 在<buildtool_depend>ament_cmake</buildtool_depend>后增加
<depend>rclcpp</depend>
<depend>std_msgs</depend>
  • 编译CMakelist.txt
  • 在find_package(ament_cmake REQUIRED)后增加
find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)
  • 在增加可执行文件，ros2 run能够调用的名称
add_executable(multithreaded_executor src/multithreaded_executor.cpp)
ament_target_dependencies(multithreaded_executor rclcpp std_msgs)
  • 增加可执行文件位置，ros2 run可以找到这个可执行文件
install(TARGETS
  multithreaded_executor
  DESTINATION lib/${PROJECT_NAME}
)
  • 安装相关依赖
cd ~/dev_ws/
rosdep install -i --from-path src --rosdistro galactic -y
  • 编译包
colcon build --symlink-install --packages-select cpp_threads
  • 加载工作空间
. install/setup.bash
  • 执行
ros2 run cpp_threads multithreaded_executor
  • 效果如下：
$ ros2 run cpp_threads multithreaded_executor
[INFO] [1651808341.560346425] [PublisherNode]: 
<<THREAD 14073912263493022284>> Publishing 'Hello World! 0'
[INFO] [1651808341.560782457] [DualThreadedNode]: THREAD 12781006303109063165 => Heard 'Hello World! 0' at 1651808341560771372
[INFO] [1651808341.560981750] [DualThreadedNode]: THREAD 14073912263493022284 => Heard 'Hello World! 0' at 1651808341560970655
[INFO] [1651808342.060389617] [PublisherNode]: 
<<THREAD 17356008714308031491>> Publishing 'Hello World! 1'
[INFO] [1651808342.060738294] [DualThreadedNode]: THREAD 6975958009959714317 => Heard 'Hello World! 1' at 1651808342060725810
[INFO] [1651808342.060783434] [DualThreadedNode]: THREAD 17356008714308031491 => Heard 'Hello World! 1' at 1651808342060772314
[INFO] [1651808342.560456090] [PublisherNode]: 
<<THREAD 1791470884547062228>> Publishing 'Hello World! 2'
[INFO] [1651808342.560708367] [DualThreadedNode]: THREAD 14073912263493022284 => Heard 'Hello World! 2' at 1651808342560696566
[INFO] [1651808342.560833530] [DualThreadedNode]: THREAD 1791470884547062228 => Heard 'Hello World! 2' at 1651808342560818497
[INFO] [1651808343.060426560] [PublisherNode]: 
<<THREAD 520078779124681865>> Publishing 'Hello World! 3'
[INFO] [1651808343.060688311] [DualThreadedNode]: THREAD 15999819906855827312 => Heard 'Hello World! 3' at 1651808343060675877
[INFO] [1651808343.060711107] [DualThreadedNode]: THREAD 520078779124681865 => Heard 'Hello World! 3' at 1651808343060702653
[INFO] [1651808343.560402444] [PublisherNode]: 
<<THREAD 14603931345993028093>> Publishing 'Hello World! 4'
[INFO] [1651808343.560678334] [DualThreadedNode]: THREAD 6975958009959714317 => Heard 'Hello World! 4' at 1651808343560664444
[INFO] [1651808343.560690455] [DualThreadedNode]: THREAD 14603931345993028093 => Heard 'Hello World! 4' at 1651808343560681564
[INFO] [1651808344.060394028] [PublisherNode]: 
<<THREAD 17356008714308031491>> Publishing 'Hello World! 5'
[INFO] [1651808344.060664727] [DualThreadedNode]: THREAD 520078779124681865 => Heard 'Hello World! 5' at 1651808344060649093
[INFO] [1651808344.060727468] [DualThreadedNode]: THREAD 17356008714308031491 => Heard 'Hello World! 5' at 1651808344060718433

参考：

　- 资料1

纠错,疑问,交流: 请进入讨论区或 点击加入Q群

获取最新文章: 扫一扫右上角的二维码加入“创客智造”公众号


标签: ROS2与C++入门教程