Revert "Revert "feat(shift_decider): put the gear in park when vehicl…

…e reaches the goal (#1818)"" This reverts commit e68f86b. Signed-off-by: Mamoru Sobue <[email protected]>
autowarefoundation · Sep 14, 2022 · 6c1950e · 6c1950e
1 parent e9e9824
commit 6c1950e
Show file tree

Hide file tree

Showing 5 changed files with 29 additions and 2 deletions.
diff --git a/control/shift_decider/include/shift_decider/shift_decider.hpp b/control/shift_decider/include/shift_decider/shift_decider.hpp
@@ -18,6 +18,7 @@
 #include <rclcpp/rclcpp.hpp>
 
 #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 <memory>
@@ -30,16 +31,21 @@ class ShiftDecider : public rclcpp::Node
 private:
   void onTimer();
   void onControlCmd(autoware_auto_control_msgs::msg::AckermannControlCommand::SharedPtr msg);
+  void onAutowareState(autoware_auto_system_msgs::msg::AutowareState::SharedPtr msg);
   void updateCurrentShiftCmd();
   void initTimer(double period_s);
 
   rclcpp::Publisher<autoware_auto_vehicle_msgs::msg::GearCommand>::SharedPtr pub_shift_cmd_;
   rclcpp::Subscription<autoware_auto_control_msgs::msg::AckermannControlCommand>::SharedPtr
     sub_control_cmd_;
+  rclcpp::Subscription<autoware_auto_system_msgs::msg::AutowareState>::SharedPtr
+    sub_autoware_state_;
   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_;
+  bool park_on_goal_;
 };
 
 #endif  // SHIFT_DECIDER__SHIFT_DECIDER_HPP_
diff --git a/control/shift_decider/launch/shift_decider.launch.xml b/control/shift_decider/launch/shift_decider.launch.xml
@@ -1,6 +1,8 @@
 <launch>
   <node pkg="shift_decider" exec="shift_decider" name="shift_decider" output="screen">
     <remap from="input/control_cmd" to="/control/trajectory_follower/control_cmd"/>
+    <remap from="input/state" to="/autoware/state"/>
     <remap from="output/gear_cmd" to="/control/shift_decider/gear_cmd"/>
+    <param name="park_on_goal" value="true"/>
   </node>
 </launch>
diff --git a/control/shift_decider/package.xml b/control/shift_decider/package.xml
@@ -14,6 +14,7 @@
   <build_depend>autoware_cmake</build_depend>
 
   <depend>autoware_auto_control_msgs</depend>
+  <depend>autoware_auto_system_msgs</depend>
   <depend>autoware_auto_vehicle_msgs</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>

diff --git a/control/shift_decider/src/shift_decider.cpp b/control/shift_decider/src/shift_decider.cpp
@@ -30,10 +30,14 @@ ShiftDecider::ShiftDecider(const rclcpp::NodeOptions & node_options)
   rclcpp::QoS durable_qos(queue_size);
   durable_qos.transient_local();
 
+  park_on_goal_ = declare_parameter("park_on_goal", true);
+
   pub_shift_cmd_ =
     create_publisher<autoware_auto_vehicle_msgs::msg::GearCommand>("output/gear_cmd", durable_qos);
   sub_control_cmd_ = create_subscription<autoware_auto_control_msgs::msg::AckermannControlCommand>(
     "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));
 
   initTimer(0.1);
 }
@@ -44,9 +48,14 @@ void ShiftDecider::onControlCmd(
   control_cmd_ = msg;
 }
 
+void ShiftDecider::onAutowareState(autoware_auto_system_msgs::msg::AutowareState::SharedPtr msg)
+{
+  autoware_state_ = msg;
+}
+
 void ShiftDecider::onTimer()
 {
-  if (!control_cmd_) {
+  if (!autoware_state_ || !control_cmd_) {
     return;
   }
 
@@ -56,11 +65,14 @@ void ShiftDecider::onTimer()
 
 void ShiftDecider::updateCurrentShiftCmd()
 {
+  using autoware_auto_system_msgs::msg::AutowareState;
   using autoware_auto_vehicle_msgs::msg::GearCommand;
 
   shift_cmd_.stamp = now();
   static constexpr double vel_threshold = 0.01;  // to prevent chattering
-  if (control_cmd_->longitudinal.speed > vel_threshold) {
+  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;

diff --git a/launch/tier4_control_launch/launch/control.launch.py b/launch/tier4_control_launch/launch/control.launch.py
@@ -137,8 +137,14 @@ def launch_setup(context, *args, **kwargs):
         name="shift_decider",
         remappings=[
             ("input/control_cmd", "/control/trajectory_follower/control_cmd"),
+            ("input/state", "/autoware/state"),
             ("output/gear_cmd", "/control/shift_decider/gear_cmd"),
         ],
+        parameters=[
+            {
+                "park_on_goal": True,
+            }
+        ],
         extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
     )