From 56f5fa52eded832da75d058b3da5ed63fe77c775 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Sun, 17 Mar 2024 18:30:28 +0100 Subject: [PATCH 1/3] Provide launch files for different robot configurations (2WD, 4WD, Tracked). --- README.md | 13 ++++++++++++- launch/{robot.py => t07.py} | 0 launch/t07_4wd.py | 33 +++++++++++++++++++++++++++++++++ launch/t07_tracked.py | 33 +++++++++++++++++++++++++++++++++ 4 files changed, 78 insertions(+), 1 deletion(-) rename launch/{robot.py => t07.py} (100%) create mode 100644 launch/t07_4wd.py create mode 100644 launch/t07_tracked.py diff --git a/README.md b/README.md index ff4d9f5..cf7a836 100644 --- a/README.md +++ b/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 diff --git a/launch/robot.py b/launch/t07.py similarity index 100% rename from launch/robot.py rename to launch/t07.py diff --git a/launch/t07_4wd.py b/launch/t07_4wd.py new file mode 100644 index 0000000..d996d80 --- /dev/null +++ b/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}, + {'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}, + ] + ) + ]) diff --git a/launch/t07_tracked.py b/launch/t07_tracked.py new file mode 100644 index 0000000..68d84d5 --- /dev/null +++ b/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}, + ] + ) + ]) From fd3a90d41949034a85e072c4b872db53f2b5e08e Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Sun, 17 Mar 2024 18:31:50 +0100 Subject: [PATCH 2/3] Fix: track Cyphal heartbeat timeout of node. --- src/Node.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/Node.cpp b/src/Node.cpp index 6c0a4af..01f25ea 100644 --- a/src/Node.cpp +++ b/src/Node.cpp @@ -293,6 +293,12 @@ void Node::motor_right_ctrl_loop() } } +/* 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; + + void Node::init_estop() { /* Initialize the ROS publisher. */ @@ -332,12 +338,6 @@ void Node::init_estop() _estop_pub->publish(estop_msg); }); } - - -/* 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(); From 2e953393af248d407530515f608fd8e62db06726 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Sun, 17 Mar 2024 18:57:51 +0100 Subject: [PATCH 3/3] Allow passing of multiple Cyphal node ids for hearbeat monitoring. This is necessary because i.e. 4WD requires 2 CRC07 PCBs. --- launch/t07.py | 2 +- launch/t07_4wd.py | 2 +- launch/t07_tracked.py | 2 +- src/Node.cpp | 69 +++++++++++++++++++++++++++---------------- 4 files changed, 46 insertions(+), 29 deletions(-) diff --git a/launch/t07.py b/launch/t07.py index 20d93c1..c792173 100644 --- a/launch/t07.py +++ b/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}, diff --git a/launch/t07_4wd.py b/launch/t07_4wd.py index d996d80..186b4b8 100644 --- a/launch/t07_4wd.py +++ b/launch/t07_4wd.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, 20]}, {'motor_left_topic': '/motor/left/target'}, {'motor_left_topic_deadline_ms': 200}, {'motor_left_topic_liveliness_lease_duration': 1000}, diff --git a/launch/t07_tracked.py b/launch/t07_tracked.py index 68d84d5..d37270f 100644 --- a/launch/t07_tracked.py +++ b/launch/t07_tracked.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}, diff --git a/src/Node.cpp b/src/Node.cpp index 01f25ea..093caf6 100644 --- a/src/Node.cpp +++ b/src/Node.cpp @@ -293,12 +293,6 @@ void Node::motor_right_ctrl_loop() } } -/* 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; - - void Node::init_estop() { /* Initialize the ROS publisher. */ @@ -338,13 +332,26 @@ void Node::init_estop() _estop_pub->publish(estop_msg); }); } -static int crc07_node_id = 0; -static std::chrono::steady_clock::time_point crc07_prev_cyphal_heartbeat = std::chrono::steady_clock::now(); + +/* Stupid hack: somehow the subscription callback with metadata fails spectacularly + * when used in combination with a [this] capture. + */ +typedef std::tuple CyphalHeartbeatMonitorData; +static std::list 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{}); + 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 const & /* msg */, cyphal::TransferMetadata const & metadata) @@ -352,31 +359,41 @@ void Node::init_cyphal_heartbeat_check() /* 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(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(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(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(node_id), std::chrono::duration_cast(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(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; - }); }