diff --git a/include/t07_robot/Node.h b/include/t07_robot/Node.h index d5bc38d..4f82216 100644 --- a/include/t07_robot/Node.h +++ b/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(); }; /************************************************************************************** diff --git a/launch/robot.py b/launch/robot.py index 8692bbd..20d93c1 100644 --- a/launch/robot.py +++ b/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}, diff --git a/src/Node.cpp b/src/Node.cpp index 96cd209..6c0a4af 100644 --- a/src/Node.cpp +++ b/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 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(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(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 **************************************************************************************/