refactor(mpc_lateral_controller, trajectory_follower_node)!: prefix package and namespace with autoware

control/mpc_lateral_controller/CMakeLists.txt → control/autoware_mpc_lateral_controller/CMakeLists.txt

@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.14)
-project(mpc_lateral_controller)
+project(autoware_mpc_lateral_controller)
 
 find_package(autoware_cmake REQUIRED)
 autoware_package()

control/mpc_lateral_controller/include/mpc_lateral_controller/lowpass_filter.hpp → control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/lowpass_filter.hpp

@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef MPC_LATERAL_CONTROLLER__LOWPASS_FILTER_HPP_
-#define MPC_LATERAL_CONTROLLER__LOWPASS_FILTER_HPP_
+#ifndef AUTOWARE_MPC_LATERAL_CONTROLLER__LOWPASS_FILTER_HPP_
+#define AUTOWARE_MPC_LATERAL_CONTROLLER__LOWPASS_FILTER_HPP_
 
 #include <algorithm>
 #include <cmath>
@@ -102,4 +102,4 @@ namespace MoveAverageFilter
 bool filt_vector(const int num, std::vector<double> & u);
 }  // namespace MoveAverageFilter
 }  // namespace autoware::motion::control::mpc_lateral_controller
-#endif  // MPC_LATERAL_CONTROLLER__LOWPASS_FILTER_HPP_
+#endif  // AUTOWARE_MPC_LATERAL_CONTROLLER__LOWPASS_FILTER_HPP_

control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp → control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/mpc.hpp

@@ -12,14 +12,14 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef MPC_LATERAL_CONTROLLER__MPC_HPP_
-#define MPC_LATERAL_CONTROLLER__MPC_HPP_
-
-#include "mpc_lateral_controller/lowpass_filter.hpp"
-#include "mpc_lateral_controller/mpc_trajectory.hpp"
-#include "mpc_lateral_controller/qp_solver/qp_solver_interface.hpp"
-#include "mpc_lateral_controller/steering_predictor.hpp"
-#include "mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp"
+#ifndef AUTOWARE_MPC_LATERAL_CONTROLLER__MPC_HPP_
+#define AUTOWARE_MPC_LATERAL_CONTROLLER__MPC_HPP_
+
+#include "autoware_mpc_lateral_controller/lowpass_filter.hpp"
+#include "autoware_mpc_lateral_controller/mpc_trajectory.hpp"
+#include "autoware_mpc_lateral_controller/qp_solver/qp_solver_interface.hpp"
+#include "autoware_mpc_lateral_controller/steering_predictor.hpp"
+#include "autoware_mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp"
 #include "rclcpp/rclcpp.hpp"
 
 #include "autoware_control_msgs/msg/lateral.hpp"
@@ -525,4 +525,4 @@ class MPC
 };  // class MPC
 }  // namespace autoware::motion::control::mpc_lateral_controller
 
-#endif  // MPC_LATERAL_CONTROLLER__MPC_HPP_
+#endif  // AUTOWARE_MPC_LATERAL_CONTROLLER__MPC_HPP_

control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_lateral_controller.hpp → control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/mpc_lateral_controller.hpp

@@ -12,13 +12,13 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef MPC_LATERAL_CONTROLLER__MPC_LATERAL_CONTROLLER_HPP_
-#define MPC_LATERAL_CONTROLLER__MPC_LATERAL_CONTROLLER_HPP_
+#ifndef AUTOWARE_MPC_LATERAL_CONTROLLER__MPC_LATERAL_CONTROLLER_HPP_
+#define AUTOWARE_MPC_LATERAL_CONTROLLER__MPC_LATERAL_CONTROLLER_HPP_
 
-#include "mpc_lateral_controller/mpc.hpp"
-#include "mpc_lateral_controller/mpc_trajectory.hpp"
-#include "mpc_lateral_controller/mpc_utils.hpp"
-#include "mpc_lateral_controller/steering_offset/steering_offset.hpp"
+#include "autoware_mpc_lateral_controller/mpc.hpp"
+#include "autoware_mpc_lateral_controller/mpc_trajectory.hpp"
+#include "autoware_mpc_lateral_controller/mpc_utils.hpp"
+#include "autoware_mpc_lateral_controller/steering_offset/steering_offset.hpp"
 #include "rclcpp/rclcpp.hpp"
 #include "trajectory_follower_base/lateral_controller_base.hpp"
 
@@ -281,4 +281,4 @@ class MpcLateralController : public trajectory_follower::LateralControllerBase
 };
 }  // namespace autoware::motion::control::mpc_lateral_controller
 
-#endif  // MPC_LATERAL_CONTROLLER__MPC_LATERAL_CONTROLLER_HPP_
+#endif  // AUTOWARE_MPC_LATERAL_CONTROLLER__MPC_LATERAL_CONTROLLER_HPP_

control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_trajectory.hpp → control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/mpc_trajectory.hpp

@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef MPC_LATERAL_CONTROLLER__MPC_TRAJECTORY_HPP_
-#define MPC_LATERAL_CONTROLLER__MPC_TRAJECTORY_HPP_
+#ifndef AUTOWARE_MPC_LATERAL_CONTROLLER__MPC_TRAJECTORY_HPP_
+#define AUTOWARE_MPC_LATERAL_CONTROLLER__MPC_TRAJECTORY_HPP_
 
 #include "tier4_autoware_utils/geometry/geometry.hpp"
 
@@ -125,4 +125,4 @@ class MPCTrajectory
   }
 };
 }  // namespace autoware::motion::control::mpc_lateral_controller
-#endif  // MPC_LATERAL_CONTROLLER__MPC_TRAJECTORY_HPP_
+#endif  // AUTOWARE_MPC_LATERAL_CONTROLLER__MPC_TRAJECTORY_HPP_

control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_utils.hpp → control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/mpc_utils.hpp

@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef MPC_LATERAL_CONTROLLER__MPC_UTILS_HPP_
-#define MPC_LATERAL_CONTROLLER__MPC_UTILS_HPP_
+#ifndef AUTOWARE_MPC_LATERAL_CONTROLLER__MPC_UTILS_HPP_
+#define AUTOWARE_MPC_LATERAL_CONTROLLER__MPC_UTILS_HPP_
 
 #include "rclcpp/rclcpp.hpp"
 #include "tf2/utils.h"
@@ -26,7 +26,7 @@
 #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
 #endif
 
-#include "mpc_lateral_controller/mpc_trajectory.hpp"
+#include "autoware_mpc_lateral_controller/mpc_trajectory.hpp"
 
 #include "autoware_planning_msgs/msg/trajectory.hpp"
 #include "autoware_planning_msgs/msg/trajectory_point.hpp"
@@ -230,4 +230,4 @@ void update_param(
 
 }  // namespace MPCUtils
 }  // namespace autoware::motion::control::mpc_lateral_controller
-#endif  // MPC_LATERAL_CONTROLLER__MPC_UTILS_HPP_
+#endif  // AUTOWARE_MPC_LATERAL_CONTROLLER__MPC_UTILS_HPP_

control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_interface.hpp → control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/qp_solver/qp_solver_interface.hpp

@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_INTERFACE_HPP_
-#define MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_INTERFACE_HPP_
+#ifndef AUTOWARE_MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_INTERFACE_HPP_
+#define AUTOWARE_MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_INTERFACE_HPP_
 
 #include <Eigen/Core>
 
@@ -51,4 +51,4 @@ class QPSolverInterface
   virtual double getObjVal() const { return 0.0; }
 };
 }  // namespace autoware::motion::control::mpc_lateral_controller
-#endif  // MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_INTERFACE_HPP_
+#endif  // AUTOWARE_MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_INTERFACE_HPP_

control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp → control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp

@@ -12,10 +12,10 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_OSQP_HPP_
-#define MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_OSQP_HPP_
+#ifndef AUTOWARE_MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_OSQP_HPP_
+#define AUTOWARE_MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_OSQP_HPP_
 
-#include "mpc_lateral_controller/qp_solver/qp_solver_interface.hpp"
+#include "autoware_mpc_lateral_controller/qp_solver/qp_solver_interface.hpp"
 #include "osqp_interface/osqp_interface.hpp"
 #include "rclcpp/rclcpp.hpp"
 
@@ -62,4 +62,4 @@ class QPSolverOSQP : public QPSolverInterface
   rclcpp::Logger logger_;
 };
 }  // namespace autoware::motion::control::mpc_lateral_controller
-#endif  // MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_OSQP_HPP_
+#endif  // AUTOWARE_MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_OSQP_HPP_

control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_unconstraint_fast.hpp → control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/qp_solver/qp_solver_unconstraint_fast.hpp

@@ -12,10 +12,10 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_UNCONSTRAINT_FAST_HPP_
-#define MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_UNCONSTRAINT_FAST_HPP_
+#ifndef AUTOWARE_MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_UNCONSTRAINT_FAST_HPP_
+#define AUTOWARE_MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_UNCONSTRAINT_FAST_HPP_
 
-#include "mpc_lateral_controller/qp_solver/qp_solver_interface.hpp"
+#include "autoware_mpc_lateral_controller/qp_solver/qp_solver_interface.hpp"
 
 namespace autoware::motion::control::mpc_lateral_controller
 {
@@ -62,4 +62,4 @@ class QPSolverEigenLeastSquareLLT : public QPSolverInterface
 private:
 };
 }  // namespace autoware::motion::control::mpc_lateral_controller
-#endif  // MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_UNCONSTRAINT_FAST_HPP_
+#endif  // AUTOWARE_MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_UNCONSTRAINT_FAST_HPP_

control/mpc_lateral_controller/include/mpc_lateral_controller/steering_offset/steering_offset.hpp → control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/steering_offset/steering_offset.hpp

@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef MPC_LATERAL_CONTROLLER__STEERING_OFFSET__STEERING_OFFSET_HPP_
-#define MPC_LATERAL_CONTROLLER__STEERING_OFFSET__STEERING_OFFSET_HPP_
+#ifndef AUTOWARE_MPC_LATERAL_CONTROLLER__STEERING_OFFSET__STEERING_OFFSET_HPP_
+#define AUTOWARE_MPC_LATERAL_CONTROLLER__STEERING_OFFSET__STEERING_OFFSET_HPP_
 
 #include <geometry_msgs/msg/twist.hpp>
 
@@ -45,4 +45,4 @@ class SteeringOffsetEstimator
   double steering_offset_ = 0.0;
 };
 
-#endif  // MPC_LATERAL_CONTROLLER__STEERING_OFFSET__STEERING_OFFSET_HPP_
+#endif  // AUTOWARE_MPC_LATERAL_CONTROLLER__STEERING_OFFSET__STEERING_OFFSET_HPP_

control/mpc_lateral_controller/include/mpc_lateral_controller/steering_predictor.hpp → control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/steering_predictor.hpp

@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef MPC_LATERAL_CONTROLLER__STEERING_PREDICTOR_HPP_
-#define MPC_LATERAL_CONTROLLER__STEERING_PREDICTOR_HPP_
+#ifndef AUTOWARE_MPC_LATERAL_CONTROLLER__STEERING_PREDICTOR_HPP_
+#define AUTOWARE_MPC_LATERAL_CONTROLLER__STEERING_PREDICTOR_HPP_
 
 #include "rclcpp/rclcpp.hpp"
 
@@ -81,4 +81,4 @@ class SteeringPredictor
 
 }  // namespace autoware::motion::control::mpc_lateral_controller
 
-#endif  // MPC_LATERAL_CONTROLLER__STEERING_PREDICTOR_HPP_
+#endif  // AUTOWARE_MPC_LATERAL_CONTROLLER__STEERING_PREDICTOR_HPP_

control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp → control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp

@@ -44,10 +44,10 @@
  * Tracking", Robotics Institute, Carnegie Mellon University, February 2009.
  */
 
-#ifndef MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_DYNAMICS_HPP_
-#define MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_DYNAMICS_HPP_
+#ifndef AUTOWARE_MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_DYNAMICS_HPP_
+#define AUTOWARE_MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_DYNAMICS_HPP_
 
-#include "mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp"
+#include "autoware_mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp"
 
 #include <Eigen/Core>
 #include <Eigen/LU>
@@ -122,4 +122,4 @@ class DynamicsBicycleModel : public VehicleModelInterface
   double m_cr;    //!< @brief rear cornering power [N/rad]
 };
 }  // namespace autoware::motion::control::mpc_lateral_controller
-#endif  // MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_DYNAMICS_HPP_
+#endif  // AUTOWARE_MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_DYNAMICS_HPP_

control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp → control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp

@@ -38,10 +38,10 @@
  *
  */
 
-#ifndef MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_HPP_
-#define MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_HPP_
+#ifndef AUTOWARE_MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_HPP_
+#define AUTOWARE_MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_HPP_
 
-#include "mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp"
+#include "autoware_mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp"
 
 #include <Eigen/Core>
 #include <Eigen/LU>
@@ -106,4 +106,4 @@ class KinematicsBicycleModel : public VehicleModelInterface
   double m_steer_tau;  //!< @brief steering time constant for 1d-model [s]
 };
 }  // namespace autoware::motion::control::mpc_lateral_controller
-#endif  // MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_HPP_
+#endif  // AUTOWARE_MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_HPP_

control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp → control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp

@@ -35,10 +35,10 @@
  *
  */
 
-#ifndef MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_NO_DELAY_HPP_
-#define MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_NO_DELAY_HPP_
+#ifndef AUTOWARE_MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_NO_DELAY_HPP_  // NOLINT
+#define AUTOWARE_MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_NO_DELAY_HPP_  // NOLINT
 
-#include "mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp"
+#include "autoware_mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp"
 
 #include <Eigen/Core>
 #include <Eigen/LU>
@@ -101,4 +101,6 @@ class KinematicsBicycleModelNoDelay : public VehicleModelInterface
   double m_steer_lim;  //!< @brief steering angle limit [rad]
 };
 }  // namespace autoware::motion::control::mpc_lateral_controller
-#endif  // MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_NO_DELAY_HPP_
+// clang-format off
+#endif  // AUTOWARE_MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_NO_DELAY_HPP_  // NOLINT
+// clang-format on

control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp → control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp

@@ -12,10 +12,10 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_
-#define MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_
+#ifndef AUTOWARE_MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_
+#define AUTOWARE_MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_
 
-#include "mpc_lateral_controller/mpc_trajectory.hpp"
+#include "autoware_mpc_lateral_controller/mpc_trajectory.hpp"
 
 #include <Eigen/Core>
 
@@ -149,4 +149,4 @@ class VehicleModelInterface
     const MPCTrajectory & reference_trajectory, const double dt) const = 0;
 };
 }  // namespace autoware::motion::control::mpc_lateral_controller
-#endif  // MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_
+#endif  // AUTOWARE_MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_

control/mpc_lateral_controller/package.xml → control/autoware_mpc_lateral_controller/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
-  <name>mpc_lateral_controller</name>
+  <name>autoware_mpc_lateral_controller</name>
   <version>1.0.0</version>
   <description>MPC-based lateral controller</description>
 

control/mpc_lateral_controller/src/lowpass_filter.cpp → control/autoware_mpc_lateral_controller/src/lowpass_filter.cpp

@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "mpc_lateral_controller/lowpass_filter.hpp"
+#include "autoware_mpc_lateral_controller/lowpass_filter.hpp"
 
 #include <vector>
 

control/mpc_lateral_controller/src/mpc.cpp → control/autoware_mpc_lateral_controller/src/mpc.cpp

@@ -1,4 +1,4 @@
 // Copyright 2018-2021 The Autoware Foundation
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -12,11 +12,11 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "mpc_lateral_controller/mpc.hpp"
+#include "autoware_mpc_lateral_controller/mpc.hpp"
 
+#include "autoware_mpc_lateral_controller/mpc_utils.hpp"
 #include "interpolation/linear_interpolation.hpp"
 #include "motion_utils/trajectory/trajectory.hpp"
-#include "mpc_lateral_controller/mpc_utils.hpp"
 #include "rclcpp/rclcpp.hpp"
 #include "tier4_autoware_utils/math/unit_conversion.hpp"
 
@@ -35,84 +35,84 @@
     "~/debug/predicted_trajectory_in_frenet_coordinate", rclcpp::QoS(1));
 }
 
 bool MPC::calculateMPC(
   const SteeringReport & current_steer, const Odometry & current_kinematics, Lateral & ctrl_cmd,
   Trajectory & predicted_trajectory, Float32MultiArrayStamped & diagnostic)
 {
   // since the reference trajectory does not take into account the current velocity of the ego
   // vehicle, it needs to calculate the trajectory velocity considering the longitudinal dynamics.
   const auto reference_trajectory =
     applyVelocityDynamicsFilter(m_reference_trajectory, current_kinematics);
 
   // get the necessary data
   const auto [success_data, mpc_data] =
     getData(reference_trajectory, current_steer, current_kinematics);
   if (!success_data) {
     return fail_warn_throttle("fail to get MPC Data. Stop MPC.");
   }
 
   // calculate initial state of the error dynamics
   const auto x0 = getInitialState(mpc_data);
 
   // apply time delay compensation to the initial state
   const auto [success_delay, x0_delayed] =
     updateStateForDelayCompensation(reference_trajectory, mpc_data.nearest_time, x0);
   if (!success_delay) {
     return fail_warn_throttle("delay compensation failed. Stop MPC.");
   }
 
   // resample reference trajectory with mpc sampling time
   const double mpc_start_time = mpc_data.nearest_time + m_param.input_delay;
   const double prediction_dt =
     getPredictionDeltaTime(mpc_start_time, reference_trajectory, current_kinematics);
 
   const auto [success_resample, mpc_resampled_ref_trajectory] =
     resampleMPCTrajectoryByTime(mpc_start_time, prediction_dt, reference_trajectory);
   if (!success_resample) {
     return fail_warn_throttle("trajectory resampling failed. Stop MPC.");
   }
 
   // generate mpc matrix : predict equation Xec = Aex * x0 + Bex * Uex + Wex
   const auto mpc_matrix = generateMPCMatrix(mpc_resampled_ref_trajectory, prediction_dt);
 
   // solve Optimization problem
   const auto [success_opt, Uex] = executeOptimization(
     mpc_matrix, x0_delayed, prediction_dt, mpc_resampled_ref_trajectory,
     current_kinematics.twist.twist.linear.x);
   if (!success_opt) {
     return fail_warn_throttle("optimization failed. Stop MPC.");
   }
 
   // apply filters for the input limitation and low pass filter
   const double u_saturated = std::clamp(Uex(0), -m_steer_lim, m_steer_lim);
   const double u_filtered = m_lpf_steering_cmd.filter(u_saturated);
 
   // set control command
   ctrl_cmd.steering_tire_angle = static_cast<float>(u_filtered);
   ctrl_cmd.steering_tire_rotation_rate = static_cast<float>(calcDesiredSteeringRate(
     mpc_matrix, x0_delayed, Uex, u_filtered, current_steer.steering_tire_angle, prediction_dt));
 
   // save the control command for the steering prediction
   m_steering_predictor->storeSteerCmd(u_filtered);
 
   // save input to buffer for delay compensation
   m_input_buffer.push_back(ctrl_cmd.steering_tire_angle);
   m_input_buffer.pop_front();
 
   // save previous input for the mpc rate limit
   m_raw_steer_cmd_pprev = m_raw_steer_cmd_prev;
   m_raw_steer_cmd_prev = Uex(0);
 
   /* calculate predicted trajectory */
   predicted_trajectory =
     calculatePredictedTrajectory(mpc_matrix, x0, Uex, mpc_resampled_ref_trajectory, prediction_dt);
 
   // prepare diagnostic message
   diagnostic =
     generateDiagData(reference_trajectory, mpc_data, mpc_matrix, ctrl_cmd, Uex, current_kinematics);
 
   return true;
 }
 
 Float32MultiArrayStamped MPC::generateDiagData(
   const MPCTrajectory & reference_trajectory, const MPCData & mpc_data,
@@ -162,42 +162,42 @@
   return diagnostic;
 }
 
 void MPC::setReferenceTrajectory(
   const Trajectory & trajectory_msg, const TrajectoryFilteringParam & param,
   const Odometry & current_kinematics)
 {
   const size_t nearest_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints(
     trajectory_msg.points, current_kinematics.pose.pose, ego_nearest_dist_threshold,
     ego_nearest_yaw_threshold);
   const double ego_offset_to_segment = motion_utils::calcLongitudinalOffsetToSegment(
     trajectory_msg.points, nearest_seg_idx, current_kinematics.pose.pose.position);
 
   const auto mpc_traj_raw = MPCUtils::convertToMPCTrajectory(trajectory_msg);
 
   // resampling
   const auto [success_resample, mpc_traj_resampled] = MPCUtils::resampleMPCTrajectoryByDistance(
     mpc_traj_raw, param.traj_resample_dist, nearest_seg_idx, ego_offset_to_segment);
   if (!success_resample) {
     warn_throttle("[setReferenceTrajectory] spline error when resampling by distance");
     return;
   }
 
   const auto is_forward_shift =
     motion_utils::isDrivingForward(mpc_traj_resampled.toTrajectoryPoints());
 
   // if driving direction is unknown, use previous value
   m_is_forward_shift = is_forward_shift ? is_forward_shift.value() : m_is_forward_shift;
 
   // path smoothing
   MPCTrajectory mpc_traj_smoothed = mpc_traj_resampled;  // smooth filtered trajectory
   const int mpc_traj_resampled_size = static_cast<int>(mpc_traj_resampled.size());
   if (
     param.enable_path_smoothing && mpc_traj_resampled_size > 2 * param.path_filter_moving_ave_num) {
     using MoveAverageFilter::filt_vector;
     if (
       !filt_vector(param.path_filter_moving_ave_num, mpc_traj_smoothed.x) ||
       !filt_vector(param.path_filter_moving_ave_num, mpc_traj_smoothed.y) ||
       !filt_vector(param.path_filter_moving_ave_num, mpc_traj_smoothed.yaw) ||
       !filt_vector(param.path_filter_moving_ave_num, mpc_traj_smoothed.vx)) {
       RCLCPP_DEBUG(m_logger, "path callback: filtering error. stop filtering.");
       mpc_traj_smoothed = mpc_traj_resampled;
@@ -410,114 +410,114 @@
  * cost function: J = Xex' * Qex * Xex + (Uex - Uref)' * R1ex * (Uex - Uref_ex) + Uex' * R2ex * Uex
  * Qex = diag([Q,Q,...]), R1ex = diag([R,R,...])
  */
 MPCMatrix MPC::generateMPCMatrix(
   const MPCTrajectory & reference_trajectory, const double prediction_dt)
 {
   const int N = m_param.prediction_horizon;
   const double DT = prediction_dt;
   const int DIM_X = m_vehicle_model_ptr->getDimX();
   const int DIM_U = m_vehicle_model_ptr->getDimU();
   const int DIM_Y = m_vehicle_model_ptr->getDimY();
 
   MPCMatrix m;
   m.Aex = MatrixXd::Zero(DIM_X * N, DIM_X);
   m.Bex = MatrixXd::Zero(DIM_X * N, DIM_U * N);
   m.Wex = MatrixXd::Zero(DIM_X * N, 1);
   m.Cex = MatrixXd::Zero(DIM_Y * N, DIM_X * N);
   m.Qex = MatrixXd::Zero(DIM_Y * N, DIM_Y * N);
   m.R1ex = MatrixXd::Zero(DIM_U * N, DIM_U * N);
   m.R2ex = MatrixXd::Zero(DIM_U * N, DIM_U * N);
   m.Uref_ex = MatrixXd::Zero(DIM_U * N, 1);
 
   // weight matrix depends on the vehicle model
   MatrixXd Q = MatrixXd::Zero(DIM_Y, DIM_Y);
   MatrixXd R = MatrixXd::Zero(DIM_U, DIM_U);
   MatrixXd Q_adaptive = MatrixXd::Zero(DIM_Y, DIM_Y);
   MatrixXd R_adaptive = MatrixXd::Zero(DIM_U, DIM_U);
 
   MatrixXd Ad(DIM_X, DIM_X);
   MatrixXd Bd(DIM_X, DIM_U);
   MatrixXd Wd(DIM_X, 1);
   MatrixXd Cd(DIM_Y, DIM_X);
   MatrixXd Uref(DIM_U, 1);
 
   const double sign_vx = m_is_forward_shift ? 1 : -1;
 
   // predict dynamics for N times
   for (int i = 0; i < N; ++i) {
     const double ref_vx = reference_trajectory.vx.at(i);
     const double ref_vx_squared = ref_vx * ref_vx;
 
     // NOTE: When driving backward, the curvature's sign should be reversed.
     const double ref_k = reference_trajectory.k.at(i) * sign_vx;
     const double ref_smooth_k = reference_trajectory.smooth_k.at(i) * sign_vx;
 
     // get discrete state matrix A, B, C, W
     m_vehicle_model_ptr->setVelocity(ref_vx);
     m_vehicle_model_ptr->setCurvature(ref_k);
     m_vehicle_model_ptr->calculateDiscreteMatrix(Ad, Bd, Cd, Wd, DT);
 
     Q = MatrixXd::Zero(DIM_Y, DIM_Y);
     R = MatrixXd::Zero(DIM_U, DIM_U);
     const auto mpc_weight = getWeight(ref_k);
     Q(0, 0) = mpc_weight.lat_error;
     Q(1, 1) = mpc_weight.heading_error;
     R(0, 0) = mpc_weight.steering_input;
 
     Q_adaptive = Q;
     R_adaptive = R;
     if (i == N - 1) {
       Q_adaptive(0, 0) = m_param.nominal_weight.terminal_lat_error;
       Q_adaptive(1, 1) = m_param.nominal_weight.terminal_heading_error;
     }
     Q_adaptive(1, 1) += ref_vx_squared * mpc_weight.heading_error_squared_vel;
     R_adaptive(0, 0) += ref_vx_squared * mpc_weight.steering_input_squared_vel;
 
     // update mpc matrix
     int idx_x_i = i * DIM_X;
     int idx_x_i_prev = (i - 1) * DIM_X;
     int idx_u_i = i * DIM_U;
     int idx_y_i = i * DIM_Y;
     if (i == 0) {
       m.Aex.block(0, 0, DIM_X, DIM_X) = Ad;
       m.Bex.block(0, 0, DIM_X, DIM_U) = Bd;
       m.Wex.block(0, 0, DIM_X, 1) = Wd;
     } else {
       m.Aex.block(idx_x_i, 0, DIM_X, DIM_X) = Ad * m.Aex.block(idx_x_i_prev, 0, DIM_X, DIM_X);
       for (int j = 0; j < i; ++j) {
         int idx_u_j = j * DIM_U;
         m.Bex.block(idx_x_i, idx_u_j, DIM_X, DIM_U) =
           Ad * m.Bex.block(idx_x_i_prev, idx_u_j, DIM_X, DIM_U);
       }
       m.Wex.block(idx_x_i, 0, DIM_X, 1) = Ad * m.Wex.block(idx_x_i_prev, 0, DIM_X, 1) + Wd;
     }
     m.Bex.block(idx_x_i, idx_u_i, DIM_X, DIM_U) = Bd;
     m.Cex.block(idx_y_i, idx_x_i, DIM_Y, DIM_X) = Cd;
     m.Qex.block(idx_y_i, idx_y_i, DIM_Y, DIM_Y) = Q_adaptive;
     m.R1ex.block(idx_u_i, idx_u_i, DIM_U, DIM_U) = R_adaptive;
 
     // get reference input (feed-forward)
     m_vehicle_model_ptr->setCurvature(ref_smooth_k);
     m_vehicle_model_ptr->calculateReferenceInput(Uref);
     if (std::fabs(Uref(0, 0)) < tier4_autoware_utils::deg2rad(m_param.zero_ff_steer_deg)) {
       Uref(0, 0) = 0.0;  // ignore curvature noise
     }
     m.Uref_ex.block(i * DIM_U, 0, DIM_U, 1) = Uref;
   }
 
   // add lateral jerk : weight for (v * {u(i) - u(i-1)} )^2
   for (int i = 0; i < N - 1; ++i) {
     const double ref_vx = reference_trajectory.vx.at(i);
     const double ref_k = reference_trajectory.k.at(i) * sign_vx;
     const double j = ref_vx * ref_vx * getWeight(ref_k).lat_jerk / (DT * DT);
     const Eigen::Matrix2d J = (Eigen::Matrix2d() << j, -j, -j, j).finished();
     m.R2ex.block(i, i, 2, 2) += J;
   }
 
   addSteerWeightR(prediction_dt, m.R1ex);
 
   return m;
 }
 
 /*
  * solve quadratic optimization.
@@ -541,66 +541,66 @@
  *                            ~~~
  * [    -au_lim * dt    ] < [uN-uN-1] < [     au_lim * dt    ] (*N... DIM_U)
  */
 std::pair<bool, VectorXd> MPC::executeOptimization(
   const MPCMatrix & m, const VectorXd & x0, const double prediction_dt, const MPCTrajectory & traj,
   const double current_velocity)
 {
   VectorXd Uex;
 
   if (!isValid(m)) {
     warn_throttle("model matrix is invalid. stop MPC.");
     return {false, {}};
   }
 
   const int DIM_U_N = m_param.prediction_horizon * m_vehicle_model_ptr->getDimU();
 
   // cost function: 1/2 * Uex' * H * Uex + f' * Uex,  H = B' * C' * Q * C * B + R
   const MatrixXd CB = m.Cex * m.Bex;
   const MatrixXd QCB = m.Qex * CB;
   // MatrixXd H = CB.transpose() * QCB + m.R1ex + m.R2ex; // This calculation is heavy. looking for
   // a good way.  //NOLINT
   MatrixXd H = MatrixXd::Zero(DIM_U_N, DIM_U_N);
   H.triangularView<Eigen::Upper>() = CB.transpose() * QCB;
   H.triangularView<Eigen::Upper>() += m.R1ex + m.R2ex;
   H.triangularView<Eigen::Lower>() = H.transpose();
   MatrixXd f = (m.Cex * (m.Aex * x0 + m.Wex)).transpose() * QCB - m.Uref_ex.transpose() * m.R1ex;
   addSteerWeightF(prediction_dt, f);
 
   MatrixXd A = MatrixXd::Identity(DIM_U_N, DIM_U_N);
   for (int i = 1; i < DIM_U_N; i++) {
     A(i, i - 1) = -1.0;
   }
 
   // steering angle limit
   VectorXd lb = VectorXd::Constant(DIM_U_N, -m_steer_lim);  // min steering angle
   VectorXd ub = VectorXd::Constant(DIM_U_N, m_steer_lim);   // max steering angle
 
   // steering angle rate limit
   VectorXd steer_rate_limits = calcSteerRateLimitOnTrajectory(traj, current_velocity);
   VectorXd ubA = steer_rate_limits * prediction_dt;
   VectorXd lbA = -steer_rate_limits * prediction_dt;
   ubA(0) = m_raw_steer_cmd_prev + steer_rate_limits(0) * m_ctrl_period;
   lbA(0) = m_raw_steer_cmd_prev - steer_rate_limits(0) * m_ctrl_period;
 
   auto t_start = std::chrono::system_clock::now();
   bool solve_result = m_qpsolver_ptr->solve(H, f.transpose(), A, lb, ub, lbA, ubA, Uex);
   auto t_end = std::chrono::system_clock::now();
   if (!solve_result) {
     warn_throttle("qp solver error");
     return {false, {}};
   }
 
   {
     auto t = std::chrono::duration_cast<std::chrono::milliseconds>(t_end - t_start).count();
     RCLCPP_DEBUG(m_logger, "qp solver calculation time = %ld [ms]", t);
   }
 
   if (Uex.array().isNaN().any()) {
     warn_throttle("model Uex includes NaN, stop MPC.");
     return {false, {}};
   }
   return {true, Uex};
 }
 
 void MPC::addSteerWeightR(const double prediction_dt, MatrixXd & R) const
 {
@@ -728,57 +728,57 @@
   return steer_rate;
 }
 
 VectorXd MPC::calcSteerRateLimitOnTrajectory(
   const MPCTrajectory & trajectory, const double current_velocity) const
 {
   const auto interp = [&](const auto & steer_rate_limit_map, const auto & current) {
     std::vector<double> reference, limits;
     for (const auto & p : steer_rate_limit_map) {
       reference.push_back(p.first);
       limits.push_back(p.second);
     }
 
     // If the speed is out of range of the reference, apply zero-order hold.
     if (current <= reference.front()) {
       return limits.front();
     }
     if (current >= reference.back()) {
       return limits.back();
     }
 
     // Apply linear interpolation
     for (size_t i = 0; i < reference.size() - 1; ++i) {
       if (reference.at(i) <= current && current <= reference.at(i + 1)) {
         auto ratio =
           (current - reference.at(i)) / std::max(reference.at(i + 1) - reference.at(i), 1.0e-5);
         ratio = std::clamp(ratio, 0.0, 1.0);
         const auto interp = limits.at(i) + ratio * (limits.at(i + 1) - limits.at(i));
         return interp;
       }
     }
 
     std::cerr << "MPC::calcSteerRateLimitOnTrajectory() interpolation logic is broken. Command "
                  "filter is not working. Please check the code."
               << std::endl;
     return reference.back();
   };
 
   // when the vehicle is stopped, no steering rate limit.
   const bool is_vehicle_stopped = std::fabs(current_velocity) < 0.01;
   if (is_vehicle_stopped) {
     return VectorXd::Zero(m_param.prediction_horizon);
   }
 
   // calculate steering rate limit
   VectorXd steer_rate_limits = VectorXd::Zero(m_param.prediction_horizon);
   for (int i = 0; i < m_param.prediction_horizon; ++i) {
     const auto limit_by_curvature = interp(m_steer_rate_lim_map_by_curvature, trajectory.k.at(i));
     const auto limit_by_velocity = interp(m_steer_rate_lim_map_by_velocity, trajectory.vx.at(i));
     steer_rate_limits(i) = std::min(limit_by_curvature, limit_by_velocity);
   }
 
   return steer_rate_limits;
 }
 
 Trajectory MPC::calculatePredictedTrajectory(
   const MPCMatrix & mpc_matrix, const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex,
@@ -809,19 +809,19 @@
   return predicted_trajectory;
 }
 
 bool MPC::isValid(const MPCMatrix & m) const
 {
   if (
     m.Aex.array().isNaN().any() || m.Bex.array().isNaN().any() || m.Cex.array().isNaN().any() ||
     m.Wex.array().isNaN().any() || m.Qex.array().isNaN().any() || m.R1ex.array().isNaN().any() ||
     m.R2ex.array().isNaN().any() || m.Uref_ex.array().isNaN().any()) {
     return false;
   }
 
   if (
     m.Aex.array().isInf().any() || m.Bex.array().isInf().any() || m.Cex.array().isInf().any() ||
     m.Wex.array().isInf().any() || m.Qex.array().isInf().any() || m.R1ex.array().isInf().any() ||
     m.R2ex.array().isInf().any() || m.Uref_ex.array().isInf().any()) {
     return false;
   }
 

control/mpc_lateral_controller/src/mpc_lateral_controller.cpp → control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp

@@ -12,14 +12,14 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "mpc_lateral_controller/mpc_lateral_controller.hpp"
+#include "autoware_mpc_lateral_controller/mpc_lateral_controller.hpp"
 
+#include "autoware_mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp"
+#include "autoware_mpc_lateral_controller/qp_solver/qp_solver_unconstraint_fast.hpp"
+#include "autoware_mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp"
+#include "autoware_mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp"
+#include "autoware_mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp"
 #include "motion_utils/trajectory/trajectory.hpp"
-#include "mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp"
-#include "mpc_lateral_controller/qp_solver/qp_solver_unconstraint_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 "tf2/utils.h"
 #include "tf2_ros/create_timer_ros.h"
 #include "vehicle_info_util/vehicle_info_util.hpp"
@@ -227,87 +227,87 @@
   return steering_offset_;
 }
 
 trajectory_follower::LateralOutput MpcLateralController::run(
   trajectory_follower::InputData const & input_data)
 {
   // set input data
   setTrajectory(input_data.current_trajectory, input_data.current_odometry);
 
   m_current_kinematic_state = input_data.current_odometry;
   m_current_steering = input_data.current_steering;
   if (enable_auto_steering_offset_removal_) {
     m_current_steering.steering_tire_angle -= steering_offset_->getOffset();
   }
 
   Lateral ctrl_cmd;
   Trajectory predicted_traj;
   Float32MultiArrayStamped debug_values;
 
   const bool is_under_control = input_data.current_operation_mode.is_autoware_control_enabled &&
                                 input_data.current_operation_mode.mode ==
                                   autoware_adapi_v1_msgs::msg::OperationModeState::AUTONOMOUS;
 
   if (!m_is_ctrl_cmd_prev_initialized || !is_under_control) {
     m_ctrl_cmd_prev = getInitialControlCommand();
     m_is_ctrl_cmd_prev_initialized = true;
   }
 
   const bool is_mpc_solved = m_mpc->calculateMPC(
     m_current_steering, m_current_kinematic_state, ctrl_cmd, predicted_traj, debug_values);
 
   // reset previous MPC result
   // Note: When a large deviation from the trajectory occurs, the optimization stops and
   // the vehicle will return to the path by re-planning the trajectory or external operation.
   // After the recovery, the previous value of the optimization may deviate greatly from
   // the actual steer angle, and it may make the optimization result unstable.
   if (!is_mpc_solved) {
     m_mpc->resetPrevResult(m_current_steering);
   } else {
     setSteeringToHistory(ctrl_cmd);
   }
 
   if (enable_auto_steering_offset_removal_) {
     steering_offset_->updateOffset(
       m_current_kinematic_state.twist.twist,
       input_data.current_steering.steering_tire_angle);  // use unbiased steering
     ctrl_cmd.steering_tire_angle += steering_offset_->getOffset();
   }
 
   publishPredictedTraj(predicted_traj);
   publishDebugValues(debug_values);
 
   const auto createLateralOutput = [this](const auto & cmd, const bool is_mpc_solved) {
     trajectory_follower::LateralOutput output;
     output.control_cmd = createCtrlCmdMsg(cmd);
     // To be sure current steering of the vehicle is desired steering angle, we need to check
     // following conditions.
     // 1. At the last loop, mpc should be solved because command should be optimized output.
     // 2. The mpc should be converged.
     // 3. The steer angle should be converged.
     output.sync_data.is_steer_converged =
       is_mpc_solved && isMpcConverged() && isSteerConverged(cmd);
 
     return output;
   };
 
   if (isStoppedState()) {
     // Reset input buffer
     for (auto & value : m_mpc->m_input_buffer) {
       value = m_ctrl_cmd_prev.steering_tire_angle;
     }
     // Use previous command value as previous raw steer command
     m_mpc->m_raw_steer_cmd_prev = m_ctrl_cmd_prev.steering_tire_angle;
     return createLateralOutput(m_ctrl_cmd_prev, false);
   }
 
   if (!is_mpc_solved) {
     warn_throttle("MPC is not solved. publish 0 velocity.");
     ctrl_cmd = getStopControlCommand();
   }
 
   m_ctrl_cmd_prev = ctrl_cmd;
   return createLateralOutput(ctrl_cmd, is_mpc_solved);
 }
 
 bool MpcLateralController::isSteerConverged(const Lateral & cmd) const
 {
@@ -625,16 +625,16 @@
   return false;
 }
 
 bool MpcLateralController::isValidTrajectory(const Trajectory & traj) const
 {
   for (const auto & p : traj.points) {
     if (
       !isfinite(p.pose.position.x) || !isfinite(p.pose.position.y) ||
       !isfinite(p.pose.orientation.w) || !isfinite(p.pose.orientation.x) ||
       !isfinite(p.pose.orientation.y) || !isfinite(p.pose.orientation.z) ||
       !isfinite(p.longitudinal_velocity_mps) || !isfinite(p.lateral_velocity_mps) ||
       !isfinite(p.lateral_velocity_mps) || !isfinite(p.heading_rate_rps) ||
       !isfinite(p.front_wheel_angle_rad) || !isfinite(p.rear_wheel_angle_rad)) {
       return false;
     }
   }

control/mpc_lateral_controller/src/mpc_trajectory.cpp → control/autoware_mpc_lateral_controller/src/mpc_trajectory.cpp

@@ -12,23 +12,23 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "mpc_lateral_controller/mpc_trajectory.hpp"
+#include "autoware_mpc_lateral_controller/mpc_trajectory.hpp"
 
 namespace autoware::motion::control::mpc_lateral_controller
 {
 void MPCTrajectory::push_back(
   const double & xp, const double & yp, const double & zp, const double & yawp, const double & vxp,
   const double & kp, const double & smooth_kp, const double & tp)
 {
   x.push_back(xp);
   y.push_back(yp);
   z.push_back(zp);
   yaw.push_back(yawp);
   vx.push_back(vxp);
   k.push_back(kp);
   smooth_k.push_back(smooth_kp);
   relative_time.push_back(tp);
 }
 
 void MPCTrajectory::push_back(const MPCTrajectoryPoint & p)
 {
@@ -88,9 +88,9 @@
 
 size_t MPCTrajectory::size() const
 {
   if (
     x.size() == y.size() && x.size() == z.size() && x.size() == yaw.size() &&
     x.size() == vx.size() && x.size() == k.size() && x.size() == smooth_k.size() &&
     x.size() == relative_time.size()) {
     return x.size();
   } else {

control/mpc_lateral_controller/src/mpc_utils.cpp → control/autoware_mpc_lateral_controller/src/mpc_utils.cpp

@@ -1,4 +1,4 @@
 // Copyright 2018-2021 The Autoware Foundation
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "mpc_lateral_controller/mpc_utils.hpp"
+#include "autoware_mpc_lateral_controller/mpc_utils.hpp"
 
 #include "interpolation/linear_interpolation.hpp"
 #include "interpolation/spline_interpolation.hpp"
@@ -307,34 +307,34 @@
   return true;
 }
 
 void dynamicSmoothingVelocity(
   const size_t start_seg_idx, const double start_vel, const double acc_lim, const double tau,
   MPCTrajectory & traj)
 {
   double curr_v = start_vel;
   // set current velocity in both start and end point of the segment
   traj.vx.at(start_seg_idx) = start_vel;
   if (1 < traj.vx.size()) {
     traj.vx.at(start_seg_idx + 1) = start_vel;
   }
 
   for (size_t i = start_seg_idx + 2; i < traj.size(); ++i) {
     const double ds = calcDistance2d(traj, i, i - 1);
     const double dt = ds / std::max(std::fabs(curr_v), std::numeric_limits<double>::epsilon());
     const double a = tau / std::max(tau + dt, std::numeric_limits<double>::epsilon());
     const double updated_v = a * curr_v + (1.0 - a) * traj.vx.at(i);
     const double dv = std::max(-acc_lim * dt, std::min(acc_lim * dt, updated_v - curr_v));
     curr_v = curr_v + dv;
     traj.vx.at(i) = curr_v;
   }
   calcMPCTrajectoryTime(traj);
 }
 
 bool calcNearestPoseInterp(
   const MPCTrajectory & traj, const Pose & self_pose, Pose * nearest_pose, size_t * nearest_index,
   double * nearest_time, const double max_dist, const double max_yaw)
 {
   if (traj.empty() || !nearest_pose || !nearest_index || !nearest_time) {
     return false;
   }
 
@@ -412,37 +412,37 @@
   return true;
 }
 
 double calcStopDistance(const Trajectory & current_trajectory, const int origin)
 {
   constexpr float zero_velocity = std::numeric_limits<float>::epsilon();
   const float origin_velocity =
     current_trajectory.points.at(static_cast<size_t>(origin)).longitudinal_velocity_mps;
   double stop_dist = 0.0;
 
   // search forward
   if (std::fabs(origin_velocity) > zero_velocity) {
     for (int i = origin + 1; i < static_cast<int>(current_trajectory.points.size()) - 1; ++i) {
       const auto & p0 = current_trajectory.points.at(i);
       const auto & p1 = current_trajectory.points.at(i - 1);
       stop_dist += calcDistance2d(p0, p1);
       if (std::fabs(p0.longitudinal_velocity_mps) < zero_velocity) {
         break;
       }
     }
     return stop_dist;
   }
 
   // search backward
   for (int i = origin - 1; 0 < i; --i) {
     const auto & p0 = current_trajectory.points.at(i);
     const auto & p1 = current_trajectory.points.at(i + 1);
     if (std::fabs(p0.longitudinal_velocity_mps) > zero_velocity) {
       break;
     }
     stop_dist -= calcDistance2d(p0, p1);
   }
   return stop_dist;
 }
 
 void extendTrajectoryInYawDirection(
   const double yaw, const double interval, const bool is_forward_shift, MPCTrajectory & traj)

control/mpc_lateral_controller/src/qp_solver/qp_solver_osqp.cpp → control/autoware_mpc_lateral_controller/src/qp_solver/qp_solver_osqp.cpp

@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp"
+#include "autoware_mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp"
 
 #include <string>
 #include <vector>
@@ -22,62 +22,62 @@
 QPSolverOSQP::QPSolverOSQP(const rclcpp::Logger & logger) : logger_{logger}
 {
 }
 bool QPSolverOSQP::solve(
   const Eigen::MatrixXd & h_mat, const Eigen::MatrixXd & f_vec, const Eigen::MatrixXd & a,
   const Eigen::VectorXd & lb, const Eigen::VectorXd & ub, const Eigen::VectorXd & lb_a,
   const Eigen::VectorXd & ub_a, Eigen::VectorXd & u)
 {
   const Eigen::Index raw_a = a.rows();
   const Eigen::Index col_a = a.cols();
   const Eigen::Index dim_u = ub.size();
   Eigen::MatrixXd Identity = Eigen::MatrixXd::Identity(dim_u, dim_u);
 
   // convert matrix to vector for osqpsolver
   std::vector<double> f(&f_vec(0), f_vec.data() + f_vec.cols() * f_vec.rows());
 
   std::vector<double> lower_bound;
   std::vector<double> upper_bound;
 
   for (int i = 0; i < dim_u; ++i) {
     lower_bound.push_back(lb(i));
     upper_bound.push_back(ub(i));
   }
 
   for (int i = 0; i < col_a; ++i) {
     lower_bound.push_back(lb_a(i));
     upper_bound.push_back(ub_a(i));
   }
 
   Eigen::MatrixXd osqpA = Eigen::MatrixXd(dim_u + col_a, raw_a);
   osqpA << Identity, a;
 
   /* execute optimization */
   auto result = osqpsolver_.optimize(h_mat, osqpA, f, lower_bound, upper_bound);
 
   std::vector<double> U_osqp = std::get<0>(result);
   u = Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, 1>>(
     &U_osqp[0], static_cast<Eigen::Index>(U_osqp.size()), 1);
 
   const int status_val = std::get<3>(result);
   if (status_val != 1) {
     RCLCPP_WARN(logger_, "optimization failed : %s", osqpsolver_.getStatusMessage().c_str());
     return false;
   }
   const auto has_nan =
     std::any_of(U_osqp.begin(), U_osqp.end(), [](const auto v) { return std::isnan(v); });
   if (has_nan) {
     RCLCPP_WARN(logger_, "optimization failed: result contains NaN values");
     return false;
   }
 
   // polish status: successful (1), unperformed (0), (-1) unsuccessful
   int status_polish = std::get<2>(result);
   if (status_polish == -1 || status_polish == 0) {
     const auto s = (status_polish == 0) ? "Polish process is not performed in osqp."
                                         : "Polish process failed in osqp.";
     RCLCPP_INFO(logger_, "%s The required accuracy is met, but the solution can be inaccurate.", s);
     return true;
   }
   return true;
 }
 }  // namespace autoware::motion::control::mpc_lateral_controller

control/mpc_lateral_controller/src/qp_solver/qp_solver_unconstraint_fast.cpp → control/autoware_mpc_lateral_controller/src/qp_solver/qp_solver_unconstraint_fast.cpp

@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "mpc_lateral_controller/qp_solver/qp_solver_unconstraint_fast.hpp"
+#include "autoware_mpc_lateral_controller/qp_solver/qp_solver_unconstraint_fast.hpp"
 
 #include <Eigen/Dense>
 
@@ -21,17 +21,17 @@
 QPSolverEigenLeastSquareLLT::QPSolverEigenLeastSquareLLT()
 {
 }
 bool QPSolverEigenLeastSquareLLT::solve(
   const Eigen::MatrixXd & h_mat, const Eigen::MatrixXd & f_vec, const Eigen::MatrixXd & /*a*/,
   const Eigen::VectorXd & /*lb*/, const Eigen::VectorXd & /*ub*/, const Eigen::VectorXd & /*lb_a*/,
   const Eigen::VectorXd & /*ub_a*/, Eigen::VectorXd & u)
 {
   if (std::fabs(h_mat.determinant()) < 1.0E-9) {
     return false;
   }
 
   u = -h_mat.llt().solve(f_vec);
 
   return true;
 }
 }  // namespace autoware::motion::control::mpc_lateral_controller

control/mpc_lateral_controller/src/steering_offset/steering_offset.cpp → control/autoware_mpc_lateral_controller/src/steering_offset/steering_offset.cpp

@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "mpc_lateral_controller/steering_offset/steering_offset.hpp"
+#include "autoware_mpc_lateral_controller/steering_offset/steering_offset.hpp"
 
 #include <algorithm>
 #include <iostream>