feat(longitudinal_controller): skip integral in manual mode (#2619)

* feat(longitudinal_controller): skip integral in manual mode

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* change control_mode to operation_mode

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* fix test

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp

@@ -30,6 +30,7 @@
 #include "trajectory_follower_base/longitudinal_controller_base.hpp"
 #include "vehicle_info_util/vehicle_info_util.hpp"
 
+#include "autoware_adapi_v1_msgs/msg/operation_mode_state.hpp"
 #include "autoware_auto_control_msgs/msg/longitudinal_command.hpp"
 #include "autoware_auto_planning_msgs/msg/trajectory.hpp"
 #include "autoware_auto_vehicle_msgs/msg/vehicle_odometry.hpp"
@@ -46,7 +47,7 @@
 
 namespace autoware::motion::control::pid_longitudinal_controller
 {
-
+using autoware_adapi_v1_msgs::msg::OperationModeState;
 namespace trajectory_follower = ::autoware::motion::control::trajectory_follower;
 
 /// \class PidLongitudinalController
@@ -88,6 +89,7 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro
   nav_msgs::msg::Odometry m_current_kinematic_state;
   geometry_msgs::msg::AccelWithCovarianceStamped m_current_accel;
   autoware_auto_planning_msgs::msg::Trajectory m_trajectory;
+  OperationModeState m_current_operation_mode;
 
   // vehicle info
   double m_wheel_base;
@@ -225,6 +227,12 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro
    */
   void setCurrentAcceleration(const geometry_msgs::msg::AccelWithCovarianceStamped & msg);
 
+  /**
+   * @brief set current operation mode with received message
+   * @param [in] msg operation mode report message
+   */
+  void setCurrentOperationMode(const OperationModeState & msg);
+
   /**
    * @brief set reference trajectory with received message
    * @param [in] msg trajectory message

control/pid_longitudinal_controller/package.xml

@@ -18,6 +18,7 @@
 
   <build_depend>autoware_cmake</build_depend>
 
+  <depend>autoware_adapi_v1_msgs</depend>
   <depend>autoware_auto_control_msgs</depend>
   <depend>autoware_auto_geometry</depend>
   <depend>autoware_auto_planning_msgs</depend>

control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp

@@ -203,6 +203,11 @@ void PidLongitudinalController::setCurrentAcceleration(
   m_current_accel = msg;
 }
 
+void PidLongitudinalController::setCurrentOperationMode(const OperationModeState & msg)
+{
+  m_current_operation_mode = msg;
+}
+
 void PidLongitudinalController::setTrajectory(
   const autoware_auto_planning_msgs::msg::Trajectory & msg)
 {
@@ -357,6 +362,7 @@ trajectory_follower::LongitudinalOutput PidLongitudinalController::run(
   setTrajectory(input_data.current_trajectory);
   setKinematicState(input_data.current_odometry);
   setCurrentAcceleration(input_data.current_accel);
+  setCurrentOperationMode(input_data.current_operation_mode);
 
   // calculate current pose and control data
   geometry_msgs::msg::Pose current_pose = m_current_kinematic_state.pose.pose;
@@ -908,7 +914,9 @@ double PidLongitudinalController::applyVelocityFeedback(
 {
   const double current_vel_abs = std::fabs(current_vel);
   const double target_vel_abs = std::fabs(target_motion.vel);
-  const bool enable_integration = (current_vel_abs > m_current_vel_threshold_pid_integrate);
+  const bool is_under_control = m_current_operation_mode.mode == OperationModeState::AUTONOMOUS;
+  const bool enable_integration =
+    (current_vel_abs > m_current_vel_threshold_pid_integrate) && is_under_control;
   const double error_vel_filtered = m_lpf_vel_error->filter(target_vel_abs - current_vel_abs);
 
   std::vector<double> pid_contributions(3);

control/trajectory_follower_base/include/trajectory_follower_base/input_data.hpp

@@ -15,6 +15,7 @@
 #ifndef TRAJECTORY_FOLLOWER_BASE__INPUT_DATA_HPP_
 #define TRAJECTORY_FOLLOWER_BASE__INPUT_DATA_HPP_
 
+#include "autoware_adapi_v1_msgs/msg/operation_mode_state.hpp"
 #include "autoware_auto_planning_msgs/msg/trajectory.hpp"
 #include "autoware_auto_vehicle_msgs/msg/steering_report.hpp"
 #include "geometry_msgs/msg/accel_with_covariance_stamped.hpp"
@@ -28,6 +29,7 @@ struct InputData
   nav_msgs::msg::Odometry current_odometry;
   autoware_auto_vehicle_msgs::msg::SteeringReport current_steering;
   geometry_msgs::msg::AccelWithCovarianceStamped current_accel;
+  autoware_adapi_v1_msgs::msg::OperationModeState current_operation_mode;
 };
 }  // namespace autoware::motion::control::trajectory_follower
 

control/trajectory_follower_base/package.xml

@@ -20,6 +20,7 @@
 
   <build_depend>autoware_cmake</build_depend>
 
+  <depend>autoware_adapi_v1_msgs</depend>
   <depend>autoware_auto_control_msgs</depend>
   <depend>autoware_auto_geometry</depend>
   <depend>autoware_auto_planning_msgs</depend>

control/trajectory_follower_node/include/trajectory_follower_node/controller_node.hpp

@@ -48,6 +48,8 @@ using trajectory_follower::LongitudinalOutput;
 namespace trajectory_follower_node
 {
 
+using autoware_adapi_v1_msgs::msg::OperationModeState;
+
 namespace trajectory_follower = ::autoware::motion::control::trajectory_follower;
 
 /// \classController
@@ -70,6 +72,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC Controller : public rclcpp::Node
   rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr sub_odometry_;
   rclcpp::Subscription<autoware_auto_vehicle_msgs::msg::SteeringReport>::SharedPtr sub_steering_;
   rclcpp::Subscription<geometry_msgs::msg::AccelWithCovarianceStamped>::SharedPtr sub_accel_;
+  rclcpp::Subscription<OperationModeState>::SharedPtr sub_operation_mode_;
   rclcpp::Publisher<autoware_auto_control_msgs::msg::AckermannControlCommand>::SharedPtr
     control_cmd_pub_;
   rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr debug_marker_pub_;
@@ -78,6 +81,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC Controller : public rclcpp::Node
   nav_msgs::msg::Odometry::SharedPtr current_odometry_ptr_;
   autoware_auto_vehicle_msgs::msg::SteeringReport::SharedPtr current_steering_ptr_;
   geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr current_accel_ptr_;
+  OperationModeState::SharedPtr current_operation_mode_ptr_;
 
   enum class LateralControllerMode {
     INVALID = 0,

control/trajectory_follower_node/package.xml

@@ -20,6 +20,7 @@
 
   <build_depend>autoware_cmake</build_depend>
 
+  <depend>autoware_adapi_v1_msgs</depend>
   <depend>autoware_auto_control_msgs</depend>
   <depend>autoware_auto_planning_msgs</depend>
   <depend>autoware_auto_system_msgs</depend>

control/trajectory_follower_node/src/controller_node.cpp

@@ -69,6 +69,9 @@ Controller::Controller(const rclcpp::NodeOptions & node_options) : Node("control
     "~/input/current_odometry", rclcpp::QoS{1}, std::bind(&Controller::onOdometry, this, _1));
   sub_accel_ = create_subscription<geometry_msgs::msg::AccelWithCovarianceStamped>(
     "~/input/current_accel", rclcpp::QoS{1}, std::bind(&Controller::onAccel, this, _1));
+  sub_operation_mode_ = create_subscription<OperationModeState>(
+    "~/input/current_operation_mode", rclcpp::QoS{1},
+    [this](const OperationModeState::SharedPtr msg) { current_operation_mode_ptr_ = msg; });
   control_cmd_pub_ = create_publisher<autoware_auto_control_msgs::msg::AckermannControlCommand>(
     "~/output/control_cmd", rclcpp::QoS{1}.transient_local());
   debug_marker_pub_ =
@@ -163,11 +166,17 @@ boost::optional<trajectory_follower::InputData> Controller::createInputData(
     return {};
   }
 
+  if (!current_operation_mode_ptr_) {
+    RCLCPP_INFO_THROTTLE(get_logger(), clock, 5000, "Waiting for current operation mode.");
+    return {};
+  }
+
   trajectory_follower::InputData input_data;
   input_data.current_trajectory = *current_trajectory_ptr_;
   input_data.current_odometry = *current_odometry_ptr_;
   input_data.current_steering = *current_steering_ptr_;
   input_data.current_accel = *current_accel_ptr_;
+  input_data.current_operation_mode = *current_operation_mode_ptr_;
 
   return input_data;
 }

control/trajectory_follower_node/test/test_controller_node.cpp

@@ -20,6 +20,7 @@
 #include "trajectory_follower_node/controller_node.hpp"
 #include "trajectory_follower_test_utils.hpp"
 
+#include "autoware_adapi_v1_msgs/msg/operation_mode_state.hpp"
 #include "autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp"
 #include "autoware_auto_planning_msgs/msg/trajectory.hpp"
 #include "autoware_auto_vehicle_msgs/msg/steering_report.hpp"
@@ -39,6 +40,7 @@ using Trajectory = autoware_auto_planning_msgs::msg::Trajectory;
 using TrajectoryPoint = autoware_auto_planning_msgs::msg::TrajectoryPoint;
 using VehicleOdometry = nav_msgs::msg::Odometry;
 using SteeringReport = autoware_auto_vehicle_msgs::msg::SteeringReport;
+using autoware_adapi_v1_msgs::msg::OperationModeState;
 using geometry_msgs::msg::AccelWithCovarianceStamped;
 
 using FakeNodeFixture = autoware::tools::testing::FakeTestNode;
@@ -136,6 +138,14 @@ class ControllerTester
     accel_pub->publish(acc_msg);
   };
 
+  void publish_autonomous_operation_mode()
+  {
+    OperationModeState msg;
+    msg.stamp = node->now();
+    msg.mode = OperationModeState::AUTONOMOUS;
+    operation_mode_pub->publish(msg);
+  };
+
   void publish_default_traj()
   {
     Trajectory traj_msg;
@@ -167,6 +177,9 @@ class ControllerTester
   rclcpp::Publisher<AccelWithCovarianceStamped>::SharedPtr accel_pub =
     fnf->create_publisher<AccelWithCovarianceStamped>("controller/input/current_accel");
 
+  rclcpp::Publisher<OperationModeState>::SharedPtr operation_mode_pub =
+    fnf->create_publisher<OperationModeState>("controller/input/current_operation_mode");
+
   rclcpp::Subscription<AckermannControlCommand>::SharedPtr cmd_sub =
     fnf->create_subscription<AckermannControlCommand>(
       "controller/output/control_cmd", *fnf->get_fake_node(),
@@ -209,6 +222,7 @@ TEST_F(FakeNodeFixture, empty_trajectory)
   // Empty trajectory: expect a stopped command
   tester.publish_default_traj();
   tester.publish_default_odom();
+  tester.publish_autonomous_operation_mode();
   tester.publish_default_acc();
   tester.publish_default_steer();
 
@@ -225,6 +239,7 @@ TEST_F(FakeNodeFixture, straight_trajectory)
 
   tester.send_default_transform();
   tester.publish_odom_vx(1.0);
+  tester.publish_autonomous_operation_mode();
   tester.publish_default_steer();
   tester.publish_default_acc();
 
@@ -251,6 +266,7 @@ TEST_F(FakeNodeFixture, right_turn)
 
   tester.send_default_transform();
   tester.publish_odom_vx(1.0);
+  tester.publish_autonomous_operation_mode();
   tester.publish_default_steer();
   tester.publish_default_acc();
 
@@ -278,6 +294,7 @@ TEST_F(FakeNodeFixture, left_turn)
 
   tester.send_default_transform();
   tester.publish_odom_vx(1.0);
+  tester.publish_autonomous_operation_mode();
   tester.publish_default_steer();
   tester.publish_default_acc();
 
@@ -305,6 +322,7 @@ TEST_F(FakeNodeFixture, stopped)
 
   tester.send_default_transform();
   tester.publish_default_odom();
+  tester.publish_autonomous_operation_mode();
   tester.publish_default_acc();
 
   const double steering_tire_angle = -0.5;
@@ -335,6 +353,7 @@ TEST_F(FakeNodeFixture, longitudinal_keep_velocity)
 
   tester.send_default_transform();
   tester.publish_odom_vx(1.0);
+  tester.publish_autonomous_operation_mode();
   tester.publish_default_steer();
   tester.publish_default_acc();
 
@@ -373,6 +392,8 @@ TEST_F(FakeNodeFixture, longitudinal_slow_down)
   const double odom_vx = 1.0;
   tester.publish_odom_vx(odom_vx);
 
+  tester.publish_autonomous_operation_mode();
+
   // Publish non stopping trajectory
   Trajectory traj;
   traj.header.stamp = tester.node->now();
@@ -408,6 +429,8 @@ TEST_F(FakeNodeFixture, longitudinal_accelerate)
   const double odom_vx = 0.5;
   tester.publish_odom_vx(odom_vx);
 
+  tester.publish_autonomous_operation_mode();
+
   // Publish non stopping trajectory
   Trajectory traj;
   traj.header.stamp = tester.node->now();
@@ -438,6 +461,7 @@ TEST_F(FakeNodeFixture, longitudinal_stopped)
 
   tester.send_default_transform();
   tester.publish_default_odom();
+  tester.publish_autonomous_operation_mode();
   tester.publish_default_steer();
   tester.publish_default_acc();
 
@@ -467,6 +491,7 @@ TEST_F(FakeNodeFixture, longitudinal_reverse)
   tester.send_default_transform();
 
   tester.publish_default_odom();
+  tester.publish_autonomous_operation_mode();
   tester.publish_default_steer();
   tester.publish_default_acc();
 
@@ -493,6 +518,7 @@ TEST_F(FakeNodeFixture, longitudinal_emergency)
 
   tester.send_default_transform();
   tester.publish_default_odom();
+  tester.publish_autonomous_operation_mode();
   tester.publish_default_steer();
   tester.publish_default_acc();
 
@@ -520,6 +546,7 @@ TEST_F(FakeNodeFixture, longitudinal_not_check_steer_converged)
 
   tester.send_default_transform();
   tester.publish_default_odom();
+  tester.publish_autonomous_operation_mode();
   tester.publish_default_acc();
 
   // steering_tire_angle has to be larger than the threshold to check convergence.
@@ -550,6 +577,7 @@ TEST_F(FakeNodeFixture, longitudinal_check_steer_converged)
 
   tester.send_default_transform();
   tester.publish_default_odom();
+  tester.publish_autonomous_operation_mode();
   tester.publish_default_acc();
 
   // steering_tire_angle has to be larger than the threshold to check convergence.

launch/tier4_control_launch/launch/control.launch.py

@@ -70,6 +70,7 @@ def launch_setup(context, *args, **kwargs):
             ("~/input/current_odometry", "/localization/kinematic_state"),
             ("~/input/current_steering", "/vehicle/status/steering_status"),
             ("~/input/current_accel", "/localization/acceleration"),
+            ("~/input/current_operation_mode", "/system/operation_mode/state"),
             ("~/output/predicted_trajectory", "lateral/predicted_trajectory"),
             ("~/output/lateral_diagnostic", "lateral/diagnostic"),
             ("~/output/slope_angle", "longitudinal/slope_angle"),