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

系列文章目录

• ROS2与C++入门教程-目录

• ROS2与C++入门教程-新建ros2工作空间

• ROS2与C++入门教程-新建ros2包

• ROS2与C++入门教程-编写订阅和发布

• ROS2与C++入门教程-编写服务端和客户端

• ROS2与C++入门教程-创建消息（msg）文件

• ROS2与C++入门教程-创建服务（srv）文件

• ROS2与C++入门教程-创建ros2接口

• ROS2与C++入门教程-使用参数

• ROS2与C++入门教程-创建和使用插件

• ROS2与C++入门教程-编写动作服务器

• ROS2与C++入门教程-编写动作客户端

• ROS2与C++入门教程-Topic Statistics查看话题

• ROS2与C++入门教程-增加头文件

• ROS2与C++入门教程-在C++包里增加python支持

• ROS2与C++入门教程-增加命名空间

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

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

• ROS2与C++入门教程-话题组件演示

• ROS2与C++入门教程-话题组件增加launch启动演示

• ROS2与C++入门教程-服务组件演示

• ROS2与C++入门教程-服务组件增加launch启动演示

• ROS2与C++入门教程-进程内(intra_process)话题发布和订阅演示

• ROS2与C++入门教程-进程内(intra_process)话题发布和订阅演示2

• ROS2与C++入门教程-进程内(intra_process)图像处理演示

• 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 
#include 
#include 
#include 
#include #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::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("topic", 10);auto timer_callback =[this]() -> void {auto message = std_msgs::msg::String();message.data = "Hello World! " + std::to_string(this->count_++);// Extract current threadauto curr_thread = string_thread_id();// Prep display messageRCLCPP_INFO(this->get_logger(), "\n<> 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::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 threadauto 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("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 commentsstd::bind(&DualThreadedNode::subscriber1_cb,  // First parameter is a reference to the functionthis,                               // What the function should be bound tostd::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 subscriber1subscription2_ = this->create_subscription("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 threadRCLCPP_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 messageRCLCPP_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::SharedPtr subscription1_;rclcpp::Subscription::SharedPtr subscription2_;
};int main(int argc, char * argv[])
{rclcpp::init(argc, argv);// You MUST use the MultiThreadedExecutor to use, well, multiple threadsrclcpp::executors::MultiThreadedExecutor executor;auto pubnode = std::make_shared();auto subnode = std::make_shared();  // This contains BOTH subscriber callbacks.// They will still run on different threads// One Node. Two callbacks. Two Threadsexecutor.add_node(pubnode);executor.add_node(subnode);executor.spin();rclcpp::shutdown();return 0;
}

• 编译package.xml
• 在ament_cmake后增加

rclcpp
std_msgs

• 编译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(TARGETSmultithreaded_executorDESTINATION 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]: 
<> 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]: 
<> 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]: 
<> 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]: 
<> 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]: 
<> 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]: 
<> 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

本文来自互联网用户投稿，文章观点仅代表作者本人，不代表本站立场，不承担相关法律责任。如若转载，请注明出处。 如若内容造成侵权/违法违规/事实不符，请点击【内容举报】进行投诉反馈！