feat(shift_decider): send current gear if the autoware state is not d…

…riving (autowarefoundation#3684) Signed-off-by: Berkay Karaman <[email protected]>
Interplai · May 17, 2023 · 8fcac0e · 8fcac0e
1 parent 8f5aa34
commit 8fcac0e
Show file tree

Hide file tree

Showing 3 changed files with 29 additions and 7 deletions.
diff --git a/control/shift_decider/include/shift_decider/shift_decider.hpp b/control/shift_decider/include/shift_decider/shift_decider.hpp
@@ -20,6 +20,7 @@
 #include <autoware_auto_control_msgs/msg/ackermann_control_command.hpp>
 #include <autoware_auto_system_msgs/msg/autoware_state.hpp>
 #include <autoware_auto_vehicle_msgs/msg/gear_command.hpp>
+#include <autoware_auto_vehicle_msgs/msg/gear_report.hpp>
 
 #include <memory>
 
@@ -32,6 +33,7 @@ class ShiftDecider : public rclcpp::Node
   void onTimer();
   void onControlCmd(autoware_auto_control_msgs::msg::AckermannControlCommand::SharedPtr msg);
   void onAutowareState(autoware_auto_system_msgs::msg::AutowareState::SharedPtr msg);
+  void onCurrentGear(autoware_auto_vehicle_msgs::msg::GearReport::SharedPtr msg);
   void updateCurrentShiftCmd();
   void initTimer(double period_s);
 
@@ -40,11 +42,15 @@ class ShiftDecider : public rclcpp::Node
     sub_control_cmd_;
   rclcpp::Subscription<autoware_auto_system_msgs::msg::AutowareState>::SharedPtr
     sub_autoware_state_;
+  rclcpp::Subscription<autoware_auto_vehicle_msgs::msg::GearReport>::SharedPtr sub_current_gear_;
+
   rclcpp::TimerBase::SharedPtr timer_;
 
   autoware_auto_control_msgs::msg::AckermannControlCommand::SharedPtr control_cmd_;
   autoware_auto_system_msgs::msg::AutowareState::SharedPtr autoware_state_;
   autoware_auto_vehicle_msgs::msg::GearCommand shift_cmd_;
+  autoware_auto_vehicle_msgs::msg::GearReport::SharedPtr current_gear_ptr_;
+
   bool park_on_goal_;
 };
 

diff --git a/control/shift_decider/src/shift_decider.cpp b/control/shift_decider/src/shift_decider.cpp
@@ -38,6 +38,8 @@ ShiftDecider::ShiftDecider(const rclcpp::NodeOptions & node_options)
     "input/control_cmd", queue_size, std::bind(&ShiftDecider::onControlCmd, this, _1));
   sub_autoware_state_ = create_subscription<autoware_auto_system_msgs::msg::AutowareState>(
     "input/state", queue_size, std::bind(&ShiftDecider::onAutowareState, this, _1));
+  sub_current_gear_ = create_subscription<autoware_auto_vehicle_msgs::msg::GearReport>(
+    "input/current_gear", queue_size, std::bind(&ShiftDecider::onCurrentGear, this, _1));
 
   initTimer(0.1);
 }
@@ -53,9 +55,14 @@ void ShiftDecider::onAutowareState(autoware_auto_system_msgs::msg::AutowareState
   autoware_state_ = msg;
 }
 
+void ShiftDecider::onCurrentGear(autoware_auto_vehicle_msgs::msg::GearReport::SharedPtr msg)
+{
+  current_gear_ptr_ = msg;
+}
+
 void ShiftDecider::onTimer()
 {
-  if (!autoware_state_ || !control_cmd_) {
+  if (!autoware_state_ || !control_cmd_ || !current_gear_ptr_) {
     return;
   }
 
@@ -70,12 +77,20 @@ void ShiftDecider::updateCurrentShiftCmd()
 
   shift_cmd_.stamp = now();
   static constexpr double vel_threshold = 0.01;  // to prevent chattering
-  if (autoware_state_->state == AutowareState::ARRIVED_GOAL && park_on_goal_) {
-    shift_cmd_.command = GearCommand::PARK;
-  } else if (control_cmd_->longitudinal.speed > vel_threshold) {
-    shift_cmd_.command = GearCommand::DRIVE;
-  } else if (control_cmd_->longitudinal.speed < -vel_threshold) {
-    shift_cmd_.command = GearCommand::REVERSE;
+  if (autoware_state_->state == AutowareState::DRIVING) {
+    if (control_cmd_->longitudinal.speed > vel_threshold) {
+      shift_cmd_.command = GearCommand::DRIVE;
+    } else if (control_cmd_->longitudinal.speed < -vel_threshold) {
+      shift_cmd_.command = GearCommand::REVERSE;
+    } else {
+      shift_cmd_.command = current_gear_ptr_->report;
+    }
+  } else {
+    if (autoware_state_->state == AutowareState::ARRIVED_GOAL && park_on_goal_) {
+      shift_cmd_.command = GearCommand::PARK;
+    } else {
+      shift_cmd_.command = current_gear_ptr_->report;
+    }
   }
 }
 

diff --git a/launch/tier4_control_launch/launch/control.launch.py b/launch/tier4_control_launch/launch/control.launch.py
@@ -121,6 +121,7 @@ def launch_setup(context, *args, **kwargs):
         remappings=[
             ("input/control_cmd", "/control/trajectory_follower/control_cmd"),
             ("input/state", "/autoware/state"),
+            ("input/current_gear", "/vehicle/status/gear_status"),
             ("output/gear_cmd", "/control/shift_decider/gear_cmd"),
         ],
         parameters=[