Run node_hdl in dedicated IO loop.

107-systems · Nov 15, 2023 · bc2e042 · bc2e042
1 parent e5d5ff6
commit bc2e042
Show file tree

Hide file tree

Showing 2 changed files with 29 additions and 2 deletions.
diff --git a/include/t07_robot/Node.h b/include/t07_robot/Node.h
@@ -82,6 +82,9 @@ class Node : public rclcpp::Node
   rclcpp::TimerBase::SharedPtr _motor_right_ctrl_loop_timer;
   void init_motor_right();
   void motor_right_ctrl_loop();
+
+  static std::chrono::milliseconds constexpr IO_LOOP_RATE{1};
+  rclcpp::TimerBase::SharedPtr _io_loop_timer;
 };
 
 /**************************************************************************************

diff --git a/src/Node.cpp b/src/Node.cpp
@@ -34,6 +34,7 @@ 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),
@@ -72,6 +73,31 @@ Node::Node()
   init_motor_left();
   init_motor_right();
 
+  _io_loop_timer = create_wall_timer(IO_LOOP_RATE,
+                                     [this]()
+                                     {
+                                       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;
+                                       }
+                                     });
+
+
   RCLCPP_INFO(get_logger(), "%s init complete.", get_name());
 }
 
@@ -179,7 +205,6 @@ void Node::motor_left_ctrl_loop()
   {
     std::lock_guard <std::mutex> lock(_node_mtx);
     _motor_left_pwm_pub->publish(pwm_left_msg);
-    _node_hdl.spinSome();
   }
 }
 
@@ -244,7 +269,6 @@ void Node::motor_right_ctrl_loop()
   {
     std::lock_guard <std::mutex> lock(_node_mtx);
     _motor_right_pwm_pub->publish(pwm_right_msg);
-    _node_hdl.spinSome();
   }
 }