refactor: replace acc calculation in planning control modules

control/trajectory_follower/include/trajectory_follower/input_data.hpp

@@ -17,6 +17,7 @@
 
 #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"
 #include "nav_msgs/msg/odometry.hpp"
 
 namespace autoware
@@ -32,6 +33,7 @@ struct InputData
   autoware_auto_planning_msgs::msg::Trajectory::SharedPtr current_trajectory_ptr;
   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;
 };
 }  // namespace trajectory_follower
 }  // namespace control

control/trajectory_follower/include/trajectory_follower/pid_longitudinal_controller.hpp

@@ -97,7 +97,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC PidLongitudinalController : public Longitudinal
 
   // pointers for ros topic
   nav_msgs::msg::Odometry::ConstSharedPtr m_current_kinematic_state_ptr{nullptr};
-  nav_msgs::msg::Odometry::ConstSharedPtr m_prev_kienmatic_state_ptr{nullptr};
+  geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr m_current_accel_ptr{nullptr};
   autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr m_trajectory_ptr{nullptr};
 
   // vehicle info
@@ -185,9 +185,6 @@ class TRAJECTORY_FOLLOWER_PUBLIC PidLongitudinalController : public Longitudinal
   double m_ego_nearest_dist_threshold;
   double m_ego_nearest_yaw_threshold;
 
-  // 1st order lowpass filter for acceleration
-  std::shared_ptr<trajectory_follower::LowpassFilter1d> m_lpf_acc{nullptr};
-
   // buffer of send command
   std::vector<autoware_auto_control_msgs::msg::LongitudinalCommand> m_ctrl_cmd_vec;
 
@@ -213,6 +210,13 @@ class TRAJECTORY_FOLLOWER_PUBLIC PidLongitudinalController : public Longitudinal
    */
   void setKinematicState(const nav_msgs::msg::Odometry::ConstSharedPtr msg);
 
+  /**
+   * @brief set current acceleration with received message
+   * @param [in] msg trajectory message
+   */
+  void setCurrentAcceleration(
+    const geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr msg);
+
   /**
    * @brief set reference trajectory with received message
    * @param [in] msg trajectory message

control/trajectory_follower/src/pid_longitudinal_controller.cpp

@@ -199,26 +199,27 @@ PidLongitudinalController::PidLongitudinalController(rclcpp::Node & node) : node
   // set parameter callback
   m_set_param_res = node_->add_on_set_parameters_callback(
     std::bind(&PidLongitudinalController::paramCallback, this, _1));
-
-  // set lowpass filter for acc
-  m_lpf_acc = std::make_shared<trajectory_follower::LowpassFilter1d>(0.0, 0.2);
 }
 void PidLongitudinalController::setInputData(InputData const & input_data)
 {
   setTrajectory(input_data.current_trajectory_ptr);
   setKinematicState(input_data.current_odometry_ptr);
+  setCurrentAcceleration(input_data.current_accel_ptr);
 }
 
 void PidLongitudinalController::setKinematicState(const nav_msgs::msg::Odometry::ConstSharedPtr msg)
 {
   if (!msg) return;
-
-  if (m_current_kinematic_state_ptr) {
-    m_prev_kienmatic_state_ptr = m_current_kinematic_state_ptr;
-  }
   m_current_kinematic_state_ptr = msg;
 }
 
+void PidLongitudinalController::setCurrentAcceleration(
+  const geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr msg)
+{
+  if (!msg) return;
+  m_current_accel_ptr = msg;
+}
+
 void PidLongitudinalController::setTrajectory(
   const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg)
 {
@@ -365,7 +366,7 @@ rcl_interfaces::msg::SetParametersResult PidLongitudinalController::paramCallbac
 boost::optional<LongitudinalOutput> PidLongitudinalController::run()
 {
   // wait for initial pointers
-  if (!m_current_kinematic_state_ptr || !m_prev_kienmatic_state_ptr || !m_trajectory_ptr) {
+  if (!m_current_kinematic_state_ptr || !m_trajectory_ptr || !m_current_accel_ptr) {
     return boost::none;
   }
 
@@ -415,7 +416,8 @@ PidLongitudinalController::ControlData PidLongitudinalController::getControlData
   control_data.dt = getDt();
 
   // current velocity and acceleration
-  control_data.current_motion = getCurrentMotion();
+  control_data.current_motion.vel = m_current_kinematic_state_ptr->twist.twist.linear.x;
+  control_data.current_motion.acc = m_current_accel_ptr->accel.accel.linear.x;
 
   // nearest idx
   const size_t nearest_idx = motion_utils::findFirstNearestIndexWithSoftConstraints(
@@ -699,23 +701,6 @@ float64_t PidLongitudinalController::getDt()
   return std::max(std::min(dt, max_dt), min_dt);
 }
 
-PidLongitudinalController::Motion PidLongitudinalController::getCurrentMotion() const
-{
-  const float64_t dv = m_current_kinematic_state_ptr->twist.twist.linear.x -
-                       m_prev_kienmatic_state_ptr->twist.twist.linear.x;
-  const float64_t dt = std::max(
-    (rclcpp::Time(m_current_kinematic_state_ptr->header.stamp) -
-     rclcpp::Time(m_prev_kienmatic_state_ptr->header.stamp))
-      .seconds(),
-    1e-03);
-  const float64_t accel = dv / dt;
-
-  const float64_t current_vel = m_current_kinematic_state_ptr->twist.twist.linear.x;
-  const float64_t current_acc = m_lpf_acc->filter(accel);
-
-  return Motion{current_vel, current_acc};
-}
-
 enum PidLongitudinalController::Shift PidLongitudinalController::getCurrentShift(
   const size_t nearest_idx) const
 {

control/trajectory_follower_nodes/include/trajectory_follower_nodes/controller_node.hpp

@@ -37,6 +37,8 @@
 #include "autoware_auto_planning_msgs/msg/trajectory.hpp"
 #include "autoware_auto_system_msgs/msg/float32_multi_array_diagnostic.hpp"
 #include "autoware_auto_vehicle_msgs/msg/vehicle_odometry.hpp"
+#include "geometry_msgs/msg/accel_stamped.hpp"
+#include "geometry_msgs/msg/accel_with_covariance_stamped.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
 #include "nav_msgs/msg/odometry.hpp"
 #include "tf2_msgs/msg/tf_message.hpp"
@@ -82,6 +84,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC Controller : public rclcpp::Node
   rclcpp::Subscription<autoware_auto_planning_msgs::msg::Trajectory>::SharedPtr sub_ref_path_;
   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::Publisher<autoware_auto_control_msgs::msg::AckermannControlCommand>::SharedPtr
     control_cmd_pub_;
 
@@ -102,6 +105,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC Controller : public rclcpp::Node
   void onTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr);
   void onOdometry(const nav_msgs::msg::Odometry::SharedPtr msg);
   void onSteering(const autoware_auto_vehicle_msgs::msg::SteeringReport::SharedPtr msg);
+  void onAccel(const geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr msg);
   bool isTimeOut();
   LateralControllerMode getLateralControllerMode(const std::string & algorithm_name) const;
   LongitudinalControllerMode getLongitudinalControllerMode(

control/trajectory_follower_nodes/src/controller_node.cpp

@@ -76,6 +76,8 @@ Controller::Controller(const rclcpp::NodeOptions & node_options) : Node("control
     "~/input/current_steering", rclcpp::QoS{1}, std::bind(&Controller::onSteering, this, _1));
   sub_odometry_ = create_subscription<nav_msgs::msg::Odometry>(
     "~/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));
   control_cmd_pub_ = create_publisher<autoware_auto_control_msgs::msg::AckermannControlCommand>(
     "~/output/control_cmd", rclcpp::QoS{1}.transient_local());
 
@@ -120,6 +122,11 @@ void Controller::onSteering(const autoware_auto_vehicle_msgs::msg::SteeringRepor
   input_data_.current_steering_ptr = msg;
 }
 
+void Controller::onAccel(const geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr msg)
+{
+  input_data_.current_accel_ptr = msg;
+}
+
 bool Controller::isTimeOut()
 {
   const auto now = this->now();

launch/tier4_control_launch/launch/control.launch.py

@@ -99,6 +99,7 @@ def launch_setup(context, *args, **kwargs):
             ("~/input/reference_trajectory", "/planning/scenario_planning/trajectory"),
             ("~/input/current_odometry", "/localization/kinematic_state"),
             ("~/input/current_steering", "/vehicle/status/steering_status"),
+            ("~/input/current_accel", "/localization/acceleration"),
             ("~/output/predicted_trajectory", "lateral/predicted_trajectory"),
             ("~/output/lateral_diagnostic", "lateral/diagnostic"),
             ("~/output/slope_angle", "longitudinal/slope_angle"),

launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml

@@ -11,8 +11,6 @@
       min_object_accel_for_rss: -1.0 # front obstacle's acceleration to calculate RSS distance [m/ss]
       safe_distance_margin : 6.0 # This is also used as a stop margin [m]
 
-      lpf_gain_for_accel: 0.2 # gain of low pass filter for ego acceleration [-]
-
       nearest_dist_deviation_threshold: 3.0 # [m] for finding nearest index
       nearest_yaw_deviation_threshold: 1.57 # [rad] for finding nearest index
       min_behavior_stop_margin: 3.0 # [m]

launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py

@@ -160,6 +160,7 @@ def launch_setup(context, *args, **kwargs):
                 "/planning/scenario_planning/clear_velocity_limit",
             ),
             ("~/output/trajectory", "/planning/scenario_planning/lane_driving/trajectory"),
+            ("~/input/acceleration", "/localization/acceleration"),
             (
                 "~/input/pointcloud",
                 "/perception/obstacle_segmentation/pointcloud",
@@ -198,6 +199,7 @@ def launch_setup(context, *args, **kwargs):
         remappings=[
             ("~/input/trajectory", "obstacle_avoidance_planner/trajectory"),
             ("~/input/odometry", "/localization/kinematic_state"),
+            ("~/input/acceleration", "/localization/acceleration"),
             ("~/input/objects", "/perception/object_recognition/objects"),
             ("~/output/trajectory", "/planning/scenario_planning/lane_driving/trajectory"),
             ("~/output/velocity_limit", "/planning/scenario_planning/max_velocity_candidates"),

planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml

@@ -11,8 +11,6 @@
       min_object_accel_for_rss: -1.0 # front obstacle's acceleration to calculate RSS distance [m/ss]
       safe_distance_margin : 6.0 # This is also used as a stop margin [m]
 
-      lpf_gain_for_accel: 0.2 # gain of low pass filter for ego acceleration [-]
-
       nearest_dist_deviation_threshold: 3.0 # [m] for finding nearest index
       nearest_yaw_deviation_threshold: 1.57 # [rad] for finding nearest index
       min_behavior_stop_margin: 3.0 # [m]

planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp

@@ -26,6 +26,8 @@
 #include "autoware_auto_perception_msgs/msg/predicted_object.hpp"
 #include "autoware_auto_perception_msgs/msg/predicted_objects.hpp"
 #include "autoware_auto_planning_msgs/msg/trajectory.hpp"
+#include "geometry_msgs/msg/accel_stamped.hpp"
+#include "geometry_msgs/msg/accel_with_covariance_stamped.hpp"
 #include "geometry_msgs/msg/point_stamped.hpp"
 #include "geometry_msgs/msg/transform_stamped.hpp"
 #include "geometry_msgs/msg/twist_stamped.hpp"
@@ -48,6 +50,8 @@ using autoware_auto_perception_msgs::msg::PredictedObjects;
 using autoware_auto_perception_msgs::msg::PredictedPath;
 using autoware_auto_planning_msgs::msg::Trajectory;
 using autoware_auto_planning_msgs::msg::TrajectoryPoint;
+using geometry_msgs::msg::AccelStamped;
+using geometry_msgs::msg::AccelWithCovarianceStamped;
 using nav_msgs::msg::Odometry;
 using tier4_debug_msgs::msg::Float32Stamped;
 using tier4_planning_msgs::msg::StopReasonArray;
@@ -68,6 +72,7 @@ class ObstacleCruisePlannerNode : public rclcpp::Node
     const std::vector<rclcpp::Parameter> & parameters);
   void onObjects(const PredictedObjects::ConstSharedPtr msg);
   void onOdometry(const Odometry::ConstSharedPtr);
+  void onAccel(const AccelWithCovarianceStamped::ConstSharedPtr);
   void onTrajectory(const Trajectory::ConstSharedPtr msg);
   void onSmoothedTrajectory(const Trajectory::ConstSharedPtr msg);
 
@@ -78,7 +83,6 @@ class ObstacleCruisePlannerNode : public rclcpp::Node
   ObstacleCruisePlannerData createStopData(
     const Trajectory & trajectory, const geometry_msgs::msg::Pose & current_pose,
     const std::vector<TargetObstacle> & obstacles, const bool is_driving_forward);
-  double calcCurrentAccel() const;
   std::vector<TargetObstacle> getTargetObstacles(
     const Trajectory & trajectory, const geometry_msgs::msg::Pose & current_pose,
     const double current_vel, const bool is_driving_forward, DebugData & debug_data);
@@ -129,17 +133,16 @@ class ObstacleCruisePlannerNode : public rclcpp::Node
   rclcpp::Subscription<Trajectory>::SharedPtr smoothed_trajectory_sub_;
   rclcpp::Subscription<PredictedObjects>::SharedPtr objects_sub_;
   rclcpp::Subscription<Odometry>::SharedPtr odom_sub_;
+  rclcpp::Subscription<AccelWithCovarianceStamped>::SharedPtr acc_sub_;
 
   // self pose listener
   tier4_autoware_utils::SelfPoseListener self_pose_listener_;
 
   // data for callback functions
   PredictedObjects::ConstSharedPtr in_objects_ptr_;
   geometry_msgs::msg::TwistStamped::SharedPtr current_twist_ptr_;
-  geometry_msgs::msg::TwistStamped::SharedPtr prev_twist_ptr_;
 
-  // low pass filter of acceleration
-  std::shared_ptr<LowpassFilter1d> lpf_acc_ptr_;
+  geometry_msgs::msg::AccelStamped::SharedPtr current_accel_ptr_;
 
   // Vehicle Parameters
   VehicleInfo vehicle_info_;

planning/obstacle_cruise_planner/src/node.cpp

@@ -195,7 +195,6 @@ ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions &
 : Node("obstacle_cruise_planner", node_options),
   self_pose_listener_(this),
   in_objects_ptr_(nullptr),
-  lpf_acc_ptr_(nullptr),
   vehicle_info_(vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo())
 {
   using std::placeholders::_1;
@@ -212,6 +211,9 @@ ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions &
   odom_sub_ = create_subscription<Odometry>(
     "~/input/odometry", rclcpp::QoS{1},
     std::bind(&ObstacleCruisePlannerNode::onOdometry, this, std::placeholders::_1));
+  acc_sub_ = create_subscription<AccelStamped>(
+    "~/input/acceleration", rclcpp::QoS{1},
+    std::bind(&ObstacleCruisePlannerNode::onAccel, this, std::placeholders::_1));
 
   // publisher
   trajectory_pub_ = create_publisher<Trajectory>("~/output/trajectory", 1);
@@ -260,10 +262,6 @@ ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions &
 
   is_showing_debug_info_ = declare_parameter<bool>("common.is_showing_debug_info");
 
-  // low pass filter for ego acceleration
-  const double lpf_gain_for_accel = declare_parameter<double>("common.lpf_gain_for_accel");
-  lpf_acc_ptr_ = std::make_shared<LowpassFilter1d>(lpf_gain_for_accel);
-
   {  // Obstacle filtering parameters
     obstacle_filtering_param_.rough_detection_area_expand_width =
       declare_parameter<double>("obstacle_filtering.rough_detection_area_expand_width");
@@ -502,15 +500,18 @@ void ObstacleCruisePlannerNode::onObjects(const PredictedObjects::ConstSharedPtr
 
 void ObstacleCruisePlannerNode::onOdometry(const Odometry::ConstSharedPtr msg)
 {
-  if (current_twist_ptr_) {
-    prev_twist_ptr_ = current_twist_ptr_;
-  }
-
   current_twist_ptr_ = std::make_unique<geometry_msgs::msg::TwistStamped>();
   current_twist_ptr_->header = msg->header;
   current_twist_ptr_->twist = msg->twist.twist;
 }
 
+void ObstacleCruisePlannerNode::onAccel(const AccelWithCovarianceStamped::ConstSharedPtr msg)
+{
+  current_accel_ptr_ = std::make_unique<geometry_msgs::msg::AccelStamped>();
+  current_accel_ptr_->header = msg->header;
+  current_accel_ptr_->accel = msg->accel.accel;
+}
+
 void ObstacleCruisePlannerNode::onSmoothedTrajectory(const Trajectory::ConstSharedPtr msg)
 {
   planner_ptr_->setSmoothedTrajectory(msg);
@@ -521,9 +522,7 @@ void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr ms
   const auto current_pose_ptr = self_pose_listener_.getCurrentPose();
 
   // check if subscribed variables are ready
-  if (
-    msg->points.empty() || !current_twist_ptr_ || !prev_twist_ptr_ || !in_objects_ptr_ ||
-    !current_pose_ptr) {
+  if (msg->points.empty() || !current_twist_ptr_ || !in_objects_ptr_ || !current_pose_ptr) {
     return;
   }
 
@@ -588,7 +587,7 @@ ObstacleCruisePlannerData ObstacleCruisePlannerNode::createStopData(
 {
   const auto current_time = now();
   const double current_vel = current_twist_ptr_->twist.linear.x;
-  const double current_accel = calcCurrentAccel();
+  const double current_accel = current_accel_ptr_->accel.linear.x;
 
   // create planner_stop data
   ObstacleCruisePlannerData planner_data;
@@ -614,7 +613,7 @@ ObstacleCruisePlannerData ObstacleCruisePlannerNode::createCruiseData(
 {
   const auto current_time = now();
   const double current_vel = current_twist_ptr_->twist.linear.x;
-  const double current_accel = calcCurrentAccel();
+  const double current_accel = current_accel_ptr_->accel.linear.x;
 
   // create planner_stop data
   ObstacleCruisePlannerData planner_data;
@@ -633,19 +632,6 @@ ObstacleCruisePlannerData ObstacleCruisePlannerNode::createCruiseData(
   return planner_data;
 }
 
-double ObstacleCruisePlannerNode::calcCurrentAccel() const
-{
-  const double diff_vel = current_twist_ptr_->twist.linear.x - prev_twist_ptr_->twist.linear.x;
-  const double diff_time = std::max(
-    (rclcpp::Time(current_twist_ptr_->header.stamp) - rclcpp::Time(prev_twist_ptr_->header.stamp))
-      .seconds(),
-    1e-03);
-
-  const double accel = diff_vel / diff_time;
-
-  return lpf_acc_ptr_->filter(accel);
-}
-
 std::vector<TargetObstacle> ObstacleCruisePlannerNode::getTargetObstacles(
   const Trajectory & trajectory, const geometry_msgs::msg::Pose & current_pose,
   const double current_vel, const bool is_driving_forward, DebugData & debug_data)

planning/obstacle_stop_planner/README.md

@@ -36,7 +36,6 @@
 | `enable_slow_down`  | bool   | enable slow down planner [-]                                                           |
 | `max_velocity`      | double | max velocity [m/s]                                                                     |
 | `hunting_threshold` | double | even if the obstacle disappears, the stop judgment continues for hunting_threshold [s] |
-| `lowpass_gain`      | double | low pass gain for calculating acceleration [-]                                         |
 
 ## Obstacle Stop Planner
 

planning/obstacle_stop_planner/config/obstacle_stop_planner.param.yaml

@@ -1,7 +1,6 @@
 /**:
   ros__parameters:
     hunting_threshold: 0.5                   # even if the obstacle disappears, the stop judgment continues for hunting_threshold [s]
-    lowpass_gain: 0.9                        # gain parameter for low pass filter [-]
     max_velocity: 20.0                       # max velocity [m/s]
     enable_slow_down: False                  # whether to use slow down planner [-]