Merge pull request autowarefoundation#1481 from tier4/feat/vehicle_cm…

…d_gate_check_cmd_continuity

feat(autoware_vehicle_cmd_gate): check the timestamp of input topics to avoid using old topics

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.

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,23 @@ 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 {
+    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;
+  }
+}
+
 void VehicleCmdGate::onTimer()
 {
   // Subscriber for auto
@@ -447,6 +468,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 +480,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)

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,11 @@ 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;