From d6e96a82779f08eefab16293fae33b473b675b30 Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Wed, 11 Jan 2023 11:19:22 +0900 Subject: [PATCH 01/49] refactor(trajectory_followers): cosmetic changes (#2620) * refactor: namespace mainly Signed-off-by: Takamasa Horibe * pre-commit Signed-off-by: Takamasa Horibe * Update control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp Signed-off-by: Takamasa Horibe --- .../mpc_lateral_controller/interpolate.hpp | 13 +--- .../mpc_lateral_controller/lowpass_filter.hpp | 13 +--- .../include/mpc_lateral_controller/mpc.hpp | 13 +--- .../mpc_lateral_controller.hpp | 13 +--- .../mpc_lateral_controller/mpc_trajectory.hpp | 13 +--- .../mpc_lateral_controller/mpc_utils.hpp | 13 +--- .../qp_solver/qp_solver_interface.hpp | 13 +--- .../qp_solver/qp_solver_osqp.hpp | 13 +--- .../qp_solver/qp_solver_unconstr_fast.hpp | 13 +--- .../vehicle_model_bicycle_dynamics.hpp | 13 +--- .../vehicle_model_bicycle_kinematics.hpp | 13 +--- ...icle_model_bicycle_kinematics_no_delay.hpp | 13 +--- .../vehicle_model/vehicle_model_interface.hpp | 13 +--- .../src/interpolate.cpp | 13 +--- .../src/longitudinal_controller_utils.cpp | 13 +--- .../src/lowpass_filter.cpp | 13 +--- control/mpc_lateral_controller/src/mpc.cpp | 13 +--- .../src/mpc_lateral_controller.cpp | 15 +--- .../src/mpc_trajectory.cpp | 13 +--- .../mpc_lateral_controller/src/mpc_utils.cpp | 13 +--- .../src/qp_solver/qp_solver_osqp.cpp | 13 +--- .../src/qp_solver/qp_solver_unconstr_fast.cpp | 13 +--- .../src/smooth_stop.cpp | 13 +--- .../vehicle_model_bicycle_dynamics.cpp | 13 +--- .../vehicle_model_bicycle_kinematics.cpp | 13 +--- ...icle_model_bicycle_kinematics_no_delay.cpp | 13 +--- .../vehicle_model/vehicle_model_interface.cpp | 13 +--- .../debug_values.hpp | 13 +--- .../longitudinal_controller_utils.hpp | 13 +--- .../lowpass_filter.hpp | 13 +--- .../pid_longitudinal_controller/pid.hpp | 13 +--- .../pid_longitudinal_controller.hpp | 13 +--- .../smooth_stop.hpp | 13 +--- .../src/longitudinal_controller_utils.cpp | 13 +--- .../pid_longitudinal_controller/src/pid.cpp | 13 +--- .../src/pid_longitudinal_controller.cpp | 68 +++++++------------ .../src/smooth_stop.cpp | 13 +--- 37 files changed, 95 insertions(+), 443 deletions(-) diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/interpolate.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/interpolate.hpp index 7f83383a5d65d..2ab29aa0b0afa 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/interpolate.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/interpolate.hpp @@ -19,13 +19,7 @@ #include #include -namespace autoware -{ -namespace motion -{ -namespace control -{ -namespace mpc_lateral_controller +namespace autoware::motion::control::mpc_lateral_controller { /** @@ -48,8 +42,5 @@ bool linearInterpolate( bool linearInterpolate( const std::vector & base_index, const std::vector & base_value, const double & return_index, double & return_value); -} // namespace mpc_lateral_controller -} // namespace control -} // namespace motion -} // namespace autoware +} // namespace autoware::motion::control::mpc_lateral_controller #endif // MPC_LATERAL_CONTROLLER__INTERPOLATE_HPP_ diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/lowpass_filter.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/lowpass_filter.hpp index e3bd55526adea..073d198a937d4 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/lowpass_filter.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/lowpass_filter.hpp @@ -20,13 +20,7 @@ #include #include -namespace autoware -{ -namespace motion -{ -namespace control -{ -namespace mpc_lateral_controller +namespace autoware::motion::control::mpc_lateral_controller { /** * @brief 2nd-order Butterworth Filter @@ -108,8 +102,5 @@ namespace MoveAverageFilter */ bool filt_vector(const int num, std::vector & u); } // namespace MoveAverageFilter -} // namespace mpc_lateral_controller -} // namespace control -} // namespace motion -} // namespace autoware +} // namespace autoware::motion::control::mpc_lateral_controller #endif // MPC_LATERAL_CONTROLLER__LOWPASS_FILTER_HPP_ diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp index 5616f26bab641..c18711d7a4ffa 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp @@ -43,13 +43,7 @@ #include #include -namespace autoware -{ -namespace motion -{ -namespace control -{ -namespace mpc_lateral_controller +namespace autoware::motion::control::mpc_lateral_controller { struct MPCParam @@ -444,9 +438,6 @@ class MPC */ inline void setClock(rclcpp::Clock::SharedPtr clock) { m_clock = clock; } }; // class MPC -} // namespace mpc_lateral_controller -} // namespace control -} // namespace motion -} // namespace autoware +} // namespace autoware::motion::control::mpc_lateral_controller #endif // MPC_LATERAL_CONTROLLER__MPC_HPP_ diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_lateral_controller.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_lateral_controller.hpp index b2c18d0d7d721..d2dc63bb074ec 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_lateral_controller.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_lateral_controller.hpp @@ -48,13 +48,7 @@ #include #include -namespace autoware -{ -namespace motion -{ -namespace control -{ -namespace mpc_lateral_controller +namespace autoware::motion::control::mpc_lateral_controller { namespace trajectory_follower = ::autoware::motion::control::trajectory_follower; @@ -212,9 +206,6 @@ class MpcLateralController : public trajectory_follower::LateralControllerBase rcl_interfaces::msg::SetParametersResult paramCallback( const std::vector & parameters); }; -} // namespace mpc_lateral_controller -} // namespace control -} // namespace motion -} // namespace autoware +} // namespace autoware::motion::control::mpc_lateral_controller #endif // MPC_LATERAL_CONTROLLER__MPC_LATERAL_CONTROLLER_HPP_ diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_trajectory.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_trajectory.hpp index 5abb267eb9d9d..e9a8e50318757 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_trajectory.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_trajectory.hpp @@ -22,13 +22,7 @@ #include #include -namespace autoware -{ -namespace motion -{ -namespace control -{ -namespace mpc_lateral_controller +namespace autoware::motion::control::mpc_lateral_controller { /** @@ -96,8 +90,5 @@ class MPCTrajectory return points; } }; -} // namespace mpc_lateral_controller -} // namespace control -} // namespace motion -} // namespace autoware +} // namespace autoware::motion::control::mpc_lateral_controller #endif // MPC_LATERAL_CONTROLLER__MPC_TRAJECTORY_HPP_ diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_utils.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_utils.hpp index f72e7157d6a8f..b7d35eca6cbe5 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_utils.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_utils.hpp @@ -41,13 +41,7 @@ #include #include -namespace autoware -{ -namespace motion -{ -namespace control -{ -namespace mpc_lateral_controller +namespace autoware::motion::control::mpc_lateral_controller { namespace MPCUtils { @@ -192,8 +186,5 @@ void extendTrajectoryInYawDirection( const double yaw, const double interval, const bool is_forward_shift, MPCTrajectory & traj); } // namespace MPCUtils -} // namespace mpc_lateral_controller -} // namespace control -} // namespace motion -} // namespace autoware +} // namespace autoware::motion::control::mpc_lateral_controller #endif // MPC_LATERAL_CONTROLLER__MPC_UTILS_HPP_ diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_interface.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_interface.hpp index 27123abfa8beb..cb4f92c0c6774 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_interface.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_interface.hpp @@ -19,13 +19,7 @@ #include "eigen3/Eigen/Dense" #include "eigen3/Eigen/LU" -namespace autoware -{ -namespace motion -{ -namespace control -{ -namespace mpc_lateral_controller +namespace autoware::motion::control::mpc_lateral_controller { /// Interface for solvers of Quadratic Programming (QP) problems @@ -54,8 +48,5 @@ class QPSolverInterface const Eigen::VectorXd & lb, const Eigen::VectorXd & ub, const Eigen::VectorXd & lb_a, const Eigen::VectorXd & ub_a, Eigen::VectorXd & u) = 0; }; -} // namespace mpc_lateral_controller -} // namespace control -} // namespace motion -} // namespace autoware +} // namespace autoware::motion::control::mpc_lateral_controller #endif // MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_INTERFACE_HPP_ diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp index 744ec33db2e57..8b7158610b385 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp @@ -20,13 +20,7 @@ #include "osqp_interface/osqp_interface.hpp" #include "rclcpp/rclcpp.hpp" -namespace autoware -{ -namespace motion -{ -namespace control -{ -namespace mpc_lateral_controller +namespace autoware::motion::control::mpc_lateral_controller { /// Solver for QP problems using the OSQP library @@ -64,8 +58,5 @@ class QPSolverOSQP : public QPSolverInterface autoware::common::osqp::OSQPInterface osqpsolver_; rclcpp::Logger logger_; }; -} // namespace mpc_lateral_controller -} // namespace control -} // namespace motion -} // namespace autoware +} // namespace autoware::motion::control::mpc_lateral_controller #endif // MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_OSQP_HPP_ diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_unconstr_fast.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_unconstr_fast.hpp index 82d88b15465ab..0130cf6c4a279 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_unconstr_fast.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_unconstr_fast.hpp @@ -22,13 +22,7 @@ #include -namespace autoware -{ -namespace motion -{ -namespace control -{ -namespace mpc_lateral_controller +namespace autoware::motion::control::mpc_lateral_controller { /** @@ -65,8 +59,5 @@ class QPSolverEigenLeastSquareLLT : public QPSolverInterface const Eigen::VectorXd & lb, const Eigen::VectorXd & ub, const Eigen::VectorXd & lb_a, const Eigen::VectorXd & ub_a, Eigen::VectorXd & u) override; }; -} // namespace mpc_lateral_controller -} // namespace control -} // namespace motion -} // namespace autoware +} // namespace autoware::motion::control::mpc_lateral_controller #endif // MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_UNCONSTR_FAST_HPP_ diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp index 94a6326d3980a..c6f27ab3219d8 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp @@ -51,13 +51,7 @@ #include "eigen3/Eigen/LU" #include "mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp" -namespace autoware -{ -namespace motion -{ -namespace control -{ -namespace mpc_lateral_controller +namespace autoware::motion::control::mpc_lateral_controller { /** @@ -112,8 +106,5 @@ class DynamicsBicycleModel : public VehicleModelInterface double m_cf; //!< @brief front cornering power [N/rad] double m_cr; //!< @brief rear cornering power [N/rad] }; -} // namespace mpc_lateral_controller -} // namespace control -} // namespace motion -} // namespace autoware +} // namespace autoware::motion::control::mpc_lateral_controller #endif // MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_DYNAMICS_HPP_ diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp index dfd62a540a077..244ded818a1a2 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp @@ -45,13 +45,7 @@ #include "eigen3/Eigen/LU" #include "mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp" -namespace autoware -{ -namespace motion -{ -namespace control -{ -namespace mpc_lateral_controller +namespace autoware::motion::control::mpc_lateral_controller { /** @@ -96,8 +90,5 @@ class KinematicsBicycleModel : public VehicleModelInterface double m_steer_lim; //!< @brief steering angle limit [rad] double m_steer_tau; //!< @brief steering time constant for 1d-model [s] }; -} // namespace mpc_lateral_controller -} // namespace control -} // namespace motion -} // namespace autoware +} // namespace autoware::motion::control::mpc_lateral_controller #endif // MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_HPP_ diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp index 0306826344bbc..133c4df251fea 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp @@ -42,13 +42,7 @@ #include "eigen3/Eigen/LU" #include "mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp" -namespace autoware -{ -namespace motion -{ -namespace control -{ -namespace mpc_lateral_controller +namespace autoware::motion::control::mpc_lateral_controller { /** @@ -91,8 +85,5 @@ class KinematicsBicycleModelNoDelay : public VehicleModelInterface private: double m_steer_lim; //!< @brief steering angle limit [rad] }; -} // namespace mpc_lateral_controller -} // namespace control -} // namespace motion -} // namespace autoware +} // namespace autoware::motion::control::mpc_lateral_controller #endif // MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_NO_DELAY_HPP_ diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp index 86dc86941274e..36a5be08b29f8 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp @@ -17,13 +17,7 @@ #include "eigen3/Eigen/Core" -namespace autoware -{ -namespace motion -{ -namespace control -{ -namespace mpc_lateral_controller +namespace autoware::motion::control::mpc_lateral_controller { /** @@ -109,8 +103,5 @@ class VehicleModelInterface */ virtual void calculateReferenceInput(Eigen::MatrixXd & u_ref) = 0; }; -} // namespace mpc_lateral_controller -} // namespace control -} // namespace motion -} // namespace autoware +} // namespace autoware::motion::control::mpc_lateral_controller #endif // MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_ diff --git a/control/mpc_lateral_controller/src/interpolate.cpp b/control/mpc_lateral_controller/src/interpolate.cpp index e3309fed61652..b261b2818f052 100644 --- a/control/mpc_lateral_controller/src/interpolate.cpp +++ b/control/mpc_lateral_controller/src/interpolate.cpp @@ -22,13 +22,7 @@ * linear interpolation */ -namespace autoware -{ -namespace motion -{ -namespace control -{ -namespace mpc_lateral_controller +namespace autoware::motion::control::mpc_lateral_controller { namespace { @@ -137,7 +131,4 @@ bool linearInterpolate( return_value = return_value_v.at(0); return true; } -} // namespace mpc_lateral_controller -} // namespace control -} // namespace motion -} // namespace autoware +} // namespace autoware::motion::control::mpc_lateral_controller diff --git a/control/mpc_lateral_controller/src/longitudinal_controller_utils.cpp b/control/mpc_lateral_controller/src/longitudinal_controller_utils.cpp index 6f65171e2c80a..70c610433fa06 100644 --- a/control/mpc_lateral_controller/src/longitudinal_controller_utils.cpp +++ b/control/mpc_lateral_controller/src/longitudinal_controller_utils.cpp @@ -28,13 +28,7 @@ #include #include -namespace autoware -{ -namespace motion -{ -namespace control -{ -namespace mpc_lateral_controller +namespace autoware::motion::control::mpc_lateral_controller { namespace longitudinal_utils { @@ -182,7 +176,4 @@ double applyDiffLimitFilter( return applyDiffLimitFilter(input_val, prev_val, dt, max_val, min_val); } } // namespace longitudinal_utils -} // namespace mpc_lateral_controller -} // namespace control -} // namespace motion -} // namespace autoware +} // namespace autoware::motion::control::mpc_lateral_controller diff --git a/control/mpc_lateral_controller/src/lowpass_filter.cpp b/control/mpc_lateral_controller/src/lowpass_filter.cpp index 40097e46cd1c4..c40ee6987a6d2 100644 --- a/control/mpc_lateral_controller/src/lowpass_filter.cpp +++ b/control/mpc_lateral_controller/src/lowpass_filter.cpp @@ -16,13 +16,7 @@ #include -namespace autoware -{ -namespace motion -{ -namespace control -{ -namespace mpc_lateral_controller +namespace autoware::motion::control::mpc_lateral_controller { Butterworth2dFilter::Butterworth2dFilter(double dt, double f_cutoff_hz) { @@ -142,7 +136,4 @@ bool filt_vector(const int num, std::vector & u) return true; } } // namespace MoveAverageFilter -} // namespace mpc_lateral_controller -} // namespace control -} // namespace motion -} // namespace autoware +} // namespace autoware::motion::control::mpc_lateral_controller diff --git a/control/mpc_lateral_controller/src/mpc.cpp b/control/mpc_lateral_controller/src/mpc.cpp index 5a3d56319a7be..f5da49b1d5a82 100644 --- a/control/mpc_lateral_controller/src/mpc.cpp +++ b/control/mpc_lateral_controller/src/mpc.cpp @@ -27,13 +27,7 @@ #define DEG2RAD 3.1415926535 / 180.0 #define RAD2DEG 180.0 / 3.1415926535 -namespace autoware -{ -namespace motion -{ -namespace control -{ -namespace mpc_lateral_controller +namespace autoware::motion::control::mpc_lateral_controller { using namespace std::literals::chrono_literals; @@ -866,7 +860,4 @@ bool MPC::isValid(const MPCMatrix & m) const return true; } -} // namespace mpc_lateral_controller -} // namespace control -} // namespace motion -} // namespace autoware +} // namespace autoware::motion::control::mpc_lateral_controller diff --git a/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp b/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp index 9ccb35bac795d..80f05255079e9 100644 --- a/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp +++ b/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp @@ -25,18 +25,10 @@ #include #include -namespace autoware -{ -namespace motion -{ -namespace control -{ -namespace mpc_lateral_controller +namespace autoware::motion::control::mpc_lateral_controller { namespace { -using namespace std::literals::chrono_literals; - template void update_param( const std::vector & parameters, const std::string & name, T & value) @@ -525,7 +517,4 @@ bool MpcLateralController::isValidTrajectory( return true; } -} // namespace mpc_lateral_controller -} // namespace control -} // namespace motion -} // namespace autoware +} // namespace autoware::motion::control::mpc_lateral_controller diff --git a/control/mpc_lateral_controller/src/mpc_trajectory.cpp b/control/mpc_lateral_controller/src/mpc_trajectory.cpp index 810d7eaf7997f..60eded2b22637 100644 --- a/control/mpc_lateral_controller/src/mpc_trajectory.cpp +++ b/control/mpc_lateral_controller/src/mpc_trajectory.cpp @@ -14,13 +14,7 @@ #include "mpc_lateral_controller/mpc_trajectory.hpp" -namespace autoware -{ -namespace motion -{ -namespace control -{ -namespace mpc_lateral_controller +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, @@ -60,7 +54,4 @@ size_t MPCTrajectory::size() const return 0; } } -} // namespace mpc_lateral_controller -} // namespace control -} // namespace motion -} // namespace autoware +} // namespace autoware::motion::control::mpc_lateral_controller diff --git a/control/mpc_lateral_controller/src/mpc_utils.cpp b/control/mpc_lateral_controller/src/mpc_utils.cpp index 142dfc50f0751..4f408505bbeb1 100644 --- a/control/mpc_lateral_controller/src/mpc_utils.cpp +++ b/control/mpc_lateral_controller/src/mpc_utils.cpp @@ -22,13 +22,7 @@ #include #include -namespace autoware -{ -namespace motion -{ -namespace control -{ -namespace mpc_lateral_controller +namespace autoware::motion::control::mpc_lateral_controller { namespace MPCUtils { @@ -429,7 +423,4 @@ void extendTrajectoryInYawDirection( } } // namespace MPCUtils -} // namespace mpc_lateral_controller -} // namespace control -} // namespace motion -} // namespace autoware +} // namespace autoware::motion::control::mpc_lateral_controller diff --git a/control/mpc_lateral_controller/src/qp_solver/qp_solver_osqp.cpp b/control/mpc_lateral_controller/src/qp_solver/qp_solver_osqp.cpp index a90c4457e36a5..dbb11e7c0e100 100644 --- a/control/mpc_lateral_controller/src/qp_solver/qp_solver_osqp.cpp +++ b/control/mpc_lateral_controller/src/qp_solver/qp_solver_osqp.cpp @@ -17,13 +17,7 @@ #include #include -namespace autoware -{ -namespace motion -{ -namespace control -{ -namespace mpc_lateral_controller +namespace autoware::motion::control::mpc_lateral_controller { QPSolverOSQP::QPSolverOSQP(const rclcpp::Logger & logger) : logger_{logger} {} bool QPSolverOSQP::solve( @@ -80,7 +74,4 @@ bool QPSolverOSQP::solve( } return true; } -} // namespace mpc_lateral_controller -} // namespace control -} // namespace motion -} // namespace autoware +} // namespace autoware::motion::control::mpc_lateral_controller diff --git a/control/mpc_lateral_controller/src/qp_solver/qp_solver_unconstr_fast.cpp b/control/mpc_lateral_controller/src/qp_solver/qp_solver_unconstr_fast.cpp index 7208473ccef6d..33cd60fe0f318 100644 --- a/control/mpc_lateral_controller/src/qp_solver/qp_solver_unconstr_fast.cpp +++ b/control/mpc_lateral_controller/src/qp_solver/qp_solver_unconstr_fast.cpp @@ -14,13 +14,7 @@ #include "mpc_lateral_controller/qp_solver/qp_solver_unconstr_fast.hpp" -namespace autoware -{ -namespace motion -{ -namespace control -{ -namespace mpc_lateral_controller +namespace autoware::motion::control::mpc_lateral_controller { QPSolverEigenLeastSquareLLT::QPSolverEigenLeastSquareLLT() {} bool QPSolverEigenLeastSquareLLT::solve( @@ -36,7 +30,4 @@ bool QPSolverEigenLeastSquareLLT::solve( return true; } -} // namespace mpc_lateral_controller -} // namespace control -} // namespace motion -} // namespace autoware +} // namespace autoware::motion::control::mpc_lateral_controller diff --git a/control/mpc_lateral_controller/src/smooth_stop.cpp b/control/mpc_lateral_controller/src/smooth_stop.cpp index 26cfcabfebead..564acd62c0d82 100644 --- a/control/mpc_lateral_controller/src/smooth_stop.cpp +++ b/control/mpc_lateral_controller/src/smooth_stop.cpp @@ -22,13 +22,7 @@ #include #include -namespace autoware -{ -namespace motion -{ -namespace control -{ -namespace mpc_lateral_controller +namespace autoware::motion::control::mpc_lateral_controller { void SmoothStop::init(const double pred_vel_in_target, const double pred_stop_dist) { @@ -164,7 +158,4 @@ double SmoothStop::calculate( // when the car is not running return m_params.strong_stop_acc; } -} // namespace mpc_lateral_controller -} // namespace control -} // namespace motion -} // namespace autoware +} // namespace autoware::motion::control::mpc_lateral_controller diff --git a/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_dynamics.cpp b/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_dynamics.cpp index a634fa02232b4..576907e79da4b 100644 --- a/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_dynamics.cpp +++ b/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_dynamics.cpp @@ -16,13 +16,7 @@ #include -namespace autoware -{ -namespace motion -{ -namespace control -{ -namespace mpc_lateral_controller +namespace autoware::motion::control::mpc_lateral_controller { DynamicsBicycleModel::DynamicsBicycleModel( const double wheelbase, const double mass_fl, const double mass_fr, const double mass_rl, @@ -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 mpc_lateral_controller -} // namespace control -} // namespace motion -} // namespace autoware +} // namespace autoware::motion::control::mpc_lateral_controller diff --git a/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp b/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp index 9955ce43fcb1b..35bef667125c5 100644 --- a/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp +++ b/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp @@ -16,13 +16,7 @@ #include -namespace autoware -{ -namespace motion -{ -namespace control -{ -namespace mpc_lateral_controller +namespace autoware::motion::control::mpc_lateral_controller { KinematicsBicycleModel::KinematicsBicycleModel( const double wheelbase, const double steer_lim, const double steer_tau) @@ -74,7 +68,4 @@ void KinematicsBicycleModel::calculateReferenceInput(Eigen::MatrixXd & u_ref) { u_ref(0, 0) = std::atan(m_wheelbase * m_curvature); } -} // namespace mpc_lateral_controller -} // namespace control -} // namespace motion -} // namespace autoware +} // namespace autoware::motion::control::mpc_lateral_controller diff --git a/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp b/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp index 7cd94e1406e10..1536beba9cd00 100644 --- a/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp +++ b/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp @@ -16,13 +16,7 @@ #include -namespace autoware -{ -namespace motion -{ -namespace control -{ -namespace mpc_lateral_controller +namespace autoware::motion::control::mpc_lateral_controller { KinematicsBicycleModelNoDelay::KinematicsBicycleModelNoDelay( const double wheelbase, const double steer_lim) @@ -65,7 +59,4 @@ void KinematicsBicycleModelNoDelay::calculateReferenceInput(Eigen::MatrixXd & u_ { u_ref(0, 0) = std::atan(m_wheelbase * m_curvature); } -} // namespace mpc_lateral_controller -} // namespace control -} // namespace motion -} // namespace autoware +} // namespace autoware::motion::control::mpc_lateral_controller diff --git a/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_interface.cpp b/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_interface.cpp index 432e98a672e88..c3e276f628e93 100644 --- a/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_interface.cpp +++ b/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_interface.cpp @@ -14,13 +14,7 @@ #include "mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp" -namespace autoware -{ -namespace motion -{ -namespace control -{ -namespace mpc_lateral_controller +namespace autoware::motion::control::mpc_lateral_controller { VehicleModelInterface::VehicleModelInterface(int dim_x, int dim_u, int dim_y, double wheelbase) : m_dim_x(dim_x), m_dim_u(dim_u), m_dim_y(dim_y), m_wheelbase(wheelbase) @@ -32,7 +26,4 @@ int VehicleModelInterface::getDimY() { return m_dim_y; } double VehicleModelInterface::getWheelbase() { return m_wheelbase; } void VehicleModelInterface::setVelocity(const double velocity) { m_velocity = velocity; } void VehicleModelInterface::setCurvature(const double curvature) { m_curvature = curvature; } -} // namespace mpc_lateral_controller -} // namespace control -} // namespace motion -} // namespace autoware +} // namespace autoware::motion::control::mpc_lateral_controller diff --git a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/debug_values.hpp b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/debug_values.hpp index 2f20f28dd6da9..f1d7dab6c5b09 100644 --- a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/debug_values.hpp +++ b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/debug_values.hpp @@ -17,13 +17,7 @@ #include -namespace autoware -{ -namespace motion -{ -namespace control -{ -namespace pid_longitudinal_controller +namespace autoware::motion::control::pid_longitudinal_controller { /// Debug Values used for debugging or controller tuning @@ -94,9 +88,6 @@ class DebugValues private: std::array(TYPE::SIZE)> m_values; }; -} // namespace pid_longitudinal_controller -} // namespace control -} // namespace motion -} // namespace autoware +} // namespace autoware::motion::control::pid_longitudinal_controller #endif // PID_LONGITUDINAL_CONTROLLER__DEBUG_VALUES_HPP_ diff --git a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp index 32928d64a12b8..afb02acfc2934 100644 --- a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp +++ b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp @@ -30,13 +30,7 @@ #include #include -namespace autoware -{ -namespace motion -{ -namespace control -{ -namespace pid_longitudinal_controller +namespace autoware::motion::control::pid_longitudinal_controller { namespace longitudinal_utils { @@ -156,9 +150,6 @@ double applyDiffLimitFilter( const double input_val, const double prev_val, const double dt, const double max_val, const double min_val); } // namespace longitudinal_utils -} // namespace pid_longitudinal_controller -} // namespace control -} // namespace motion -} // namespace autoware +} // namespace autoware::motion::control::pid_longitudinal_controller #endif // PID_LONGITUDINAL_CONTROLLER__LONGITUDINAL_CONTROLLER_UTILS_HPP_ diff --git a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/lowpass_filter.hpp b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/lowpass_filter.hpp index 6215ee7a15826..06fe1793c8123 100644 --- a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/lowpass_filter.hpp +++ b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/lowpass_filter.hpp @@ -20,13 +20,7 @@ #include #include -namespace autoware -{ -namespace motion -{ -namespace control -{ -namespace pid_longitudinal_controller +namespace autoware::motion::control::pid_longitudinal_controller { /** @@ -68,8 +62,5 @@ class LowpassFilter1d return ret; } }; -} // namespace pid_longitudinal_controller -} // namespace control -} // namespace motion -} // namespace autoware +} // namespace autoware::motion::control::pid_longitudinal_controller #endif // PID_LONGITUDINAL_CONTROLLER__LOWPASS_FILTER_HPP_ diff --git a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid.hpp b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid.hpp index b261191075e2c..8b981c0485ed9 100644 --- a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid.hpp +++ b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid.hpp @@ -17,13 +17,7 @@ #include -namespace autoware -{ -namespace motion -{ -namespace control -{ -namespace pid_longitudinal_controller +namespace autoware::motion::control::pid_longitudinal_controller { /// @brief implementation of a PID controller @@ -95,9 +89,6 @@ class PIDController bool m_is_gains_set; bool m_is_limits_set; }; -} // namespace pid_longitudinal_controller -} // namespace control -} // namespace motion -} // namespace autoware +} // namespace autoware::motion::control::pid_longitudinal_controller #endif // PID_LONGITUDINAL_CONTROLLER__PID_HPP_ diff --git a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp index ce4aa88bc7a10..505fede84647c 100644 --- a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp +++ b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp @@ -44,13 +44,7 @@ #include #include -namespace autoware -{ -namespace motion -{ -namespace control -{ -namespace pid_longitudinal_controller +namespace autoware::motion::control::pid_longitudinal_controller { namespace trajectory_follower = ::autoware::motion::control::trajectory_follower; @@ -379,9 +373,6 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro const Motion & ctrl_cmd, const geometry_msgs::msg::Pose & current_pose, const ControlData & control_data); }; -} // namespace pid_longitudinal_controller -} // namespace control -} // namespace motion -} // namespace autoware +} // namespace autoware::motion::control::pid_longitudinal_controller #endif // PID_LONGITUDINAL_CONTROLLER__PID_LONGITUDINAL_CONTROLLER_HPP_ diff --git a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/smooth_stop.hpp b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/smooth_stop.hpp index 45c042dc026be..9a0a36123e143 100644 --- a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/smooth_stop.hpp +++ b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/smooth_stop.hpp @@ -25,13 +25,7 @@ #include #include -namespace autoware -{ -namespace motion -{ -namespace control -{ -namespace pid_longitudinal_controller +namespace autoware::motion::control::pid_longitudinal_controller { /** @@ -116,9 +110,6 @@ class SmoothStop rclcpp::Time m_weak_acc_time; bool m_is_set_params = false; }; -} // namespace pid_longitudinal_controller -} // namespace control -} // namespace motion -} // namespace autoware +} // namespace autoware::motion::control::pid_longitudinal_controller #endif // PID_LONGITUDINAL_CONTROLLER__SMOOTH_STOP_HPP_ diff --git a/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp b/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp index a056acc901d8d..a6affe01ed034 100644 --- a/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp +++ b/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp @@ -28,13 +28,7 @@ #include #include -namespace autoware -{ -namespace motion -{ -namespace control -{ -namespace pid_longitudinal_controller +namespace autoware::motion::control::pid_longitudinal_controller { namespace longitudinal_utils { @@ -182,7 +176,4 @@ double applyDiffLimitFilter( return applyDiffLimitFilter(input_val, prev_val, dt, max_val, min_val); } } // namespace longitudinal_utils -} // namespace pid_longitudinal_controller -} // namespace control -} // namespace motion -} // namespace autoware +} // namespace autoware::motion::control::pid_longitudinal_controller diff --git a/control/pid_longitudinal_controller/src/pid.cpp b/control/pid_longitudinal_controller/src/pid.cpp index b08bb78455a10..530b5cd15e754 100644 --- a/control/pid_longitudinal_controller/src/pid.cpp +++ b/control/pid_longitudinal_controller/src/pid.cpp @@ -20,13 +20,7 @@ #include #include -namespace autoware -{ -namespace motion -{ -namespace control -{ -namespace pid_longitudinal_controller +namespace autoware::motion::control::pid_longitudinal_controller { PIDController::PIDController() : m_error_integral(0.0), @@ -108,7 +102,4 @@ void PIDController::reset() m_prev_error = 0.0; m_is_first_time = true; } -} // namespace pid_longitudinal_controller -} // namespace control -} // namespace motion -} // namespace autoware +} // namespace autoware::motion::control::pid_longitudinal_controller diff --git a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp index 5f8031c425cfd..01641244cc14e 100644 --- a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp +++ b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp @@ -24,13 +24,7 @@ #include #include -namespace autoware -{ -namespace motion -{ -namespace control -{ -namespace pid_longitudinal_controller +namespace autoware::motion::control::pid_longitudinal_controller { PidLongitudinalController::PidLongitudinalController(rclcpp::Node & node) : node_{&node}, diagnostic_updater_(&node) @@ -102,8 +96,7 @@ PidLongitudinalController::PidLongitudinalController(rclcpp::Node & node) // set lowpass filter for vel error and pitch const double lpf_vel_error_gain{node_->declare_parameter("lpf_vel_error_gain")}; - m_lpf_vel_error = - std::make_shared(0.0, lpf_vel_error_gain); + m_lpf_vel_error = std::make_shared(0.0, lpf_vel_error_gain); m_current_vel_threshold_pid_integrate = node_->declare_parameter("current_vel_threshold_pid_integration"); // [m/s] @@ -171,7 +164,7 @@ PidLongitudinalController::PidLongitudinalController(rclcpp::Node & node) // parameters for slope compensation m_use_traj_for_pitch = node_->declare_parameter("use_trajectory_for_pitch_calculation"); const double lpf_pitch_gain{node_->declare_parameter("lpf_pitch_gain")}; - m_lpf_pitch = std::make_shared(0.0, lpf_pitch_gain); + m_lpf_pitch = std::make_shared(0.0, lpf_pitch_gain); m_max_pitch_rad = node_->declare_parameter("max_pitch_rad"); // [rad] m_min_pitch_rad = node_->declare_parameter("min_pitch_rad"); // [rad] @@ -213,7 +206,7 @@ void PidLongitudinalController::setCurrentAcceleration( void PidLongitudinalController::setTrajectory( const autoware_auto_planning_msgs::msg::Trajectory & msg) { - if (!pid_longitudinal_controller::longitudinal_utils::isValidTrajectory(msg)) { + if (!longitudinal_utils::isValidTrajectory(msg)) { RCLCPP_ERROR_THROTTLE( node_->get_logger(), *node_->get_clock(), 3000, "received invalid trajectory. ignore."); return; @@ -447,14 +440,13 @@ PidLongitudinalController::ControlData PidLongitudinalController::getControlData m_prev_shift = control_data.shift; // distance to stopline - control_data.stop_dist = pid_longitudinal_controller::longitudinal_utils::calcStopDistance( + control_data.stop_dist = longitudinal_utils::calcStopDistance( current_pose, m_trajectory, m_ego_nearest_dist_threshold, m_ego_nearest_yaw_threshold); // pitch - const double raw_pitch = - pid_longitudinal_controller::longitudinal_utils::getPitchByPose(current_pose.orientation); - const double traj_pitch = pid_longitudinal_controller::longitudinal_utils::getPitchByTraj( - m_trajectory, control_data.nearest_idx, m_wheel_base); + const double raw_pitch = longitudinal_utils::getPitchByPose(current_pose.orientation); + const double traj_pitch = + longitudinal_utils::getPitchByTraj(m_trajectory, control_data.nearest_idx, m_wheel_base); control_data.slope_angle = m_use_traj_for_pitch ? traj_pitch : m_lpf_pitch->filter(raw_pitch); updatePitchDebugValues(control_data.slope_angle, traj_pitch, raw_pitch); @@ -466,10 +458,10 @@ PidLongitudinalController::Motion PidLongitudinalController::calcEmergencyCtrlCm { // These accelerations are without slope compensation const auto & p = m_emergency_state_params; - const double vel = pid_longitudinal_controller::longitudinal_utils::applyDiffLimitFilter( - p.vel, m_prev_raw_ctrl_cmd.vel, dt, p.acc); - const double acc = pid_longitudinal_controller::longitudinal_utils::applyDiffLimitFilter( - p.acc, m_prev_raw_ctrl_cmd.acc, dt, p.jerk); + const double vel = + longitudinal_utils::applyDiffLimitFilter(p.vel, m_prev_raw_ctrl_cmd.vel, dt, p.acc); + const double acc = + longitudinal_utils::applyDiffLimitFilter(p.acc, m_prev_raw_ctrl_cmd.acc, dt, p.jerk); RCLCPP_ERROR_THROTTLE( node_->get_logger(), *node_->get_clock(), 3000, "[Emergency stop] vel: %3.3f, acc: %3.3f", vel, @@ -622,9 +614,8 @@ PidLongitudinalController::Motion PidLongitudinalController::calcCtrlCmd( Motion raw_ctrl_cmd{}; Motion target_motion{}; if (m_control_state == ControlState::DRIVE) { - const auto target_pose = - pid_longitudinal_controller::longitudinal_utils::calcPoseAfterTimeDelay( - current_pose, m_delay_compensation_time, current_vel); + const auto target_pose = longitudinal_utils::calcPoseAfterTimeDelay( + current_pose, m_delay_compensation_time, current_vel); const auto target_interpolated_point = calcInterpolatedTargetValue(m_trajectory, target_pose); target_motion = Motion{ target_interpolated_point.longitudinal_velocity_mps, @@ -634,8 +625,7 @@ PidLongitudinalController::Motion PidLongitudinalController::calcCtrlCmd( const double pred_vel_in_target = predictedVelocityInTargetPoint(control_data.current_motion, m_delay_compensation_time); - m_debug_values.setValues( - pid_longitudinal_controller::DebugValues::TYPE::PREDICTED_VEL, pred_vel_in_target); + m_debug_values.setValues(DebugValues::TYPE::PREDICTED_VEL, pred_vel_in_target); raw_ctrl_cmd.vel = target_motion.vel; raw_ctrl_cmd.acc = applyVelocityFeedback(target_motion, control_data.dt, pred_vel_in_target); @@ -657,7 +647,7 @@ PidLongitudinalController::Motion PidLongitudinalController::calcCtrlCmd( // This acceleration is without slope compensation const auto & p = m_stopped_state_params; raw_ctrl_cmd.vel = p.vel; - raw_ctrl_cmd.acc = pid_longitudinal_controller::longitudinal_utils::applyDiffLimitFilter( + raw_ctrl_cmd.acc = longitudinal_utils::applyDiffLimitFilter( p.acc, m_prev_raw_ctrl_cmd.acc, control_data.dt, p.jerk); RCLCPP_DEBUG( @@ -703,7 +693,6 @@ autoware_auto_control_msgs::msg::LongitudinalCommand PidLongitudinalController:: void PidLongitudinalController::publishDebugData( const Motion & ctrl_cmd, const ControlData & control_data) { - using pid_longitudinal_controller::DebugValues; // set debug values m_debug_values.setValues(DebugValues::TYPE::DT, control_data.dt); m_debug_values.setValues(DebugValues::TYPE::CALCULATED_ACC, control_data.current_motion.acc); @@ -762,7 +751,6 @@ enum PidLongitudinalController::Shift PidLongitudinalController::getCurrentShift double PidLongitudinalController::calcFilteredAcc( const double raw_acc, const ControlData & control_data) { - using pid_longitudinal_controller::DebugValues; const double acc_max_filtered = std::clamp(raw_acc, m_min_acc, m_max_acc); m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_ACC_LIMITED, acc_max_filtered); @@ -774,9 +762,8 @@ double PidLongitudinalController::calcFilteredAcc( m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_SLOPE_APPLIED, acc_slope_filtered); // This jerk filter must be applied after slope compensation - const double acc_jerk_filtered = - pid_longitudinal_controller::longitudinal_utils::applyDiffLimitFilter( - acc_slope_filtered, m_prev_ctrl_cmd.acc, control_data.dt, m_max_jerk, m_min_jerk); + const double acc_jerk_filtered = longitudinal_utils::applyDiffLimitFilter( + acc_slope_filtered, m_prev_ctrl_cmd.acc, control_data.dt, m_max_jerk, m_min_jerk); m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_JERK_LIMITED, acc_jerk_filtered); return acc_jerk_filtered; @@ -864,7 +851,7 @@ PidLongitudinalController::calcInterpolatedTargetValue( } // apply linear interpolation - return pid_longitudinal_controller::longitudinal_utils::lerpTrajectoryPoint( + return longitudinal_utils::lerpTrajectoryPoint( traj.points, pose, m_ego_nearest_dist_threshold, m_ego_nearest_yaw_threshold); } @@ -919,7 +906,6 @@ double PidLongitudinalController::predictedVelocityInTargetPoint( double PidLongitudinalController::applyVelocityFeedback( const Motion target_motion, const double dt, const double current_vel) { - using pid_longitudinal_controller::DebugValues; const double current_vel_abs = std::fabs(current_vel); const double target_vel_abs = std::fabs(target_motion.vel); const bool enable_integration = (current_vel_abs > m_current_vel_threshold_pid_integrate); @@ -932,12 +918,9 @@ double PidLongitudinalController::applyVelocityFeedback( m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_PID_APPLIED, feedback_acc); m_debug_values.setValues(DebugValues::TYPE::ERROR_VEL_FILTERED, error_vel_filtered); - m_debug_values.setValues( - DebugValues::TYPE::ACC_CMD_FB_P_CONTRIBUTION, pid_contributions.at(0)); // P - m_debug_values.setValues( - DebugValues::TYPE::ACC_CMD_FB_I_CONTRIBUTION, pid_contributions.at(1)); // I - m_debug_values.setValues( - DebugValues::TYPE::ACC_CMD_FB_D_CONTRIBUTION, pid_contributions.at(2)); // D + m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_FB_P_CONTRIBUTION, pid_contributions.at(0)); + m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_FB_I_CONTRIBUTION, pid_contributions.at(1)); + m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_FB_D_CONTRIBUTION, pid_contributions.at(2)); return feedback_acc; } @@ -945,7 +928,6 @@ double PidLongitudinalController::applyVelocityFeedback( void PidLongitudinalController::updatePitchDebugValues( const double pitch, const double traj_pitch, const double raw_pitch) { - using pid_longitudinal_controller::DebugValues; const double to_degrees = (180.0 / static_cast(M_PI)); m_debug_values.setValues(DebugValues::TYPE::PITCH_LPF_RAD, pitch); m_debug_values.setValues(DebugValues::TYPE::PITCH_LPF_DEG, pitch * to_degrees); @@ -959,7 +941,6 @@ void PidLongitudinalController::updateDebugVelAcc( const Motion & target_motion, const geometry_msgs::msg::Pose & current_pose, const ControlData & control_data) { - using pid_longitudinal_controller::DebugValues; const double current_vel = control_data.current_motion.vel; const auto interpolated_point = calcInterpolatedTargetValue(m_trajectory, current_pose); @@ -1012,7 +993,4 @@ void PidLongitudinalController::checkControlState( stat.summary(level, msg); } -} // namespace pid_longitudinal_controller -} // namespace control -} // namespace motion -} // namespace autoware +} // namespace autoware::motion::control::pid_longitudinal_controller diff --git a/control/pid_longitudinal_controller/src/smooth_stop.cpp b/control/pid_longitudinal_controller/src/smooth_stop.cpp index db8d81b837012..27fab43fa506f 100644 --- a/control/pid_longitudinal_controller/src/smooth_stop.cpp +++ b/control/pid_longitudinal_controller/src/smooth_stop.cpp @@ -22,13 +22,7 @@ #include #include -namespace autoware -{ -namespace motion -{ -namespace control -{ -namespace pid_longitudinal_controller +namespace autoware::motion::control::pid_longitudinal_controller { void SmoothStop::init(const double pred_vel_in_target, const double pred_stop_dist) { @@ -164,7 +158,4 @@ double SmoothStop::calculate( // when the car is not running return m_params.strong_stop_acc; } -} // namespace pid_longitudinal_controller -} // namespace control -} // namespace motion -} // namespace autoware +} // namespace autoware::motion::control::pid_longitudinal_controller From 4b26217166b431688b6b412b973d103df547bf95 Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Wed, 11 Jan 2023 11:41:10 +0900 Subject: [PATCH 02/49] feat(lane_change): update resamplePath function for target section (#2622) * feat(lane_change): update resamplePath function for target section Signed-off-by: Takamasa Horibe * update Signed-off-by: Takamasa Horibe * fix: apply pre-commit Signed-off-by: tomoya.kimura Signed-off-by: Takamasa Horibe Signed-off-by: tomoya.kimura Co-authored-by: tomoya.kimura --- .../include/behavior_path_planner/path_utilities.hpp | 12 +++++++++++- .../behavior_path_planner/src/path_utilities.cpp | 12 +++++++----- .../src/scene_module/lane_change/util.cpp | 3 ++- 3 files changed, 20 insertions(+), 7 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/path_utilities.hpp b/planning/behavior_path_planner/include/behavior_path_planner/path_utilities.hpp index 2d6f44633cc5d..09269e8cb7456 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/path_utilities.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/path_utilities.hpp @@ -41,8 +41,18 @@ std::vector calcPathArcLengthArray( const PathWithLaneId & path, const size_t start = 0, const size_t end = std::numeric_limits::max(), const double offset = 0.0); +/** + * @brief resample path by spline with constant interval distance + * @param [in] path original path to be resampled + * @param [in] interval constant interval distance + * @param [in] keep_input_points original points are kept in the resampled points + * @param [in] target_section target section defined by arclength if you want to resample a part of + * the path + * @return resampled path + */ PathWithLaneId resamplePathWithSpline( - const PathWithLaneId & path, const double interval, const bool keep_input_points = false); + const PathWithLaneId & path, const double interval, const bool keep_input_points = false, + const std::pair target_section = {0.0, std::numeric_limits::max()}); Path toPath(const PathWithLaneId & input); diff --git a/planning/behavior_path_planner/src/path_utilities.cpp b/planning/behavior_path_planner/src/path_utilities.cpp index 457687a6667d3..659006752f45e 100644 --- a/planning/behavior_path_planner/src/path_utilities.cpp +++ b/planning/behavior_path_planner/src/path_utilities.cpp @@ -56,7 +56,8 @@ std::vector calcPathArcLengthArray( * @brief resamplePathWithSpline */ PathWithLaneId resamplePathWithSpline( - const PathWithLaneId & path, const double interval, const bool keep_input_points) + const PathWithLaneId & path, const double interval, const bool keep_input_points, + const std::pair target_section) { if (path.points.size() < 2) { return path; @@ -96,16 +97,17 @@ PathWithLaneId resamplePathWithSpline( std::vector s_out = s_in; - const double path_len = motion_utils::calcArcLength(transformed_path); - for (double s = 0.0; s < path_len; s += interval) { + const auto start_s = std::max(target_section.first, 0.0); + const auto end_s = std::min(target_section.second, motion_utils::calcArcLength(transformed_path)); + for (double s = start_s; s < end_s; s += interval) { if (!has_almost_same_value(s_out, s)) { s_out.push_back(s); } } // Insert Terminal Point - if (!has_almost_same_value(s_out, path_len)) { - s_out.push_back(path_len); + if (!has_almost_same_value(s_out, end_s)) { + s_out.push_back(end_s); } // Insert Stop Point diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp index 35d89fc91322e..bc6afcdfee65b 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp @@ -546,7 +546,8 @@ PathWithLaneId getReferencePathFromTargetLane( constexpr auto resampling_dt{0.2}; const auto resample_interval = std::max(lane_changing_distance / min_resampling_points, lane_changing_speed * resampling_dt); - return util::resamplePathWithSpline(lane_changing_reference_path, resample_interval); + return util::resamplePathWithSpline( + lane_changing_reference_path, resample_interval, true, {0.0, lane_changing_distance}); } PathWithLaneId getReferencePathFromTargetLane( From 72470412a319238f84e781799f133d832a8c8f5c Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Wed, 11 Jan 2023 11:53:36 +0900 Subject: [PATCH 03/49] feat(tier4_state_rviz_plugin): add Fail Safe Visualization (#2626) * feat(tier4_state_rviz_plugin): add information for Fail Safe * fix color * ci(pre-commit): autofix Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../src/autoware_state_panel.cpp | 92 +++++++++++++++++++ .../src/autoware_state_panel.hpp | 12 +++ 2 files changed, 104 insertions(+) diff --git a/common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp b/common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp index 76d204602d299..0b9225bf94238 100644 --- a/common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp +++ b/common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp @@ -64,6 +64,7 @@ AutowareStatePanel::AutowareStatePanel(QWidget * parent) : rviz_common::Panel(pa h_layout->addWidget(makeRoutingGroup()); h_layout->addWidget(makeLocalizationGroup()); h_layout->addWidget(makeMotionGroup()); + h_layout->addWidget(makeFailSafeGroup()); v_layout->addLayout(h_layout); } @@ -186,6 +187,25 @@ QGroupBox * AutowareStatePanel::makeMotionGroup() return group; } +QGroupBox * AutowareStatePanel::makeFailSafeGroup() +{ + auto * group = new QGroupBox("FalSafe"); + auto * grid = new QGridLayout; + + mrm_state_label_ptr_ = new QLabel("INIT"); + mrm_state_label_ptr_->setAlignment(Qt::AlignCenter); + mrm_state_label_ptr_->setStyleSheet("border:1px solid black;"); + grid->addWidget(mrm_state_label_ptr_, 0, 0); + + mrm_behavior_label_ptr_ = new QLabel("INIT"); + mrm_behavior_label_ptr_->setAlignment(Qt::AlignCenter); + mrm_behavior_label_ptr_->setStyleSheet("border:1px solid black;"); + grid->addWidget(mrm_behavior_label_ptr_, 1, 0); + + group->setLayout(grid); + return group; +} + void AutowareStatePanel::onInitialize() { raw_node_ = this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); @@ -234,6 +254,12 @@ void AutowareStatePanel::onInitialize() client_accept_start_ = raw_node_->create_client( "/api/motion/accept_start", rmw_qos_profile_services_default); + // FailSafe + sub_mrm_ = raw_node_->create_subscription( + "/api/fail_safe/mrm_state", rclcpp::QoS{1}.transient_local(), + std::bind(&AutowareStatePanel::onMRMState, this, _1)); + + // Others sub_gear_ = raw_node_->create_subscription( "/vehicle/status/gear_status", 10, std::bind(&AutowareStatePanel::onShift, this, _1)); @@ -421,6 +447,72 @@ void AutowareStatePanel::onMotion(const MotionState::ConstSharedPtr msg) } } +void AutowareStatePanel::onMRMState(const MRMState::ConstSharedPtr msg) +{ + // state + { + QString text = ""; + QString style_sheet = ""; + switch (msg->state) { + case MRMState::NONE: + text = "NONE"; + style_sheet = "background-color: #00FF00;"; // green + break; + + case MRMState::MRM_OPERATING: + text = "MRM_OPERATING"; + style_sheet = "background-color: #FFA500;"; // orange + break; + + case MRMState::MRM_SUCCEEDED: + text = "MRM_SUCCEEDED"; + style_sheet = "background-color: #FFFF00;"; // yellow + break; + + case MRMState::MRM_FAILED: + text = "MRM_FAILED"; + style_sheet = "background-color: #FF0000;"; // red + break; + + default: + text = "UNKNOWN"; + style_sheet = "background-color: #FF0000;"; // red + break; + } + + updateLabel(mrm_state_label_ptr_, text, style_sheet); + } + + // behavior + { + QString text = ""; + QString style_sheet = ""; + switch (msg->state) { + case MRMState::NONE: + text = "NONE"; + style_sheet = "background-color: #00FF00;"; // green + break; + + case MRMState::COMFORTABLE_STOP: + text = "COMFORTABLE_STOP"; + style_sheet = "background-color: #FFFF00;"; // yellow + break; + + case MRMState::EMERGENCY_STOP: + text = "EMERGENCY_STOP"; + style_sheet = "background-color: #FFA500;"; // orange + break; + + default: + text = "UNKNOWN"; + style_sheet = "background-color: #FF0000;"; // red + break; + } + + updateLabel(mrm_behavior_label_ptr_, text, style_sheet); + } +} + void AutowareStatePanel::onShift( const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr msg) { diff --git a/common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp b/common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp index 47e1cfc5c1195..b3e9b08152b16 100644 --- a/common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp +++ b/common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp @@ -27,6 +27,7 @@ #include #include +#include #include #include #include @@ -51,6 +52,7 @@ class AutowareStatePanel : public rviz_common::Panel autoware_adapi_v1_msgs::msg::LocalizationInitializationState; using MotionState = autoware_adapi_v1_msgs::msg::MotionState; using AcceptStart = autoware_adapi_v1_msgs::srv::AcceptStart; + using MRMState = autoware_adapi_v1_msgs::msg::MrmState; Q_OBJECT @@ -77,6 +79,7 @@ public Q_SLOTS: // NOLINT for Qt QGroupBox * makeRoutingGroup(); QGroupBox * makeLocalizationGroup(); QGroupBox * makeMotionGroup(); + QGroupBox * makeFailSafeGroup(); void onShift(const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr msg); void onEmergencyStatus(const tier4_external_api_msgs::msg::Emergency::ConstSharedPtr msg); @@ -139,6 +142,15 @@ public Q_SLOTS: // NOLINT for Qt void onMotion(const MotionState::ConstSharedPtr msg); + // FailSafe + QLabel * mrm_state_label_ptr_{nullptr}; + QLabel * mrm_behavior_label_ptr_{nullptr}; + + rclcpp::Subscription::SharedPtr sub_mrm_; + + void onMRMState(const MRMState::ConstSharedPtr msg); + + // Others QPushButton * velocity_limit_button_ptr_; QLabel * gear_label_ptr_; From 3522a10619b967b930b45f38f82fadd2b4cbd0fc Mon Sep 17 00:00:00 2001 From: Yusuke Muramatsu Date: Wed, 11 Jan 2023 13:47:41 +0900 Subject: [PATCH 04/49] feat(image_projection_based_fusion): add filtering-based roi-detected-object fusion (#2178) * feat(image_projection_based_fusion): update roi detection-objects fusion Signed-off-by: yukke42 * refactor(image_projection_based_fusion): fuse 3D objects and 2D RoIs Signed-off-by: yukke42 * fix: incorrect transform stamp Signed-off-by: yukke42 * feat: update launcher Signed-off-by: yukke42 * feat: move debug-publisher Signed-off-by: yukke42 * Revert "feat: move debug-publisher" This reverts commit f1e9bb8e99dd26cbe1c890068a63a0f766ef84c8. * chore: change debug message namespace Signed-off-by: yukke42 * chore: use debug_publisher_ Signed-off-by: yukke42 * refactor Signed-off-by: yukke42 * docs: update README Signed-off-by: yukke42 * Update launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml * Update launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml * docs: fix topic name Signed-off-by: yukke42 * refactor: use min_iou_threshold Signed-off-by: yukke42 * docs: remove deprecated param Signed-off-by: yukke42 * docs: update topic name Signed-off-by: yukke42 * refactor: remove underbar of public member variables Signed-off-by: yukke42 * docs: update pointpainting docs Signed-off-by: yukke42 * docs: update roi-detected-object-fusion Signed-off-by: yukke42 Signed-off-by: yukke42 Signed-off-by: yukke42 --- .../docs/pointpainting-fusion.md | 24 ++-- .../docs/roi-cluster-fusion.md | 20 +-- .../docs/roi-detected-object-fusion.md | 45 +++--- .../fusion_node.hpp | 2 +- .../roi_detected_object_fusion/node.hpp | 32 +++-- .../roi_detected_object_fusion.launch.xml | 8 +- .../src/debugger.cpp | 2 +- .../src/fusion_node.cpp | 2 +- .../src/roi_detected_object_fusion/node.cpp | 135 ++++++++++++------ 9 files changed, 170 insertions(+), 100 deletions(-) diff --git a/perception/image_projection_based_fusion/docs/pointpainting-fusion.md b/perception/image_projection_based_fusion/docs/pointpainting-fusion.md index f64cf76ed4996..0d5c9a280cca5 100644 --- a/perception/image_projection_based_fusion/docs/pointpainting-fusion.md +++ b/perception/image_projection_based_fusion/docs/pointpainting-fusion.md @@ -14,22 +14,20 @@ The lidar points are projected onto the output of an image-only 2d object detect ### Input -| Name | Type | Description | -| --------------------- | -------------------------------------------------------- | ---------------------------------------------------------------------------------- | -| `input` | `sensor_msgs::msg::PointCloud2` | input pointcloud | -| `input/camera_infoID` | `sensor_msgs::msg::CameraInfo` | camera information to project 3d points onto image planes, `ID` is between 0 and 7 | -| `input/roisID` | `tier4_perception_msgs::msg::DetectedObjectsWithFeature` | ROIs from each image, `ID` is between 0 and 7 | -| `input/image_rawID` | `sensor_msgs::msg::Image` | images for visualization, `ID` is between 0 and 7 | - -| ` +| Name | Type | Description | +| ------------------------ | -------------------------------------------------------- | --------------------------------------------------------- | +| `input` | `sensor_msgs::msg::PointCloud2` | pointcloud | +| `input/camera_info[0-7]` | `sensor_msgs::msg::CameraInfo` | camera information to project 3d points onto image planes | +| `input/rois[0-7]` | `tier4_perception_msgs::msg::DetectedObjectsWithFeature` | ROIs from each image | +| `input/image_raw[0-7]` | `sensor_msgs::msg::Image` | images for visualization | ### Output -| Name | Type | Description | -| -------------------- | ----------------------------------------------------- | ------------------------------------------------- | -| `output` | `sensor_msgs::msg::PointCloud2` | painted pointclouda | -| `~/output/objects` | `autoware_auto_perception_msgs::msg::DetectedObjects` | detected objects | -| `output/image_rawID` | `sensor_msgs::msg::Image` | images for visualization, `ID` is between 0 and 7 | +| Name | Type | Description | +| ------------------------ | ----------------------------------------------------- | ------------------------ | +| `output` | `sensor_msgs::msg::PointCloud2` | painted pointcloud | +| `~/output/objects` | `autoware_auto_perception_msgs::msg::DetectedObjects` | detected objects | +| `~/debug/image_raw[0-7]` | `sensor_msgs::msg::Image` | images for visualization | ## Parameters diff --git a/perception/image_projection_based_fusion/docs/roi-cluster-fusion.md b/perception/image_projection_based_fusion/docs/roi-cluster-fusion.md index 2a4c881ca1539..4b2be683efc18 100644 --- a/perception/image_projection_based_fusion/docs/roi-cluster-fusion.md +++ b/perception/image_projection_based_fusion/docs/roi-cluster-fusion.md @@ -14,19 +14,19 @@ The clusters are projected onto image planes, and then if the ROIs of clusters a ### Input -| Name | Type | Description | -| --------------------- | -------------------------------------------------------- | ---------------------------------------------------------------------------------- | -| `input` | `tier4_perception_msgs::msg::DetectedObjectsWithFeature` | clustered pointcloud | -| `input/camera_infoID` | `sensor_msgs::msg::CameraInfo` | camera information to project 3d points onto image planes, `ID` is between 0 and 7 | -| `input/roisID` | `tier4_perception_msgs::msg::DetectedObjectsWithFeature` | ROIs from each image, `ID` is between 0 and 7 | -| `input/image_rawID` | `sensor_msgs::msg::Image` | images for visualization, `ID` is between 0 and 7 | +| Name | Type | Description | +| ------------------------ | -------------------------------------------------------- | --------------------------------------------------------- | +| `input` | `tier4_perception_msgs::msg::DetectedObjectsWithFeature` | clustered pointcloud | +| `input/camera_info[0-7]` | `sensor_msgs::msg::CameraInfo` | camera information to project 3d points onto image planes | +| `input/rois[0-7]` | `tier4_perception_msgs::msg::DetectedObjectsWithFeature` | ROIs from each image | +| `input/image_raw[0-7]` | `sensor_msgs::msg::Image` | images for visualization | ### Output -| Name | Type | Description | -| -------------------- | -------------------------------------------------------- | ------------------------------------------------- | -| `output` | `tier4_perception_msgs::msg::DetectedObjectsWithFeature` | labeled cluster pointcloud | -| `output/image_rawID` | `sensor_msgs::msg::Image` | images for visualization, `ID` is between 0 and 7 | +| Name | Type | Description | +| ------------------------ | -------------------------------------------------------- | -------------------------- | +| `output` | `tier4_perception_msgs::msg::DetectedObjectsWithFeature` | labeled cluster pointcloud | +| `~/debug/image_raw[0-7]` | `sensor_msgs::msg::Image` | images for visualization | ## Parameters diff --git a/perception/image_projection_based_fusion/docs/roi-detected-object-fusion.md b/perception/image_projection_based_fusion/docs/roi-detected-object-fusion.md index 084051c052296..accdc350bf256 100644 --- a/perception/image_projection_based_fusion/docs/roi-detected-object-fusion.md +++ b/perception/image_projection_based_fusion/docs/roi-detected-object-fusion.md @@ -6,9 +6,12 @@ The `roi_detected_object_fusion` is a package to overwrite labels of detected ob ## Inner-workings / Algorithms -The detected objects are projected onto image planes, and then if the ROIs of detected objects (3D ROIs) and from a 2D detector (2D ROIs) are overlapped, the labels of detected objects are overwritten with that of 2D ROIs. Intersection over Union (IoU) is used to determine if there are overlaps between them. +In what follows, we describe the algorithm utilized by `roi_detected_object_fusion` (the meaning of each parameter can be found in the `Parameters` section): -The `DetectedObject` has three shape and the polygon vertices of a object are as below: +1. If the `existence_probability` of a detected object is greater than the threshold, it is accepted without any further processing and published in `output`. +2. The remaining detected objects are projected onto image planes, and if the resulting ROIs overlap with the ones from the 2D detector, they are published as fused objects in `output`. The Intersection over Union (IoU) is used to determine if there are overlaps between the detections from `input` and the ROIs from `input/rois`. + +The DetectedObject has three possible shape choices/implementations, where the polygon's vertices for each case are defined as follows: - `BOUNDING_BOX`: The 8 corners of a bounding box. - `CYLINDER`: The circle is approximated by a hexagon. @@ -18,32 +21,34 @@ The `DetectedObject` has three shape and the polygon vertices of a object are as ### Input -| Name | Type | Description | -| --------------------- | -------------------------------------------------------- | ---------------------------------------------------------------------------------- | -| `input` | `autoware_auto_perception_msgs::msg::DetectedObjects` | detected objects | -| `input/camera_infoID` | `sensor_msgs::msg::CameraInfo` | camera information to project 3d points onto image planes, `ID` is between 0 and 7 | -| `input/roisID` | `tier4_perception_msgs::msg::DetectedObjectsWithFeature` | ROIs from each image, `ID` is between 0 and 7 | -| `input/image_rawID` | `sensor_msgs::msg::Image` | images for visualization, `ID` is between 0 and 7 | +| Name | Type | Description | +| ------------------------ | -------------------------------------------------------- | ---------------------------------------------------------- | +| `input` | `autoware_auto_perception_msgs::msg::DetectedObjects` | input detected objects | +| `input/camera_info[0-7]` | `sensor_msgs::msg::CameraInfo` | camera information to project 3d points onto image planes. | +| `input/rois[0-7]` | `tier4_perception_msgs::msg::DetectedObjectsWithFeature` | ROIs from each image. | +| `input/image_raw[0-7]` | `sensor_msgs::msg::Image` | images for visualization. | ### Output -| Name | Type | Description | -| -------------------- | ----------------------------------------------------- | ------------------------------------------------- | -| `output` | `autoware_auto_perception_msgs::msg::DetectedObjects` | detected objects | -| `output/image_rawID` | `sensor_msgs::msg::Image` | images for visualization, `ID` is between 0 and 7 | +| Name | Type | Description | +| ------------------------- | ----------------------------------------------------- | -------------------------- | +| `output` | `autoware_auto_perception_msgs::msg::DetectedObjects` | detected objects | +| `~/debug/image_raw[0-7]` | `sensor_msgs::msg::Image` | images for visualization, | +| `~/debug/fused_objects` | `autoware_auto_perception_msgs::msg::DetectedObjects` | fused detected objects | +| `~/debug/ignored_objects` | `autoware_auto_perception_msgs::msg::DetectedObjects` | not fused detected objects | ## Parameters ### Core Parameters -| Name | Type | Description | -| --------------- | ----- | ------------------------------------------------------------------------------ | -| `use_iou_x` | bool | calculate IoU only along x-axis | -| `use_iou_y` | bool | calculate IoU only along y-axis | -| `use_iou` | bool | calculate IoU both along x-axis and y-axis | -| `iou_threshold` | float | the IoU threshold to overwrite a label of a detected object with that of a roi | -| `rois_number` | int | the number of input rois | -| `debug_mode` | bool | If `true`, subscribe and publish images for visualization. | +| Name | Type | Description | +| ----------------------------------------------- | ------ | ------------------------------------------------------------------------------------------------------------------------- | +| `rois_number` | int | the number of input rois | +| `debug_mode` | bool | If set to `true`, the node subscribes to the image topic and publishes an image with debug drawings. | +| `min_iou_threshold` | double | If the iou between detected objects and rois is greater than `min_iou_threshold`, the objects are classified as fused. | +| `passthrough_lower_bound_probability_threshold` | double | If the `existence_probability` of a detected object is greater than the threshold, it is published in output. | +| `use_roi_probability` | float | If set to `true`, the algorithm uses `existence_probability` of ROIs to match with the that of detected objects. | +| `roi_probability_threshold` | double | If the `existence_probability` of ROIs is greater than the threshold, matched detected objects are published in `output`. | ## Assumptions / Known limits diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp index c8275839ae39a..bf704b0a9311d 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp @@ -82,7 +82,7 @@ class FusionNode : public rclcpp::Node // set args if you need virtual void postprocess(Msg & output_msg); - void publish(const Msg & output_msg); + virtual void publish(const Msg & output_msg); void timer_callback(); void setPeriod(const int64_t new_period); diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_detected_object_fusion/node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_detected_object_fusion/node.hpp index a252f4d02bbd0..8168ad2c9f265 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_detected_object_fusion/node.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_detected_object_fusion/node.hpp @@ -16,6 +16,7 @@ #define IMAGE_PROJECTION_BASED_FUSION__ROI_DETECTED_OBJECT_FUSION__NODE_HPP_ #include "image_projection_based_fusion/fusion_node.hpp" +#include "tier4_autoware_utils/ros/debug_publisher.hpp" #include #include @@ -24,34 +25,45 @@ namespace image_projection_based_fusion { +using sensor_msgs::msg::RegionOfInterest; + class RoiDetectedObjectFusionNode : public FusionNode { public: explicit RoiDetectedObjectFusionNode(const rclcpp::NodeOptions & options); protected: + void preprocess(DetectedObjects & output_msg) override; + void fuseOnSingleImage( const DetectedObjects & input_object_msg, const std::size_t image_id, const DetectedObjectsWithFeature & input_roi_msg, const sensor_msgs::msg::CameraInfo & camera_info, DetectedObjects & output_object_msg) override; - void generateDetectedObjectRois( + std::map generateDetectedObjectRoIs( const std::vector & input_objects, const double image_width, const double image_height, const Eigen::Affine3d & object2camera_affine, - const Eigen::Matrix4d & camera_projection, - std::map & object_roi_map); + const Eigen::Matrix4d & camera_projection); - void updateDetectedObjectClassification( + void fuseObjectsOnImage( + const std::vector & objects, const std::vector & image_rois, - const std::map & object_roi_map, - std::vector & output_objects); + const std::map & object_roi_map); - bool use_iou_{false}; - bool use_iou_x_{false}; - bool use_iou_y_{false}; - float iou_threshold_{0.0f}; + void publish(const DetectedObjects & output_msg) override; bool out_of_scope(const DetectedObject & obj); + +private: + struct + { + double passthrough_lower_bound_probability_threshold{}; + double min_iou_threshold{}; + bool use_roi_probability{}; + double roi_probability_threshold{}; + } fusion_params_; + + std::vector passthrough_object_flags_, fused_object_flags_, ignored_object_flags_; }; } // namespace image_projection_based_fusion diff --git a/perception/image_projection_based_fusion/launch/roi_detected_object_fusion.launch.xml b/perception/image_projection_based_fusion/launch/roi_detected_object_fusion.launch.xml index 5b2c30819a4e9..03e4abed5ecd0 100644 --- a/perception/image_projection_based_fusion/launch/roi_detected_object_fusion.launch.xml +++ b/perception/image_projection_based_fusion/launch/roi_detected_object_fusion.launch.xml @@ -62,14 +62,14 @@ - - - - + + + + diff --git a/perception/image_projection_based_fusion/src/debugger.cpp b/perception/image_projection_based_fusion/src/debugger.cpp index 0bcfb019d93df..93b7c6b52986a 100644 --- a/perception/image_projection_based_fusion/src/debugger.cpp +++ b/perception/image_projection_based_fusion/src/debugger.cpp @@ -55,7 +55,7 @@ Debugger::Debugger( } auto pub = - image_transport::create_publisher(node_ptr, "output/image_raw" + std::to_string(img_i)); + image_transport::create_publisher(node_ptr, "~/debug/image_raw" + std::to_string(img_i)); image_pubs_.push_back(pub); image_buffers_.at(img_i).set_capacity(image_buffer_size_); } diff --git a/perception/image_projection_based_fusion/src/fusion_node.cpp b/perception/image_projection_based_fusion/src/fusion_node.cpp index 3bcedaac65589..72a36b0afcc6a 100644 --- a/perception/image_projection_based_fusion/src/fusion_node.cpp +++ b/perception/image_projection_based_fusion/src/fusion_node.cpp @@ -111,7 +111,7 @@ FusionNode::FusionNode( using tier4_autoware_utils::DebugPublisher; using tier4_autoware_utils::StopWatch; stop_watch_ptr_ = std::make_unique>(); - debug_publisher_ = std::make_unique(this, "fusion_node"); + debug_publisher_ = std::make_unique(this, "image_projection_based_fusion"); stop_watch_ptr_->tic("cyclic_time"); stop_watch_ptr_->tic("processing_time"); } diff --git a/perception/image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp b/perception/image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp index 1e1df50482b6e..387dd08bc3c55 100644 --- a/perception/image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp @@ -23,22 +23,41 @@ namespace image_projection_based_fusion RoiDetectedObjectFusionNode::RoiDetectedObjectFusionNode(const rclcpp::NodeOptions & options) : FusionNode("roi_detected_object_fusion", options) { - use_iou_x_ = declare_parameter("use_iou_x", false); - use_iou_y_ = declare_parameter("use_iou_y", false); - use_iou_ = declare_parameter("use_iou", false); - iou_threshold_ = declare_parameter("iou_threshold", 0.1); + fusion_params_.passthrough_lower_bound_probability_threshold = + declare_parameter("passthrough_lower_bound_probability_threshold"); + fusion_params_.use_roi_probability = declare_parameter("use_roi_probability"); + fusion_params_.roi_probability_threshold = declare_parameter("roi_probability_threshold"); + fusion_params_.min_iou_threshold = declare_parameter("min_iou_threshold"); +} + +void RoiDetectedObjectFusionNode::preprocess(DetectedObjects & output_msg) +{ + passthrough_object_flags_.clear(); + passthrough_object_flags_.resize(output_msg.objects.size()); + fused_object_flags_.clear(); + fused_object_flags_.resize(output_msg.objects.size()); + ignored_object_flags_.clear(); + ignored_object_flags_.resize(output_msg.objects.size()); + for (std::size_t obj_i = 0; obj_i < output_msg.objects.size(); ++obj_i) { + if ( + output_msg.objects.at(obj_i).existence_probability > + fusion_params_.passthrough_lower_bound_probability_threshold) { + passthrough_object_flags_.at(obj_i) = true; + } + } } void RoiDetectedObjectFusionNode::fuseOnSingleImage( const DetectedObjects & input_object_msg, const std::size_t image_id, const DetectedObjectsWithFeature & input_roi_msg, - const sensor_msgs::msg::CameraInfo & camera_info, DetectedObjects & output_object_msg) + const sensor_msgs::msg::CameraInfo & camera_info, + DetectedObjects & output_object_msg __attribute__((unused))) { Eigen::Affine3d object2camera_affine; { const auto transform_stamped_optional = getTransformStamped( tf_buffer_, /*target*/ camera_info.header.frame_id, - /*source*/ input_object_msg.header.frame_id, camera_info.header.stamp); + /*source*/ input_object_msg.header.frame_id, input_object_msg.header.stamp); if (!transform_stamped_optional) { return; } @@ -51,13 +70,10 @@ void RoiDetectedObjectFusionNode::fuseOnSingleImage( camera_info.p.at(7), camera_info.p.at(8), camera_info.p.at(9), camera_info.p.at(10), camera_info.p.at(11); - std::map object_roi_map; - generateDetectedObjectRois( + const auto object_roi_map = generateDetectedObjectRoIs( input_object_msg.objects, static_cast(camera_info.width), - static_cast(camera_info.height), object2camera_affine, camera_projection, - object_roi_map); - updateDetectedObjectClassification( - input_roi_msg.feature_objects, object_roi_map, output_object_msg.objects); + static_cast(camera_info.height), object2camera_affine, camera_projection); + fuseObjectsOnImage(input_object_msg.objects, input_roi_msg.feature_objects, object_roi_map); if (debugger_) { debugger_->image_rois_.reserve(input_roi_msg.feature_objects.size()); @@ -68,17 +84,21 @@ void RoiDetectedObjectFusionNode::fuseOnSingleImage( } } -void RoiDetectedObjectFusionNode::generateDetectedObjectRois( +std::map RoiDetectedObjectFusionNode::generateDetectedObjectRoIs( const std::vector & input_objects, const double image_width, const double image_height, const Eigen::Affine3d & object2camera_affine, - const Eigen::Matrix4d & camera_projection, - std::map & object_roi_map) + const Eigen::Matrix4d & camera_projection) { + std::map object_roi_map; for (std::size_t obj_i = 0; obj_i < input_objects.size(); ++obj_i) { std::vector vertices_camera_coord; { const auto & object = input_objects.at(obj_i); + if (passthrough_object_flags_.at(obj_i)) { + continue; + } + // filter point out of scope if (debugger_ && out_of_scope(object)) { continue; @@ -130,7 +150,7 @@ void RoiDetectedObjectFusionNode::generateDetectedObjectRois( max_y = std::min(max_y, image_height - 1); // build roi - sensor_msgs::msg::RegionOfInterest roi; + RegionOfInterest roi; roi.x_offset = static_cast(min_x); roi.y_offset = static_cast(min_y); roi.width = static_cast(max_x) - static_cast(min_x); @@ -141,38 +161,44 @@ void RoiDetectedObjectFusionNode::generateDetectedObjectRois( debugger_->obstacle_rois_.push_back(roi); } } + + return object_roi_map; } -void RoiDetectedObjectFusionNode::updateDetectedObjectClassification( +void RoiDetectedObjectFusionNode::fuseObjectsOnImage( + const std::vector & objects __attribute__((unused)), const std::vector & image_rois, - const std::map & object_roi_map, - std::vector & output_objects) + const std::map & object_roi_map) { - for (const auto & image_roi : image_rois) { - std::size_t object_i = 0; - double max_iou = 0.0; - for (const auto & object_map : object_roi_map) { - double iou(0.0), iou_x(0.0), iou_y(0.0); - if (use_iou_) { - iou = calcIoU(object_map.second, image_roi.feature.roi); - } - if (use_iou_x_) { - iou_x = calcIoUX(object_map.second, image_roi.feature.roi); - } - if (use_iou_y_) { - iou_y = calcIoUY(object_map.second, image_roi.feature.roi); - } + for (const auto & object_pair : object_roi_map) { + const auto & obj_i = object_pair.first; + if (fused_object_flags_.at(obj_i)) { + continue; + } - if (iou + iou_x + iou_y > max_iou) { - object_i = object_map.first; - max_iou = iou + iou_x + iou_y; + float roi_prob = 0.0f; + float max_iou = 0.0f; + for (const auto & image_roi : image_rois) { + const auto & object_roi = object_pair.second; + const double iou = calcIoU(object_roi, image_roi.feature.roi); + if (iou > max_iou) { + max_iou = iou; + roi_prob = image_roi.object.existence_probability; } } - if ( - max_iou > iou_threshold_ && - output_objects.at(object_i).existence_probability <= image_roi.object.existence_probability) { - output_objects.at(object_i).classification = image_roi.object.classification; + if (max_iou > fusion_params_.min_iou_threshold) { + if (fusion_params_.use_roi_probability) { + if (roi_prob > fusion_params_.roi_probability_threshold) { + fused_object_flags_.at(obj_i) = true; + } else { + ignored_object_flags_.at(obj_i) = true; + } + } else { + fused_object_flags_.at(obj_i) = true; + } + } else { + ignored_object_flags_.at(obj_i) = true; } } } @@ -200,6 +226,35 @@ bool RoiDetectedObjectFusionNode::out_of_scope(const DetectedObject & obj) return is_out; } +void RoiDetectedObjectFusionNode::publish(const DetectedObjects & output_msg) +{ + if (pub_ptr_->get_subscription_count() < 1) { + return; + } + + DetectedObjects output_objects_msg, debug_fused_objects_msg, debug_ignored_objects_msg; + output_objects_msg.header = output_msg.header; + debug_fused_objects_msg.header = output_msg.header; + debug_ignored_objects_msg.header = output_msg.header; + for (std::size_t obj_i = 0; obj_i < output_msg.objects.size(); ++obj_i) { + const auto & obj = output_msg.objects.at(obj_i); + if (passthrough_object_flags_.at(obj_i)) { + output_objects_msg.objects.emplace_back(obj); + } + if (fused_object_flags_.at(obj_i)) { + output_objects_msg.objects.emplace_back(obj); + debug_fused_objects_msg.objects.emplace_back(obj); + } else if (ignored_object_flags_.at(obj_i)) { + debug_ignored_objects_msg.objects.emplace_back(obj); + } + } + + pub_ptr_->publish(output_objects_msg); + + debug_publisher_->publish("debug/fused_objects", debug_fused_objects_msg); + debug_publisher_->publish("debug/ignored_objects", debug_ignored_objects_msg); +} + } // namespace image_projection_based_fusion #include From d94d532058c6feaa78ec652e56ec0e8e134b3aa0 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Wed, 11 Jan 2023 15:26:40 +0900 Subject: [PATCH 05/49] fix(lane_change): use current lane for num to preferred lane input (#2615) * fix(lane_change): use current lane for num to preferred lane input Signed-off-by: Muhammad Zulfaqar Azmi * fix lane change distance from deadend Signed-off-by: Muhammad Zulfaqar Azmi * Make separate function to compute resampling interval Signed-off-by: Muhammad Zulfaqar Azmi * Change default config Signed-off-by: Muhammad Zulfaqar Azmi * Make phase info data structure Signed-off-by: Muhammad Zulfaqar Azmi * Added error for finish judge buffer Signed-off-by: Muhammad Zulfaqar Azmi * Fix rebase Signed-off-by: Muhammad Zulfaqar Azmi * ci(pre-commit): autofix * warn user of the modified values Signed-off-by: Muhammad Zulfaqar Azmi Signed-off-by: Muhammad Zulfaqar Azmi Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../config/lane_change/lane_change.param.yaml | 4 +- .../lane_change/lane_change_module_data.hpp | 7 + .../scene_module/lane_change/util.hpp | 20 +- .../src/behavior_path_planner_node.cpp | 16 +- .../src/scene_module/lane_change/util.cpp | 274 ++++++++++-------- 5 files changed, 192 insertions(+), 129 deletions(-) diff --git a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml index 8b8c989baec12..41de868418491 100644 --- a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml +++ b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml @@ -6,8 +6,8 @@ minimum_lane_change_prepare_distance: 2.0 # [m] minimum_lane_change_length: 16.5 # [m] - backward_length_buffer_for_end_of_lane: 2.0 # [m] - lane_change_finish_judge_buffer: 3.0 # [m] + backward_length_buffer_for_end_of_lane: 3.0 # [m] + lane_change_finish_judge_buffer: 2.0 # [m] lane_changing_lateral_jerk: 0.5 # [m/s3] lane_changing_lateral_acc: 0.5 # [m/s2] diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module_data.hpp index dfcb0f5e24c3b..ebbadd6b352ee 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module_data.hpp @@ -76,6 +76,13 @@ enum class LaneChangeStates { Stop, }; +struct LaneChangePhaseInfo +{ + double prepare{0.0}; + double lane_changing{0.0}; + + [[nodiscard]] double sum() const { return prepare + lane_changing; } +}; } // namespace behavior_path_planner #endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__LANE_CHANGE_MODULE_DATA_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp index 4c8a7e9fb91a8..d9b6f0dad4929 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp @@ -113,23 +113,21 @@ ShiftLine getLaneChangeShiftLine( PathWithLaneId getReferencePathFromTargetLane( const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, - const Pose & lane_changing_start_pose, const double prepare_distance, - const double lane_changing_distance, const double forward_path_length, - const double lane_changing_speed); - -PathWithLaneId getReferencePathFromTargetLane( - const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, - const Pose & in_target_front_pose, const Pose & in_target_end_pose); + const Pose & lane_changing_start_pose, const double target_lane_length, + const LaneChangePhaseInfo dist_prepare_to_lc_end, const double min_total_lane_changing_distance, + const double forward_path_length, const double resample_interval, const bool is_goal_in_route); PathWithLaneId getLaneChangePathPrepareSegment( const RouteHandler & route_handler, const lanelet::ConstLanelets & original_lanelets, - const Pose & current_pose, const double & backward_path_length, const double & prepare_distance, - const double & prepare_duration, const double & minimum_lane_change_velocity); + const double arc_length_from_current, const double backward_path_length, + const double prepare_distance, const double prepare_speed); PathWithLaneId getLaneChangePathLaneChangingSegment( const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanelets, - const Pose & current_pose, const double prepare_distance, const double lane_change_distance, - const double lane_changing_speed, const BehaviorPathPlannerParameters & common_param); + const double forward_path_length, const double arc_length_from_target, + const double target_lane_length, const LaneChangePhaseInfo dist_prepare_to_lc_end, + const double lane_changing_speed, const double total_required_min_dist); + bool isEgoWithinOriginalLane( const lanelet::ConstLanelets & current_lanes, const Pose & current_pose, const BehaviorPathPlannerParameters & common_param); diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index df89958e37c18..285b310c4c9ce 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -250,6 +250,11 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() p.rear_vehicle_reaction_time = declare_parameter("rear_vehicle_reaction_time", 2.0); p.rear_vehicle_safety_time_margin = declare_parameter("rear_vehicle_safety_time_margin", 2.0); + + if (p.backward_length_buffer_for_end_of_lane < 1.0) { + RCLCPP_WARN_STREAM( + get_logger(), "Lane change buffer must be more than 1 meter. Modifying the buffer."); + } return p; } @@ -266,8 +271,8 @@ SideShiftParameters BehaviorPathPlannerNode::getSideShiftParam() p.shifting_lateral_jerk = dp("shifting_lateral_jerk", 0.2); p.min_shifting_distance = dp("min_shifting_distance", 5.0); p.min_shifting_speed = dp("min_shifting_speed", 5.56); - p.shift_request_time_limit = dp("shift_request_time_limit", 1.0); p.drivable_area_right_bound_offset = dp("drivable_area_right_bound_offset", 0.0); + p.shift_request_time_limit = dp("shift_request_time_limit", 1.0); p.drivable_area_left_bound_offset = dp("drivable_area_left_bound_offset", 0.0); return p; @@ -437,6 +442,15 @@ LaneChangeParameters BehaviorPathPlannerNode::getLaneChangeParam() exit(EXIT_FAILURE); } + const auto lc_buffer = + this->get_parameter("lane_change.backward_length_buffer_for_end_of_lane").get_value(); + if (lc_buffer < p.lane_change_finish_judge_buffer + 1.0) { + p.lane_change_finish_judge_buffer = lc_buffer - 1; + RCLCPP_WARN_STREAM( + get_logger(), "lane change buffer is less than finish buffer. Modifying the value to " + << p.lane_change_finish_judge_buffer << "...."); + } + return p; } diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp index bc6afcdfee65b..1774dddc10d42 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp @@ -39,6 +39,15 @@ namespace behavior_path_planner::lane_change_utils using autoware_auto_planning_msgs::msg::PathPointWithLaneId; using lanelet::ArcCoordinates; +inline double calcLaneChangeResamplingInterval( + const double lane_changing_distance, const double lane_changing_speed) +{ + constexpr auto min_resampling_points{30.0}; + constexpr auto resampling_dt{0.2}; + return std::max( + lane_changing_distance / min_resampling_points, lane_changing_speed * resampling_dt); +} + PathWithLaneId combineReferencePath(const PathWithLaneId & path1, const PathWithLaneId & path2) { PathWithLaneId path; @@ -86,29 +95,13 @@ double getDistanceWhenDecelerate( return std::max(distance, minimum_distance); } -std::pair calcLaneChangingSpeedAndDistanceWhenDecelerate( - const double velocity, const double shift_length, const double deceleration, - const double min_total_lc_len, const BehaviorPathPlannerParameters & com_param, - const LaneChangeParameters & lc_param) -{ - const auto required_time = PathShifter::calcShiftTimeFromJerkAndJerk( - shift_length, lc_param.lane_changing_lateral_jerk, lc_param.lane_changing_lateral_acc); - - const auto lane_changing_average_speed = - std::max(velocity + deceleration * 0.5 * required_time, lc_param.minimum_lane_change_velocity); - const auto expected_dist = lane_changing_average_speed * required_time; - const auto lane_changing_distance = - (expected_dist < min_total_lc_len) ? expected_dist : com_param.minimum_lane_change_length; - return {lane_changing_average_speed, lane_changing_distance}; -} - std::optional constructCandidatePath( const PathWithLaneId & prepare_segment, const PathWithLaneId & lane_changing_segment, const PathWithLaneId & target_lane_reference_path, const ShiftLine & shift_line, const lanelet::ConstLanelets & original_lanelets, const lanelet::ConstLanelets & target_lanelets, - const double acceleration, const double prepare_distance, const double prepare_duration, - const double prepare_speed, const double lane_change_distance, const double lane_changing_speed, - const BehaviorPathPlannerParameters & params, const LaneChangeParameters & lane_change_param) + const double acceleration, const double prepare_duration, const LaneChangePhaseInfo distance, + const LaneChangePhaseInfo speed, const BehaviorPathPlannerParameters & params, + const LaneChangeParameters & lane_change_param) { PathShifter path_shifter; path_shifter.setPath(target_lane_reference_path); @@ -118,6 +111,8 @@ std::optional constructCandidatePath( // offset front side bool offset_back = false; + const auto lane_changing_speed = speed.lane_changing; + path_shifter.setVelocity(lane_changing_speed); path_shifter.setLateralAccelerationLimit(std::abs(lane_change_param.lane_changing_lateral_acc)); @@ -127,6 +122,9 @@ std::optional constructCandidatePath( "failed to generate shifted path."); } + const auto prepare_distance = distance.prepare; + const auto lane_change_distance = distance.lane_changing; + LaneChangePath candidate_path; candidate_path.acceleration = acceleration; candidate_path.preparation_length = prepare_distance; @@ -135,6 +133,13 @@ std::optional constructCandidatePath( candidate_path.reference_lanelets = original_lanelets; candidate_path.target_lanelets = target_lanelets; + RCLCPP_DEBUG( + rclcpp::get_logger("behavior_path_planner") + .get_child("lane_change") + .get_child("util") + .get_child("constructCandidatePath"), + "prepare_distance: %f, lane_change: %f", prepare_distance, lane_change_distance); + const PathPointWithLaneId & lane_changing_start_point = prepare_segment.points.back(); const PathPointWithLaneId & lane_changing_end_point = lane_changing_segment.points.front(); const Pose & lane_changing_end_pose = lane_changing_end_point.point.pose; @@ -171,7 +176,7 @@ std::optional constructCandidatePath( } candidate_path.turn_signal_info = lane_change_utils::calc_turn_signal_info( - prepare_segment, prepare_speed, params.minimum_lane_change_prepare_distance, prepare_duration, + prepare_segment, speed.prepare, params.minimum_lane_change_prepare_distance, prepare_duration, shift_line, shifted_path); // check candidate path is in lanelet if (!isPathInLanelets(candidate_path.path, original_lanelets, target_lanelets)) { @@ -208,20 +213,27 @@ LaneChangePaths getLaneChangePaths( const double target_distance = util::getArcLengthToTargetLanelet(original_lanelets, target_lanelets.front(), pose); - const auto num_lane_change = + const auto num_to_preferred_lane = std::abs(route_handler.getNumLaneToPreferredLane(original_lanelets.back())); - const auto min_total_lane_changing_dist = - util::calcLaneChangeBuffer(common_parameter, num_lane_change); + const auto is_goal_in_route = route_handler.isInGoalRouteSection(target_lanelets.back()); const auto end_of_lane_dist = std::invoke([&]() { - if (route_handler.isInGoalRouteSection(target_lanelets.back())) { + const auto required_dist = util::calcLaneChangeBuffer(common_parameter, num_to_preferred_lane); + if (is_goal_in_route) { return util::getSignedDistance(pose, route_handler.getGoalPose(), original_lanelets) - - min_total_lane_changing_dist; + required_dist; } - - return util::getDistanceToEndOfLane(pose, original_lanelets) - min_total_lane_changing_dist; + return util::getDistanceToEndOfLane(pose, original_lanelets) - required_dist; }); + const auto num_lane_change = std::max(0, num_to_preferred_lane - 1); + const auto required_total_min_distance = + util::calcLaneChangeBuffer(common_parameter, num_lane_change); + + const auto arc_position_from_current = lanelet::utils::getArcCoordinates(original_lanelets, pose); + const auto arc_position_from_target = lanelet::utils::getArcCoordinates(target_lanelets, pose); + const auto target_lane_length = lanelet::utils::getLaneletLength2d(target_lanelets); + for (double acceleration = 0.0; acceleration >= -maximum_deceleration; acceleration -= acceleration_resolution) { const double prepare_speed = getExpectedVelocityWhenDecelerate( @@ -241,9 +253,9 @@ LaneChangePaths getLaneChangePaths( continue; } - const PathWithLaneId prepare_segment_reference = getLaneChangePathPrepareSegment( - route_handler, original_lanelets, pose, backward_path_length, prepare_distance, - lane_change_prepare_duration, minimum_lane_change_velocity); + const auto prepare_segment_reference = getLaneChangePathPrepareSegment( + route_handler, original_lanelets, arc_position_from_current.length, backward_path_length, + prepare_distance, std::max(prepare_speed, minimum_lane_change_velocity)); const auto estimated_shift_length = lanelet::utils::getArcCoordinates( target_lanelets, prepare_segment_reference.points.front().point.pose); @@ -253,9 +265,11 @@ LaneChangePaths getLaneChangePaths( prepare_speed, estimated_shift_length.distance, acceleration, end_of_lane_dist, common_parameter, parameter); - const PathWithLaneId lane_changing_segment_reference = getLaneChangePathLaneChangingSegment( - route_handler, target_lanelets, pose, prepare_distance, lane_changing_distance, - lane_changing_speed, common_parameter); + const auto lc_dist = LaneChangePhaseInfo{prepare_distance, lane_changing_distance}; + + const auto lane_changing_segment_reference = getLaneChangePathLaneChangingSegment( + route_handler, target_lanelets, forward_path_length, arc_position_from_target.length, + target_lane_length, lc_dist, lane_changing_speed, required_total_min_distance); if ( prepare_segment_reference.points.empty() || lane_changing_segment_reference.points.empty()) { @@ -267,19 +281,21 @@ LaneChangePaths getLaneChangePaths( const Pose & lane_changing_start_pose = prepare_segment_reference.points.back().point.pose; - const PathWithLaneId target_lane_reference_path = getReferencePathFromTargetLane( - route_handler, target_lanelets, lane_changing_start_pose, prepare_distance, - lane_changing_distance, forward_path_length, lane_changing_speed); + const auto resample_interval = + calcLaneChangeResamplingInterval(lane_changing_distance, lane_changing_speed); + const auto target_lane_reference_path = getReferencePathFromTargetLane( + route_handler, target_lanelets, lane_changing_start_pose, target_lane_length, lc_dist, + required_total_min_distance, forward_path_length, resample_interval, is_goal_in_route); const ShiftLine shift_line = getLaneChangeShiftLine( prepare_segment_reference, lane_changing_segment_reference, target_lanelets, target_lane_reference_path); + const auto lc_speed = LaneChangePhaseInfo{prepare_speed, lane_changing_speed}; const auto candidate_path = constructCandidatePath( prepare_segment_reference, lane_changing_segment_reference, target_lane_reference_path, - shift_line, original_lanelets, target_lanelets, acceleration, prepare_distance, - lane_change_prepare_duration, prepare_speed, lane_changing_distance, lane_changing_speed, - common_parameter, parameter); + shift_line, original_lanelets, target_lanelets, acceleration, lane_change_prepare_duration, + lc_dist, lc_speed, common_parameter, parameter); if (!candidate_path) { continue; @@ -524,52 +540,6 @@ bool isLaneChangePathSafe( return true; } -PathWithLaneId getReferencePathFromTargetLane( - const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, - const Pose & lane_changing_start_pose, const double prepare_distance, - const double lane_changing_distance, const double forward_path_length, - const double lane_changing_speed) -{ - const ArcCoordinates lane_change_start_arc_position = - lanelet::utils::getArcCoordinates(target_lanes, lane_changing_start_pose); - const double & s_start = lane_change_start_arc_position.length; - double s_end = s_start + prepare_distance + lane_changing_distance + forward_path_length; - if (route_handler.isInGoalRouteSection(target_lanes.back())) { - const auto goal_arc_coordinates = - lanelet::utils::getArcCoordinates(target_lanes, route_handler.getGoalPose()); - s_end = std::min(s_end, goal_arc_coordinates.length); - } - const auto lane_changing_reference_path = - route_handler.getCenterLinePath(target_lanes, s_start, s_end); - - constexpr auto min_resampling_points{30.0}; - constexpr auto resampling_dt{0.2}; - const auto resample_interval = - std::max(lane_changing_distance / min_resampling_points, lane_changing_speed * resampling_dt); - return util::resamplePathWithSpline( - lane_changing_reference_path, resample_interval, true, {0.0, lane_changing_distance}); -} - -PathWithLaneId getReferencePathFromTargetLane( - const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, - const Pose & in_target_front_pose, const Pose & in_target_end_pose) -{ - const ArcCoordinates lane_change_start_arc_position = - lanelet::utils::getArcCoordinates(target_lanes, in_target_front_pose); - - const ArcCoordinates lane_change_end_arc_position = - lanelet::utils::getArcCoordinates(target_lanes, in_target_end_pose); - - const double & s_start = lane_change_start_arc_position.length; - double s_end = lane_change_end_arc_position.length; - if (route_handler.isInGoalRouteSection(target_lanes.back())) { - const auto goal_arc_coordinates = - lanelet::utils::getArcCoordinates(target_lanes, route_handler.getGoalPose()); - s_end = std::min(s_end, goal_arc_coordinates.length); - } - return route_handler.getCenterLinePath(target_lanes, s_start, s_end); -} - ShiftLine getLaneChangeShiftLine( const PathWithLaneId & path1, const PathWithLaneId & path2, const lanelet::ConstLanelets & target_lanes, const PathWithLaneId & reference_path) @@ -588,61 +558,98 @@ ShiftLine getLaneChangeShiftLine( shift_line.end_idx = motion_utils::findNearestIndex(reference_path.points, lane_change_end_on_target_lane.position); + RCLCPP_DEBUG( + rclcpp::get_logger("behavior_path_planner") + .get_child("lane_change") + .get_child("util") + .get_child("getLaneChangeShiftLine"), + "shift_line distance: %f", + util::getSignedDistance(shift_line.start, shift_line.end, target_lanes)); return shift_line; } PathWithLaneId getLaneChangePathPrepareSegment( const RouteHandler & route_handler, const lanelet::ConstLanelets & original_lanelets, - const Pose & current_pose, const double & backward_path_length, const double & prepare_distance, - const double & prepare_duration, const double & minimum_lane_change_velocity) + const double arc_length_from_current, const double backward_path_length, + const double prepare_distance, const double prepare_speed) { if (original_lanelets.empty()) { return PathWithLaneId(); } - const ArcCoordinates arc_position = - lanelet::utils::getArcCoordinates(original_lanelets, current_pose); - const double s_start = arc_position.length - backward_path_length; - const double s_end = arc_position.length + prepare_distance; + const double s_start = arc_length_from_current - backward_path_length; + const double s_end = arc_length_from_current + prepare_distance; + + RCLCPP_DEBUG( + rclcpp::get_logger("behavior_path_planner") + .get_child("lane_change") + .get_child("util") + .get_child("getLaneChangePathPrepareSegment"), + "start: %f, end: %f", s_start, s_end); PathWithLaneId prepare_segment = route_handler.getCenterLinePath(original_lanelets, s_start, s_end); prepare_segment.points.back().point.longitudinal_velocity_mps = std::min( prepare_segment.points.back().point.longitudinal_velocity_mps, - static_cast( - std::max(prepare_distance / prepare_duration, minimum_lane_change_velocity))); + static_cast(prepare_speed)); return prepare_segment; } -PathWithLaneId getLaneChangePathLaneChangingSegment( - const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanelets, - const Pose & current_pose, const double prepare_distance, const double lane_change_distance, - const double lane_changing_speed, const BehaviorPathPlannerParameters & params) +std::pair calcLaneChangingSpeedAndDistanceWhenDecelerate( + const double velocity, const double shift_length, const double deceleration, + const double min_total_lc_len, const BehaviorPathPlannerParameters & com_param, + const LaneChangeParameters & lc_param) { - if (target_lanelets.empty()) { - return PathWithLaneId(); - } + const auto required_time = PathShifter::calcShiftTimeFromJerkAndJerk( + shift_length, lc_param.lane_changing_lateral_jerk, lc_param.lane_changing_lateral_acc); - const auto minimum_lane_change_length = params.minimum_lane_change_length; - const ArcCoordinates arc_position = - lanelet::utils::getArcCoordinates(target_lanelets, current_pose); - const double lane_length = lanelet::utils::getLaneletLength2d(target_lanelets); - const int num = std::abs(route_handler.getNumLaneToPreferredLane(target_lanelets.back())); + const auto lane_changing_average_speed = + std::max(velocity + deceleration * 0.5 * required_time, lc_param.minimum_lane_change_velocity); + const auto expected_dist = lane_changing_average_speed * required_time; + const auto lane_changing_distance = + (expected_dist < min_total_lc_len) ? expected_dist : com_param.minimum_lane_change_length; - const double s_start = std::invoke([&arc_position, &prepare_distance, &lane_change_distance, - &lane_length, &num, &minimum_lane_change_length]() { - const double s_start = arc_position.length + prepare_distance + lane_change_distance; - return std::min(s_start, lane_length - num * minimum_lane_change_length); - }); + RCLCPP_DEBUG( + rclcpp::get_logger("behavior_path_planner") + .get_child("lane_change") + .get_child("util") + .get_child("calcLaneChangingSpeedAndDistanceWhenDecelerate"), + "required_time: %f [s] average_speed: %f [m/s], lane_changing_distance : %f [m]", required_time, + lane_changing_average_speed, lane_changing_distance); - const double s_end = std::invoke([&s_start, ¶ms, &lane_length, &num]() { - const double s_end = std::min( - s_start + params.forward_path_length, lane_length - util::calcLaneChangeBuffer(params, num)); - return std::max(s_end, s_start + std::numeric_limits::epsilon()); + return {lane_changing_average_speed, lane_changing_distance}; +} + +PathWithLaneId getLaneChangePathLaneChangingSegment( + const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanelets, + const double forward_path_length, const double arc_length_from_target, + const double target_lane_length, const LaneChangePhaseInfo dist_prepare_to_lc_end, + const double lane_changing_speed, const double total_required_min_dist) +{ + const double s_start = std::invoke([&arc_length_from_target, &dist_prepare_to_lc_end, + &target_lane_length, &total_required_min_dist]() { + const double dist_from_current_pose = arc_length_from_target + dist_prepare_to_lc_end.sum(); + const double end_of_lane_dist_without_buffer = target_lane_length - total_required_min_dist; + return std::min(dist_from_current_pose, end_of_lane_dist_without_buffer); }); + const double s_end = + std::invoke([&s_start, &forward_path_length, &target_lane_length, &total_required_min_dist]() { + const double dist_from_start = s_start + forward_path_length; + const double dist_from_end = target_lane_length - total_required_min_dist; + return std::max( + std::min(dist_from_start, dist_from_end), s_start + std::numeric_limits::epsilon()); + }); + + RCLCPP_DEBUG( + rclcpp::get_logger("behavior_path_planner") + .get_child("lane_change") + .get_child("util") + .get_child("getLaneChangePathLaneChangingSegment"), + "start: %f, end: %f", s_start, s_end); + PathWithLaneId lane_changing_segment = route_handler.getCenterLinePath(target_lanelets, s_start, s_end); for (auto & point : lane_changing_segment.points) { @@ -653,6 +660,43 @@ PathWithLaneId getLaneChangePathLaneChangingSegment( return lane_changing_segment; } +PathWithLaneId getReferencePathFromTargetLane( + const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, + const Pose & lane_changing_start_pose, const double target_lane_length, + const LaneChangePhaseInfo dist_prepare_to_lc_end, const double min_total_lane_changing_distance, + const double forward_path_length, const double resample_interval, const bool is_goal_in_route) +{ + const ArcCoordinates lane_change_start_arc_position = + lanelet::utils::getArcCoordinates(target_lanes, lane_changing_start_pose); + + const double s_start = lane_change_start_arc_position.length; + const double s_end = std::invoke([&]() { + const auto dist_from_lc_start = s_start + dist_prepare_to_lc_end.sum() + forward_path_length; + if (is_goal_in_route) { + const auto goal_arc_coordinates = + lanelet::utils::getArcCoordinates(target_lanes, route_handler.getGoalPose()); + const auto dist_to_goal = goal_arc_coordinates.length - min_total_lane_changing_distance; + return std::min(dist_from_lc_start, dist_to_goal); + } + const auto dist_from_end = target_lane_length - min_total_lane_changing_distance; + return std::min(dist_from_lc_start, dist_from_end); + }); + + RCLCPP_DEBUG( + rclcpp::get_logger("behavior_path_planner") + .get_child("lane_change") + .get_child("util") + .get_child("getReferencePathFromTargetLane"), + "start: %f, end: %f", s_start, s_end); + + const auto lane_changing_reference_path = + route_handler.getCenterLinePath(target_lanes, s_start, s_end); + + return util::resamplePathWithSpline( + lane_changing_reference_path, resample_interval, true, + {0.0, dist_prepare_to_lc_end.lane_changing}); +} + bool isEgoWithinOriginalLane( const lanelet::ConstLanelets & current_lanes, const Pose & current_pose, const BehaviorPathPlannerParameters & common_param) @@ -763,7 +807,7 @@ std::optional getAbortPaths( const auto ego_nearest_dist_threshold = planner_data->parameters.ego_nearest_dist_threshold; const auto ego_nearest_yaw_threshold = planner_data->parameters.ego_nearest_yaw_threshold; - const double minimum_lane_change_length = util::calcLaneChangeBuffer(common_param, 1, 0.0); + const double minimum_lane_change_length = util::calcLaneChangeBuffer(common_param, 1); const auto & lane_changing_path = selected_path.path; const auto lane_changing_end_pose_idx = std::invoke([&]() { From f1714f1cd390054fd3e2fea29eea17986ce391f3 Mon Sep 17 00:00:00 2001 From: Berkay Karaman Date: Wed, 11 Jan 2023 11:00:19 +0300 Subject: [PATCH 06/49] feat(pure_pursuit): update parameter file (#2629) --- .../pure_pursuit/config/pure_pursuit.param.yaml | 17 +++++++++++++---- 1 file changed, 13 insertions(+), 4 deletions(-) diff --git a/control/pure_pursuit/config/pure_pursuit.param.yaml b/control/pure_pursuit/config/pure_pursuit.param.yaml index ee3e6b742ff44..0b8b464e9f969 100644 --- a/control/pure_pursuit/config/pure_pursuit.param.yaml +++ b/control/pure_pursuit/config/pure_pursuit.param.yaml @@ -1,7 +1,16 @@ /**: ros__parameters: - # -- algorithm -- - lookahead_distance_ratio: 2.2 - min_lookahead_distance: 2.5 - reverse_min_lookahead_distance: 7.0 + ld_velocity_ratio: 2.4 + ld_lateral_error_ratio: 3.6 + ld_curvature_ratio: 120.0 + long_ld_lateral_error_threshold: 0.5 + min_lookahead_distance: 4.35 + max_lookahead_distance: 15.0 converged_steer_rad: 0.1 + reverse_min_lookahead_distance: 7.0 + prediction_ds: 0.3 + prediction_distance_length: 21.0 + resampling_ds: 0.1 + curvature_calculation_distance: 4.0 + enable_path_smoothing: false + path_filter_moving_ave_num: 25 From 6cee3969378d7cf71412469dbc31087c2543b140 Mon Sep 17 00:00:00 2001 From: Yoshi Ri Date: Wed, 11 Jan 2023 20:02:35 +0900 Subject: [PATCH 07/49] feat(multi_object_tracker): canceling unnecessary pose/velocity update caused by shape change (#2238) * add function to get nearest corner or points * add finding nearest corner util * add calc offset function * add nearest edge tracking function * add self_transform to tracker init and update * update big vehicle tracker * add self_transform to each tracker * update normal vehicle tracker * fix anchor point calculation * fix bounding_box_.width representing length issue * fix initial bounding box size * fix util length/width dimensions * use last input bounding box size * temporal set to post process * manage reversed orientation detection * use enum to represent BBOX index * demonstration of preprocessing * change post process method * add get_measurement function * update to use tracking point and offset vector * add invalid index to handle exception * refactoring some comments * add convex hull to bounding box converter * track convex shape in converted bbox shape Signed-off-by: yoshiri Signed-off-by: yoshiri --- .../multi_object_tracker_core.hpp | 4 +- .../tracker/model/bicycle_tracker.hpp | 10 +- .../tracker/model/big_vehicle_tracker.hpp | 15 +- .../model/multiple_vehicle_tracker.hpp | 7 +- .../tracker/model/normal_vehicle_tracker.hpp | 14 +- .../tracker/model/pass_through_tracker.hpp | 7 +- .../model/pedestrian_and_bicycle_tracker.hpp | 7 +- .../tracker/model/pedestrian_tracker.hpp | 9 +- .../tracker/model/tracker_base.hpp | 6 +- .../tracker/model/unknown_tracker.hpp | 7 +- .../multi_object_tracker/utils/utils.hpp | 263 ++++++++++++++++++ .../src/multi_object_tracker_core.cpp | 24 +- .../src/tracker/model/bicycle_tracker.cpp | 14 +- .../src/tracker/model/big_vehicle_tracker.cpp | 120 ++++++-- .../model/multiple_vehicle_tracker.cpp | 14 +- .../tracker/model/normal_vehicle_tracker.cpp | 102 +++++-- .../tracker/model/pass_through_tracker.cpp | 7 +- .../model/pedestrian_and_bicycle_tracker.cpp | 14 +- .../src/tracker/model/pedestrian_tracker.cpp | 15 +- .../src/tracker/model/tracker_base.cpp | 4 +- .../src/tracker/model/unknown_tracker.cpp | 6 +- 21 files changed, 550 insertions(+), 119 deletions(-) diff --git a/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp b/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp index 59552d217fcbc..eb8607b91b50c 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp @@ -78,8 +78,8 @@ class MultiObjectTracker : public rclcpp::Node void sanitizeTracker( std::list> & list_tracker, const rclcpp::Time & time); std::shared_ptr createNewTracker( - const autoware_auto_perception_msgs::msg::DetectedObject & object, - const rclcpp::Time & time) const; + const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, + const geometry_msgs::msg::Transform & self_transform) const; void publish(const rclcpp::Time & time) const; inline bool shouldTrackerPublish(const std::shared_ptr tracker) const; diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp index e521a7679b8a9..8a30af39870b5 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp @@ -64,21 +64,23 @@ class BicycleTracker : public Tracker private: struct BoundingBox { - double width; double length; + double width; double height; }; BoundingBox bounding_box_; + BoundingBox last_input_bounding_box_; public: BicycleTracker( - const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object); + const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object, + const geometry_msgs::msg::Transform & self_transform); bool predict(const rclcpp::Time & time) override; bool predict(const double dt, KalmanFilter & ekf) const; bool measure( - const autoware_auto_perception_msgs::msg::DetectedObject & object, - const rclcpp::Time & time) override; + const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, + const geometry_msgs::msg::Transform & self_transform) override; bool measureWithPose(const autoware_auto_perception_msgs::msg::DetectedObject & object); bool measureWithShape(const autoware_auto_perception_msgs::msg::DetectedObject & object); bool getTrackedObject( diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp index a8736e28c2b84..614faa6a7d5b4 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp @@ -22,12 +22,12 @@ #include "multi_object_tracker/tracker/model/tracker_base.hpp" #include - class BigVehicleTracker : public Tracker { private: autoware_auto_perception_msgs::msg::DetectedObject object_; rclcpp::Logger logger_; + int last_nearest_corner_index_; private: KalmanFilter ekf_; @@ -65,26 +65,31 @@ class BigVehicleTracker : public Tracker private: struct BoundingBox { - double width; double length; + double width; double height; }; BoundingBox bounding_box_; + BoundingBox last_input_bounding_box_; + Eigen::Vector2d tracking_offset_; public: BigVehicleTracker( - const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object); + const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object, + const geometry_msgs::msg::Transform & self_transform); bool predict(const rclcpp::Time & time) override; bool predict(const double dt, KalmanFilter & ekf) const; bool measure( - const autoware_auto_perception_msgs::msg::DetectedObject & object, - const rclcpp::Time & time) override; + const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, + const geometry_msgs::msg::Transform & self_transform) override; bool measureWithPose(const autoware_auto_perception_msgs::msg::DetectedObject & object); bool measureWithShape(const autoware_auto_perception_msgs::msg::DetectedObject & object); bool getTrackedObject( const rclcpp::Time & time, autoware_auto_perception_msgs::msg::TrackedObject & object) const override; + double getMeasurementYaw(const autoware_auto_perception_msgs::msg::DetectedObject & object); + void setNearestCornerOrSurfaceIndex(const geometry_msgs::msg::Transform & self_transform); virtual ~BigVehicleTracker() {} }; diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp index 144b1b6c9b8b4..5722566b4a633 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp @@ -34,12 +34,13 @@ class MultipleVehicleTracker : public Tracker public: MultipleVehicleTracker( - const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object); + const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object, + const geometry_msgs::msg::Transform & self_transform); bool predict(const rclcpp::Time & time) override; bool measure( - const autoware_auto_perception_msgs::msg::DetectedObject & object, - const rclcpp::Time & time) override; + const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, + const geometry_msgs::msg::Transform & self_transform) override; bool getTrackedObject( const rclcpp::Time & time, autoware_auto_perception_msgs::msg::TrackedObject & object) const override; diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp index bd401207b18dc..8175f20c5d671 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp @@ -28,6 +28,7 @@ class NormalVehicleTracker : public Tracker private: autoware_auto_perception_msgs::msg::DetectedObject object_; rclcpp::Logger logger_; + int last_nearest_corner_index_; private: KalmanFilter ekf_; @@ -39,6 +40,7 @@ class NormalVehicleTracker : public Tracker VX = 3, WZ = 4, }; + struct EkfParams { char dim_x = 5; @@ -66,26 +68,30 @@ class NormalVehicleTracker : public Tracker private: struct BoundingBox { - double width; double length; + double width; double height; }; BoundingBox bounding_box_; + BoundingBox last_input_bounding_box_; + Eigen::Vector2d tracking_offset_; public: NormalVehicleTracker( - const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object); + const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object, + const geometry_msgs::msg::Transform & self_transform); bool predict(const rclcpp::Time & time) override; bool predict(const double dt, KalmanFilter & ekf) const; bool measure( - const autoware_auto_perception_msgs::msg::DetectedObject & object, - const rclcpp::Time & time) override; + const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, + const geometry_msgs::msg::Transform & self_transform) override; bool measureWithPose(const autoware_auto_perception_msgs::msg::DetectedObject & object); bool measureWithShape(const autoware_auto_perception_msgs::msg::DetectedObject & object); bool getTrackedObject( const rclcpp::Time & time, autoware_auto_perception_msgs::msg::TrackedObject & object) const override; + void setNearestCornerOrSurfaceIndex(const geometry_msgs::msg::Transform & self_transform); virtual ~NormalVehicleTracker() {} }; diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pass_through_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pass_through_tracker.hpp index 8c5033e2c3900..48a8702d182a0 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pass_through_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pass_through_tracker.hpp @@ -33,11 +33,12 @@ class PassThroughTracker : public Tracker public: PassThroughTracker( - const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object); + const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object, + const geometry_msgs::msg::Transform & self_transform); bool predict(const rclcpp::Time & time) override; bool measure( - const autoware_auto_perception_msgs::msg::DetectedObject & object, - const rclcpp::Time & time) override; + const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, + const geometry_msgs::msg::Transform & self_transform) override; bool getTrackedObject( const rclcpp::Time & time, autoware_auto_perception_msgs::msg::TrackedObject & object) const override; diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp index ca1e5b519d708..cf6a82088c7b6 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp @@ -33,12 +33,13 @@ class PedestrianAndBicycleTracker : public Tracker public: PedestrianAndBicycleTracker( - const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object); + const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object, + const geometry_msgs::msg::Transform & self_transform); bool predict(const rclcpp::Time & time) override; bool measure( - const autoware_auto_perception_msgs::msg::DetectedObject & object, - const rclcpp::Time & time) override; + const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, + const geometry_msgs::msg::Transform & self_transform) override; bool getTrackedObject( const rclcpp::Time & time, autoware_auto_perception_msgs::msg::TrackedObject & object) const override; diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_tracker.hpp index ef0c681e27d68..0219d3b227044 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_tracker.hpp @@ -64,8 +64,8 @@ class PedestrianTracker : public Tracker private: struct BoundingBox { - double width; double length; + double width; double height; }; struct Cylinder @@ -78,13 +78,14 @@ class PedestrianTracker : public Tracker public: PedestrianTracker( - const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object); + const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object, + const geometry_msgs::msg::Transform & self_transform); bool predict(const rclcpp::Time & time) override; bool predict(const double dt, KalmanFilter & ekf) const; bool measure( - const autoware_auto_perception_msgs::msg::DetectedObject & object, - const rclcpp::Time & time) override; + const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, + const geometry_msgs::msg::Transform & self_transform) override; bool measureWithPose(const autoware_auto_perception_msgs::msg::DetectedObject & object); bool measureWithShape(const autoware_auto_perception_msgs::msg::DetectedObject & object); bool getTrackedObject( diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/tracker_base.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/tracker_base.hpp index a094e32d1ef6b..398acd438b690 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/tracker_base.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/tracker_base.hpp @@ -58,7 +58,7 @@ class Tracker virtual ~Tracker() {} bool updateWithMeasurement( const autoware_auto_perception_msgs::msg::DetectedObject & object, - const rclcpp::Time & measurement_time); + const rclcpp::Time & measurement_time, const geometry_msgs::msg::Transform & self_transform); bool updateWithoutMeasurement(); std::vector getClassification() const { @@ -84,8 +84,8 @@ class Tracker protected: virtual bool measure( - const autoware_auto_perception_msgs::msg::DetectedObject & object, - const rclcpp::Time & time) = 0; + const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, + const geometry_msgs::msg::Transform & self_transform) = 0; public: virtual bool getTrackedObject( diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/unknown_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/unknown_tracker.hpp index 4e7621759d329..18e6c0606a535 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/unknown_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/unknown_tracker.hpp @@ -57,13 +57,14 @@ class UnknownTracker : public Tracker public: UnknownTracker( - const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object); + const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object, + const geometry_msgs::msg::Transform & self_transform); bool predict(const rclcpp::Time & time) override; bool predict(const double dt, KalmanFilter & ekf) const; bool measure( - const autoware_auto_perception_msgs::msg::DetectedObject & object, - const rclcpp::Time & time) override; + const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, + const geometry_msgs::msg::Transform & self_transform) override; bool measureWithPose(const autoware_auto_perception_msgs::msg::DetectedObject & object); bool measureWithShape(const autoware_auto_perception_msgs::msg::DetectedObject & object); bool getTrackedObject( diff --git a/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp b/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp index a61ac10717f3e..7544a204d3e8e 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp @@ -19,6 +19,8 @@ #ifndef MULTI_OBJECT_TRACKER__UTILS__UTILS_HPP_ #define MULTI_OBJECT_TRACKER__UTILS__UTILS_HPP_ +#include +#include #include #include @@ -27,6 +29,7 @@ #include #include +#include #include #include #include @@ -72,12 +75,272 @@ enum MSG_COV_IDX { YAW_YAW = 35 }; +enum BBOX_IDX { + FRONT_SURFACE = 0, + RIGHT_SURFACE = 1, + REAR_SURFACE = 2, + LEFT_SURFACE = 3, + FRONT_R_CORNER = 4, + REAR_R_CORNER = 5, + REAR_L_CORNER = 6, + FRONT_L_CORNER = 7, + INSIDE = 8, + INVALID = -1 +}; + +/** + * @brief check if object label belongs to "large vehicle" + * @param label: input object label + * @return True if object label means large vehicle + */ inline bool isLargeVehicleLabel(const uint8_t label) { using Label = autoware_auto_perception_msgs::msg::ObjectClassification; return label == Label::BUS || label == Label::TRUCK || label == Label::TRAILER; } +/** + * @brief Determine the Nearest Corner or Surface of detected object observed from ego vehicle + * + * @param x: object x coordinate in map frame + * @param y: object y coordinate in map frame + * @param yaw: object yaw orientation in map frame + * @param width: object bounding box width + * @param length: object bounding box length + * @param self_transform: Ego vehicle position in map frame + * @return int index + */ +inline int getNearestCornerOrSurface( + const double x, const double y, const double yaw, const double width, const double length, + const geometry_msgs::msg::Transform & self_transform) +{ + // get local vehicle pose + const double x0 = self_transform.translation.x; + const double y0 = self_transform.translation.y; + + // localize self vehicle pose to object coordinate + // R.T (X0-X) + const double xl = std::cos(yaw) * (x0 - x) + std::sin(yaw) * (y0 - y); + const double yl = -std::sin(yaw) * (x0 - x) + std::cos(yaw) * (y0 - y); + + // Determine Index + // x+ (front) + // __ + // y+ | | y- + // (left) | | (right) + // -- + // x- (rear) + int xgrid, ygrid; + const int labels[3][3] = { + {BBOX_IDX::FRONT_L_CORNER, BBOX_IDX::FRONT_SURFACE, BBOX_IDX::FRONT_R_CORNER}, + {BBOX_IDX::LEFT_SURFACE, BBOX_IDX::INSIDE, BBOX_IDX::RIGHT_SURFACE}, + {BBOX_IDX::REAR_L_CORNER, BBOX_IDX::REAR_SURFACE, BBOX_IDX::REAR_R_CORNER}}; + if (xl > length / 2.0) { + xgrid = 0; // front + } else if (xl > -length / 2.0) { + xgrid = 1; // middle + } else { + xgrid = 2; // rear + } + if (yl > width / 2.0) { + ygrid = 0; // left + } else if (yl > -width / 2.0) { + ygrid = 1; // middle + } else { + ygrid = 2; // right + } + + return labels[xgrid][ygrid]; // 0 to 7 + 1(null) value +} + +/** + * @brief Get the Nearest Corner or Surface from detected object + * @param object: input object + * @param yaw: object yaw angle (after solved front and back uncertainty) + * @param self_transform + * @return nearest corner or surface index + */ +inline int getNearestCornerOrSurfaceFromObject( + const autoware_auto_perception_msgs::msg::DetectedObject & object, const double & yaw, + const geometry_msgs::msg::Transform & self_transform) +{ + // only work for BBOX shape + if (object.shape.type != autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + return BBOX_IDX::INVALID; + } + + // extract necessary information from input object + double x, y, width, length; + x = object.kinematics.pose_with_covariance.pose.position.x; + y = object.kinematics.pose_with_covariance.pose.position.y; + // yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); //not use raw yaw + // now + width = object.shape.dimensions.y; + length = object.shape.dimensions.x; + + return getNearestCornerOrSurface(x, y, yaw, width, length, self_transform); +} + +/** + * @brief Calc bounding box center offset caused by shape change + * @param dw: width update [m] = w_new - w_old + * @param dl: length update [m] = l_new - l_old + * @param indx: nearest corner index + * @return 2d offset vector caused by shape change + */ +inline Eigen::Vector2d calcOffsetVectorFromShapeChange( + const double dw, const double dl, const int indx) +{ + Eigen::Vector2d offset; + // if surface + if (indx == BBOX_IDX::FRONT_SURFACE) { + offset(0, 0) = dl / 2.0; // move forward + offset(1, 0) = 0; + } else if (indx == BBOX_IDX::RIGHT_SURFACE) { + offset(0, 0) = 0; + offset(1, 0) = -dw / 2.0; // move right + } else if (indx == BBOX_IDX::REAR_SURFACE) { + offset(0, 0) = -dl / 2.0; // move backward + offset(1, 0) = 0; + } else if (indx == BBOX_IDX::LEFT_SURFACE) { + offset(0, 0) = 0; + offset(1, 0) = dw / 2.0; // move left + } + // if corner + if (indx == BBOX_IDX::FRONT_R_CORNER) { + offset(0, 0) = dl / 2.0; // move forward + offset(1, 0) = -dw / 2.0; // move right + } else if (indx == BBOX_IDX::REAR_R_CORNER) { + offset(0, 0) = -dl / 2.0; // move backward + offset(1, 0) = -dw / 2.0; // move right + } else if (indx == BBOX_IDX::REAR_L_CORNER) { + offset(0, 0) = -dl / 2.0; // move backward + offset(1, 0) = dw / 2.0; // move left + } else if (indx == BBOX_IDX::FRONT_L_CORNER) { + offset(0, 0) = dl / 2.0; // move forward + offset(1, 0) = dw / 2.0; // move left + } + return offset; // do nothing if indx == INVALID or INSIDE +} + +/** + * @brief post-processing to recover bounding box center from tracking point and offset vector + * @param x: x of tracking point estimated with ekf + * @param y: y of tracking point estimated with ekf + * @param yaw: yaw of tracking point estimated with ekf + * @param dw: diff of width: w_estimated - w_input + * @param dl: diff of length: l_estimated - l_input + * @param indx: closest corner or surface index + * @param tracking_offset: tracking offset between bounding box center and tracking point + */ +inline Eigen::Vector2d recoverFromTrackingPoint( + const double x, const double y, const double yaw, const double dw, const double dl, + const int indx, const Eigen::Vector2d & tracking_offset) +{ + const Eigen::Vector2d tracking_point{x, y}; + const Eigen::Matrix2d R = Eigen::Rotation2Dd(yaw).toRotationMatrix(); + + const Eigen::Vector2d shape_change_offset = calcOffsetVectorFromShapeChange(dw, dl, indx); + + Eigen::Vector2d output_center = tracking_point - R * tracking_offset - R * shape_change_offset; + + return output_center; +} + +/** + * @brief Convert input object center to tracking point based on nearest corner information + * 1. update anchor offset vector, 2. offset input bbox based on tracking_offset vector and + * prediction yaw angle + * @param w: last input bounding box width + * @param l: last input bounding box length + * @param indx: last input bounding box closest corner index + * @param input_object: input object bounding box + * @param yaw: current yaw estimation + * @param offset_object: output tracking measurement to feed ekf + * @return nearest corner index(int) + */ +inline void calcAnchorPointOffset( + const double w, const double l, const int indx, + const autoware_auto_perception_msgs::msg::DetectedObject & input_object, const double & yaw, + autoware_auto_perception_msgs::msg::DetectedObject & offset_object, + Eigen::Vector2d & tracking_offset) +{ + // copy value + offset_object = input_object; + // invalid index + if (indx == BBOX_IDX::INSIDE) { + return; // do nothing + } + + // current object width and height + double w_n, l_n; + l_n = input_object.shape.dimensions.x; + w_n = input_object.shape.dimensions.y; + + // update offset + const Eigen::Vector2d offset = calcOffsetVectorFromShapeChange(w_n - w, l_n - l, indx); + tracking_offset += offset; + + // offset input object + const Eigen::Matrix2d R = Eigen::Rotation2Dd(yaw).toRotationMatrix(); + const Eigen::Vector2d rotated_offset = R * tracking_offset; + offset_object.kinematics.pose_with_covariance.pose.position.x += rotated_offset.x(); + offset_object.kinematics.pose_with_covariance.pose.position.y += rotated_offset.y(); +} + +/** + * @brief convert convex hull shape object to bounding box object + * @param input_object: input convex hull objects + * @param output_object: output bounding box objects + */ +inline void convertConvexHullToBoundingBox( + const autoware_auto_perception_msgs::msg::DetectedObject & input_object, + autoware_auto_perception_msgs::msg::DetectedObject & output_object) +{ + const Eigen::Vector2d center{ + input_object.kinematics.pose_with_covariance.pose.position.x, + input_object.kinematics.pose_with_covariance.pose.position.y}; + const auto yaw = tf2::getYaw(input_object.kinematics.pose_with_covariance.pose.orientation); + const Eigen::Matrix2d Rinv = Eigen::Rotation2Dd(-yaw).toRotationMatrix(); + + double max_x = 0; + double max_y = 0; + double min_x = 0; + double min_y = 0; + double max_z = 0; + + // look for bounding box boundary + for (size_t i = 0; i < input_object.shape.footprint.points.size(); ++i) { + Eigen::Vector2d vertex{ + input_object.shape.footprint.points.at(i).x, input_object.shape.footprint.points.at(i).y}; + + const Eigen::Vector2d local_vertex = Rinv * (vertex - center); + max_x = std::max(max_x, local_vertex.x()); + max_y = std::max(max_y, local_vertex.y()); + min_x = std::min(min_x, local_vertex.x()); + min_y = std::min(min_y, local_vertex.y()); + + max_z = std::max(max_z, static_cast(input_object.shape.footprint.points.at(i).z)); + } + + // calc bounding box state + const double length = max_x - min_x; + const double width = max_y - min_y; + const double height = max_z; + const Eigen::Vector2d new_local_center{(max_x + min_x) / 2.0, (max_y + min_y) / 2.0}; + const Eigen::Vector2d new_center = center + Rinv.transpose() * new_local_center; + + // set output paramters + output_object = input_object; + output_object.kinematics.pose_with_covariance.pose.position.x = new_center.x(); + output_object.kinematics.pose_with_covariance.pose.position.y = new_center.y(); + + output_object.shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + output_object.shape.dimensions.x = length; + output_object.shape.dimensions.y = width; + output_object.shape.dimensions.z = height; +} + } // namespace utils #endif // MULTI_OBJECT_TRACKER__UTILS__UTILS_HPP_ diff --git a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp index 3ef9c66db9665..ea3e8604dd8a0 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp @@ -158,7 +158,7 @@ void MultiObjectTracker::onMeasurement( (*(tracker_itr)) ->updateWithMeasurement( transformed_objects.objects.at(direct_assignment.find(tracker_idx)->second), - measurement_time); + measurement_time, *self_transform); } else { // not found (*(tracker_itr))->updateWithoutMeasurement(); } @@ -175,7 +175,7 @@ void MultiObjectTracker::onMeasurement( continue; } std::shared_ptr tracker = - createNewTracker(transformed_objects.objects.at(i), measurement_time); + createNewTracker(transformed_objects.objects.at(i), measurement_time, *self_transform); if (tracker) list_tracker_.push_back(tracker); } @@ -185,30 +185,30 @@ void MultiObjectTracker::onMeasurement( } std::shared_ptr MultiObjectTracker::createNewTracker( - const autoware_auto_perception_msgs::msg::DetectedObject & object, - const rclcpp::Time & time) const + const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, + const geometry_msgs::msg::Transform & self_transform) const { const std::uint8_t label = perception_utils::getHighestProbLabel(object.classification); if (tracker_map_.count(label) != 0) { const auto tracker = tracker_map_.at(label); if (tracker == "bicycle_tracker") { - return std::make_shared(time, object); + return std::make_shared(time, object, self_transform); } else if (tracker == "big_vehicle_tracker") { - return std::make_shared(time, object); + return std::make_shared(time, object, self_transform); } else if (tracker == "multi_vehicle_tracker") { - return std::make_shared(time, object); + return std::make_shared(time, object, self_transform); } else if (tracker == "normal_vehicle_tracker") { - return std::make_shared(time, object); + return std::make_shared(time, object, self_transform); } else if (tracker == "pass_through_tracker") { - return std::make_shared(time, object); + return std::make_shared(time, object, self_transform); } else if (tracker == "pedestrian_and_bicycle_tracker") { - return std::make_shared(time, object); + return std::make_shared(time, object, self_transform); } else if (tracker == "pedestrian_tracker") { - return std::make_shared(time, object); + return std::make_shared(time, object, self_transform); } } - return std::make_shared(time, object); + return std::make_shared(time, object, self_transform); } void MultiObjectTracker::onTimer() diff --git a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp index d761c299d7d25..20dbd51e88eaf 100644 --- a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp @@ -41,7 +41,8 @@ using Label = autoware_auto_perception_msgs::msg::ObjectClassification; BicycleTracker::BicycleTracker( - const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object) + const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object, + const geometry_msgs::msg::Transform & /*self_transform*/) : Tracker(time, object.classification), logger_(rclcpp::get_logger("BicycleTracker")), last_update_time_(time), @@ -335,15 +336,16 @@ bool BicycleTracker::measureWithShape( } constexpr float gain = 0.9; - bounding_box_.width = gain * bounding_box_.width + (1.0 - gain) * object.shape.dimensions.x; - bounding_box_.length = gain * bounding_box_.length + (1.0 - gain) * object.shape.dimensions.y; + bounding_box_.length = gain * bounding_box_.length + (1.0 - gain) * object.shape.dimensions.x; + bounding_box_.width = gain * bounding_box_.width + (1.0 - gain) * object.shape.dimensions.y; bounding_box_.height = gain * bounding_box_.height + (1.0 - gain) * object.shape.dimensions.z; return true; } bool BicycleTracker::measure( - const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time) + const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, + const geometry_msgs::msg::Transform & /*self_transform*/) { const auto & current_classification = getClassification(); object_ = object; @@ -434,8 +436,8 @@ bool BicycleTracker::getTrackedObject( twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = P(IDX::WZ, IDX::WZ); // set shape - object.shape.dimensions.x = bounding_box_.width; - object.shape.dimensions.y = bounding_box_.length; + object.shape.dimensions.x = bounding_box_.length; + object.shape.dimensions.y = bounding_box_.width; object.shape.dimensions.z = bounding_box_.height; const auto origin_yaw = tf2::getYaw(object_.kinematics.pose_with_covariance.pose.orientation); const auto ekf_pose_yaw = tf2::getYaw(pose_with_cov.pose.orientation); diff --git a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp index 9473d291d333c..06200df9400f6 100644 --- a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp @@ -39,7 +39,8 @@ using Label = autoware_auto_perception_msgs::msg::ObjectClassification; BigVehicleTracker::BigVehicleTracker( - const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object) + const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object, + const geometry_msgs::msg::Transform & self_transform) : Tracker(time, object.classification), logger_(rclcpp::get_logger("BigVehicleTracker")), last_update_time_(time), @@ -131,10 +132,23 @@ BigVehicleTracker::BigVehicleTracker( if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { bounding_box_ = { object.shape.dimensions.x, object.shape.dimensions.y, object.shape.dimensions.z}; + last_input_bounding_box_ = bounding_box_; } else { - bounding_box_ = {2.0, 7.0, 2.0}; + // past defalut value + // bounding_box_ = {7.0, 2.0, 2.0}; + autoware_auto_perception_msgs::msg::DetectedObject bbox_object; + utils::convertConvexHullToBoundingBox(object, bbox_object); + bounding_box_ = { + bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, + bbox_object.shape.dimensions.z}; + last_input_bounding_box_ = { + bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, + bbox_object.shape.dimensions.z}; } ekf_.init(X, P); + + /* calc nearest corner index*/ + setNearestCornerOrSurfaceIndex(self_transform); // this index is used in next measure step } bool BigVehicleTracker::predict(const rclcpp::Time & time) @@ -249,29 +263,34 @@ bool BigVehicleTracker::measureWithPose( enable_velocity_measurement = true; } } + // pos x, pos y, yaw, vx depending on pose measurement const int dim_y = enable_velocity_measurement ? 4 : 3; - double measurement_yaw = tier4_autoware_utils::normalizeRadian( - tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation)); - { - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); - ekf_.getX(X_t); - // Fixed measurement_yaw to be in the range of +-90 degrees of X_t(IDX::YAW) - while (M_PI_2 <= X_t(IDX::YAW) - measurement_yaw) { - measurement_yaw = measurement_yaw + M_PI; - } - while (M_PI_2 <= measurement_yaw - X_t(IDX::YAW)) { - measurement_yaw = measurement_yaw - M_PI; - } + double measurement_yaw = getMeasurementYaw(object); // get sign-solved yaw angle + Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state + ekf_.getX(X_t); + + // convert to boundingbox if input is convex shape + autoware_auto_perception_msgs::msg::DetectedObject bbox_object; + if (object.shape.type != autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + utils::convertConvexHullToBoundingBox(object, bbox_object); + } else { + bbox_object = object; } + /* get offset measurement*/ + autoware_auto_perception_msgs::msg::DetectedObject offset_object; + utils::calcAnchorPointOffset( + last_input_bounding_box_.width, last_input_bounding_box_.length, last_nearest_corner_index_, + bbox_object, X_t(IDX::YAW), offset_object, tracking_offset_); + /* Set measurement matrix */ Eigen::MatrixXd Y(dim_y, 1); Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, ekf_params_.dim_x); Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y); - Y(IDX::X, 0) = object.kinematics.pose_with_covariance.pose.position.x; - Y(IDX::Y, 0) = object.kinematics.pose_with_covariance.pose.position.y; + Y(IDX::X, 0) = offset_object.kinematics.pose_with_covariance.pose.position.x; + Y(IDX::Y, 0) = offset_object.kinematics.pose_with_covariance.pose.position.y; Y(IDX::YAW, 0) = measurement_yaw; C(0, IDX::X) = 1.0; // for pos x C(1, IDX::Y) = 1.0; // for pos y @@ -310,6 +329,7 @@ bool BigVehicleTracker::measureWithPose( } } + /* ekf tracks constant tracking point */ if (!ekf_.update(Y, C, R)) { RCLCPP_WARN(logger_, "Cannot update"); } @@ -340,20 +360,29 @@ bool BigVehicleTracker::measureWithPose( bool BigVehicleTracker::measureWithShape( const autoware_auto_perception_msgs::msg::DetectedObject & object) { + autoware_auto_perception_msgs::msg::DetectedObject bbox_object; + + // if input is convex shape convert it to bbox shape if (object.shape.type != autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { - return false; + utils::convertConvexHullToBoundingBox(object, bbox_object); + } else { + bbox_object = object; } - constexpr float gain = 0.9; - - bounding_box_.width = gain * bounding_box_.width + (1.0 - gain) * object.shape.dimensions.x; - bounding_box_.length = gain * bounding_box_.length + (1.0 - gain) * object.shape.dimensions.y; - bounding_box_.height = gain * bounding_box_.height + (1.0 - gain) * object.shape.dimensions.z; + constexpr float gain = 0.9; + bounding_box_.length = + gain * bounding_box_.length + (1.0 - gain) * bbox_object.shape.dimensions.x; + bounding_box_.width = gain * bounding_box_.width + (1.0 - gain) * bbox_object.shape.dimensions.y; + bounding_box_.height = + gain * bounding_box_.height + (1.0 - gain) * bbox_object.shape.dimensions.z; + last_input_bounding_box_ = { + bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, bbox_object.shape.dimensions.z}; return true; } bool BigVehicleTracker::measure( - const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time) + const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, + const geometry_msgs::msg::Transform & self_transform) { const auto & current_classification = getClassification(); object_ = object; @@ -370,6 +399,9 @@ bool BigVehicleTracker::measure( measureWithPose(object); measureWithShape(object); + /* calc nearest corner index*/ + setNearestCornerOrSurfaceIndex(self_transform); // this index is used in next measure step + return true; } @@ -391,9 +423,18 @@ bool BigVehicleTracker::getTrackedObject( tmp_ekf_for_no_update.getX(X_t); tmp_ekf_for_no_update.getP(P); + /* put predicted pose and twist to output object */ auto & pose_with_cov = object.kinematics.pose_with_covariance; auto & twist_with_cov = object.kinematics.twist_with_covariance; + // recover bounding box from tracking point + const double dl = bounding_box_.length - last_input_bounding_box_.length; + const double dw = bounding_box_.width - last_input_bounding_box_.width; + const Eigen::Vector2d recovered_pose = utils::recoverFromTrackingPoint( + X_t(IDX::X), X_t(IDX::Y), X_t(IDX::YAW), dw, dl, last_nearest_corner_index_, tracking_offset_); + X_t(IDX::X) = recovered_pose.x(); + X_t(IDX::Y) = recovered_pose.y(); + // position pose_with_cov.pose.position.x = X_t(IDX::X); pose_with_cov.pose.position.y = X_t(IDX::Y); @@ -444,8 +485,8 @@ bool BigVehicleTracker::getTrackedObject( twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = P(IDX::WZ, IDX::WZ); // set shape - object.shape.dimensions.x = bounding_box_.width; - object.shape.dimensions.y = bounding_box_.length; + object.shape.dimensions.x = bounding_box_.length; + object.shape.dimensions.y = bounding_box_.width; object.shape.dimensions.z = bounding_box_.height; const auto origin_yaw = tf2::getYaw(object_.kinematics.pose_with_covariance.pose.orientation); const auto ekf_pose_yaw = tf2::getYaw(pose_with_cov.pose.orientation); @@ -454,3 +495,32 @@ bool BigVehicleTracker::getTrackedObject( return true; } + +void BigVehicleTracker::setNearestCornerOrSurfaceIndex( + const geometry_msgs::msg::Transform & self_transform) +{ + Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); + ekf_.getX(X_t); + last_nearest_corner_index_ = utils::getNearestCornerOrSurface( + X_t(IDX::X), X_t(IDX::Y), X_t(IDX::YAW), bounding_box_.width, bounding_box_.length, + self_transform); +} + +double BigVehicleTracker::getMeasurementYaw( + const autoware_auto_perception_msgs::msg::DetectedObject & object) +{ + double measurement_yaw = tier4_autoware_utils::normalizeRadian( + tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation)); + { + Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); + ekf_.getX(X_t); + // Fixed measurement_yaw to be in the range of +-90 degrees of X_t(IDX::YAW) + while (M_PI_2 <= X_t(IDX::YAW) - measurement_yaw) { + measurement_yaw = measurement_yaw + M_PI; + } + while (M_PI_2 <= measurement_yaw - X_t(IDX::YAW)) { + measurement_yaw = measurement_yaw - M_PI; + } + } + return measurement_yaw; +} diff --git a/perception/multi_object_tracker/src/tracker/model/multiple_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/multiple_vehicle_tracker.cpp index e84006495b420..99e72d26ac0c8 100644 --- a/perception/multi_object_tracker/src/tracker/model/multiple_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/multiple_vehicle_tracker.cpp @@ -23,10 +23,11 @@ using Label = autoware_auto_perception_msgs::msg::ObjectClassification; MultipleVehicleTracker::MultipleVehicleTracker( - const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object) + const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object, + const geometry_msgs::msg::Transform & self_transform) : Tracker(time, object.classification), - normal_vehicle_tracker_(time, object), - big_vehicle_tracker_(time, object) + normal_vehicle_tracker_(time, object, self_transform), + big_vehicle_tracker_(time, object, self_transform) { } @@ -38,10 +39,11 @@ bool MultipleVehicleTracker::predict(const rclcpp::Time & time) } bool MultipleVehicleTracker::measure( - const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time) + const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, + const geometry_msgs::msg::Transform & self_transform) { - big_vehicle_tracker_.measure(object, time); - normal_vehicle_tracker_.measure(object, time); + big_vehicle_tracker_.measure(object, time, self_transform); + normal_vehicle_tracker_.measure(object, time, self_transform); if (perception_utils::getHighestProbLabel(object.classification) != Label::UNKNOWN) setClassification(object.classification); return true; diff --git a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp index 77650de5e2313..69d7e9b19ae88 100644 --- a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp @@ -39,7 +39,8 @@ using Label = autoware_auto_perception_msgs::msg::ObjectClassification; NormalVehicleTracker::NormalVehicleTracker( - const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object) + const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object, + const geometry_msgs::msg::Transform & self_transform) : Tracker(time, object.classification), logger_(rclcpp::get_logger("NormalVehicleTracker")), last_update_time_(time), @@ -131,10 +132,24 @@ NormalVehicleTracker::NormalVehicleTracker( if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { bounding_box_ = { object.shape.dimensions.x, object.shape.dimensions.y, object.shape.dimensions.z}; + last_input_bounding_box_ = { + object.shape.dimensions.x, object.shape.dimensions.y, object.shape.dimensions.z}; } else { - bounding_box_ = {1.7, 4.0, 2.0}; + // past default value + // bounding_box_ = {4.0, 1.7, 2.0}; + autoware_auto_perception_msgs::msg::DetectedObject bbox_object; + utils::convertConvexHullToBoundingBox(object, bbox_object); + bounding_box_ = { + bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, + bbox_object.shape.dimensions.z}; + last_input_bounding_box_ = { + bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, + bbox_object.shape.dimensions.z}; } ekf_.init(X, P); + + /* calc nearest corner index*/ + setNearestCornerOrSurfaceIndex(self_transform); // this index is used in next measure step } bool NormalVehicleTracker::predict(const rclcpp::Time & time) @@ -236,11 +251,13 @@ bool NormalVehicleTracker::measureWithPose( r_cov_y = ekf_params_.r_cov_y; } + // extract current state + Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state + ekf_.getX(X_t); + // Decide dimension of measurement vector bool enable_velocity_measurement = false; if (object.kinematics.has_twist) { - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state - ekf_.getX(X_t); const double predicted_vx = X_t(IDX::VX); const double observed_vx = object.kinematics.twist_with_covariance.twist.linear.x; @@ -249,13 +266,12 @@ bool NormalVehicleTracker::measureWithPose( enable_velocity_measurement = true; } } + // pos x, pos y, yaw, vx depending on pose output const int dim_y = enable_velocity_measurement ? 4 : 3; double measurement_yaw = tier4_autoware_utils::normalizeRadian( tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation)); { - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); - ekf_.getX(X_t); // Fixed measurement_yaw to be in the range of +-90 degrees of X_t(IDX::YAW) while (M_PI_2 <= X_t(IDX::YAW) - measurement_yaw) { measurement_yaw = measurement_yaw + M_PI; @@ -265,13 +281,27 @@ bool NormalVehicleTracker::measureWithPose( } } + // convert to boundingbox if input is convex shape + autoware_auto_perception_msgs::msg::DetectedObject bbox_object; + if (object.shape.type != autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + utils::convertConvexHullToBoundingBox(object, bbox_object); + } else { + bbox_object = object; + } + + /* get offset measurement*/ + autoware_auto_perception_msgs::msg::DetectedObject offset_object; + utils::calcAnchorPointOffset( + last_input_bounding_box_.width, last_input_bounding_box_.length, last_nearest_corner_index_, + bbox_object, X_t(IDX::YAW), offset_object, tracking_offset_); + /* Set measurement matrix and noise covariance*/ Eigen::MatrixXd Y(dim_y, 1); Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, ekf_params_.dim_x); Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y); - Y(IDX::X, 0) = object.kinematics.pose_with_covariance.pose.position.x; - Y(IDX::Y, 0) = object.kinematics.pose_with_covariance.pose.position.y; + Y(IDX::X, 0) = offset_object.kinematics.pose_with_covariance.pose.position.x; + Y(IDX::Y, 0) = offset_object.kinematics.pose_with_covariance.pose.position.y; Y(IDX::YAW, 0) = measurement_yaw; C(0, IDX::X) = 1.0; // for pos x C(1, IDX::Y) = 1.0; // for pos y @@ -311,7 +341,7 @@ bool NormalVehicleTracker::measureWithPose( } } - // update + // ekf update: this tracks tracking point if (!ekf_.update(Y, C, R)) { RCLCPP_WARN(logger_, "Cannot update"); } @@ -342,20 +372,29 @@ bool NormalVehicleTracker::measureWithPose( bool NormalVehicleTracker::measureWithShape( const autoware_auto_perception_msgs::msg::DetectedObject & object) { + autoware_auto_perception_msgs::msg::DetectedObject bbox_object; + + // if input is convex shape convert it to bbox shape if (object.shape.type != autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { - return false; + utils::convertConvexHullToBoundingBox(object, bbox_object); + } else { + bbox_object = object; } - constexpr float gain = 0.9; - - bounding_box_.width = gain * bounding_box_.width + (1.0 - gain) * object.shape.dimensions.x; - bounding_box_.length = gain * bounding_box_.length + (1.0 - gain) * object.shape.dimensions.y; - bounding_box_.height = gain * bounding_box_.height + (1.0 - gain) * object.shape.dimensions.z; + constexpr float gain = 0.9; + bounding_box_.length = + gain * bounding_box_.length + (1.0 - gain) * bbox_object.shape.dimensions.x; + bounding_box_.width = gain * bounding_box_.width + (1.0 - gain) * bbox_object.shape.dimensions.y; + bounding_box_.height = + gain * bounding_box_.height + (1.0 - gain) * bbox_object.shape.dimensions.z; + last_input_bounding_box_ = { + bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, bbox_object.shape.dimensions.z}; return true; } bool NormalVehicleTracker::measure( - const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time) + const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, + const geometry_msgs::msg::Transform & self_transform) { const auto & current_classification = getClassification(); object_ = object; @@ -372,6 +411,15 @@ bool NormalVehicleTracker::measure( measureWithPose(object); measureWithShape(object); + // refinement + Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); + Eigen::MatrixXd P_t(ekf_params_.dim_x, ekf_params_.dim_x); + ekf_.getX(X_t); + ekf_.getP(P_t); + + /* calc nearest corner index*/ + setNearestCornerOrSurfaceIndex(self_transform); // this index is used in next measure step + return true; } @@ -396,6 +444,14 @@ bool NormalVehicleTracker::getTrackedObject( auto & pose_with_cov = object.kinematics.pose_with_covariance; auto & twist_with_cov = object.kinematics.twist_with_covariance; + // recover bounding box from tracking point + const double dl = bounding_box_.length - last_input_bounding_box_.length; + const double dw = bounding_box_.width - last_input_bounding_box_.width; + const Eigen::Vector2d recovered_pose = utils::recoverFromTrackingPoint( + X_t(IDX::X), X_t(IDX::Y), X_t(IDX::YAW), dw, dl, last_nearest_corner_index_, tracking_offset_); + X_t(IDX::X) = recovered_pose.x(); + X_t(IDX::Y) = recovered_pose.y(); + // position pose_with_cov.pose.position.x = X_t(IDX::X); pose_with_cov.pose.position.y = X_t(IDX::Y); @@ -446,8 +502,8 @@ bool NormalVehicleTracker::getTrackedObject( twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = P(IDX::WZ, IDX::WZ); // set shape - object.shape.dimensions.x = bounding_box_.width; - object.shape.dimensions.y = bounding_box_.length; + object.shape.dimensions.x = bounding_box_.length; + object.shape.dimensions.y = bounding_box_.width; object.shape.dimensions.z = bounding_box_.height; const auto origin_yaw = tf2::getYaw(object_.kinematics.pose_with_covariance.pose.orientation); const auto ekf_pose_yaw = tf2::getYaw(pose_with_cov.pose.orientation); @@ -455,3 +511,13 @@ bool NormalVehicleTracker::getTrackedObject( tier4_autoware_utils::rotatePolygon(object.shape.footprint, origin_yaw - ekf_pose_yaw); return true; } + +void NormalVehicleTracker::setNearestCornerOrSurfaceIndex( + const geometry_msgs::msg::Transform & self_transform) +{ + Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); + ekf_.getX(X_t); + last_nearest_corner_index_ = utils::getNearestCornerOrSurface( + X_t(IDX::X), X_t(IDX::Y), X_t(IDX::YAW), bounding_box_.width, bounding_box_.length, + self_transform); +} diff --git a/perception/multi_object_tracker/src/tracker/model/pass_through_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/pass_through_tracker.cpp index 427e5263ee5a2..b9349128879b5 100644 --- a/perception/multi_object_tracker/src/tracker/model/pass_through_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/pass_through_tracker.cpp @@ -37,7 +37,8 @@ #include PassThroughTracker::PassThroughTracker( - const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object) + const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object, + const geometry_msgs::msg::Transform & /*self_transform*/) : Tracker(time, object.classification), logger_(rclcpp::get_logger("PassThroughTracker")), last_update_time_(time) @@ -58,7 +59,8 @@ bool PassThroughTracker::predict(const rclcpp::Time & time) } bool PassThroughTracker::measure( - const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time) + const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, + const geometry_msgs::msg::Transform & self_transform) { prev_observed_object_ = object_; object_ = object; @@ -74,6 +76,7 @@ bool PassThroughTracker::measure( } last_update_time_ = time; + (void)self_transform; // currently do not use self vehicle position return true; } diff --git a/perception/multi_object_tracker/src/tracker/model/pedestrian_and_bicycle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/pedestrian_and_bicycle_tracker.cpp index 6ddc2d9aee4f2..14bcab256400a 100644 --- a/perception/multi_object_tracker/src/tracker/model/pedestrian_and_bicycle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/pedestrian_and_bicycle_tracker.cpp @@ -23,10 +23,11 @@ using Label = autoware_auto_perception_msgs::msg::ObjectClassification; PedestrianAndBicycleTracker::PedestrianAndBicycleTracker( - const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object) + const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object, + const geometry_msgs::msg::Transform & self_transform) : Tracker(time, object.classification), - pedestrian_tracker_(time, object), - bicycle_tracker_(time, object) + pedestrian_tracker_(time, object, self_transform), + bicycle_tracker_(time, object, self_transform) { } @@ -38,10 +39,11 @@ bool PedestrianAndBicycleTracker::predict(const rclcpp::Time & time) } bool PedestrianAndBicycleTracker::measure( - const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time) + const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, + const geometry_msgs::msg::Transform & self_transform) { - pedestrian_tracker_.measure(object, time); - bicycle_tracker_.measure(object, time); + pedestrian_tracker_.measure(object, time, self_transform); + bicycle_tracker_.measure(object, time, self_transform); if (perception_utils::getHighestProbLabel(object.classification) != Label::UNKNOWN) setClassification(object.classification); return true; diff --git a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp index 54b815aea3295..b006d47df68b3 100644 --- a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp @@ -41,7 +41,8 @@ using Label = autoware_auto_perception_msgs::msg::ObjectClassification; PedestrianTracker::PedestrianTracker( - const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object) + const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object, + const geometry_msgs::msg::Transform & /*self_transform*/) : Tracker(time, object.classification), logger_(rclcpp::get_logger("PedestrianTracker")), last_update_time_(time), @@ -299,8 +300,8 @@ bool PedestrianTracker::measureWithShape( { constexpr float gain = 0.9; if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { - bounding_box_.width = gain * bounding_box_.width + (1.0 - gain) * object.shape.dimensions.x; - bounding_box_.length = gain * bounding_box_.length + (1.0 - gain) * object.shape.dimensions.y; + bounding_box_.length = gain * bounding_box_.length + (1.0 - gain) * object.shape.dimensions.x; + bounding_box_.width = gain * bounding_box_.width + (1.0 - gain) * object.shape.dimensions.y; bounding_box_.height = gain * bounding_box_.height + (1.0 - gain) * object.shape.dimensions.z; } else if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { cylinder_.width = gain * cylinder_.width + (1.0 - gain) * object.shape.dimensions.x; @@ -313,7 +314,8 @@ bool PedestrianTracker::measureWithShape( } bool PedestrianTracker::measure( - const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time) + const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, + const geometry_msgs::msg::Transform & self_transform) { const auto & current_classification = getClassification(); object_ = object; @@ -330,6 +332,7 @@ bool PedestrianTracker::measure( measureWithPose(object); measureWithShape(object); + (void)self_transform; // currently do not use self vehicle position return true; } @@ -405,8 +408,8 @@ bool PedestrianTracker::getTrackedObject( // set shape if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { - object.shape.dimensions.x = bounding_box_.width; - object.shape.dimensions.y = bounding_box_.length; + object.shape.dimensions.x = bounding_box_.length; + object.shape.dimensions.y = bounding_box_.width; object.shape.dimensions.z = bounding_box_.height; } else if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { object.shape.dimensions.x = cylinder_.width; diff --git a/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp b/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp index 82db76d9901e2..ba684e4777947 100644 --- a/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp +++ b/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp @@ -38,12 +38,12 @@ Tracker::Tracker( bool Tracker::updateWithMeasurement( const autoware_auto_perception_msgs::msg::DetectedObject & object, - const rclcpp::Time & measurement_time) + const rclcpp::Time & measurement_time, const geometry_msgs::msg::Transform & self_transform) { no_measurement_count_ = 0; ++total_measurement_count_; last_update_with_measurement_time_ = measurement_time; - measure(object, measurement_time); + measure(object, measurement_time, self_transform); return true; } diff --git a/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp index a6a32dbd248b3..dde191534d363 100644 --- a/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp @@ -33,7 +33,8 @@ #include UnknownTracker::UnknownTracker( - const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object) + const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object, + const geometry_msgs::msg::Transform & /*self_transform*/) : Tracker(time, object.classification), logger_(rclcpp::get_logger("UnknownTracker")), last_update_time_(time), @@ -227,7 +228,8 @@ bool UnknownTracker::measureWithPose( } bool UnknownTracker::measure( - const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time) + const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, + const geometry_msgs::msg::Transform & /*self_transform*/) { object_ = object; From 0d372fb13ad816cac77985d7a5780b0b43472148 Mon Sep 17 00:00:00 2001 From: beyzanurkaya <32412808+beyzanurkaya@users.noreply.github.com> Date: Wed, 11 Jan 2023 18:38:06 +0300 Subject: [PATCH 08/49] feat(behavior_velocity_planner): add speed bump module (#647) Signed-off-by: Mehmet Dogru <42mehmetdogru42@gmail.com> Signed-off-by: Beyza Nur Kaya Co-authored-by: Kosuke Takeuchi Co-authored-by: Takayuki Murooka --- .../behavior_planning.launch.py | 3 + .../lanelet2_map_visualization_node.cpp | 11 +- .../behavior_velocity_planner/CMakeLists.txt | 1 + planning/behavior_velocity_planner/README.md | 2 + .../behavior_velocity_planner.param.yaml | 1 + .../config/speed_bump.param.yaml | 13 + .../docs/speed_bump/speed_bump_design.svg | 278 ++++++++++++++++++ .../docs/speed_bump/speed_bump_scenarios.svg | 219 ++++++++++++++ .../docs/speed_bump/speed_bump_vel_calc.png | Bin 0 -> 5864 bytes .../scene_module/speed_bump/manager.hpp | 46 +++ .../include/scene_module/speed_bump/scene.hpp | 84 ++++++ .../include/scene_module/speed_bump/util.hpp | 73 +++++ .../behavior_velocity_planner.launch.xml | 3 + .../speed-bump-design.md | 62 ++++ .../behavior_velocity_planner/src/node.cpp | 4 + .../src/scene_module/speed_bump/debug.cpp | 121 ++++++++ .../src/scene_module/speed_bump/manager.cpp | 80 +++++ .../src/scene_module/speed_bump/scene.cpp | 172 +++++++++++ .../src/scene_module/speed_bump/util.cpp | 171 +++++++++++ 19 files changed, 1342 insertions(+), 2 deletions(-) create mode 100644 planning/behavior_velocity_planner/config/speed_bump.param.yaml create mode 100644 planning/behavior_velocity_planner/docs/speed_bump/speed_bump_design.svg create mode 100644 planning/behavior_velocity_planner/docs/speed_bump/speed_bump_scenarios.svg create mode 100644 planning/behavior_velocity_planner/docs/speed_bump/speed_bump_vel_calc.png create mode 100644 planning/behavior_velocity_planner/include/scene_module/speed_bump/manager.hpp create mode 100644 planning/behavior_velocity_planner/include/scene_module/speed_bump/scene.hpp create mode 100644 planning/behavior_velocity_planner/include/scene_module/speed_bump/util.hpp create mode 100644 planning/behavior_velocity_planner/speed-bump-design.md create mode 100644 planning/behavior_velocity_planner/src/scene_module/speed_bump/debug.cpp create mode 100644 planning/behavior_velocity_planner/src/scene_module/speed_bump/manager.cpp create mode 100644 planning/behavior_velocity_planner/src/scene_module/speed_bump/scene.cpp create mode 100644 planning/behavior_velocity_planner/src/scene_module/speed_bump/util.cpp diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py index 1eccb82e4ae82..620610a629fdb 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py @@ -129,6 +129,8 @@ def launch_setup(context, *args, **kwargs): no_stopping_area_param = yaml.safe_load(f)["/**"]["ros__parameters"] with open(LaunchConfiguration("run_out_param_path").perform(context), "r") as f: run_out_param = yaml.safe_load(f)["/**"]["ros__parameters"] + with open(LaunchConfiguration("speed_bump_param_path").perform(context), "r") as f: + speed_bump_param = yaml.safe_load(f)["/**"]["ros__parameters"] with open( LaunchConfiguration("behavior_velocity_planner_param_path").perform(context), "r" ) as f: @@ -192,6 +194,7 @@ def launch_setup(context, *args, **kwargs): no_stopping_area_param, vehicle_param, run_out_param, + speed_bump_param, common_param, motion_velocity_smoother_param, behavior_velocity_smoother_type_param, diff --git a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp index 472b28264f617..6e94294c0900f 100644 --- a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp +++ b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp @@ -109,6 +109,8 @@ void Lanelet2MapVisualizationNode::onMapBin( lanelet::utils::query::detectionAreas(all_lanelets); std::vector no_reg_elems = lanelet::utils::query::noStoppingAreas(all_lanelets); + std::vector sb_reg_elems = + lanelet::utils::query::speedBumps(all_lanelets); lanelet::ConstLineStrings3d parking_spaces = lanelet::utils::query::getAllParkingSpaces(viz_lanelet_map); lanelet::ConstPolygons3d parking_lots = lanelet::utils::query::getAllParkingLots(viz_lanelet_map); @@ -122,8 +124,9 @@ void Lanelet2MapVisualizationNode::onMapBin( std_msgs::msg::ColorRGBA cl_road, cl_shoulder, cl_cross, cl_partitions, cl_pedestrian_markings, cl_ll_borders, cl_shoulder_borders, cl_stoplines, cl_trafficlights, cl_detection_areas, - cl_parking_lots, cl_parking_spaces, cl_lanelet_id, cl_obstacle_polygons, cl_no_stopping_areas, - cl_no_obstacle_segmentation_area, cl_no_obstacle_segmentation_area_for_run_out; + cl_speed_bumps, cl_parking_lots, cl_parking_spaces, cl_lanelet_id, cl_obstacle_polygons, + cl_no_stopping_areas, cl_no_obstacle_segmentation_area, + cl_no_obstacle_segmentation_area_for_run_out; setColor(&cl_road, 0.27, 0.27, 0.27, 0.999); setColor(&cl_shoulder, 0.15, 0.15, 0.15, 0.999); setColor(&cl_cross, 0.27, 0.3, 0.27, 0.5); @@ -135,6 +138,7 @@ void Lanelet2MapVisualizationNode::onMapBin( setColor(&cl_trafficlights, 0.5, 0.5, 0.5, 0.8); setColor(&cl_detection_areas, 0.27, 0.27, 0.37, 0.5); setColor(&cl_no_stopping_areas, 0.37, 0.37, 0.37, 0.5); + setColor(&cl_speed_bumps, 0.56, 0.40, 0.27, 0.5); setColor(&cl_obstacle_polygons, 0.4, 0.27, 0.27, 0.5); setColor(&cl_parking_lots, 0.5, 0.5, 0.0, 0.3); setColor(&cl_parking_spaces, 1.0, 0.647, 0.0, 0.6); @@ -173,6 +177,9 @@ void Lanelet2MapVisualizationNode::onMapBin( insertMarkerArray( &map_marker_array, lanelet::visualization::noStoppingAreasAsMarkerArray(no_reg_elems, cl_no_stopping_areas)); + insertMarkerArray( + &map_marker_array, + lanelet::visualization::speedBumpsAsMarkerArray(sb_reg_elems, cl_speed_bumps)); insertMarkerArray( &map_marker_array, lanelet::visualization::parkingLotsAsMarkerArray(parking_lots, cl_parking_lots)); diff --git a/planning/behavior_velocity_planner/CMakeLists.txt b/planning/behavior_velocity_planner/CMakeLists.txt index 3bbc956cb8b8c..044cacacf6e9c 100644 --- a/planning/behavior_velocity_planner/CMakeLists.txt +++ b/planning/behavior_velocity_planner/CMakeLists.txt @@ -22,6 +22,7 @@ set(scene_modules virtual_traffic_light occlusion_spot run_out + speed_bump ) foreach(scene_module IN LISTS scene_modules) diff --git a/planning/behavior_velocity_planner/README.md b/planning/behavior_velocity_planner/README.md index f0801787a8709..bf522589cf0b1 100644 --- a/planning/behavior_velocity_planner/README.md +++ b/planning/behavior_velocity_planner/README.md @@ -18,6 +18,7 @@ It consists of several modules. Please refer to the links listed below for detai - [Occlusion Spot](occlusion-spot-design.md) - [No Stopping Area](no-stopping-area-design.md) - [Run Out](run-out-design.md) +- [Speed Bump](speed-bump-design.md) When each module plans velocity, it considers based on `base_link`(center of rear-wheel axis) pose. So for example, in order to stop at a stop line with the vehicles' front on the stop line, it calculates `base_link` position from the distance between `base_link` to front and modifies path velocity from the `base_link` position. @@ -55,6 +56,7 @@ So for example, in order to stop at a stop line with the vehicles' front on the | `launch_stop_line` | bool | whether to launch stop_line module | | `launch_occlusion_spot` | bool | whether to launch occlusion_spot module | | `launch_run_out` | bool | whether to launch run_out module | +| `launch_speed_bump` | bool | whether to launch speed_bump module | | `forward_path_length` | double | forward path length | | `backward_path_length` | double | backward path length | | `max_accel` | double | (to be a global parameter) max acceleration of the vehicle | diff --git a/planning/behavior_velocity_planner/config/behavior_velocity_planner.param.yaml b/planning/behavior_velocity_planner/config/behavior_velocity_planner.param.yaml index 1e0777ecf994a..09ee0b00a0d4c 100644 --- a/planning/behavior_velocity_planner/config/behavior_velocity_planner.param.yaml +++ b/planning/behavior_velocity_planner/config/behavior_velocity_planner.param.yaml @@ -10,6 +10,7 @@ launch_occlusion_spot: true launch_no_stopping_area: true launch_run_out: false + launch_speed_bump: true forward_path_length: 1000.0 backward_path_length: 5.0 max_accel: -2.8 diff --git a/planning/behavior_velocity_planner/config/speed_bump.param.yaml b/planning/behavior_velocity_planner/config/speed_bump.param.yaml new file mode 100644 index 0000000000000..ef61bdb7439ec --- /dev/null +++ b/planning/behavior_velocity_planner/config/speed_bump.param.yaml @@ -0,0 +1,13 @@ +/**: + ros__parameters: + speed_bump: + slow_start_margin: 1.0 + slow_end_margin: 1.0 + print_debug_info: false + + # limits for speed bump height and slow down speed to create a linear equation + speed_calculation: + min_height: 0.05 # [m] + max_height: 0.30 # [m] + min_speed: 1.39 # [m/s] = [5 kph] + max_speed: 2.78 # [m/s] = [10 kph] diff --git a/planning/behavior_velocity_planner/docs/speed_bump/speed_bump_design.svg b/planning/behavior_velocity_planner/docs/speed_bump/speed_bump_design.svg new file mode 100644 index 0000000000000..b4b3271f112ff --- /dev/null +++ b/planning/behavior_velocity_planner/docs/speed_bump/speed_bump_design.svg @@ -0,0 +1,278 @@ + + + + + + + + + + + + + + +
+
+
+ Speed Bump +
+ Polygon +
+
+
+ + Speed B... + + + + + + + + + + + + + + + + +
+
+
+ second +
+ intersection +
+ point +
+
+
+ + second... + + + + + + +
+
+
+ slow +
+ end +
+ point +
+
+
+
+ + slow... + + + + + + + + +
+
+
+ first +
+ intersection +
+ point +
+
+
+ + first... + + + + + + +
+
+
+
+ slow +
+ start +
+ point +
+
+
+ + slow... + + + + + + +
+
+
+ EGO +
+
+
+ + EGO + + + + + + +
+
+
+ Path +
+
+
+ + Path + + + + + + + + + + + + + + +
+
+
+ slow_end_margin_to_base_link +
+
+
+ + slow_end_margin_to_base_link + + + + + + +
+
+
+ slow_start_margin_to_base_link +
+
+
+ + slow_start_margin_to_base_link + + + + + + +
+
+
+ slow_end_margin_to_base_link + = slow_end_margin + rear_overhang +
+
+
+ + slow_end_margin_to_base_link = slow_end_margin + rear_overhang + + + + + + +
+
+
+ slow_start_margin_to_base_link + = slow_start_margin + wheel_base + front_overhang +
+
+
+ + slow_start_margin_to_base_link = slow_start_margin + wheel_base + front_overhang + + + + + + + + +
+
+
+ direction +
+ of +
+ motion +
+
+
+ + direction... + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_velocity_planner/docs/speed_bump/speed_bump_scenarios.svg b/planning/behavior_velocity_planner/docs/speed_bump/speed_bump_scenarios.svg new file mode 100644 index 0000000000000..98434b6ca827a --- /dev/null +++ b/planning/behavior_velocity_planner/docs/speed_bump/speed_bump_scenarios.svg @@ -0,0 +1,219 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + virtual slow start point +
+ +
+
+
+ + virtual slow... + + + + + + + + + + + + + + + + + + + + +
+
+
+ + virtual slow end point +
+ +
+
+
+ + virtual slow... + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ EGO +
+
+
+ + EGO + + + + + + +
+
+
+ EGO +
+
+
+ + EGO + + + + + + + + + + + + +
+
+
+ + decide + + slow start point index + +
+ wrt the + + + first + + intersection point between path and speed bump polygon +
+
+
+ + decide slow start point index... + + + + + + +
+
+
+ + decide + + slow end point index + +
+ wrt the + + + second + + intersection point between path and speed bump polygon +
+
+
+ + decide slow end point index... + + + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_velocity_planner/docs/speed_bump/speed_bump_vel_calc.png b/planning/behavior_velocity_planner/docs/speed_bump/speed_bump_vel_calc.png new file mode 100644 index 0000000000000000000000000000000000000000..6412b3bc3ffeb2651f5c38c32477384971a5568b GIT binary patch literal 5864 zcmds5`9GB3-@fglvcyyrl08cyk)5FsVk{v`N=6tmcG;65kqnCL`@WN1jKpC0WXoXe zV;{0+eQuxSyF9Pg^A9}dhx^>;yw3VQ@Aq|G=Y&1fxp$6^jSc{Sa~k)d`T#)o68zty zB?rILq(EUXqd=-?7|_zv;-_>c!Qy3CwMVWnr{}I7Ph4yOTZEINjR^9oi;WEe33qZO zQ8X(60K`WFs%+r-d3D_Xff=!}V?&Icf}egM)xC;;)H+s4qYYAF7%q|}SdDo2@`9lB zyEl!lj1rmDqgj(`H=GM@$G^{OZxm7tLbp>0BDu4~KS8F1GB0II?o__!WVY-6j&5Uv zu)pIm8jDmdX4++74mxv200h3ew?YPRWv2=P zT$%Tr!ifRuw%a0JY_@_oO?{zHbvmg0!eD0G`zPCShgWQ~ig#2N()0?-`ov2yRb)z6 zf=DnH!}Ez6>W3x;+YKjA_#;hv_>uO6PQMWub`!VmmOTvK6BTnC1|$XLUiu$4M8%P##H3#!ka$Um0Lm z%RM~Y>e+^W7MQKA(Rtc7{@i?i&%?N;!U9oH?J{fUMaV-S-BMO>`u*tt&A`pADUvHb zJ0EAV93dMaqk}g|MtS&7l=xLBxe)Iv#3v3UK7&FxX16=FLmiX0Ya`X8=3}n!5SG8p zoac-_DST@nw3s@`CMlCNUMz7gAr+?!)sMZ~xyok#)n5l#htDS;e0Md2eNOQaSXh!y zG5bhdZEmSm=(n$dSH&j1n#?BkX1B;X>*X7DqVAU~C-t23Jft|fplNfLsq<`T#xZ)-!mJYtx)Tr6rIxDyM|@n|}3Z*@^@Z*LrC5jkn+R{VO8-Uv%<;Kn`N)`(4DpA3S|Ztu^!{8Z&U4_JRZX^MJqa@R?*0kI-xV26vvN2oFHuZk(RjWo_{;(UV8RC+@|jhVYm;_B&%T zx=qSno9|!l@ERJXg%byn{`NZ-FIao37nR8?#IhMR{$Rc1Z<#v;uR5{$GzKe<WLLS1$-*K5a#mfwnTUC7zwks7x&KPE7ok9+*w49JA z02LJ&pmaZa1f7?cx3TfEEC&eSXA0Bq?Cw77aq_#q!vd`Ha2E^m>n4Bx{8`RziKSWy z=ogqy_@l3Ho#cOVfOl$kbaZ6(fdHNk(Szt1eqD=Nmk^X^vJ(QaBeJpR3!tRI`ZsT= zQ*bpe2Juez4eIESoh;2OoZyU9TZZD*E)OCza z#jg{-V;SyT%w}Th4}r|w(@91(aF8I0%f6i}GL_k>Q>B8k4eaDfujxrFnq0nJ-*O@jvzogK~c#1gwlk@YEAX2g~uSVY8(noZ?c&)$L_= zRw#zXNk6jID<0FRYwS^-DnpvkaIZfat3~?y`qGPf&3t>eNyL?yRxKUZ7n#@Z&?xRH zf04Mr)L%pJla-P((mzc>jceZnfkjR9%{rHp>T6^dU9F4Gj(b{Ep*9%vuv2?c&8O5%%^z+k?h>5FqXKZWHdZmLmf(|IU)f z(v3)$E#r#nKQy3QQCfaK?+P+8X%HldJ0G`X-Kyg0v3EigOWzL_o10l$I#v8EwrJd6 zDk!P0uKpz_w(}6^7ePFKuD)^^VjAeH34_6qNaVeH9aynuIiF|lxL@rTEv<0mjOW!A zQN$pqk_ z&(QvqkU&RC{^Q4wC{Kkh@^uv0@@p_nOiT<74GrhAUWLu?8O{?_IUq-yq21ZpdF#?y z5FT{bN4mQofCDLCI_ge*sIIO)ipOtg$OOnltifIf$L{ESoBPXdw31&Cd6Q^Bjg(29z zy)P(NLIM~@Q_%NcWtj}d=3yO-at*-WTceD7smwgC9U5=3Am6!M3 zobORNN-#(RyHSAMcvMZe^3w#F=xhQ7Lb9QfXsYz{8QNv|RqEit8R;?_FCGDUD3f}k zxTK`(;ob)O9Ry1L`%@(oE}@&F=%uA4C9YHhp>~ehvNU1FDR4X$mDYsY_D!2f@S~Xw zVaBrb?L<0wS@{DUop5=U((~G3-kaERug&LZDojW=h89i5O_`x>_u@rYSJ$aCU^t*w z0`#cJ{%lP9X$q5XcTqM`f<{PXrKeki7&sSi3GsPvBWWC0(tONj6Uf>IhmDlEuZ{-O zFr*8YTKBQ=rT&?{0W)&&fCmN)xEMbsCqsM~nZT;T|2*?{4ydF6z3>I~#1AnsPj6n* zhrz(AX9D!JqJO($etv!z4o4`rc{>tZI`?Z13!=W|s|_^`4ULyCH}VY&Fv+y(%oztP zUpN&6X@ZnsT$EkpPgG!5OakfgS)G<48I43y#zLwSnpZB?ex8h1?f^*@XRV}#KmCJ|CpM$}!cT#{6ASG!tSB&m#O zyIN`06Rm6T=cp+UP1HDhx}woJ}5MbFRD6=lb!eAyt%L?yIm)eHS}a!tQx&0 z85ehX+sTA_Z+(sLb6Q&3Oh**q%S+W2!C$_VLxQphI)d&PFC3uBgX4)pG?lq&Vv7tLw2v_p(6X?VyfV;ziCP= zo7c9zZBOdz>h_0<%-E$I*1a2tODrw3>$|_2LmIITCZ?vQavTg@%o5XWK3`NSVn5re zM0KLs@(x<6g=9KAPL0DKAWfb47Tes2`>{y?} zIF!=!e^+2#$KWrvB`M9|DQ!)L(-S#P#|3GKnZ}A0cOOej!co-P^76rO^YJzqMWWKZ zn}^kocqzn~^obRSYT)GR>fa87!4Ma|aU(`EYm=dA;~P7uKIn^0^!0xoZsKr?zXo8s zv9Ymv2Kfi1U(;{JCj8@blD(J0Nad5b)AAc+{PgtnMGMbsY@}ZqoJVkX97s8T3=C}5 zG#|a2jLyo;l$4SRmv5Joc(Ux5llm1&!gdx5wWZyVkdTNmpG!Dd^JvV;yDJM8k#^15 zV;lD`78VxD`R=>!1(UUO(L1GxPv!a!FUb*bC#pEs+ma=Y{mB9g#dq@Ynr^n4aw*m? z4Dy4h?W>x$7(OH#JUXxsRG|f!v{rIOtf_Vc-@SYH>C>l+Viu2w>RV=7>oCAybi%(w zTZ;PfMOs+9vW+A~1zfvTr%M)K6Z9_?m7!hhd*J>LlF;6|eLKW0%pHWKrX~d$882t& zhbzil7?4fMMEgPX0zH<6l{G6nJ6xXj_4hxigWmtU_L28+Y#YHDhp!UQ92bV^Ff=g$#t zVJ{-2)UxzN_7>kWo&lQ%^b-|;(g*+2ejt<}YVg)&5m8ZZ!NoY&K7JGr?|{Q}QWPAe z5<9+q!*EclxVpJLF;BmmK{{Z$c(HkB)ShV@j}HJx3+&I%%sEYO<*9^UyM>WODD3_( z!~B~p0vR90UuuEFz`%**RP>wqcCL)(uet?XMx*b+&cMLH(qKXJ!8l2jU_SZsiMch9 zLE`2AGgwgn=LB{LMDIWJK&fJN+DRK+L+gT1C%Dm4-T#!dpfwSMrmM7+**Q7w;Ku%H zomWz#tE0nlOGf*vqP!2re`X=Y@qyLW{%qy1?8yX;S|H1%WqKGa9kXkn|h^? zf}PvP`5&u7&zv~}qL-!rQI&xZt52~HcbVV~isp^$;!n&ubaH;kT#3YBFlaQo(Pww< zX9a({lpxC|oG)Od$FK8w4{LeM9n;BO!B^485|9hxlam9X>8LMFHQFS`#FIe7^`a1z zl?(kD*OY%ZxQ=rWRXH&xbP+}b-_WCut&)NBl&Zb1pMMEUzioC_+8ru8;Pmat9ef18 zJgFK9-iyk#_2x=HJ&SUl{+KZ*p?jY*C^_zh@mIe#3eF@%J{CDt?XnpKw$nqjKAFga z-59nz>FBx8|9Vp?&8S|jVCa&_MNe)V*}LAQ>ul9*hP9*QzG`lH-uRVoAr;0;Rrzkq z)m5HBV`WPaW8I{U)PsrGlNVK;55Bpk!?J3rk&d7Aa`cTWO&L}M2c7iU@zQ_4(2Q^H z$inHATCm^I$H0p+0o>9>?x~{p67`c~67p3FP zN*sC9H55`e#(ye%%4hsBW}FGld}W>?tqXF2$ELZ#MAH z@n!{~lB2QZHb0rU^MrX9nl*T?*FK(i`;8J`lM5fuZa7%0w~hkdw$#a0dulAi(>;Ca z(X(G$B99cCdXv2Sk#L0M@4!`sT5dt*cF9n@|NS`PBkrP>P~!hx7n&@gBf1xGk49De zn!nD{w*0z$N_~Sx%3}qR;bTcEuI$|WL_>M?ZAg)~hi%u5pB}q>AA-7E{2SHld*Dfr z;~SkjRG`qyOJiwwdlW)X)&f_wUyw|Or|Y9-{AzBb8srdUTh?qh$v#du_f36ZVJVE? zQ2$RHPD|A!%EqAUT{ef6sXRlphMPHjA%txDazFTuL_ z)$3G7y;tsDzQr@ap}zA)r!N-O*2lLXJAw^4Rjsd{>ofE8_}Txa?&PMikupy<{BWIL zQ*dt*Wp~7EOnFVXNt8l6dsIShqv6+Zb=?)&A>RFNOhdqUWz|#{825wot=#w?H3kcW zz^f9o?rWDT7wQfzPokq%CO|U8o=Y&_(J0hnv7W5OT_S43gi!b63f3*_Hp6#=f`^^< zab{YCKbAerK3_=J!A7bL^IF*LyQM{-WbtBHnC9W5_=UCsB8-pI^CotnFD||N{l0G8 z5<89LLF^6LOEZu{dc{7|{^=69m!&en$~>Kc;sCwn@Ova66!lFmIcHdK`l77#)W!ow zCXLJwY6KgKDUmu+XmU2)azeuVuAx5nFOZqQPAnSAgFELprR1YCt(pzlT+xuT25y6| zub}HhS0$Eac;&?ITy6C~Ym7->c2Utg2*Z|0vg=0kw88Vx*5>Kw=}8AbOV^XhS_YsH g6u5uCkQ{T;J{M}Rdgf>YzEuMnYC6zj6|12C0UH_*YXATM literal 0 HcmV?d00001 diff --git a/planning/behavior_velocity_planner/include/scene_module/speed_bump/manager.hpp b/planning/behavior_velocity_planner/include/scene_module/speed_bump/manager.hpp new file mode 100644 index 0000000000000..d64870f6a95d4 --- /dev/null +++ b/planning/behavior_velocity_planner/include/scene_module/speed_bump/manager.hpp @@ -0,0 +1,46 @@ +// Copyright 2020 Tier IV, Inc., Leo Drive Teknoloji A.Ş. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef SCENE_MODULE__SPEED_BUMP__MANAGER_HPP_ +#define SCENE_MODULE__SPEED_BUMP__MANAGER_HPP_ + +#include +#include +#include + +#include + +#include +#include + +namespace behavior_velocity_planner +{ +class SpeedBumpModuleManager : public SceneModuleManagerInterface +{ +public: + explicit SpeedBumpModuleManager(rclcpp::Node & node); + + const char * getModuleName() override { return "speed_bump"; } + +private: + SpeedBumpModule::PlannerParam planner_param_; + + void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + + std::function &)> getModuleExpiredFunction( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; +}; +} // namespace behavior_velocity_planner + +#endif // SCENE_MODULE__SPEED_BUMP__MANAGER_HPP_ diff --git a/planning/behavior_velocity_planner/include/scene_module/speed_bump/scene.hpp b/planning/behavior_velocity_planner/include/scene_module/speed_bump/scene.hpp new file mode 100644 index 0000000000000..74f9da149386e --- /dev/null +++ b/planning/behavior_velocity_planner/include/scene_module/speed_bump/scene.hpp @@ -0,0 +1,84 @@ +// Copyright 2020 Tier IV, Inc., Leo Drive Teknoloji A.Ş. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef SCENE_MODULE__SPEED_BUMP__SCENE_HPP_ +#define SCENE_MODULE__SPEED_BUMP__SCENE_HPP_ + +#include +#include +#include + +#include +#include + +namespace behavior_velocity_planner +{ +using autoware_auto_planning_msgs::msg::PathWithLaneId; + +class SpeedBumpModule : public SceneModuleInterface +{ +public: + struct DebugData + { + double base_link2front; + PathPolygonIntersectionStatus path_polygon_intersection_status; + std::vector slow_start_poses; + std::vector slow_end_points; + std::vector speed_bump_polygon; + }; + + struct PlannerParam + { + double slow_start_margin; + double slow_end_margin; + bool print_debug_info; + float speed_calculation_min_height; + float speed_calculation_max_height; + float speed_calculation_min_speed; + float speed_calculation_max_speed; + }; + + SpeedBumpModule( + const int64_t module_id, const int64_t lane_id, + const lanelet::autoware::SpeedBump & speed_bump_reg_elem, const PlannerParam & planner_param, + const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock); + + bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override; + + visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; + visualization_msgs::msg::MarkerArray createVirtualWallMarkerArray() override; + +private: + int64_t module_id_; + int64_t lane_id_; + + // Speed Bump Regulatory Element + const lanelet::autoware::SpeedBump & speed_bump_reg_elem_; + + // Parameter + PlannerParam planner_param_; + + // Debug + DebugData debug_data_; + + bool applySlowDownSpeed( + PathWithLaneId & output, const float speed_bump_speed, + const PathPolygonIntersectionStatus & path_polygon_intersection_status); + + float speed_bump_slow_down_speed_; +}; + +} // namespace behavior_velocity_planner + +#endif // SCENE_MODULE__SPEED_BUMP__SCENE_HPP_ diff --git a/planning/behavior_velocity_planner/include/scene_module/speed_bump/util.hpp b/planning/behavior_velocity_planner/include/scene_module/speed_bump/util.hpp new file mode 100644 index 0000000000000..2aafee4a113ab --- /dev/null +++ b/planning/behavior_velocity_planner/include/scene_module/speed_bump/util.hpp @@ -0,0 +1,73 @@ +// Copyright 2020 Tier IV, Inc., Leo Drive Teknoloji A.Ş. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef SCENE_MODULE__SPEED_BUMP__UTIL_HPP_ +#define SCENE_MODULE__SPEED_BUMP__UTIL_HPP_ + +#include +#include + +#include +#include +#include +#include +#include + +#define EIGEN_MPL2_ONLY + +#include +#include + +#include + +#include + +namespace behavior_velocity_planner +{ + +namespace bg = boost::geometry; + +using autoware_auto_planning_msgs::msg::PathPointWithLaneId; +using autoware_auto_planning_msgs::msg::PathWithLaneId; +using geometry_msgs::msg::Point32; + +// the status of intersection between path and speed bump +struct PathPolygonIntersectionStatus +{ + bool is_path_inside_of_polygon = false; // true if path is completely inside the speed bump + // polygon (no intersection point) + boost::optional first_intersection_point; + boost::optional second_intersection_point; +}; + +PathPolygonIntersectionStatus getPathPolygonIntersectionStatus( + const PathWithLaneId & ego_path, const lanelet::BasicPolygon2d & polygon, + const geometry_msgs::msg::Point & ego_pos, const size_t max_num); + +bool isNoRelation(const PathPolygonIntersectionStatus & status); + +bool insertConstSpeedToPathSection( + std::vector & output, const size_t start_idx, const size_t end_idx, + const float speed); + +boost::optional insertPointWithOffset( + const geometry_msgs::msg::Point & src_point, const double longitudinal_offset, + std::vector & output, const double overlap_threshold = 1e-3); + +// returns y (speed) for y=mx+b +float calcSlowDownSpeed(const Point32 & p1, const Point32 & p2, const float speed_bump_height); + +} // namespace behavior_velocity_planner + +#endif // SCENE_MODULE__SPEED_BUMP__UTIL_HPP_ diff --git a/planning/behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml b/planning/behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml index adbf844ce0376..12c1db523efe8 100644 --- a/planning/behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml +++ b/planning/behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml @@ -18,6 +18,7 @@ + @@ -36,6 +37,7 @@ + @@ -62,6 +64,7 @@ + diff --git a/planning/behavior_velocity_planner/speed-bump-design.md b/planning/behavior_velocity_planner/speed-bump-design.md new file mode 100644 index 0000000000000..5cd72e0745a4e --- /dev/null +++ b/planning/behavior_velocity_planner/speed-bump-design.md @@ -0,0 +1,62 @@ +## Speed Bump + +### Role + +This module plans the velocity of the related part of the path in case there is speed bump +regulatory element referring to it. + +![speed_bump_design.svg](docs%2Fspeed_bump%2Fspeed_bump_design.svg) + +### Activation Timing + +The manager launch speed bump scene module when there is speed bump regulatory element referring to +the reference path. + +### Module Parameters + +| Parameter | Type | Description | +| ------------------- | ------ | --------------------------------------------------------- | +| `slow_start_margin` | double | [m] margin for ego vehicle to slow down before speed_bump | +| `slow_end_margin` | double | [m] margin for ego vehicle to accelerate after speed_bump | +| `print_debug_info` | bool | whether debug info will be printed or not | + +#### Speed Calculation + +- limits for speed bump height and slow down speed to create a linear equation + +| Parameter | Type | Description | +| ------------ | ------ | ------------------------------------------------- | +| `min_height` | double | [m] minimum height assumption of the speed bump | +| `max_height` | double | [m] maximum height assumption of the speed bump | +| `min_speed` | double | [m/s] minimum speed assumption of slow down speed | +| `max_speed` | double | [m/s] maximum speed assumption of slow down speed | + +### Inner-workings / Algorithms + +- Get speed bump regulatory element on the path from lanelet2 map +- Calculate `slow_down_speed` wrt to `speed_bump_height` specified in regulatory element or + read `slow_down_speed` tag from speed bump annotation if available + +![speed_bump_vel_calc](docs/speed_bump/speed_bump_vel_calc.png) + +**Note:** If in speed bump annotation `slow_down_speed` tag is used then calculating the speed wrt +the speed bump height will be ignored. In such case, specified `slow_down_speed` value in **[kph]** +is being used. + +- Get the intersection points between path and speed bump polygon +- Calculate `slow_start_point` & `slow_end_point` wrt the intersection points and insert them to + path +- If `slow_start_point` or `slow_end_point` can not be inserted with given/calculated offset values + check if any path point can be virtually assigned as `slow_start_point` or `slow_end_point` + +![speed_bump_scenarios.svg](docs%2Fspeed_bump%2Fspeed_bump_scenarios.svg) + +- Assign `slow_down_speed` to the path points between `slow_start_point` or `slow_end_point` + +### Future Work + +- In an article [here](https://journals.sagepub.com/doi/10.1155/2014/736576), a bump modeling method + is proposed. Simply it is based on fitting the bump in a circle and a radius calculation is done + with it. Although the velocity calculation is based on just the height of the bump in the recent + implementation, applying this method is intended in the future which will yield more realistic + results. diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp index ab5d31e01c224..d01df2a0a3449 100644 --- a/planning/behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/src/node.cpp @@ -41,6 +41,7 @@ #include #include #include +#include #include #include #include @@ -207,6 +208,9 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio if (this->declare_parameter("launch_run_out", false)) { planner_manager_.launchSceneModule(std::make_shared(*this)); } + if (this->declare_parameter("launch_speed_bump", true)) { + planner_manager_.launchSceneModule(std::make_shared(*this)); + } } // NOTE: argument planner_data must not be referenced for multithreading diff --git a/planning/behavior_velocity_planner/src/scene_module/speed_bump/debug.cpp b/planning/behavior_velocity_planner/src/scene_module/speed_bump/debug.cpp new file mode 100644 index 0000000000000..a51a14f747f4b --- /dev/null +++ b/planning/behavior_velocity_planner/src/scene_module/speed_bump/debug.cpp @@ -0,0 +1,121 @@ +// Copyright 2020 Tier IV, Inc., Leo Drive Teknoloji A.Ş. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "scene_module/speed_bump/scene.hpp" + +#include +#include +#include + +#include + +namespace behavior_velocity_planner +{ +using motion_utils::createSlowDownVirtualWallMarker; +using tier4_autoware_utils::appendMarkerArray; +using tier4_autoware_utils::calcOffsetPose; +using tier4_autoware_utils::createDefaultMarker; +using tier4_autoware_utils::createMarkerColor; +using tier4_autoware_utils::createMarkerScale; +using tier4_autoware_utils::createPoint; +using visualization_msgs::msg::Marker; + +namespace +{ +visualization_msgs::msg::MarkerArray createSpeedBumpMarkers( + const SpeedBumpModule::DebugData & debug_data, const rclcpp::Time & now, const int64_t module_id) +{ + visualization_msgs::msg::MarkerArray msg; + const int32_t uid = planning_utils::bitShift(module_id); + + // Speed bump polygon + if (!debug_data.speed_bump_polygon.empty()) { + auto marker = createDefaultMarker( + "map", now, "speed_bump polygon", uid, Marker::LINE_STRIP, createMarkerScale(0.1, 0.0, 0.0), + createMarkerColor(0.0, 0.0, 1.0, 0.999)); + for (const auto & p : debug_data.speed_bump_polygon) { + marker.points.push_back(createPoint(p.x, p.y, p.z)); + } + marker.points.push_back(marker.points.front()); + msg.markers.push_back(marker); + } + + // Slow start point + if (!debug_data.slow_start_poses.empty()) { + auto marker = createDefaultMarker( + "map", now, "slow start point", uid, Marker::POINTS, createMarkerScale(0.25, 0.25, 0.0), + createMarkerColor(1.0, 1.0, 0.0, 0.999)); + for (const auto & p : debug_data.slow_start_poses) { + marker.points.push_back(createPoint(p.position.x, p.position.y, p.position.z)); + } + msg.markers.push_back(marker); + } + + // Slow end point + if (!debug_data.slow_end_points.empty()) { + auto marker = createDefaultMarker( + "map", now, "slow end point", uid, Marker::POINTS, createMarkerScale(0.25, 0.25, 0.0), + createMarkerColor(0.2, 0.8, 1.0, 0.999)); + for (const auto & p : debug_data.slow_end_points) { + marker.points.push_back(createPoint(p.x, p.y, p.z)); + } + msg.markers.push_back(marker); + } + + // Path - polygon intersection points + { + auto marker = createDefaultMarker( + "map", now, "path_polygon intersection points", uid, Marker::POINTS, + createMarkerScale(0.25, 0.25, 0.0), createMarkerColor(1.0, 0.0, 0.0, 0.999)); + const auto & p_first = debug_data.path_polygon_intersection_status.first_intersection_point; + if (p_first) { + marker.points.push_back(createPoint(p_first->x, p_first->y, p_first->z)); + } + const auto & p_second = debug_data.path_polygon_intersection_status.second_intersection_point; + if (p_second) { + marker.points.push_back(createPoint(p_second->x, p_second->y, p_second->z)); + } + if (!marker.points.empty()) msg.markers.push_back(marker); + } + + return msg; +} +} // namespace + +visualization_msgs::msg::MarkerArray SpeedBumpModule::createVirtualWallMarkerArray() +{ + const auto now = this->clock_->now(); + auto id = module_id_; + + visualization_msgs::msg::MarkerArray wall_marker; + for (const auto & p : debug_data_.slow_start_poses) { + const auto & p_front = calcOffsetPose(p, debug_data_.base_link2front, 0.0, 0.0); + appendMarkerArray( + createSlowDownVirtualWallMarker(p_front, "speed_bump", now, id++), &wall_marker); + } + + return wall_marker; +} + +visualization_msgs::msg::MarkerArray SpeedBumpModule::createDebugMarkerArray() +{ + visualization_msgs::msg::MarkerArray debug_marker_array; + + appendMarkerArray( + createSpeedBumpMarkers(debug_data_, this->clock_->now(), module_id_), &debug_marker_array); + + return debug_marker_array; +} + +} // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/src/scene_module/speed_bump/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/speed_bump/manager.cpp new file mode 100644 index 0000000000000..14882f0c2a17b --- /dev/null +++ b/planning/behavior_velocity_planner/src/scene_module/speed_bump/manager.cpp @@ -0,0 +1,80 @@ +// Copyright 2020 Tier IV, Inc., Leo Drive Teknoloji A.Ş. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "scene_module/speed_bump/manager.hpp" + +#include + +#include + +#include +#include +#include +#include +#include +#include + +namespace behavior_velocity_planner +{ +using lanelet::autoware::SpeedBump; + +SpeedBumpModuleManager::SpeedBumpModuleManager(rclcpp::Node & node) +: SceneModuleManagerInterface(node, getModuleName()) +{ + std::string ns(getModuleName()); + planner_param_.slow_start_margin = node.declare_parameter(ns + ".slow_start_margin", 1.0); + planner_param_.slow_end_margin = node.declare_parameter(ns + ".slow_end_margin", 1.0); + planner_param_.print_debug_info = node.declare_parameter(ns + ".print_debug_info", false); + + // limits for speed bump height and slow down speed + ns += ".speed_calculation"; + planner_param_.speed_calculation_min_height = + static_cast(node.declare_parameter(ns + ".min_height", 0.05)); + planner_param_.speed_calculation_max_height = + static_cast(node.declare_parameter(ns + ".max_height", 0.30)); + planner_param_.speed_calculation_min_speed = + static_cast(node.declare_parameter(ns + ".min_speed", 1.39)); + planner_param_.speed_calculation_max_speed = + static_cast(node.declare_parameter(ns + ".max_speed", 2.78)); +} + +void SpeedBumpModuleManager::launchNewModules( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path) +{ + for (const auto & speed_bump_with_lane_id : planning_utils::getRegElemMapOnPath( + path, planner_data_->route_handler_->getLaneletMapPtr(), + planner_data_->current_pose.pose)) { + const auto lane_id = speed_bump_with_lane_id.second.id(); + const auto module_id = speed_bump_with_lane_id.first->id(); + if (!isModuleRegistered(module_id)) { + registerModule(std::make_shared( + module_id, lane_id, *speed_bump_with_lane_id.first, planner_param_, + logger_.get_child("speed_bump_module"), clock_)); + } + } +} + +std::function &)> +SpeedBumpModuleManager::getModuleExpiredFunction( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path) +{ + const auto speed_bump_id_set = planning_utils::getRegElemIdSetOnPath( + path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_pose.pose); + + return [speed_bump_id_set](const std::shared_ptr & scene_module) { + return speed_bump_id_set.count(scene_module->getModuleId()) == 0; + }; +} + +} // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/src/scene_module/speed_bump/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/speed_bump/scene.cpp new file mode 100644 index 0000000000000..bfbe86a80c718 --- /dev/null +++ b/planning/behavior_velocity_planner/src/scene_module/speed_bump/scene.cpp @@ -0,0 +1,172 @@ +// Copyright 2020 Tier IV, Inc., Leo Drive Teknoloji A.Ş. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "scene_module/speed_bump/scene.hpp" + +#include "motion_utils/motion_utils.hpp" +#include "scene_module/speed_bump/util.hpp" +#include "tier4_autoware_utils/tier4_autoware_utils.hpp" + +#include + +namespace behavior_velocity_planner +{ +using motion_utils::calcSignedArcLength; +using tier4_autoware_utils::createPoint; + +using geometry_msgs::msg::Point32; + +SpeedBumpModule::SpeedBumpModule( + const int64_t module_id, const int64_t lane_id, + const lanelet::autoware::SpeedBump & speed_bump_reg_elem, const PlannerParam & planner_param, + const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock) +: SceneModuleInterface(module_id, logger, clock), + module_id_(module_id), + lane_id_(lane_id), + speed_bump_reg_elem_(std::move(speed_bump_reg_elem)), + planner_param_(planner_param) +{ + // Read speed bump height [m] from map + const auto speed_bump_height = + static_cast(speed_bump_reg_elem_.speedBump().attributeOr("height", 0.5)); + + // If slow_down_speed is specified on speed_bump annotation use it instead of calculating it + if (speed_bump_reg_elem_.speedBump().hasAttribute("slow_down_speed")) { + speed_bump_slow_down_speed_ = static_cast( + speed_bump_reg_elem_.speedBump().attribute("slow_down_speed").asDouble().get() / 3.6); + } else { + // point.x : height [m] -- point.y : speed [m/s] + Point32 p1; + Point32 p2; + + p1.x = planner_param_.speed_calculation_min_height; + p2.x = planner_param_.speed_calculation_max_height; + + p1.y = planner_param_.speed_calculation_max_speed; + p2.y = planner_param_.speed_calculation_min_speed; + + // Calculate the speed [m/s] for speed bump + speed_bump_slow_down_speed_ = calcSlowDownSpeed(p1, p2, speed_bump_height); + } + + if (planner_param_.print_debug_info) { + std::cout << "------------------------------" << std::endl; + std::cout << "Speed Bump ID: " << module_id_ << std::endl; + std::cout << "Speed Bump Height [cm]: " << speed_bump_height * 100 << std::endl; + std::cout << "Slow Down Speed [kph]: " << speed_bump_slow_down_speed_ * 3.6 << std::endl; + std::cout << "------------------------------" << std::endl; + } +} + +bool SpeedBumpModule::modifyPathVelocity( + PathWithLaneId * path, [[maybe_unused]] StopReason * stop_reason) +{ + if (path->points.empty()) { + return false; + } + + debug_data_ = DebugData(); + debug_data_.base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; + + const auto & ego_pos = planner_data_->current_pose.pose.position; + const auto & speed_bump = speed_bump_reg_elem_.speedBump(); + const auto & speed_bump_polygon = lanelet::utils::to2D(speed_bump).basicPolygon(); + + const auto & ego_path = *path; + const auto & path_polygon_intersection_status = + getPathPolygonIntersectionStatus(ego_path, speed_bump_polygon, ego_pos, 2); + + debug_data_.path_polygon_intersection_status = path_polygon_intersection_status; + + for (const auto & p : speed_bump_reg_elem_.speedBump().basicPolygon()) { + debug_data_.speed_bump_polygon.push_back(createPoint(p.x(), p.y(), ego_pos.z)); + } + + return applySlowDownSpeed(*path, speed_bump_slow_down_speed_, path_polygon_intersection_status); +} + +bool SpeedBumpModule::applySlowDownSpeed( + PathWithLaneId & output, const float speed_bump_speed, + const PathPolygonIntersectionStatus & path_polygon_intersection_status) +{ + if (isNoRelation(path_polygon_intersection_status)) { + return false; + } + + // decide slow_start_point index + size_t slow_start_point_idx{}; + // if first intersection point exists + if (path_polygon_intersection_status.first_intersection_point) { + // calculate & insert slow_start_point position wrt the first intersection point between + // path and the speed bump polygon + const auto & src_point = *path_polygon_intersection_status.first_intersection_point; + const auto & slow_start_margin_to_base_link = + -1 * + (planner_data_->vehicle_info_.max_longitudinal_offset_m + planner_param_.slow_start_margin); + auto slow_start_point_idx_candidate = + insertPointWithOffset(src_point, slow_start_margin_to_base_link, output.points, 5e-2); + if (slow_start_point_idx_candidate) { + slow_start_point_idx = *slow_start_point_idx_candidate; + debug_data_.slow_start_poses.push_back(output.points.at(slow_start_point_idx).point.pose); + } else if (calcSignedArcLength(output.points, src_point, 0) > slow_start_margin_to_base_link) { + // There is first intersection point but slow_start_point can not be inserted because it is + // behind the first path point (assign virtual slow_start_point_idx) + slow_start_point_idx = 0; + } else { + return false; + } + } else if ( + path_polygon_intersection_status.second_intersection_point || + path_polygon_intersection_status.is_path_inside_of_polygon) { + // assign virtual slow_start_point_idx + slow_start_point_idx = 0; + } + + // decide slow_end_point index + size_t slow_end_point_idx{}; + // if second intersection point exists + if (path_polygon_intersection_status.second_intersection_point) { + // calculate & insert slow_end_point position wrt the second intersection point between path + // and the speed bump polygon + const auto & src_point = *path_polygon_intersection_status.second_intersection_point; + const auto & slow_end_margin_to_base_link = + planner_data_->vehicle_info_.rear_overhang_m + planner_param_.slow_end_margin; + auto slow_end_point_idx_candidate = + insertPointWithOffset(src_point, slow_end_margin_to_base_link, output.points, 5e-2); + if (slow_end_point_idx_candidate) { + slow_end_point_idx = *slow_end_point_idx_candidate; + debug_data_.slow_end_points.push_back( + output.points.at(slow_end_point_idx).point.pose.position); + } else if ( + calcSignedArcLength(output.points, src_point, output.points.size() - 1) < + slow_end_margin_to_base_link) { + // There is second intersection point but slow_end_point can not be inserted because it is in + // front of the last path point (assign virtual slow_end_point_idx) + slow_end_point_idx = output.points.size() - 1; + } else { + return false; + } + } else if ( + path_polygon_intersection_status.first_intersection_point || + path_polygon_intersection_status.is_path_inside_of_polygon) { + // assign virtual slow_end_point_idx + slow_end_point_idx = output.points.size() - 1; + } + + // insert constant speed to path points that intersects with speed bump area + return insertConstSpeedToPathSection( + output.points, slow_start_point_idx, slow_end_point_idx, speed_bump_speed); +} + +} // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/src/scene_module/speed_bump/util.cpp b/planning/behavior_velocity_planner/src/scene_module/speed_bump/util.cpp new file mode 100644 index 0000000000000..7a8bd9edfd515 --- /dev/null +++ b/planning/behavior_velocity_planner/src/scene_module/speed_bump/util.cpp @@ -0,0 +1,171 @@ +// Copyright 2020 Tier IV, Inc., Leo Drive Teknoloji A.Ş. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "motion_utils/motion_utils.hpp" + +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace behavior_velocity_planner +{ + +namespace bg = boost::geometry; +using Point = bg::model::d2::point_xy; +using Polygon = bg::model::polygon; +using Line = bg::model::linestring; + +using motion_utils::calcLongitudinalOffsetPoint; +using motion_utils::calcSignedArcLength; +using motion_utils::findNearestSegmentIndex; +using motion_utils::insertTargetPoint; +using tier4_autoware_utils::createPoint; + +PathPolygonIntersectionStatus getPathPolygonIntersectionStatus( + const PathWithLaneId & ego_path, const lanelet::BasicPolygon2d & polygon, + const geometry_msgs::msg::Point & ego_pos, const size_t max_num) +{ + PathPolygonIntersectionStatus polygon_intersection_status; + std::vector intersects{}; + + bool found_max_num = false; + for (size_t i = 0; i < ego_path.points.size() - 1; ++i) { + const auto & p_back = ego_path.points.at(i).point.pose.position; + const auto & p_front = ego_path.points.at(i + 1).point.pose.position; + const Line segment{{p_back.x, p_back.y}, {p_front.x, p_front.y}}; + + std::vector tmp_intersects{}; + bg::intersection(segment, polygon, tmp_intersects); + + for (const auto & p : tmp_intersects) { + intersects.push_back(p); + if (intersects.size() == max_num) { + found_max_num = true; + break; + } + } + + if (found_max_num) { + break; + } + } + + const auto compare = [&](const Point & p1, const Point & p2) { + const auto dist_l1 = + calcSignedArcLength(ego_path.points, size_t(0), createPoint(p1.x(), p1.y(), ego_pos.z)); + + const auto dist_l2 = + calcSignedArcLength(ego_path.points, size_t(0), createPoint(p2.x(), p2.y(), ego_pos.z)); + + return dist_l1 < dist_l2; + }; + + std::sort(intersects.begin(), intersects.end(), compare); + + const auto & p_last = ego_path.points.back().point.pose.position; + const auto & p_first = ego_path.points.front().point.pose.position; + const Point & last_path_point{p_last.x, p_last.y}; + const Point & first_path_point{p_first.x, p_first.y}; + + auto const & is_first_path_point_inside_polygon = bg::within(first_path_point, polygon); + auto const & is_last_path_point_inside_polygon = bg::within(last_path_point, polygon); + + if ( + intersects.empty() && is_first_path_point_inside_polygon && is_last_path_point_inside_polygon) { + polygon_intersection_status.is_path_inside_of_polygon = true; + } else { + // classify first and second intersection points + for (size_t i = 0; i < intersects.size(); ++i) { + const auto & p = intersects.at(i); + if ( + (intersects.size() == 2 && i == 0) || + (intersects.size() == 1 && is_last_path_point_inside_polygon)) { + polygon_intersection_status.first_intersection_point = createPoint(p.x(), p.y(), ego_pos.z); + } else if ( + (intersects.size() == 2 && i == 1) || + (intersects.size() == 1 && is_first_path_point_inside_polygon)) { + polygon_intersection_status.second_intersection_point = + createPoint(p.x(), p.y(), ego_pos.z); + } + } + } + return polygon_intersection_status; +} + +bool isNoRelation(const PathPolygonIntersectionStatus & status) +{ + return !status.is_path_inside_of_polygon && !status.first_intersection_point && + !status.second_intersection_point; +} + +bool insertConstSpeedToPathSection( + std::vector & output, const size_t start_idx, const size_t end_idx, + const float speed) +{ + if (start_idx > end_idx) { + return false; + } + + const size_t start_idx_clamped = std::min(start_idx, output.size() - 1); + const size_t end_idx_clamped = std::min(end_idx, output.size() - 1); + + for (size_t i = start_idx_clamped; i <= end_idx_clamped; ++i) { + auto & p = output.at(i); + const auto & original_velocity = p.point.longitudinal_velocity_mps; + p.point.longitudinal_velocity_mps = std::min(original_velocity, speed); + } + return true; +} + +boost::optional insertPointWithOffset( + const geometry_msgs::msg::Point & src_point, const double longitudinal_offset, + std::vector & output, const double overlap_threshold) +{ + const auto & path = output; + const auto & target_point = calcLongitudinalOffsetPoint(path, src_point, longitudinal_offset); + + if (!target_point) { + return {}; + } + + const auto & target_segment_idx = findNearestSegmentIndex(path, *target_point); + + const auto & target_point_idx = + insertTargetPoint(target_segment_idx, *target_point, output, overlap_threshold); + + return target_point_idx; +} + +float calcSlowDownSpeed(const Point32 & p1, const Point32 & p2, const float speed_bump_height) +{ + const float m = (p1.y - p2.y) / (p1.x - p2.x); + const float b = p1.y - (m * p1.x); + + // y=mx+b + auto speed = m * speed_bump_height + b; + + return std::clamp(speed, p2.y, p1.y); +} + +} // namespace behavior_velocity_planner From 60896911f3c981d49368a92ac741533fcf33fab3 Mon Sep 17 00:00:00 2001 From: Sujith Vemishetty Date: Wed, 11 Jan 2023 22:08:50 +0530 Subject: [PATCH 09/49] fix(detection_area): add color debug markers (#2401) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: sujithvemi Co-authored-by: Ismet Atabay Co-authored-by: M. Fatih Cırıt --- .../src/scene_module/detection_area/debug.cpp | 26 ++++++++++++++++--- 1 file changed, 22 insertions(+), 4 deletions(-) diff --git a/planning/behavior_velocity_planner/src/scene_module/detection_area/debug.cpp b/planning/behavior_velocity_planner/src/scene_module/detection_area/debug.cpp index 9505828d1989a..5d62dfbc35d4e 100644 --- a/planning/behavior_velocity_planner/src/scene_module/detection_area/debug.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/detection_area/debug.cpp @@ -22,11 +22,13 @@ #else #include #endif +#include #include namespace behavior_velocity_planner { +using std_msgs::msg::ColorRGBA; using tier4_autoware_utils::appendMarkerArray; using tier4_autoware_utils::createDefaultMarker; using tier4_autoware_utils::createMarkerColor; @@ -55,7 +57,8 @@ geometry_msgs::msg::Point toMsg(const lanelet::BasicPoint3d & point) } visualization_msgs::msg::MarkerArray createCorrespondenceMarkerArray( - const lanelet::autoware::DetectionArea & detection_area_reg_elem, const rclcpp::Time & now) + const lanelet::autoware::DetectionArea & detection_area_reg_elem, const rclcpp::Time & now, + ColorRGBA & marker_color) { visualization_msgs::msg::MarkerArray msg; @@ -87,7 +90,7 @@ visualization_msgs::msg::MarkerArray createCorrespondenceMarkerArray( auto marker = createDefaultMarker( "map", now, "detection_area_polygon", detection_area_reg_elem.id(), visualization_msgs::msg::Marker::LINE_LIST, createMarkerScale(0.1, 0.0, 0.0), - createMarkerColor(0.1, 0.1, 1.0, 0.500)); + createMarkerColor(marker_color.r, marker_color.g, marker_color.b, marker_color.a)); marker.lifetime = rclcpp::Duration::from_seconds(0.5); for (const auto & detection_area : detection_area_reg_elem.detectionAreas()) { @@ -113,7 +116,7 @@ visualization_msgs::msg::MarkerArray createCorrespondenceMarkerArray( auto marker = createDefaultMarker( "map", now, "detection_area_correspondence", detection_area_reg_elem.id(), visualization_msgs::msg::Marker::LINE_LIST, createMarkerScale(0.1, 0.0, 0.0), - createMarkerColor(0.1, 0.1, 1.0, 0.500)); + createMarkerColor(marker_color.r, marker_color.g, marker_color.b, marker_color.a)); marker.lifetime = rclcpp::Duration::from_seconds(0.5); for (const auto & detection_area : detection_area_reg_elem.detectionAreas()) { @@ -138,13 +141,28 @@ visualization_msgs::msg::MarkerArray DetectionAreaModule::createDebugMarkerArray const rclcpp::Time now = clock_->now(); if (!debug_data_.stop_poses.empty()) { + ColorRGBA marker_color; + marker_color.r = 1.0; + marker_color.g = 0.0; + marker_color.b = 0.0; + marker_color.a = 1.0; + appendMarkerArray( - createCorrespondenceMarkerArray(detection_area_reg_elem_, now), &wall_marker, now); + createCorrespondenceMarkerArray(detection_area_reg_elem_, now, marker_color), &wall_marker); appendMarkerArray( debug::createPointsMarkerArray( debug_data_.obstacle_points, "obstacles", module_id_, now, 0.6, 0.6, 0.6, 1.0, 0.0, 0.0), &wall_marker, now); + } else { + ColorRGBA marker_color; + marker_color.r = 0.0; + marker_color.g = 1.0; + marker_color.b = 0.0; + marker_color.a = 1.0; + + appendMarkerArray( + createCorrespondenceMarkerArray(detection_area_reg_elem_, now, marker_color), &wall_marker); } return wall_marker; From d01b453e2ec36f30d230f2226b7ce787cfad13c2 Mon Sep 17 00:00:00 2001 From: kminoda <44218668+kminoda@users.noreply.github.com> Date: Thu, 12 Jan 2023 09:18:39 +0900 Subject: [PATCH 10/49] fix(pose_initializer): add default ad api description in readme (#2635) * fix(pose_initializer): add default ad api description in readme Signed-off-by: kminoda * ci(pre-commit): autofix * resolve conflict Signed-off-by: kminoda * remove svg file Signed-off-by: kminoda Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- localization/pose_initializer/docs/pose_initializer.md | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/localization/pose_initializer/docs/pose_initializer.md b/localization/pose_initializer/docs/pose_initializer.md index c1b2cc3d8b353..8a2163e56a649 100644 --- a/localization/pose_initializer/docs/pose_initializer.md +++ b/localization/pose_initializer/docs/pose_initializer.md @@ -44,3 +44,9 @@ Finally, it publishes the initial pose to `ekf_localizer`. | ------------------------------------ | ------------------------------------------------------------ | --------------------------- | | `/localization/initialization_state` | autoware_adapi_v1_msgs::msg::LocalizationInitializationState | pose initialization state | | `/initialpose3d` | geometry_msgs::msg::PoseWithCovarianceStamped | calculated initial ego pose | + +## Connection with Default AD API + +This `pose_initializer` is used via default AD API. For detailed description of the API description, please refer to [the description of `default_ad_api`](https://github.com/autowarefoundation/autoware.universe/blob/main/system/default_ad_api/document/localization.md). + +drawing From 54750628344fdbf56f4eb1dc18b602c9e7ad0783 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Thu, 12 Jan 2023 09:24:47 +0900 Subject: [PATCH 11/49] feat(avoidance): implement yield maneuver in avoidance module (#2515) * feat(avoidance): implement yield maneuver Signed-off-by: satoshi-ota * feat(avoidance_utils): add util functions Signed-off-by: satoshi-ota * feat(avoidance): add stop/slow behavior Signed-off-by: satoshi-ota * feat(avoidance): enhance avoidance debug markers Signed-off-by: satoshi-ota * fix(behavior_path_planner): cleanup avoidance params Signed-off-by: satoshi-ota * fix(avoidance_utils): improve util functions for readability Signed-off-by: satoshi-ota * fix(avoidance): remove no use optional Signed-off-by: satoshi-ota * fix(avoidance): fix param name for readability Signed-off-by: satoshi-ota * feat(avoidance): sort object by longitudinal distance Signed-off-by: satoshi-ota Signed-off-by: satoshi-ota --- .../config/avoidance/avoidance.param.yaml | 151 +++---- .../avoidance/avoidance_module.hpp | 68 +++- .../avoidance/avoidance_module_data.hpp | 63 +++ .../avoidance/avoidance_utils.hpp | 8 + .../scene_module/avoidance/debug.hpp | 5 + .../src/behavior_path_planner_node.cpp | 210 ++++++---- .../avoidance/avoidance_module.cpp | 369 ++++++++++++++++-- .../avoidance/avoidance_utils.cpp | 228 +++++++++++ .../src/scene_module/avoidance/debug.cpp | 92 +++++ 9 files changed, 1007 insertions(+), 187 deletions(-) diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml index 9d19382184adc..40de19bd6cd7a 100644 --- a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml +++ b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml @@ -2,76 +2,26 @@ /**: ros__parameters: avoidance: - resample_interval_for_planning: 0.3 - resample_interval_for_output: 4.0 - detection_area_right_expand_dist: 0.0 - detection_area_left_expand_dist: 1.0 - + resample_interval_for_planning: 0.3 # [m] + resample_interval_for_output: 4.0 # [m] + detection_area_right_expand_dist: 0.0 # [m] + detection_area_left_expand_dist: 1.0 # [m] + drivable_area_right_bound_offset: 0.0 # [m] + drivable_area_left_bound_offset: 0.0 # [m] + object_envelope_buffer: 0.3 # [m] + + # avoidance module common setting + enable_bound_clipping: false enable_avoidance_over_same_direction: true enable_avoidance_over_opposite_direction: true enable_update_path_when_object_is_gone: false enable_safety_check: false - - # For target object filtering - threshold_speed_object_is_stopped: 1.0 # [m/s] - threshold_time_object_is_moving: 1.0 # [s] - - object_check_forward_distance: 150.0 # [m] - object_check_backward_distance: 2.0 # [m] - object_check_goal_distance: 20.0 # [m] - - threshold_distance_object_is_on_center: 1.0 # [m] - - object_check_shiftable_ratio: 0.6 # [-] - object_check_min_road_shoulder_width: 0.5 # [m] - - # For safety check - safety_check_backward_distance: 50.0 # [m] - safety_check_time_horizon: 10.0 # [s] - safety_check_idling_time: 1.5 # [s] - safety_check_accel_for_rss: 2.5 # [m/ss] - - # For lateral margin - object_envelope_buffer: 0.3 # [m] - lateral_collision_margin: 1.0 # [m] - lateral_collision_safety_buffer: 0.7 # [m] - - # For longitudinal margin - longitudinal_collision_margin_buffer: 0.0 # [m] - - prepare_time: 2.0 # [s] - min_prepare_distance: 1.0 # [m] - min_avoidance_distance: 10.0 # [m] - - min_nominal_avoidance_speed: 7.0 # [m/s] - min_sharp_avoidance_speed: 1.0 # [m/s] - - road_shoulder_safety_margin: 0.0 # [m] - - max_right_shift_length: 5.0 - max_left_shift_length: 5.0 - - nominal_lateral_jerk: 0.2 # [m/s3] - max_lateral_jerk: 1.0 # [m/s3] - - # For detection loss compensation - object_last_seen_threshold: 2.0 # [s] - - # For prevention of large acceleration while avoidance - min_avoidance_speed_for_acc_prevention: 3.0 # [m/s] - max_avoidance_acceleration: 0.5 # [m/s2] - - # bound clipping for objects - enable_bound_clipping: false + enable_yield_maneuver: false # for debug publish_debug_marker: false print_debug_info: false - # not enabled yet - longitudinal_collision_margin_min_distance: 0.0 # [m] - longitudinal_collision_margin_time: 0.0 - # avoidance is performed for the object type with true target_object: car: true @@ -83,11 +33,80 @@ motorcycle: false pedestrian: false - # ---------- advanced parameters ---------- - avoidance_execution_lateral_threshold: 0.499 + # For target object filtering + target_filtering: + # filtering moving objects + threshold_speed_object_is_stopped: 1.0 # [m/s] + threshold_time_object_is_moving: 1.0 # [s] + # detection range + object_check_forward_distance: 150.0 # [m] + object_check_backward_distance: 2.0 # [m] + object_check_goal_distance: 20.0 # [m] + # filtering parking objects + threshold_distance_object_is_on_center: 1.0 # [m] + object_check_shiftable_ratio: 0.6 # [-] + object_check_min_road_shoulder_width: 0.5 # [m] + # lost object compensation + object_last_seen_threshold: 2.0 + + # For safety check + safety_check: + safety_check_backward_distance: 50.0 # [m] + safety_check_time_horizon: 10.0 # [s] + safety_check_idling_time: 1.5 # [s] + safety_check_accel_for_rss: 2.5 # [m/ss] + safety_check_hysteresis_factor: 2.0 # [-] + + # For avoidance maneuver + avoidance: + # avoidance lateral parameters + lateral: + lateral_collision_margin: 1.0 # [m] + lateral_collision_safety_buffer: 0.7 # [m] + lateral_passable_safety_buffer: 0.0 # [m] + road_shoulder_safety_margin: 0.0 # [m] + avoidance_execution_lateral_threshold: 0.499 + max_right_shift_length: 5.0 + max_left_shift_length: 5.0 + # avoidance distance parameters + longitudinal: + prepare_time: 2.0 # [s] + longitudinal_collision_safety_buffer: 0.0 # [m] + min_prepare_distance: 1.0 # [m] + min_avoidance_distance: 10.0 # [m] + min_nominal_avoidance_speed: 7.0 # [m/s] + min_sharp_avoidance_speed: 1.0 # [m/s] + + # For yield maneuver + yield: + yield_velocity: 2.78 # [m/s] + + # For stop maneuver + stop: + min_distance: 10.0 # [m] + max_distance: 20.0 # [m] + + constraints: + # vehicle slows down under longitudinal constraints + use_constraints_for_decel: false # [-] + + # lateral constraints + lateral: + nominal_lateral_jerk: 0.2 # [m/s3] + max_lateral_jerk: 1.0 # [m/s3] + + # longitudinal constraints + longitudinal: + nominal_deceleration: -1.0 # [m/ss] + nominal_jerk: 0.5 # [m/sss] + max_deceleration: -2.0 # [m/ss] + max_jerk: 1.0 + # For prevention of large acceleration while avoidance + min_avoidance_speed_for_acc_prevention: 3.0 # [m/s] + max_avoidance_acceleration: 0.5 # [m/ss] target_velocity_matrix: - col_size: 4 + col_size: 2 matrix: - [2.78, 5.56, 11.1, 13.9, # velocity [m/s] - 0.20, 0.90, 1.10, 1.20] # margin [m] + [2.78, 13.9, # velocity [m/s] + 0.50, 1.00] # margin [m] diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp index 8e812ae915d63..a3727740ee2e3 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp @@ -202,12 +202,18 @@ class AvoidanceModule : public SceneModuleInterface * object pre-process */ void fillAvoidanceTargetObjects(AvoidancePlanningData & data, DebugData & debug) const; + void fillObjectEnvelopePolygon(const Pose & closest_pose, ObjectData & object_data) const; + void fillObjectMovingTime(ObjectData & object_data) const; + void compensateDetectionLost( ObjectDataArray & target_objects, ObjectDataArray & other_objects) const; + void fillShiftLine(AvoidancePlanningData & data, DebugData & debug) const; + void fillDebugData(const AvoidancePlanningData & data, DebugData & debug) const; + // data used in previous planning ShiftedPath prev_output_; ShiftedPath prev_linear_shift_path_; // used for shift point check @@ -231,17 +237,17 @@ class AvoidanceModule : public SceneModuleInterface ObjectDataArray registered_objects_; void updateRegisteredObject(const ObjectDataArray & objects); - // -- for shift point generation -- + // ========= shift line generator ====== + + AvoidLineArray calcRawShiftLinesFromObjects( + const AvoidancePlanningData & data, DebugData & debug) const; + AvoidLineArray applyPreProcessToRawShiftLines( AvoidLineArray & current_raw_shift_points, DebugData & debug) const; - // shift point generation: generator double getShiftLength( const ObjectData & object, const bool & is_object_on_right, const double & avoid_margin) const; - AvoidLineArray calcRawShiftLinesFromObjects( - const AvoidancePlanningData & data, DebugData & debug) const; - // shift point generation: combiner AvoidLineArray combineRawShiftLinesWithUniqueCheck( const AvoidLineArray & base_lines, const AvoidLineArray & added_lines) const; @@ -289,9 +295,6 @@ class AvoidanceModule : public SceneModuleInterface std::shared_ptr ego_velocity_starting_avoidance_ptr_; void modifyPathVelocityToPreventAccelerationOnAvoidance(ShiftedPath & shifted_path); - // clean up shifter - void postProcess(PathShifter & shifter) const; - // turn signal TurnSignalInfo calcTurnSignalInfo(const ShiftedPath & path) const; @@ -316,6 +319,35 @@ class AvoidanceModule : public SceneModuleInterface ObjectDataArray getAdjacentLaneObjects(const lanelet::ConstLanelets & adjacent_lanes) const; + // ========= plan ====================== + + AvoidanceState updateEgoState(const AvoidancePlanningData & data) const; + + void updateEgoBehavior(const AvoidancePlanningData & data, ShiftedPath & path); + + void insertWaitPoint(const bool use_constraints_for_decel, ShiftedPath & shifted_path) const; + + void insertPrepareVelocity(const bool avoidable, ShiftedPath & shifted_path) const; + + void insertYieldVelocity(ShiftedPath & shifted_path) const; + + void removeAllRegisteredShiftPoints(PathShifter & path_shifter) + { + current_raw_shift_lines_.clear(); + registered_raw_shift_lines_.clear(); + path_shifter.setShiftLines(ShiftLineArray{}); + } + + void postProcess(PathShifter & path_shifter) const + { + const size_t nearest_idx = findEgoIndex(path_shifter.getReferencePath().points); + path_shifter.removeBehindShiftLineAndSetBaseOffset(nearest_idx); + } + + double getFeasibleDecelDistance(const double target_velocity) const; + + double getMildDecelDistance(const double target_velocity) const; + // ========= safety check ============== lanelet::ConstLanelets getAdjacentLane( @@ -350,6 +382,13 @@ class AvoidanceModule : public SceneModuleInterface return std::max(getEgoSpeed(), parameters_->min_sharp_avoidance_speed); } + float getMinimumAvoidanceEgoSpeed() const { return parameters_->target_velocity_matrix.front(); } + + float getMaximumAvoidanceEgoSpeed() const + { + return parameters_->target_velocity_matrix.at(parameters_->col_size - 1); + } + double getNominalPrepareDistance() const { const auto & p = parameters_; @@ -363,7 +402,16 @@ class AvoidanceModule : public SceneModuleInterface { const auto & p = parameters_; const auto distance_by_jerk = PathShifter::calcLongitudinalDistFromJerk( - shift_length, parameters_->nominal_lateral_jerk, getNominalAvoidanceEgoSpeed()); + shift_length, p->nominal_lateral_jerk, getNominalAvoidanceEgoSpeed()); + + return std::max(p->min_avoidance_distance, distance_by_jerk); + } + + double getMinimumAvoidanceDistance(const double shift_length) const + { + const auto & p = parameters_; + const auto distance_by_jerk = path_shifter_.calcLongitudinalDistFromJerk( + shift_length, p->nominal_lateral_jerk, getMinimumAvoidanceEgoSpeed()); return std::max(p->min_avoidance_distance, distance_by_jerk); } @@ -372,7 +420,7 @@ class AvoidanceModule : public SceneModuleInterface { const auto & p = parameters_; const auto distance_by_jerk = PathShifter::calcLongitudinalDistFromJerk( - shift_length, parameters_->max_lateral_jerk, getSharpAvoidanceEgoSpeed()); + shift_length, p->max_lateral_jerk, getSharpAvoidanceEgoSpeed()); return std::max(p->min_avoidance_distance, distance_by_jerk); } diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp index fb3560a6bfe90..dfb776ca8ecd0 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp @@ -73,6 +73,24 @@ struct AvoidanceParameters // enable safety check. if avoidance path is NOT safe, the ego will execute yield maneuver bool enable_safety_check{false}; + // enable yield maneuver. + bool enable_yield_maneuver{false}; + + // constrains + bool use_constraints_for_decel{false}; + + // max deceleration for + double max_deceleration; + + // max jerk + double max_jerk; + + // comfortable deceleration + double nominal_deceleration; + + // comfortable jerk + double nominal_jerk; + // Vehicles whose distance to the center of the path is // less than this will not be considered for avoidance. double threshold_distance_object_is_on_center; @@ -108,6 +126,9 @@ struct AvoidanceParameters // don't ever set this value to 0 double lateral_collision_safety_buffer{0.5}; + // if object overhang is less than this value, the ego stops behind the object. + double lateral_passable_safety_buffer{0.5}; + // margin between object back and end point of avoid shift point double longitudinal_collision_safety_buffer{0.0}; @@ -134,6 +155,18 @@ struct AvoidanceParameters // use in RSS calculation double safety_check_accel_for_rss; + // transit hysteresis (unsafe to safe) + double safety_check_hysteresis_factor; + + // keep target velocity in yield maneuver + double yield_velocity; + + // minimum stop distance + double stop_min_distance; + + // maximum stop distance + double stop_max_distance; + // start avoidance after this time to avoid sudden path change double prepare_time; @@ -268,6 +301,9 @@ struct ObjectData // avoidance target // lateral distance from overhang to the road shoulder double to_road_shoulder_distance{0.0}; + // if lateral margin is NOT enough, the ego must avoid the object. + bool avoid_required{false}; + // unavoidable reason std::string reason{""}; }; @@ -301,11 +337,25 @@ struct AvoidLine : public ShiftLine }; using AvoidLineArray = std::vector; +/* + * avoidance state + */ +enum class AvoidanceState { + NOT_AVOID = 0, + AVOID_EXECUTE, + YIELD, + AVOID_PATH_READY, + AVOID_PATH_NOT_READY, +}; + /* * Common data for avoidance planning */ struct AvoidancePlanningData { + // ego final state + AvoidanceState state{AvoidanceState::NOT_AVOID}; + // un-shifted pose (for current lane detection) Pose reference_pose; @@ -337,9 +387,18 @@ struct AvoidancePlanningData // new shift point AvoidLineArray unapproved_new_sl{}; + // safe shift point + AvoidLineArray safe_new_sl{}; + bool safe{false}; bool avoiding_now{false}; + + bool avoid_required{false}; + + bool yield_required{false}; + + bool found_avoidance_path{false}; }; /* @@ -415,6 +474,10 @@ struct DebugData std::vector total_shift; std::vector output_shift; + boost::optional stop_pose{boost::none}; + boost::optional slow_pose{boost::none}; + boost::optional feasible_bound{boost::none}; + bool exist_adjacent_objects{false}; // future pose diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_utils.hpp index 5e2ecf2e61211..8a4cdf54f0238 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_utils.hpp @@ -110,6 +110,14 @@ bool isCentroidWithinLanelets( lanelet::ConstLanelets getTargetLanelets( const std::shared_ptr & planner_data, lanelet::ConstLanelets & route_lanelets, const double left_offset, const double right_offset); + +double calcDecelDistWithJerkAndAccConstraints( + const double current_vel, const double target_vel, const double current_acc, const double acc_min, + const double jerk_acc, const double jerk_dec); + +void insertDecelPoint( + const Point & p_src, const double offset, const double velocity, PathWithLaneId & path, + boost::optional & p_out); } // namespace behavior_path_planner #endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__AVOIDANCE__AVOIDANCE_UTILS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/debug.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/debug.hpp index 5d1ff1254eb24..e67bc01b98b1f 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/debug.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/debug.hpp @@ -37,7 +37,9 @@ namespace marker_utils::avoidance_marker using autoware_auto_perception_msgs::msg::PredictedObjects; using autoware_auto_planning_msgs::msg::PathWithLaneId; using behavior_path_planner::AvoidancePlanningData; +using behavior_path_planner::AvoidanceState; using behavior_path_planner::AvoidLineArray; +using behavior_path_planner::DebugData; using behavior_path_planner::ObjectDataArray; using behavior_path_planner::ShiftLineArray; using geometry_msgs::msg::Point; @@ -48,6 +50,9 @@ using visualization_msgs::msg::MarkerArray; MarkerArray createEgoStatusMarkerArray( const AvoidancePlanningData & data, const Pose & p_ego, std::string && ns); +MarkerArray createSafetyCheckMarkerArray( + const AvoidanceState & state, const Pose & pose, const DebugData & data); + MarkerArray createAvoidLineMarkerArray( const AvoidLineArray & shift_points, std::string && ns, const float & r, const float & g, const float & b, const double & w); diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 285b310c4c9ce..54b8c66250132 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -280,89 +280,151 @@ SideShiftParameters BehaviorPathPlannerNode::getSideShiftParam() AvoidanceParameters BehaviorPathPlannerNode::getAvoidanceParam() { - const auto dp = [this](const std::string & str, auto def_val) { - std::string name = "avoidance." + str; - return this->declare_parameter(name, def_val); - }; - AvoidanceParameters p{}; - p.resample_interval_for_planning = dp("resample_interval_for_planning", 0.3); - p.resample_interval_for_output = dp("resample_interval_for_output", 3.0); - p.detection_area_right_expand_dist = dp("detection_area_right_expand_dist", 0.0); - p.detection_area_left_expand_dist = dp("detection_area_left_expand_dist", 1.0); - p.enable_avoidance_over_same_direction = dp("enable_avoidance_over_same_direction", true); - p.enable_avoidance_over_opposite_direction = dp("enable_avoidance_over_opposite_direction", true); - p.enable_update_path_when_object_is_gone = dp("enable_update_path_when_object_is_gone", false); - p.enable_safety_check = dp("enable_safety_check", false); - - p.threshold_distance_object_is_on_center = dp("threshold_distance_object_is_on_center", 1.0); - p.threshold_speed_object_is_stopped = dp("threshold_speed_object_is_stopped", 1.0); - p.threshold_time_object_is_moving = dp("threshold_time_object_is_moving", 1.0); - p.object_check_forward_distance = dp("object_check_forward_distance", 150.0); - p.object_check_backward_distance = dp("object_check_backward_distance", 2.0); - p.object_check_goal_distance = dp("object_check_goal_distance", 20.0); - p.object_check_shiftable_ratio = dp("object_check_shiftable_ratio", 1.0); - p.object_check_min_road_shoulder_width = dp("object_check_min_road_shoulder_width", 0.5); - p.object_envelope_buffer = dp("object_envelope_buffer", 0.1); - p.lateral_collision_margin = dp("lateral_collision_margin", 2.0); - p.lateral_collision_safety_buffer = dp("lateral_collision_safety_buffer", 0.5); - p.longitudinal_collision_safety_buffer = dp("longitudinal_collision_safety_buffer", 0.0); - - p.safety_check_min_longitudinal_margin = dp("safety_check_min_longitudinal_margin", 0.0); - p.safety_check_backward_distance = dp("safety_check_backward_distance", 0.0); - p.safety_check_time_horizon = dp("safety_check_time_horizon", 10.0); - p.safety_check_idling_time = dp("safety_check_idling_time", 1.5); - p.safety_check_accel_for_rss = dp("safety_check_accel_for_rss", 2.5); - - p.prepare_time = dp("prepare_time", 3.0); - p.min_prepare_distance = dp("min_prepare_distance", 10.0); - p.min_avoidance_distance = dp("min_avoidance_distance", 10.0); - - p.min_nominal_avoidance_speed = dp("min_nominal_avoidance_speed", 5.0); - p.min_sharp_avoidance_speed = dp("min_sharp_avoidance_speed", 1.0); - - p.road_shoulder_safety_margin = dp("road_shoulder_safety_margin", 0.0); - - p.max_right_shift_length = dp("max_right_shift_length", 1.5); - p.max_left_shift_length = dp("max_left_shift_length", 1.5); - - p.nominal_lateral_jerk = dp("nominal_lateral_jerk", 0.3); - p.max_lateral_jerk = dp("max_lateral_jerk", 2.0); - - p.longitudinal_collision_margin_min_distance = - dp("longitudinal_collision_margin_min_distance", 0.0); - p.longitudinal_collision_margin_time = dp("longitudinal_collision_margin_time", 0.0); - - p.object_last_seen_threshold = dp("object_last_seen_threshold", 2.0); - - p.min_avoidance_speed_for_acc_prevention = dp("min_avoidance_speed_for_acc_prevention", 3.0); - p.max_avoidance_acceleration = dp("max_avoidance_acceleration", 0.5); + // general params + { + std::string ns = "avoidance."; + p.resample_interval_for_planning = + declare_parameter(ns + "resample_interval_for_planning"); + p.resample_interval_for_output = declare_parameter(ns + "resample_interval_for_output"); + p.detection_area_right_expand_dist = + declare_parameter(ns + "detection_area_right_expand_dist"); + p.detection_area_left_expand_dist = + declare_parameter(ns + "detection_area_left_expand_dist"); + p.drivable_area_right_bound_offset = + declare_parameter(ns + "drivable_area_right_bound_offset"); + p.drivable_area_left_bound_offset = + declare_parameter(ns + "drivable_area_left_bound_offset"); + p.object_envelope_buffer = declare_parameter(ns + "object_envelope_buffer"); + p.enable_bound_clipping = declare_parameter(ns + "enable_bound_clipping"); + p.enable_avoidance_over_same_direction = + declare_parameter(ns + "enable_avoidance_over_same_direction"); + p.enable_avoidance_over_opposite_direction = + declare_parameter(ns + "enable_avoidance_over_opposite_direction"); + p.enable_update_path_when_object_is_gone = + declare_parameter(ns + "enable_update_path_when_object_is_gone"); + p.enable_safety_check = declare_parameter(ns + "enable_safety_check"); + p.enable_yield_maneuver = declare_parameter(ns + "enable_yield_maneuver"); + p.publish_debug_marker = declare_parameter(ns + "publish_debug_marker"); + p.print_debug_info = declare_parameter(ns + "print_debug_info"); + } - p.publish_debug_marker = dp("publish_debug_marker", false); - p.print_debug_info = dp("print_debug_info", false); + // target object + { + std::string ns = "avoidance.target_object."; + p.avoid_car = declare_parameter(ns + "car"); + p.avoid_truck = declare_parameter(ns + "truck"); + p.avoid_bus = declare_parameter(ns + "bus"); + p.avoid_trailer = declare_parameter(ns + "trailer"); + p.avoid_unknown = declare_parameter(ns + "unknown"); + p.avoid_bicycle = declare_parameter(ns + "bicycle"); + p.avoid_motorcycle = declare_parameter(ns + "motorcycle"); + p.avoid_pedestrian = declare_parameter(ns + "pedestrian"); + } - // velocity matrix + // target filtering { - std::string ns = "avoidance.target_velocity_matrix."; - p.col_size = declare_parameter(ns + "col_size"); - p.target_velocity_matrix = declare_parameter>(ns + "matrix"); + std::string ns = "avoidance.target_filtering."; + p.threshold_speed_object_is_stopped = + declare_parameter(ns + "threshold_speed_object_is_stopped"); + p.threshold_time_object_is_moving = + declare_parameter(ns + "threshold_time_object_is_moving"); + p.object_check_forward_distance = + declare_parameter(ns + "object_check_forward_distance"); + p.object_check_backward_distance = + declare_parameter(ns + "object_check_backward_distance"); + p.object_check_goal_distance = declare_parameter(ns + "object_check_goal_distance"); + p.threshold_distance_object_is_on_center = + declare_parameter(ns + "threshold_distance_object_is_on_center"); + p.object_check_shiftable_ratio = declare_parameter(ns + "object_check_shiftable_ratio"); + p.object_check_min_road_shoulder_width = + declare_parameter(ns + "object_check_min_road_shoulder_width"); + p.object_last_seen_threshold = declare_parameter(ns + "object_last_seen_threshold"); } - p.avoid_car = dp("target_object.car", true); - p.avoid_truck = dp("target_object.truck", true); - p.avoid_bus = dp("target_object.bus", true); - p.avoid_trailer = dp("target_object.trailer", true); - p.avoid_unknown = dp("target_object.unknown", false); - p.avoid_bicycle = dp("target_object.bicycle", false); - p.avoid_motorcycle = dp("target_object.motorcycle", false); - p.avoid_pedestrian = dp("target_object.pedestrian", false); + // safety check + { + std::string ns = "avoidance.safety_check."; + p.safety_check_backward_distance = + declare_parameter(ns + "safety_check_backward_distance"); + p.safety_check_time_horizon = declare_parameter(ns + "safety_check_time_horizon"); + p.safety_check_idling_time = declare_parameter(ns + "safety_check_idling_time"); + p.safety_check_accel_for_rss = declare_parameter(ns + "safety_check_accel_for_rss"); + p.safety_check_hysteresis_factor = + declare_parameter(ns + "safety_check_hysteresis_factor"); + } - p.drivable_area_right_bound_offset = dp("drivable_area_right_bound_offset", 0.0); - p.drivable_area_left_bound_offset = dp("drivable_area_left_bound_offset", 0.0); + // avoidance maneuver (lateral) + { + std::string ns = "avoidance.avoidance.lateral."; + p.lateral_collision_margin = declare_parameter(ns + "lateral_collision_margin"); + p.lateral_collision_safety_buffer = + declare_parameter(ns + "lateral_collision_safety_buffer"); + p.lateral_passable_safety_buffer = + declare_parameter(ns + "lateral_passable_safety_buffer"); + p.road_shoulder_safety_margin = declare_parameter(ns + "road_shoulder_safety_margin"); + p.avoidance_execution_lateral_threshold = + declare_parameter(ns + "avoidance_execution_lateral_threshold"); + p.max_right_shift_length = declare_parameter(ns + "max_right_shift_length"); + p.max_left_shift_length = declare_parameter(ns + "max_left_shift_length"); + } - p.enable_bound_clipping = dp("enable_bound_clipping", false); + // avoidance maneuver (longitudinal) + { + std::string ns = "avoidance.avoidance.longitudinal."; + p.prepare_time = declare_parameter(ns + "prepare_time"); + p.longitudinal_collision_safety_buffer = + declare_parameter(ns + "longitudinal_collision_safety_buffer"); + p.min_prepare_distance = declare_parameter(ns + "min_prepare_distance"); + p.min_avoidance_distance = declare_parameter(ns + "min_avoidance_distance"); + p.min_nominal_avoidance_speed = declare_parameter(ns + "min_nominal_avoidance_speed"); + p.min_sharp_avoidance_speed = declare_parameter(ns + "min_sharp_avoidance_speed"); + } - p.avoidance_execution_lateral_threshold = dp("avoidance_execution_lateral_threshold", 0.499); + // yield + { + std::string ns = "avoidance.yield."; + p.yield_velocity = declare_parameter(ns + "yield_velocity"); + } + + // stop + { + std::string ns = "avoidance.stop."; + p.stop_min_distance = declare_parameter(ns + "min_distance"); + p.stop_max_distance = declare_parameter(ns + "max_distance"); + } + + // constraints + { + std::string ns = "avoidance.constraints."; + p.use_constraints_for_decel = declare_parameter(ns + "use_constraints_for_decel"); + } + + // constraints (longitudinal) + { + std::string ns = "avoidance.constraints.longitudinal."; + p.nominal_deceleration = declare_parameter(ns + "nominal_deceleration"); + p.nominal_jerk = declare_parameter(ns + "nominal_jerk"); + p.max_deceleration = declare_parameter(ns + "max_deceleration"); + p.max_jerk = declare_parameter(ns + "max_jerk"); + p.min_avoidance_speed_for_acc_prevention = + declare_parameter(ns + "min_avoidance_speed_for_acc_prevention"); + p.max_avoidance_acceleration = declare_parameter(ns + "max_avoidance_acceleration"); + } + + // constraints (lateral) + { + std::string ns = "avoidance.constraints.lateral."; + p.nominal_lateral_jerk = declare_parameter(ns + "nominal_lateral_jerk"); + p.max_lateral_jerk = declare_parameter(ns + "max_lateral_jerk"); + } + + // velocity matrix + { + std::string ns = "avoidance.target_velocity_matrix."; + p.col_size = declare_parameter(ns + "col_size"); + p.target_velocity_matrix = declare_parameter>(ns + "matrix"); + } return p; } diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index 89548855f233a..1ef1a124c2919 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -43,6 +43,7 @@ namespace behavior_path_planner { +using motion_utils::calcLongitudinalOffsetPose; using motion_utils::calcSignedArcLength; using motion_utils::findNearestIndex; using motion_utils::findNearestSegmentIndex; @@ -201,6 +202,9 @@ AvoidancePlanningData AvoidanceModule::calcAvoidancePlanningData(DebugData & deb planner_data_->route_handler, reference_pose, planner_data_->parameters.forward_path_length, planner_data_->parameters.backward_path_length); + // keep avoidance state + data.state = avoidance_data_.state; + // target objects for avoidance fillAvoidanceTargetObjects(data, debug); @@ -291,6 +295,21 @@ void AvoidanceModule::fillAvoidanceTargetObjects( // Calc moving time. fillObjectMovingTime(object_data); + // Calc lateral deviation from path to target object. + object_data.lateral = calcLateralDeviation(object_closest_pose, object_pose.position); + avoidance_debug_msg.lateral_distance_from_centerline = object_data.lateral; + + // Find the footprint point closest to the path, set to object_data.overhang_distance. + object_data.overhang_dist = calcEnvelopeOverhangDistance( + object_data, object_closest_pose, object_data.overhang_pose.position); + + // Check whether the the ego should avoid the object. + const auto & vehicle_width = planner_data_->parameters.vehicle_width; + const auto safety_margin = 0.5 * vehicle_width + parameters_->lateral_passable_safety_buffer; + object_data.avoid_required = + (isOnRight(object_data) && std::abs(object_data.overhang_dist) < safety_margin) || + (!isOnRight(object_data) && object_data.overhang_dist < safety_margin); + if (object_data.move_time > parameters_->threshold_time_object_is_moving) { avoidance_debug_array_false_and_push_back(AvoidanceDebugFactor::MOVING_OBJECT); object_data.reason = AvoidanceDebugFactor::MOVING_OBJECT; @@ -329,14 +348,6 @@ void AvoidanceModule::fillAvoidanceTargetObjects( continue; } - // Calc lateral deviation from path to target object. - object_data.lateral = calcLateralDeviation(object_closest_pose, object_pose.position); - avoidance_debug_msg.lateral_distance_from_centerline = object_data.lateral; - - // Find the footprint point closest to the path, set to object_data.overhang_distance. - object_data.overhang_dist = calcEnvelopeOverhangDistance( - object_data, object_closest_pose, object_data.overhang_pose.position); - lanelet::ConstLanelet overhang_lanelet; if (!rh->getClosestLaneletWithinRoute(object_closest_pose, &overhang_lanelet)) { continue; @@ -574,6 +585,9 @@ void AvoidanceModule::fillShiftLine(AvoidancePlanningData & data, DebugData & de * Find new shift point */ data.unapproved_new_sl = findNewShiftLine(processed_raw_sp, path_shifter); + const auto found_new_sl = data.unapproved_new_sl.size() > 0; + const auto registered = path_shifter.getShiftLines().size() > 0; + data.found_avoidance_path = found_new_sl || registered; /** * STEP 4 @@ -595,12 +609,137 @@ void AvoidanceModule::fillShiftLine(AvoidancePlanningData & data, DebugData & de * check that there is a certain amount of margin in the lateral and longitudinal direction. */ data.safe = isSafePath(path_shifter, candidate_path, debug); - data.candidate_path = candidate_path; - { - debug.output_shift = data.candidate_path.shift_length; - debug.current_raw_shift = data.unapproved_raw_sl; - debug.new_shift_lines = data.unapproved_new_sl; + if (data.safe) { + data.safe_new_sl = data.unapproved_new_sl; + data.candidate_path = candidate_path; + } + + /** + * Check whether the ego should avoid the front object. When the ego follows reference path, + * if the lateral distance is smaller than minimum margin, the ego should avoid the object. + */ + if (!data.target_objects.empty()) { + const auto o_front = data.target_objects.front(); + data.avoid_required = o_front.avoid_required; + } + + /** + * If the avoidance path is not safe in situation where the ego should avoid object, the ego + * stops in front of the front object with the necessary distance to avoid the object. + */ + if (!data.safe && data.avoid_required) { + data.yield_required = data.found_avoidance_path && data.avoid_required; + data.candidate_path = toShiftedPath(data.reference_path); + RCLCPP_WARN_THROTTLE( + getLogger(), *clock_, 5000, "not found safe avoidance path. transit yield maneuver..."); + } + + /** + * TODO(Satoshi OTA) Think yield maneuver in the middle of avoidance. + * Even if it is determined that a yield is necessary, the yield maneuver is not executed + * if the avoidance has already been initiated. + */ + if (!data.safe && data.avoiding_now) { + data.yield_required = false; + data.safe = true; // OVERWRITE SAFETY JUDGE + data.safe_new_sl = data.unapproved_new_sl; + RCLCPP_WARN_THROTTLE( + getLogger(), *clock_, 5000, "avoiding now. could not transit yield maneuver!!!"); + } + + fillDebugData(data, debug); +} + +void AvoidanceModule::fillDebugData(const AvoidancePlanningData & data, DebugData & debug) const +{ + debug.output_shift = data.candidate_path.shift_length; + debug.current_raw_shift = data.unapproved_raw_sl; + debug.new_shift_lines = data.unapproved_new_sl; + + if (data.target_objects.empty()) { + return; + } + + if (data.avoiding_now) { + return; + } + + if (data.unapproved_new_sl.empty()) { + return; + } + + const auto o_front = data.target_objects.front(); + const auto & base_link2front = planner_data_->parameters.base_link2front; + const auto & vehicle_width = planner_data_->parameters.vehicle_width; + + const auto avoid_margin = parameters_->lateral_collision_safety_buffer + + parameters_->lateral_collision_margin + 0.5 * vehicle_width; + + const auto variable = + getSharpAvoidanceDistance(getShiftLength(o_front, isOnRight(o_front), avoid_margin)); + const auto constant = getNominalPrepareDistance() + + parameters_->longitudinal_collision_safety_buffer + base_link2front; + const auto total_avoid_distance = variable + constant; + + const auto opt_feasible_bound = calcLongitudinalOffsetPose( + data.reference_path.points, getEgoPosition(), o_front.longitudinal - total_avoid_distance); + + if (opt_feasible_bound) { + debug.feasible_bound = opt_feasible_bound.get(); + } else { + debug.feasible_bound = getPose(data.reference_path.points.front()); + } +} + +AvoidanceState AvoidanceModule::updateEgoState(const AvoidancePlanningData & data) const +{ + if (!data.avoid_required) { + return AvoidanceState::NOT_AVOID; + } + + if (!data.found_avoidance_path) { + return AvoidanceState::AVOID_PATH_NOT_READY; + } + + if (data.yield_required && parameters_->enable_yield_maneuver) { + return AvoidanceState::YIELD; + } + + if (isWaitingApproval() && path_shifter_.getShiftLines().empty()) { + return AvoidanceState::AVOID_PATH_READY; + } + + return AvoidanceState::AVOID_EXECUTE; +} + +void AvoidanceModule::updateEgoBehavior(const AvoidancePlanningData & data, ShiftedPath & path) +{ + switch (data.state) { + case AvoidanceState::NOT_AVOID: { + break; + } + case AvoidanceState::YIELD: { + insertYieldVelocity(path); + insertWaitPoint(parameters_->use_constraints_for_decel, path); + removeAllRegisteredShiftPoints(path_shifter_); + break; + } + case AvoidanceState::AVOID_PATH_NOT_READY: { + insertPrepareVelocity(false, path); + insertWaitPoint(parameters_->use_constraints_for_decel, path); + break; + } + case AvoidanceState::AVOID_PATH_READY: { + insertPrepareVelocity(true, path); + insertWaitPoint(parameters_->use_constraints_for_decel, path); + break; + } + case AvoidanceState::AVOID_EXECUTE: { + break; + } + default: + throw std::domain_error("invalid behavior"); } } @@ -741,6 +880,7 @@ double AvoidanceModule::getShiftLength( return is_object_on_right ? std::min(shift_length, getLeftShiftBound()) : std::max(shift_length, getRightShiftBound()); } + /** * calcRawShiftLinesFromObjects * @@ -2162,11 +2302,16 @@ bool AvoidanceModule::isEnoughMargin( const auto v_obj_lon = getLongitudinalVelocity(p_ref, p_obj.get(), v_obj); + double hysteresis_factor = 1.0; + if (avoidance_data_.state == AvoidanceState::YIELD) { + hysteresis_factor = parameters_->safety_check_hysteresis_factor; + } + const auto shift_length = calcLateralDeviation(p_ref, getPoint(p_ego)); const auto lateral_distance = std::abs(object.overhang_dist - shift_length) - 0.5 * vehicle_width; const auto lateral_margin = getLateralMarginFromVelocity(std::abs(v_ego_lon - v_obj_lon)); - if (lateral_distance > lateral_margin) { + if (lateral_distance > lateral_margin * hysteresis_factor) { return true; } @@ -2190,7 +2335,7 @@ bool AvoidanceModule::isEnoughMargin( margin_data.base_link2rear = base_link2rear; } - if (longitudinal_distance > longitudinal_margin) { + if (longitudinal_distance > longitudinal_margin * hysteresis_factor) { return true; } @@ -2693,12 +2838,12 @@ BehaviorModuleOutput AvoidanceModule::plan() * Yes -> clear WAIT_APPROVAL state. * No -> do nothing. */ - if (!data.unapproved_new_sl.empty()) { - debug_data_.new_shift_lines = data.unapproved_new_sl; - DEBUG_PRINT("new_shift_lines size = %lu", data.unapproved_new_sl.size()); - printShiftLines(data.unapproved_new_sl, "new_shift_lines"); + if (!data.safe_new_sl.empty()) { + debug_data_.new_shift_lines = data.safe_new_sl; + DEBUG_PRINT("new_shift_lines size = %lu", data.safe_new_sl.size()); + printShiftLines(data.safe_new_sl, "new_shift_lines"); - const auto sl = getNonStraightShiftLine(data.unapproved_new_sl); + const auto sl = getNonStraightShiftLine(data.safe_new_sl); if (sl.getRelativeLength() > 0.0) { removePreviousRTCStatusRight(); } else if (sl.getRelativeLength() < 0.0) { @@ -2706,7 +2851,7 @@ BehaviorModuleOutput AvoidanceModule::plan() } else { RCLCPP_WARN_STREAM(getLogger(), "Direction is UNKNOWN"); } - addShiftLineIfApproved(data.unapproved_new_sl); + addShiftLineIfApproved(data.safe_new_sl); } else if (isWaitingApproval()) { clearWaitingApproval(); removeCandidateRTCStatus(); @@ -2726,11 +2871,6 @@ BehaviorModuleOutput AvoidanceModule::plan() prev_linear_shift_path_ = toShiftedPath(avoidance_data_.reference_path); path_shifter_.generate(&prev_linear_shift_path_, true, SHIFT_TYPE::LINEAR); prev_reference_ = avoidance_data_.reference_path; - if (parameters_->publish_debug_marker) { - setDebugData(avoidance_data_, path_shifter_, debug_data_); - } else { - debug_marker_.markers.clear(); - } } BehaviorModuleOutput output; @@ -2740,6 +2880,16 @@ BehaviorModuleOutput AvoidanceModule::plan() avoidance_path.path = util::resamplePathWithSpline(avoidance_path.path, parameters_->resample_interval_for_output); } + + avoidance_data_.state = updateEgoState(data); + updateEgoBehavior(data, avoidance_path); + + if (parameters_->publish_debug_marker) { + setDebugData(avoidance_data_, path_shifter_, debug_data_); + } else { + debug_marker_.markers.clear(); + } + output.path = std::make_shared(avoidance_path.path); const size_t ego_idx = findEgoIndex(output.path->points); @@ -2763,12 +2913,12 @@ CandidateOutput AvoidanceModule::planCandidate() const auto shifted_path = data.candidate_path; - if (!data.unapproved_new_sl.empty()) { // clip from shift start index for visualize - clipByMinStartIdx(data.unapproved_new_sl, shifted_path.path); + if (!data.safe_new_sl.empty()) { // clip from shift start index for visualize + clipByMinStartIdx(data.safe_new_sl, shifted_path.path); - const auto sl = getNonStraightShiftLine(data.unapproved_new_sl); - const auto sl_front = data.unapproved_new_sl.front(); - const auto sl_back = data.unapproved_new_sl.back(); + const auto sl = getNonStraightShiftLine(data.safe_new_sl); + const auto sl_front = data.safe_new_sl.front(); + const auto sl_back = data.safe_new_sl.back(); output.lateral_shift = sl.getRelativeLength(); output.start_distance_to_path_change = sl_front.start_longitudinal; @@ -3000,12 +3150,6 @@ ShiftedPath AvoidanceModule::generateAvoidancePath(PathShifter & path_shifter) c return shifted_path; } -void AvoidanceModule::postProcess(PathShifter & path_shifter) const -{ - const size_t nearest_idx = findEgoIndex(path_shifter.getReferencePath().points); - path_shifter.removeBehindShiftLineAndSetBaseOffset(nearest_idx); -} - void AvoidanceModule::updateData() { debug_data_ = DebugData(); @@ -3015,6 +3159,10 @@ void AvoidanceModule::updateData() updateRegisteredObject(avoidance_data_.target_objects); compensateDetectionLost(avoidance_data_.target_objects, avoidance_data_.other_objects); + std::sort( + avoidance_data_.target_objects.begin(), avoidance_data_.target_objects.end(), + [](auto a, auto b) { return a.longitudinal < b.longitudinal; }); + path_shifter_.setPath(avoidance_data_.reference_path); // update registered shift point for new reference path & remove past objects @@ -3290,12 +3438,20 @@ void AvoidanceModule::setDebugData( using marker_utils::avoidance_marker::createOtherObjectsMarkerArray; using marker_utils::avoidance_marker::createOverhangFurthestLineStringMarkerArray; using marker_utils::avoidance_marker::createPredictedVehiclePositions; + using marker_utils::avoidance_marker::createSafetyCheckMarkerArray; using marker_utils::avoidance_marker::createTargetObjectsMarkerArray; using marker_utils::avoidance_marker::createUnavoidableObjectsMarkerArray; using marker_utils::avoidance_marker::createUnsafeObjectsMarkerArray; using marker_utils::avoidance_marker::makeOverhangToRoadShoulderMarkerArray; + using motion_utils::createDeadLineVirtualWallMarker; + using motion_utils::createSlowDownVirtualWallMarker; + using motion_utils::createStopVirtualWallMarker; + using tier4_autoware_utils::appendMarkerArray; + using tier4_autoware_utils::calcOffsetPose; debug_marker_.markers.clear(); + const auto & base_link2front = planner_data_->parameters.base_link2front; + const auto current_time = rclcpp::Clock{RCL_ROS_TIME}.now(); const auto add = [this](const MarkerArray & added) { tier4_autoware_utils::appendMarkerArray(added, &debug_marker_); @@ -3321,6 +3477,26 @@ void AvoidanceModule::setDebugData( add(createPathMarkerArray(prev_linear_shift_path_.path, "prev_linear_shift", 0, 0.5, 0.4, 0.6)); add(createPoseMarkerArray(data.reference_pose, "reference_pose", 0, 0.9, 0.3, 0.3)); + if (debug.stop_pose) { + const auto p_front = calcOffsetPose(debug.stop_pose.get(), base_link2front, 0.0, 0.0); + appendMarkerArray( + createStopVirtualWallMarker(p_front, "avoidance stop", current_time, 0L), &debug_marker_); + } + + if (debug.slow_pose) { + const auto p_front = calcOffsetPose(debug.slow_pose.get(), base_link2front, 0.0, 0.0); + appendMarkerArray( + createSlowDownVirtualWallMarker(p_front, "avoidance slow", current_time, 0L), &debug_marker_); + } + + if (debug.feasible_bound) { + const auto p_front = calcOffsetPose(debug.feasible_bound.get(), base_link2front, 0.0, 0.0); + appendMarkerArray( + createDeadLineVirtualWallMarker(p_front, "feasible bound", current_time, 0L), &debug_marker_); + } + + add(createSafetyCheckMarkerArray(data.state, getEgoPose(), debug)); + add(createLaneletsAreaMarkerArray(*debug.current_lanelets, "current_lanelet", 0.0, 1.0, 0.0)); add(createLaneletsAreaMarkerArray(*debug.expanded_lanelets, "expanded_lanelet", 0.8, 0.8, 0.0)); add(createTargetObjectsMarkerArray(data.target_objects, "target_objects")); @@ -3376,6 +3552,125 @@ void AvoidanceModule::updateAvoidanceDebugData( } } +double AvoidanceModule::getFeasibleDecelDistance(const double target_velocity) const +{ + const auto & a_now = planner_data_->self_acceleration->accel.accel.linear.x; + const auto & a_lim = parameters_->max_deceleration; + const auto & j_lim = parameters_->max_jerk; + return calcDecelDistWithJerkAndAccConstraints( + getEgoSpeed(), target_velocity, a_now, a_lim, j_lim, -1.0 * j_lim); +} + +double AvoidanceModule::getMildDecelDistance(const double target_velocity) const +{ + const auto & a_now = planner_data_->self_acceleration->accel.accel.linear.x; + const auto & a_lim = parameters_->nominal_deceleration; + const auto & j_lim = parameters_->nominal_jerk; + return calcDecelDistWithJerkAndAccConstraints( + getEgoSpeed(), target_velocity, a_now, a_lim, j_lim, -1.0 * j_lim); +} + +void AvoidanceModule::insertWaitPoint( + const bool use_constraints_for_decel, ShiftedPath & shifted_path) const +{ + const auto & p = parameters_; + const auto & data = avoidance_data_; + const auto & base_link2front = planner_data_->parameters.base_link2front; + const auto & vehicle_width = planner_data_->parameters.vehicle_width; + + if (data.target_objects.empty()) { + return; + } + + if (data.avoiding_now) { + return; + } + + // D5 + // |<---->| D4 + // |<----------------------------------------------------------------------->| + // +-----------+ D1 D2 D3 +-----------+ + // | | |<------->|<------------------------->|<----------------->| | + // | ego |======= x ======= x ========================= x ==================| obj | + // | | stop_point avoid avoid | | + // +-----------+ start end +-----------+ + // + // D1: p.min_prepare_distance + // D2: min_avoid_distance + // D3: longitudinal_avoid_margin_front (margin + D5) + // D4: o_front.longitudinal + // D5: base_link2front + + const auto o_front = data.target_objects.front(); + + const auto avoid_margin = + p->lateral_collision_safety_buffer + p->lateral_collision_margin + 0.5 * vehicle_width; + const auto variable = + getMinimumAvoidanceDistance(getShiftLength(o_front, isOnRight(o_front), avoid_margin)); + const auto constant = + p->min_prepare_distance + p->longitudinal_collision_safety_buffer + base_link2front; + const auto start_longitudinal = + o_front.longitudinal - + std::clamp(variable + constant, p->stop_min_distance, p->stop_max_distance); + + if (!use_constraints_for_decel) { + insertDecelPoint( + getEgoPosition(), start_longitudinal, 0.0, shifted_path.path, debug_data_.stop_pose); + return; + } + + const auto stop_distance = getMildDecelDistance(0.0); + + const auto insert_distance = std::max(start_longitudinal, stop_distance); + + insertDecelPoint( + getEgoPosition(), insert_distance, 0.0, shifted_path.path, debug_data_.stop_pose); +} + +void AvoidanceModule::insertYieldVelocity(ShiftedPath & shifted_path) const +{ + const auto & p = parameters_; + const auto & data = avoidance_data_; + + if (data.target_objects.empty()) { + return; + } + + if (data.avoiding_now) { + return; + } + + const auto decel_distance = getMildDecelDistance(p->yield_velocity); + + insertDecelPoint( + getEgoPosition(), decel_distance, p->yield_velocity, shifted_path.path, debug_data_.slow_pose); +} + +void AvoidanceModule::insertPrepareVelocity(const bool avoidable, ShiftedPath & shifted_path) const +{ + const auto & data = avoidance_data_; + + if (data.target_objects.empty()) { + return; + } + + if (data.target_objects.front().reason != AvoidanceDebugFactor::TOO_LARGE_JERK) { + return; + } + + if (data.avoiding_now) { + return; + } + + if (avoidable) { + return; + } + + const auto decel_distance = getFeasibleDecelDistance(0.0); + + insertDecelPoint(getEgoPosition(), decel_distance, 0.0, shifted_path.path, debug_data_.slow_pose); +} + std::shared_ptr AvoidanceModule::get_debug_msg_array() const { debug_data_.avoidance_debug_msg_array.header.stamp = clock_->now(); diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_utils.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_utils.cpp index 7e378dda5954a..a4e599bc60961 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_utils.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_utils.cpp @@ -33,10 +33,14 @@ namespace behavior_path_planner { +using motion_utils::calcLongitudinalOffsetPoint; +using motion_utils::findNearestSegmentIndex; +using motion_utils::insertTargetPoint; using tier4_autoware_utils::calcDistance2d; using tier4_autoware_utils::calcOffsetPose; using tier4_autoware_utils::calcYawDeviation; using tier4_autoware_utils::createQuaternionFromRPY; +using tier4_autoware_utils::getPose; using tier4_autoware_utils::pose2transform; namespace @@ -60,6 +64,174 @@ geometry_msgs::msg::Polygon toMsg(const tier4_autoware_utils::Polygon2d & polygo return ret; } +/** + * @brief update traveling distance, velocity and acceleration under constant jerk. + * @param (x) current traveling distance [m/s] + * @param (v) current velocity [m/s] + * @param (a) current acceleration [m/ss] + * @param (j) target jerk [m/sss] + * @param (t) time [s] + * @return updated traveling distance, velocity and acceleration + */ +std::tuple update( + const double x, const double v, const double a, const double j, const double t) +{ + const double a_new = a + j * t; + const double v_new = v + a * t + 0.5 * j * t * t; + const double x_new = x + v * t + 0.5 * a * t * t + (1.0 / 6.0) * j * t * t * t; + + return {x_new, v_new, a_new}; +} + +/** + * @brief calculate distance until velocity is reached target velocity (TYPE: TRAPEZOID ACCELERATION + * PROFILE). this type of profile has ZERO JERK time. + * + * [ACCELERATION PROFILE] + * a ^ + * | + * a0 * + * |* + * ----+-*-------------------*------> t + * | * * + * | * * + * | a1 *************** + * | + * + * [JERK PROFILE] + * j ^ + * | + * | ja **** + * | * + * ----+----***************---------> t + * | * + * | * + * jd ****** + * | + * + * @param (v0) current velocity [m/s] + * @param (a0) current acceleration [m/ss] + * @param (am) minimum deceleration [m/ss] + * @param (ja) maximum jerk [m/sss] + * @param (jd) minimum jerk [m/sss] + * @param (t_min) duration of constant deceleration [s] + * @return moving distance until velocity is reached vt [m] + */ +double calcDecelDistPlanType1( + const double v0, const double a0, const double am, const double ja, const double jd, + const double t_min) +{ + constexpr double epsilon = 1e-3; + + // negative jerk time + const double j1 = am < a0 ? jd : ja; + const double t1 = epsilon < (am - a0) / j1 ? (am - a0) / j1 : 0.0; + const auto [x1, v1, a1] = update(0.0, v0, a0, j1, t1); + + // zero jerk time + const double t2 = epsilon < t_min ? t_min : 0.0; + const auto [x2, v2, a2] = update(x1, v1, a1, 0.0, t2); + + // positive jerk time + const double t3 = epsilon < (0.0 - am) / ja ? (0.0 - am) / ja : 0.0; + const auto [x3, v3, a3] = update(x2, v2, a2, ja, t3); + + return x3; +} + +/** + * @brief calculate distance until velocity is reached target velocity (TYPE: TRIANGLE ACCELERATION + * PROFILE), This type of profile do NOT have ZERO JERK time. + * + * [ACCELERATION PROFILE] + * a ^ + * | + * a0 * + * |* + * ----+-*-----*--------------------> t + * | * * + * | * * + * | a1 * + * | + * + * [JERK PROFILE] + * j ^ + * | + * | ja **** + * | * + * ----+----*-----------------------> t + * | * + * | * + * jd ****** + * | + * + * @param (v0) current velocity [m/s] + * @param (a0) current acceleration [m/ss] + * @param (am) minimum deceleration [m/ss] + * @param (ja) maximum jerk [m/sss] + * @param (jd) minimum jerk [m/sss] + * @return moving distance until velocity is reached vt [m] + */ +double calcDecelDistPlanType2( + const double v0, const double vt, const double a0, const double ja, const double jd) +{ + constexpr double epsilon = 1e-3; + + const double a1_square = (vt - v0 - 0.5 * (0.0 - a0) / jd * a0) * (2.0 * ja * jd / (ja - jd)); + const double a1 = -std::sqrt(a1_square); + + // negative jerk time + const double t1 = epsilon < (a1 - a0) / jd ? (a1 - a0) / jd : 0.0; + const auto [x1, v1, no_use_a1] = update(0.0, v0, a0, jd, t1); + + // positive jerk time + const double t2 = epsilon < (0.0 - a1) / ja ? (0.0 - a1) / ja : 0.0; + const auto [x2, v2, a2] = update(x1, v1, a1, ja, t2); + + return x2; +} + +/** + * @brief calculate distance until velocity is reached target velocity (TYPE: LINEAR ACCELERATION + * PROFILE). This type of profile has only positive jerk time. + * + * [ACCELERATION PROFILE] + * a ^ + * | + * ----+----*-----------------------> t + * | * + * | * + * | * + * |* + * a0 * + * | + * + * [JERK PROFILE] + * j ^ + * | + * ja ****** + * | * + * | * + * ----+----*-----------------------> t + * | + * + * @param (v0) current velocity [m/s] + * @param (a0) current acceleration [m/ss] + * @param (ja) maximum jerk [m/sss] + * @return moving distance until velocity is reached vt [m] + */ +double calcDecelDistPlanType3(const double v0, const double a0, const double ja) +{ + constexpr double epsilon = 1e-3; + + // positive jerk time + const double t_acc = (0.0 - a0) / ja; + const double t1 = epsilon < t_acc ? t_acc : 0.0; + const auto [x1, v1, a1] = update(0.0, v0, a0, ja, t1); + + return x1; +} + } // namespace bool isOnRight(const ObjectData & obj) { return obj.lateral < 0.0; } @@ -559,4 +731,60 @@ lanelet::ConstLanelets getTargetLanelets( return target_lanelets; } +double calcDecelDistWithJerkAndAccConstraints( + const double current_vel, const double target_vel, const double current_acc, const double acc_min, + const double jerk_acc, const double jerk_dec) +{ + constexpr double epsilon = 1e-3; + const double t_dec = + acc_min < current_acc ? (acc_min - current_acc) / jerk_dec : (acc_min - current_acc) / jerk_acc; + const double t_acc = (0.0 - acc_min) / jerk_acc; + const double t_min = (target_vel - current_vel - current_acc * t_dec - + 0.5 * jerk_dec * t_dec * t_dec - 0.5 * acc_min * t_acc) / + acc_min; + + // check if it is possible to decelerate to the target velocity + // by simply bringing the current acceleration to zero. + const auto is_decel_needed = + 0.5 * (0.0 - current_acc) / jerk_acc * current_acc > target_vel - current_vel; + + if (t_min > epsilon) { + return calcDecelDistPlanType1(current_vel, current_acc, acc_min, jerk_acc, jerk_dec, t_min); + } else if (is_decel_needed || current_acc > epsilon) { + return calcDecelDistPlanType2(current_vel, target_vel, current_acc, jerk_acc, jerk_dec); + } + + return calcDecelDistPlanType3(current_vel, current_acc, jerk_acc); +} + +void insertDecelPoint( + const Point & p_src, const double offset, const double velocity, PathWithLaneId & path, + boost::optional & p_out) +{ + const auto decel_point = calcLongitudinalOffsetPoint(path.points, p_src, offset); + + if (!decel_point) { + // TODO(Satoshi OTA) Think later the process in the case of no decel point found. + return; + } + + const auto seg_idx = findNearestSegmentIndex(path.points, decel_point.get()); + const auto insert_idx = insertTargetPoint(seg_idx, decel_point.get(), path.points); + + if (!insert_idx) { + // TODO(Satoshi OTA) Think later the process in the case of no decel point found. + return; + } + + const auto insertVelocity = [&insert_idx](PathWithLaneId & path, const float v) { + for (size_t i = insert_idx.get(); i < path.points.size(); ++i) { + const auto & original_velocity = path.points.at(i).point.longitudinal_velocity_mps; + path.points.at(i).point.longitudinal_velocity_mps = std::min(original_velocity, v); + } + }; + + insertVelocity(path, velocity); + + p_out = getPose(path.points.at(insert_idx.get())); +} } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/debug.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/debug.cpp index 86afb38acca38..ed0f5c068142b 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/debug.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/debug.cpp @@ -29,6 +29,7 @@ using behavior_path_planner::AvoidLine; using behavior_path_planner::util::shiftPose; using tier4_autoware_utils::appendMarkerArray; using tier4_autoware_utils::calcDistance2d; +using tier4_autoware_utils::calcOffsetPose; using tier4_autoware_utils::createDefaultMarker; using tier4_autoware_utils::createMarkerColor; using tier4_autoware_utils::createMarkerScale; @@ -120,12 +121,103 @@ MarkerArray createEgoStatusMarkerArray( std::ostringstream string_stream; string_stream << std::fixed << std::setprecision(2) << std::boolalpha; string_stream << "avoid_now:" << data.avoiding_now << "," + << "avoid_req:" << data.avoid_required << "," + << "yield_req:" << data.yield_required << "," << "safe:" << data.safe; marker.text = string_stream.str(); msg.markers.push_back(marker); } + { + std::ostringstream string_stream; + string_stream << "ego_state:"; + switch (data.state) { + case AvoidanceState::NOT_AVOID: + string_stream << "NOT_AVOID"; + break; + case AvoidanceState::AVOID_PATH_NOT_READY: + string_stream << "AVOID_PATH_NOT_READY"; + marker.color = createMarkerColor(1.0, 0.0, 0.0, 0.999); + break; + case AvoidanceState::YIELD: + string_stream << "YIELD"; + marker.color = createMarkerColor(1.0, 1.0, 0.0, 0.999); + break; + case AvoidanceState::AVOID_PATH_READY: + string_stream << "AVOID_PATH_READY"; + marker.color = createMarkerColor(0.0, 1.0, 0.0, 0.999); + break; + case AvoidanceState::AVOID_EXECUTE: + string_stream << "AVOID_EXECUTE"; + marker.color = createMarkerColor(0.0, 1.0, 0.0, 0.999); + break; + default: + throw std::domain_error("invalid behavior"); + } + marker.text = string_stream.str(); + marker.pose.position.z += 2.0; + marker.id++; + + msg.markers.push_back(marker); + } + + return msg; +} + +MarkerArray createSafetyCheckMarkerArray( + const AvoidanceState & state, const Pose & pose, const DebugData & data) +{ + const auto current_time = rclcpp::Clock{RCL_ROS_TIME}.now(); + MarkerArray msg; + + if (data.exist_adjacent_objects) { + auto marker = createDefaultMarker( + "map", current_time, "safety_alert", 0L, Marker::CYLINDER, createMarkerScale(0.2, 0.2, 2.0), + createMarkerColor(1.0, 1.0, 0.0, 0.8)); + + marker.color = state == AvoidanceState::YIELD ? createMarkerColor(1.0, 0.0, 0.0, 0.8) + : createMarkerColor(1.0, 1.0, 0.0, 0.8); + + marker.pose = calcOffsetPose(pose, 0.0, 1.5, 1.0); + msg.markers.push_back(marker); + + marker.pose = calcOffsetPose(pose, 0.0, -1.5, 1.0); + marker.id++; + msg.markers.push_back(marker); + } + + if (state == AvoidanceState::YIELD) { + return msg; + } + + { + auto marker = createDefaultMarker( + "map", current_time, "safety_longitudinal_margin", 0L, Marker::CUBE, + createMarkerScale(3.0, 1.5, 1.5), createMarkerColor(1.0, 1.0, 1.0, 0.1)); + + for (const auto & m : data.margin_data_array) { + if (m.enough_lateral_margin) { + continue; + } + + constexpr double max_x = 10.0; + + const auto offset = 0.5 * (m.base_link2front + m.base_link2rear) - m.base_link2rear; + const auto diff = m.longitudinal_distance - m.longitudinal_margin; + const auto scale_x = std::min(max_x, 2.0 * (m.base_link2front + m.base_link2rear + diff)); + + const auto ratio = std::clamp(diff / max_x, 0.0, 1.0); + + marker.pose = calcOffsetPose(m.pose, offset, 0.0, 0.0); + marker.pose.position.z += 1.0; + marker.scale = createMarkerScale(scale_x, 2.0 * m.vehicle_width, 2.0); + marker.color = createMarkerColor(1.0 - ratio, ratio, 0.0, 0.1); + marker.id++; + msg.markers.push_back(marker); + } + } + return msg; } From 99208eb32498233b1ed757ad291eef245cfde740 Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Thu, 12 Jan 2023 12:20:47 +0900 Subject: [PATCH 12/49] fix(system_error_monitor): hold emergency when timeout happens (#2634) * fix(system_error_monitor): hold emergency when timeout * fix: initialize other fault --- .../system_error_monitor_core.hpp | 1 + .../src/system_error_monitor_core.cpp | 49 +++++++++++++------ 2 files changed, 34 insertions(+), 16 deletions(-) diff --git a/system/system_error_monitor/include/system_error_monitor/system_error_monitor_core.hpp b/system/system_error_monitor/include/system_error_monitor/system_error_monitor_core.hpp index 102986eff5809..75d80b56ea711 100644 --- a/system/system_error_monitor/include/system_error_monitor/system_error_monitor_core.hpp +++ b/system/system_error_monitor/include/system_error_monitor/system_error_monitor_core.hpp @@ -141,6 +141,7 @@ class AutowareErrorMonitor : public rclcpp::Node autoware_auto_system_msgs::msg::HazardStatus * hazard_status) const; autoware_auto_system_msgs::msg::HazardStatus judgeHazardStatus() const; void updateHazardStatus(); + void updateTimeoutHazardStatus(); bool canAutoRecovery() const; bool isEmergencyHoldingRequired() const; }; diff --git a/system/system_error_monitor/src/system_error_monitor_core.cpp b/system/system_error_monitor/src/system_error_monitor_core.cpp index 9240563741015..d6cc771e5dbd2 100644 --- a/system/system_error_monitor/src/system_error_monitor_core.cpp +++ b/system/system_error_monitor/src/system_error_monitor_core.cpp @@ -182,20 +182,6 @@ std::set getErrorModules( return error_modules; } -autoware_auto_system_msgs::msg::HazardStatus createTimeoutHazardStatus() -{ - autoware_auto_system_msgs::msg::HazardStatus hazard_status; - hazard_status.level = autoware_auto_system_msgs::msg::HazardStatus::SINGLE_POINT_FAULT; - hazard_status.emergency = true; - hazard_status.emergency_holding = false; - diagnostic_msgs::msg::DiagnosticStatus diag; - diag.name = "system_error_monitor/input_data_timeout"; - diag.hardware_id = "system_error_monitor"; - diag.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - hazard_status.diag_single_point_fault.push_back(diag); - return hazard_status; -} - int isInNoFaultCondition( const autoware_auto_system_msgs::msg::AutowareState & autoware_state, const tier4_control_msgs::msg::GateMode & current_gate_mode) @@ -459,13 +445,15 @@ void AutowareErrorMonitor::onTimer() RCLCPP_WARN_THROTTLE( get_logger(), *get_clock(), std::chrono::milliseconds(1000).count(), "input data is timeout"); - publishHazardStatus(createTimeoutHazardStatus()); + updateTimeoutHazardStatus(); + publishHazardStatus(hazard_status_); } return; } if (isDataHeartbeatTimeout()) { - publishHazardStatus(createTimeoutHazardStatus()); + updateTimeoutHazardStatus(); + publishHazardStatus(hazard_status_); return; } @@ -610,6 +598,35 @@ void AutowareErrorMonitor::updateHazardStatus() } } +void AutowareErrorMonitor::updateTimeoutHazardStatus() +{ + const bool prev_emergency_status = hazard_status_.emergency; + + hazard_status_.level = autoware_auto_system_msgs::msg::HazardStatus::SINGLE_POINT_FAULT; + hazard_status_.emergency = true; + + if (hazard_status_.emergency != prev_emergency_status) { + emergency_state_switch_time_ = this->now(); + } + + hazard_status_.diag_no_fault = std::vector(); + hazard_status_.diag_safe_fault = std::vector(); + hazard_status_.diag_latent_fault = std::vector(); + + diagnostic_msgs::msg::DiagnosticStatus diag; + diag.name = "system_error_monitor/input_data_timeout"; + diag.hardware_id = "system_error_monitor"; + diag.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + hazard_status_.diag_single_point_fault = + std::vector{diag}; + + // Update emergency_holding condition + if (params_.use_emergency_hold) { + const auto emergency_duration = (this->now() - emergency_state_switch_time_).seconds(); + hazard_status_.emergency_holding = emergency_duration > params_.hazard_recovery_timeout; + } +} + bool AutowareErrorMonitor::canAutoRecovery() const { const auto error_modules = getErrorModules(hazard_status_, params_.emergency_hazard_level); From 33ecdb59d87682e1583138b19d9f5edfce9e8bf7 Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Thu, 12 Jan 2023 12:24:39 +0900 Subject: [PATCH 13/49] feat(obstacle_stop_planner): add parameter for voxel grid filter and publish pointcloud as debug (#2624) * feat: publish pointcloud as debug * feat: add voxel grid parameter * ci(pre-commit): autofix * fix spell check Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../config/obstacle_stop_planner.param.yaml | 3 +++ .../include/obstacle_stop_planner/node.hpp | 2 ++ .../include/obstacle_stop_planner/planner_data.hpp | 9 +++++++++ planning/obstacle_stop_planner/src/node.cpp | 9 ++++++++- 4 files changed, 22 insertions(+), 1 deletion(-) diff --git a/planning/obstacle_stop_planner/config/obstacle_stop_planner.param.yaml b/planning/obstacle_stop_planner/config/obstacle_stop_planner.param.yaml index ea506d349dc34..de07e9e41b7c1 100644 --- a/planning/obstacle_stop_planner/config/obstacle_stop_planner.param.yaml +++ b/planning/obstacle_stop_planner/config/obstacle_stop_planner.param.yaml @@ -3,6 +3,9 @@ hunting_threshold: 0.5 # even if the obstacle disappears, the stop judgment continues for hunting_threshold [s] max_velocity: 20.0 # max velocity [m/s] enable_slow_down: False # whether to use slow down planner [-] + voxel_grid_x: 0.05 # voxel grid x parameter for filtering pointcloud [m] + voxel_grid_y: 0.05 # voxel grid y parameter for filtering pointcloud [m] + voxel_grid_z: 100000.0 # voxel grid z parameter for filtering pointcloud [m] stop_planner: # params for stop position diff --git a/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp b/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp index 2312400065eaf..236d7338bcdd7 100644 --- a/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp +++ b/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp @@ -117,6 +117,8 @@ class ObstacleStopPlannerNode : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_velocity_limit_; + rclcpp::Publisher::SharedPtr pub_obstacle_pointcloud_; + std::unique_ptr acc_controller_; std::shared_ptr debug_ptr_; boost::optional latest_stop_point_{boost::none}; diff --git a/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_data.hpp b/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_data.hpp index 9ce3bd6213b88..58895a9d40c78 100644 --- a/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_data.hpp +++ b/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_data.hpp @@ -70,6 +70,15 @@ struct NodeParam // yaw threshold for ego's nearest index double ego_nearest_yaw_threshold; + + // voxel grid x parameter for filtering pointcloud [m] + double voxel_grid_x; + + // voxel grid y parameter for filtering pointcloud [m] + double voxel_grid_y; + + // voxel grid z parameter for filtering pointcloud [m] + double voxel_grid_z; }; struct StopParam diff --git a/planning/obstacle_stop_planner/src/node.cpp b/planning/obstacle_stop_planner/src/node.cpp index 578d72292d8c3..85f28c3052994 100644 --- a/planning/obstacle_stop_planner/src/node.cpp +++ b/planning/obstacle_stop_planner/src/node.cpp @@ -68,6 +68,9 @@ ObstacleStopPlannerNode::ObstacleStopPlannerNode(const rclcpp::NodeOptions & nod p.hunting_threshold = declare_parameter("hunting_threshold"); p.ego_nearest_dist_threshold = declare_parameter("ego_nearest_dist_threshold"); p.ego_nearest_yaw_threshold = declare_parameter("ego_nearest_yaw_threshold"); + p.voxel_grid_x = declare_parameter("voxel_grid_x", 0.05); + p.voxel_grid_y = declare_parameter("voxel_grid_y", 0.05); + p.voxel_grid_z = declare_parameter("voxel_grid_z", 100000.0); } { @@ -163,6 +166,9 @@ ObstacleStopPlannerNode::ObstacleStopPlannerNode(const rclcpp::NodeOptions & nod pub_velocity_limit_ = this->create_publisher( "~/output/max_velocity", rclcpp::QoS{1}.transient_local()); + pub_obstacle_pointcloud_ = + this->create_publisher("~/debug/obstacle_pointcloud", 1); + // Subscribers sub_point_cloud_ = this->create_subscription( "~/input/pointcloud", rclcpp::SensorDataQoS(), @@ -213,10 +219,11 @@ void ObstacleStopPlannerNode::onPointCloud(const PointCloud2::ConstSharedPtr inp no_height_pointcloud_ptr->push_back(pcl::PointXYZ(point.x, point.y, 0.0)); } filter.setInputCloud(no_height_pointcloud_ptr); - filter.setLeafSize(0.05f, 0.05f, 100000.0f); + filter.setLeafSize(node_param_.voxel_grid_x, node_param_.voxel_grid_y, node_param_.voxel_grid_z); filter.filter(*no_height_filtered_pointcloud_ptr); pcl::toROSMsg(*no_height_filtered_pointcloud_ptr, *obstacle_ros_pointcloud_ptr_); obstacle_ros_pointcloud_ptr_->header = input_msg->header; + pub_obstacle_pointcloud_->publish(*obstacle_ros_pointcloud_ptr_); } void ObstacleStopPlannerNode::onTrigger(const Trajectory::ConstSharedPtr input_msg) From 30cda9c27f13334e066851af50df15a9a25e6007 Mon Sep 17 00:00:00 2001 From: Takeshi Ishita Date: Thu, 12 Jan 2023 18:34:47 +0900 Subject: [PATCH 14/49] refactor(ekf_localizer): remove 'dim_x_ex_' as it is no longer used (#2640) --- .../ekf_localizer/include/ekf_localizer/ekf_localizer.hpp | 3 +-- localization/ekf_localizer/src/ekf_localizer.cpp | 2 -- 2 files changed, 1 insertion(+), 4 deletions(-) diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp index dc5e5659001dd..85f7aa8b1829f 100644 --- a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp @@ -170,8 +170,7 @@ class EKFLocalizer : public rclcpp::Node /* parameters */ - int dim_x_; //!< @brief dimension of EKF state - int dim_x_ex_; //!< @brief dimension of extended EKF state (dim_x_ * extended_state_step) + int dim_x_; //!< @brief dimension of EKF state /* process noise variance for discrete model */ double proc_cov_yaw_d_; //!< @brief discrete yaw process noise diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp index d505f5c528a35..528fbdc5344e0 100644 --- a/localization/ekf_localizer/src/ekf_localizer.cpp +++ b/localization/ekf_localizer/src/ekf_localizer.cpp @@ -94,8 +94,6 @@ EKFLocalizer::EKFLocalizer(const std::string & node_name, const rclcpp::NodeOpti &EKFLocalizer::serviceTriggerNode, this, std::placeholders::_1, std::placeholders::_2), rclcpp::ServicesQoS().get_rmw_qos_profile()); - dim_x_ex_ = dim_x_ * params_.extend_state_step; - tf_br_ = std::make_shared( std::shared_ptr(this, [](auto) {})); From e77bc201f59e96df706af1a188b16c1f0520a5aa Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Thu, 12 Jan 2023 18:50:01 +0900 Subject: [PATCH 15/49] fix(obstacle_stop_planner): remove `no_height_pointcloud_ptr` in filtering pointcloud (#2623) * fix(obstacle_stop_planner): fix point.z in filtering pointcloud * fix: remoge no_height_pointcloud_ptr --- planning/obstacle_stop_planner/src/node.cpp | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/planning/obstacle_stop_planner/src/node.cpp b/planning/obstacle_stop_planner/src/node.cpp index 85f28c3052994..0a29beec48840 100644 --- a/planning/obstacle_stop_planner/src/node.cpp +++ b/planning/obstacle_stop_planner/src/node.cpp @@ -210,15 +210,11 @@ void ObstacleStopPlannerNode::onPointCloud(const PointCloud2::ConstSharedPtr inp obstacle_ros_pointcloud_ptr_ = std::make_shared(); pcl::VoxelGrid filter; PointCloud::Ptr pointcloud_ptr(new PointCloud); - PointCloud::Ptr no_height_pointcloud_ptr(new PointCloud); PointCloud::Ptr no_height_filtered_pointcloud_ptr(new PointCloud); pcl::fromROSMsg(*input_msg, *pointcloud_ptr); - for (const auto & point : pointcloud_ptr->points) { - no_height_pointcloud_ptr->push_back(pcl::PointXYZ(point.x, point.y, 0.0)); - } - filter.setInputCloud(no_height_pointcloud_ptr); + filter.setInputCloud(pointcloud_ptr); filter.setLeafSize(node_param_.voxel_grid_x, node_param_.voxel_grid_y, node_param_.voxel_grid_z); filter.filter(*no_height_filtered_pointcloud_ptr); pcl::toROSMsg(*no_height_filtered_pointcloud_ptr, *obstacle_ros_pointcloud_ptr_); From 7d027baed7ea2e3ed87b7fd49178ca536e3fdea5 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> Date: Thu, 12 Jan 2023 20:24:25 +0900 Subject: [PATCH 16/49] feat(behavior_path_planner): param to skip some linestring types when expanding the drivable area (#2288) Signed-off-by: Maxime CLEMENT --- .../drivable_area_expansion.param.yaml | 14 ++++++++++---- planning/behavior_path_planner/README.md | 11 +++++++++++ .../avoidance/avoidance_module_data.hpp | 5 +++-- .../lane_change/lane_change_module_data.hpp | 2 ++ .../lane_following/lane_following_module.hpp | 2 ++ .../scene_module/pull_out/pull_out_parameters.hpp | 2 ++ .../pull_over/pull_over_parameters.hpp | 2 ++ .../scene_module/side_shift/side_shift_module.hpp | 2 ++ .../src/behavior_path_planner_node.cpp | 12 ++++++++++++ .../scene_module/avoidance/avoidance_module.cpp | 2 +- .../lane_change/lane_change_module.cpp | 4 ++-- .../lane_following/lane_following_module.cpp | 2 +- .../src/scene_module/pull_out/pull_out_module.cpp | 4 ++-- .../scene_module/pull_over/pull_over_module.cpp | 6 +++--- .../scene_module/side_shift/side_shift_module.cpp | 3 ++- 15 files changed, 57 insertions(+), 16 deletions(-) rename planning/behavior_path_planner/config/drivable_area_expansion.yaml => launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/drivable_area_expansion.param.yaml (50%) diff --git a/planning/behavior_path_planner/config/drivable_area_expansion.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/drivable_area_expansion.param.yaml similarity index 50% rename from planning/behavior_path_planner/config/drivable_area_expansion.yaml rename to launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/drivable_area_expansion.param.yaml index 8a584f471194c..29d577a7ad60a 100644 --- a/planning/behavior_path_planner/config/drivable_area_expansion.yaml +++ b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/drivable_area_expansion.param.yaml @@ -1,20 +1,26 @@ /**: ros__parameters: avoidance: - drivable_area_right_bound_offset: 0.5 - drivable_area_left_bound_offset: 0.5 + drivable_area_right_bound_offset: 0.0 + drivable_area_left_bound_offset: 0.0 + drivable_area_types_to_skip: [road_border] lane_change: drivable_area_right_bound_offset: 0.0 drivable_area_left_bound_offset: 0.0 + drivable_area_types_to_skip: [road_border] lane_following: - drivable_area_right_bound_offset: 0.5 - drivable_area_left_bound_offset: 0.5 + drivable_area_right_bound_offset: 0.0 + drivable_area_left_bound_offset: 0.0 + drivable_area_types_to_skip: [road_border] pull_out: drivable_area_right_bound_offset: 0.0 drivable_area_left_bound_offset: 0.0 + drivable_area_types_to_skip: [road_border] pull_over: drivable_area_right_bound_offset: 0.0 drivable_area_left_bound_offset: 0.0 + drivable_area_types_to_skip: [road_border] side_shift: drivable_area_right_bound_offset: 0.0 drivable_area_left_bound_offset: 0.0 + drivable_area_types_to_skip: [road_border] diff --git a/planning/behavior_path_planner/README.md b/planning/behavior_path_planner/README.md index aa2462bbaed0e..a9d04d307fdd2 100644 --- a/planning/behavior_path_planner/README.md +++ b/planning/behavior_path_planner/README.md @@ -54,6 +54,17 @@ The following modules are currently supported: ## Inner-workings / Algorithms +### Parameters for drivable area expansion + +Optionally, the drivable area can be expanded by a static distance. +Expansion parameters are defined for each module of the `behavior_path_planner` and should be prefixed accordingly (see `config/drivable_area_expansion.yaml` for an example). + +| Name | Unit | Type | Description | Default value | +| :------------------------------- | :--- | :-------------- | :----------------------------------------- | :------------ | +| drivable_area_right_bound_offset | [m] | double | expansion distance of the right bound | 0.0 | +| drivable_area_right_bound_offset | [m] | double | expansion distance of the left bound | 0.0 | +| drivable_area_types_to_skip | | list of strings | types of linestrings that are not expanded | [road_border] | + ### Behavior Tree In the behavior path planner, the behavior tree mechanism is used to manage which modules are activated in which situations. In general, this "behavior manager" like function is expected to become bigger as more and more modules are added in the future. To improve maintainability, we adopted the behavior tree. The behavior tree has the following advantages: easy visualization, easy configuration management (behaviors can be changed by replacing configuration files), and high scalability compared to the state machine. diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp index dfb776ca8ecd0..781c59bcc3901 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp @@ -242,8 +242,9 @@ struct AvoidanceParameters bool avoid_pedestrian{false}; // avoidance is performed for type object pedestrian // drivable area expansion - double drivable_area_right_bound_offset; - double drivable_area_left_bound_offset; + double drivable_area_right_bound_offset{}; + double drivable_area_left_bound_offset{}; + std::vector drivable_area_types_to_skip{}; // clip left and right bounds for objects bool enable_bound_clipping{false}; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module_data.hpp index ebbadd6b352ee..b9597fd9de8c2 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module_data.hpp @@ -19,6 +19,7 @@ #include "autoware_auto_planning_msgs/msg/path_point_with_lane_id.hpp" +#include #include namespace behavior_path_planner @@ -52,6 +53,7 @@ struct LaneChangeParameters // drivable area expansion double drivable_area_right_bound_offset{0.0}; double drivable_area_left_bound_offset{0.0}; + std::vector drivable_area_types_to_skip{}; // debug marker bool publish_debug_marker{false}; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_following/lane_following_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_following/lane_following_module.hpp index 46895de3a2307..36d42322d1aa6 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_following/lane_following_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_following/lane_following_module.hpp @@ -23,6 +23,7 @@ #include #include +#include namespace behavior_path_planner { @@ -34,6 +35,7 @@ struct LaneFollowingParameters // drivable area expansion double drivable_area_right_bound_offset; double drivable_area_left_bound_offset; + std::vector drivable_area_types_to_skip{}; }; class LaneFollowingModule : public SceneModuleInterface diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_parameters.hpp index 21b66d1ad8457..c206b39c5fdf6 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_parameters.hpp @@ -17,6 +17,7 @@ #define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__PULL_OUT__PULL_OUT_PARAMETERS_HPP_ #include +#include namespace behavior_path_planner { @@ -51,6 +52,7 @@ struct PullOutParameters // drivable area expansion double drivable_area_right_bound_offset; double drivable_area_left_bound_offset; + std::vector drivable_area_types_to_skip; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_parameters.hpp index e0c7d5b86f9f1..5f43cd152832b 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_parameters.hpp @@ -17,6 +17,7 @@ #define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__PULL_OVER__PULL_OVER_PARAMETERS_HPP_ #include +#include namespace behavior_path_planner { @@ -83,6 +84,7 @@ struct PullOverParameters // drivable area expansion double drivable_area_right_bound_offset; double drivable_area_left_bound_offset; + std::vector drivable_area_types_to_skip; // debug bool print_debug_info; }; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/side_shift_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/side_shift_module.hpp index 52073c7f50cbe..e4e39443b2350 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/side_shift_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/side_shift_module.hpp @@ -26,6 +26,7 @@ #include #include #include +#include namespace behavior_path_planner { @@ -50,6 +51,7 @@ struct SideShiftParameters // drivable area expansion double drivable_area_right_bound_offset; double drivable_area_left_bound_offset; + std::vector drivable_area_types_to_skip; }; class SideShiftModule : public SceneModuleInterface diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 54b8c66250132..bd6fe13254a5d 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -274,6 +274,8 @@ SideShiftParameters BehaviorPathPlannerNode::getSideShiftParam() p.drivable_area_right_bound_offset = dp("drivable_area_right_bound_offset", 0.0); p.shift_request_time_limit = dp("shift_request_time_limit", 1.0); p.drivable_area_left_bound_offset = dp("drivable_area_left_bound_offset", 0.0); + p.drivable_area_types_to_skip = + dp("drivable_area_types_to_skip", std::vector({"road_border"})); return p; } @@ -295,6 +297,8 @@ AvoidanceParameters BehaviorPathPlannerNode::getAvoidanceParam() declare_parameter(ns + "drivable_area_right_bound_offset"); p.drivable_area_left_bound_offset = declare_parameter(ns + "drivable_area_left_bound_offset"); + p.drivable_area_types_to_skip = + declare_parameter>(ns + "drivable_area_types_to_skip"); p.object_envelope_buffer = declare_parameter(ns + "object_envelope_buffer"); p.enable_bound_clipping = declare_parameter(ns + "enable_bound_clipping"); p.enable_avoidance_over_same_direction = @@ -436,6 +440,8 @@ LaneFollowingParameters BehaviorPathPlannerNode::getLaneFollowingParam() declare_parameter("lane_following.drivable_area_right_bound_offset", 0.0); p.drivable_area_left_bound_offset = declare_parameter("lane_following.drivable_area_left_bound_offset", 0.0); + p.drivable_area_types_to_skip = declare_parameter( + "lane_following.drivable_area_types_to_skip", std::vector({"road_border"})); p.lane_change_prepare_duration = declare_parameter("lane_following.lane_change_prepare_duration", 2.0); return p; @@ -477,6 +483,8 @@ LaneChangeParameters BehaviorPathPlannerNode::getLaneChangeParam() // drivable area expansion p.drivable_area_right_bound_offset = dp("drivable_area_right_bound_offset", 0.0); p.drivable_area_left_bound_offset = dp("drivable_area_left_bound_offset", 0.0); + p.drivable_area_types_to_skip = + dp("drivable_area_types_to_skip", std::vector({"road_border"})); // debug marker p.publish_debug_marker = dp("publish_debug_marker", false); @@ -587,6 +595,8 @@ PullOverParameters BehaviorPathPlannerNode::getPullOverParam() // drivable area p.drivable_area_right_bound_offset = dp("drivable_area_right_bound_offset", 0.0); p.drivable_area_left_bound_offset = dp("drivable_area_left_bound_offset", 0.0); + p.drivable_area_types_to_skip = + dp("drivable_area_types_to_skip", std::vector({"road_border"})); // debug p.print_debug_info = dp("print_debug_info", false); @@ -648,6 +658,8 @@ PullOutParameters BehaviorPathPlannerNode::getPullOutParam() // drivable area p.drivable_area_right_bound_offset = dp("drivable_area_right_bound_offset", 0.0); p.drivable_area_left_bound_offset = dp("drivable_area_left_bound_offset", 0.0); + p.drivable_area_types_to_skip = + dp("drivable_area_types_to_skip", std::vector({"road_border"})); // validation of parameters if (p.pull_out_sampling_num < 1) { diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index 1ef1a124c2919..2d8cd0da7f0fb 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -2619,7 +2619,7 @@ void AvoidanceModule::generateExtendedDrivableArea(PathWithLaneId & path) const const auto extended_lanes = util::expandLanelets( shorten_lanes, parameters_->drivable_area_left_bound_offset, - parameters_->drivable_area_right_bound_offset, {"road_border"}); + parameters_->drivable_area_right_bound_offset, parameters_->drivable_area_types_to_skip); { const auto & p = planner_data_->parameters; diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp index c78da5e8e1769..fe07e46e2e562 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp @@ -324,7 +324,7 @@ PathWithLaneId LaneChangeModule::getReferencePath() const const auto shorten_lanes = util::cutOverlappedLanes(reference_path, drivable_lanes); const auto expanded_lanes = util::expandLanelets( shorten_lanes, parameters_->drivable_area_left_bound_offset, - parameters_->drivable_area_right_bound_offset); + parameters_->drivable_area_right_bound_offset, parameters_->drivable_area_types_to_skip); util::generateDrivableArea( reference_path, expanded_lanes, common_parameters.vehicle_length, planner_data_); @@ -661,7 +661,7 @@ void LaneChangeModule::generateExtendedDrivableArea(PathWithLaneId & path) const auto shorten_lanes = util::cutOverlappedLanes(path, drivable_lanes); const auto expanded_lanes = util::expandLanelets( shorten_lanes, parameters_->drivable_area_left_bound_offset, - parameters_->drivable_area_right_bound_offset); + parameters_->drivable_area_right_bound_offset, parameters_->drivable_area_types_to_skip); util::generateDrivableArea(path, expanded_lanes, common_parameters.vehicle_length, planner_data_); } diff --git a/planning/behavior_path_planner/src/scene_module/lane_following/lane_following_module.cpp b/planning/behavior_path_planner/src/scene_module/lane_following/lane_following_module.cpp index 6d958cfc9810c..449a3ce887889 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_following/lane_following_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_following/lane_following_module.cpp @@ -140,7 +140,7 @@ PathWithLaneId LaneFollowingModule::getReferencePath() const const auto expanded_lanes = util::expandLanelets( shorten_lanes, parameters_.drivable_area_left_bound_offset, - parameters_.drivable_area_right_bound_offset); + parameters_.drivable_area_right_bound_offset, parameters_.drivable_area_types_to_skip); util::generateDrivableArea(reference_path, expanded_lanes, p.vehicle_length, planner_data_); diff --git a/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp b/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp index 79c1a84fafce3..df56a0c7913ef 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp @@ -192,7 +192,7 @@ BehaviorModuleOutput PullOutModule::plan() const auto shorten_lanes = util::cutOverlappedLanes(path, status_.lanes); const auto expanded_lanes = util::expandLanelets( shorten_lanes, parameters_.drivable_area_left_bound_offset, - parameters_.drivable_area_right_bound_offset); + parameters_.drivable_area_right_bound_offset, parameters_.drivable_area_types_to_skip); util::generateDrivableArea( path, expanded_lanes, planner_data_->parameters.vehicle_length, planner_data_); @@ -287,7 +287,7 @@ BehaviorModuleOutput PullOutModule::planWaitingApproval() const auto expanded_lanes = util::expandLanelets( drivable_lanes, parameters_.drivable_area_left_bound_offset, - parameters_.drivable_area_right_bound_offset); + parameters_.drivable_area_right_bound_offset, parameters_.drivable_area_types_to_skip); auto candidate_path = status_.back_finished ? getCurrentPath() : status_.backward_path; util::generateDrivableArea( diff --git a/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp b/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp index b8ddf7c8e0727..7b2540946a1a3 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp @@ -488,7 +488,7 @@ BehaviorModuleOutput PullOverModule::plan() const auto shorten_lanes = util::cutOverlappedLanes(path, status_.lanes); const auto expanded_lanes = util::expandLanelets( shorten_lanes, parameters_.drivable_area_left_bound_offset, - parameters_.drivable_area_right_bound_offset); + parameters_.drivable_area_right_bound_offset, parameters_.drivable_area_types_to_skip); util::generateDrivableArea( path, expanded_lanes, planner_data_->parameters.vehicle_length, planner_data_); } @@ -682,7 +682,7 @@ PathWithLaneId PullOverModule::generateStopPath() const auto shorten_lanes = util::cutOverlappedLanes(reference_path, drivable_lanes); const auto expanded_lanes = util::expandLanelets( shorten_lanes, parameters_.drivable_area_left_bound_offset, - parameters_.drivable_area_right_bound_offset); + parameters_.drivable_area_right_bound_offset, parameters_.drivable_area_types_to_skip); util::generateDrivableArea( reference_path, expanded_lanes, common_parameters.vehicle_length, planner_data_); @@ -739,7 +739,7 @@ PathWithLaneId PullOverModule::generateEmergencyStopPath() const auto shorten_lanes = util::cutOverlappedLanes(stop_path, drivable_lanes); const auto expanded_lanes = util::expandLanelets( shorten_lanes, parameters_.drivable_area_left_bound_offset, - parameters_.drivable_area_right_bound_offset); + parameters_.drivable_area_right_bound_offset, parameters_.drivable_area_types_to_skip); util::generateDrivableArea( stop_path, expanded_lanes, common_parameters.vehicle_length, planner_data_); diff --git a/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp b/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp index c68934d06be7e..9d6e2e76aca4c 100644 --- a/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp @@ -374,7 +374,8 @@ void SideShiftModule::adjustDrivableArea(ShiftedPath * path) const const auto drivable_lanes = util::generateDrivableLanes(current_lanelets_); const auto shorten_lanes = util::cutOverlappedLanes(path->path, drivable_lanes); - const auto expanded_lanes = util::expandLanelets(shorten_lanes, left_offset, right_offset); + const auto expanded_lanes = util::expandLanelets( + shorten_lanes, left_offset, right_offset, parameters_.drivable_area_types_to_skip); { const auto & p = planner_data_->parameters; From 214912aa978dab10da5e0804de9ff0d16ced917c Mon Sep 17 00:00:00 2001 From: Shintaro Tomie <58775300+Shin-kyoto@users.noreply.github.com> Date: Thu, 12 Jan 2023 20:57:33 +0900 Subject: [PATCH 17/49] feat(elevation_map_loader): reduce memory usage of elevation_map_loader (#2571) * feat: reduce memory usage of elevation_map_loader Signed-off-by: Shin-kyoto <58775300+Shin-kyoto@users.noreply.github.com> * chore: remove unnecessary comment Signed-off-by: Shin-kyoto <58775300+Shin-kyoto@users.noreply.github.com> * fix: modify variables' name Signed-off-by: Shin-kyoto <58775300+Shin-kyoto@users.noreply.github.com> Signed-off-by: Shin-kyoto <58775300+Shin-kyoto@users.noreply.github.com> --- .../elevation_map_loader_node.hpp | 5 ++- .../src/elevation_map_loader_node.cpp | 37 ++++++++++--------- 2 files changed, 23 insertions(+), 19 deletions(-) diff --git a/perception/elevation_map_loader/include/elevation_map_loader/elevation_map_loader_node.hpp b/perception/elevation_map_loader/include/elevation_map_loader/elevation_map_loader_node.hpp index aa41c6d8c0b03..f5c294dd27207 100644 --- a/perception/elevation_map_loader/include/elevation_map_loader/elevation_map_loader_node.hpp +++ b/perception/elevation_map_loader/include/elevation_map_loader/elevation_map_loader_node.hpp @@ -73,7 +73,8 @@ class ElevationMapLoaderNode : public rclcpp::Node void publish(); void createElevationMap(); void setVerbosityLevelToDebugIfFlagSet(); - void createElevationMapFromPointcloud(); + void createElevationMapFromPointcloud( + const pcl::shared_ptr & grid_map_pcl_loader); tier4_autoware_utils::LinearRing2d getConvexHull( const pcl::PointCloud::Ptr & input_cloud); lanelet::ConstLanelets getIntersectedLanelets( @@ -97,7 +98,7 @@ class ElevationMapLoaderNode : public rclcpp::Node bool use_inpaint_; float inpaint_radius_; bool use_elevation_map_cloud_publisher_; - pcl::shared_ptr grid_map_pcl_loader_; + std::string param_file_path_; DataManager data_manager_; struct LaneFilter diff --git a/perception/elevation_map_loader/src/elevation_map_loader_node.cpp b/perception/elevation_map_loader/src/elevation_map_loader_node.cpp index 4474fe2740467..5434447a78c1f 100644 --- a/perception/elevation_map_loader/src/elevation_map_loader_node.cpp +++ b/perception/elevation_map_loader/src/elevation_map_loader_node.cpp @@ -50,7 +50,7 @@ ElevationMapLoaderNode::ElevationMapLoaderNode(const rclcpp::NodeOptions & optio : Node("elevation_map_loader", options) { layer_name_ = this->declare_parameter("map_layer_name", std::string("elevation")); - std::string param_file_path = this->declare_parameter("param_file_path", "path_default"); + param_file_path_ = this->declare_parameter("param_file_path", "path_default"); map_frame_ = this->declare_parameter("map_frame", "map"); use_inpaint_ = this->declare_parameter("use_inpaint", true); inpaint_radius_ = this->declare_parameter("inpaint_radius", 0.3); @@ -67,11 +67,6 @@ ElevationMapLoaderNode::ElevationMapLoaderNode(const rclcpp::NodeOptions & optio lane_filter_.voxel_size_y_ = declare_parameter("lane_filter_voxel_size_y", 0.04); lane_filter_.voxel_size_z_ = declare_parameter("lane_filter_voxel_size_z", 0.04); - auto grid_map_logger = rclcpp::get_logger("grid_map_logger"); - grid_map_logger.set_level(rclcpp::Logger::Level::Error); - grid_map_pcl_loader_ = pcl::make_shared(grid_map_logger); - grid_map_pcl_loader_->loadParameters(param_file_path); - rclcpp::QoS durable_qos{1}; durable_qos.transient_local(); pub_elevation_map_ = @@ -153,9 +148,11 @@ void ElevationMapLoaderNode::onPointcloudMap( const sensor_msgs::msg::PointCloud2::ConstSharedPtr pointcloud_map) { RCLCPP_INFO(this->get_logger(), "subscribe pointcloud_map"); - pcl::PointCloud map_pcl; - pcl::fromROSMsg(*pointcloud_map, map_pcl); - data_manager_.map_pcl_ptr_ = pcl::make_shared>(map_pcl); + { + pcl::PointCloud map_pcl; + pcl::fromROSMsg(*pointcloud_map, map_pcl); + data_manager_.map_pcl_ptr_ = pcl::make_shared>(map_pcl); + } if (data_manager_.isInitialized()) { publish(); } @@ -177,30 +174,36 @@ void ElevationMapLoaderNode::onVectorMap( void ElevationMapLoaderNode::createElevationMap() { + auto grid_map_logger = rclcpp::get_logger("grid_map_logger"); + grid_map_logger.set_level(rclcpp::Logger::Level::Error); + pcl::shared_ptr grid_map_pcl_loader = + pcl::make_shared(grid_map_logger); + grid_map_pcl_loader->loadParameters(param_file_path_); if (lane_filter_.use_lane_filter_) { const auto convex_hull = getConvexHull(data_manager_.map_pcl_ptr_); lanelet::ConstLanelets intersected_lanelets = getIntersectedLanelets(convex_hull, lane_filter_.road_lanelets_); pcl::PointCloud::Ptr lane_filtered_map_pcl_ptr = getLaneFilteredPointCloud(intersected_lanelets, data_manager_.map_pcl_ptr_); - grid_map_pcl_loader_->setInputCloud(lane_filtered_map_pcl_ptr); + grid_map_pcl_loader->setInputCloud(lane_filtered_map_pcl_ptr); } else { - grid_map_pcl_loader_->setInputCloud(data_manager_.map_pcl_ptr_); + grid_map_pcl_loader->setInputCloud(data_manager_.map_pcl_ptr_); } - createElevationMapFromPointcloud(); - elevation_map_ = grid_map_pcl_loader_->getGridMap(); + createElevationMapFromPointcloud(grid_map_pcl_loader); + elevation_map_ = grid_map_pcl_loader->getGridMap(); if (use_inpaint_) { inpaintElevationMap(inpaint_radius_); } saveElevationMap(); } -void ElevationMapLoaderNode::createElevationMapFromPointcloud() +void ElevationMapLoaderNode::createElevationMapFromPointcloud( + const pcl::shared_ptr & grid_map_pcl_loader) { const auto start = std::chrono::high_resolution_clock::now(); - grid_map_pcl_loader_->preProcessInputCloud(); - grid_map_pcl_loader_->initializeGridMapGeometryFromInputCloud(); - grid_map_pcl_loader_->addLayerFromInputCloud(layer_name_); + grid_map_pcl_loader->preProcessInputCloud(); + grid_map_pcl_loader->initializeGridMapGeometryFromInputCloud(); + grid_map_pcl_loader->addLayerFromInputCloud(layer_name_); grid_map::grid_map_pcl::printTimeElapsedToRosInfoStream( start, "Finish creating elevation map. Total time: ", this->get_logger()); } From b083510a2487fdd861a05af0ea976b653f4b7222 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Thu, 12 Jan 2023 22:40:49 +0900 Subject: [PATCH 18/49] feat(behavior_path_planner): visualize full pull out path as candidate (#2643) feat(behavior_path_planner): visualize full pull out path as candidate Signed-off-by: kosuke55 Signed-off-by: kosuke55 --- .../src/scene_module/pull_out/pull_out_module.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp b/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp index df56a0c7913ef..499f0526f0b9a 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp @@ -198,6 +198,7 @@ BehaviorModuleOutput PullOutModule::plan() output.path = std::make_shared(path); output.turn_signal_info = calcTurnSignalInfo(); + path_candidate_ = std::make_shared(getFullPath()); const uint16_t steering_factor_direction = std::invoke([&output]() { if (output.turn_signal_info.turn_signal.command == TurnIndicatorsCommand::ENABLE_LEFT) { @@ -289,17 +290,16 @@ BehaviorModuleOutput PullOutModule::planWaitingApproval() drivable_lanes, parameters_.drivable_area_left_bound_offset, parameters_.drivable_area_right_bound_offset, parameters_.drivable_area_types_to_skip); - auto candidate_path = status_.back_finished ? getCurrentPath() : status_.backward_path; + auto stop_path = status_.back_finished ? getCurrentPath() : status_.backward_path; util::generateDrivableArea( - candidate_path, expanded_lanes, planner_data_->parameters.vehicle_length, planner_data_); - auto stop_path = candidate_path; + stop_path, expanded_lanes, planner_data_->parameters.vehicle_length, planner_data_); for (auto & p : stop_path.points) { p.point.longitudinal_velocity_mps = 0.0; } output.path = std::make_shared(stop_path); output.turn_signal_info = calcTurnSignalInfo(); - path_candidate_ = std::make_shared(candidate_path); + path_candidate_ = std::make_shared(getFullPath()); const uint16_t steering_factor_direction = std::invoke([&output]() { if (output.turn_signal_info.turn_signal.command == TurnIndicatorsCommand::ENABLE_LEFT) { @@ -312,10 +312,10 @@ BehaviorModuleOutput PullOutModule::planWaitingApproval() if (status_.back_finished) { const double start_distance = motion_utils::calcSignedArcLength( - candidate_path.points, planner_data_->self_pose->pose.position, + stop_path.points, planner_data_->self_pose->pose.position, status_.pull_out_path.start_pose.position); const double finish_distance = motion_utils::calcSignedArcLength( - candidate_path.points, planner_data_->self_pose->pose.position, + stop_path.points, planner_data_->self_pose->pose.position, status_.pull_out_path.end_pose.position); updateRTCStatus(start_distance, finish_distance); steering_factor_interface_ptr_->updateSteeringFactor( @@ -324,7 +324,7 @@ BehaviorModuleOutput PullOutModule::planWaitingApproval() SteeringFactor::APPROACHING, ""); } else { const double distance = motion_utils::calcSignedArcLength( - candidate_path.points, planner_data_->self_pose->pose.position, + stop_path.points, planner_data_->self_pose->pose.position, status_.pull_out_path.start_pose.position); updateRTCStatus(0.0, distance); steering_factor_interface_ptr_->updateSteeringFactor( From a1dfc11fbc3d9e0b47d383de56103f33ddd18a87 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Fri, 13 Jan 2023 01:00:40 +0900 Subject: [PATCH 19/49] feat(behavior_path_planner): error messsage when no pull over/out planners (#2644) feat(behavior_path_planner): error messsage when no pull over/out planner Signed-off-by: kosuke55 Signed-off-by: kosuke55 --- .../src/scene_module/pull_out/pull_out_module.cpp | 2 +- .../src/scene_module/pull_over/pull_over_module.cpp | 3 +++ 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp b/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp index 499f0526f0b9a..eb16159f0ed4b 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp @@ -55,7 +55,7 @@ PullOutModule::PullOutModule( std::make_shared(node, parameters, getGeometricPullOutParameters())); } if (pull_out_planners_.empty()) { - RCLCPP_DEBUG(getLogger(), "Not found enabled planner"); + RCLCPP_ERROR(getLogger(), "Not found enabled planner"); } } diff --git a/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp b/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp index 7b2540946a1a3..8f4ffb9ada331 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp @@ -77,6 +77,9 @@ PullOverModule::PullOverModule( node, parameters, getGeometricPullOverParameters(), lane_departure_checker, occupancy_grid_map_, is_forward)); } + if (pull_over_planners_.empty()) { + RCLCPP_ERROR(getLogger(), "Not found enabled planner"); + } // set selected goal searcher // currently there is only one goal_searcher_type From 966ef94ee1fc8df5e9cb90d6a13d76db4dc1d48c Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Fri, 13 Jan 2023 01:01:19 +0900 Subject: [PATCH 20/49] feat(lane_departure_checker): check shoulder lane (#2614) Signed-off-by: kosuke55 Signed-off-by: kosuke55 --- .../lane_departure_checker/lane_departure_checker.hpp | 1 + .../lane_departure_checker_node.hpp | 1 + .../lane_departure_checker.cpp | 10 +++++++++- .../lane_departure_checker_node.cpp | 5 +++++ 4 files changed, 16 insertions(+), 1 deletion(-) diff --git a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp index a59df6340ad00..fd21d18b98a64 100644 --- a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp +++ b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp @@ -70,6 +70,7 @@ struct Input lanelet::LaneletMapPtr lanelet_map{}; LaneletRoute::ConstSharedPtr route{}; lanelet::ConstLanelets route_lanelets{}; + lanelet::ConstLanelets shoulder_lanelets{}; Trajectory::ConstSharedPtr reference_trajectory{}; Trajectory::ConstSharedPtr predicted_trajectory{}; }; diff --git a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker_node.hpp b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker_node.hpp index 8869825890b8b..53b5461652970 100644 --- a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker_node.hpp +++ b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker_node.hpp @@ -66,6 +66,7 @@ class LaneDepartureCheckerNode : public rclcpp::Node geometry_msgs::msg::PoseStamped::ConstSharedPtr current_pose_; nav_msgs::msg::Odometry::ConstSharedPtr current_odom_; lanelet::LaneletMapPtr lanelet_map_; + lanelet::ConstLanelets shoulder_lanelets_; lanelet::traffic_rules::TrafficRulesPtr traffic_rules_; lanelet::routing::RoutingGraphPtr routing_graph_; LaneletRoute::ConstSharedPtr route_; diff --git a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp index db20dd7c8b981..e3e753d9e4023 100644 --- a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp +++ b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp @@ -123,7 +123,15 @@ Output LaneDepartureChecker::update(const Input & input) output.vehicle_passing_areas = createVehiclePassingAreas(output.vehicle_footprints); output.processing_time_map["createVehiclePassingAreas"] = stop_watch.toc(true); - output.candidate_lanelets = getCandidateLanelets(input.route_lanelets, output.vehicle_footprints); + const auto candidate_road_lanelets = + getCandidateLanelets(input.route_lanelets, output.vehicle_footprints); + const auto candidate_shoulder_lanelets = + getCandidateLanelets(input.shoulder_lanelets, output.vehicle_footprints); + output.candidate_lanelets = candidate_road_lanelets; + output.candidate_lanelets.insert( + output.candidate_lanelets.end(), candidate_shoulder_lanelets.begin(), + candidate_shoulder_lanelets.end()); + output.processing_time_map["getCandidateLanelets"] = stop_watch.toc(true); output.will_leave_lane = willLeaveLane(output.candidate_lanelets, output.vehicle_footprints); diff --git a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp index d45fd2590a769..f0274e5c4712f 100644 --- a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp +++ b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp @@ -197,6 +197,10 @@ void LaneDepartureCheckerNode::onLaneletMapBin(const HADMapBin::ConstSharedPtr m { lanelet_map_ = std::make_shared(); lanelet::utils::conversion::fromBinMsg(*msg, lanelet_map_, &traffic_rules_, &routing_graph_); + + // get all shoulder lanes + lanelet::ConstLanelets all_lanelets = lanelet::utils::query::laneletLayer(lanelet_map_); + shoulder_lanelets_ = lanelet::utils::query::shoulderLanelets(all_lanelets); } void LaneDepartureCheckerNode::onRoute(const LaneletRoute::ConstSharedPtr msg) { route_ = msg; } @@ -313,6 +317,7 @@ void LaneDepartureCheckerNode::onTimer() input_.lanelet_map = lanelet_map_; input_.route = route_; input_.route_lanelets = route_lanelets_; + input_.shoulder_lanelets = shoulder_lanelets_; input_.reference_trajectory = reference_trajectory_; input_.predicted_trajectory = predicted_trajectory_; processing_time_map["Node: setInputData"] = stop_watch.toc(true); From d91f84ea181a75da8364ea752e06d0da4de26999 Mon Sep 17 00:00:00 2001 From: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com> Date: Fri, 13 Jan 2023 10:47:43 +0900 Subject: [PATCH 21/49] fix(obstacle_avoidance_planner): fix drivable area checker (#2639) * fix(obstacle_avoidance_planner): fix drivable area checker Signed-off-by: yutaka * fix format Signed-off-by: yutaka Signed-off-by: yutaka --- .../utils/utils.hpp | 4 +- .../src/utils/utils.cpp | 74 ++++++++++++++----- 2 files changed, 57 insertions(+), 21 deletions(-) diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/utils.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/utils.hpp index 1f1b3dbdf9249..fd8a8f88e054d 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/utils.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/utils.hpp @@ -358,8 +358,8 @@ namespace drivable_area_utils { bool isOutsideDrivableAreaFromRectangleFootprint( const autoware_auto_planning_msgs::msg::TrajectoryPoint & traj_point, - const std::vector left_bound, - const std::vector right_bound, const VehicleParam & vehicle_param); + const std::vector & left_bound, + const std::vector & right_bound, const VehicleParam & vehicle_param); } #endif // OBSTACLE_AVOIDANCE_PLANNER__UTILS__UTILS_HPP_ diff --git a/planning/obstacle_avoidance_planner/src/utils/utils.cpp b/planning/obstacle_avoidance_planner/src/utils/utils.cpp index ad7257e907047..bb74a3e57677c 100644 --- a/planning/obstacle_avoidance_planner/src/utils/utils.cpp +++ b/planning/obstacle_avoidance_planner/src/utils/utils.cpp @@ -32,6 +32,10 @@ #include #include +namespace bg = boost::geometry; +typedef bg::model::d2::point_xy bg_point; +typedef bg::model::polygon bg_polygon; + namespace { std::vector convertEulerAngleToMonotonic(const std::vector & angle) @@ -628,7 +632,7 @@ geometry_msgs::msg::Point getStartPoint( return bound.front(); } -bool isOutsideDrivableArea( +bool isFrontDrivableArea( const geometry_msgs::msg::Point & point, const std::vector & left_bound, const std::vector & right_bound) @@ -644,23 +648,33 @@ bool isOutsideDrivableArea( // ignore point behind of the front line const std::vector front_bound = {left_start_point, right_start_point}; const double lat_dist_to_front_bound = motion_utils::calcLateralOffset(front_bound, point); - if (lat_dist_to_front_bound < -min_dist) { - return false; + if (lat_dist_to_front_bound < min_dist) { + return true; } - // left bound check - const double lat_dist_to_left_bound = motion_utils::calcLateralOffset(left_bound, point); - if (lat_dist_to_left_bound > min_dist) { - return true; + return false; +} + +bg_polygon createDrivablePolygon( + const std::vector & left_bound, + const std::vector & right_bound) +{ + bg_polygon drivable_area_poly; + + // right bound + for (const auto rp : right_bound) { + drivable_area_poly.outer().push_back(bg_point(rp.x, rp.y)); } - // right bound check - const double lat_dist_to_right_bound = motion_utils::calcLateralOffset(right_bound, point); - if (lat_dist_to_right_bound < -min_dist) { - return true; + // left bound + auto reversed_left_bound = left_bound; + std::reverse(reversed_left_bound.begin(), reversed_left_bound.end()); + for (const auto lp : reversed_left_bound) { + drivable_area_poly.outer().push_back(bg_point(lp.x, lp.y)); } - return false; + drivable_area_poly.outer().push_back(drivable_area_poly.outer().front()); + return drivable_area_poly; } } // namespace @@ -668,8 +682,8 @@ namespace drivable_area_utils { bool isOutsideDrivableAreaFromRectangleFootprint( const autoware_auto_planning_msgs::msg::TrajectoryPoint & traj_point, - const std::vector left_bound, - const std::vector right_bound, const VehicleParam & vehicle_param) + const std::vector & left_bound, + const std::vector & right_bound, const VehicleParam & vehicle_param) { if (left_bound.empty() || right_bound.empty()) { return false; @@ -694,12 +708,34 @@ bool isOutsideDrivableAreaFromRectangleFootprint( tier4_autoware_utils::calcOffsetPose(traj_point.pose, -base_to_rear, -base_to_left, 0.0) .position; - const bool out_top_left = isOutsideDrivableArea(top_left_pos, left_bound, right_bound); - const bool out_top_right = isOutsideDrivableArea(top_right_pos, left_bound, right_bound); - const bool out_bottom_left = isOutsideDrivableArea(bottom_left_pos, left_bound, right_bound); - const bool out_bottom_right = isOutsideDrivableArea(bottom_right_pos, left_bound, right_bound); + const bool front_top_left = isFrontDrivableArea(top_left_pos, left_bound, right_bound); + const bool front_top_right = isFrontDrivableArea(top_right_pos, left_bound, right_bound); + const bool front_bottom_left = isFrontDrivableArea(bottom_left_pos, left_bound, right_bound); + const bool front_bottom_right = isFrontDrivableArea(bottom_right_pos, left_bound, right_bound); - if (out_top_left || out_top_right || out_bottom_left || out_bottom_right) { + // create rectangle + const auto drivable_area_poly = createDrivablePolygon(left_bound, right_bound); + + if ( + !front_top_left && !bg::within(bg_point(top_left_pos.x, top_left_pos.y), drivable_area_poly)) { + return true; + } + + if ( + !front_top_right && + !bg::within(bg_point(top_right_pos.x, top_right_pos.y), drivable_area_poly)) { + return true; + } + + if ( + !front_bottom_left && + !bg::within(bg_point(bottom_left_pos.x, bottom_left_pos.y), drivable_area_poly)) { + return true; + } + + if ( + !front_bottom_right && + !bg::within(bg_point(bottom_right_pos.x, bottom_right_pos.y), drivable_area_poly)) { return true; } From 1832adf5743c164b020d9af2ef00ac02a3400c97 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" <43976882+isamu-takagi@users.noreply.github.com> Date: Fri, 13 Jan 2023 12:43:24 +0900 Subject: [PATCH 22/49] feat: replace tier4_ad_api_adaptor with default_ad_api (#2438) * feat: add options Signed-off-by: Takagi, Isamu * fix: rosbridge max message size Signed-off-by: Takagi, Isamu * feat: remove old apis Signed-off-by: Takagi, Isamu * feat: remove api adaptors except rtc controller Signed-off-by: Takagi, Isamu * feat: remove external engage Signed-off-by: Takagi, Isamu * feat: add deprecated api option Signed-off-by: Takagi, Isamu * fix: path Signed-off-by: Takagi, Isamu Signed-off-by: Takagi, Isamu --- .../launch/joy_controller.launch.xml | 2 +- .../launch/autoware_api.launch.xml | 47 +++++----- .../launch/deprecated_api.launch.xml | 20 +++++ .../include/external_api_adaptor.launch.py | 86 ------------------- .../include/internal_api_adaptor.launch.py | 44 ---------- .../include/internal_api_relay.launch.xml | 20 ----- 6 files changed, 41 insertions(+), 178 deletions(-) create mode 100644 launch/tier4_autoware_api_launch/launch/deprecated_api.launch.xml delete mode 100644 launch/tier4_autoware_api_launch/launch/include/external_api_adaptor.launch.py delete mode 100644 launch/tier4_autoware_api_launch/launch/include/internal_api_adaptor.launch.py delete mode 100644 launch/tier4_autoware_api_launch/launch/include/internal_api_relay.launch.xml diff --git a/control/joy_controller/launch/joy_controller.launch.xml b/control/joy_controller/launch/joy_controller.launch.xml index b2d4035411b0c..f719d8bd78cb5 100644 --- a/control/joy_controller/launch/joy_controller.launch.xml +++ b/control/joy_controller/launch/joy_controller.launch.xml @@ -28,7 +28,7 @@ - + diff --git a/launch/tier4_autoware_api_launch/launch/autoware_api.launch.xml b/launch/tier4_autoware_api_launch/launch/autoware_api.launch.xml index efd9db8fdbb38..05e280277c7e7 100644 --- a/launch/tier4_autoware_api_launch/launch/autoware_api.launch.xml +++ b/launch/tier4_autoware_api_launch/launch/autoware_api.launch.xml @@ -1,45 +1,38 @@ - - + + - - - - - - - - + + + + - - - + + + - + - - - - - - - + + - + - - + + + + - - + - + + diff --git a/launch/tier4_autoware_api_launch/launch/deprecated_api.launch.xml b/launch/tier4_autoware_api_launch/launch/deprecated_api.launch.xml new file mode 100644 index 0000000000000..0856b9ca073cd --- /dev/null +++ b/launch/tier4_autoware_api_launch/launch/deprecated_api.launch.xml @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/launch/tier4_autoware_api_launch/launch/include/external_api_adaptor.launch.py b/launch/tier4_autoware_api_launch/launch/include/external_api_adaptor.launch.py deleted file mode 100644 index fc730ff19bf40..0000000000000 --- a/launch/tier4_autoware_api_launch/launch/include/external_api_adaptor.launch.py +++ /dev/null @@ -1,86 +0,0 @@ -# Copyright 2021 Tier IV, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -import launch -from launch.actions import DeclareLaunchArgument -from launch.conditions import IfCondition -from launch.substitutions import LaunchConfiguration -from launch_ros.actions import ComposableNodeContainer -from launch_ros.actions import LoadComposableNodes -from launch_ros.descriptions import ComposableNode - - -def _create_api_node(node_name, class_name, **kwargs): - return ComposableNode( - namespace="external", - name=node_name, - package="autoware_iv_external_api_adaptor", - plugin="external_api::" + class_name, - **kwargs - ) - - -def generate_launch_description(): - launch_arguments = [] - - def add_launch_arg(name: str, default_value=None, description=None): - launch_arguments.append( - DeclareLaunchArgument(name, default_value=default_value, description=description) - ) - - add_launch_arg("launch_calibration_status_api", None, "launch calibration status api") - add_launch_arg("launch_start_api", None, "launch start api") - - default_components = [ - _create_api_node("cpu_usage", "CpuUsage"), - _create_api_node("diagnostics", "Diagnostics"), - _create_api_node("door", "Door"), - _create_api_node("emergency", "Emergency"), - _create_api_node("engage", "Engage"), - _create_api_node("fail_safe_state", "FailSafeState"), - _create_api_node("initial_pose", "InitialPose"), - _create_api_node("map", "Map"), - _create_api_node("operator", "Operator"), - _create_api_node("metadata_packages", "MetadataPackages"), - _create_api_node("route", "Route"), - _create_api_node("rtc_controller", "RTCController"), - _create_api_node("service", "Service"), - _create_api_node("vehicle_status", "VehicleStatus"), - _create_api_node("velocity", "Velocity"), - _create_api_node("version", "Version"), - ] - container = ComposableNodeContainer( - namespace="external", - name="autoware_iv_adaptor", - package="rclcpp_components", - executable="component_container_mt", - composable_node_descriptions=default_components, - output="screen", - ) - - calibration_status_loader = LoadComposableNodes( - composable_node_descriptions=[_create_api_node("calibration_status", "CalibrationStatus")], - target_container=container, - condition=IfCondition(LaunchConfiguration("launch_calibration_status_api")), - ) - - start_loader = LoadComposableNodes( - composable_node_descriptions=[_create_api_node("start", "Start")], - target_container=container, - condition=IfCondition(LaunchConfiguration("launch_start_api")), - ) - - return launch.LaunchDescription( - launch_arguments + [container, calibration_status_loader, start_loader] - ) diff --git a/launch/tier4_autoware_api_launch/launch/include/internal_api_adaptor.launch.py b/launch/tier4_autoware_api_launch/launch/include/internal_api_adaptor.launch.py deleted file mode 100644 index f1962908c8749..0000000000000 --- a/launch/tier4_autoware_api_launch/launch/include/internal_api_adaptor.launch.py +++ /dev/null @@ -1,44 +0,0 @@ -# Copyright 2021 Tier IV, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -import launch -from launch_ros.actions import ComposableNodeContainer -from launch_ros.descriptions import ComposableNode - - -def _create_api_node(node_name, class_name, **kwargs): - return ComposableNode( - namespace="internal", - name=node_name, - package="autoware_iv_internal_api_adaptor", - plugin="internal_api::" + class_name, - **kwargs - ) - - -def generate_launch_description(): - components = [ - _create_api_node("iv_msgs", "IVMsgs"), - _create_api_node("operator", "Operator"), - _create_api_node("velocity", "Velocity"), - ] - container = ComposableNodeContainer( - namespace="internal", - name="autoware_iv_adaptor", - package="rclcpp_components", - executable="component_container_mt", - composable_node_descriptions=components, - output="screen", - ) - return launch.LaunchDescription([container]) diff --git a/launch/tier4_autoware_api_launch/launch/include/internal_api_relay.launch.xml b/launch/tier4_autoware_api_launch/launch/include/internal_api_relay.launch.xml deleted file mode 100644 index 27c62523ab6bd..0000000000000 --- a/launch/tier4_autoware_api_launch/launch/include/internal_api_relay.launch.xml +++ /dev/null @@ -1,20 +0,0 @@ - - - - - - - - - - From 5975cebe3dcf7f9a31904acb09f240b4245292fa Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Fri, 13 Jan 2023 12:57:01 +0900 Subject: [PATCH 23/49] fix(behavior_path_planner): lane change turn signal during approval (#2645) * fix(behavior_path_planner): lane change turn signal during approval Signed-off-by: Muhammad Zulfaqar Azmi * some refactoring Signed-off-by: Muhammad Zulfaqar Azmi Signed-off-by: Muhammad Zulfaqar Azmi --- .../lane_change/lane_change_module.hpp | 1 + .../scene_module/lane_change/util.hpp | 5 --- .../lane_change/lane_change_module.cpp | 44 +++++++++++++++++++ .../src/scene_module/lane_change/util.cpp | 40 +---------------- 4 files changed, 47 insertions(+), 43 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp index 0ce55c2796f08..1a2004937697a 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp @@ -189,6 +189,7 @@ class LaneChangeModule : public SceneModuleInterface void updateOutputTurnSignal(BehaviorModuleOutput & output); void updateSteeringFactorPtr(const BehaviorModuleOutput & output); bool isApprovedPathSafe(Pose & ego_pose_before_collision) const; + void calcTurnSignalInfo(); void updateSteeringFactorPtr( const CandidateOutput & output, const LaneChangePath & selected_path) const; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp index d9b6f0dad4929..d91a9373bb492 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp @@ -132,11 +132,6 @@ bool isEgoWithinOriginalLane( const lanelet::ConstLanelets & current_lanes, const Pose & current_pose, const BehaviorPathPlannerParameters & common_param); -TurnSignalInfo calc_turn_signal_info( - const PathWithLaneId & prepare_path, const double prepare_velocity, - const double min_prepare_distance, const double prepare_duration, const ShiftLine & shift_line, - const ShiftedPath & lane_changing_path); - void get_turn_signal_info( const LaneChangePath & lane_change_path, TurnSignalInfo * turn_signal_info); diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp index fe07e46e2e562..9a5522f378e7d 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp @@ -697,6 +697,7 @@ bool LaneChangeModule::isApprovedPathSafe(Pose & ego_pose_before_collision) cons void LaneChangeModule::updateOutputTurnSignal(BehaviorModuleOutput & output) { + calcTurnSignalInfo(); const auto turn_signal_info = util::getPathTurnSignal( status_.current_lanes, status_.lane_change_path.shifted_path, status_.lane_change_path.shift_line, getEgoPose(), getEgoTwist().linear.x, @@ -706,6 +707,49 @@ void LaneChangeModule::updateOutputTurnSignal(BehaviorModuleOutput & output) lane_change_utils::get_turn_signal_info(status_.lane_change_path, &output.turn_signal_info); } +void LaneChangeModule::calcTurnSignalInfo() +{ + const auto get_blinker_pose = + [this](const PathWithLaneId & path, const lanelet::ConstLanelets & lanes, const double length) { + const auto & points = path.points; + const auto arc_front = lanelet::utils::getArcCoordinates(lanes, points.front().point.pose); + for (const auto & point : points) { + const auto & pt = point.point.pose; + const auto arc_current = lanelet::utils::getArcCoordinates(lanes, pt); + const auto diff = arc_current.length - arc_front.length; + if (diff > length) { + return pt; + } + } + + RCLCPP_WARN(getLogger(), "unable to determine blinker pose..."); + return points.front().point.pose; + }; + + const auto & path = status_.lane_change_path; + TurnSignalInfo turn_signal_info{}; + + turn_signal_info.desired_start_point = std::invoke([&]() { + const auto blinker_start_duration = planner_data_->parameters.turn_signal_search_time; + const auto prepare_duration = parameters_->lane_change_prepare_duration; + const auto prepare_to_blinker_start_diff = prepare_duration - blinker_start_duration; + if (prepare_to_blinker_start_diff < 1e-5) { + return path.path.points.front().point.pose; + } + + return get_blinker_pose(path.path, path.reference_lanelets, prepare_to_blinker_start_diff); + }); + turn_signal_info.desired_end_point = path.shift_line.end; + + turn_signal_info.required_start_point = path.shift_line.start; + const auto mid_lane_change_length = path.lane_change_length / 2; + const auto & shifted_path = path.shifted_path.path; + turn_signal_info.required_end_point = + get_blinker_pose(shifted_path, path.target_lanelets, mid_lane_change_length); + + status_.lane_change_path.turn_signal_info = turn_signal_info; +} + void LaneChangeModule::resetParameters() { is_abort_path_approved_ = false; diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp index 1774dddc10d42..fc0305e9b402b 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp @@ -99,8 +99,7 @@ std::optional constructCandidatePath( const PathWithLaneId & prepare_segment, const PathWithLaneId & lane_changing_segment, const PathWithLaneId & target_lane_reference_path, const ShiftLine & shift_line, const lanelet::ConstLanelets & original_lanelets, const lanelet::ConstLanelets & target_lanelets, - const double acceleration, const double prepare_duration, const LaneChangePhaseInfo distance, - const LaneChangePhaseInfo speed, const BehaviorPathPlannerParameters & params, + const double acceleration, const LaneChangePhaseInfo distance, const LaneChangePhaseInfo speed, const LaneChangeParameters & lane_change_param) { PathShifter path_shifter; @@ -175,9 +174,6 @@ std::optional constructCandidatePath( return std::nullopt; } - candidate_path.turn_signal_info = lane_change_utils::calc_turn_signal_info( - prepare_segment, speed.prepare, params.minimum_lane_change_prepare_distance, prepare_duration, - shift_line, shifted_path); // check candidate path is in lanelet if (!isPathInLanelets(candidate_path.path, original_lanelets, target_lanelets)) { return std::nullopt; @@ -294,8 +290,7 @@ LaneChangePaths getLaneChangePaths( const auto lc_speed = LaneChangePhaseInfo{prepare_speed, lane_changing_speed}; const auto candidate_path = constructCandidatePath( prepare_segment_reference, lane_changing_segment_reference, target_lane_reference_path, - shift_line, original_lanelets, target_lanelets, acceleration, lane_change_prepare_duration, - lc_dist, lc_speed, common_parameter, parameter); + shift_line, original_lanelets, target_lanelets, acceleration, lc_dist, lc_speed, parameter); if (!candidate_path) { continue; @@ -710,37 +705,6 @@ bool isEgoWithinOriginalLane( lanelet::utils::to2D(lane_poly).basicPolygon()); } -TurnSignalInfo calc_turn_signal_info( - const PathWithLaneId & prepare_path, const double prepare_velocity, - const double min_prepare_distance, const double prepare_duration, const ShiftLine & shift_line, - const ShiftedPath & lane_changing_path) -{ - TurnSignalInfo turn_signal_info{}; - constexpr double turn_signal_start_duration{3.0}; - turn_signal_info.desired_start_point = - std::invoke([&prepare_path, &prepare_velocity, &min_prepare_distance, &prepare_duration]() { - if (prepare_velocity * turn_signal_start_duration > min_prepare_distance) { - const auto duration = static_cast(prepare_path.points.size()) / prepare_duration; - double time{-duration}; - for (auto itr = prepare_path.points.crbegin(); itr != prepare_path.points.crend(); ++itr) { - time += duration; - if (time >= turn_signal_start_duration) { - return itr->point.pose; - } - } - } - return prepare_path.points.front().point.pose; - }); - - turn_signal_info.required_start_point = shift_line.start; - turn_signal_info.required_end_point = std::invoke([&lane_changing_path]() { - const auto mid_path_idx = lane_changing_path.path.points.size() / 2; - return lane_changing_path.path.points.at(mid_path_idx).point.pose; - }); - turn_signal_info.desired_end_point = shift_line.end; - return turn_signal_info; -} - void get_turn_signal_info( const LaneChangePath & lane_change_path, TurnSignalInfo * turn_signal_info) { From 833fe1f4d5219dfe6b9bf80f398f93336492a31f Mon Sep 17 00:00:00 2001 From: kminoda <44218668+kminoda@users.noreply.github.com> Date: Fri, 13 Jan 2023 13:57:21 +0900 Subject: [PATCH 24/49] feat(vehicle_velocity_converter): add `speed_scale_factor` (#2641) * feat(vehicle_velocity_converter): add speed_scale_factor Signed-off-by: kminoda * add parameter description in readme * ci(pre-commit): autofix Signed-off-by: kminoda Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- sensing/vehicle_velocity_converter/README.md | 11 ++++++----- .../config/vehicle_velocity_converter.param.yaml | 1 + .../vehicle_velocity_converter.hpp | 1 + .../src/vehicle_velocity_converter.cpp | 3 ++- 4 files changed, 10 insertions(+), 6 deletions(-) diff --git a/sensing/vehicle_velocity_converter/README.md b/sensing/vehicle_velocity_converter/README.md index 7554457d37149..92881f9321f28 100644 --- a/sensing/vehicle_velocity_converter/README.md +++ b/sensing/vehicle_velocity_converter/README.md @@ -20,8 +20,9 @@ This package converts autoware_auto_vehicle_msgs::msg::VehicleReport message to ## Parameters -| Name | Type | Description | -| ---------------------------- | ------ | ------------------------------- | -| `frame_id` | string | frame id for output message | -| `velocity_stddev_xx` | double | standard deviation for vx | -| `angular_velocity_stddev_zz` | double | standard deviation for yaw rate | +| Name | Type | Description | +| ---------------------------- | ------ | --------------------------------------- | +| `speed_scale_factor` | double | speed scale factor (ideal value is 1.0) | +| `frame_id` | string | frame id for output message | +| `velocity_stddev_xx` | double | standard deviation for vx | +| `angular_velocity_stddev_zz` | double | standard deviation for yaw rate | diff --git a/sensing/vehicle_velocity_converter/config/vehicle_velocity_converter.param.yaml b/sensing/vehicle_velocity_converter/config/vehicle_velocity_converter.param.yaml index 7dd04823453c5..649296ff6ec0a 100644 --- a/sensing/vehicle_velocity_converter/config/vehicle_velocity_converter.param.yaml +++ b/sensing/vehicle_velocity_converter/config/vehicle_velocity_converter.param.yaml @@ -1,5 +1,6 @@ /**: ros__parameters: + speed_scale_factor: 1.0 # [.] velocity_stddev_xx: 0.2 # [m/s] angular_velocity_stddev_zz: 0.1 # [rad/s] frame_id: base_link diff --git a/sensing/vehicle_velocity_converter/include/vehicle_velocity_converter/vehicle_velocity_converter.hpp b/sensing/vehicle_velocity_converter/include/vehicle_velocity_converter/vehicle_velocity_converter.hpp index bad4d4bc06326..660ad330f07f3 100644 --- a/sensing/vehicle_velocity_converter/include/vehicle_velocity_converter/vehicle_velocity_converter.hpp +++ b/sensing/vehicle_velocity_converter/include/vehicle_velocity_converter/vehicle_velocity_converter.hpp @@ -43,6 +43,7 @@ class VehicleVelocityConverter : public rclcpp::Node std::string frame_id_; double stddev_vx_; double stddev_wz_; + double speed_scale_factor_; std::array twist_covariance_; }; diff --git a/sensing/vehicle_velocity_converter/src/vehicle_velocity_converter.cpp b/sensing/vehicle_velocity_converter/src/vehicle_velocity_converter.cpp index 844a17df4715b..997e06e8c01b0 100644 --- a/sensing/vehicle_velocity_converter/src/vehicle_velocity_converter.cpp +++ b/sensing/vehicle_velocity_converter/src/vehicle_velocity_converter.cpp @@ -20,6 +20,7 @@ VehicleVelocityConverter::VehicleVelocityConverter() : Node("vehicle_velocity_co stddev_vx_ = declare_parameter("velocity_stddev_xx", 0.2); stddev_wz_ = declare_parameter("angular_velocity_stddev_zz", 0.1); frame_id_ = declare_parameter("frame_id", "base_link"); + speed_scale_factor_ = declare_parameter("speed_scale_factor", 1.0); vehicle_report_sub_ = create_subscription( "velocity_status", rclcpp::QoS{100}, @@ -39,7 +40,7 @@ void VehicleVelocityConverter::callbackVelocityReport( // set twist with covariance msg from vehicle report msg geometry_msgs::msg::TwistWithCovarianceStamped twist_with_covariance_msg; twist_with_covariance_msg.header = msg->header; - twist_with_covariance_msg.twist.twist.linear.x = msg->longitudinal_velocity; + twist_with_covariance_msg.twist.twist.linear.x = msg->longitudinal_velocity * speed_scale_factor_; twist_with_covariance_msg.twist.twist.linear.y = msg->lateral_velocity; twist_with_covariance_msg.twist.twist.angular.z = msg->heading_rate; twist_with_covariance_msg.twist.covariance[0 + 0 * 6] = stddev_vx_ * stddev_vx_; From 333e1c579f4d3a0472c692423e33442bc3791f2e Mon Sep 17 00:00:00 2001 From: Takeshi Ishita Date: Fri, 13 Jan 2023 15:15:49 +0900 Subject: [PATCH 25/49] refactor(ekf_localizer): add a function that warns if delay time < 0 (#2642) Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../ekf_localizer/src/ekf_localizer.cpp | 18 ++++++++++++------ 1 file changed, 12 insertions(+), 6 deletions(-) diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp index 528fbdc5344e0..49af18eadf6bb 100644 --- a/localization/ekf_localizer/src/ekf_localizer.cpp +++ b/localization/ekf_localizer/src/ekf_localizer.cpp @@ -395,6 +395,15 @@ void EKFLocalizer::initEKF() ekf_.init(X, P, params_.extend_state_step); } +void warnIfPoseDelayTimeLessThanZero(const Warning & warning, const double delay_time) +{ + if (delay_time < 0.0) { + const auto s = + fmt::format("Pose time stamp is inappropriate, set delay to 0[s]. delay = {}", delay_time); + warning.warnThrottle(s, 1000); + } +} + /* * measurementUpdatePose */ @@ -415,12 +424,9 @@ void EKFLocalizer::measurementUpdatePose(const geometry_msgs::msg::PoseWithCovar /* Calculate delay step */ double delay_time = (t_curr - pose.header.stamp).seconds() + params_.pose_additional_delay; - if (delay_time < 0.0) { - delay_time = 0.0; - warning_.warnThrottle( - fmt::format("Pose time stamp is inappropriate, set delay to 0[s]. delay = %f", delay_time), - 1000); - } + warnIfPoseDelayTimeLessThanZero(warning_, delay_time); + delay_time = std::max(delay_time, 0.0); + int delay_step = std::roundf(delay_time / ekf_dt_); if (delay_step > params_.extend_state_step - 1) { warning_.warnThrottle( From 64285f31c9f54a6d03e9901fd30c05f91400a13e Mon Sep 17 00:00:00 2001 From: Mingyu1991 <115005477+Mingyu1991@users.noreply.github.com> Date: Sat, 14 Jan 2023 00:51:57 +0900 Subject: [PATCH 26/49] remove pedestrain traffic lights which should not be extracted from map (#2574) Co-authored-by: Shunsuke Miura <37187849+miursh@users.noreply.github.com> --- perception/traffic_light_map_based_detector/src/node.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/perception/traffic_light_map_based_detector/src/node.cpp b/perception/traffic_light_map_based_detector/src/node.cpp index 5071adcab7e86..d2d54098859c9 100644 --- a/perception/traffic_light_map_based_detector/src/node.cpp +++ b/perception/traffic_light_map_based_detector/src/node.cpp @@ -331,6 +331,12 @@ void MapBasedDetector::getVisibleTrafficLights( std::vector & visible_traffic_lights) { for (const auto & traffic_light : all_traffic_lights) { + // some "Traffic Light" are actually not traffic lights + if ( + traffic_light.hasAttribute("subtype") == false || + traffic_light.attribute("subtype").value() == "solid") { + continue; + } const auto & tl_left_down_point = traffic_light.front(); const auto & tl_right_down_point = traffic_light.back(); const double tl_height = traffic_light.attributeOr("height", 0.0); From 1d39c6ccd65f2b3ffa7c0652892dd9196ffda5eb Mon Sep 17 00:00:00 2001 From: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com> Date: Sat, 14 Jan 2023 12:34:38 +0900 Subject: [PATCH 27/49] fix(behavior_path_planner): fix overlapping lane cutter (#2648) --- .../lane_following/lane_following_module.cpp | 29 +++++++++++-- .../behavior_path_planner/src/utilities.cpp | 42 ++++++++++--------- 2 files changed, 49 insertions(+), 22 deletions(-) diff --git a/planning/behavior_path_planner/src/scene_module/lane_following/lane_following_module.cpp b/planning/behavior_path_planner/src/scene_module/lane_following/lane_following_module.cpp index 449a3ce887889..67fc8c3ef7a0e 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_following/lane_following_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_following/lane_following_module.cpp @@ -74,6 +74,29 @@ void LaneFollowingModule::setParameters(const LaneFollowingParameters & paramete parameters_ = parameters; } +lanelet::ConstLanelets getLaneletsFromPath( + const PathWithLaneId & path, const std::shared_ptr & route_handler) +{ + std::vector unique_lanelet_ids; + for (const auto & p : path.points) { + const auto & lane_ids = p.lane_ids; + for (const auto & lane_id : lane_ids) { + if ( + std::find(unique_lanelet_ids.begin(), unique_lanelet_ids.end(), lane_id) == + unique_lanelet_ids.end()) { + unique_lanelet_ids.push_back(lane_id); + } + } + } + + lanelet::ConstLanelets lanelets; + for (const auto & lane_id : unique_lanelet_ids) { + lanelets.push_back(route_handler->getLaneletsFromId(lane_id)); + } + + return lanelets; +} + PathWithLaneId LaneFollowingModule::getReferencePath() const { PathWithLaneId reference_path{}; @@ -95,7 +118,6 @@ PathWithLaneId LaneFollowingModule::getReferencePath() const // For current_lanes with desired length const auto current_lanes = planner_data_->route_handler->getLaneletSequence( current_lane, current_pose, p.backward_path_length, p.forward_path_length); - const auto drivable_lanes = util::generateDrivableLanes(current_lanes); if (current_lanes.empty()) { return reference_path; @@ -103,8 +125,7 @@ PathWithLaneId LaneFollowingModule::getReferencePath() const // calculate path with backward margin to avoid end points' instability by spline interpolation constexpr double extra_margin = 10.0; - const double backward_length = - std::max(p.backward_path_length, p.backward_path_length + extra_margin); + const double backward_length = p.backward_path_length + extra_margin; const auto current_lanes_with_backward_margin = util::calcLaneAroundPose(route_handler, current_pose, p.forward_path_length, backward_length); reference_path = util::getCenterLinePath( @@ -115,6 +136,8 @@ PathWithLaneId LaneFollowingModule::getReferencePath() const const size_t current_seg_idx = findEgoSegmentIndex(reference_path.points); util::clipPathLength( reference_path, current_seg_idx, p.forward_path_length, p.backward_path_length); + const auto drivable_lanelets = getLaneletsFromPath(reference_path, route_handler); + const auto drivable_lanes = util::generateDrivableLanes(drivable_lanelets); { const int num_lane_change = diff --git a/planning/behavior_path_planner/src/utilities.cpp b/planning/behavior_path_planner/src/utilities.cpp index f4842edf7edd5..defdf6b6d51b1 100644 --- a/planning/behavior_path_planner/src/utilities.cpp +++ b/planning/behavior_path_planner/src/utilities.cpp @@ -1077,6 +1077,10 @@ boost::optional getOverlappedLaneletId(const std::vector } } + if (overlapped_idx == lanes.size()) { + return {}; + } + return overlapped_idx; } @@ -1090,30 +1094,30 @@ std::vector cutOverlappedLanes( const std::vector shorten_lanes{ lanes.begin(), lanes.begin() + *overlapped_lanelet_id}; - const std::vector removed_lanes{ - lanes.begin() + *overlapped_lanelet_id, lanes.end()}; - - const auto transformed_lanes = util::transformToLanelets(removed_lanes); - - auto isIncluded = [&transformed_lanes](const std::vector & lane_ids) { - if (transformed_lanes.empty() || lane_ids.empty()) return false; - - for (const auto & transformed_lane : transformed_lanes) { - for (const auto & lane_id : lane_ids) { - if (lane_id == transformed_lane.id()) { - return true; - } + const auto shorten_lanelets = util::transformToLanelets(shorten_lanes); + + // create removed lanelets + std::vector removed_lane_ids; + for (size_t i = *overlapped_lanelet_id; i < lanes.size(); ++i) { + const auto target_lanelets = util::transformToLanelets(lanes.at(i)); + for (const auto & target_lanelet : target_lanelets) { + // if target lane is inside of the shorten lanelets, we do not remove it + if (checkHasSameLane(shorten_lanelets, target_lanelet)) { + continue; } + removed_lane_ids.push_back(target_lanelet.id()); } - - return false; - }; + } for (size_t i = 0; i < path.points.size(); ++i) { const auto & lane_ids = path.points.at(i).lane_ids; - if (isIncluded(lane_ids)) { - path.points.erase(path.points.begin() + i, path.points.end()); - break; + for (const auto & lane_id : lane_ids) { + if ( + std::find(removed_lane_ids.begin(), removed_lane_ids.end(), lane_id) != + removed_lane_ids.end()) { + path.points.erase(path.points.begin() + i, path.points.end()); + break; + } } } From 2eafda6d4af8bd5c044d94cef68781fafe34968d Mon Sep 17 00:00:00 2001 From: Keisuke KATO Date: Mon, 16 Jan 2023 15:53:33 +0900 Subject: [PATCH 28/49] docs(behavior_path_planner): fix typos (#2655) fix(behavior_path_planner): fix typos Signed-off-by: kasecato Signed-off-by: kasecato --- .../behavior_path_planner_avoidance-design.md | 2 +- .../avoidance_design/target_vehicle_selection.drawio.svg | 8 ++++---- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/planning/behavior_path_planner/behavior_path_planner_avoidance-design.md b/planning/behavior_path_planner/behavior_path_planner_avoidance-design.md index 3a2bc1a0cd1c0..4143c7dfdc986 100644 --- a/planning/behavior_path_planner/behavior_path_planner_avoidance-design.md +++ b/planning/behavior_path_planner/behavior_path_planner_avoidance-design.md @@ -275,7 +275,7 @@ distance from overhang_pose to left most linestring end note else(\n no) -partition getrightMostLineString() { +partition getRightMostLineString() { repeat repeat :getRightLanelet; diff --git a/planning/behavior_path_planner/image/avoidance_design/target_vehicle_selection.drawio.svg b/planning/behavior_path_planner/image/avoidance_design/target_vehicle_selection.drawio.svg index bdcab83b107b9..451473931ac98 100644 --- a/planning/behavior_path_planner/image/avoidance_design/target_vehicle_selection.drawio.svg +++ b/planning/behavior_path_planner/image/avoidance_design/target_vehicle_selection.drawio.svg @@ -134,13 +134,13 @@ - object_check_forward_distance + object_check_backward_distance - object_check_forward_distance + object_check_backward_distance @@ -155,13 +155,13 @@ - object_check_backward_distance + object_check_forward_distance - object_check_backward_distance + object_check_forward_distance From 11c0c8ddd497e46517ddf5ae041cbc388521b84b Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Tue, 17 Jan 2023 18:38:10 +0900 Subject: [PATCH 29/49] feat(behavior_path_planner): ignore pull over goal near lane start (#2667) feat(behavior_path_planner) ignore pull over goal near lane start Signed-off-by: kosuke55 Signed-off-by: kosuke55 --- planning/behavior_path_planner/README.md | 21 ++++++++++--------- .../config/pull_over/pull_over.param.yaml | 1 + .../pull_over/pull_over_parameters.hpp | 1 + .../src/behavior_path_planner_node.cpp | 1 + .../scene_module/pull_over/goal_searcher.cpp | 8 +++++++ 5 files changed, 22 insertions(+), 10 deletions(-) diff --git a/planning/behavior_path_planner/README.md b/planning/behavior_path_planner/README.md index a9d04d307fdd2..520d61f864f64 100644 --- a/planning/behavior_path_planner/README.md +++ b/planning/behavior_path_planner/README.md @@ -253,16 +253,17 @@ searched for in certain range of the shoulder lane. ##### Parameters for goal search -| Name | Unit | Type | Description | Default value | -| :-------------------------- | :--- | :----- | :----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------- | -| search_priority | - | string | In case `efficient_path` use a goal that can generate an efficient path( priority is `shift_parking` -> `arc_forward_parking` -> `arc_backward_parking`). In case `close_goal` use the closest goal to the original one. | efficient_path | -| enable_goal_research | - | double | flag whether to search goal | true | -| forward_goal_search_length | [m] | double | length of forward range to be explored from the original goal | 20.0 | -| backward_goal_search_length | [m] | double | length of backward range to be explored from the original goal | 20.0 | -| goal_search_interval | [m] | double | distance interval for goal search | 2.0 | -| longitudinal_margin | [m] | double | margin between ego-vehicle at the goal position and obstacles | 3.0 | -| max_lateral_offset | [m] | double | maximum offset of goal search in the lateral direction | 3.0 | -| lateral_offset_interval | [m] | double | distance interval of goal search in the lateral direction | 3.0 | +| Name | Unit | Type | Description | Default value | +| :------------------------------ | :--- | :----- | :----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------- | +| search_priority | - | string | In case `efficient_path` use a goal that can generate an efficient path( priority is `shift_parking` -> `arc_forward_parking` -> `arc_backward_parking`). In case `close_goal` use the closest goal to the original one. | efficient_path | +| enable_goal_research | - | double | flag whether to search goal | true | +| forward_goal_search_length | [m] | double | length of forward range to be explored from the original goal | 20.0 | +| backward_goal_search_length | [m] | double | length of backward range to be explored from the original goal | 20.0 | +| goal_search_interval | [m] | double | distance interval for goal search | 2.0 | +| longitudinal_margin | [m] | double | margin between ego-vehicle at the goal position and obstacles | 3.0 | +| max_lateral_offset | [m] | double | maximum offset of goal search in the lateral direction | 3.0 | +| lateral_offset_interval | [m] | double | distance interval of goal search in the lateral direction | 3.0 | +| ignore_distance_from_lane_start | [m] | double | distance from start of pull over lanes for ignoring goal candidates | 15.0 | #### **Path Generation** diff --git a/planning/behavior_path_planner/config/pull_over/pull_over.param.yaml b/planning/behavior_path_planner/config/pull_over/pull_over.param.yaml index 75b5facaaaafe..5a3d4fb771381 100644 --- a/planning/behavior_path_planner/config/pull_over/pull_over.param.yaml +++ b/planning/behavior_path_planner/config/pull_over/pull_over.param.yaml @@ -19,6 +19,7 @@ longitudinal_margin: 3.0 max_lateral_offset: 1.0 lateral_offset_interval: 0.25 + ignore_distance_from_lane_start: 15.0 # occupancy grid map use_occupancy_grid: true use_occupancy_grid_for_longitudinal_margin: false diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_parameters.hpp index 5f43cd152832b..c47e0eefc5409 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_parameters.hpp @@ -39,6 +39,7 @@ struct PullOverParameters double longitudinal_margin; double max_lateral_offset; double lateral_offset_interval; + double ignore_distance_from_lane_start; // occupancy grid map bool use_occupancy_grid; bool use_occupancy_grid_for_longitudinal_margin; diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index bd6fe13254a5d..3820021e8a241 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -548,6 +548,7 @@ PullOverParameters BehaviorPathPlannerNode::getPullOverParam() p.longitudinal_margin = dp("longitudinal_margin", 3.0); p.max_lateral_offset = dp("max_lateral_offset", 1.0); p.lateral_offset_interval = dp("lateral_offset_interval", 0.25); + p.ignore_distance_from_lane_start = dp("ignore_distance_from_lane_start", 15.0); // occupancy grid map p.use_occupancy_grid = dp("use_occupancy_grid", true); p.use_occupancy_grid_for_longitudinal_margin = diff --git a/planning/behavior_path_planner/src/scene_module/pull_over/goal_searcher.cpp b/planning/behavior_path_planner/src/scene_module/pull_over/goal_searcher.cpp index f8ef93edc55a5..fa1beb8ddee9e 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_over/goal_searcher.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_over/goal_searcher.cpp @@ -49,6 +49,7 @@ GoalCandidates GoalSearcher::search(const Pose & original_goal_pose) const double margin_from_boundary = parameters_.margin_from_boundary; const double lateral_offset_interval = parameters_.lateral_offset_interval; const double max_lateral_offset = parameters_.max_lateral_offset; + const double ignore_distance_from_lane_start = parameters_.ignore_distance_from_lane_start; const auto pull_over_lanes = pull_over_utils::getPullOverLanes(*route_handler); auto lanes = util::getExtendedCurrentLanes(planner_data_); @@ -70,6 +71,13 @@ GoalCandidates GoalSearcher::search(const Pose & original_goal_pose) for (const auto & p : center_line_path.points) { const Pose & center_pose = p.point.pose; + // ignore goal_pose near lane start + const double distance_from_lane_start = + lanelet::utils::getArcCoordinates(pull_over_lanes, center_pose).length; + if (distance_from_lane_start < ignore_distance_from_lane_start) { + continue; + } + const auto distance_from_left_bound = util::getSignedDistanceFromShoulderLeftBoundary( pull_over_lanes, vehicle_footprint_, center_pose); if (!distance_from_left_bound) continue; From 19bba848b29e611131b8734b22f91446f62beb9d Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Tue, 17 Jan 2023 21:01:49 +0900 Subject: [PATCH 30/49] feat(behavior_path_planner): add option for combining arc pull out paths (#2669) * feat(behavior_path_planner): add option for combining arc pull out paths Signed-off-by: kosuke55 * divide_pull_out_path Signed-off-by: kosuke55 Signed-off-by: kosuke55 --- .../config/pull_out/pull_out.param.yaml | 1 + .../scene_module/pull_out/pull_out_parameters.hpp | 1 + .../src/behavior_path_planner_node.cpp | 1 + .../src/scene_module/pull_out/geometric_pull_out.cpp | 12 +++++++++++- 4 files changed, 14 insertions(+), 1 deletion(-) diff --git a/planning/behavior_path_planner/config/pull_out/pull_out.param.yaml b/planning/behavior_path_planner/config/pull_out/pull_out.param.yaml index b092eb7fc8802..bde6daa42d231 100644 --- a/planning/behavior_path_planner/config/pull_out/pull_out.param.yaml +++ b/planning/behavior_path_planner/config/pull_out/pull_out.param.yaml @@ -16,6 +16,7 @@ deceleration_interval: 15.0 # geometric pull out enable_geometric_pull_out: true + divide_pull_out_path: false geometric_pull_out_velocity: 1.0 arc_path_interval: 1.0 lane_departure_margin: 0.2 diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_parameters.hpp index c206b39c5fdf6..fca7d5d2bfc14 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_parameters.hpp @@ -38,6 +38,7 @@ struct PullOutParameters double deceleration_interval; // geometric pull out bool enable_geometric_pull_out; + bool divide_pull_out_path; double geometric_pull_out_velocity; double arc_path_interval; double lane_departure_margin; diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 3820021e8a241..242fd1fd09af9 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -644,6 +644,7 @@ PullOutParameters BehaviorPathPlannerNode::getPullOutParam() p.deceleration_interval = dp("deceleration_interval", 10.0); // geometric pull out p.enable_geometric_pull_out = dp("enable_geometric_pull_out", true); + p.divide_pull_out_path = dp("divide_pull_out_path", false); p.geometric_pull_out_velocity = dp("geometric_pull_out_velocity", 1.0); p.arc_path_interval = dp("arc_path_interval", 1.0); p.lane_departure_margin = dp("lane_departure_margin", 0.2); diff --git a/planning/behavior_path_planner/src/scene_module/pull_out/geometric_pull_out.cpp b/planning/behavior_path_planner/src/scene_module/pull_out/geometric_pull_out.cpp index a080eefdf64f7..2316ac35c2ef5 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_out/geometric_pull_out.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_out/geometric_pull_out.cpp @@ -59,7 +59,17 @@ boost::optional GeometricPullOut::plan(Pose start_pose, Pose goal_p return {}; } - output.partial_paths = planner_.getPaths(); + if (parameters_.divide_pull_out_path) { + output.partial_paths = planner_.getPaths(); + } else { + const auto partial_paths = planner_.getPaths(); + auto combined_path = combineReferencePath(partial_paths.at(0), partial_paths.at(1)); + // set same velocity to all points not to stop + for (auto & point : combined_path.points) { + point.point.longitudinal_velocity_mps = parameters_.geometric_pull_out_velocity; + } + output.partial_paths.push_back(combined_path); + } output.start_pose = planner_.getArcPaths().at(0).points.back().point.pose; output.end_pose = planner_.getArcPaths().at(1).points.back().point.pose; From baefe27a961ff8528c6ffeaef52d369e19d29479 Mon Sep 17 00:00:00 2001 From: melike tanrikulu <41450930+meliketanrikulu@users.noreply.github.com> Date: Tue, 17 Jan 2023 15:09:24 +0300 Subject: [PATCH 31/49] fix(ndt_scan_matcher): check first old_pose_msg for initialization (#2660) * fix(ndt_scan_matcher): check first old_pose_msg for initialization Signed-off-by: melike tanrikulu * fix(ndt_scan_matcher): check with old_pose_time_stamp Signed-off-by: melike tanrikulu Signed-off-by: melike tanrikulu --- localization/ndt_scan_matcher/src/util_func.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/localization/ndt_scan_matcher/src/util_func.cpp b/localization/ndt_scan_matcher/src/util_func.cpp index 237b85ad75e10..2db6b51540d51 100644 --- a/localization/ndt_scan_matcher/src/util_func.cpp +++ b/localization/ndt_scan_matcher/src/util_func.cpp @@ -129,7 +129,8 @@ void get_nearest_timestamp_pose( const rclcpp::Time pose_time_stamp = output_new_pose_cov_msg_ptr->header.stamp; if (pose_time_stamp > time_stamp) { // TODO(Tier IV): refactor - if (pose_time_stamp.seconds() == 0.0) { + const rclcpp::Time old_pose_time_stamp = output_old_pose_cov_msg_ptr->header.stamp; + if (old_pose_time_stamp.seconds() == 0.0) { output_old_pose_cov_msg_ptr = output_new_pose_cov_msg_ptr; } break; From 0bfa943b6d4aa77cd515bf46eb9012901c1f76db Mon Sep 17 00:00:00 2001 From: Tomoya Kimura Date: Tue, 17 Jan 2023 21:52:11 +0900 Subject: [PATCH 32/49] fix(lane_change): fix lane change force approval by rtc (#2672) Signed-off-by: tomoya.kimura Signed-off-by: tomoya.kimura --- .../scene_module/lane_change/lane_change_module.hpp | 1 + .../scene_module/lane_change/lane_change_module_data.hpp | 1 + .../src/scene_module/lane_change/lane_change_module.cpp | 8 ++++++-- 3 files changed, 8 insertions(+), 2 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp index 1a2004937697a..aff6ec098368a 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp @@ -194,6 +194,7 @@ class LaneChangeModule : public SceneModuleInterface void updateSteeringFactorPtr( const CandidateOutput & output, const LaneChangePath & selected_path) const; bool isSafe() const; + bool isValidPath() const; bool isValidPath(const PathWithLaneId & path) const; bool isNearEndOfLane() const; bool isCurrentSpeedLow() const; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module_data.hpp index b9597fd9de8c2..77a7f6925a01f 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module_data.hpp @@ -68,6 +68,7 @@ struct LaneChangeStatus std::vector lane_follow_lane_ids; std::vector lane_change_lane_ids; bool is_safe; + bool is_valid_path = true; double start_distance; }; diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp index 9a5522f378e7d..8fc29f554e5a4 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp @@ -111,7 +111,7 @@ bool LaneChangeModule::isExecutionReady() const BT::NodeStatus LaneChangeModule::updateState() { RCLCPP_DEBUG(getLogger(), "LANE_CHANGE updateState"); - if (!isSafe()) { + if (!isValidPath()) { current_state_ = BT::NodeStatus::SUCCESS; return current_state_; } @@ -148,8 +148,10 @@ BehaviorModuleOutput LaneChangeModule::plan() PathWithLaneId path = status_.lane_change_path.path; if (!isValidPath(path)) { - status_.is_safe = false; + status_.is_valid_path = false; return BehaviorModuleOutput{}; + } else { + status_.is_valid_path = true; } if ((is_abort_condition_satisfied_ && isNearEndOfLane() && isCurrentSpeedLow())) { @@ -421,6 +423,8 @@ std::pair LaneChangeModule::getSafePath( bool LaneChangeModule::isSafe() const { return status_.is_safe; } +bool LaneChangeModule::isValidPath() const { return status_.is_valid_path; } + bool LaneChangeModule::isValidPath(const PathWithLaneId & path) const { const auto & route_handler = planner_data_->route_handler; From 0ca53ae4c6c2e667a9d0dd30dcd7f808b1895c49 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Tue, 17 Jan 2023 22:07:09 +0900 Subject: [PATCH 33/49] fix(behavior_path_planner): pull_over checks lane departure only for parking part (#2673) Signed-off-by: kosuke55 Signed-off-by: kosuke55 --- .../pull_over/shift_pull_over.cpp | 22 +++++++++---------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/planning/behavior_path_planner/src/scene_module/pull_over/shift_pull_over.cpp b/planning/behavior_path_planner/src/scene_module/pull_over/shift_pull_over.cpp index d322a9613c6cf..731b818666347 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_over/shift_pull_over.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_over/shift_pull_over.cpp @@ -167,17 +167,6 @@ boost::optional ShiftPullOver::generatePullOverPath( p.point.pose.position.z = goal_pose.position.z; } - // check lane departure with road and shoulder lanes - const auto drivable_lanes = - util::generateDrivableLanesWithShoulderLanes(road_lanes, shoulder_lanes); - const auto expanded_lanes = util::expandLanelets( - drivable_lanes, parameters_.drivable_area_left_bound_offset, - parameters_.drivable_area_right_bound_offset); - if (lane_departure_checker_.checkPathWillLeaveLane( - util::transformToLanelets(expanded_lanes), shifted_path.path)) { - return {}; - } - // set lane_id and velocity to shifted_path for (size_t i = path_shifter.getShiftLines().front().start_idx; i < shifted_path.path.points.size() - 1; ++i) { @@ -212,6 +201,17 @@ boost::optional ShiftPullOver::generatePullOverPath( pull_over_path.debug_poses.push_back(shift_end_pose_road_lane); pull_over_path.debug_poses.push_back(actual_shift_end_pose); + // check if the parking path will leave lanes + const auto drivable_lanes = + util::generateDrivableLanesWithShoulderLanes(road_lanes, shoulder_lanes); + const auto expanded_lanes = util::expandLanelets( + drivable_lanes, parameters_.drivable_area_left_bound_offset, + parameters_.drivable_area_right_bound_offset); + if (lane_departure_checker_.checkPathWillLeaveLane( + util::transformToLanelets(expanded_lanes), pull_over_path.getParkingPath())) { + return {}; + } + return pull_over_path; } From 6651892d412b74dcce1668a3967b5c39922e372d Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Wed, 18 Jan 2023 10:47:12 +0900 Subject: [PATCH 34/49] refactor(tier4_planning_launch): organize arguments (#2666) * refactor(tier4_planning_launch): organize arguments Signed-off-by: Takayuki Murooka * update Signed-off-by: Takayuki Murooka Signed-off-by: Takayuki Murooka --- .../launch/planning.launch.xml | 5 +++- .../scenario_planning/lane_driving.launch.xml | 3 ++ .../behavior_planning.launch.py | 29 ++++++++++--------- .../motion_planning/motion_planning.launch.py | 28 ++++++------------ .../scenario_planning.launch.xml | 3 ++ .../config/behavior_path_planner.param.yaml | 2 ++ 6 files changed, 37 insertions(+), 33 deletions(-) diff --git a/launch/tier4_planning_launch/launch/planning.launch.xml b/launch/tier4_planning_launch/launch/planning.launch.xml index e628c120a0e36..2b86e7a300aa3 100644 --- a/launch/tier4_planning_launch/launch/planning.launch.xml +++ b/launch/tier4_planning_launch/launch/planning.launch.xml @@ -8,7 +8,7 @@ - + @@ -65,6 +65,9 @@ + + + diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving.launch.xml index ffe3a32b6a1af..9a19cb686ad4d 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving.launch.xml @@ -26,6 +26,9 @@ + + + diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py index 620610a629fdb..c5950ff84ff68 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py @@ -86,11 +86,19 @@ def launch_setup(context, *args, **kwargs): behavior_path_planner_param, vehicle_param, { - "bt_tree_config_path": [ - FindPackageShare("behavior_path_planner"), - "/config/behavior_path_planner_tree.xml", - ], - "planning_hz": 10.0, + "lane_change.enable_abort_lane_change": LaunchConfiguration( + "use_experimental_lane_change_function" + ), + "lane_change.enable_collision_check_at_prepare_phase": LaunchConfiguration( + "use_experimental_lane_change_function" + ), + "lane_change.use_predicted_path_outside_lanelet": LaunchConfiguration( + "use_experimental_lane_change_function" + ), + "lane_change.use_all_predicted_path": LaunchConfiguration( + "use_experimental_lane_change_function" + ), + "bt_tree_config_path": LaunchConfiguration("behavior_path_planner_tree_param_path"), }, ], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], @@ -280,15 +288,10 @@ def add_launch_arg(name: str, default_value=None, description=None): DeclareLaunchArgument(name, default_value=default_value, description=description) ) - add_launch_arg( - "vehicle_param_file", - [ - FindPackageShare("vehicle_info_util"), - "/config/vehicle_info.param.yaml", - ], - "path to the parameter file of vehicle information", - ) + # vehicle parameter + add_launch_arg("vehicle_param_file") + # interface parameter add_launch_arg( "input_route_topic_name", "/planning/mission_planning/route", "input topic of route" ) diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py index e7e1292f1d2c1..de8104af44576 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py @@ -24,7 +24,6 @@ from launch_ros.actions import ComposableNodeContainer from launch_ros.actions import LoadComposableNodes from launch_ros.descriptions import ComposableNode -from launch_ros.substitutions import FindPackageShare import yaml @@ -61,8 +60,6 @@ def launch_setup(context, *args, **kwargs): nearest_search_param, obstacle_avoidance_planner_param, vehicle_info_param, - {"is_showing_debug_info": False}, - {"is_stopping_if_outside_drivable_area": True}, ], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) @@ -220,19 +217,19 @@ def launch_setup(context, *args, **kwargs): obstacle_stop_planner_loader = LoadComposableNodes( composable_node_descriptions=[obstacle_stop_planner_component], target_container=container, - condition=LaunchConfigurationEquals("cruise_planner", "obstacle_stop_planner"), + condition=LaunchConfigurationEquals("cruise_planner_type", "obstacle_stop_planner"), ) obstacle_cruise_planner_loader = LoadComposableNodes( composable_node_descriptions=[obstacle_cruise_planner_component], target_container=container, - condition=LaunchConfigurationEquals("cruise_planner", "obstacle_cruise_planner"), + condition=LaunchConfigurationEquals("cruise_planner_type", "obstacle_cruise_planner"), ) obstacle_cruise_planner_relay_loader = LoadComposableNodes( composable_node_descriptions=[obstacle_cruise_planner_relay_component], target_container=container, - condition=LaunchConfigurationEquals("cruise_planner", "none"), + condition=LaunchConfigurationEquals("cruise_planner_type", "none"), ) surround_obstacle_checker_loader = LoadComposableNodes( @@ -261,27 +258,20 @@ def add_launch_arg(name: str, default_value=None, description=None): DeclareLaunchArgument(name, default_value=default_value, description=description) ) - # vehicle information parameter file - add_launch_arg( - "vehicle_param_file", - [ - FindPackageShare("vehicle_info_util"), - "/config/vehicle_info.param.yaml", - ], - "path to the parameter file of vehicle information", - ) + # vehicle parameter + add_launch_arg("vehicle_param_file") - # obstacle_avoidance_planner + # interface parameter add_launch_arg( "input_path_topic", "/planning/scenario_planning/lane_driving/behavior_planning/path", "input path topic of obstacle_avoidance_planner", ) - # surround obstacle checker - add_launch_arg("use_surround_obstacle_check", "true", "launch surround_obstacle_checker or not") + # package parameter + add_launch_arg("use_surround_obstacle_check") # launch surround_obstacle_checker or not add_launch_arg( - "cruise_planner", "obstacle_stop_planner", "cruise planner type" + "cruise_planner_type" ) # select from "obstacle_stop_planner", "obstacle_cruise_planner", "none" add_launch_arg("use_intra_process", "false", "use ROS2 component container communication") diff --git a/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml index 103c2172ec7f0..657792f7db01f 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml @@ -45,6 +45,9 @@ + + + diff --git a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml b/planning/behavior_path_planner/config/behavior_path_planner.param.yaml index df742e7fd4142..1df3df2bbbcbf 100644 --- a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml +++ b/planning/behavior_path_planner/config/behavior_path_planner.param.yaml @@ -1,5 +1,7 @@ /**: ros__parameters: + planning_hz: 10.0 + backward_path_length: 5.0 forward_path_length: 200.0 backward_length_buffer_for_end_of_pull_over: 5.0 From 5bef74f65e2aae599462a8db84e0f76b42aebf23 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Wed, 18 Jan 2023 11:56:59 +0900 Subject: [PATCH 35/49] fix(tier4_planning_launch): make use_experimental_lane_change_function available (#2676) Signed-off-by: Takayuki Murooka Signed-off-by: Takayuki Murooka --- launch/tier4_planning_launch/launch/planning.launch.xml | 1 + .../launch/scenario_planning/lane_driving.launch.xml | 2 +- .../lane_driving/behavior_planning/behavior_planning.launch.py | 3 +++ 3 files changed, 5 insertions(+), 1 deletion(-) diff --git a/launch/tier4_planning_launch/launch/planning.launch.xml b/launch/tier4_planning_launch/launch/planning.launch.xml index 2b86e7a300aa3..81d4b89a99e3f 100644 --- a/launch/tier4_planning_launch/launch/planning.launch.xml +++ b/launch/tier4_planning_launch/launch/planning.launch.xml @@ -9,6 +9,7 @@ + diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving.launch.xml index 9a19cb686ad4d..24a33b9d3f3df 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving.launch.xml @@ -11,6 +11,7 @@ + @@ -28,7 +29,6 @@ - diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py index c5950ff84ff68..93b8c4197645a 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py @@ -297,6 +297,9 @@ def add_launch_arg(name: str, default_value=None, description=None): ) add_launch_arg("map_topic_name", "/map/vector_map", "input topic of map") + # package parameter + add_launch_arg("use_experimental_lane_change_function") + # component add_launch_arg("use_intra_process", "false", "use ROS2 component container communication") add_launch_arg("use_multithread", "false", "use multithread") From 8cb82b39f1f696a417b95b1856c9576ac85af0dd Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Wed, 18 Jan 2023 16:02:52 +0900 Subject: [PATCH 36/49] feat(behavior_path_planner): ignore pull out start near lane end (#2675) * feat(behavior_path_planner): ignore pull out start near lane end Signed-off-by: kosuke55 * update warn message Signed-off-by: kosuke55 Signed-off-by: kosuke55 --- planning/behavior_path_planner/README.md | 1 + .../config/pull_out/pull_out.param.yaml | 1 + .../pull_out/pull_out_parameters.hpp | 1 + .../src/behavior_path_planner_node.cpp | 1 + .../scene_module/pull_out/pull_out_module.cpp | 47 ++++++++++++------- 5 files changed, 33 insertions(+), 18 deletions(-) diff --git a/planning/behavior_path_planner/README.md b/planning/behavior_path_planner/README.md index 520d61f864f64..fb3d3a7e21b3d 100644 --- a/planning/behavior_path_planner/README.md +++ b/planning/behavior_path_planner/README.md @@ -453,6 +453,7 @@ If a safe path cannot be generated from the current position, search backwards f | max_back_distance | [m] | double | maximum back distance | 15.0 | | backward_search_resolution | [m] | double | distance interval for searching backward pull out start point | 2.0 | | backward_path_update_duration | [s] | double | time interval for searching backward pull out start point. this prevents chattering between back driving and pull_out | 3.0 | +| ignore_distance_from_lane_end | [m] | double | distance from end of pull out lanes for ignoring start candidates | 15.0 | #### Unimplemented parts / limitations for pull put diff --git a/planning/behavior_path_planner/config/pull_out/pull_out.param.yaml b/planning/behavior_path_planner/config/pull_out/pull_out.param.yaml index bde6daa42d231..27c2b0c9b8bdc 100644 --- a/planning/behavior_path_planner/config/pull_out/pull_out.param.yaml +++ b/planning/behavior_path_planner/config/pull_out/pull_out.param.yaml @@ -28,3 +28,4 @@ max_back_distance: 15.0 backward_search_resolution: 2.0 backward_path_update_duration: 3.0 + ignore_distance_from_lane_end: 15.0 diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_parameters.hpp index fca7d5d2bfc14..82fcb271d1b22 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_parameters.hpp @@ -50,6 +50,7 @@ struct PullOutParameters double max_back_distance; double backward_search_resolution; double backward_path_update_duration; + double ignore_distance_from_lane_end; // drivable area expansion double drivable_area_right_bound_offset; double drivable_area_left_bound_offset; diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 242fd1fd09af9..85196a21a2718 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -657,6 +657,7 @@ PullOutParameters BehaviorPathPlannerNode::getPullOutParam() p.max_back_distance = dp("max_back_distance", 15.0); p.backward_search_resolution = dp("backward_search_resolution", 2.0); p.backward_path_update_duration = dp("backward_path_update_duration", 3.0); + p.ignore_distance_from_lane_end = dp("ignore_distance_from_lane_end", 15.0); // drivable area p.drivable_area_right_bound_offset = dp("drivable_area_right_bound_offset", 0.0); p.drivable_area_left_bound_offset = dp("drivable_area_left_bound_offset", 0.0); diff --git a/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp b/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp index eb16159f0ed4b..225e842cda2e6 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp @@ -174,7 +174,8 @@ BehaviorModuleOutput PullOutModule::plan() BehaviorModuleOutput output; if (!status_.is_safe) { - RCLCPP_INFO(getLogger(), "not found safe path"); + RCLCPP_WARN_THROTTLE( + getLogger(), *clock_, 5000, "Not found safe pull out path, publish stop path"); return output; } @@ -376,12 +377,13 @@ void PullOutModule::planWithPriorityOnEfficientPath( // plan with each planner for (const auto & planner : pull_out_planners_) { - for (size_t i = 0; i < start_pose_candidates.size(); ++i) { - // pull out start pose is current_pose - if (i == 0) { - status_.back_finished = true; - } - const auto & pull_out_start_pose = start_pose_candidates.at(i); + for (const Pose & pull_out_start_pose : start_pose_candidates) { + // check pull_out_start_pose is current_pose + const double distance_from_current_pose = tier4_autoware_utils::calcDistance2d( + pull_out_start_pose.position, planner_data_->self_pose->pose.position); + constexpr double epsilon = 0.1; + status_.back_finished = distance_from_current_pose < epsilon; + planner->setPlannerData(planner_data_); const auto pull_out_path = planner->plan(pull_out_start_pose, goal_pose); if (pull_out_path) { // found safe path @@ -391,8 +393,6 @@ void PullOutModule::planWithPriorityOnEfficientPath( status_.planner_type = planner->getPlannerType(); break; } - // pull out start pose is not current_pose(index > 0), so need back. - status_.back_finished = false; } if (status_.is_safe) { break; @@ -406,12 +406,12 @@ void PullOutModule::planWithPriorityOnShortBackDistance( status_.is_safe = false; status_.planner_type = PlannerType::NONE; - for (size_t i = 0; i < start_pose_candidates.size(); ++i) { - // pull out start pose is current_pose - if (i == 0) { - status_.back_finished = true; - } - const auto & pull_out_start_pose = start_pose_candidates.at(i); + for (const Pose & pull_out_start_pose : start_pose_candidates) { + // check pull_out_start_pose is current_pose + const double distance_from_current_pose = tier4_autoware_utils::calcDistance2d( + pull_out_start_pose.position, planner_data_->self_pose->pose.position); + constexpr double epsilon = 0.1; + status_.back_finished = distance_from_current_pose < epsilon; // plan with each planner for (const auto & planner : pull_out_planners_) { planner->setPlannerData(planner_data_); @@ -427,8 +427,6 @@ void PullOutModule::planWithPriorityOnShortBackDistance( if (status_.is_safe) { break; } - // pull out start pose is not current_pose(index > 0), so need back. - status_.back_finished = false; } } @@ -518,7 +516,7 @@ void PullOutModule::updatePullOutStatus() // make this class? std::vector PullOutModule::searchBackedPoses() { - const auto current_pose = planner_data_->self_pose->pose; + const Pose & current_pose = planner_data_->self_pose->pose; // get backward shoulder path const auto arc_position_pose = @@ -545,6 +543,19 @@ std::vector PullOutModule::searchBackedPoses() continue; } + // check the back pose is near the lane end + const double length_to_backed_pose = + lanelet::utils::getArcCoordinates(status_.pull_out_lanes, *backed_pose).length; + const double length_to_lane_end = + lanelet::utils::getLaneletLength2d(status_.pull_out_lanes.back()); + const double distance_from_lane_end = length_to_lane_end - length_to_backed_pose; + if (distance_from_lane_end < parameters_.ignore_distance_from_lane_end) { + RCLCPP_WARN_THROTTLE( + getLogger(), *clock_, 5000, + "the ego is too close to the lane end, so needs backward driving"); + continue; + } + if (util::checkCollisionBetweenFootprintAndObjects( local_vehicle_footprint, *backed_pose, *(planner_data_->dynamic_object), parameters_.collision_check_margin)) { From ef5173491f8712b89ff529fb9576d9fe9b701b3b Mon Sep 17 00:00:00 2001 From: kminoda <44218668+kminoda@users.noreply.github.com> Date: Wed, 18 Jan 2023 16:56:34 +0900 Subject: [PATCH 37/49] fix(map_loader): apply clang-tidy (#2668) * fix(map_loader): apply clang-tidy Signed-off-by: kminoda * ci(pre-commit): autofix Signed-off-by: kminoda Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../differential_map_loader_module.cpp | 6 +++--- .../differential_map_loader_module.hpp | 4 ++-- .../pointcloud_map_loader/partial_map_loader_module.cpp | 4 ++-- .../pointcloud_map_loader/partial_map_loader_module.hpp | 4 ++-- .../pointcloud_map_loader_module.cpp | 9 ++++----- .../pointcloud_map_loader_module.hpp | 2 +- 6 files changed, 14 insertions(+), 15 deletions(-) diff --git a/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp b/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp index b42b919edb59d..8322ac5f60b99 100644 --- a/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp +++ b/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp @@ -26,7 +26,7 @@ DifferentialMapLoaderModule::DifferentialMapLoaderModule( } void DifferentialMapLoaderModule::differentialAreaLoad( - const autoware_map_msgs::msg::AreaInfo area, const std::vector & cached_ids, + const autoware_map_msgs::msg::AreaInfo & area, const std::vector & cached_ids, GetDifferentialPointCloudMap::Response::SharedPtr & response) const { // iterate over all the available pcd map grids @@ -52,7 +52,7 @@ void DifferentialMapLoaderModule::differentialAreaLoad( } } - for (int i = 0; i < static_cast(cached_ids.size()); ++i) { + for (size_t i = 0; i < cached_ids.size(); ++i) { if (should_remove[i]) { response->ids_to_remove.push_back(cached_ids[i]); } @@ -72,7 +72,7 @@ bool DifferentialMapLoaderModule::onServiceGetDifferentialPointCloudMap( autoware_map_msgs::msg::PointCloudMapCellWithID DifferentialMapLoaderModule::loadPointCloudMapCellWithID( - const std::string path, const std::string map_id) const + const std::string & path, const std::string & map_id) const { sensor_msgs::msg::PointCloud2 pcd; if (pcl::io::loadPCDFile(path, pcd) == -1) { diff --git a/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.hpp b/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.hpp index 5d6188c0b1a1f..7069e1dbdf45c 100644 --- a/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.hpp +++ b/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.hpp @@ -50,10 +50,10 @@ class DifferentialMapLoaderModule GetDifferentialPointCloudMap::Request::SharedPtr req, GetDifferentialPointCloudMap::Response::SharedPtr res); void differentialAreaLoad( - const autoware_map_msgs::msg::AreaInfo area_info, const std::vector & cached_ids, + const autoware_map_msgs::msg::AreaInfo & area_info, const std::vector & cached_ids, GetDifferentialPointCloudMap::Response::SharedPtr & response) const; autoware_map_msgs::msg::PointCloudMapCellWithID loadPointCloudMapCellWithID( - const std::string path, const std::string map_id) const; + const std::string & path, const std::string & map_id) const; }; #endif // POINTCLOUD_MAP_LOADER__DIFFERENTIAL_MAP_LOADER_MODULE_HPP_ diff --git a/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.cpp b/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.cpp index af53438e8e321..1454b01147ac2 100644 --- a/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.cpp +++ b/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.cpp @@ -25,7 +25,7 @@ PartialMapLoaderModule::PartialMapLoaderModule( } void PartialMapLoaderModule::partialAreaLoad( - const autoware_map_msgs::msg::AreaInfo area, + const autoware_map_msgs::msg::AreaInfo & area, GetPartialPointCloudMap::Response::SharedPtr & response) const { // iterate over all the available pcd map grids @@ -56,7 +56,7 @@ bool PartialMapLoaderModule::onServiceGetPartialPointCloudMap( } autoware_map_msgs::msg::PointCloudMapCellWithID PartialMapLoaderModule::loadPointCloudMapCellWithID( - const std::string path, const std::string map_id) const + const std::string & path, const std::string & map_id) const { sensor_msgs::msg::PointCloud2 pcd; if (pcl::io::loadPCDFile(path, pcd) == -1) { diff --git a/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.hpp b/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.hpp index 052aa4245c099..4d97ab90667ec 100644 --- a/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.hpp +++ b/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.hpp @@ -50,10 +50,10 @@ class PartialMapLoaderModule GetPartialPointCloudMap::Request::SharedPtr req, GetPartialPointCloudMap::Response::SharedPtr res); void partialAreaLoad( - const autoware_map_msgs::msg::AreaInfo area, + const autoware_map_msgs::msg::AreaInfo & area, GetPartialPointCloudMap::Response::SharedPtr & response) const; autoware_map_msgs::msg::PointCloudMapCellWithID loadPointCloudMapCellWithID( - const std::string path, const std::string map_id) const; + const std::string & path, const std::string & map_id) const; }; #endif // POINTCLOUD_MAP_LOADER__PARTIAL_MAP_LOADER_MODULE_HPP_ diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.cpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.cpp index cd1a75fad5a03..1c67d25f08f7a 100644 --- a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.cpp +++ b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.cpp @@ -40,8 +40,8 @@ sensor_msgs::msg::PointCloud2 downsample( } PointcloudMapLoaderModule::PointcloudMapLoaderModule( - rclcpp::Node * node, const std::vector & pcd_paths, const std::string publisher_name, - const bool use_downsample) + rclcpp::Node * node, const std::vector & pcd_paths, + const std::string & publisher_name, const bool use_downsample) : logger_(node->get_logger()) { rclcpp::QoS durable_qos{1}; @@ -72,12 +72,11 @@ sensor_msgs::msg::PointCloud2 PointcloudMapLoaderModule::loadPCDFiles( sensor_msgs::msg::PointCloud2 whole_pcd; sensor_msgs::msg::PointCloud2 partial_pcd; - for (int i = 0; i < static_cast(pcd_paths.size()); ++i) { + for (size_t i = 0; i < pcd_paths.size(); ++i) { auto & path = pcd_paths[i]; if (i % 50 == 0) { RCLCPP_INFO_STREAM( - logger_, - fmt::format("Load {} ({} out of {})", path, i + 1, static_cast(pcd_paths.size()))); + logger_, fmt::format("Load {} ({} out of {})", path, i + 1, pcd_paths.size())); } if (pcl::io::loadPCDFile(path, partial_pcd) == -1) { diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.hpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.hpp index 4cd0e981597c6..aebc5a17454ab 100644 --- a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.hpp +++ b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.hpp @@ -29,7 +29,7 @@ class PointcloudMapLoaderModule public: explicit PointcloudMapLoaderModule( rclcpp::Node * node, const std::vector & pcd_paths, - const std::string publisher_name, const bool use_downsample); + const std::string & publisher_name, const bool use_downsample); private: rclcpp::Logger logger_; From f54c350821ca05600dce6e6b0fe0e1dbbd1126c0 Mon Sep 17 00:00:00 2001 From: Takeshi Ishita Date: Wed, 18 Jan 2023 17:20:47 +0900 Subject: [PATCH 38/49] refactor(ekf_localizer): remove info structs (#2656) * Remove smoothing_steps from the pose/twist structs * Remove the PoseInfo and TwistInfo structs Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../include/ekf_localizer/ekf_localizer.hpp | 24 +++---- .../ekf_localizer/src/ekf_localizer.cpp | 67 +++++++++++-------- 2 files changed, 47 insertions(+), 44 deletions(-) diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp index 85f7aa8b1829f..8ebcc8b7e379f 100644 --- a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp @@ -47,20 +47,6 @@ #include #include -struct PoseInfo -{ - geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr pose; - int counter; - int smoothing_steps; -}; - -struct TwistInfo -{ - geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr twist; - int counter; - int smoothing_steps; -}; - class Simple1DFilter { public: @@ -181,8 +167,14 @@ class EKFLocalizer : public rclcpp::Node bool is_activated_; /* for model prediction */ - std::queue current_twist_info_queue_; //!< @brief current measured pose - std::queue current_pose_info_queue_; //!< @brief current measured pose + std::queue + current_twist_queue_; //!< @brief current measured twist + std::queue current_twist_count_queue_; + + std::queue + current_pose_queue_; //!< @brief current measured pose + std::queue current_pose_count_queue_; + geometry_msgs::msg::PoseStamped current_ekf_pose_; //!< @brief current estimated pose geometry_msgs::msg::PoseStamped current_biased_ekf_pose_; //!< @brief current estimated pose without yaw bias correction diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp index 49af18eadf6bb..e12e6dc8d9bb9 100644 --- a/localization/ekf_localizer/src/ekf_localizer.cpp +++ b/localization/ekf_localizer/src/ekf_localizer.cpp @@ -171,18 +171,22 @@ void EKFLocalizer::timerCallback() DEBUG_INFO(get_logger(), "------------------------- end prediction -------------------------\n"); /* pose measurement update */ - if (!current_pose_info_queue_.empty()) { + if (!current_pose_queue_.empty()) { DEBUG_INFO(get_logger(), "------------------------- start Pose -------------------------"); stop_watch_.tic(); - int pose_info_queue_size = static_cast(current_pose_info_queue_.size()); - for (int i = 0; i < pose_info_queue_size; ++i) { - PoseInfo pose_info = current_pose_info_queue_.front(); - current_pose_info_queue_.pop(); - measurementUpdatePose(*pose_info.pose); - ++pose_info.counter; - if (pose_info.counter < pose_info.smoothing_steps) { - current_pose_info_queue_.push(pose_info); + for (size_t i = 0; i < current_pose_queue_.size(); ++i) { + const auto pose = current_pose_queue_.front(); + current_pose_queue_.pop(); + + const int count = current_pose_count_queue_.front(); + current_pose_count_queue_.pop(); + + measurementUpdatePose(*pose); + + if (count + 1 < params_.pose_smoothing_steps) { + current_pose_queue_.push(pose); + current_pose_count_queue_.push(count + 1); } } DEBUG_INFO(get_logger(), "[EKF] measurementUpdatePose calc time = %f [ms]", stop_watch_.toc()); @@ -190,18 +194,22 @@ void EKFLocalizer::timerCallback() } /* twist measurement update */ - if (!current_twist_info_queue_.empty()) { + if (!current_twist_queue_.empty()) { DEBUG_INFO(get_logger(), "------------------------- start Twist -------------------------"); stop_watch_.tic(); - int twist_info_queue_size = static_cast(current_twist_info_queue_.size()); - for (int i = 0; i < twist_info_queue_size; ++i) { - TwistInfo twist_info = current_twist_info_queue_.front(); - current_twist_info_queue_.pop(); - measurementUpdateTwist(*twist_info.twist); - ++twist_info.counter; - if (twist_info.counter < twist_info.smoothing_steps) { - current_twist_info_queue_.push(twist_info); + for (size_t i = 0; i < current_twist_queue_.size(); ++i) { + const auto twist = current_twist_queue_.front(); + current_twist_queue_.pop(); + + const int count = current_twist_count_queue_.front(); + current_twist_count_queue_.pop(); + + measurementUpdateTwist(*twist); + + if (count + 1 < params_.twist_smoothing_steps) { + current_twist_queue_.push(twist); + current_twist_count_queue_.push(count + 1); } } DEBUG_INFO(get_logger(), "[EKF] measurementUpdateTwist calc time = %f [ms]", stop_watch_.toc()); @@ -361,9 +369,9 @@ void EKFLocalizer::callbackPoseWithCovariance( if (!is_activated_) { return; } - PoseInfo pose_info = {msg, 0, params_.pose_smoothing_steps}; - current_pose_info_queue_.push(pose_info); + current_pose_queue_.push(msg); + current_pose_count_queue_.push(0); updateSimple1DFilters(*msg); } @@ -374,8 +382,8 @@ void EKFLocalizer::callbackPoseWithCovariance( void EKFLocalizer::callbackTwistWithCovariance( geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg) { - TwistInfo twist_info = {msg, 0, params_.twist_smoothing_steps}; - current_twist_info_queue_.push(twist_info); + current_twist_queue_.push(msg); + current_twist_count_queue_.push(0); } /* @@ -616,17 +624,17 @@ void EKFLocalizer::publishEstimateResult() pub_odom_->publish(odometry); /* debug measured pose */ - if (!current_pose_info_queue_.empty()) { + if (!current_pose_queue_.empty()) { geometry_msgs::msg::PoseStamped p; - p.pose = current_pose_info_queue_.back().pose->pose.pose; + p.pose = current_pose_queue_.back()->pose.pose; p.header.stamp = current_time; pub_measured_pose_->publish(p); } /* debug publish */ double pose_yaw = 0.0; - if (!current_pose_info_queue_.empty()) { - pose_yaw = tf2::getYaw(current_pose_info_queue_.back().pose->pose.pose.orientation); + if (!current_pose_queue_.empty()) { + pose_yaw = tf2::getYaw(current_pose_queue_.back()->pose.pose.orientation); } tier4_debug_msgs::msg::Float64MultiArrayStamped msg; @@ -683,8 +691,11 @@ void EKFLocalizer::serviceTriggerNode( std_srvs::srv::SetBool::Response::SharedPtr res) { if (req->data) { - while (!current_pose_info_queue_.empty()) current_pose_info_queue_.pop(); - while (!current_twist_info_queue_.empty()) current_twist_info_queue_.pop(); + while (!current_pose_queue_.empty()) current_pose_queue_.pop(); + while (!current_pose_count_queue_.empty()) current_pose_count_queue_.pop(); + + while (!current_twist_queue_.empty()) current_twist_queue_.pop(); + while (!current_twist_count_queue_.empty()) current_twist_count_queue_.pop(); is_activated_ = true; } else { is_activated_ = false; From 5f9d8478c97d8991cd5dd1ec85c9b5a50d582654 Mon Sep 17 00:00:00 2001 From: Tomoya Kimura Date: Wed, 18 Jan 2023 19:55:56 +0900 Subject: [PATCH 39/49] fix(accel_brake_map_calibrator): fix usage of transform listener (#2682) * fix(accel_brake_map_calibrator): fix usage of transform listener Signed-off-by: tomoya.kimura * add maintainer Signed-off-by: tomoya.kimura Signed-off-by: tomoya.kimura --- .../accel_brake_map_calibrator/package.xml | 3 +++ .../src/accel_brake_map_calibrator_node.cpp | 10 ++++------ 2 files changed, 7 insertions(+), 6 deletions(-) diff --git a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/package.xml b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/package.xml index 705d6ee5e5fc3..8163a1209883c 100644 --- a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/package.xml +++ b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/package.xml @@ -5,6 +5,9 @@ 0.1.0 The accel_brake_map_calibrator Tomoya Kimura + Taiki Tanaka + Takeshi Miura + Apache License 2.0 ament_cmake_auto diff --git a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp index 8c4550332a316..b664352f70fab 100644 --- a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp +++ b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp @@ -232,15 +232,13 @@ bool AccelBrakeMapCalibrator::getCurrentPitchFromTF(double * pitch) } // get tf - geometry_msgs::msg::TransformStamped::ConstSharedPtr transform; - try { - transform = transform_listener_->getTransform( - "map", "base_link", rclcpp::Time(0), rclcpp::Duration::from_seconds(0.5)); - } catch (tf2::TransformException & ex) { + const auto transform = transform_listener_->getTransform( + "map", "base_link", rclcpp::Time(0), rclcpp::Duration::from_seconds(0.5)); + if (!transform) { auto & clk = *this->get_clock(); RCLCPP_WARN_STREAM_THROTTLE( rclcpp::get_logger("accel_brake_map_calibrator"), clk, 5000, - "cannot get map to base_link transform. " << ex.what()); + "cannot get map to base_link transform. "); return false; } double roll, raw_pitch, yaw; From d15141e25e437d5244f925482c5a7fd0fac2cb41 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Thu, 19 Jan 2023 01:54:39 +0900 Subject: [PATCH 40/49] fix(route_handler): fix getShoulderLaneletSequence (#2683) Signed-off-by: kosuke55 Signed-off-by: kosuke55 --- planning/route_handler/src/route_handler.cpp | 28 ++------------------ 1 file changed, 2 insertions(+), 26 deletions(-) diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp index 8fe5e350a5599..f33a2e2d5c4f3 100644 --- a/planning/route_handler/src/route_handler.cpp +++ b/planning/route_handler/src/route_handler.cpp @@ -577,20 +577,8 @@ lanelet::ConstLanelets RouteHandler::getLaneletSequence( bool RouteHandler::getFollowingShoulderLanelet( const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelet * following_lanelet) const { - Pose back_pose; - back_pose.position.x = lanelet.centerline2d().back().x(); - back_pose.position.y = lanelet.centerline2d().back().y(); - back_pose.position.z = 0; - for (const auto & shoulder_lanelet : shoulder_lanelets_) { - Pose front_pose; - front_pose.position.x = shoulder_lanelet.centerline2d().front().x(); - front_pose.position.y = shoulder_lanelet.centerline2d().front().y(); - front_pose.position.z = 0; - if ( - std::hypot( - front_pose.position.x - back_pose.position.x, - front_pose.position.y - back_pose.position.y) < 5) { + if (lanelet::geometry::follows(lanelet, shoulder_lanelet)) { *following_lanelet = shoulder_lanelet; return true; } @@ -625,20 +613,8 @@ lanelet::ConstLanelets RouteHandler::getShoulderLaneletSequenceAfter( bool RouteHandler::getPreviousShoulderLanelet( const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelet * prev_lanelet) const { - Pose front_pose; - front_pose.position.x = lanelet.centerline2d().front().x(); - front_pose.position.y = lanelet.centerline2d().front().y(); - front_pose.position.z = 0; - for (const auto & shoulder_lanelet : shoulder_lanelets_) { - Pose back_pose; - back_pose.position.x = shoulder_lanelet.centerline2d().back().x(); - back_pose.position.y = shoulder_lanelet.centerline2d().back().y(); - back_pose.position.z = 0; - if ( - std::hypot( - front_pose.position.x - back_pose.position.x, - front_pose.position.y - back_pose.position.y) < 5) { + if (lanelet::geometry::follows(shoulder_lanelet, lanelet)) { *prev_lanelet = shoulder_lanelet; return true; } From 9e9beb85cd5947256a65c8569e87b7006cd6f4d7 Mon Sep 17 00:00:00 2001 From: Mehmet Dogru <48479081+mehmetdogru@users.noreply.github.com> Date: Thu, 19 Jan 2023 01:17:40 +0300 Subject: [PATCH 41/49] feat(obstacle_avoidance_planner): add fitting_uniform_circle configuration for mpt (#2646) * feat(obstacle_avoidance_planner): add fitting uniform circle configuration to mpt Signed-off-by: Mehmet Dogru <42mehmetdogru42@gmail.com> * feat: add update param Signed-off-by: Mehmet Dogru <42mehmetdogru42@gmail.com> Signed-off-by: Mehmet Dogru <42mehmetdogru42@gmail.com> --- .../obstacle_avoidance_planner.param.yaml | 3 ++ .../obstacle_avoidance_planner/src/node.cpp | 35 +++++++++++++++++++ 2 files changed, 38 insertions(+) diff --git a/planning/obstacle_avoidance_planner/config/obstacle_avoidance_planner.param.yaml b/planning/obstacle_avoidance_planner/config/obstacle_avoidance_planner.param.yaml index a18e531806171..fbe6b5d756bb2 100644 --- a/planning/obstacle_avoidance_planner/config/obstacle_avoidance_planner.param.yaml +++ b/planning/obstacle_avoidance_planner/config/obstacle_avoidance_planner.param.yaml @@ -148,6 +148,9 @@ num: 3 radius_ratio: 0.8 + fitting_uniform_circle: + num: 3 # must be greater than 1 + rear_drive: num_for_calculation: 3 front_radius_ratio: 1.0 diff --git a/planning/obstacle_avoidance_planner/src/node.cpp b/planning/obstacle_avoidance_planner/src/node.cpp index 2775089335128..8508a71013479 100644 --- a/planning/obstacle_avoidance_planner/src/node.cpp +++ b/planning/obstacle_avoidance_planner/src/node.cpp @@ -85,6 +85,25 @@ std::tuple, std::vector> calcVehicleCirclesInfo( return {radiuses, longitudinal_offsets}; } +std::tuple, std::vector> calcVehicleCirclesInfo( + const VehicleParam & vehicle_param, size_t circle_num) +{ + circle_num = std::max(circle_num, static_cast(2)); + + std::vector longitudinal_offsets; + std::vector radiuses; + + const double radius = vehicle_param.width / 2.0; + const double unit_lon_length = vehicle_param.length / static_cast(circle_num - 1); + + for (size_t i = 0; i < circle_num; ++i) { + longitudinal_offsets.push_back(unit_lon_length * i - vehicle_param.rear_overhang); + radiuses.push_back(radius); + } + + return {radiuses, longitudinal_offsets}; +} + std::tuple, std::vector> calcVehicleCirclesInfoByBicycleModel( const VehicleParam & vehicle_param, const size_t circle_num, const double rear_radius_ratio, const double front_radius_ratio) @@ -443,6 +462,13 @@ ObstacleAvoidancePlanner::ObstacleAvoidancePlanner(const rclcpp::NodeOptions & n calcVehicleCirclesInfo( vehicle_param_, vehicle_circle_num_for_calculation_, vehicle_circle_radius_ratios_.front()); + } else if (vehicle_circle_method_ == "fitting_uniform_circle") { + vehicle_circle_num_for_calculation_ = declare_parameter( + "advanced.mpt.collision_free_constraints.vehicle_circles.fitting_uniform_circle.num"); + + std::tie( + mpt_param_.vehicle_circle_radiuses, mpt_param_.vehicle_circle_longitudinal_offsets) = + calcVehicleCirclesInfo(vehicle_param_, vehicle_circle_num_for_calculation_); } else if (vehicle_circle_method_ == "rear_drive") { vehicle_circle_num_for_calculation_ = declare_parameter( "advanced.mpt.collision_free_constraints.vehicle_circles.rear_drive.num_for_calculation"); @@ -720,6 +746,15 @@ rcl_interfaces::msg::SetParametersResult ObstacleAvoidancePlanner::onParam( calcVehicleCirclesInfo( vehicle_param_, vehicle_circle_num_for_calculation_, vehicle_circle_radius_ratios_.front()); + } else if (vehicle_circle_method_ == "fitting_uniform_circle") { + updateParam( + parameters, + "advanced.mpt.collision_free_constraints.vehicle_circles.fitting_uniform_circle.num", + vehicle_circle_num_for_calculation_); + + std::tie( + mpt_param_.vehicle_circle_radiuses, mpt_param_.vehicle_circle_longitudinal_offsets) = + calcVehicleCirclesInfo(vehicle_param_, vehicle_circle_num_for_calculation_); } else if (vehicle_circle_method_ == "rear_drive") { updateParam( parameters, From 74bbb04fd15578743d1a05dd87cc66ca496471ec Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Thu, 19 Jan 2023 09:27:24 +0900 Subject: [PATCH 42/49] chore(lidar_apollo_instance_segmentation): modify invalid link (#2685) Signed-off-by: kosuke55 Signed-off-by: kosuke55 --- perception/lidar_apollo_instance_segmentation/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/perception/lidar_apollo_instance_segmentation/README.md b/perception/lidar_apollo_instance_segmentation/README.md index 2d61a84e37292..121f6d60e8dce 100644 --- a/perception/lidar_apollo_instance_segmentation/README.md +++ b/perception/lidar_apollo_instance_segmentation/README.md @@ -68,7 +68,7 @@ Original URL Supported lidars are velodyne 16, 64 and 128, but you can also use velodyne 32 and other lidars with good accuracy. -1. [apollo 3D Obstacle Perception description](https://github.com/ApolloAuto/apollo/blob/master/docs/specs/3d_obstacle_perception.md) +1. [apollo 3D Obstacle Perception description](https://github.com/ApolloAuto/apollo/blob/r7.0.0/docs/specs/3d_obstacle_perception.md) ```txt /****************************************************************************** From 9e3e3a0d43fcc1ff4207a593d3c26604741deda6 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Thu, 19 Jan 2023 09:29:55 +0900 Subject: [PATCH 43/49] fix(behavior_path_planner): modify pull out stop path (#2678) Signed-off-by: kosuke55 Signed-off-by: kosuke55 --- .../scene_module/pull_out/pull_out_module.hpp | 2 +- .../scene_module/pull_out/pull_out_module.cpp | 23 ++++++++++++++----- 2 files changed, 18 insertions(+), 7 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp index f7e3413f8f40c..7c47caf2dc90d 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp @@ -118,7 +118,7 @@ class PullOutModule : public SceneModuleInterface const std::vector & start_pose_candidates, const Pose & goal_pose); void planWithPriorityOnShortBackDistance( const std::vector & start_pose_candidates, const Pose & goal_pose); - void generateStopPath(); + PathWithLaneId generateStopPath() const; void updatePullOutStatus(); static bool isOverlappedWithLane( const lanelet::ConstLanelet & candidate_lanelet, diff --git a/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp b/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp index 225e842cda2e6..409fab42cd5d2 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp @@ -176,6 +176,9 @@ BehaviorModuleOutput PullOutModule::plan() if (!status_.is_safe) { RCLCPP_WARN_THROTTLE( getLogger(), *clock_, 5000, "Not found safe pull out path, publish stop path"); + const PathWithLaneId stop_path = generateStopPath(); + output.path = std::make_shared(stop_path); + path_candidate_ = std::make_shared(stop_path); return output; } @@ -430,11 +433,11 @@ void PullOutModule::planWithPriorityOnShortBackDistance( } } -void PullOutModule::generateStopPath() +PathWithLaneId PullOutModule::generateStopPath() const { const auto & current_pose = planner_data_->self_pose->pose; constexpr double dummy_path_distance = 1.0; - const auto & moved_pose = calcOffsetPose(current_pose, dummy_path_distance, 0, 0); + const auto moved_pose = calcOffsetPose(current_pose, dummy_path_distance, 0, 0); // convert Pose to PathPointWithLaneId with 0 velocity. auto toPathPointWithLaneId = [this](const Pose & pose) { @@ -452,9 +455,15 @@ void PullOutModule::generateStopPath() path.points.push_back(toPathPointWithLaneId(current_pose)); path.points.push_back(toPathPointWithLaneId(moved_pose)); - status_.pull_out_path.partial_paths.push_back(path); - status_.pull_out_path.start_pose = current_pose; - status_.pull_out_path.end_pose = current_pose; + // generate drivable area + const auto shorten_lanes = util::cutOverlappedLanes(path, status_.lanes); + const auto expanded_lanes = util::expandLanelets( + shorten_lanes, parameters_.drivable_area_left_bound_offset, + parameters_.drivable_area_right_bound_offset, parameters_.drivable_area_types_to_skip); + util::generateDrivableArea( + path, expanded_lanes, planner_data_->parameters.vehicle_length, planner_data_); + + return path; } void PullOutModule::updatePullOutStatus() @@ -498,7 +507,9 @@ void PullOutModule::updatePullOutStatus() RCLCPP_WARN_THROTTLE( getLogger(), *clock_, 5000, "Not found safe pull out path, generate stop path"); status_.back_finished = true; // no need to drive backward - generateStopPath(); + status_.pull_out_path.partial_paths.push_back(generateStopPath()); + status_.pull_out_path.start_pose = current_pose; + status_.pull_out_path.end_pose = current_pose; } checkBackFinished(); From fcacc809db290ce111c96d04b8594ed1b29eccb9 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Thu, 19 Jan 2023 14:44:45 +0900 Subject: [PATCH 44/49] fix(behavior_path_planner): fix pull out length to lane end (#2692) Signed-off-by: kosuke55 Signed-off-by: kosuke55 --- .../src/scene_module/pull_out/pull_out_module.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp b/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp index 409fab42cd5d2..59b248ce9b11e 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp @@ -557,8 +557,10 @@ std::vector PullOutModule::searchBackedPoses() // check the back pose is near the lane end const double length_to_backed_pose = lanelet::utils::getArcCoordinates(status_.pull_out_lanes, *backed_pose).length; - const double length_to_lane_end = - lanelet::utils::getLaneletLength2d(status_.pull_out_lanes.back()); + double length_to_lane_end = 0.0; + for (const auto & lane : status_.pull_out_lanes) { + length_to_lane_end += lanelet::utils::getLaneletLength2d(lane); + } const double distance_from_lane_end = length_to_lane_end - length_to_backed_pose; if (distance_from_lane_end < parameters_.ignore_distance_from_lane_end) { RCLCPP_WARN_THROTTLE( From 5bd8c2a7d5000bfaa96c40e0a5aa551bf03efae6 Mon Sep 17 00:00:00 2001 From: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com> Date: Thu, 19 Jan 2023 15:11:04 +0900 Subject: [PATCH 45/49] feat(behavior_path_planner): update clipping boundary for obstacles (#2628) * feat(behavior_path_planner): update clipping boundary for obstacles Signed-off-by: yutaka * fix format Signed-off-by: yutaka Signed-off-by: yutaka --- .../avoidance/avoidance_utils.hpp | 9 +- .../avoidance/avoidance_utils.cpp | 103 +++++++++++++----- 2 files changed, 81 insertions(+), 31 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_utils.hpp index 8a4cdf54f0238..b97da23c5d4bf 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_utils.hpp @@ -85,17 +85,16 @@ Polygon2d createEnvelopePolygon( void getEdgePoints( const Polygon2d & object_polygon, const double threshold, std::vector & edge_points); -void getEdgePoints( +void getPointData( const std::vector & bound, const std::vector & edge_points, - const double lat_dist_to_path, std::vector & edge_points_data, - size_t & start_segment_idx, size_t & end_segment_idx); + const double lat_dist_to_path, std::vector & edge_points_data); void sortPolygonPoints( const std::vector & points, std::vector & sorted_points); std::vector updateBoundary( - const std::vector & original_bound, const std::vector & points, - const size_t start_segment_idx, const size_t end_segment_idx); + const std::vector & original_bound, const std::vector & edge_points, + const std::vector & sorted_points); void generateDrivableArea( PathWithLaneId & path, const std::vector & lanes, const double vehicle_length, diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_utils.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_utils.cpp index a4e599bc60961..83263cb423a68 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_utils.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_utils.cpp @@ -582,16 +582,11 @@ void sortPolygonPoints( sorted_points = {first_point, second_point}; } -void getEdgePoints( +void getPointData( const std::vector & bound, const std::vector & edge_points, - const double lat_dist_to_path, std::vector & edge_points_data, - size_t & start_segment_idx, size_t & end_segment_idx) + const double lat_dist_to_path, std::vector & edge_points_data) { for (const auto & edge_point : edge_points) { - const size_t segment_idx = motion_utils::findNearestSegmentIndex(bound, edge_point); - start_segment_idx = std::min(start_segment_idx, segment_idx); - end_segment_idx = std::max(end_segment_idx, segment_idx); - PolygonPoint edge_point_data; edge_point_data.point = edge_point; edge_point_data.lat_dist_to_bound = motion_utils::calcLateralOffset(bound, edge_point); @@ -607,23 +602,86 @@ void getEdgePoints( } std::vector updateBoundary( - const std::vector & original_bound, const std::vector & points, - const size_t start_segment_idx, const size_t end_segment_idx) + const std::vector & original_bound, const std::vector & edge_points, + const std::vector & sorted_points) { - if (start_segment_idx >= end_segment_idx) { + if (edge_points.empty() || sorted_points.empty()) { return original_bound; } + const size_t bound_size = original_bound.size(); + double min_dist_from_start_point = std::numeric_limits::max(); + double min_dist_from_end_point = std::numeric_limits::max(); + Point start_edge_point = edge_points.front(); + Point end_edge_point = edge_points.back(); + for (const auto & edge_point : edge_points) { + const double dist_from_start_point = + motion_utils::calcSignedArcLength(original_bound, 0, edge_point); + const double dist_from_end_point = + std::fabs(motion_utils::calcSignedArcLength(original_bound, bound_size - 1, edge_point)); + + if (dist_from_start_point < min_dist_from_start_point) { + start_edge_point = edge_point; + min_dist_from_start_point = dist_from_start_point; + } + + if (dist_from_end_point < min_dist_from_end_point) { + end_edge_point = edge_point; + min_dist_from_end_point = dist_from_end_point; + } + } + + // get start and end point + const size_t start_segment_idx = + motion_utils::findNearestSegmentIndex(original_bound, start_edge_point); + const double front_offset = motion_utils::calcLongitudinalOffsetToSegment( + original_bound, start_segment_idx, start_edge_point); + const auto closest_front_point = + motion_utils::calcLongitudinalOffsetPoint(original_bound, start_segment_idx, front_offset); + const size_t end_segment_idx = + motion_utils::findNearestSegmentIndex(original_bound, end_edge_point); + const double end_offset = + motion_utils::calcLongitudinalOffsetToSegment(original_bound, end_segment_idx, end_edge_point); + const auto closest_end_point = + motion_utils::calcLongitudinalOffsetPoint(original_bound, end_segment_idx, end_offset); + std::vector updated_bound; + + const double min_dist = 1e-3; + // copy original points until front point std::copy( original_bound.begin(), original_bound.begin() + start_segment_idx + 1, std::back_inserter(updated_bound)); - for (size_t i = 0; i < points.size(); ++i) { - updated_bound.push_back(points.at(i).point); + + // insert closest front point + if ( + closest_front_point && + tier4_autoware_utils::calcDistance2d(*closest_front_point, updated_bound.back()) > min_dist) { + updated_bound.push_back(*closest_front_point); } - std::copy( - original_bound.begin() + end_segment_idx + 1, original_bound.end(), - std::back_inserter(updated_bound)); + + // insert sorted points + for (const auto & sorted_point : sorted_points) { + if (tier4_autoware_utils::calcDistance2d(sorted_point.point, updated_bound.back()) > min_dist) { + updated_bound.push_back(sorted_point.point); + } + } + + // insert closest end point + if ( + closest_end_point && + tier4_autoware_utils::calcDistance2d(*closest_end_point, updated_bound.back()) > min_dist) { + updated_bound.push_back(*closest_end_point); + } + + // copy original points until the end of the original bound + for (size_t i = end_segment_idx + 1; i < original_bound.size(); ++i) { + if ( + tier4_autoware_utils::calcDistance2d(original_bound.at(i), updated_bound.back()) > min_dist) { + updated_bound.push_back(original_bound.at(i)); + } + } + return updated_bound; } @@ -637,36 +695,29 @@ void generateDrivableArea( return; } - path.left_bound = motion_utils::resamplePointVector(path.left_bound, 1.0, true); - path.right_bound = motion_utils::resamplePointVector(path.right_bound, 1.0, true); - for (const auto & object : objects) { const auto & obj_pose = object.object.kinematics.initial_pose_with_covariance.pose; const auto & obj_poly = object.envelope_poly; constexpr double threshold = 0.01; - // get edge points + // get edge points of the object std::vector edge_points; getEdgePoints(obj_poly, threshold, edge_points); - // get boundary + // get a boundary that we have to change const double lat_dist_to_path = motion_utils::calcLateralOffset(path.points, obj_pose.position); auto & bound = lat_dist_to_path < 0.0 ? path.right_bound : path.left_bound; - size_t start_segment_idx = (bound.size() == 1 ? 0 : (bound.size() - 2)); - size_t end_segment_idx = 0; - // get edge points data std::vector edge_points_data; - getEdgePoints( - bound, edge_points, lat_dist_to_path, edge_points_data, start_segment_idx, end_segment_idx); + getPointData(bound, edge_points, lat_dist_to_path, edge_points_data); // sort points std::vector sorted_points; sortPolygonPoints(edge_points_data, sorted_points); // update boundary - bound = updateBoundary(bound, sorted_points, start_segment_idx, end_segment_idx); + bound = updateBoundary(bound, edge_points, sorted_points); } } From b38fa6a211614b74c5d70b149811fc372fa9bb7e Mon Sep 17 00:00:00 2001 From: Mehmet Dogru <48479081+mehmetdogru@users.noreply.github.com> Date: Thu, 19 Jan 2023 09:33:56 +0300 Subject: [PATCH 46/49] refactor(bpp-avoidance): remove redundant parameters (#2671) Signed-off-by: Mehmet Dogru <42mehmetdogru42@gmail.com> Signed-off-by: Mehmet Dogru <42mehmetdogru42@gmail.com> --- .../behavior_path_planner/config/avoidance/avoidance.param.yaml | 2 -- 1 file changed, 2 deletions(-) diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml index 40de19bd6cd7a..e42f188a62445 100644 --- a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml +++ b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml @@ -6,8 +6,6 @@ resample_interval_for_output: 4.0 # [m] detection_area_right_expand_dist: 0.0 # [m] detection_area_left_expand_dist: 1.0 # [m] - drivable_area_right_bound_offset: 0.0 # [m] - drivable_area_left_bound_offset: 0.0 # [m] object_envelope_buffer: 0.3 # [m] # avoidance module common setting From 87641bff81a70b5b0df587a25903d800cc146163 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Thu, 19 Jan 2023 16:59:13 +0900 Subject: [PATCH 47/49] fix(behavior_path_planner): improve isPathInLanelet function for lane change (#2693) * fix(behavior_path_planner): improve isPathInLanelet function for lane change Signed-off-by: Muhammad Zulfaqar Azmi * simplify the functions Signed-off-by: Muhammad Zulfaqar Azmi Signed-off-by: Muhammad Zulfaqar Azmi --- .../src/scene_module/lane_change/util.cpp | 25 +++++++++++-------- 1 file changed, 14 insertions(+), 11 deletions(-) diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp index fc0305e9b402b..2535596d89801 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp @@ -63,24 +63,26 @@ bool isPathInLanelets( const PathWithLaneId & path, const lanelet::ConstLanelets & original_lanelets, const lanelet::ConstLanelets & target_lanelets) { + const auto current_lane_poly = lanelet::utils::getPolygonFromArcLength( + original_lanelets, 0, std::numeric_limits::max()); + const auto target_lane_poly = + lanelet::utils::getPolygonFromArcLength(target_lanelets, 0, std::numeric_limits::max()); + const auto current_lane_poly_2d = lanelet::utils::to2D(current_lane_poly).basicPolygon(); + const auto target_lane_poly_2d = lanelet::utils::to2D(target_lane_poly).basicPolygon(); for (const auto & pt : path.points) { - bool is_in_lanelet = false; - for (const auto & llt : original_lanelets) { - if (lanelet::utils::isInLanelet(pt.point.pose, llt, 0.1)) { - is_in_lanelet = true; - } - } - for (const auto & llt : target_lanelets) { - if (lanelet::utils::isInLanelet(pt.point.pose, llt, 0.1)) { - is_in_lanelet = true; - } + const lanelet::BasicPoint2d ll_pt(pt.point.pose.position.x, pt.point.pose.position.y); + const auto is_in_current = boost::geometry::covered_by(ll_pt, current_lane_poly_2d); + if (is_in_current) { + continue; } - if (!is_in_lanelet) { + const auto is_in_target = boost::geometry::covered_by(ll_pt, target_lane_poly_2d); + if (!is_in_target) { return false; } } return true; } + double getExpectedVelocityWhenDecelerate( const double & velocity, const double & expected_acceleration, const double & duration) { @@ -228,6 +230,7 @@ LaneChangePaths getLaneChangePaths( const auto arc_position_from_current = lanelet::utils::getArcCoordinates(original_lanelets, pose); const auto arc_position_from_target = lanelet::utils::getArcCoordinates(target_lanelets, pose); + const auto target_lane_length = lanelet::utils::getLaneletLength2d(target_lanelets); for (double acceleration = 0.0; acceleration >= -maximum_deceleration; From 1a99f54b78eba49181906dcc99107db3a1be85a2 Mon Sep 17 00:00:00 2001 From: Yoshi Ri Date: Thu, 19 Jan 2023 23:36:31 +0900 Subject: [PATCH 48/49] feat(multi_object_tracker): change vehicle model from constant velocity and turn rate model to bicycle model (#2679) * try bicycle model Signed-off-by: yoshiri * apply bicycle model to bicycle tracker Signed-off-by: yoshiri Signed-off-by: yoshiri --- .../tracker/model/bicycle_tracker.hpp | 16 ++-- .../tracker/model/big_vehicle_tracker.hpp | 16 ++-- .../tracker/model/normal_vehicle_tracker.hpp | 16 ++-- .../src/tracker/model/bicycle_tracker.cpp | 85 ++++++++++-------- .../src/tracker/model/big_vehicle_tracker.cpp | 88 +++++++++++-------- .../tracker/model/normal_vehicle_tracker.cpp | 85 ++++++++++-------- 6 files changed, 161 insertions(+), 145 deletions(-) diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp index 8a30af39870b5..ad268283d5890 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp @@ -32,23 +32,17 @@ class BicycleTracker : public Tracker private: KalmanFilter ekf_; rclcpp::Time last_update_time_; - enum IDX { - X = 0, - Y = 1, - YAW = 2, - VX = 3, - WZ = 4, - }; + enum IDX { X = 0, Y = 1, YAW = 2, VX = 3, SLIP = 4 }; struct EkfParams { char dim_x = 5; float q_cov_x; float q_cov_y; float q_cov_yaw; - float q_cov_wz; + float q_cov_slip; float q_cov_vx; float p0_cov_vx; - float p0_cov_wz; + float p0_cov_slip; float r_cov_x; float r_cov_y; float r_cov_yaw; @@ -58,7 +52,9 @@ class BicycleTracker : public Tracker } ekf_params_; double max_vx_; - double max_wz_; + double max_slip_; + double lf_; + double lr_; float z_; private: diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp index 614faa6a7d5b4..8a9b6adfc9cd5 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp @@ -32,23 +32,17 @@ class BigVehicleTracker : public Tracker private: KalmanFilter ekf_; rclcpp::Time last_update_time_; - enum IDX { - X = 0, - Y = 1, - YAW = 2, - VX = 3, - WZ = 4, - }; + enum IDX { X = 0, Y = 1, YAW = 2, VX = 3, SLIP = 4 }; struct EkfParams { char dim_x = 5; float q_cov_x; float q_cov_y; float q_cov_yaw; - float q_cov_wz; + float q_cov_slip; float q_cov_vx; float p0_cov_vx; - float p0_cov_wz; + float p0_cov_slip; float r_cov_x; float r_cov_y; float r_cov_yaw; @@ -58,7 +52,9 @@ class BigVehicleTracker : public Tracker float p0_cov_yaw; } ekf_params_; double max_vx_; - double max_wz_; + double max_slip_; + double lf_; + double lr_; float z_; double velocity_deviation_threshold_; diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp index 8175f20c5d671..79e9a20a10421 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp @@ -33,13 +33,7 @@ class NormalVehicleTracker : public Tracker private: KalmanFilter ekf_; rclcpp::Time last_update_time_; - enum IDX { - X = 0, - Y = 1, - YAW = 2, - VX = 3, - WZ = 4, - }; + enum IDX { X = 0, Y = 1, YAW = 2, VX = 3, SLIP = 4 }; struct EkfParams { @@ -47,10 +41,10 @@ class NormalVehicleTracker : public Tracker float q_cov_x; float q_cov_y; float q_cov_yaw; - float q_cov_wz; + float q_cov_slip; float q_cov_vx; float p0_cov_vx; - float p0_cov_wz; + float p0_cov_slip; float r_cov_x; float r_cov_y; float r_cov_yaw; @@ -61,7 +55,9 @@ class NormalVehicleTracker : public Tracker } ekf_params_; double max_vx_; - double max_wz_; + double max_slip_; + double lf_; + double lr_; float z_; double velocity_deviation_threshold_; diff --git a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp index 20dbd51e88eaf..5de81eb2d4538 100644 --- a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp @@ -55,7 +55,7 @@ BicycleTracker::BicycleTracker( float q_stddev_y = 0.6; // [m/s] float q_stddev_yaw = tier4_autoware_utils::deg2rad(10); // [rad/s] float q_stddev_vx = tier4_autoware_utils::kmph2mps(10); // [m/(s*s)] - float q_stddev_wz = tier4_autoware_utils::deg2rad(25); // [rad/(s*s)] + float q_stddev_slip = tier4_autoware_utils::deg2rad(25); // [rad/(s*s)] float r_stddev_x = 0.6; // [m] float r_stddev_y = 0.4; // [m] float r_stddev_yaw = tier4_autoware_utils::deg2rad(30); // [rad] @@ -63,12 +63,12 @@ BicycleTracker::BicycleTracker( float p0_stddev_y = 0.5; // [m/s] float p0_stddev_yaw = tier4_autoware_utils::deg2rad(1000); // [rad/s] float p0_stddev_vx = tier4_autoware_utils::kmph2mps(1000); // [m/(s*s)] - float p0_stddev_wz = tier4_autoware_utils::deg2rad(10); // [rad/(s*s)] + float p0_stddev_slip = tier4_autoware_utils::deg2rad(10); // [rad/(s*s)] ekf_params_.q_cov_x = std::pow(q_stddev_x, 2.0); ekf_params_.q_cov_y = std::pow(q_stddev_y, 2.0); ekf_params_.q_cov_yaw = std::pow(q_stddev_yaw, 2.0); ekf_params_.q_cov_vx = std::pow(q_stddev_vx, 2.0); - ekf_params_.q_cov_wz = std::pow(q_stddev_wz, 2.0); + ekf_params_.q_cov_slip = std::pow(q_stddev_slip, 2.0); ekf_params_.r_cov_x = std::pow(r_stddev_x, 2.0); ekf_params_.r_cov_y = std::pow(r_stddev_y, 2.0); ekf_params_.r_cov_yaw = std::pow(r_stddev_yaw, 2.0); @@ -76,9 +76,9 @@ BicycleTracker::BicycleTracker( ekf_params_.p0_cov_y = std::pow(p0_stddev_y, 2.0); ekf_params_.p0_cov_yaw = std::pow(p0_stddev_yaw, 2.0); ekf_params_.p0_cov_vx = std::pow(p0_stddev_vx, 2.0); - ekf_params_.p0_cov_wz = std::pow(p0_stddev_wz, 2.0); + ekf_params_.p0_cov_slip = std::pow(p0_stddev_slip, 2.0); max_vx_ = tier4_autoware_utils::kmph2mps(100); // [m/s] - max_wz_ = tier4_autoware_utils::deg2rad(30); // [rad/s] + max_slip_ = tier4_autoware_utils::deg2rad(30); // [rad/s] // initialize X matrix Eigen::MatrixXd X(ekf_params_.dim_x, 1); @@ -87,11 +87,10 @@ BicycleTracker::BicycleTracker( X(IDX::YAW) = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); if (object.kinematics.has_twist) { X(IDX::VX) = object.kinematics.twist_with_covariance.twist.linear.x; - X(IDX::WZ) = object.kinematics.twist_with_covariance.twist.angular.z; } else { X(IDX::VX) = 0.0; - X(IDX::WZ) = 0.0; } + X(IDX::SLIP) = 0.0; // initialize P matrix Eigen::MatrixXd P = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); @@ -110,7 +109,7 @@ BicycleTracker::BicycleTracker( P(IDX::Y, IDX::X) = P(IDX::X, IDX::Y); P(IDX::YAW, IDX::YAW) = ekf_params_.p0_cov_yaw; P(IDX::VX, IDX::VX) = ekf_params_.p0_cov_vx; - P(IDX::WZ, IDX::WZ) = ekf_params_.p0_cov_wz; + P(IDX::SLIP, IDX::SLIP) = ekf_params_.p0_cov_slip; } else { P(IDX::X, IDX::X) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; P(IDX::X, IDX::Y) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y]; @@ -121,12 +120,10 @@ BicycleTracker::BicycleTracker( if (object.kinematics.has_twist_covariance) { P(IDX::VX, IDX::VX) = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; - P(IDX::WZ, IDX::WZ) = - object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW]; } else { P(IDX::VX, IDX::VX) = ekf_params_.p0_cov_vx; - P(IDX::WZ, IDX::WZ) = ekf_params_.p0_cov_wz; } + P(IDX::SLIP, IDX::SLIP) = ekf_params_.p0_cov_slip; } if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { @@ -136,6 +133,11 @@ BicycleTracker::BicycleTracker( bounding_box_ = {1.0, 0.5, 1.7}; } ekf_.init(X, P); + + // Set lf, lr + double point_ratio = 0.5; // comes to front if larger + lf_ = bounding_box_.length * point_ratio; + lr_ = bounding_box_.length * (1.0 - point_ratio); } bool BicycleTracker::predict(const rclcpp::Time & time) @@ -150,47 +152,53 @@ bool BicycleTracker::predict(const rclcpp::Time & time) bool BicycleTracker::predict(const double dt, KalmanFilter & ekf) const { - /* == Nonlinear model == + /* == Nonlinear model == static bicycle model * - * x_{k+1} = x_k + vx_k * cos(yaw_k) * dt - * y_{k+1} = y_k + vx_k * sin(yaw_k) * dt - * yaw_{k+1} = yaw_k + (wz_k) * dt + * x_{k+1} = x_k + vx_k * cos(yaw_k + slip_k) * dt + * y_{k+1} = y_k + vx_k * sin(yaw_k + slip_k) * dt + * yaw_{k+1} = yaw_k + vx_k / l_r * sin(slip_k) * dt * vx_{k+1} = vx_k - * wz_{k+1} = wz_k + * slip_{k+1} = slip_k * */ /* == Linearized model == * - * A = [ 1, 0, -vx*sin(yaw)*dt, cos(yaw)*dt, 0] - * [ 0, 1, vx*cos(yaw)*dt, sin(yaw)*dt, 0] - * [ 0, 0, 1, 0, dt] - * [ 0, 0, 0, 1, 0] - * [ 0, 0, 0, 0, 1] + * A = [ 1, 0, -vx*sin(yaw+slip)*dt, cos(yaw+slip)*dt, -vx*sin(yaw+slip)*dt] + * [ 0, 1, vx*cos(yaw+slip)*dt, sin(yaw+slip)*dt, vx*cos(yaw+slip)*dt] + * [ 0, 0, 1, 1/l_r*sin(slip)*dt, vx/l_r*cos(slip)*dt] + * [ 0, 0, 0, 1, 0] + * [ 0, 0, 0, 0, 1] */ // X t Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state ekf.getX(X_t); - const double cos_yaw = std::cos(X_t(IDX::YAW)); - const double sin_yaw = std::sin(X_t(IDX::YAW)); + const double cos_yaw = std::cos(X_t(IDX::YAW) + X_t(IDX::SLIP)); + const double sin_yaw = std::sin(X_t(IDX::YAW) + X_t(IDX::SLIP)); + const double cos_slip = std::cos(X_t(IDX::SLIP)); + const double sin_slip = std::sin(X_t(IDX::SLIP)); + const double vx = X_t(IDX::VX); const double sin_2yaw = std::sin(2.0f * X_t(IDX::YAW)); // X t+1 - Eigen::MatrixXd X_next_t(ekf_params_.dim_x, 1); // predicted state - X_next_t(IDX::X) = X_t(IDX::X) + X_t(IDX::VX) * cos_yaw * dt; // dx = v * cos(yaw) - X_next_t(IDX::Y) = X_t(IDX::Y) + X_t(IDX::VX) * sin_yaw * dt; // dy = v * sin(yaw) - X_next_t(IDX::YAW) = X_t(IDX::YAW) + (X_t(IDX::WZ)) * dt; // dyaw = omega + Eigen::MatrixXd X_next_t(ekf_params_.dim_x, 1); // predicted state + X_next_t(IDX::X) = X_t(IDX::X) + vx * cos_yaw * dt; // dx = v * cos(yaw) + X_next_t(IDX::Y) = X_t(IDX::Y) + vx * sin_yaw * dt; // dy = v * sin(yaw) + X_next_t(IDX::YAW) = X_t(IDX::YAW) + vx / lr_ * sin_slip * dt; // dyaw = omega X_next_t(IDX::VX) = X_t(IDX::VX); - X_next_t(IDX::WZ) = X_t(IDX::WZ); + X_next_t(IDX::SLIP) = X_t(IDX::SLIP); // A Eigen::MatrixXd A = Eigen::MatrixXd::Identity(ekf_params_.dim_x, ekf_params_.dim_x); - A(IDX::X, IDX::YAW) = -X_t(IDX::VX) * sin_yaw * dt; + A(IDX::X, IDX::YAW) = -vx * sin_yaw * dt; A(IDX::X, IDX::VX) = cos_yaw * dt; - A(IDX::Y, IDX::YAW) = X_t(IDX::VX) * cos_yaw * dt; + A(IDX::X, IDX::SLIP) = -vx * sin_yaw * dt; + A(IDX::Y, IDX::YAW) = vx * cos_yaw * dt; A(IDX::Y, IDX::VX) = sin_yaw * dt; - A(IDX::YAW, IDX::WZ) = dt; + A(IDX::Y, IDX::SLIP) = vx * cos_yaw * dt; + A(IDX::YAW, IDX::VX) = 1.0 / lr_ * sin_slip * dt; + A(IDX::YAW, IDX::SLIP) = vx / lr_ * cos_slip * dt; // Q Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); @@ -204,7 +212,7 @@ bool BicycleTracker::predict(const double dt, KalmanFilter & ekf) const Q(IDX::Y, IDX::X) = Q(IDX::X, IDX::Y); Q(IDX::YAW, IDX::YAW) = ekf_params_.q_cov_yaw * dt * dt; Q(IDX::VX, IDX::VX) = ekf_params_.q_cov_vx * dt * dt; - Q(IDX::WZ, IDX::WZ) = ekf_params_.q_cov_wz * dt * dt; + Q(IDX::SLIP, IDX::SLIP) = ekf_params_.q_cov_slip * dt * dt; Eigen::MatrixXd B = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); Eigen::MatrixXd u = Eigen::MatrixXd::Zero(ekf_params_.dim_x, 1); @@ -315,8 +323,8 @@ bool BicycleTracker::measureWithPose( if (!(-max_vx_ <= X_t(IDX::VX) && X_t(IDX::VX) <= max_vx_)) { X_t(IDX::VX) = X_t(IDX::VX) < 0 ? -max_vx_ : max_vx_; } - if (!(-max_wz_ <= X_t(IDX::WZ) && X_t(IDX::WZ) <= max_wz_)) { - X_t(IDX::WZ) = X_t(IDX::WZ) < 0 ? -max_wz_ : max_wz_; + if (!(-max_slip_ <= X_t(IDX::SLIP) && X_t(IDX::SLIP) <= max_slip_)) { + X_t(IDX::SLIP) = X_t(IDX::SLIP) < 0 ? -max_slip_ : max_slip_; } ekf_.init(X_t, P_t); } @@ -420,7 +428,8 @@ bool BicycleTracker::getTrackedObject( // twist twist_with_cov.twist.linear.x = X_t(IDX::VX); - twist_with_cov.twist.angular.z = X_t(IDX::WZ); + twist_with_cov.twist.angular.z = + X_t(IDX::VX) / lr_ * std::sin(X_t(IDX::SLIP)); // yaw_rate = vx_k / l_r * sin(slip_k) // twist covariance constexpr double vy_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative constexpr double vz_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative @@ -429,11 +438,11 @@ bool BicycleTracker::getTrackedObject( twist_with_cov.covariance[utils::MSG_COV_IDX::X_X] = P(IDX::VX, IDX::VX); twist_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = vy_cov; twist_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = vz_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::X_YAW] = P(IDX::VX, IDX::WZ); - twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_X] = P(IDX::WZ, IDX::VX); + twist_with_cov.covariance[utils::MSG_COV_IDX::X_YAW] = P(IDX::VX, IDX::SLIP); + twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_X] = P(IDX::SLIP, IDX::VX); twist_with_cov.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = wx_cov; twist_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = wy_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = P(IDX::WZ, IDX::WZ); + twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = P(IDX::SLIP, IDX::SLIP); // set shape object.shape.dimensions.x = bounding_box_.length; diff --git a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp index 06200df9400f6..12086daa23463 100644 --- a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp @@ -53,7 +53,7 @@ BigVehicleTracker::BigVehicleTracker( float q_stddev_y = 1.5; // [m/s] float q_stddev_yaw = tier4_autoware_utils::deg2rad(20); // [rad/s] float q_stddev_vx = tier4_autoware_utils::kmph2mps(10); // [m/(s*s)] - float q_stddev_wz = tier4_autoware_utils::deg2rad(20); // [rad/(s*s)] + float q_stddev_slip = tier4_autoware_utils::deg2rad(20); // [rad/(s*s)] float r_stddev_x = 1.5; // [m] float r_stddev_y = 0.5; // [m] float r_stddev_yaw = tier4_autoware_utils::deg2rad(30); // [rad] @@ -62,12 +62,12 @@ BigVehicleTracker::BigVehicleTracker( float p0_stddev_y = 0.5; // [m] float p0_stddev_yaw = tier4_autoware_utils::deg2rad(30); // [rad] float p0_stddev_vx = tier4_autoware_utils::kmph2mps(1000); // [m/s] - float p0_stddev_wz = tier4_autoware_utils::deg2rad(10); // [rad/s] + float p0_stddev_slip = tier4_autoware_utils::deg2rad(10); // [rad/s] ekf_params_.q_cov_x = std::pow(q_stddev_x, 2.0); ekf_params_.q_cov_y = std::pow(q_stddev_y, 2.0); ekf_params_.q_cov_yaw = std::pow(q_stddev_yaw, 2.0); ekf_params_.q_cov_vx = std::pow(q_stddev_vx, 2.0); - ekf_params_.q_cov_wz = std::pow(q_stddev_wz, 2.0); + ekf_params_.q_cov_slip = std::pow(q_stddev_slip, 2.0); ekf_params_.r_cov_x = std::pow(r_stddev_x, 2.0); ekf_params_.r_cov_y = std::pow(r_stddev_y, 2.0); ekf_params_.r_cov_yaw = std::pow(r_stddev_yaw, 2.0); @@ -76,9 +76,9 @@ BigVehicleTracker::BigVehicleTracker( ekf_params_.p0_cov_y = std::pow(p0_stddev_y, 2.0); ekf_params_.p0_cov_yaw = std::pow(p0_stddev_yaw, 2.0); ekf_params_.p0_cov_vx = std::pow(p0_stddev_vx, 2.0); - ekf_params_.p0_cov_wz = std::pow(p0_stddev_wz, 2.0); + ekf_params_.p0_cov_slip = std::pow(p0_stddev_slip, 2.0); max_vx_ = tier4_autoware_utils::kmph2mps(100); // [m/s] - max_wz_ = tier4_autoware_utils::deg2rad(30); // [rad/s] + max_slip_ = tier4_autoware_utils::deg2rad(30); // [rad] velocity_deviation_threshold_ = tier4_autoware_utils::kmph2mps(10); // [m/s] // initialize X matrix @@ -86,12 +86,12 @@ BigVehicleTracker::BigVehicleTracker( X(IDX::X) = object.kinematics.pose_with_covariance.pose.position.x; X(IDX::Y) = object.kinematics.pose_with_covariance.pose.position.y; X(IDX::YAW) = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + X(IDX::SLIP) = 0.0; if (object.kinematics.has_twist) { X(IDX::VX) = object.kinematics.twist_with_covariance.twist.linear.x; - X(IDX::WZ) = object.kinematics.twist_with_covariance.twist.angular.z; + // X(IDX::YAW) = object.kinematics.twist_with_covariance.twist.angular.z; } else { X(IDX::VX) = 0.0; - X(IDX::WZ) = 0.0; } // initialize P matrix @@ -110,7 +110,7 @@ BigVehicleTracker::BigVehicleTracker( P(IDX::Y, IDX::X) = P(IDX::X, IDX::Y); P(IDX::YAW, IDX::YAW) = ekf_params_.p0_cov_yaw; P(IDX::VX, IDX::VX) = ekf_params_.p0_cov_vx; - P(IDX::WZ, IDX::WZ) = ekf_params_.p0_cov_wz; + P(IDX::SLIP, IDX::SLIP) = ekf_params_.p0_cov_slip; } else { P(IDX::X, IDX::X) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; P(IDX::X, IDX::Y) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y]; @@ -121,12 +121,10 @@ BigVehicleTracker::BigVehicleTracker( if (object.kinematics.has_twist_covariance) { P(IDX::VX, IDX::VX) = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; - P(IDX::WZ, IDX::WZ) = - object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW]; } else { P(IDX::VX, IDX::VX) = ekf_params_.p0_cov_vx; - P(IDX::WZ, IDX::WZ) = ekf_params_.p0_cov_wz; } + P(IDX::SLIP, IDX::SLIP) = ekf_params_.p0_cov_slip; } if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { @@ -149,6 +147,11 @@ BigVehicleTracker::BigVehicleTracker( /* calc nearest corner index*/ setNearestCornerOrSurfaceIndex(self_transform); // this index is used in next measure step + + // Set lf, lr + double point_ratio = 0.5; // comes to front if larger + lf_ = bounding_box_.length * point_ratio; + lr_ = bounding_box_.length * (1.0 - point_ratio); } bool BigVehicleTracker::predict(const rclcpp::Time & time) @@ -163,47 +166,53 @@ bool BigVehicleTracker::predict(const rclcpp::Time & time) bool BigVehicleTracker::predict(const double dt, KalmanFilter & ekf) const { - /* == Nonlinear model == + /* == Nonlinear model == static bicycle model * - * x_{k+1} = x_k + vx_k * cos(yaw_k) * dt - * y_{k+1} = y_k + vx_k * sin(yaw_k) * dt - * yaw_{k+1} = yaw_k + (wz_k) * dt + * x_{k+1} = x_k + vx_k * cos(yaw_k + slip_k) * dt + * y_{k+1} = y_k + vx_k * sin(yaw_k + slip_k) * dt + * yaw_{k+1} = yaw_k + vx_k / l_r * sin(slip_k) * dt * vx_{k+1} = vx_k - * wz_{k+1} = wz_k + * slip_{k+1} = slip_k * */ /* == Linearized model == * - * A = [ 1, 0, -vx*sin(yaw)*dt, cos(yaw)*dt, 0] - * [ 0, 1, vx*cos(yaw)*dt, sin(yaw)*dt, 0] - * [ 0, 0, 1, 0, dt] - * [ 0, 0, 0, 1, 0] - * [ 0, 0, 0, 0, 1] + * A = [ 1, 0, -vx*sin(yaw+slip)*dt, cos(yaw+slip)*dt, -vx*sin(yaw+slip)*dt] + * [ 0, 1, vx*cos(yaw+slip)*dt, sin(yaw+slip)*dt, vx*cos(yaw+slip)*dt] + * [ 0, 0, 1, 1/l_r*sin(slip)*dt, vx/l_r*cos(slip)*dt] + * [ 0, 0, 0, 1, 0] + * [ 0, 0, 0, 0, 1] */ // X t Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state ekf.getX(X_t); - const double cos_yaw = std::cos(X_t(IDX::YAW)); - const double sin_yaw = std::sin(X_t(IDX::YAW)); + const double cos_yaw = std::cos(X_t(IDX::YAW) + X_t(IDX::SLIP)); + const double sin_yaw = std::sin(X_t(IDX::YAW) + X_t(IDX::SLIP)); + const double cos_slip = std::cos(X_t(IDX::SLIP)); + const double sin_slip = std::sin(X_t(IDX::SLIP)); + const double vx = X_t(IDX::VX); const double sin_2yaw = std::sin(2.0f * X_t(IDX::YAW)); // X t+1 - Eigen::MatrixXd X_next_t(ekf_params_.dim_x, 1); // predicted state - X_next_t(IDX::X) = X_t(IDX::X) + X_t(IDX::VX) * cos_yaw * dt; // dx = v * cos(yaw) - X_next_t(IDX::Y) = X_t(IDX::Y) + X_t(IDX::VX) * sin_yaw * dt; // dy = v * sin(yaw) - X_next_t(IDX::YAW) = X_t(IDX::YAW) + (X_t(IDX::WZ)) * dt; // dyaw = omega + Eigen::MatrixXd X_next_t(ekf_params_.dim_x, 1); // predicted state + X_next_t(IDX::X) = X_t(IDX::X) + vx * cos_yaw * dt; // dx = v * cos(yaw) + X_next_t(IDX::Y) = X_t(IDX::Y) + vx * sin_yaw * dt; // dy = v * sin(yaw) + X_next_t(IDX::YAW) = X_t(IDX::YAW) + vx / lr_ * sin_slip * dt; // dyaw = omega X_next_t(IDX::VX) = X_t(IDX::VX); - X_next_t(IDX::WZ) = X_t(IDX::WZ); + X_next_t(IDX::SLIP) = X_t(IDX::SLIP); // A Eigen::MatrixXd A = Eigen::MatrixXd::Identity(ekf_params_.dim_x, ekf_params_.dim_x); - A(IDX::X, IDX::YAW) = -X_t(IDX::VX) * sin_yaw * dt; + A(IDX::X, IDX::YAW) = -vx * sin_yaw * dt; A(IDX::X, IDX::VX) = cos_yaw * dt; - A(IDX::Y, IDX::YAW) = X_t(IDX::VX) * cos_yaw * dt; + A(IDX::X, IDX::SLIP) = -vx * sin_yaw * dt; + A(IDX::Y, IDX::YAW) = vx * cos_yaw * dt; A(IDX::Y, IDX::VX) = sin_yaw * dt; - A(IDX::YAW, IDX::WZ) = dt; + A(IDX::Y, IDX::SLIP) = vx * cos_yaw * dt; + A(IDX::YAW, IDX::VX) = 1.0 / lr_ * sin_slip * dt; + A(IDX::YAW, IDX::SLIP) = vx / lr_ * cos_slip * dt; // Q Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); @@ -217,7 +226,7 @@ bool BigVehicleTracker::predict(const double dt, KalmanFilter & ekf) const Q(IDX::Y, IDX::X) = Q(IDX::X, IDX::Y); Q(IDX::YAW, IDX::YAW) = ekf_params_.q_cov_yaw * dt * dt; Q(IDX::VX, IDX::VX) = ekf_params_.q_cov_vx * dt * dt; - Q(IDX::WZ, IDX::WZ) = ekf_params_.q_cov_wz * dt * dt; + Q(IDX::SLIP, IDX::SLIP) = ekf_params_.q_cov_slip * dt * dt; Eigen::MatrixXd B = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); Eigen::MatrixXd u = Eigen::MatrixXd::Zero(ekf_params_.dim_x, 1); @@ -334,7 +343,7 @@ bool BigVehicleTracker::measureWithPose( RCLCPP_WARN(logger_, "Cannot update"); } - // normalize yaw and limit vx, wz + // normalize yaw and limit vx, slip { Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); Eigen::MatrixXd P_t(ekf_params_.dim_x, ekf_params_.dim_x); @@ -344,8 +353,8 @@ bool BigVehicleTracker::measureWithPose( if (!(-max_vx_ <= X_t(IDX::VX) && X_t(IDX::VX) <= max_vx_)) { X_t(IDX::VX) = X_t(IDX::VX) < 0 ? -max_vx_ : max_vx_; } - if (!(-max_wz_ <= X_t(IDX::WZ) && X_t(IDX::WZ) <= max_wz_)) { - X_t(IDX::WZ) = X_t(IDX::WZ) < 0 ? -max_wz_ : max_wz_; + if (!(-max_slip_ <= X_t(IDX::SLIP) && X_t(IDX::SLIP) <= max_slip_)) { + X_t(IDX::SLIP) = X_t(IDX::SLIP) < 0 ? -max_slip_ : max_slip_; } ekf_.init(X_t, P_t); } @@ -469,7 +478,8 @@ bool BigVehicleTracker::getTrackedObject( // twist twist_with_cov.twist.linear.x = X_t(IDX::VX); - twist_with_cov.twist.angular.z = X_t(IDX::WZ); + twist_with_cov.twist.angular.z = + X_t(IDX::VX) / lr_ * std::sin(X_t(IDX::SLIP)); // yaw_rate = vx_k / l_r * sin(slip_k) // twist covariance constexpr double vy_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative constexpr double vz_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative @@ -478,11 +488,11 @@ bool BigVehicleTracker::getTrackedObject( twist_with_cov.covariance[utils::MSG_COV_IDX::X_X] = P(IDX::VX, IDX::VX); twist_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = vy_cov; twist_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = vz_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::X_YAW] = P(IDX::VX, IDX::WZ); - twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_X] = P(IDX::WZ, IDX::VX); + twist_with_cov.covariance[utils::MSG_COV_IDX::X_YAW] = P(IDX::VX, IDX::SLIP); + twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_X] = P(IDX::SLIP, IDX::VX); twist_with_cov.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = wx_cov; twist_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = wy_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = P(IDX::WZ, IDX::WZ); + twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = P(IDX::SLIP, IDX::SLIP); // set shape object.shape.dimensions.x = bounding_box_.length; diff --git a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp index 69d7e9b19ae88..f2a8488ec6089 100644 --- a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp @@ -53,7 +53,7 @@ NormalVehicleTracker::NormalVehicleTracker( float q_stddev_y = 1.0; // object coordinate [m/s] float q_stddev_yaw = tier4_autoware_utils::deg2rad(20); // map coordinate[rad/s] float q_stddev_vx = tier4_autoware_utils::kmph2mps(10); // object coordinate [m/(s*s)] - float q_stddev_wz = tier4_autoware_utils::deg2rad(20); // object coordinate [rad/(s*s)] + float q_stddev_slip = tier4_autoware_utils::deg2rad(20); // object coordinate [rad/(s*s)] float r_stddev_x = 1.0; // object coordinate [m] float r_stddev_y = 0.3; // object coordinate [m] float r_stddev_yaw = tier4_autoware_utils::deg2rad(30); // map coordinate [rad] @@ -62,12 +62,12 @@ NormalVehicleTracker::NormalVehicleTracker( float p0_stddev_y = 0.3; // object coordinate [m/s] float p0_stddev_yaw = tier4_autoware_utils::deg2rad(30); // map coordinate [rad] float p0_stddev_vx = tier4_autoware_utils::kmph2mps(1000); // object coordinate [m/s] - float p0_stddev_wz = tier4_autoware_utils::deg2rad(10); // object coordinate [rad/s] + float p0_stddev_slip = tier4_autoware_utils::deg2rad(10); // object coordinate [rad/s] ekf_params_.q_cov_x = std::pow(q_stddev_x, 2.0); ekf_params_.q_cov_y = std::pow(q_stddev_y, 2.0); ekf_params_.q_cov_yaw = std::pow(q_stddev_yaw, 2.0); ekf_params_.q_cov_vx = std::pow(q_stddev_vx, 2.0); - ekf_params_.q_cov_wz = std::pow(q_stddev_wz, 2.0); + ekf_params_.q_cov_slip = std::pow(q_stddev_slip, 2.0); ekf_params_.r_cov_x = std::pow(r_stddev_x, 2.0); ekf_params_.r_cov_y = std::pow(r_stddev_y, 2.0); ekf_params_.r_cov_yaw = std::pow(r_stddev_yaw, 2.0); @@ -76,9 +76,9 @@ NormalVehicleTracker::NormalVehicleTracker( ekf_params_.p0_cov_y = std::pow(p0_stddev_y, 2.0); ekf_params_.p0_cov_yaw = std::pow(p0_stddev_yaw, 2.0); ekf_params_.p0_cov_vx = std::pow(p0_stddev_vx, 2.0); - ekf_params_.p0_cov_wz = std::pow(p0_stddev_wz, 2.0); + ekf_params_.p0_cov_slip = std::pow(p0_stddev_slip, 2.0); max_vx_ = tier4_autoware_utils::kmph2mps(100); // [m/s] - max_wz_ = tier4_autoware_utils::deg2rad(30); // [rad/s] + max_slip_ = tier4_autoware_utils::deg2rad(30); // [rad/s] velocity_deviation_threshold_ = tier4_autoware_utils::kmph2mps(10); // [m/s] // initialize X matrix @@ -88,11 +88,10 @@ NormalVehicleTracker::NormalVehicleTracker( X(IDX::YAW) = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); if (object.kinematics.has_twist) { X(IDX::VX) = object.kinematics.twist_with_covariance.twist.linear.x; - X(IDX::WZ) = object.kinematics.twist_with_covariance.twist.angular.z; } else { X(IDX::VX) = 0.0; - X(IDX::WZ) = 0.0; } + X(IDX::SLIP) = 0.0; // initialize P matrix Eigen::MatrixXd P = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); @@ -110,7 +109,7 @@ NormalVehicleTracker::NormalVehicleTracker( P(IDX::Y, IDX::X) = P(IDX::X, IDX::Y); P(IDX::YAW, IDX::YAW) = ekf_params_.p0_cov_yaw; P(IDX::VX, IDX::VX) = ekf_params_.p0_cov_vx; - P(IDX::WZ, IDX::WZ) = ekf_params_.p0_cov_wz; + P(IDX::SLIP, IDX::SLIP) = ekf_params_.p0_cov_slip; } else { P(IDX::X, IDX::X) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; P(IDX::X, IDX::Y) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y]; @@ -121,12 +120,10 @@ NormalVehicleTracker::NormalVehicleTracker( if (object.kinematics.has_twist_covariance) { P(IDX::VX, IDX::VX) = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; - P(IDX::WZ, IDX::WZ) = - object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW]; } else { P(IDX::VX, IDX::VX) = ekf_params_.p0_cov_vx; - P(IDX::WZ, IDX::WZ) = ekf_params_.p0_cov_wz; } + P(IDX::SLIP, IDX::SLIP) = ekf_params_.p0_cov_slip; } if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { @@ -150,6 +147,11 @@ NormalVehicleTracker::NormalVehicleTracker( /* calc nearest corner index*/ setNearestCornerOrSurfaceIndex(self_transform); // this index is used in next measure step + + // Set lf, lr + double point_ratio = 0.5; // comes to front if larger + lf_ = bounding_box_.length * point_ratio; + lr_ = bounding_box_.length * (1.0 - point_ratio); } bool NormalVehicleTracker::predict(const rclcpp::Time & time) @@ -164,47 +166,53 @@ bool NormalVehicleTracker::predict(const rclcpp::Time & time) bool NormalVehicleTracker::predict(const double dt, KalmanFilter & ekf) const { - /* == Nonlinear model == + /* == Nonlinear model == static bicycle model * - * x_{k+1} = x_k + vx_k * cos(yaw_k) * dt - * y_{k+1} = y_k + vx_k * sin(yaw_k) * dt - * yaw_{k+1} = yaw_k + (wz_k) * dt + * x_{k+1} = x_k + vx_k * cos(yaw_k + slip_k) * dt + * y_{k+1} = y_k + vx_k * sin(yaw_k + slip_k) * dt + * yaw_{k+1} = yaw_k + vx_k / l_r * sin(slip_k) * dt * vx_{k+1} = vx_k - * wz_{k+1} = wz_k + * slip_{k+1} = slip_k * */ /* == Linearized model == * - * A = [ 1, 0, -vx*sin(yaw)*dt, cos(yaw)*dt, 0] - * [ 0, 1, vx*cos(yaw)*dt, sin(yaw)*dt, 0] - * [ 0, 0, 1, 0, dt] - * [ 0, 0, 0, 1, 0] - * [ 0, 0, 0, 0, 1] + * A = [ 1, 0, -vx*sin(yaw+slip)*dt, cos(yaw+slip)*dt, -vx*sin(yaw+slip)*dt] + * [ 0, 1, vx*cos(yaw+slip)*dt, sin(yaw+slip)*dt, vx*cos(yaw+slip)*dt] + * [ 0, 0, 1, 1/l_r*sin(slip)*dt, vx/l_r*cos(slip)*dt] + * [ 0, 0, 0, 1, 0] + * [ 0, 0, 0, 0, 1] */ // X t Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state ekf.getX(X_t); - const double cos_yaw = std::cos(X_t(IDX::YAW)); - const double sin_yaw = std::sin(X_t(IDX::YAW)); + const double cos_yaw = std::cos(X_t(IDX::YAW) + X_t(IDX::SLIP)); + const double sin_yaw = std::sin(X_t(IDX::YAW) + X_t(IDX::SLIP)); + const double cos_slip = std::cos(X_t(IDX::SLIP)); + const double sin_slip = std::sin(X_t(IDX::SLIP)); + const double vx = X_t(IDX::VX); const double sin_2yaw = std::sin(2.0f * X_t(IDX::YAW)); // X t+1 - Eigen::MatrixXd X_next_t(ekf_params_.dim_x, 1); // predicted state - X_next_t(IDX::X) = X_t(IDX::X) + X_t(IDX::VX) * cos_yaw * dt; // dx = v * cos(yaw) - X_next_t(IDX::Y) = X_t(IDX::Y) + X_t(IDX::VX) * sin_yaw * dt; // dy = v * sin(yaw) - X_next_t(IDX::YAW) = X_t(IDX::YAW) + (X_t(IDX::WZ)) * dt; // dyaw = omega + Eigen::MatrixXd X_next_t(ekf_params_.dim_x, 1); // predicted state + X_next_t(IDX::X) = X_t(IDX::X) + vx * cos_yaw * dt; // dx = v * cos(yaw) + X_next_t(IDX::Y) = X_t(IDX::Y) + vx * sin_yaw * dt; // dy = v * sin(yaw) + X_next_t(IDX::YAW) = X_t(IDX::YAW) + vx / lr_ * sin_slip * dt; // dyaw = omega X_next_t(IDX::VX) = X_t(IDX::VX); - X_next_t(IDX::WZ) = X_t(IDX::WZ); + X_next_t(IDX::SLIP) = X_t(IDX::SLIP); // A Eigen::MatrixXd A = Eigen::MatrixXd::Identity(ekf_params_.dim_x, ekf_params_.dim_x); - A(IDX::X, IDX::YAW) = -X_t(IDX::VX) * sin_yaw * dt; + A(IDX::X, IDX::YAW) = -vx * sin_yaw * dt; A(IDX::X, IDX::VX) = cos_yaw * dt; - A(IDX::Y, IDX::YAW) = X_t(IDX::VX) * cos_yaw * dt; + A(IDX::X, IDX::SLIP) = -vx * sin_yaw * dt; + A(IDX::Y, IDX::YAW) = vx * cos_yaw * dt; A(IDX::Y, IDX::VX) = sin_yaw * dt; - A(IDX::YAW, IDX::WZ) = dt; + A(IDX::Y, IDX::SLIP) = vx * cos_yaw * dt; + A(IDX::YAW, IDX::VX) = 1.0 / lr_ * sin_slip * dt; + A(IDX::YAW, IDX::SLIP) = vx / lr_ * cos_slip * dt; // Q Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); @@ -218,7 +226,7 @@ bool NormalVehicleTracker::predict(const double dt, KalmanFilter & ekf) const Q(IDX::Y, IDX::X) = Q(IDX::X, IDX::Y); Q(IDX::YAW, IDX::YAW) = ekf_params_.q_cov_yaw * dt * dt; Q(IDX::VX, IDX::VX) = ekf_params_.q_cov_vx * dt * dt; - Q(IDX::WZ, IDX::WZ) = ekf_params_.q_cov_wz * dt * dt; + Q(IDX::SLIP, IDX::SLIP) = ekf_params_.q_cov_slip * dt * dt; Eigen::MatrixXd B = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); Eigen::MatrixXd u = Eigen::MatrixXd::Zero(ekf_params_.dim_x, 1); @@ -356,8 +364,8 @@ bool NormalVehicleTracker::measureWithPose( if (!(-max_vx_ <= X_t(IDX::VX) && X_t(IDX::VX) <= max_vx_)) { X_t(IDX::VX) = X_t(IDX::VX) < 0 ? -max_vx_ : max_vx_; } - if (!(-max_wz_ <= X_t(IDX::WZ) && X_t(IDX::WZ) <= max_wz_)) { - X_t(IDX::WZ) = X_t(IDX::WZ) < 0 ? -max_wz_ : max_wz_; + if (!(-max_slip_ <= X_t(IDX::SLIP) && X_t(IDX::SLIP) <= max_slip_)) { + X_t(IDX::SLIP) = X_t(IDX::SLIP) < 0 ? -max_slip_ : max_slip_; } ekf_.init(X_t, P_t); } @@ -486,7 +494,8 @@ bool NormalVehicleTracker::getTrackedObject( // twist twist_with_cov.twist.linear.x = X_t(IDX::VX); - twist_with_cov.twist.angular.z = X_t(IDX::WZ); + twist_with_cov.twist.angular.z = + X_t(IDX::VX) / lr_ * std::sin(X_t(IDX::SLIP)); // yaw_rate = vx_k / l_r * sin(slip_k) // twist covariance constexpr double vy_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative constexpr double vz_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative @@ -495,11 +504,11 @@ bool NormalVehicleTracker::getTrackedObject( twist_with_cov.covariance[utils::MSG_COV_IDX::X_X] = P(IDX::VX, IDX::VX); twist_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = vy_cov; twist_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = vz_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::X_YAW] = P(IDX::VX, IDX::WZ); - twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_X] = P(IDX::WZ, IDX::VX); + twist_with_cov.covariance[utils::MSG_COV_IDX::X_YAW] = P(IDX::VX, IDX::SLIP); + twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_X] = P(IDX::SLIP, IDX::VX); twist_with_cov.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = wx_cov; twist_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = wy_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = P(IDX::WZ, IDX::WZ); + twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = P(IDX::SLIP, IDX::SLIP); // set shape object.shape.dimensions.x = bounding_box_.length; From 030590645d835127f49d1cbd0715d9966ad5df11 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Fri, 20 Jan 2023 02:52:36 +0900 Subject: [PATCH 49/49] fix(behavior_path_planner): fix pull out stop path (#2699) Signed-off-by: kosuke55 Signed-off-by: kosuke55 --- .../src/scene_module/pull_out/pull_out_module.cpp | 15 +++++++++++++-- 1 file changed, 13 insertions(+), 2 deletions(-) diff --git a/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp b/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp index 59b248ce9b11e..690e7ff0a0045 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp @@ -176,7 +176,8 @@ BehaviorModuleOutput PullOutModule::plan() if (!status_.is_safe) { RCLCPP_WARN_THROTTLE( getLogger(), *clock_, 5000, "Not found safe pull out path, publish stop path"); - const PathWithLaneId stop_path = generateStopPath(); + // the path of getCurrent() is generated by generateStopPath() + const PathWithLaneId stop_path = getCurrentPath(); output.path = std::make_shared(stop_path); path_candidate_ = std::make_shared(stop_path); return output; @@ -285,11 +286,20 @@ BehaviorModuleOutput PullOutModule::planWaitingApproval() waitApproval(); BehaviorModuleOutput output; + if (!status_.is_safe) { + RCLCPP_WARN_THROTTLE( + getLogger(), *clock_, 5000, "Not found safe pull out path, publish stop path"); + // the path of getCurrent() is generated by generateStopPath() + const PathWithLaneId stop_path = getCurrentPath(); + output.path = std::make_shared(stop_path); + path_candidate_ = std::make_shared(stop_path); + return output; + } + const auto current_lanes = util::getExtendedCurrentLanes(planner_data_); const auto pull_out_lanes = pull_out_utils::getPullOutLanes(planner_data_); const auto drivable_lanes = util::generateDrivableLanesWithShoulderLanes(current_lanes, pull_out_lanes); - const auto expanded_lanes = util::expandLanelets( drivable_lanes, parameters_.drivable_area_left_bound_offset, parameters_.drivable_area_right_bound_offset, parameters_.drivable_area_types_to_skip); @@ -507,6 +517,7 @@ void PullOutModule::updatePullOutStatus() RCLCPP_WARN_THROTTLE( getLogger(), *clock_, 5000, "Not found safe pull out path, generate stop path"); status_.back_finished = true; // no need to drive backward + status_.pull_out_path.partial_paths.clear(); status_.pull_out_path.partial_paths.push_back(generateStopPath()); status_.pull_out_path.start_pose = current_pose; status_.pull_out_path.end_pose = current_pose;