From 24a7c076950bf73b6f164c201a2075fc1418d5bf Mon Sep 17 00:00:00 2001
From: Alex Mikhalev <alexmikhalevalex@gmail.com>
Date: Wed, 25 Sep 2019 19:34:08 -0700
Subject: [PATCH] Use timer instead of rate

---
 src/uaspire_controls/main.cc | 24 +++++++++++-------------
 1 file changed, 11 insertions(+), 13 deletions(-)

diff --git a/src/uaspire_controls/main.cc b/src/uaspire_controls/main.cc
index d1cb770..943724a 100644
--- a/src/uaspire_controls/main.cc
+++ b/src/uaspire_controls/main.cc
@@ -1,6 +1,7 @@
 #include <functional>
 
 #include <rclcpp/rclcpp.hpp>
+#include <rclcpp/timer.hpp>
 #include <std_msgs/msg/string.hpp>
 #include <uaspire_msgs/msg/command.hpp>
 #include <uaspire_msgs/msg/sensors.hpp>
@@ -15,14 +16,13 @@ class ControlsNode : public ros::Node
 private:
     ros::Subscription<UaspireCommand>::SharedPtr _command_subscription;
     ros::Publisher<UaspireSensors>::SharedPtr _sensors_publisher;
-    ros::WallRate _rate;
+    ros::TimerBase::SharedPtr _timer;
 
 public:
     RCLCPP_SHARED_PTR_DEFINITIONS(ControlsNode)
 
     ControlsNode()
-        : Node("uaspire_controls"),
-        _rate(1.0)
+        : Node("uaspire_controls")
     {
         _command_subscription = create_subscription<UaspireCommand>(
             "uaspire/commands",
@@ -33,6 +33,8 @@ public:
             "uaspire/sensors",
             ros::QoS(10)
         );
+        _timer = create_wall_timer(std::chrono::milliseconds(1000),
+            std::bind(&ControlsNode::on_loop, this));
     }
 
     void on_message(const UaspireCommand::SharedPtr cmd)
@@ -40,15 +42,11 @@ public:
         RCLCPP_INFO(get_logger(), "Command received: %s", cmd->data.c_str());
     }
 
-    void loop() {
-        while (ros::ok()) {
-            UaspireSensors sensors;
-            sensors.position = ros::Clock(RCL_STEADY_TIME).now().seconds();
-            _sensors_publisher->publish(sensors);
-            RCLCPP_INFO(get_logger(), "loop");
-            ros::spin_some(shared_from_this());
-            _rate.sleep();
-        }
+    void on_loop() {
+		UaspireSensors sensors;
+		sensors.position = ros::Clock(RCL_STEADY_TIME).now().seconds();
+		_sensors_publisher->publish(sensors);
+		RCLCPP_INFO(get_logger(), "loop");
     }
 };
 
@@ -56,7 +54,7 @@ int main(int argc, char** argv) {
     ros::init(argc, argv);
     {
         auto node = ControlsNode::make_shared();
-        node->loop();
+		ros::spin(node);
     }
     ros::shutdown();
     return 0;