diff --git a/control/latlon_muxer/include/latlon_muxer/node.hpp b/control/latlon_muxer/include/latlon_muxer/node.hpp index e9aac6d25889d..e7ed815712717 100644 --- a/control/latlon_muxer/include/latlon_muxer/node.hpp +++ b/control/latlon_muxer/include/latlon_muxer/node.hpp @@ -30,6 +30,7 @@ class LatLonMuxer : public rclcpp::Node void latCtrlCmdCallback(const autoware_control_msgs::msg::ControlCommandStamped::SharedPtr msg); void lonCtrlCmdCallback(const autoware_control_msgs::msg::ControlCommandStamped::SharedPtr msg); void publishCmd(); + bool checkTimeout(); rclcpp::Publisher::SharedPtr control_cmd_pub_; rclcpp::Subscription::SharedPtr @@ -39,6 +40,7 @@ class LatLonMuxer : public rclcpp::Node std::shared_ptr lat_cmd_; std::shared_ptr lon_cmd_; + double timeout_thr_sec_; }; #endif // LATLON_MUXER__NODE_HPP_ diff --git a/control/latlon_muxer/src/node.cpp b/control/latlon_muxer/src/node.cpp index 978af0780b4cb..6f6baac821b5c 100644 --- a/control/latlon_muxer/src/node.cpp +++ b/control/latlon_muxer/src/node.cpp @@ -35,6 +35,21 @@ LatLonMuxer::LatLonMuxer(const rclcpp::NodeOptions & node_options) create_subscription( "input/longitudinal/control_cmd", rclcpp::QoS{1}, std::bind(&LatLonMuxer::lonCtrlCmdCallback, this, std::placeholders::_1)); + timeout_thr_sec_ = declare_parameter("timeout_thr_sec", 0.5); +} + +bool LatLonMuxer::checkTimeout() +{ + const auto now = this->now(); + if ((now - lat_cmd_->header.stamp).seconds() > timeout_thr_sec_) { + RCLCPP_ERROR_THROTTLE(get_logger(), *get_clock(), 1000/*ms*/, "lat_cmd_ timeout failed."); + return false; + } + if ((now - lon_cmd_->header.stamp).seconds() > timeout_thr_sec_) { + RCLCPP_ERROR_THROTTLE(get_logger(), *get_clock(), 1000/*ms*/, "lon_cmd_ timeout failed."); + return false; + } + return true; } void LatLonMuxer::publishCmd() @@ -42,6 +57,10 @@ void LatLonMuxer::publishCmd() if (!lat_cmd_ || !lon_cmd_) { return; } + if (!checkTimeout()) { + RCLCPP_ERROR_THROTTLE(get_logger(), *get_clock(), 1000/*ms*/, "timeout failed. stop publish command."); + return; + } autoware_control_msgs::msg::ControlCommandStamped out; out.header.stamp = rclcpp::Node::now();