feat(autoware_vehicle_cmd_gate): check the timestamp of input topics …

…to avoid using old topics (#8084) * 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 --------- 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]>
autowarefoundation · Aug 9, 2024 · e5cd62f · e5cd62f
1 parent 21c01aa
commit e5cd62f
Show file tree

Hide file tree

Showing 3 changed files with 72 additions and 4 deletions.
diff --git a/control/autoware_vehicle_cmd_gate/README.md b/control/autoware_vehicle_cmd_gate/README.md
@@ -84,6 +84,13 @@ This functionality for starting/stopping was embedded in the source code but was
 
 ## Assumptions / Known limits
 
+### External Emergency Heartbeat
+
 The parameter `check_external_emergency_heartbeat` (true by default) enables an emergency stop request from external modules.
 This feature requires a `~/input/external_emergency_stop_heartbeat` topic for health monitoring of the external module, and the vehicle_cmd_gate module will not start without the topic.
 The `check_external_emergency_heartbeat` parameter must be false when the "external emergency stop" function is not used.
+
+### Commands on Mode changes
+
+Output commands' topics: `turn_indicators_cmd`, `hazard_light` and `gear_cmd` are selected based on `gate_mode`.
+However, to ensure the continuity of commands, these commands will not change until the topics of new input commands arrive, even if a mode change occurs.
diff --git a/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp
@@ -53,6 +53,10 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options)
   using std::placeholders::_2;
   using std::placeholders::_3;
 
+  prev_turn_indicator_ = nullptr;
+  prev_hazard_light_ = nullptr;
+  prev_gear_ = nullptr;
+
   rclcpp::QoS durable_qos{1};
   durable_qos.transient_local();
 
@@ -352,6 +356,21 @@ void VehicleCmdGate::onEmergencyCtrlCmd(Control::ConstSharedPtr msg)
   }
 }
 
+// check the continuity of topics
+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) {
+    return current_topic;
+  } else {
+    RCLCPP_INFO(
+      get_logger(),
+      "The operation mode is changed, but the %s is not received yet:", topic_name.c_str());
+    return *prev_topic;
+  }
+}
+
 void VehicleCmdGate::onTimer()
 {
   // Subscriber for auto
@@ -447,6 +466,8 @@ void VehicleCmdGate::onTimer()
       if (!is_engaged_) {
         turn_indicator.command = TurnIndicatorsCommand::NO_COMMAND;
         hazard_light.command = HazardLightsCommand::NO_COMMAND;
+        turn_indicator.stamp = this->now();
+        hazard_light.stamp = this->now();
       }
     } else if (current_gate_mode_.data == GateMode::EXTERNAL) {
       turn_indicator = remote_commands_.turn_indicator;
@@ -457,10 +478,40 @@ void VehicleCmdGate::onTimer()
     }
   }
 
-  // Publish topics
-  turn_indicator_cmd_pub_->publish(turn_indicator);
-  hazard_light_cmd_pub_->publish(hazard_light);
-  gear_cmd_pub_->publish(gear);
+  // Publish Turn Indicators, Hazard Lights and Gear Command
+  if (prev_turn_indicator_ != nullptr) {
+    *prev_turn_indicator_ =
+      getContinuousTopic(prev_turn_indicator_, turn_indicator, "TurnIndicatorsCommand");
+    turn_indicator_cmd_pub_->publish(*prev_turn_indicator_);
+  } else {
+    if (msg_auto_command_turn_indicator || msg_remote_command_turn_indicator) {
+      prev_turn_indicator_ = std::make_shared<TurnIndicatorsCommand>(turn_indicator);
+    }
+    turn_indicator_cmd_pub_->publish(turn_indicator);
+  }
+
+  if (prev_hazard_light_ != nullptr) {
+    *prev_hazard_light_ =
+      getContinuousTopic(prev_hazard_light_, hazard_light, "HazardLightsCommand");
+    hazard_light_cmd_pub_->publish(*prev_hazard_light_);
+  } else {
+    if (
+      msg_auto_command_hazard_light || msg_remote_command_hazard_light ||
+      msg_emergency_command_hazard_light) {
+      prev_hazard_light_ = std::make_shared<HazardLightsCommand>(hazard_light);
+    }
+    hazard_light_cmd_pub_->publish(hazard_light);
+  }
+
+  if (prev_gear_ != nullptr) {
+    *prev_gear_ = getContinuousTopic(prev_gear_, gear, "GearCommand");
+    gear_cmd_pub_->publish(*prev_gear_);
+  } else {
+    if (msg_auto_command_gear || msg_remote_command_gear || msg_emergency_command_gear) {
+      prev_gear_ = std::make_shared<GearCommand>(gear);
+    }
+    gear_cmd_pub_->publish(gear);
+  }
 }
 
 void VehicleCmdGate::publishControlCommands(const Commands & commands)

diff --git a/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.hpp b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.hpp
@@ -50,6 +50,7 @@
 #include <visualization_msgs/msg/marker_array.hpp>
 
 #include <memory>
+#include <string>
 #include <vector>
 
 namespace autoware::vehicle_cmd_gate
@@ -183,6 +184,11 @@ class VehicleCmdGate : public rclcpp::Node
     this, "input/emergency/gear_cmd"};
   void onEmergencyCtrlCmd(Control::ConstSharedPtr msg);
 
+  // Previous Turn Indicators, Hazard Lights and Gear
+  TurnIndicatorsCommand::SharedPtr prev_turn_indicator_;
+  HazardLightsCommand::SharedPtr prev_hazard_light_;
+  GearCommand::SharedPtr prev_gear_;
+
   // Parameter
   bool use_emergency_handling_;
   bool check_external_emergency_heartbeat_;
@@ -232,6 +238,10 @@ class VehicleCmdGate : public rclcpp::Node
 
   void checkExternalEmergencyStop(diagnostic_updater::DiagnosticStatusWrapper & stat);
 
+  template <typename T>
+  T getContinuousTopic(
+    const std::shared_ptr<T> & prev_topic, const T & current_topic, const std::string & topic_name);
+
   // Algorithm
   Control prev_control_cmd_;
   Control createStopControlCmd() const;