Encapsulate IO loop in separate function.

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

Hide file tree

Showing 2 changed files with 32 additions and 31 deletions.
diff --git a/include/t07_robot/Node.h b/include/t07_robot/Node.h
@@ -53,6 +53,10 @@ class Node : public rclcpp::Node
   std::chrono::steady_clock::time_point const _node_start;
   std::unique_ptr<CanManager> _can_mgr;
 
+  static std::chrono::milliseconds constexpr IO_LOOP_RATE{1};
+  rclcpp::TimerBase::SharedPtr _io_loop_timer;
+  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};
@@ -82,9 +86,6 @@ 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
@@ -48,9 +48,6 @@ Node::Node()
 }
 , _motor_right_target{0. * m/s}
 {
-  init_cyphal_heartbeat();
-  init_cyphal_node_info();
-
   declare_parameter("can_iface", "can0");
   declare_parameter("can_node_id", 100);
 
@@ -70,33 +67,13 @@ Node::Node()
       _node_hdl.onCanFrameReceived(frame);
     });
 
-  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);
+  init_cyphal_heartbeat();
+  init_cyphal_node_info();
 
-                                         _prev_heartbeat_timepoint = now;
-                                       }
-                                     });
+  _io_loop_timer = create_wall_timer(IO_LOOP_RATE, [this]() { this->io_loop(); });
 
+  init_motor_left();
+  init_motor_right();
 
   RCLCPP_INFO(get_logger(), "%s init complete.", get_name());
 }
@@ -110,6 +87,29 @@ Node::~Node()
  * PRIVATE MEMBER FUNCTIONS
  **************************************************************************************/
 
+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. */);