feat(longitudinal_controller): skip integral in manual mode

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

control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp

@@ -52,7 +52,7 @@ namespace control
 {
 namespace pid_longitudinal_controller
 {
-
+using autoware_auto_vehicle_msgs::msg::ControlModeReport;
 namespace trajectory_follower = ::autoware::motion::control::trajectory_follower;
 
 /// \class PidLongitudinalController
@@ -94,6 +94,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;
+  autoware_auto_vehicle_msgs::msg::ControlModeReport m_current_control_mode;
 
   // vehicle info
   double m_wheel_base;
@@ -231,6 +232,12 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro
    */
   void setCurrentAcceleration(const geometry_msgs::msg::AccelWithCovarianceStamped & msg);
 
+  /**
+   * @brief set current control mode with received message
+   * @param [in] msg control mode report message
+   */
+  void setCurrentControlMode(const autoware_auto_vehicle_msgs::msg::ControlModeReport & msg);
+
   /**
    * @brief set reference trajectory with received message
    * @param [in] msg trajectory message

control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp

@@ -210,6 +210,12 @@ void PidLongitudinalController::setCurrentAcceleration(
   m_current_accel = msg;
 }
 
+void PidLongitudinalController::setCurrentControlMode(
+  const autoware_auto_vehicle_msgs::msg::ControlModeReport & msg)
+{
+  m_current_control_mode = msg;
+}
+
 void PidLongitudinalController::setTrajectory(
   const autoware_auto_planning_msgs::msg::Trajectory & msg)
 {
@@ -364,6 +370,7 @@ trajectory_follower::LongitudinalOutput PidLongitudinalController::run(
   setTrajectory(input_data.current_trajectory);
   setKinematicState(input_data.current_odometry);
   setCurrentAcceleration(input_data.current_accel);
+  setCurrentControlMode(input_data.current_control_mode);
 
   // calculate current pose and control data
   geometry_msgs::msg::Pose current_pose = m_current_kinematic_state.pose.pose;
@@ -922,7 +929,11 @@ double PidLongitudinalController::applyVelocityFeedback(
   using pid_longitudinal_controller::DebugValues;
   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_control_mode.mode == ControlModeReport::AUTONOMOUS ||
+    m_current_control_mode.mode == ControlModeReport::AUTONOMOUS_VELOCITY_ONLY;
+  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

@@ -16,6 +16,7 @@
 #define TRAJECTORY_FOLLOWER_BASE__INPUT_DATA_HPP_
 
 #include "autoware_auto_planning_msgs/msg/trajectory.hpp"
+#include "autoware_auto_vehicle_msgs/msg/control_mode_report.hpp"
 #include "autoware_auto_vehicle_msgs/msg/steering_report.hpp"
 #include "geometry_msgs/msg/accel_with_covariance_stamped.hpp"
 #include "nav_msgs/msg/odometry.hpp"
@@ -34,6 +35,7 @@ struct InputData
   nav_msgs::msg::Odometry current_odometry;
   autoware_auto_vehicle_msgs::msg::SteeringReport current_steering;
   geometry_msgs::msg::AccelWithCovarianceStamped current_accel;
+  autoware_auto_vehicle_msgs::msg::ControlModeReport current_control_mode;
 };
 }  // namespace trajectory_follower
 }  // namespace control

control/trajectory_follower_node/include/trajectory_follower_node/controller_node.hpp

@@ -52,6 +52,8 @@ using trajectory_follower::LongitudinalOutput;
 namespace trajectory_follower_node
 {
 
+using autoware_auto_vehicle_msgs::msg::ControlModeReport;
+
 namespace trajectory_follower = ::autoware::motion::control::trajectory_follower;
 
 /// \classController
@@ -74,6 +76,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<ControlModeReport>::SharedPtr sub_control_mode_;
   rclcpp::Publisher<autoware_auto_control_msgs::msg::AckermannControlCommand>::SharedPtr
     control_cmd_pub_;
   rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr debug_marker_pub_;
@@ -82,6 +85,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_;
+  ControlModeReport::SharedPtr current_control_mode_ptr_;
 
   enum class LateralControllerMode {
     INVALID = 0,

control/trajectory_follower_node/src/controller_node.cpp

@@ -75,6 +75,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_control_mode_ = create_subscription<ControlModeReport>(
+    "~/input/current_control_mode", rclcpp::QoS{1},
+    [this](const ControlModeReport::SharedPtr msg) { current_control_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_ =
@@ -169,11 +172,17 @@ boost::optional<trajectory_follower::InputData> Controller::createInputData(
     return {};
   }
 
+  if (!current_control_mode_ptr_) {
+    RCLCPP_INFO_THROTTLE(get_logger(), clock, 5000, "Waiting for current control 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_control_mode = *current_control_mode_ptr_;
 
   return input_data;
 }

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_control_mode", "/vehicle/status/control_mode"),
             ("~/output/predicted_trajectory", "lateral/predicted_trajectory"),
             ("~/output/lateral_diagnostic", "lateral/diagnostic"),
             ("~/output/slope_angle", "longitudinal/slope_angle"),