Merge pull request #8 from 107-systems/t07-all-config

Provide launch files for different robot configurations (2WD, 4WD, Tracked).

README.md

@@ -20,7 +20,18 @@ colcon build --packages-select t07_robot
 ```bash
 cd $COLCON_WS
 . install/setup.bash
-ros2 launch t07_robot robot.py
+```
+* [T07W(heels)](https://github.com/107-systems/T07) (2WD tricycle configuration)
+```bash
+ros2 launch t07_robot t07.py
+```
+* [T07W(heels)](https://github.com/107-systems/T07) (4WD configuration)
+```bash
+ros2 launch t07_robot t07_4wd.py
+```
+* [T07T(racks)](https://github.com/107-systems/T07_threaded) (Tracked configuration)
+```bash
+ros2 launch t07_robot t07_tracked.py
 ```
 
 #### Interface Documentation

launch/robot.py → launch/t07.py

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

launch/t07_4wd.py

@@ -0,0 +1,33 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+def generate_launch_description():
+  return LaunchDescription([
+    Node(
+      package='t07_robot',
+      executable='t07_robot_node',
+      name='t07_robot',
+      namespace='t07',
+      output='screen',
+      emulate_tty=True,
+      parameters=[
+        {'can_iface' : 'can0'},
+        {'can_node_id' : 100},
+        {'crc07_can_node_id' : [10, 20]},
+        {'motor_left_topic': '/motor/left/target'},
+        {'motor_left_topic_deadline_ms': 200},
+        {'motor_left_topic_liveliness_lease_duration': 1000},
+        {'motor_left_rpm_port_id': 1000},
+        {'motor_right_topic': '/motor/right/target'},
+        {'motor_right_topic_deadline_ms': 200},
+        {'motor_right_topic_liveliness_lease_duration': 1000},
+        {'motor_right_rpm_port_id': 1001},
+        {'wheel_left_diameter_mm': 154.0},
+        {'wheel_right_diameter_mm': 154.0},
+        {'estop_topic': '/estop/actual'},
+        {'estop_topic_deadline_ms': 100},
+        {'estop_topic_liveliness_lease_duration': 1000},
+        {'estop_port_id': 100},
+      ]
+    )
+  ])

launch/t07_tracked.py

@@ -0,0 +1,33 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+def generate_launch_description():
+  return LaunchDescription([
+    Node(
+      package='t07_robot',
+      executable='t07_robot_node',
+      name='t07_robot',
+      namespace='t07',
+      output='screen',
+      emulate_tty=True,
+      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},
+        {'motor_left_rpm_port_id': 1000},
+        {'motor_right_topic': '/motor/right/target'},
+        {'motor_right_topic_deadline_ms': 200},
+        {'motor_right_topic_liveliness_lease_duration': 1000},
+        {'motor_right_rpm_port_id': 1001},
+        {'wheel_left_diameter_mm': 70.0},
+        {'wheel_right_diameter_mm': 70.0},
+        {'estop_topic': '/estop/actual'},
+        {'estop_topic_deadline_ms': 100},
+        {'estop_topic_liveliness_lease_duration': 1000},
+        {'estop_port_id': 100},
+      ]
+    )
+  ])

src/Node.cpp

@@ -333,50 +333,67 @@ 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();
+typedef std::tuple<CanardNodeID, std::chrono::steady_clock::time_point, bool, bool> CyphalHeartbeatMonitorData;
+static std::list<CyphalHeartbeatMonitorData> crc07_heartbeat_monitor_list;
 
 void Node::init_cyphal_heartbeat_check()
 {
-  declare_parameter("crc07_can_node_id", 10);
-  crc07_node_id = get_parameter("crc07_can_node_id").as_int();
+  declare_parameter("crc07_can_node_id", std::vector<long int>{});
+  auto const crc07_node_id_array = get_parameter("crc07_can_node_id").as_integer_array();
+
+  for (auto const node_id : crc07_node_id_array)
+  {
+    auto const monitor_data = std::make_tuple(node_id,
+                                              std::chrono::steady_clock::now(),
+                                              false,
+                                              false);
+    crc07_heartbeat_monitor_list.push_back(monitor_data);
+  }
 
   _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)
+      auto const iter = std::find_if(crc07_heartbeat_monitor_list.begin(),
+                                     crc07_heartbeat_monitor_list.end(),
+                                     [metadata](CyphalHeartbeatMonitorData const & data)
+                                     {
+                                        auto const [node_id, prev_cyphal_heartbeat, is_heartbeat_timeout, prev_is_heartbeat_timeout] = data;
+                                        return (node_id == metadata.remote_node_id);
+                                     });
+
+      if (iter != crc07_heartbeat_monitor_list.end())
       {
-        crc07_is_heartbeat_timeout = false;
-        crc07_prev_cyphal_heartbeat = std::chrono::steady_clock::now();
+        auto & [node_id, prev_cyphal_heartbeat, is_heartbeat_timeout, prev_is_heartbeat_timeout] = *iter;
+        is_heartbeat_timeout = false;
+        prev_cyphal_heartbeat = std::chrono::steady_clock::now();
       }
     });
 
   _heartbeat_cyphal_loop_timer = create_wall_timer(
-    std::chrono::milliseconds(100),
+    std::chrono::milliseconds(1000),
     [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))
+      for (auto & [node_id, prev_cyphal_heartbeat, is_heartbeat_timeout, prev_is_heartbeat_timeout] : crc07_heartbeat_monitor_list)
       {
-        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());
+        auto const now = std::chrono::steady_clock::now();
+        auto const duration_since_last_heartbeat = (now - prev_cyphal_heartbeat);
+
+        if (std::chrono::duration_cast<std::chrono::seconds>(duration_since_last_heartbeat) > std::chrono::seconds(2))
+        {
+          is_heartbeat_timeout = true;
+          RCLCPP_ERROR(get_logger(), "cyphal robot controller 07 (%d) offline since %ld seconds.", static_cast<int>(node_id), std::chrono::duration_cast<std::chrono::seconds>(duration_since_last_heartbeat).count());
+        }
+
+        if (!is_heartbeat_timeout && prev_is_heartbeat_timeout)
+          RCLCPP_INFO(get_logger(), "cyphal robot controller 07 (%d) back online.", static_cast<int>(node_id));
+        prev_is_heartbeat_timeout = is_heartbeat_timeout;
       }
-
-      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;
-
     });
 }