feat(autoware_vehicle_cmd_gate): accept same topic unless mode change…

… occurs (autowarefoundation#8479) * add prev_commands_ and check cmd's time stamp Signed-off-by: shtokuda <[email protected]> * add timestamp when is_engaged is false Signed-off-by: shtokuda <[email protected]> * style(pre-commit): autofix * add initialization for hazard_light timestamp in Commands Signed-off-by: shtokuda <[email protected]> * style(pre-commit): autofix * update README.md Signed-off-by: shtokuda <[email protected]> * style(pre-commit): autofix * fix typo Signed-off-by: shtokuda <[email protected]> * fix(autoware_vehicle_cmd_gate): rename the function that checks the continuity of topics Signed-off-by: shtokuda <[email protected]> Co-authored-by: Takamasa Horibe <[email protected]> * style(pre-commit): autofix * feat(autoware_vehicle_cmd_gate): check continuity using shared_ptr Signed-off-by: shtokuda <[email protected]> * feat(autoware_vehicle_cmd_gate): add INFO message for topics that are not receiving Signed-off-by: shtokuda <[email protected]> * fix template function to pass build-and-test-differential Signed-off-by: shtokuda <[email protected]> * fix(autoware_vehicle_cmd_gate): add #include <string> according to pre-commit.ci Signed-off-by: shtokuda <[email protected]> * fix(vehicle_cmd_gate) add underscores to member variable names for consistency Signed-off-by: shtokuda <[email protected]> * style(pre-commit): autofix * feat(vehicle_cmd_gate): accept same topic unless mode change occurs Signed-off-by: shtokuda <[email protected]> * feat(vehicle_cmd_gate): add default topic_name to getContinuousTopic function Signed-off-by: shtokuda <[email protected]> --------- Signed-off-by: shtokuda <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Takamasa Horibe <[email protected]> Co-authored-by: Shumpei Wakabayashi <[email protected]>
tier4 · Aug 21, 2024 · 60b733a · 60b733a
1 parent d73a09a
commit 60b733a
Show file tree

Hide file tree

Showing 2 changed files with 8 additions and 5 deletions.
diff --git a/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp
@@ -361,12 +361,14 @@ template <typename T>
 T VehicleCmdGate::getContinuousTopic(
   const std::shared_ptr<T> & prev_topic, const T & current_topic, const std::string & topic_name)
 {
-  if ((rclcpp::Time(current_topic.stamp) - rclcpp::Time(prev_topic->stamp)).seconds() > 0.0) {
+  if ((rclcpp::Time(current_topic.stamp) - rclcpp::Time(prev_topic->stamp)).seconds() >= 0.0) {
     return current_topic;
   } else {
-    RCLCPP_INFO(
-      get_logger(),
-      "The operation mode is changed, but the %s is not received yet:", topic_name.c_str());
+    if (topic_name != "") {
+      RCLCPP_INFO(
+        get_logger(),
+        "The operation mode is changed, but the %s is not received yet:", topic_name.c_str());
+    }
     return *prev_topic;
   }
 }

diff --git a/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.hpp b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.hpp
@@ -240,7 +240,8 @@ class VehicleCmdGate : public rclcpp::Node
 
   template <typename T>
   T getContinuousTopic(
-    const std::shared_ptr<T> & prev_topic, const T & current_topic, const std::string & topic_name);
+    const std::shared_ptr<T> & prev_topic, const T & current_topic,
+    const std::string & topic_name = "");
 
   // Algorithm
   Control prev_control_cmd_;