[update to v0.8.0] latlon muxer (autowarefoundation#248)

* Remove src/ directory Signed-off-by: Kenji Miyake <[email protected]> * Update cmake_minimum_required to 3.0.2 Signed-off-by: Kenji Miyake <[email protected]> * latlon_muxer: add timeout check (autowarefoundation#893) Signed-off-by: Takamasa Horibe <[email protected]> * Replace ROS_ERROR with ROS_ERROR_THROTTLE (autowarefoundation#1041) Signed-off-by: Hiroaki ISHIKAWA <[email protected]> Co-authored-by: Hiroaki ISHIKAWA <[email protected]> * update v0.8.0 Signed-off-by: Takamasa Horibe <[email protected]> Co-authored-by: Kenji Miyake <[email protected]> Co-authored-by: Kenji Miyake <[email protected]> Co-authored-by: Hiroaki ISHIKAWA <[email protected]>
kosuke55 · Jan 30, 2021 · 0466015 · 0466015
1 parent c1367e6
commit 0466015
Show file tree

Hide file tree

Showing 2 changed files with 21 additions and 0 deletions.
diff --git 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<autoware_control_msgs::msg::ControlCommandStamped>::SharedPtr control_cmd_pub_;
   rclcpp::Subscription<autoware_control_msgs::msg::ControlCommandStamped>::SharedPtr
@@ -39,6 +40,7 @@ class LatLonMuxer : public rclcpp::Node
 
   std::shared_ptr<autoware_control_msgs::msg::ControlCommandStamped> lat_cmd_;
   std::shared_ptr<autoware_control_msgs::msg::ControlCommandStamped> 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
@@ -35,13 +35,32 @@ LatLonMuxer::LatLonMuxer(const rclcpp::NodeOptions & node_options)
     create_subscription<autoware_control_msgs::msg::ControlCommandStamped>(
     "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()
 {
   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();