feat(vehicle_cmd_gate): do not send current gear if autoware is not e…

…ngaged (autowarefoundation#3683) This reverts commit be31385. Signed-off-by: Berkay Karaman <[email protected]>
Interplai · May 17, 2023 · 8f5aa34 · 8f5aa34
1 parent 14b183f
commit 8f5aa34
Show file tree

Hide file tree

Showing 4 changed files with 1 addition and 24 deletions.
diff --git a/control/vehicle_cmd_gate/launch/vehicle_cmd_gate.launch.xml b/control/vehicle_cmd_gate/launch/vehicle_cmd_gate.launch.xml
@@ -26,7 +26,6 @@
     <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"/>

diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp
@@ -88,8 +88,6 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options)
     [this](const OperationModeState::SharedPtr msg) { current_operation_mode_ = *msg; });
   mrm_state_sub_ = create_subscription<MrmState>(
     "input/mrm_state", 1, std::bind(&VehicleCmdGate::onMrmState, this, _1));
-  gear_status_sub_ = create_subscription<GearReport>(
-    "input/gear_status", 1, [this](GearReport::SharedPtr msg) { current_gear_ptr_ = msg; });
 
   // Subscriber for auto
   auto_control_cmd_sub_ = create_subscription<AckermannControlCommand>(
@@ -321,15 +319,6 @@ 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;
@@ -346,11 +335,6 @@ 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;
       }
@@ -577,7 +561,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),

diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp
@@ -27,7 +27,6 @@
 #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>
@@ -52,7 +51,6 @@ 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;
@@ -104,7 +102,6 @@ class VehicleCmdGate : public rclcpp::Node
   rclcpp::Subscription<GateMode>::SharedPtr gate_mode_sub_;
   rclcpp::Subscription<OperationModeState>::SharedPtr operation_mode_sub_;
   rclcpp::Subscription<MrmState>::SharedPtr mrm_state_sub_;
-  rclcpp::Subscription<GearReport>::SharedPtr gear_status_sub_;
   rclcpp::Subscription<Odometry>::SharedPtr kinematics_sub_;             // for filter
   rclcpp::Subscription<AccelWithCovarianceStamped>::SharedPtr acc_sub_;  // for filter
   rclcpp::Subscription<SteeringReport>::SharedPtr steer_sub_;            // for filter
@@ -116,11 +113,9 @@ class VehicleCmdGate : public rclcpp::Node
   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_;
   Odometry current_kinematics_;
   double current_acceleration_ = 0.0;
 

diff --git a/launch/tier4_control_launch/launch/control.launch.py b/launch/tier4_control_launch/launch/control.launch.py
@@ -178,7 +178,6 @@ 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"),
             ("input/kinematics", "/localization/kinematic_state"),
             ("input/acceleration", "/localization/acceleration"),
             ("output/vehicle_cmd_emergency", "/control/command/emergency_cmd"),