Merge pull request #7 from 107-systems/fix-6

Observe CRC07 heartbeat and warn user if IO controller is offline.

include/t07_robot/Node.h

@@ -94,6 +94,10 @@ class Node : public rclcpp::Node
   cyphal::Subscription _estop_cyphal_sub;
   bool _is_estop_active;
   void init_estop();
+
+  cyphal::Subscription _heartbeat_cyphal_sub;
+  rclcpp::TimerBase::SharedPtr _heartbeat_cyphal_loop_timer;
+  void init_cyphal_heartbeat_check();
 };
 
 /**************************************************************************************

launch/robot.py

@@ -13,6 +13,7 @@ def generate_launch_description():
       parameters=[
         {'can_iface' : 'can0'},
         {'can_node_id' : 100},
+        {'crc07_can_node_id' : 10},
         {'motor_left_topic': '/motor/left/target'},
         {'motor_left_topic_deadline_ms': 200},
         {'motor_left_topic_liveliness_lease_duration': 1000},

src/Node.cpp

@@ -75,6 +75,7 @@ Node::Node()
   init_motor_left();
   init_motor_right();
   init_estop();
+  init_cyphal_heartbeat_check();
 
   RCLCPP_INFO(get_logger(), "%s init complete.", get_name());
 }
@@ -332,6 +333,53 @@ void Node::init_estop()
     });
 }
 
+
+/* Stupid hack: somehow the subscription callback with metadata fails spectacularly
+ * when used in combination with a [this] capture.
+ */
+static bool crc07_is_heartbeat_timeout = false;
+static int crc07_node_id = 0;
+static std::chrono::steady_clock::time_point crc07_prev_cyphal_heartbeat = std::chrono::steady_clock::now();
+
+void Node::init_cyphal_heartbeat_check()
+{
+  declare_parameter("crc07_can_node_id", 10);
+  crc07_node_id = get_parameter("crc07_can_node_id").as_int();
+
+  _estop_cyphal_sub = _node_hdl.create_subscription<uavcan::node::Heartbeat_1_0>(
+    [](uavcan::node::Heartbeat_1_0 const & /* msg */, cyphal::TransferMetadata const & metadata)
+    {
+      /* If the received heartbeat originates from the CyphalRobotController07
+       * then we update the timestamp of the last received heartbeat message.
+       */
+      if (crc07_node_id == metadata.remote_node_id)
+      {
+        crc07_is_heartbeat_timeout = false;
+        crc07_prev_cyphal_heartbeat = std::chrono::steady_clock::now();
+      }
+    });
+
+  _heartbeat_cyphal_loop_timer = create_wall_timer(
+    std::chrono::milliseconds(100),
+    [this]()
+    {
+      auto const now = std::chrono::steady_clock::now();
+      auto const duration_since_last_heartbeat = (now - crc07_prev_cyphal_heartbeat);
+
+      if (std::chrono::duration_cast<std::chrono::seconds>(duration_since_last_heartbeat) > std::chrono::seconds(2))
+      {
+        crc07_is_heartbeat_timeout = true;
+        RCLCPP_ERROR_THROTTLE(get_logger(), *get_clock(), 1000UL, "cyphal robot controller 07 offline since %ld seconds.", std::chrono::duration_cast<std::chrono::seconds>(duration_since_last_heartbeat).count());
+      }
+
+      static bool prev_crc07_is_heartbeat_timeout = false;
+      if (!crc07_is_heartbeat_timeout && prev_crc07_is_heartbeat_timeout)
+        RCLCPP_INFO(get_logger(), "cyphal robot controller 07 back online.");
+      prev_crc07_is_heartbeat_timeout = crc07_is_heartbeat_timeout;
+
+    });
+}
+
 /**************************************************************************************
  * NAMESPACE
  **************************************************************************************/