【ROS2】类ROS1的rate定时器写法（c++&python例子）

在ROS2官方教程里，代码和ROS1例程有很大不同，大部分节点都使用了类的写法，并且用到了很多C++的新功能。其中，发布消息是采用了一个定时器timer，并且注册一个回调函数实现的。那么，可不可以还是采用ROS1的rate sleep写法呢，答案是可以的。
例程如下：

#include <memory>

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
#include "tutorial_interfaces/msg/num.hpp"
#include "tutorial_interfaces/msg/sphere.hpp"

class MinimalPublisher : public rclcpp::Node {
 public:
  MinimalPublisher() : Node("minimal_publisher"), count_(0) {
    publisher_ = this->create_publisher<std_msgs::msg::String>("topic", 10);
    publisher2_ =
        this->create_publisher<tutorial_interfaces::msg::Num>("topicNum", 10);
    publisher3_ =
        this->create_publisher<tutorial_interfaces::msg::Sphere>("topicSp", 10);
    f = std::bind(&MinimalPublisher::pub_msg, this);
  }
  std::function<void(void)> f;

 private:
  // rclcpp::TimerBase::SharedPtr timer_;
  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
  rclcpp::Publisher<tutorial_interfaces::msg::Num>::SharedPtr publisher2_;
  rclcpp::Publisher<tutorial_interfaces::msg::Sphere>::SharedPtr publisher3_;
  size_t count_;
  void pub_msg() {
    auto message = std_msgs::msg::String();
    auto message2 = tutorial_interfaces::msg::Num();
    auto message3 = tutorial_interfaces::msg::Sphere();
    message3.radius = 223;
    message3.center.x = 33.4 + this->count_;
    message2.num = this->count_;
    message.data = "Hello, world! " + std::to_string(count_++);
    RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", message.data.c_str());
    RCLCPP_INFO_STREAM(this->get_logger(),
                       "Publishing: '" << message2.num << "'");
    RCLCPP_INFO_STREAM(this->get_logger(),
                       "Publishing: '" << message3.center.x << "'");
    publisher_->publish(message);
    publisher2_->publish(message2);
    publisher3_->publish(message3);
  }
};

int main(int argc, char* argv[]) {
  rclcpp::init(argc, argv);
  std::shared_ptr<MinimalPublisher> pp(new MinimalPublisher());
  //auto pp = std::make_shared<MinimalPublisher>();

  rclcpp::WallRate loop_rate(10);
  while (rclcpp::ok()) {
    rclcpp::spin_some(pp);
    pp->f();
    loop_rate.sleep();
  }
  rclcpp::shutdown();
  return 0;
}

类MinimalPublisher 继承了rclcpp::Node基类，作为节点类，在它的构造函数中，构建了3个publisher。

主函数中，首先新建了一个MinimalPublisher 的实例，并用一个智能指针pp指向它，这里也有好几种写法，既可以直接new，也可以用make_shared方法。后面rclcpp的spin函数都需要节点指针作为参数。C++11中引入了智能指针（smart pointer）的概念，其中shared_ptr是其中一个最为常用的智能指针。相对于裸指针，shared_ptr是一个功能强大的智能指针，提供了更好的内存管理，减少了手动管理内存的烦恼，并且可以用于STL容器中，非常方便。

rclcpp中实现了两个rate类，分别是WallRate和Rate，二者的区别是，rclcpp::Rate类使用系统时钟来控制程序的运行速率，rclcpp::WallRate类则使用墙上时钟（Wall Clock）来控制程序的运行速率，墙上时钟可以理解为系统的真实时间，它不会受到时间调整和时钟漂移等因素的影响。该类中的主要成员函数也是sleep()，用法与rclcpp::Rate类类似。墙上时钟使用的是真实时间，误差相对较小，稳定性较好。而系统时钟是基于计算机硬件的时钟，分辨率可以很高，误差相对较小，但是受到外部干扰的影响比较大，稳定性相对较差。因此，从精确度的角度来看，墙上时钟和系统时钟各有优缺点，应根据实际需求选择适当的时钟。如果需要高精度和高稳定性，则可以使用墙上时钟。如果需要高分辨率和较高精度，则可以使用系统时钟。以上是GPT的回答，实际上我使用没发现大的区别：P

另外还要指出的是，ROS2里面c++中没有spinonce，而是用spin_some替代了，关于二者区别，这里再次copy GPT的回答（真是方便偷懒啊）：在ROS 1中，spinOnce()是一个函数，其作用是运行一个循环来处理ROS消息，直到没有新的消息到达。而在ROS 2中，没有spinOnce()函数，取而代之的是spin_some()函数。
spin_some()函数与spinOnce()函数的功能相似，都是处理ROS消息。不同之处在于，spin_some()函数只处理已经到达的消息，而不会等待新的消息到达。这意味着，如果没有新消息到达，spin_some()函数将立即返回，而不会进入一个死循环等待新消息。
在ROS 1中，使用spinOnce()函数可以确保ROS节点能够及时处理所有的ROS消息。但是，如果不加限制地使用spinOnce()函数，可能会导致ROS节点长时间占用CPU资源，降低整个系统的性能。因此，ROS 2中引入了spin_some()函数，以提高ROS节点的性能和稳定性。
总的来说，spin_some()函数相对于spinOnce()函数更加灵活和高效，可以更好地处理ROS消息。但是，需要注意的是，在使用spin_some()函数时，需要自行控制循环的次数或者时间间隔，以确保ROS节点能够及时处理所有的ROS消息。

最后，在while循环中发送消息时，测试了一下std::function和std::bind的用法，这是不必要的代码，仅仅为测试功能，注意到在构造函数中，有这样一句代码：

f = std::bind(&MinimalPublisher::pub_msg, this);

这是利用bind将类的私有方法MinimalPublisher::pub_msg重新打包成一个新函数。因为类的成员函数都会有个一个隐藏参数，就是类的指针，所以bind的时候要加上this去掉这个指针参数。我们会注意到在ROS2官方例程中，将类的成员函数注册为回调函数时有很多这样的用法。

最后，python一样可以用rate和while循环实现程序架构，见下面的例程：

import rclpy
from rclpy.node import Node

from std_msgs.msg import String


class MinimalPublisher(Node):

    def __init__(self):
        super().__init__('minimal_publisher')
        self.publisher_ = self.create_publisher(String, 'topic', 10)
        #timer_period = 0.5  # seconds
        #self.timer = self.create_timer(timer_period, self.timer_callback)
        self.i = 0

    def timer_callback(self):
        msg = String()
        msg.data = 'Hello World: %d' % self.i
        self.publisher_.publish(msg)
        self.get_logger().info('Publishing: "%s"' % msg.data)
        self.i += 1


def main(args=None):
    rclpy.init(args=args)

    minimal_publisher = MinimalPublisher()
    rate = minimal_publisher.create_rate(10)
    while rclpy.ok():
        rclpy.spin_once(minimal_publisher)
        minimal_publisher.timer_callback()
        rate.sleep()

    # Destroy the node explicitly
    # (optional - otherwise it will be done automatically
    # when the garbage collector destroys the node object)
    minimal_publisher.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()