refactor: replace acc calculation in planning control modules (tier4#…

…1213) * [obstacle_cruise_planner] replace acceleration calculation Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * [obstacle_stop_planner] replace acceleration calculation Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * [trajectory_follower] replace acceleration calculation Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * remap topic name in lanuch Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * fix nullptr check Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * fix controller test Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * fix Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>
boyali · Oct 3, 2022 · 51892f6 · 51892f6
1 parent c925002
commit 51892f6
Show file tree

Hide file tree

Showing 17 changed files with 401 additions and 812 deletions.
diff --git a/control/trajectory_follower/include/trajectory_follower/input_data.hpp b/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

diff --git a/control/trajectory_follower/include/trajectory_follower/pid_longitudinal_controller.hpp b/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

diff --git a/control/trajectory_follower/src/pid_longitudinal_controller.cpp b/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
 {

diff --git a/control/trajectory_follower_nodes/include/trajectory_follower_nodes/controller_node.hpp b/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(

diff --git a/control/trajectory_follower_nodes/src/controller_node.cpp b/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();