Fix: send heartbeat via ROS loop.

107-systems · Nov 16, 2023 · 863c9d3 · 863c9d3
1 parent 2f0abf1
commit 863c9d3
Show file tree

Hide file tree

Showing 2 changed files with 19 additions and 19 deletions.
diff --git a/include/t07_robot/Node.h b/include/t07_robot/Node.h
@@ -58,8 +58,8 @@ class Node : public rclcpp::Node
   void io_loop();
 
   cyphal::Publisher<uavcan::node::Heartbeat_1_0> _cyphal_heartbeat_pub;
-  std::chrono::steady_clock::time_point _prev_heartbeat_timepoint;
   static std::chrono::milliseconds constexpr CYPHAL_HEARTBEAT_PERIOD{1000};
+  rclcpp::TimerBase::SharedPtr _cyphal_heartbeat_timer;
   void init_cyphal_heartbeat();
 
   cyphal::NodeInfo _cyphal_node_info;

diff --git a/src/Node.cpp b/src/Node.cpp
@@ -34,7 +34,6 @@ Node::Node()
             cyphal::Node::DEFAULT_MTU_SIZE}
 , _node_mtx{}
 , _node_start{std::chrono::steady_clock::now()}
-, _prev_heartbeat_timepoint{std::chrono::steady_clock::now()}
 , _motor_left_qos_profile
 {
   rclcpp::KeepLast(1),
@@ -90,29 +89,30 @@ Node::~Node()
 void Node::io_loop()
 {
   std::lock_guard <std::mutex> lock(_node_mtx);
-
   _node_hdl.spinSome();
-
-  auto const now = std::chrono::steady_clock::now();
-
-  if ((now - _prev_heartbeat_timepoint) > CYPHAL_HEARTBEAT_PERIOD)
-  {
-    uavcan::node::Heartbeat_1_0 msg;
-
-    msg.uptime = std::chrono::duration_cast<std::chrono::seconds>(now - _node_start).count();
-    msg.health.value = uavcan::node::Health_1_0::NOMINAL;
-    msg.mode.value = uavcan::node::Mode_1_0::OPERATIONAL;
-    msg.vendor_specific_status_code = 0;
-
-    _cyphal_heartbeat_pub->publish(msg);
-
-    _prev_heartbeat_timepoint = now;
-  }
 }
 
 void Node::init_cyphal_heartbeat()
 {
   _cyphal_heartbeat_pub = _node_hdl.create_publisher<uavcan::node::Heartbeat_1_0>(1*1000*1000UL /* = 1 sec in usecs. */);
+
+  _cyphal_heartbeat_timer = create_wall_timer(CYPHAL_HEARTBEAT_PERIOD,
+                                              [this]()
+                                              {
+                                                uavcan::node::Heartbeat_1_0 msg;
+
+                                                auto const now = std::chrono::steady_clock::now();
+
+                                                msg.uptime = std::chrono::duration_cast<std::chrono::seconds>(now - _node_start).count();
+                                                msg.health.value = uavcan::node::Health_1_0::NOMINAL;
+                                                msg.mode.value = uavcan::node::Mode_1_0::OPERATIONAL;
+                                                msg.vendor_specific_status_code = 0;
+
+                                                {
+                                                  std::lock_guard <std::mutex> lock(_node_mtx);
+                                                  _cyphal_heartbeat_pub->publish(msg);
+                                                }
+                                              });
 }
 
 void Node::init_cyphal_node_info()