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

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

说明：

• 介绍在ros2下如何使用单线程　

步骤：

• 如果包已有，跳过，如果没有新建一个包cpp_threads

cd ~/dev_ws/src
ros2 pkg create --build-type ament_cmake cpp_threads

• 进入src目录，新建文件singlethreaded_executor.cpp

cd dev_ws/src/cpp_threads/src
touch singlethreaded_executor.cpp

• 内容如下：　

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

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

using namespace std::chrono_literals;
using std::placeholders::_1;

/**
 * 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 SingleThreadedNode : public rclcpp::Node
{
public:
  SingleThreadedNode()
  : Node("SingleThreadedNode")
  {
    // Select MutuallyExclusive or Reentrant Callback Group Type
    my_callback_group = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
    // my_callback_group = create_callback_group(rclcpp::CallbackGroupType::Reentrant);

    // Run timer at 10fps
    timer_ = this->create_wall_timer(
      100ms, std::bind(&SingleThreadedNode::timer_callback, this), my_callback_group);

    // Run subscription with depth 10
    subscription_ = this->create_subscription<std_msgs::msg::String>(
      "topic", 10, std::bind(&SingleThreadedNode::topic_callback, this, _1));
  }

private:
  void timer_callback()
  {
    std::cout << "timer callback" << std::endl;

    // Sleep here, to produce a long callback
    std::this_thread::sleep_for(5ms);
  }

  void topic_callback(const std_msgs::msg::String &) const
  {
    std::cout << "subscription callback" << std::endl;
  }

  rclcpp::TimerBase::SharedPtr timer_;
  rclcpp::CallbackGroup::SharedPtr my_callback_group;
  rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscription_;
};

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

  auto node1 = std::make_shared<PublisherNode>();
  auto node2 = std::make_shared<SingleThreadedNode>();

  // Select SingleThreadedExecutor or MultiThreadedExecutor
  rclcpp::executors::SingleThreadedExecutor executor;
  // rclcpp::executors::MultiThreadedExecutor executor;

  executor.add_node(node1);
  executor.add_node(node2);
  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(singlethreaded_executor src/singlethreaded_executor.cpp)
ament_target_dependencies(singlethreaded_executor rclcpp std_msgs)

• 增加可执行文件位置，ros2 run可以找到这个可执行文件

install(TARGETS
  singlethreaded_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  singlethreaded_executor 

• 效果如下：

$ ros2 run cpp_threads singlethreaded_executor
timer callback
timer callback
timer callback
timer callback
[INFO] [1651811561.035566258] [PublisherNode]: 
<<THREAD 6076814379292605570>> Publishing 'Hello World! 0'
subscription callback
timer callback
timer callback
timer callback
timer callback
timer callback
[INFO] [1651811561.535618511] [PublisherNode]: 
<<THREAD 6076814379292605570>> Publishing 'Hello World! 1'
subscription callback
timer callback
timer callback
timer callback
timer callback
timer callback
[INFO] [1651811562.035538727] [PublisherNode]: 
<<THREAD 6076814379292605570>> Publishing 'Hello World! 2'
subscription callback
timer callback
timer callback
timer callback
timer callback
timer callback