diff --git a/include/t07_robot/Node.h b/include/t07_robot/Node.h index f013c2e..cbb558a 100644 --- a/include/t07_robot/Node.h +++ b/include/t07_robot/Node.h @@ -82,6 +82,9 @@ class Node : public rclcpp::Node rclcpp::TimerBase::SharedPtr _motor_right_ctrl_loop_timer; void init_motor_right(); void motor_right_ctrl_loop(); + + static std::chrono::milliseconds constexpr IO_LOOP_RATE{1}; + rclcpp::TimerBase::SharedPtr _io_loop_timer; }; /************************************************************************************** diff --git a/src/Node.cpp b/src/Node.cpp index 78984fd..6acfe74 100644 --- a/src/Node.cpp +++ b/src/Node.cpp @@ -34,6 +34,7 @@ Node::Node() cyphal::Node::DEFAULT_MTU_SIZE} , _node_mtx{} , _node_start{std::chrono::steady_clock::now()} +, _prev_heartbeat_timepoint{std::chrono::steady_clock::now()} , _motor_left_qos_profile { rclcpp::KeepLast(1), @@ -72,6 +73,31 @@ Node::Node() init_motor_left(); init_motor_right(); + _io_loop_timer = create_wall_timer(IO_LOOP_RATE, + [this]() + { + std::lock_guard lock(_node_mtx); + + _node_hdl.spinSome(); + + auto const now = std::chrono::steady_clock::now(); + + if ((now - _prev_heartbeat_timepoint) > CYPHAL_HEARTBEAT_PERIOD) + { + uavcan::node::Heartbeat_1_0 msg; + + msg.uptime = std::chrono::duration_cast(now - _node_start).count(); + msg.health.value = uavcan::node::Health_1_0::NOMINAL; + msg.mode.value = uavcan::node::Mode_1_0::OPERATIONAL; + msg.vendor_specific_status_code = 0; + + _cyphal_heartbeat_pub->publish(msg); + + _prev_heartbeat_timepoint = now; + } + }); + + RCLCPP_INFO(get_logger(), "%s init complete.", get_name()); } @@ -179,7 +205,6 @@ void Node::motor_left_ctrl_loop() { std::lock_guard lock(_node_mtx); _motor_left_pwm_pub->publish(pwm_left_msg); - _node_hdl.spinSome(); } } @@ -244,7 +269,6 @@ void Node::motor_right_ctrl_loop() { std::lock_guard lock(_node_mtx); _motor_right_pwm_pub->publish(pwm_right_msg); - _node_hdl.spinSome(); } }