feat(vehicle_cmd_gate): send current gear if autoware is not engaged (a…

…utowarefoundation#2555)

* feat(vehicle_cmd_gate): send current gear if autoware is not engaged

Signed-off-by: Berkay Karaman <brkay54@gmail.com>

* ci(pre-commit): autofix

* add topic map to launch file

Signed-off-by: Berkay Karaman <brkay54@gmail.com>

Signed-off-by: Berkay Karaman <brkay54@gmail.com>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

control/vehicle_cmd_gate/launch/vehicle_cmd_gate.launch.xml

@@ -26,6 +26,7 @@
     <remap from="input/emergency/hazard_lights_cmd" to="/system/emergency/hazard_lights_cmd"/>
     <remap from="input/emergency/gear_cmd" to="/system/emergency/gear_cmd"/>
     <remap from="input/mrm_state" to="/system/fail_safe/mrm_state"/>
+    <remap from="input/gear_status" to="/vehicle/status/gear_status"/>
 
     <remap from="output/vehicle_cmd_emergency" to="/control/command/emergency_cmd"/>
     <remap from="output/control_cmd" to="/control/command/control_cmd"/>

control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp

@@ -61,7 +61,6 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options)
     this->create_publisher<TurnIndicatorsCommand>("output/turn_indicators_cmd", durable_qos);
   hazard_light_cmd_pub_ =
     this->create_publisher<HazardLightsCommand>("output/hazard_lights_cmd", durable_qos);
-
   gate_mode_pub_ = this->create_publisher<GateMode>("output/gate_mode", durable_qos);
   engage_pub_ = this->create_publisher<EngageMsg>("output/engage", durable_qos);
   pub_external_emergency_ =
@@ -84,6 +83,8 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options)
     [this](const OperationModeState::SharedPtr msg) { current_operation_mode_ = *msg; });
   mrm_state_sub_ = this->create_subscription<MrmState>(
     "input/mrm_state", 1, std::bind(&VehicleCmdGate::onMrmState, this, _1));
+  gear_status_sub_ = this->create_subscription<GearReport>(
+    "input/gear_status", 1, std::bind(&VehicleCmdGate::onGearStatus, this, _1));
 
   // Subscriber for auto
   auto_control_cmd_sub_ = this->create_subscription<AckermannControlCommand>(
@@ -257,6 +258,8 @@ void VehicleCmdGate::onAutoHazardLightsCmd(HazardLightsCommand::ConstSharedPtr m
 
 void VehicleCmdGate::onAutoShiftCmd(GearCommand::ConstSharedPtr msg) { auto_commands_.gear = *msg; }
 
+void VehicleCmdGate::onGearStatus(GearReport::ConstSharedPtr msg) { current_gear_ptr_ = msg; }
+
 // for remote
 void VehicleCmdGate::onRemoteCtrlCmd(AckermannControlCommand::ConstSharedPtr msg)
 {
@@ -346,6 +349,15 @@ void VehicleCmdGate::onTimer()
     return;
   }
 
+  if (is_gate_mode_changed_) {
+    // If gate mode is external, is_engaged_ is always true
+    // While changing gate mode external to auto, the first is_engaged_ is always true for the first
+    // loop in this scope. So we need to wait for the second loop
+    // after gate mode is changed.
+    is_gate_mode_changed_ = false;
+    return;
+  }
+
   // Select commands
   TurnIndicatorsCommand turn_indicator;
   HazardLightsCommand hazard_light;
@@ -362,6 +374,11 @@ void VehicleCmdGate::onTimer()
 
       // Don't send turn signal when autoware is not engaged
       if (!is_engaged_) {
+        if (!current_gear_ptr_) {
+          gear.command = GearCommand::NONE;
+        } else {
+          gear.command = current_gear_ptr_.get()->report;
+        }
         turn_indicator.command = TurnIndicatorsCommand::NO_COMMAND;
         hazard_light.command = HazardLightsCommand::NO_COMMAND;
       }
@@ -564,7 +581,7 @@ void VehicleCmdGate::onGateMode(GateMode::ConstSharedPtr msg)
 {
   const auto prev_gate_mode = current_gate_mode_;
   current_gate_mode_ = *msg;
-
+  is_gate_mode_changed_ = true;
   if (current_gate_mode_.data != prev_gate_mode.data) {
     RCLCPP_INFO(
       get_logger(), "GateMode changed: %s -> %s", getGateModeName(prev_gate_mode.data),

control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp

@@ -27,6 +27,7 @@
 #include <autoware_auto_control_msgs/msg/ackermann_control_command.hpp>
 #include <autoware_auto_vehicle_msgs/msg/engage.hpp>
 #include <autoware_auto_vehicle_msgs/msg/gear_command.hpp>
+#include <autoware_auto_vehicle_msgs/msg/gear_report.hpp>
 #include <autoware_auto_vehicle_msgs/msg/hazard_lights_command.hpp>
 #include <autoware_auto_vehicle_msgs/msg/steering_report.hpp>
 #include <autoware_auto_vehicle_msgs/msg/turn_indicators_command.hpp>
@@ -50,6 +51,7 @@ using autoware_adapi_v1_msgs::msg::MrmState;
 using autoware_adapi_v1_msgs::msg::OperationModeState;
 using autoware_auto_control_msgs::msg::AckermannControlCommand;
 using autoware_auto_vehicle_msgs::msg::GearCommand;
+using autoware_auto_vehicle_msgs::msg::GearReport;
 using autoware_auto_vehicle_msgs::msg::HazardLightsCommand;
 using autoware_auto_vehicle_msgs::msg::SteeringReport;
 using autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand;
@@ -100,18 +102,22 @@ class VehicleCmdGate : public rclcpp::Node
   rclcpp::Subscription<SteeringReport>::SharedPtr steer_sub_;
   rclcpp::Subscription<OperationModeState>::SharedPtr operation_mode_sub_;
   rclcpp::Subscription<MrmState>::SharedPtr mrm_state_sub_;
+  rclcpp::Subscription<GearReport>::SharedPtr gear_status_sub_;
 
   void onGateMode(GateMode::ConstSharedPtr msg);
   void onExternalEmergencyStopHeartbeat(Heartbeat::ConstSharedPtr msg);
   void onSteering(SteeringReport::ConstSharedPtr msg);
   void onMrmState(MrmState::ConstSharedPtr msg);
+  void onGearStatus(GearReport::ConstSharedPtr msg);
 
   bool is_engaged_;
   bool is_system_emergency_ = false;
   bool is_external_emergency_stop_ = false;
+  bool is_gate_mode_changed_ = false;
   double current_steer_ = 0;
   GateMode current_gate_mode_;
   MrmState current_mrm_state_;
+  GearReport::ConstSharedPtr current_gear_ptr_;
 
   // Heartbeat
   std::shared_ptr<rclcpp::Time> emergency_state_heartbeat_received_time_;

launch/tier4_control_launch/launch/control.launch.py

@@ -147,6 +147,7 @@ def launch_setup(context, *args, **kwargs):
             ("input/emergency/hazard_lights_cmd", "/system/emergency/hazard_lights_cmd"),
             ("input/emergency/gear_cmd", "/system/emergency/gear_cmd"),
             ("input/mrm_state", "/system/fail_safe/mrm_state"),
+            ("input/gear_status", "/vehicle/status/gear_status"),
             ("output/vehicle_cmd_emergency", "/control/command/emergency_cmd"),
             ("output/control_cmd", "/control/command/control_cmd"),
             ("output/gear_cmd", "/control/command/gear_cmd"),