【ROS2】spin_smoe的使用示例代码及踩坑点

前言

        spin()函数虽然使用方便，但灵活性不高，运行到spin()的时候，就阻塞了，所以有时候用spin_some()会更方便，而ros2的spin_some()与ros1的spin_once()使用方法类似，不过使用起来还是有一些不同的地方，网上没有spin_some()相关简洁明了的使用示例，笔者在这里给出简单的使用示例，以及spin_some()使用时一些可能踩坑的点。关于spin_some()的一些解释我引用这篇博客的一个片段，如下图：

 

spin_smoe的使用踩坑点

关于spin_some正确的使用代码示例在本文最后，但笔者建议把踩坑点部分看完再看正确的代码示例，会有更明确的认知。下面是一段很简单的发布话题代码，每隔100ms就发送“hello word”字符串并计数：

class MinimalPublisher : public rclcpp::Node
{
public:
  MinimalPublisher()
  : Node("minimal_publisher"), count_(0)//话题数据的计数参数，一开始是0
  {
    RCLCPP_INFO(this->get_logger(),"start");//打印是否构造
    publisher_ = this->create_publisher<std_msgs::msg::String>("topic", 10);
    MinimalPublisher::callback();//后续会更改这句
  }
  ~MinimalPublisher()
  {
    RCLCPP_INFO(this->get_logger(),"end");//打印是否析构
  }

private:
  void callback()
  {
    auto message = std_msgs::msg::String();
    message.data = "Hello, world! " + std::to_string(count_++);
    RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", message.data.c_str());
    publisher_->publish(message);
  }
  //rclcpp::TimerBase::SharedPtr timer_;
  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
  size_t count_;
};

int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  rclcpp::Rate loop_rate(100ms);//设置循环频率
  auto node = std::make_shared<MinimalPublisher>();//后续会更改这段
  while(rclcpp::ok())
  {
    rclcpp::spin_some(node);
    loop_rate.sleep();
  }
  //rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}

可能和很多人预想的一样，在while(rclcpp::ok() )循环之前实例化一个node对象，然后进入循环的话，最后只会发布一条“hello word”，并且只运行了一次构造函数，析构函数在while循环中一直没有运行。

那把auto node = std::make_shared<MinimalPublisher>();放到while循环里面不就能按100ms的频率重复发送话题数据了吗？答案是：不完全能，也会存在一些问题，具体存在的问题往下看。

int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  rclcpp::Rate loop_rate(100ms);//设置循环频率
  //auto node = std::make_shared<MinimalPublisher>();//后续会更改这段
  while(rclcpp::ok())
  {
    auto node = std::make_shared<MinimalPublisher>();//移到这里
    rclcpp::spin_some(node);
    loop_rate.sleep();
  }
  //rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}


//------------------------------------------
//上面的while循环部分代码效果等效下面这段
while(rclcpp::ok())
  {
    //auto node = std::make_shared<MinimalPublisher>();//移到这里
    rclcpp::spin_some(std::make_shared<MinimalPublisher>());
    loop_rate.sleep();
  }

现在简单地修改一下后，能按100ms的频率发送，效果图如下：

 很明显，虽然能反复发送话题，但话题的计数并没有递增，并且在反复地构造和析构，浪费系统资源，而更严重的问题还在话题的订阅端，订阅端话题接受不设延时，及发及收，订阅端的效果图如下：

 同样的，也只能收到计数为0的那条话题信息，并且注意左边的时间戳，接收数据周期并不是100ms，频率非常地不稳定，根本不能达到预设的效果！

正式可用代码示例部分

在看了踩坑的部分，再看正确（能用）的示例部分，先看发布话题数据的代码：

class MinimalPublisher : public rclcpp::Node
{
public:
  MinimalPublisher()
  : Node("minimal_publisher"), count_(0)
  {
    RCLCPP_INFO(this->get_logger(),"start");
    publisher_ = this->create_publisher<std_msgs::msg::String>("topic", 10);
    MinimalPublisher::timer_callback();
    
    //使用计时器，进行回调
    timer_ = this->create_wall_timer(
      100ms, std::bind(&MinimalPublisher::timer_callback, this));
  }
  ~MinimalPublisher()
  {
    RCLCPP_INFO(this->get_logger(),"end");
  }

private:
  void timer_callback()
  {
    auto message = std_msgs::msg::String();
    message.data = "Hello, world! " + std::to_string(count_++);
    RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", message.data.c_str());
    publisher_->publish(message);
  }
  rclcpp::TimerBase::SharedPtr timer_;
  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
  size_t count_;
};

int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  //rclcpp::Rate loop_rate(100ms);
  auto node = std::make_shared<MinimalPublisher>();//在循环前实例化node对象
  while(rclcpp::ok())
  {
    rclcpp::spin_some(node);
    //printf("keep loop\n");
    //loop_rate.sleep();
  }
  //rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}

代码改动的思路就是：防止重复的构造与析构，并且保留能按设定频率循环发送话题的职能。

具体改动的代码不多，还是让node对象在在文化while循环之前实例化，loop_rate的延时部分注释掉，在节点类里面启用一个计时器，设定回调的周期为100ms，下面我们来看一下效果图：

发送端按100ms的周期发布话题数据，并且只运行一次构造函数，话题数据的计数也在累加。

然后我们再来看一下订阅端的效果是否修正了：

 

可见订阅端稳定按100ms的周期接收最新的话题数据，频率不稳定的问题解决了。订阅端的示例代码如下：

using std::placeholders::_1;

class MinimalSubscriber : public rclcpp::Node
{
  public:
    MinimalSubscriber()
    : Node("minimal_subscriber")
    {
      subscription_ = this->create_subscription<std_msgs::msg::String>(
      "topic", 1, std::bind(&MinimalSubscriber::topic_callback, this, _1));
    }

  private:
    void topic_callback(const std_msgs::msg::String::SharedPtr msg) //const
    {
      RCLCPP_INFO(this->get_logger(), "I heard: '%s'", msg->data.c_str());
    }
    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscription_;
};

int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  auto node = std::make_shared<MinimalSubscriber>();
    while(rclcpp::ok())
  {
    rclcpp::spin_some(node);

  }
  rclcpp::shutdown();
  return 0;
}

 总结&&补充

本文给出的只是简单的使用示例，但再有了原理性的认知后，读者可以拓展更多的进阶用法，还有一点需要注意的是：

用rclcpp::Rate设置while循环再与节点类的计时回调混合使用的话，谁的设定周期大（频率小），就按谁的频率运行，笔者认为这和单线程运行有关，例如：rclcpp::Rate设置while循环为500ms，而节点类里面的回调设置100ms，会发现最后按500ms的周期打印消息rclcpp::Rate为100ms，节点类里面的回调设置500ms，最后也是按500ms的周期打印消息。

最后，建议先看踩坑部分，再看正常代码示例。如有纰漏，还请留言勘误，感谢。