Completing Cyphal/CAN integration.

README.md

@@ -28,5 +28,7 @@ ros2 launch t07_ros t07.py
 |:------------:|:------------------------------------------------------------------------------:|
 
 ##### Parameters
-|                  Name                 |     Default      | Description                                                                                   |
-|:-------------------------------------:|:----------------:|-----------------------------------------------------------------------------------------------|
+|     Name      | Default | Description               |
+|:-------------:|:-------:|---------------------------|
+| `can_iface`   | `can0`  | Network name of CAN bus.  |
+| 'can_node_id' |   100   | Cyphal/CAN node id.       |

include/t07_ros/Node.h

@@ -42,11 +42,17 @@ class Node : public rclcpp::Node
   cyphal::Node::Heap<CYPHAL_O1HEAP_SIZE> _node_heap;
   cyphal::Node _node_hdl;
   std::mutex _node_mtx;
-
   std::chrono::steady_clock::time_point const _node_start;
-
   std::unique_ptr<CanManager> _can_mgr;
 
+  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};
+  void init_cyphal_heartbeat();
+
+  cyphal::NodeInfo _cyphal_node_info;
+  void init_cyphal_node_info();
+
   CanardMicrosecond micros();
 };
 

src/Node.cpp

@@ -35,6 +35,9 @@ Node::Node()
 , _node_mtx{}
 , _node_start{std::chrono::steady_clock::now()}
 {
+  init_cyphal_heartbeat();
+  init_cyphal_node_info();
+
   declare_parameter("can_iface", "can0");
   declare_parameter("can_node_id", 100);
 
@@ -53,16 +56,46 @@ Node::Node()
       std::lock_guard<std::mutex> lock(_node_mtx);
       _node_hdl.onCanFrameReceived(frame);
     });
+
+  RCLCPP_INFO(get_logger(), "%s init complete.", get_name());
 }
 
 Node::~Node()
 {
+  RCLCPP_INFO(get_logger(), "%s shut down successfully.", get_name());
 }
 
 /**************************************************************************************
  * PRIVATE MEMBER FUNCTIONS
  **************************************************************************************/
 
+void Node::init_cyphal_heartbeat()
+{
+  _cyphal_heartbeat_pub = _node_hdl.create_publisher<uavcan::node::Heartbeat_1_0>(1*1000*1000UL /* = 1 sec in usecs. */);
+}
+
+void Node::init_cyphal_node_info()
+{
+  _cyphal_node_info = _node_hdl.create_node_info(
+    /* uavcan.node.Version.1.0 protocol_version */
+    1, 0,
+    /* uavcan.node.Version.1.0 hardware_version */
+    1, 0,
+    /* uavcan.node.Version.1.0 software_version */
+    0, 1,
+    /* saturated uint64 software_vcs_revision_id */
+#ifdef CYPHAL_NODE_INFO_GIT_VERSION
+    CYPHAL_NODE_INFO_GIT_VERSION,
+#else
+    0,
+#endif
+    /* saturated uint8[16] unique_id */
+    std::array<uint8_t, 16>{0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08, 0x09, 0x0A, 0x0B, 0x0C, 0x0D, 0x0E, 0x0F},
+    /* saturated uint8[<=50] name */
+    "107-systems.t07"
+  );
+}
+
 CanardMicrosecond Node::micros()
 {
   auto const now = std::chrono::steady_clock::now();