diff --git a/control/trajectory_follower/include/trajectory_follower/debug_values.hpp b/control/trajectory_follower/include/trajectory_follower/debug_values.hpp index ab6e7e04998e7..9008234edc79a 100644 --- a/control/trajectory_follower/include/trajectory_follower/debug_values.hpp +++ b/control/trajectory_follower/include/trajectory_follower/debug_values.hpp @@ -20,13 +20,7 @@ #include -namespace autoware -{ -namespace motion -{ -namespace control -{ -namespace trajectory_follower +namespace autoware::motion::control::trajectory_follower { using autoware::common::types::float64_t; /// Debug Values used for debugging or controller tuning @@ -97,9 +91,6 @@ class TRAJECTORY_FOLLOWER_PUBLIC DebugValues private: std::array(TYPE::SIZE)> m_values; }; -} // namespace trajectory_follower -} // namespace control -} // namespace motion -} // namespace autoware +} // namespace autoware::motion::control::trajectory_follower #endif // TRAJECTORY_FOLLOWER__DEBUG_VALUES_HPP_ diff --git a/control/trajectory_follower/include/trajectory_follower/input_data.hpp b/control/trajectory_follower/include/trajectory_follower/input_data.hpp index 505e5e0de2545..a47c838258f71 100644 --- a/control/trajectory_follower/include/trajectory_follower/input_data.hpp +++ b/control/trajectory_follower/include/trajectory_follower/input_data.hpp @@ -19,13 +19,7 @@ #include "autoware_auto_vehicle_msgs/msg/steering_report.hpp" #include "nav_msgs/msg/odometry.hpp" -namespace autoware -{ -namespace motion -{ -namespace control -{ -namespace trajectory_follower +namespace autoware::motion::control::trajectory_follower { struct InputData { @@ -33,9 +27,6 @@ struct InputData nav_msgs::msg::Odometry::SharedPtr current_odometry_ptr; autoware_auto_vehicle_msgs::msg::SteeringReport::SharedPtr current_steering_ptr; }; -} // namespace trajectory_follower -} // namespace control -} // namespace motion -} // namespace autoware +} // namespace autoware::motion::control::trajectory_follower #endif // TRAJECTORY_FOLLOWER__INPUT_DATA_HPP_ diff --git a/control/trajectory_follower/include/trajectory_follower/interpolate.hpp b/control/trajectory_follower/include/trajectory_follower/interpolate.hpp index 5a3ffa0942797..34adfceff7580 100644 --- a/control/trajectory_follower/include/trajectory_follower/interpolate.hpp +++ b/control/trajectory_follower/include/trajectory_follower/interpolate.hpp @@ -22,13 +22,7 @@ #include #include -namespace autoware -{ -namespace motion -{ -namespace control -{ -namespace trajectory_follower +namespace autoware::motion::control::trajectory_follower { using autoware::common::types::bool8_t; using autoware::common::types::float64_t; @@ -52,8 +46,5 @@ TRAJECTORY_FOLLOWER_PUBLIC bool8_t linearInterpolate( TRAJECTORY_FOLLOWER_PUBLIC bool8_t linearInterpolate( const std::vector & base_index, const std::vector & base_value, const float64_t & return_index, float64_t & return_value); -} // namespace trajectory_follower -} // namespace control -} // namespace motion -} // namespace autoware +} // namespace autoware::motion::control::trajectory_follower #endif // TRAJECTORY_FOLLOWER__INTERPOLATE_HPP_ diff --git a/control/trajectory_follower/include/trajectory_follower/lateral_controller_base.hpp b/control/trajectory_follower/include/trajectory_follower/lateral_controller_base.hpp index 0a234fa0b42b1..5085cc9f17045 100644 --- a/control/trajectory_follower/include/trajectory_follower/lateral_controller_base.hpp +++ b/control/trajectory_follower/include/trajectory_follower/lateral_controller_base.hpp @@ -23,13 +23,7 @@ #include -namespace autoware -{ -namespace motion -{ -namespace control -{ -namespace trajectory_follower +namespace autoware::motion::control::trajectory_follower { struct LateralOutput { @@ -51,9 +45,6 @@ class LateralControllerBase LongitudinalSyncData longitudinal_sync_data_; }; -} // namespace trajectory_follower -} // namespace control -} // namespace motion -} // namespace autoware +} // namespace autoware::motion::control::trajectory_follower #endif // TRAJECTORY_FOLLOWER__LATERAL_CONTROLLER_BASE_HPP_ diff --git a/control/trajectory_follower/include/trajectory_follower/longitudinal_controller_base.hpp b/control/trajectory_follower/include/trajectory_follower/longitudinal_controller_base.hpp index 6eb4d13119275..eeb59e69fb910 100644 --- a/control/trajectory_follower/include/trajectory_follower/longitudinal_controller_base.hpp +++ b/control/trajectory_follower/include/trajectory_follower/longitudinal_controller_base.hpp @@ -23,13 +23,7 @@ #include -namespace autoware -{ -namespace motion -{ -namespace control -{ -namespace trajectory_follower +namespace autoware::motion::control::trajectory_follower { struct LongitudinalOutput { @@ -47,9 +41,6 @@ class LongitudinalControllerBase LateralSyncData lateral_sync_data_; }; -} // namespace trajectory_follower -} // namespace control -} // namespace motion -} // namespace autoware +} // namespace autoware::motion::control::trajectory_follower #endif // TRAJECTORY_FOLLOWER__LONGITUDINAL_CONTROLLER_BASE_HPP_ diff --git a/control/trajectory_follower/include/trajectory_follower/longitudinal_controller_utils.hpp b/control/trajectory_follower/include/trajectory_follower/longitudinal_controller_utils.hpp index 1a18f6c166f14..f948c03c0cb0b 100644 --- a/control/trajectory_follower/include/trajectory_follower/longitudinal_controller_utils.hpp +++ b/control/trajectory_follower/include/trajectory_follower/longitudinal_controller_utils.hpp @@ -33,13 +33,7 @@ #include #include -namespace autoware -{ -namespace motion -{ -namespace control -{ -namespace trajectory_follower +namespace autoware::motion::control::trajectory_follower { namespace longitudinal_utils { @@ -166,9 +160,6 @@ TRAJECTORY_FOLLOWER_PUBLIC float64_t applyDiffLimitFilter( const float64_t input_val, const float64_t prev_val, const float64_t dt, const float64_t max_val, const float64_t min_val); } // namespace longitudinal_utils -} // namespace trajectory_follower -} // namespace control -} // namespace motion -} // namespace autoware +} // namespace autoware::motion::control::trajectory_follower #endif // TRAJECTORY_FOLLOWER__LONGITUDINAL_CONTROLLER_UTILS_HPP_ diff --git a/control/trajectory_follower/include/trajectory_follower/lowpass_filter.hpp b/control/trajectory_follower/include/trajectory_follower/lowpass_filter.hpp index 6b750dd890f3f..b401e61f1dd54 100644 --- a/control/trajectory_follower/include/trajectory_follower/lowpass_filter.hpp +++ b/control/trajectory_follower/include/trajectory_follower/lowpass_filter.hpp @@ -23,13 +23,7 @@ #include #include -namespace autoware -{ -namespace motion -{ -namespace control -{ -namespace trajectory_follower +namespace autoware::motion::control::trajectory_follower { using autoware::common::types::bool8_t; using autoware::common::types::float64_t; @@ -154,8 +148,5 @@ namespace MoveAverageFilter */ TRAJECTORY_FOLLOWER_PUBLIC bool8_t filt_vector(const int64_t num, std::vector & u); } // namespace MoveAverageFilter -} // namespace trajectory_follower -} // namespace control -} // namespace motion -} // namespace autoware +} // namespace autoware::motion::control::trajectory_follower #endif // TRAJECTORY_FOLLOWER__LOWPASS_FILTER_HPP_ diff --git a/control/trajectory_follower/include/trajectory_follower/mpc.hpp b/control/trajectory_follower/include/trajectory_follower/mpc.hpp index 42298aaae5369..85bf7a9f0ffee 100644 --- a/control/trajectory_follower/include/trajectory_follower/mpc.hpp +++ b/control/trajectory_follower/include/trajectory_follower/mpc.hpp @@ -46,13 +46,7 @@ #include #include -namespace autoware -{ -namespace motion -{ -namespace control -{ -namespace trajectory_follower +namespace autoware::motion::control::trajectory_follower { using autoware::common::types::bool8_t; using autoware::common::types::float64_t; @@ -440,9 +434,6 @@ class TRAJECTORY_FOLLOWER_PUBLIC MPC */ inline void setClock(rclcpp::Clock::SharedPtr clock) { m_clock = clock; } }; // class MPC -} // namespace trajectory_follower -} // namespace control -} // namespace motion -} // namespace autoware +} // namespace autoware::motion::control::trajectory_follower #endif // TRAJECTORY_FOLLOWER__MPC_HPP_ diff --git a/control/trajectory_follower/include/trajectory_follower/mpc_lateral_controller.hpp b/control/trajectory_follower/include/trajectory_follower/mpc_lateral_controller.hpp index 311b545bdc9f2..27fd0dd2c62ec 100644 --- a/control/trajectory_follower/include/trajectory_follower/mpc_lateral_controller.hpp +++ b/control/trajectory_follower/include/trajectory_follower/mpc_lateral_controller.hpp @@ -50,13 +50,7 @@ #include #include -namespace autoware -{ -namespace motion -{ -namespace control -{ -namespace trajectory_follower +namespace autoware::motion::control::trajectory_follower { using autoware::common::types::bool8_t; using autoware::common::types::float64_t; @@ -215,9 +209,6 @@ class TRAJECTORY_FOLLOWER_PUBLIC MpcLateralController : public LateralController rcl_interfaces::msg::SetParametersResult paramCallback( const std::vector & parameters); }; -} // namespace trajectory_follower -} // namespace control -} // namespace motion -} // namespace autoware +} // namespace autoware::motion::control::trajectory_follower #endif // TRAJECTORY_FOLLOWER__MPC_LATERAL_CONTROLLER_HPP_ diff --git a/control/trajectory_follower/include/trajectory_follower/mpc_trajectory.hpp b/control/trajectory_follower/include/trajectory_follower/mpc_trajectory.hpp index 7c252dd61d995..290c97f740d53 100644 --- a/control/trajectory_follower/include/trajectory_follower/mpc_trajectory.hpp +++ b/control/trajectory_follower/include/trajectory_follower/mpc_trajectory.hpp @@ -24,13 +24,7 @@ #include #include -namespace autoware -{ -namespace motion -{ -namespace control -{ -namespace trajectory_follower +namespace autoware::motion::control::trajectory_follower { using autoware::common::types::float64_t; /** @@ -98,8 +92,5 @@ class TRAJECTORY_FOLLOWER_PUBLIC MPCTrajectory return points; } }; -} // namespace trajectory_follower -} // namespace control -} // namespace motion -} // namespace autoware +} // namespace autoware::motion::control::trajectory_follower #endif // TRAJECTORY_FOLLOWER__MPC_TRAJECTORY_HPP_ diff --git a/control/trajectory_follower/include/trajectory_follower/mpc_utils.hpp b/control/trajectory_follower/include/trajectory_follower/mpc_utils.hpp index 83f152f3e4df1..294aa997aff57 100644 --- a/control/trajectory_follower/include/trajectory_follower/mpc_utils.hpp +++ b/control/trajectory_follower/include/trajectory_follower/mpc_utils.hpp @@ -44,13 +44,7 @@ #include #include -namespace autoware -{ -namespace motion -{ -namespace control -{ -namespace trajectory_follower +namespace autoware::motion::control::trajectory_follower { namespace MPCUtils { @@ -184,8 +178,5 @@ TRAJECTORY_FOLLOWER_PUBLIC bool8_t calcNearestPoseInterp( TRAJECTORY_FOLLOWER_PUBLIC float64_t calcStopDistance( const autoware_auto_planning_msgs::msg::Trajectory & current_trajectory, const int64_t origin); } // namespace MPCUtils -} // namespace trajectory_follower -} // namespace control -} // namespace motion -} // namespace autoware +} // namespace autoware::motion::control::trajectory_follower #endif // TRAJECTORY_FOLLOWER__MPC_UTILS_HPP_ diff --git a/control/trajectory_follower/include/trajectory_follower/pid.hpp b/control/trajectory_follower/include/trajectory_follower/pid.hpp index ca64614c9411f..af4bdfaeaf4ac 100644 --- a/control/trajectory_follower/include/trajectory_follower/pid.hpp +++ b/control/trajectory_follower/include/trajectory_follower/pid.hpp @@ -20,13 +20,7 @@ #include -namespace autoware -{ -namespace motion -{ -namespace control -{ -namespace trajectory_follower +namespace autoware::motion::control::trajectory_follower { using autoware::common::types::bool8_t; using autoware::common::types::float64_t; @@ -100,9 +94,6 @@ class TRAJECTORY_FOLLOWER_PUBLIC PIDController bool8_t m_is_gains_set; bool8_t m_is_limits_set; }; -} // namespace trajectory_follower -} // namespace control -} // namespace motion -} // namespace autoware +} // namespace autoware::motion::control::trajectory_follower #endif // TRAJECTORY_FOLLOWER__PID_HPP_ diff --git a/control/trajectory_follower/include/trajectory_follower/pid_longitudinal_controller.hpp b/control/trajectory_follower/include/trajectory_follower/pid_longitudinal_controller.hpp index 9c28d057ddb02..dd1183c6468f3 100644 --- a/control/trajectory_follower/include/trajectory_follower/pid_longitudinal_controller.hpp +++ b/control/trajectory_follower/include/trajectory_follower/pid_longitudinal_controller.hpp @@ -45,13 +45,7 @@ #include #include -namespace autoware -{ -namespace motion -{ -namespace control -{ -namespace trajectory_follower +namespace autoware::motion::control::trajectory_follower { using autoware::common::types::bool8_t; using autoware::common::types::float64_t; @@ -369,9 +363,6 @@ class TRAJECTORY_FOLLOWER_PUBLIC PidLongitudinalController : public Longitudinal const Motion & ctrl_cmd, const geometry_msgs::msg::Pose & current_pose, const ControlData & control_data); }; -} // namespace trajectory_follower -} // namespace control -} // namespace motion -} // namespace autoware +} // namespace autoware::motion::control::trajectory_follower #endif // TRAJECTORY_FOLLOWER__PID_LONGITUDINAL_CONTROLLER_HPP_ diff --git a/control/trajectory_follower/include/trajectory_follower/qp_solver/qp_solver_interface.hpp b/control/trajectory_follower/include/trajectory_follower/qp_solver/qp_solver_interface.hpp index 1a1c0b4c782a4..e4420327e3636 100644 --- a/control/trajectory_follower/include/trajectory_follower/qp_solver/qp_solver_interface.hpp +++ b/control/trajectory_follower/include/trajectory_follower/qp_solver/qp_solver_interface.hpp @@ -21,13 +21,7 @@ #include "eigen3/Eigen/LU" #include "trajectory_follower/visibility_control.hpp" -namespace autoware -{ -namespace motion -{ -namespace control -{ -namespace trajectory_follower +namespace autoware::motion::control::trajectory_follower { using autoware::common::types::bool8_t; /// Interface for solvers of Quadratic Programming (QP) problems @@ -56,8 +50,5 @@ class TRAJECTORY_FOLLOWER_PUBLIC QPSolverInterface const Eigen::VectorXd & lb, const Eigen::VectorXd & ub, const Eigen::VectorXd & lb_a, const Eigen::VectorXd & ub_a, Eigen::VectorXd & u) = 0; }; -} // namespace trajectory_follower -} // namespace control -} // namespace motion -} // namespace autoware +} // namespace autoware::motion::control::trajectory_follower #endif // TRAJECTORY_FOLLOWER__QP_SOLVER__QP_SOLVER_INTERFACE_HPP_ diff --git a/control/trajectory_follower/include/trajectory_follower/qp_solver/qp_solver_osqp.hpp b/control/trajectory_follower/include/trajectory_follower/qp_solver/qp_solver_osqp.hpp index c73199a654b37..38684e8917699 100644 --- a/control/trajectory_follower/include/trajectory_follower/qp_solver/qp_solver_osqp.hpp +++ b/control/trajectory_follower/include/trajectory_follower/qp_solver/qp_solver_osqp.hpp @@ -22,13 +22,7 @@ #include "trajectory_follower/qp_solver/qp_solver_interface.hpp" #include "trajectory_follower/visibility_control.hpp" -namespace autoware -{ -namespace motion -{ -namespace control -{ -namespace trajectory_follower +namespace autoware::motion::control::trajectory_follower { using autoware::common::types::bool8_t; using autoware::common::types::float64_t; @@ -67,8 +61,5 @@ class TRAJECTORY_FOLLOWER_PUBLIC QPSolverOSQP : public QPSolverInterface autoware::common::osqp::OSQPInterface osqpsolver_; rclcpp::Logger logger_; }; -} // namespace trajectory_follower -} // namespace control -} // namespace motion -} // namespace autoware +} // namespace autoware::motion::control::trajectory_follower #endif // TRAJECTORY_FOLLOWER__QP_SOLVER__QP_SOLVER_OSQP_HPP_ diff --git a/control/trajectory_follower/include/trajectory_follower/qp_solver/qp_solver_unconstr_fast.hpp b/control/trajectory_follower/include/trajectory_follower/qp_solver/qp_solver_unconstr_fast.hpp index 9add78edd918d..c7ae0d34d19c0 100644 --- a/control/trajectory_follower/include/trajectory_follower/qp_solver/qp_solver_unconstr_fast.hpp +++ b/control/trajectory_follower/include/trajectory_follower/qp_solver/qp_solver_unconstr_fast.hpp @@ -24,13 +24,7 @@ #include -namespace autoware -{ -namespace motion -{ -namespace control -{ -namespace trajectory_follower +namespace autoware::motion::control::trajectory_follower { using autoware::common::types::bool8_t; /** @@ -67,8 +61,5 @@ class TRAJECTORY_FOLLOWER_PUBLIC QPSolverEigenLeastSquareLLT : public QPSolverIn const Eigen::VectorXd & lb, const Eigen::VectorXd & ub, const Eigen::VectorXd & lb_a, const Eigen::VectorXd & ub_a, Eigen::VectorXd & u) override; }; -} // namespace trajectory_follower -} // namespace control -} // namespace motion -} // namespace autoware +} // namespace autoware::motion::control::trajectory_follower #endif // TRAJECTORY_FOLLOWER__QP_SOLVER__QP_SOLVER_UNCONSTR_FAST_HPP_ diff --git a/control/trajectory_follower/include/trajectory_follower/smooth_stop.hpp b/control/trajectory_follower/include/trajectory_follower/smooth_stop.hpp index ad40de50a35aa..fca0c11647292 100644 --- a/control/trajectory_follower/include/trajectory_follower/smooth_stop.hpp +++ b/control/trajectory_follower/include/trajectory_follower/smooth_stop.hpp @@ -27,13 +27,7 @@ #include #include -namespace autoware -{ -namespace motion -{ -namespace control -{ -namespace trajectory_follower +namespace autoware::motion::control::trajectory_follower { using autoware::common::types::bool8_t; using autoware::common::types::float64_t; @@ -120,9 +114,6 @@ class TRAJECTORY_FOLLOWER_PUBLIC SmoothStop rclcpp::Time m_weak_acc_time; bool8_t m_is_set_params = false; }; -} // namespace trajectory_follower -} // namespace control -} // namespace motion -} // namespace autoware +} // namespace autoware::motion::control::trajectory_follower #endif // TRAJECTORY_FOLLOWER__SMOOTH_STOP_HPP_ diff --git a/control/trajectory_follower/include/trajectory_follower/sync_data.hpp b/control/trajectory_follower/include/trajectory_follower/sync_data.hpp index 48224a5498a9c..ed6a8037b13a4 100644 --- a/control/trajectory_follower/include/trajectory_follower/sync_data.hpp +++ b/control/trajectory_follower/include/trajectory_follower/sync_data.hpp @@ -15,13 +15,7 @@ #ifndef TRAJECTORY_FOLLOWER__SYNC_DATA_HPP_ #define TRAJECTORY_FOLLOWER__SYNC_DATA_HPP_ -namespace autoware -{ -namespace motion -{ -namespace control -{ -namespace trajectory_follower +namespace autoware::motion::control::trajectory_follower { struct LateralSyncData { @@ -33,9 +27,6 @@ struct LongitudinalSyncData bool is_velocity_converged{false}; }; -} // namespace trajectory_follower -} // namespace control -} // namespace motion -} // namespace autoware +} // namespace autoware::motion::control::trajectory_follower #endif // TRAJECTORY_FOLLOWER__SYNC_DATA_HPP_ diff --git a/control/trajectory_follower/include/trajectory_follower/vehicle_model/vehicle_model_bicycle_dynamics.hpp b/control/trajectory_follower/include/trajectory_follower/vehicle_model/vehicle_model_bicycle_dynamics.hpp index 23f3ca8e288fa..755c03f997592 100644 --- a/control/trajectory_follower/include/trajectory_follower/vehicle_model/vehicle_model_bicycle_dynamics.hpp +++ b/control/trajectory_follower/include/trajectory_follower/vehicle_model/vehicle_model_bicycle_dynamics.hpp @@ -53,13 +53,7 @@ #include "trajectory_follower/vehicle_model/vehicle_model_interface.hpp" #include "trajectory_follower/visibility_control.hpp" -namespace autoware -{ -namespace motion -{ -namespace control -{ -namespace trajectory_follower +namespace autoware::motion::control::trajectory_follower { using autoware::common::types::float64_t; /** @@ -114,8 +108,5 @@ class TRAJECTORY_FOLLOWER_PUBLIC DynamicsBicycleModel : public VehicleModelInter float64_t m_cf; //!< @brief front cornering power [N/rad] float64_t m_cr; //!< @brief rear cornering power [N/rad] }; -} // namespace trajectory_follower -} // namespace control -} // namespace motion -} // namespace autoware +} // namespace autoware::motion::control::trajectory_follower #endif // TRAJECTORY_FOLLOWER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_DYNAMICS_HPP_ diff --git a/control/trajectory_follower/include/trajectory_follower/vehicle_model/vehicle_model_bicycle_kinematics.hpp b/control/trajectory_follower/include/trajectory_follower/vehicle_model/vehicle_model_bicycle_kinematics.hpp index 525461f3a9e5d..36bb38a8d7879 100644 --- a/control/trajectory_follower/include/trajectory_follower/vehicle_model/vehicle_model_bicycle_kinematics.hpp +++ b/control/trajectory_follower/include/trajectory_follower/vehicle_model/vehicle_model_bicycle_kinematics.hpp @@ -47,13 +47,7 @@ #include "trajectory_follower/vehicle_model/vehicle_model_interface.hpp" #include "trajectory_follower/visibility_control.hpp" -namespace autoware -{ -namespace motion -{ -namespace control -{ -namespace trajectory_follower +namespace autoware::motion::control::trajectory_follower { using autoware::common::types::float64_t; /** @@ -99,8 +93,5 @@ class TRAJECTORY_FOLLOWER_PUBLIC KinematicsBicycleModel : public VehicleModelInt float64_t m_steer_lim; //!< @brief steering angle limit [rad] float64_t m_steer_tau; //!< @brief steering time constant for 1d-model [s] }; -} // namespace trajectory_follower -} // namespace control -} // namespace motion -} // namespace autoware +} // namespace autoware::motion::control::trajectory_follower #endif // TRAJECTORY_FOLLOWER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_HPP_ diff --git a/control/trajectory_follower/include/trajectory_follower/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp b/control/trajectory_follower/include/trajectory_follower/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp index 8656f1ab7afb8..f968dd5aa20a5 100644 --- a/control/trajectory_follower/include/trajectory_follower/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp +++ b/control/trajectory_follower/include/trajectory_follower/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp @@ -44,13 +44,7 @@ #include "trajectory_follower/vehicle_model/vehicle_model_interface.hpp" #include "trajectory_follower/visibility_control.hpp" -namespace autoware -{ -namespace motion -{ -namespace control -{ -namespace trajectory_follower +namespace autoware::motion::control::trajectory_follower { using autoware::common::types::float64_t; /** @@ -93,8 +87,5 @@ class TRAJECTORY_FOLLOWER_PUBLIC KinematicsBicycleModelNoDelay : public VehicleM private: float64_t m_steer_lim; //!< @brief steering angle limit [rad] }; -} // namespace trajectory_follower -} // namespace control -} // namespace motion -} // namespace autoware +} // namespace autoware::motion::control::trajectory_follower #endif // TRAJECTORY_FOLLOWER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_NO_DELAY_HPP_ diff --git a/control/trajectory_follower/include/trajectory_follower/vehicle_model/vehicle_model_interface.hpp b/control/trajectory_follower/include/trajectory_follower/vehicle_model/vehicle_model_interface.hpp index 1c3856246f719..65193d9fec34c 100644 --- a/control/trajectory_follower/include/trajectory_follower/vehicle_model/vehicle_model_interface.hpp +++ b/control/trajectory_follower/include/trajectory_follower/vehicle_model/vehicle_model_interface.hpp @@ -19,13 +19,7 @@ #include "eigen3/Eigen/Core" #include "trajectory_follower/visibility_control.hpp" -namespace autoware -{ -namespace motion -{ -namespace control -{ -namespace trajectory_follower +namespace autoware::motion::control::trajectory_follower { using autoware::common::types::float64_t; /** @@ -111,8 +105,5 @@ class TRAJECTORY_FOLLOWER_PUBLIC VehicleModelInterface */ virtual void calculateReferenceInput(Eigen::MatrixXd & u_ref) = 0; }; -} // namespace trajectory_follower -} // namespace control -} // namespace motion -} // namespace autoware +} // namespace autoware::motion::control::trajectory_follower #endif // TRAJECTORY_FOLLOWER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_ diff --git a/control/trajectory_follower/src/interpolate.cpp b/control/trajectory_follower/src/interpolate.cpp index 8eaa7fab36c4d..87199498fb1d0 100644 --- a/control/trajectory_follower/src/interpolate.cpp +++ b/control/trajectory_follower/src/interpolate.cpp @@ -22,13 +22,7 @@ * linear interpolation */ -namespace autoware -{ -namespace motion -{ -namespace control -{ -namespace trajectory_follower +namespace autoware::motion::control::trajectory_follower { namespace { @@ -137,7 +131,4 @@ bool8_t linearInterpolate( return_value = return_value_v.at(0); return true; } -} // namespace trajectory_follower -} // namespace control -} // namespace motion -} // namespace autoware +} // namespace autoware::motion::control::trajectory_follower diff --git a/control/trajectory_follower/src/longitudinal_controller_utils.cpp b/control/trajectory_follower/src/longitudinal_controller_utils.cpp index 55060d1b9bc7f..76c931c58cb34 100644 --- a/control/trajectory_follower/src/longitudinal_controller_utils.cpp +++ b/control/trajectory_follower/src/longitudinal_controller_utils.cpp @@ -29,13 +29,7 @@ #include #include -namespace autoware -{ -namespace motion -{ -namespace control -{ -namespace trajectory_follower +namespace autoware::motion::control::trajectory_follower { namespace longitudinal_utils { @@ -187,7 +181,4 @@ float64_t applyDiffLimitFilter( return applyDiffLimitFilter(input_val, prev_val, dt, max_val, min_val); } } // namespace longitudinal_utils -} // namespace trajectory_follower -} // namespace control -} // namespace motion -} // namespace autoware +} // namespace autoware::motion::control::trajectory_follower diff --git a/control/trajectory_follower/src/lowpass_filter.cpp b/control/trajectory_follower/src/lowpass_filter.cpp index 1f357b258e40f..7874a40e6946e 100644 --- a/control/trajectory_follower/src/lowpass_filter.cpp +++ b/control/trajectory_follower/src/lowpass_filter.cpp @@ -16,13 +16,7 @@ #include -namespace autoware -{ -namespace motion -{ -namespace control -{ -namespace trajectory_follower +namespace autoware::motion::control::trajectory_follower { Butterworth2dFilter::Butterworth2dFilter(float64_t dt, float64_t f_cutoff_hz) { @@ -143,7 +137,4 @@ bool8_t filt_vector(const int64_t num, std::vector & u) return true; } } // namespace MoveAverageFilter -} // namespace trajectory_follower -} // namespace control -} // namespace motion -} // namespace autoware +} // namespace autoware::motion::control::trajectory_follower diff --git a/control/trajectory_follower/src/mpc.cpp b/control/trajectory_follower/src/mpc.cpp index 3d89d34398ede..02f895b4122f8 100644 --- a/control/trajectory_follower/src/mpc.cpp +++ b/control/trajectory_follower/src/mpc.cpp @@ -15,6 +15,7 @@ #include "trajectory_follower/mpc.hpp" #include "motion_utils/motion_utils.hpp" +#include "tier4_autoware_utils/math/constants.hpp" #include #include @@ -24,16 +25,7 @@ #include #include -#define DEG2RAD 3.1415926535 / 180.0 -#define RAD2DEG 180.0 / 3.1415926535 - -namespace autoware -{ -namespace motion -{ -namespace control -{ -namespace trajectory_follower +namespace autoware::motion::control::trajectory_follower { using namespace std::literals::chrono_literals; using ::motion::motion_common::to_angle; @@ -204,15 +196,12 @@ void MPC::setReferenceTrajectory( mpc_traj_smoothed = mpc_traj_resampled; const int64_t mpc_traj_resampled_size = static_cast(mpc_traj_resampled.size()); if (enable_path_smoothing && mpc_traj_resampled_size > 2 * path_filter_moving_ave_num) { + using trajectory_follower::MoveAverageFilter::filt_vector; if ( - !trajectory_follower::MoveAverageFilter::filt_vector( - path_filter_moving_ave_num, mpc_traj_smoothed.x) || - !trajectory_follower::MoveAverageFilter::filt_vector( - path_filter_moving_ave_num, mpc_traj_smoothed.y) || - !trajectory_follower::MoveAverageFilter::filt_vector( - path_filter_moving_ave_num, mpc_traj_smoothed.yaw) || - !trajectory_follower::MoveAverageFilter::filt_vector( - path_filter_moving_ave_num, mpc_traj_smoothed.vx)) { + !filt_vector(path_filter_moving_ave_num, mpc_traj_smoothed.x) || + !filt_vector(path_filter_moving_ave_num, mpc_traj_smoothed.y) || + !filt_vector(path_filter_moving_ave_num, mpc_traj_smoothed.yaw) || + !filt_vector(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; } @@ -303,7 +292,8 @@ bool8_t MPC::getData( if (std::fabs(data->yaw_err) > m_admissible_yaw_error_rad) { RCLCPP_WARN_SKIPFIRST_THROTTLE( m_logger, *m_clock, duration, "yaw error is over limit. error = %f deg, limit %f deg", - RAD2DEG * data->yaw_err, RAD2DEG * m_admissible_yaw_error_rad); + tier4_autoware_utils::rad2deg(data->yaw_err), + tier4_autoware_utils::rad2deg(m_admissible_yaw_error_rad)); return false; } @@ -607,7 +597,7 @@ MPCMatrix MPC::generateMPCMatrix( /* 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)) < DEG2RAD * m_param.zero_ff_steer_deg) { + 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; @@ -836,7 +826,4 @@ bool8_t MPC::isValid(const MPCMatrix & m) const return true; } -} // namespace trajectory_follower -} // namespace control -} // namespace motion -} // namespace autoware +} // namespace autoware::motion::control::trajectory_follower diff --git a/control/trajectory_follower/src/mpc_lateral_controller.cpp b/control/trajectory_follower/src/mpc_lateral_controller.cpp index d2938e12c1617..ccfeff5413f0f 100644 --- a/control/trajectory_follower/src/mpc_lateral_controller.cpp +++ b/control/trajectory_follower/src/mpc_lateral_controller.cpp @@ -25,13 +25,7 @@ #include #include -namespace autoware -{ -namespace motion -{ -namespace control -{ -namespace trajectory_follower +namespace autoware::motion::control::trajectory_follower { namespace { @@ -251,31 +245,29 @@ bool MpcLateralController::isSteerConverged( bool8_t MpcLateralController::checkData() const { if (!m_mpc.hasVehicleModel()) { - RCLCPP_DEBUG(node_->get_logger(), "MPC does not have a vehicle model"); + RCLCPP_ERROR(node_->get_logger(), "MPC does not have a vehicle model"); return false; } if (!m_mpc.hasQPSolver()) { - RCLCPP_DEBUG(node_->get_logger(), "MPC does not have a QP solver"); + RCLCPP_ERROR(node_->get_logger(), "MPC does not have a QP solver"); return false; } - if (!m_current_kinematic_state_ptr) { - RCLCPP_DEBUG( - node_->get_logger(), "waiting data. kinematic_state = %d", - m_current_kinematic_state_ptr != nullptr); + const auto invalid = [this](const auto & msg) { + RCLCPP_DEBUG(node_->get_logger(), "%s", msg); return false; + }; + + if (!m_current_kinematic_state_ptr) { + return invalid("waiting kinematic_state"); } if (!m_current_steering_ptr) { - RCLCPP_DEBUG( - node_->get_logger(), "waiting data. current_steering = %d", - m_current_steering_ptr != nullptr); - return false; + return invalid("waiting steering"); } - if (m_mpc.m_ref_traj.size() == 0) { - RCLCPP_DEBUG(node_->get_logger(), "trajectory size is zero."); - return false; + if (m_mpc.m_ref_traj.empty()) { + return invalid("trajectory is empty"); } return true; @@ -548,7 +540,4 @@ bool8_t MpcLateralController::isValidTrajectory( return true; } -} // namespace trajectory_follower -} // namespace control -} // namespace motion -} // namespace autoware +} // namespace autoware::motion::control::trajectory_follower diff --git a/control/trajectory_follower/src/mpc_trajectory.cpp b/control/trajectory_follower/src/mpc_trajectory.cpp index d22bd4004705b..85385416bfe1b 100644 --- a/control/trajectory_follower/src/mpc_trajectory.cpp +++ b/control/trajectory_follower/src/mpc_trajectory.cpp @@ -14,13 +14,7 @@ #include "trajectory_follower/mpc_trajectory.hpp" -namespace autoware -{ -namespace motion -{ -namespace control -{ -namespace trajectory_follower +namespace autoware::motion::control::trajectory_follower { void MPCTrajectory::push_back( const float64_t & xp, const float64_t & yp, const float64_t & zp, const float64_t & yawp, @@ -60,7 +54,4 @@ size_t MPCTrajectory::size() const return 0; } } -} // namespace trajectory_follower -} // namespace control -} // namespace motion -} // namespace autoware +} // namespace autoware::motion::control::trajectory_follower diff --git a/control/trajectory_follower/src/mpc_utils.cpp b/control/trajectory_follower/src/mpc_utils.cpp index a56dc36a007de..729597c2a8840 100644 --- a/control/trajectory_follower/src/mpc_utils.cpp +++ b/control/trajectory_follower/src/mpc_utils.cpp @@ -21,13 +21,7 @@ #include #include -namespace autoware -{ -namespace motion -{ -namespace control -{ -namespace trajectory_follower +namespace autoware::motion::control::trajectory_follower { namespace MPCUtils { @@ -409,7 +403,4 @@ float64_t calcStopDistance( } } // namespace MPCUtils -} // namespace trajectory_follower -} // namespace control -} // namespace motion -} // namespace autoware +} // namespace autoware::motion::control::trajectory_follower diff --git a/control/trajectory_follower/src/pid.cpp b/control/trajectory_follower/src/pid.cpp index badb3fb51a1a0..00d12f77b71d7 100644 --- a/control/trajectory_follower/src/pid.cpp +++ b/control/trajectory_follower/src/pid.cpp @@ -20,13 +20,7 @@ #include #include -namespace autoware -{ -namespace motion -{ -namespace control -{ -namespace trajectory_follower +namespace autoware::motion::control::trajectory_follower { PIDController::PIDController() : m_error_integral(0.0), @@ -109,7 +103,4 @@ void PIDController::reset() m_prev_error = 0.0; m_is_first_time = true; } -} // namespace trajectory_follower -} // namespace control -} // namespace motion -} // namespace autoware +} // namespace autoware::motion::control::trajectory_follower diff --git a/control/trajectory_follower/src/pid_longitudinal_controller.cpp b/control/trajectory_follower/src/pid_longitudinal_controller.cpp index 4fa5f5be7f857..5db1ac31dc437 100644 --- a/control/trajectory_follower/src/pid_longitudinal_controller.cpp +++ b/control/trajectory_follower/src/pid_longitudinal_controller.cpp @@ -26,13 +26,7 @@ #include #include -namespace autoware -{ -namespace motion -{ -namespace control -{ -namespace trajectory_follower +namespace autoware::motion::control::trajectory_follower { PidLongitudinalController::PidLongitudinalController(rclcpp::Node & node) : node_{&node} { @@ -945,7 +939,4 @@ void PidLongitudinalController::updateDebugVelAcc( m_debug_values.setValues(DebugValues::TYPE::ERROR_VEL, target_motion.vel - current_vel); } -} // namespace trajectory_follower -} // namespace control -} // namespace motion -} // namespace autoware +} // namespace autoware::motion::control::trajectory_follower diff --git a/control/trajectory_follower/src/qp_solver/qp_solver_osqp.cpp b/control/trajectory_follower/src/qp_solver/qp_solver_osqp.cpp index 28cbbb9f59aae..4cc94011f690f 100644 --- a/control/trajectory_follower/src/qp_solver/qp_solver_osqp.cpp +++ b/control/trajectory_follower/src/qp_solver/qp_solver_osqp.cpp @@ -17,13 +17,7 @@ #include #include -namespace autoware -{ -namespace motion -{ -namespace control -{ -namespace trajectory_follower +namespace autoware::motion::control::trajectory_follower { QPSolverOSQP::QPSolverOSQP(const rclcpp::Logger & logger) : logger_{logger} {} bool8_t QPSolverOSQP::solve( @@ -80,7 +74,4 @@ bool8_t QPSolverOSQP::solve( } return true; } -} // namespace trajectory_follower -} // namespace control -} // namespace motion -} // namespace autoware +} // namespace autoware::motion::control::trajectory_follower diff --git a/control/trajectory_follower/src/qp_solver/qp_solver_unconstr_fast.cpp b/control/trajectory_follower/src/qp_solver/qp_solver_unconstr_fast.cpp index 4f10a43875aba..de6839cfcaaf7 100644 --- a/control/trajectory_follower/src/qp_solver/qp_solver_unconstr_fast.cpp +++ b/control/trajectory_follower/src/qp_solver/qp_solver_unconstr_fast.cpp @@ -14,13 +14,7 @@ #include "trajectory_follower/qp_solver/qp_solver_unconstr_fast.hpp" -namespace autoware -{ -namespace motion -{ -namespace control -{ -namespace trajectory_follower +namespace autoware::motion::control::trajectory_follower { QPSolverEigenLeastSquareLLT::QPSolverEigenLeastSquareLLT() {} bool8_t QPSolverEigenLeastSquareLLT::solve( @@ -36,7 +30,4 @@ bool8_t QPSolverEigenLeastSquareLLT::solve( return true; } -} // namespace trajectory_follower -} // namespace control -} // namespace motion -} // namespace autoware +} // namespace autoware::motion::control::trajectory_follower diff --git a/control/trajectory_follower/src/smooth_stop.cpp b/control/trajectory_follower/src/smooth_stop.cpp index fec11d6d71d18..7dccb7bdd59d5 100644 --- a/control/trajectory_follower/src/smooth_stop.cpp +++ b/control/trajectory_follower/src/smooth_stop.cpp @@ -22,13 +22,7 @@ #include #include -namespace autoware -{ -namespace motion -{ -namespace control -{ -namespace trajectory_follower +namespace autoware::motion::control::trajectory_follower { void SmoothStop::init(const float64_t pred_vel_in_target, const float64_t pred_stop_dist) { @@ -167,7 +161,4 @@ float64_t SmoothStop::calculate( // when the car is not running return m_params.strong_stop_acc; } -} // namespace trajectory_follower -} // namespace control -} // namespace motion -} // namespace autoware +} // namespace autoware::motion::control::trajectory_follower diff --git a/control/trajectory_follower/src/vehicle_model/vehicle_model_bicycle_dynamics.cpp b/control/trajectory_follower/src/vehicle_model/vehicle_model_bicycle_dynamics.cpp index 9576952034b1d..4f654ff0338ad 100644 --- a/control/trajectory_follower/src/vehicle_model/vehicle_model_bicycle_dynamics.cpp +++ b/control/trajectory_follower/src/vehicle_model/vehicle_model_bicycle_dynamics.cpp @@ -16,13 +16,7 @@ #include -namespace autoware -{ -namespace motion -{ -namespace control -{ -namespace trajectory_follower +namespace autoware::motion::control::trajectory_follower { DynamicsBicycleModel::DynamicsBicycleModel( const float64_t wheelbase, const float64_t mass_fl, const float64_t mass_fr, @@ -92,7 +86,4 @@ void DynamicsBicycleModel::calculateReferenceInput(Eigen::MatrixXd & u_ref) m_lr * m_mass / (2 * m_cf * m_wheelbase) - m_lf * m_mass / (2 * m_cr * m_wheelbase); u_ref(0, 0) = m_wheelbase * m_curvature + Kv * vel * vel * m_curvature; } -} // namespace trajectory_follower -} // namespace control -} // namespace motion -} // namespace autoware +} // namespace autoware::motion::control::trajectory_follower diff --git a/control/trajectory_follower/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp b/control/trajectory_follower/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp index 90b9151d5e074..d01904573ac5a 100644 --- a/control/trajectory_follower/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp +++ b/control/trajectory_follower/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp @@ -16,13 +16,7 @@ #include -namespace autoware -{ -namespace motion -{ -namespace control -{ -namespace trajectory_follower +namespace autoware::motion::control::trajectory_follower { KinematicsBicycleModel::KinematicsBicycleModel( const float64_t wheelbase, const float64_t steer_lim, const float64_t steer_tau) @@ -70,7 +64,4 @@ void KinematicsBicycleModel::calculateReferenceInput(Eigen::MatrixXd & u_ref) { u_ref(0, 0) = std::atan(m_wheelbase * m_curvature); } -} // namespace trajectory_follower -} // namespace control -} // namespace motion -} // namespace autoware +} // namespace autoware::motion::control::trajectory_follower diff --git a/control/trajectory_follower/src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp b/control/trajectory_follower/src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp index 1d6661c9ad8d4..4035b9ea6f964 100644 --- a/control/trajectory_follower/src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp +++ b/control/trajectory_follower/src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp @@ -16,13 +16,7 @@ #include -namespace autoware -{ -namespace motion -{ -namespace control -{ -namespace trajectory_follower +namespace autoware::motion::control::trajectory_follower { KinematicsBicycleModelNoDelay::KinematicsBicycleModelNoDelay( const float64_t wheelbase, const float64_t steer_lim) @@ -61,7 +55,4 @@ void KinematicsBicycleModelNoDelay::calculateReferenceInput(Eigen::MatrixXd & u_ { u_ref(0, 0) = std::atan(m_wheelbase * m_curvature); } -} // namespace trajectory_follower -} // namespace control -} // namespace motion -} // namespace autoware +} // namespace autoware::motion::control::trajectory_follower diff --git a/control/trajectory_follower/src/vehicle_model/vehicle_model_interface.cpp b/control/trajectory_follower/src/vehicle_model/vehicle_model_interface.cpp index 122040be315db..910b215a5f059 100644 --- a/control/trajectory_follower/src/vehicle_model/vehicle_model_interface.cpp +++ b/control/trajectory_follower/src/vehicle_model/vehicle_model_interface.cpp @@ -14,13 +14,7 @@ #include "trajectory_follower/vehicle_model/vehicle_model_interface.hpp" -namespace autoware -{ -namespace motion -{ -namespace control -{ -namespace trajectory_follower +namespace autoware::motion::control::trajectory_follower { VehicleModelInterface::VehicleModelInterface( int64_t dim_x, int64_t dim_u, int64_t dim_y, float64_t wheelbase) @@ -33,7 +27,4 @@ int64_t VehicleModelInterface::getDimY() { return m_dim_y; } float64_t VehicleModelInterface::getWheelbase() { return m_wheelbase; } void VehicleModelInterface::setVelocity(const float64_t velocity) { m_velocity = velocity; } void VehicleModelInterface::setCurvature(const float64_t curvature) { m_curvature = curvature; } -} // namespace trajectory_follower -} // namespace control -} // namespace motion -} // namespace autoware +} // namespace autoware::motion::control::trajectory_follower diff --git a/control/trajectory_follower_nodes/src/controller_node.cpp b/control/trajectory_follower_nodes/src/controller_node.cpp index 2eaf51d8a7652..4be54eb217aa0 100644 --- a/control/trajectory_follower_nodes/src/controller_node.cpp +++ b/control/trajectory_follower_nodes/src/controller_node.cpp @@ -28,13 +28,7 @@ #include #include -namespace autoware -{ -namespace motion -{ -namespace control -{ -namespace trajectory_follower_nodes +namespace autoware::motion::control::trajectory_follower_nodes { Controller::Controller(const rclcpp::NodeOptions & node_options) : Node("controller", node_options) { @@ -166,10 +160,7 @@ void Controller::callbackTimerControl() control_cmd_pub_->publish(out); } -} // namespace trajectory_follower_nodes -} // namespace control -} // namespace motion -} // namespace autoware +} // namespace autoware::motion::control::trajectory_follower_nodes #include "rclcpp_components/register_node_macro.hpp" RCLCPP_COMPONENTS_REGISTER_NODE(autoware::motion::control::trajectory_follower_nodes::Controller)