feat(trajectory_follower): refactor (autowarefoundation#238)

* refactor(trajectory_follower_nodes): refactor the controller framework (autowarefoundation#2432) * refactor(trajectory_follower_nodes): refactor the controller framework Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * fix Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * add test for keeping stopped state when steer is not converged Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * fix the bug that initialization of mpc does not finish Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * apply the change to pure_pursuit Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * remove initialize function and implement reset function Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * minor change Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * feat(trajectory_follower): seperate lat lon controller packages (autowarefoundation#2580) * feat(trajectory_follower): seperate controller implementation packages Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * update doc Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * fix doc Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * fix test Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * rename: mpc_follower -> mpc Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * rename to trajectory_follower_base, trajectory_follower_node Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * fix doc Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * remove unnecessary change Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
zulfaqar-azmi-t4 · Jan 11, 2023 · 9d736c4 · 9d736c4
1 parent 669c493
commit 9d736c4
Show file tree

Hide file tree

Showing 92 changed files with 1,773 additions and 940 deletions.
diff --git a/control/mpc_lateral_controller/CMakeLists.txt b/control/mpc_lateral_controller/CMakeLists.txt
@@ -0,0 +1,37 @@
+cmake_minimum_required(VERSION 3.14)
+project(mpc_lateral_controller)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+set(MPC_LAT_CON_LIB ${PROJECT_NAME}_lib)
+ament_auto_add_library(${MPC_LAT_CON_LIB} SHARED
+  src/mpc_lateral_controller.cpp
+  src/interpolate.cpp
+  src/lowpass_filter.cpp
+  src/mpc.cpp
+  src/mpc_trajectory.cpp
+  src/mpc_utils.cpp
+  src/qp_solver/qp_solver_osqp.cpp
+  src/qp_solver/qp_solver_unconstr_fast.cpp
+  src/vehicle_model/vehicle_model_bicycle_dynamics.cpp
+  src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp
+  src/vehicle_model/vehicle_model_bicycle_kinematics.cpp
+  src/vehicle_model/vehicle_model_interface.cpp
+)
+
+if(BUILD_TESTING)
+  set(TEST_LAT_SOURCES
+    test/test_mpc.cpp
+    test/test_mpc_utils.cpp
+    test/test_interpolate.cpp
+    test/test_lowpass_filter.cpp
+  )
+  set(TEST_LATERAL_CONTROLLER_EXE test_lateral_controller)
+  ament_add_ros_isolated_gtest(${TEST_LATERAL_CONTROLLER_EXE} ${TEST_LAT_SOURCES})
+  target_link_libraries(${TEST_LATERAL_CONTROLLER_EXE} ${MPC_LAT_CON_LIB})
+endif()
+
+ament_auto_package(INSTALL_TO_SHARE
+  param
+)
diff --git a/...r/design/mpc_lateral_controller-design.md → control/mpc_lateral_controller/README.md b/...r/design/mpc_lateral_controller-design.md → control/mpc_lateral_controller/README.md
@@ -1,7 +1,7 @@
 # MPC Lateral Controller
 
 This is the design document for the lateral controller node
-in the `trajectory_follower_nodes` package.
+in the `trajectory_follower_node` package.
 
 ## Purpose / Use cases
 
@@ -58,15 +58,15 @@ The tracking is not accurate if the first point of the reference trajectory is a
 
 ### Inputs
 
-Set the following from the [controller_node](../../trajectory_follower_nodes/design/trajectory_follower-design.md)
+Set the following from the [controller_node](../trajectory_follower_node/README.md)
 
 - `autoware_auto_planning_msgs/Trajectory` : reference trajectory to follow.
 - `nav_msgs/Odometry`: current odometry
 - `autoware_auto_vehicle_msgs/SteeringReport` current steering
 
 ### Outputs
 
-Return LateralOutput which contains the following to [controller_node](../../trajectory_follower_nodes/design/trajectory_follower-design.md)
+Return LateralOutput which contains the following to the controller node
 
 - `autoware_auto_control_msgs/AckermannLateralCommand`
 - LateralSyncData

diff --git a/...clude/trajectory_follower/interpolate.hpp → ...de/mpc_lateral_controller/interpolate.hpp b/...clude/trajectory_follower/interpolate.hpp → ...de/mpc_lateral_controller/interpolate.hpp
@@ -12,10 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef TRAJECTORY_FOLLOWER__INTERPOLATE_HPP_
-#define TRAJECTORY_FOLLOWER__INTERPOLATE_HPP_
-
-#include "trajectory_follower/visibility_control.hpp"
+#ifndef MPC_LATERAL_CONTROLLER__INTERPOLATE_HPP_
+#define MPC_LATERAL_CONTROLLER__INTERPOLATE_HPP_
 
 #include <cmath>
 #include <iostream>
@@ -27,7 +25,7 @@ namespace motion
 {
 namespace control
 {
-namespace trajectory_follower
+namespace mpc_lateral_controller
 {
 
 /**
@@ -37,7 +35,7 @@ namespace trajectory_follower
  * @param [in] return_index desired interpolated indexes
  * @param [out] return_value resulting interpolated values
  */
-TRAJECTORY_FOLLOWER_PUBLIC bool linearInterpolate(
+bool linearInterpolate(
   const std::vector<double> & base_index, const std::vector<double> & base_value,
   const std::vector<double> & return_index, std::vector<double> & return_value);
 /**
@@ -47,11 +45,11 @@ TRAJECTORY_FOLLOWER_PUBLIC bool linearInterpolate(
  * @param [in] return_index desired interpolated index
  * @param [out] return_value resulting interpolated value
  */
-TRAJECTORY_FOLLOWER_PUBLIC bool linearInterpolate(
+bool linearInterpolate(
   const std::vector<double> & base_index, const std::vector<double> & base_value,
   const double & return_index, double & return_value);
-}  // namespace trajectory_follower
+}  // namespace mpc_lateral_controller
 }  // namespace control
 }  // namespace motion
 }  // namespace autoware
-#endif  // TRAJECTORY_FOLLOWER__INTERPOLATE_HPP_
+#endif  // MPC_LATERAL_CONTROLLER__INTERPOLATE_HPP_
diff --git a/...de/trajectory_follower/lowpass_filter.hpp → ...mpc_lateral_controller/lowpass_filter.hpp b/...de/trajectory_follower/lowpass_filter.hpp → ...mpc_lateral_controller/lowpass_filter.hpp
@@ -12,10 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef TRAJECTORY_FOLLOWER__LOWPASS_FILTER_HPP_
-#define TRAJECTORY_FOLLOWER__LOWPASS_FILTER_HPP_
-
-#include "trajectory_follower/visibility_control.hpp"
+#ifndef MPC_LATERAL_CONTROLLER__LOWPASS_FILTER_HPP_
+#define MPC_LATERAL_CONTROLLER__LOWPASS_FILTER_HPP_
 
 #include <algorithm>
 #include <cmath>
@@ -28,54 +26,13 @@ namespace motion
 {
 namespace control
 {
-namespace trajectory_follower
-{
-
-/**
- * @brief Simple filter with gain on the first derivative of the value
- */
-class LowpassFilter1d
+namespace mpc_lateral_controller
 {
-private:
-  double m_x;     //!< @brief current filtered value
-  double m_gain;  //!< @brief gain value of first-order low-pass filter
-
-public:
-  /**
-   * @brief constructor with initial value and first-order gain
-   * @param [in] x initial value
-   * @param [in] gain first-order gain
-   */
-  LowpassFilter1d(const double x, const double gain) : m_x(x), m_gain(gain) {}
-
-  /**
-   * @brief set the current value of the filter
-   * @param [in] x new value
-   */
-  void reset(const double x) { m_x = x; }
-
-  /**
-   * @brief get the current value of the filter
-   */
-  double getValue() const { return m_x; }
-
-  /**
-   * @brief filter a new value
-   * @param [in] u new value
-   */
-  double filter(const double u)
-  {
-    const double ret = m_gain * m_x + (1.0 - m_gain) * u;
-    m_x = ret;
-    return ret;
-  }
-};
-
 /**
  * @brief 2nd-order Butterworth Filter
  * reference : S. Butterworth, "On the Theory of Filter Amplifier", Experimental wireless, 1930.
  */
-class TRAJECTORY_FOLLOWER_PUBLIC Butterworth2dFilter
+class Butterworth2dFilter
 {
 private:
   double m_y1;  //!< @brief filter coefficient calculated with cutoff frequency and sampling time
@@ -149,10 +106,10 @@ namespace MoveAverageFilter
  * @param [in] num index distance for moving average filter
  * @param [out] u object vector
  */
-TRAJECTORY_FOLLOWER_PUBLIC bool filt_vector(const int num, std::vector<double> & u);
+bool filt_vector(const int num, std::vector<double> & u);
 }  // namespace MoveAverageFilter
-}  // namespace trajectory_follower
+}  // namespace mpc_lateral_controller
 }  // namespace control
 }  // namespace motion
 }  // namespace autoware
-#endif  // TRAJECTORY_FOLLOWER__LOWPASS_FILTER_HPP_
+#endif  // MPC_LATERAL_CONTROLLER__LOWPASS_FILTER_HPP_
diff --git a/...lower/include/trajectory_follower/mpc.hpp → ...er/include/mpc_lateral_controller/mpc.hpp b/...lower/include/trajectory_follower/mpc.hpp → ...er/include/mpc_lateral_controller/mpc.hpp
@@ -12,26 +12,25 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef TRAJECTORY_FOLLOWER__MPC_HPP_
-#define TRAJECTORY_FOLLOWER__MPC_HPP_
+#ifndef MPC_LATERAL_CONTROLLER__MPC_HPP_
+#define MPC_LATERAL_CONTROLLER__MPC_HPP_
 
 #include "geometry/common_2d.hpp"
 #include "helper_functions/angle_utils.hpp"
+#include "mpc_lateral_controller/interpolate.hpp"
+#include "mpc_lateral_controller/lowpass_filter.hpp"
+#include "mpc_lateral_controller/mpc_trajectory.hpp"
+#include "mpc_lateral_controller/mpc_utils.hpp"
+#include "mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp"
+#include "mpc_lateral_controller/qp_solver/qp_solver_unconstr_fast.hpp"
+#include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp"
+#include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp"
+#include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp"
 #include "osqp_interface/osqp_interface.hpp"
 #include "rclcpp/rclcpp.hpp"
 #include "rclcpp_components/register_node_macro.hpp"
 #include "tf2_ros/buffer.h"
 #include "tf2_ros/transform_listener.h"
-#include "trajectory_follower/interpolate.hpp"
-#include "trajectory_follower/lowpass_filter.hpp"
-#include "trajectory_follower/mpc_trajectory.hpp"
-#include "trajectory_follower/mpc_utils.hpp"
-#include "trajectory_follower/qp_solver/qp_solver_osqp.hpp"
-#include "trajectory_follower/qp_solver/qp_solver_unconstr_fast.hpp"
-#include "trajectory_follower/vehicle_model/vehicle_model_bicycle_dynamics.hpp"
-#include "trajectory_follower/vehicle_model/vehicle_model_bicycle_kinematics.hpp"
-#include "trajectory_follower/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp"
-#include "trajectory_follower/visibility_control.hpp"
 
 #include "autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp"
 #include "autoware_auto_planning_msgs/msg/trajectory.hpp"
@@ -50,7 +49,7 @@ namespace motion
 {
 namespace control
 {
-namespace trajectory_follower
+namespace mpc_lateral_controller
 {
 
 struct MPCParam
@@ -143,7 +142,7 @@ struct MPCMatrix
  * MPC-based waypoints follower class
  * @brief calculate control command to follow reference waypoints
  */
-class TRAJECTORY_FOLLOWER_PUBLIC MPC
+class MPC
 {
 private:
   //!< @brief ROS logger used for debug logging
@@ -154,15 +153,15 @@ class TRAJECTORY_FOLLOWER_PUBLIC MPC
   //!< @brief vehicle model type for MPC
   std::string m_vehicle_model_type;
   //!< @brief vehicle model for MPC
-  std::shared_ptr<trajectory_follower::VehicleModelInterface> m_vehicle_model_ptr;
+  std::shared_ptr<mpc_lateral_controller::VehicleModelInterface> m_vehicle_model_ptr;
   //!< @brief qp solver for MPC
-  std::shared_ptr<trajectory_follower::QPSolverInterface> m_qpsolver_ptr;
+  std::shared_ptr<mpc_lateral_controller::QPSolverInterface> m_qpsolver_ptr;
   //!< @brief lowpass filter for steering command
-  trajectory_follower::Butterworth2dFilter m_lpf_steering_cmd;
+  mpc_lateral_controller::Butterworth2dFilter m_lpf_steering_cmd;
   //!< @brief lowpass filter for lateral error
-  trajectory_follower::Butterworth2dFilter m_lpf_lateral_error;
+  mpc_lateral_controller::Butterworth2dFilter m_lpf_lateral_error;
   //!< @brief lowpass filter for heading error
-  trajectory_follower::Butterworth2dFilter m_lpf_yaw_error;
+  mpc_lateral_controller::Butterworth2dFilter m_lpf_yaw_error;
 
   //!< @brief raw output computed two iterations ago
   double m_raw_steer_cmd_pprev = 0.0;
@@ -185,7 +184,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC MPC
    * @brief get variables for mpc calculation
    */
   bool getData(
-    const trajectory_follower::MPCTrajectory & traj,
+    const mpc_lateral_controller::MPCTrajectory & traj,
     const autoware_auto_vehicle_msgs::msg::SteeringReport & current_steer,
     const geometry_msgs::msg::Pose & current_pose, MPCData * data);
   /**
@@ -217,14 +216,14 @@ class TRAJECTORY_FOLLOWER_PUBLIC MPC
    * @param [out] x updated state at delayed_time
    */
   bool updateStateForDelayCompensation(
-    const trajectory_follower::MPCTrajectory & traj, const double & start_time,
+    const mpc_lateral_controller::MPCTrajectory & traj, const double & start_time,
     Eigen::VectorXd * x);
   /**
    * @brief generate MPC matrix with trajectory and vehicle model
    * @param [in] reference_trajectory used for linearization around reference trajectory
    */
   MPCMatrix generateMPCMatrix(
-    const trajectory_follower::MPCTrajectory & reference_trajectory, const double prediction_dt);
+    const mpc_lateral_controller::MPCTrajectory & reference_trajectory, const double prediction_dt);
   /**
    * @brief generate MPC matrix with trajectory and vehicle model
    * @param [in] mpc_matrix parameters matrix to use for optimization
@@ -240,20 +239,20 @@ class TRAJECTORY_FOLLOWER_PUBLIC MPC
    */
   bool resampleMPCTrajectoryByTime(
     const double start_time, const double prediction_dt,
-    const trajectory_follower::MPCTrajectory & input,
-    trajectory_follower::MPCTrajectory * output) const;
+    const mpc_lateral_controller::MPCTrajectory & input,
+    mpc_lateral_controller::MPCTrajectory * output) const;
   /**
    * @brief apply velocity dynamics filter with v0 from closest index
    */
-  trajectory_follower::MPCTrajectory applyVelocityDynamicsFilter(
-    const trajectory_follower::MPCTrajectory & trajectory,
+  mpc_lateral_controller::MPCTrajectory applyVelocityDynamicsFilter(
+    const mpc_lateral_controller::MPCTrajectory & trajectory,
     const geometry_msgs::msg::Pose & current_pose, const double v0) const;
   /**
    * @brief get prediction delta time of mpc.
    * If trajectory length is shorter than min_prediction length, adjust delta time.
    */
   double getPredictionDeltaTime(
-    const double start_time, const trajectory_follower::MPCTrajectory & input,
+    const double start_time, const mpc_lateral_controller::MPCTrajectory & input,
     const geometry_msgs::msg::Pose & current_pose) const;
   /**
    * @brief add weights related to lateral_jerk, steering_rate, steering_acc into R
@@ -349,7 +348,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC MPC
 
 public:
   //!< @brief reference trajectory to be followed
-  trajectory_follower::MPCTrajectory m_ref_traj;
+  mpc_lateral_controller::MPCTrajectory m_ref_traj;
   //!< @brief MPC design parameter
   MPCParam m_param;
   //!< @brief mpc_output buffer for delay time compensation
@@ -405,7 +404,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC MPC
    * @brief set the vehicle model of this MPC
    */
   inline void setVehicleModel(
-    std::shared_ptr<trajectory_follower::VehicleModelInterface> vehicle_model_ptr,
+    std::shared_ptr<mpc_lateral_controller::VehicleModelInterface> vehicle_model_ptr,
     const std::string & vehicle_model_type)
   {
     m_vehicle_model_ptr = vehicle_model_ptr;
@@ -414,7 +413,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC MPC
   /**
    * @brief set the QP solver of this MPC
    */
-  inline void setQPSolver(std::shared_ptr<trajectory_follower::QPSolverInterface> qpsolver_ptr)
+  inline void setQPSolver(std::shared_ptr<mpc_lateral_controller::QPSolverInterface> qpsolver_ptr)
   {
     m_qpsolver_ptr = qpsolver_ptr;
   }
@@ -445,9 +444,9 @@ class TRAJECTORY_FOLLOWER_PUBLIC MPC
    */
   inline void setClock(rclcpp::Clock::SharedPtr clock) { m_clock = clock; }
 };  // class MPC
-}  // namespace trajectory_follower
+}  // namespace mpc_lateral_controller
 }  // namespace control
 }  // namespace motion
 }  // namespace autoware
 
-#endif  // TRAJECTORY_FOLLOWER__MPC_HPP_
+#endif  // MPC_LATERAL_CONTROLLER__MPC_HPP_