diff --git a/.cspell-partial.json b/.cspell-partial.json index 7d41451bd840a..a3c4ca455d444 100644 --- a/.cspell-partial.json +++ b/.cspell-partial.json @@ -3,7 +3,6 @@ "**/perception/**", "**/planning/**", "**/sensing/**", - "**/control/**", "perception/bytetrack/lib/**" ], "ignoreRegExpList": [], 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 0d144726a0866..0854340ed24a5 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp @@ -379,6 +379,14 @@ class MPC const MPCMatrix & mpc_matrix, const AckermannLateralCommand & ctrl_cmd, const VectorXd & Uex, const Odometry & current_kinematics) const; + /** + * @brief calculate steering rate limit along with the target trajectory + * @param reference_trajectory The reference trajectory. + * @param current_velocity current velocity of ego. + */ + VectorXd calcSteerRateLimitOnTrajectory( + const MPCTrajectory & trajectory, const double current_velocity) const; + //!< @brief logging with warn and return false template inline bool fail_warn_throttle(Args &&... args) const diff --git a/control/mpc_lateral_controller/src/mpc.cpp b/control/mpc_lateral_controller/src/mpc.cpp index 8e99880b68050..643f8a6f91023 100644 --- a/control/mpc_lateral_controller/src/mpc.cpp +++ b/control/mpc_lateral_controller/src/mpc.cpp @@ -359,9 +359,6 @@ std::pair MPC::updateStateForDelayCompensation( MatrixXd x_curr = x0_orig; double mpc_curr_time = start_time; - // for (const auto & tt : traj.relative_time) { - // std::cerr << "traj.relative_time = " << tt << std::endl; - // } for (size_t i = 0; i < m_input_buffer.size(); ++i) { double k, v = 0.0; try { @@ -573,47 +570,16 @@ std::pair MPC::executeOptimization( A(i, i - 1) = -1.0; } - const bool is_vehicle_stopped = std::fabs(current_velocity) < 0.01; - const auto get_adaptive_steer_rate_lim = [&](const double curvature, const double velocity) { - if (is_vehicle_stopped) { - return std::numeric_limits::max(); - } - - double steer_rate_lim_by_curvature = m_steer_rate_lim_map_by_curvature.back().second; - for (const auto & steer_rate_lim_info : m_steer_rate_lim_map_by_curvature) { - if (std::abs(curvature) <= steer_rate_lim_info.first) { - steer_rate_lim_by_curvature = steer_rate_lim_info.second; - break; - } - } - - double steer_rate_lim_by_velocity = m_steer_rate_lim_map_by_velocity.back().second; - for (const auto & steer_rate_lim_info : m_steer_rate_lim_map_by_velocity) { - if (std::abs(velocity) <= steer_rate_lim_info.first) { - steer_rate_lim_by_velocity = steer_rate_lim_info.second; - break; - } - } - - return std::min(steer_rate_lim_by_curvature, steer_rate_lim_by_velocity); - }; - + // steering angle limit VectorXd lb = VectorXd::Constant(DIM_U_N, -m_steer_lim); // min steering angle VectorXd ub = VectorXd::Constant(DIM_U_N, m_steer_lim); // max steering angle - VectorXd lbA(DIM_U_N); - VectorXd ubA(DIM_U_N); - for (int i = 0; i < DIM_U_N; ++i) { - const double adaptive_steer_rate_lim = - get_adaptive_steer_rate_lim(traj.smooth_k.at(i), traj.vx.at(i)); - const double adaptive_delta_steer_lim = adaptive_steer_rate_lim * prediction_dt; - lbA(i) = -adaptive_delta_steer_lim; - ubA(i) = adaptive_delta_steer_lim; - } - const double adaptive_steer_rate_lim = - get_adaptive_steer_rate_lim(traj.smooth_k.at(0), traj.vx.at(0)); - lbA(0, 0) = m_raw_steer_cmd_prev - adaptive_steer_rate_lim * m_ctrl_period; - ubA(0, 0) = m_raw_steer_cmd_prev + adaptive_steer_rate_lim * m_ctrl_period; + // steering angle rate limit + VectorXd steer_rate_limits = calcSteerRateLimitOnTrajectory(traj, current_velocity); + VectorXd ubA = steer_rate_limits * prediction_dt; + VectorXd lbA = -steer_rate_limits * prediction_dt; + ubA(0) = m_raw_steer_cmd_prev + steer_rate_limits(0) * m_ctrl_period; + lbA(0) = m_raw_steer_cmd_prev - steer_rate_limits(0) * m_ctrl_period; auto t_start = std::chrono::system_clock::now(); bool solve_result = m_qpsolver_ptr->solve(H, f.transpose(), A, lb, ub, lbA, ubA, Uex); @@ -761,6 +727,58 @@ double MPC::calcDesiredSteeringRate( return steer_rate; } +VectorXd MPC::calcSteerRateLimitOnTrajectory( + const MPCTrajectory & trajectory, const double current_velocity) const +{ + const auto interp = [&](const auto & steer_rate_limit_map, const auto & current) { + std::vector reference, limits; + for (const auto & p : steer_rate_limit_map) { + reference.push_back(p.first); + limits.push_back(p.second); + } + + // If the speed is out of range of the reference, apply zero-order hold. + if (current <= reference.front()) { + return limits.front(); + } + if (current >= reference.back()) { + return limits.back(); + } + + // Apply linear interpolation + for (size_t i = 0; i < reference.size() - 1; ++i) { + if (reference.at(i) <= current && current <= reference.at(i + 1)) { + auto ratio = + (current - reference.at(i)) / std::max(reference.at(i + 1) - reference.at(i), 1.0e-5); + ratio = std::clamp(ratio, 0.0, 1.0); + const auto interp = limits.at(i) + ratio * (limits.at(i + 1) - limits.at(i)); + return interp; + } + } + + std::cerr << "MPC::calcSteerRateLimitOnTrajectory() interpolation logic is broken. Command " + "filter is not working. Please check the code." + << std::endl; + return reference.back(); + }; + + // when the vehicle is stopped, no steering rate limit. + const bool is_vehicle_stopped = std::fabs(current_velocity) < 0.01; + if (is_vehicle_stopped) { + return VectorXd::Zero(m_param.prediction_horizon); + } + + // calculate steering rate limit + VectorXd steer_rate_limits = VectorXd::Zero(m_param.prediction_horizon); + for (int i = 0; i < m_param.prediction_horizon; ++i) { + const auto limit_by_curvature = interp(m_steer_rate_lim_map_by_curvature, trajectory.k.at(i)); + const auto limit_by_velocity = interp(m_steer_rate_lim_map_by_velocity, trajectory.vx.at(i)); + steer_rate_limits(i) = std::min(limit_by_curvature, limit_by_velocity); + } + + return steer_rate_limits; +} + bool MPC::isValid(const MPCMatrix & m) const { if ( 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 dba803e8f5efc..7c76d67e75aa3 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 @@ -72,12 +72,10 @@ bool QPSolverOSQP::solve( // polish status: successful (1), unperformed (0), (-1) unsuccessful int status_polish = std::get<2>(result); - if (status_polish == -1) { - RCLCPP_WARN(logger_, "osqp status_polish = %d (unsuccessful)", status_polish); - return false; - } - if (status_polish == 0) { - RCLCPP_WARN(logger_, "osqp status_polish = %d (unperformed)", status_polish); + if (status_polish == -1 || status_polish == 0) { + const auto s = (status_polish == 0) ? "Polish process is not performed in osqp." + : "Polish process failed in osqp."; + RCLCPP_INFO(logger_, "%s The required accuracy is met, but the solution can be inaccurate.", s); return true; } return true; diff --git a/control/mpc_lateral_controller/test/test_mpc.cpp b/control/mpc_lateral_controller/test/test_mpc.cpp index 6066cd3419ef3..6f8a6fb598058 100644 --- a/control/mpc_lateral_controller/test/test_mpc.cpp +++ b/control/mpc_lateral_controller/test/test_mpc.cpp @@ -252,9 +252,8 @@ TEST_F(MPCTest, OsqpCalculate) AckermannLateralCommand ctrl_cmd; Trajectory pred_traj; Float32MultiArrayStamped diag; - // with OSQP this function returns false despite finding correct solutions const auto odom = makeOdometry(pose_zero, default_velocity); - EXPECT_FALSE(mpc.calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); + EXPECT_TRUE(mpc.calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); EXPECT_EQ(ctrl_cmd.steering_tire_angle, 0.0f); EXPECT_EQ(ctrl_cmd.steering_tire_rotation_rate, 0.0f); } diff --git a/launch/tier4_map_launch/launch/map.launch.py b/launch/tier4_map_launch/launch/map.launch.py index 169c53814df22..23bd2fc337c97 100644 --- a/launch/tier4_map_launch/launch/map.launch.py +++ b/launch/tier4_map_launch/launch/map.launch.py @@ -64,7 +64,6 @@ def launch_setup(context, *args, **kwargs): name="lanelet2_map_loader", remappings=[ ("output/lanelet2_map", "vector_map"), - ("input/map_projector_info", "map_projector_type"), ], parameters=[ { diff --git a/localization/ekf_localizer/config/ekf_localizer.param.yaml b/localization/ekf_localizer/config/ekf_localizer.param.yaml index 322325d239004..4d3f5b9643462 100644 --- a/localization/ekf_localizer/config/ekf_localizer.param.yaml +++ b/localization/ekf_localizer/config/ekf_localizer.param.yaml @@ -1,7 +1,7 @@ /**: ros__parameters: show_debug_info: false - enable_yaw_bias_estimation: True + enable_yaw_bias_estimation: true predict_frequency: 50.0 tf_rate: 50.0 extend_state_step: 50 diff --git a/map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp b/map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp index 2fb8b893d8fce..f422d3628fe2c 100644 --- a/map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp +++ b/map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp @@ -15,6 +15,8 @@ #ifndef MAP_LOADER__LANELET2_MAP_LOADER_NODE_HPP_ #define MAP_LOADER__LANELET2_MAP_LOADER_NODE_HPP_ +#include +#include #include #include @@ -41,9 +43,11 @@ class Lanelet2MapLoaderNode : public rclcpp::Node const rclcpp::Time & now); private: - void on_map_projector_info(const MapProjectorInfo::ConstSharedPtr msg); + using MapProjectorInfo = map_interface::MapProjectorInfo; - rclcpp::Subscription::SharedPtr sub_map_projector_type_; + void on_map_projector_info(const MapProjectorInfo::Message::ConstSharedPtr msg); + + component_interface_utils::Subscription::SharedPtr sub_map_projector_type_; rclcpp::Publisher::SharedPtr pub_map_bin_; }; diff --git a/map/map_loader/package.xml b/map/map_loader/package.xml index 93b00cd412147..fbc2572d0bc6d 100644 --- a/map/map_loader/package.xml +++ b/map/map_loader/package.xml @@ -16,6 +16,8 @@ autoware_auto_mapping_msgs autoware_map_msgs + component_interface_specs + component_interface_utils fmt geometry_msgs lanelet2_extension diff --git a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp index 42c46e2b5e96c..c1272c5893689 100644 --- a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp +++ b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp @@ -51,16 +51,22 @@ Lanelet2MapLoaderNode::Lanelet2MapLoaderNode(const rclcpp::NodeOptions & options) : Node("lanelet2_map_loader", options) { + const auto adaptor = component_interface_utils::NodeAdaptor(this); + // subscription - sub_map_projector_type_ = create_subscription( - "input/map_projector_info", rclcpp::QoS{1}.transient_local(), - [this](const MapProjectorInfo::ConstSharedPtr msg) { on_map_projector_info(msg); }); + adaptor.init_sub( + sub_map_projector_type_, + [this](const MapProjectorInfo::Message::ConstSharedPtr msg) { on_map_projector_info(msg); }); + + declare_parameter("lanelet2_map_path", ""); + declare_parameter("center_line_resolution", 5.0); } -void Lanelet2MapLoaderNode::on_map_projector_info(const MapProjectorInfo::ConstSharedPtr msg) +void Lanelet2MapLoaderNode::on_map_projector_info( + const MapProjectorInfo::Message::ConstSharedPtr msg) { - const auto lanelet2_filename = declare_parameter("lanelet2_map_path", ""); - const auto center_line_resolution = declare_parameter("center_line_resolution", 5.0); + const auto lanelet2_filename = get_parameter("lanelet2_map_path").as_string(); + const auto center_line_resolution = get_parameter("center_line_resolution").as_double(); // load map from file const auto map = diff --git a/map/map_projection_loader/include/map_projection_loader/map_projection_loader.hpp b/map/map_projection_loader/include/map_projection_loader/map_projection_loader.hpp index de19ac5dabda4..16442eb43c740 100644 --- a/map/map_projection_loader/include/map_projection_loader/map_projection_loader.hpp +++ b/map/map_projection_loader/include/map_projection_loader/map_projection_loader.hpp @@ -17,7 +17,8 @@ #include "rclcpp/rclcpp.hpp" -#include "tier4_map_msgs/msg/map_projector_info.hpp" +#include +#include #include @@ -29,7 +30,8 @@ class MapProjectionLoader : public rclcpp::Node MapProjectionLoader(); private: - rclcpp::Publisher::SharedPtr publisher_; + using MapProjectorInfo = map_interface::MapProjectorInfo; + component_interface_utils::Publisher::SharedPtr publisher_; }; #endif // MAP_PROJECTION_LOADER__MAP_PROJECTION_LOADER_HPP_ diff --git a/map/map_projection_loader/launch/map_projection_loader.launch.xml b/map/map_projection_loader/launch/map_projection_loader.launch.xml index 6448675c4a75f..fc625e3162911 100644 --- a/map/map_projection_loader/launch/map_projection_loader.launch.xml +++ b/map/map_projection_loader/launch/map_projection_loader.launch.xml @@ -3,7 +3,6 @@ - diff --git a/map/map_projection_loader/package.xml b/map/map_projection_loader/package.xml index 84f66583ed840..823cea064515b 100644 --- a/map/map_projection_loader/package.xml +++ b/map/map_projection_loader/package.xml @@ -12,6 +12,8 @@ ament_cmake_auto autoware_cmake + component_interface_specs + component_interface_utils lanelet2_extension rclcpp rclcpp_components diff --git a/map/map_projection_loader/src/map_projection_loader.cpp b/map/map_projection_loader/src/map_projection_loader.cpp index ccea8d7417cbc..6d1459c628e35 100644 --- a/map/map_projection_loader/src/map_projection_loader.cpp +++ b/map/map_projection_loader/src/map_projection_loader.cpp @@ -16,6 +16,8 @@ #include "map_projection_loader/load_info_from_lanelet2_map.hpp" +#include + #include #include @@ -64,7 +66,7 @@ MapProjectionLoader::MapProjectionLoader() : Node("map_projection_loader") } // Publish the message - publisher_ = this->create_publisher( - "~/map_projector_info", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local()); + const auto adaptor = component_interface_utils::NodeAdaptor(this); + adaptor.init_pub(publisher_); publisher_->publish(msg); } diff --git a/map/map_projection_loader/test/test_node_load_local_from_yaml.test.py b/map/map_projection_loader/test/test_node_load_local_from_yaml.test.py index aa0e3245565b5..721fd5f95e8f3 100644 --- a/map/map_projection_loader/test/test_node_load_local_from_yaml.test.py +++ b/map/map_projection_loader/test/test_node_load_local_from_yaml.test.py @@ -110,7 +110,7 @@ def test_node_link(self): # Create subscription to map_projector_info topic subscription = self.test_node.create_subscription( MapProjectorInfo, - "/map_projection_loader/map_projector_info", + "/map/map_projector_type", self.callback, custom_qos_profile, ) @@ -125,7 +125,7 @@ def test_node_link(self): get_package_share_directory("map_projection_loader"), YAML_FILE_PATH ) with open(map_projection_info_path) as f: - yaml_data = yaml.load(f) + yaml_data = yaml.load(f, Loader=yaml.FullLoader) # Test if message received self.assertIsNotNone( diff --git a/map/map_projection_loader/test/test_node_load_mgrs_from_yaml.test.py b/map/map_projection_loader/test/test_node_load_mgrs_from_yaml.test.py index ae29b4b06c61c..1805d59d54f65 100644 --- a/map/map_projection_loader/test/test_node_load_mgrs_from_yaml.test.py +++ b/map/map_projection_loader/test/test_node_load_mgrs_from_yaml.test.py @@ -110,7 +110,7 @@ def test_node_link(self): # Create subscription to map_projector_info topic subscription = self.test_node.create_subscription( MapProjectorInfo, - "/map_projection_loader/map_projector_info", + "/map/map_projector_type", self.callback, custom_qos_profile, ) @@ -125,7 +125,7 @@ def test_node_link(self): get_package_share_directory("map_projection_loader"), YAML_FILE_PATH ) with open(map_projection_info_path) as f: - yaml_data = yaml.load(f) + yaml_data = yaml.load(f, Loader=yaml.FullLoader) # Test if message received self.assertIsNotNone( diff --git a/map/map_projection_loader/test/test_node_load_utm_from_yaml.test.py b/map/map_projection_loader/test/test_node_load_utm_from_yaml.test.py index 8da98bbb8b25d..1f92802231e37 100644 --- a/map/map_projection_loader/test/test_node_load_utm_from_yaml.test.py +++ b/map/map_projection_loader/test/test_node_load_utm_from_yaml.test.py @@ -110,7 +110,7 @@ def test_node_link(self): # Create subscription to map_projector_info topic subscription = self.test_node.create_subscription( MapProjectorInfo, - "/map_projection_loader/map_projector_info", + "/map/map_projector_type", self.callback, custom_qos_profile, ) @@ -125,7 +125,7 @@ def test_node_link(self): get_package_share_directory("map_projection_loader"), YAML_FILE_PATH ) with open(map_projection_info_path) as f: - yaml_data = yaml.load(f) + yaml_data = yaml.load(f, Loader=yaml.FullLoader) # Test if message received self.assertIsNotNone( diff --git a/planning/behavior_path_planner/CMakeLists.txt b/planning/behavior_path_planner/CMakeLists.txt index 162dd855a577a..0352096d02b2b 100644 --- a/planning/behavior_path_planner/CMakeLists.txt +++ b/planning/behavior_path_planner/CMakeLists.txt @@ -31,6 +31,7 @@ ament_auto_add_library(behavior_path_planner_node SHARED src/utils/utils.cpp src/utils/path_utils.cpp src/utils/path_safety_checker/safety_check.cpp + src/utils/path_safety_checker/objects_filtering.cpp src/utils/avoidance/utils.cpp src/utils/lane_change/utils.cpp src/utils/side_shift/util.cpp @@ -43,6 +44,7 @@ ament_auto_add_library(behavior_path_planner_node SHARED src/utils/start_planner/util.cpp src/utils/start_planner/shift_pull_out.cpp src/utils/start_planner/geometric_pull_out.cpp + src/utils/start_planner/freespace_pull_out.cpp src/utils/path_shifter/path_shifter.cpp src/utils/drivable_area_expansion/drivable_area_expansion.cpp src/utils/drivable_area_expansion/map_utils.cpp diff --git a/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml b/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml index 60f292744025d..bea78664c65a3 100644 --- a/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml +++ b/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml @@ -36,3 +36,39 @@ th_turn_signal_on_lateral_offset: 1.0 intersection_search_length: 30.0 length_ratio_for_turn_signal_deactivation_near_intersection: 0.5 + # freespace planner + freespace_planner: + enable_freespace_planner: true + end_pose_search_start_distance: 20.0 + end_pose_search_end_distance: 30.0 + end_pose_search_interval: 2.0 + freespace_planner_algorithm: "astar" # options: astar, rrtstar + velocity: 1.0 + vehicle_shape_margin: 1.0 + time_limit: 3000.0 + minimum_turning_radius: 5.0 + maximum_turning_radius: 5.0 + turning_radius_size: 1 + # search configs + search_configs: + theta_size: 144 + angle_goal_range: 6.0 + curve_weight: 1.2 + reverse_weight: 1.0 + lateral_goal_range: 0.5 + longitudinal_goal_range: 2.0 + # costmap configs + costmap_configs: + obstacle_threshold: 30 + # -- A* search Configurations -- + astar: + only_behind_solutions: false + use_back: false + distance_heuristic_weight: 1.0 + # -- RRT* search Configurations -- + rrtstar: + enable_update: true + use_informed_sampling: true + max_planning_time: 150.0 + neighbor_radius: 8.0 + margin: 1.0 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 b2ed205680b45..fcde58756d4be 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 @@ -242,9 +242,8 @@ class AvoidanceModule : public SceneModuleInterface /** * @brief update main avoidance data for avoidance path generation based on latest planner data. - * @return avoidance data. */ - AvoidancePlanningData calcAvoidancePlanningData(DebugData & debug) const; + void fillFundamentalData(AvoidancePlanningData & data, DebugData & debug); /** * @brief fill additional data so that the module judges target objects. @@ -555,7 +554,7 @@ class AvoidanceModule : public SceneModuleInterface helper::avoidance::AvoidanceHelper helper_; - AvoidancePlanningData avoidance_data_; + AvoidancePlanningData avoid_data_; PathShifter path_shifter_; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp index 948a2cc4fbb2a..9a6a12427d783 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp @@ -26,10 +26,10 @@ namespace behavior_path_planner { using autoware_auto_planning_msgs::msg::PathWithLaneId; -using behavior_path_planner::utils::safety_check::ExtendedPredictedObject; -using behavior_path_planner::utils::safety_check::PoseWithVelocityAndPolygonStamped; -using behavior_path_planner::utils::safety_check::PoseWithVelocityStamped; -using behavior_path_planner::utils::safety_check::PredictedPathWithPolygon; +using behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObject; +using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityAndPolygonStamped; +using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped; +using behavior_path_planner::utils::path_safety_checker::PredictedPathWithPolygon; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp index e4e3468b45ff6..40f0dc0289cf0 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp @@ -18,6 +18,7 @@ #include "behavior_path_planner/scene_module/scene_module_interface.hpp" #include "behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp" #include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" +#include "behavior_path_planner/utils/start_planner/freespace_pull_out.hpp" #include "behavior_path_planner/utils/start_planner/geometric_pull_out.hpp" #include "behavior_path_planner/utils/start_planner/pull_out_path.hpp" #include "behavior_path_planner/utils/start_planner/shift_pull_out.hpp" @@ -96,7 +97,9 @@ class StartPlannerModule : public SceneModuleInterface { } + // Condition to disable simultaneous execution bool isBackFinished() const { return status_.back_finished; } + bool isFreespacePlanning() const { return status_.planner_type == PlannerType::FREESPACE; } private: bool canTransitSuccessState() override { return false; } @@ -117,7 +120,15 @@ class StartPlannerModule : public SceneModuleInterface std::unique_ptr last_pull_out_start_update_time_; std::unique_ptr last_approved_pose_; - std::shared_ptr getCurrentPlanner() const; + // generate freespace pull out paths in a separate thread + std::unique_ptr freespace_planner_; + rclcpp::TimerBase::SharedPtr freespace_planner_timer_; + rclcpp::CallbackGroup::SharedPtr freespace_planner_timer_cb_group_; + // TODO(kosuke55) + // Currently, we only do lock when updating a member of status_. + // However, we need to ensure that the value does not change when referring to it. + std::mutex mutex_; + PathWithLaneId getFullPath() const; std::vector searchPullOutStartPoses(); @@ -141,7 +152,9 @@ class StartPlannerModule : public SceneModuleInterface bool hasFinishedPullOut() const; void checkBackFinished(); bool isStopped(); + bool isStuck(); bool hasFinishedCurrentPath(); + void setDrivableAreaInfo(BehaviorModuleOutput & output) const; // check if the goal is located behind the ego in the same route segment. bool IsGoalBehindOfEgoInSameRouteSegment() const; @@ -149,6 +162,10 @@ class StartPlannerModule : public SceneModuleInterface // generate BehaviorPathOutput with stopping path and update status BehaviorModuleOutput generateStopOutput(); + // freespace planner + void onFreespacePlannerTimer(); + bool planFreespacePath(); + void setDebugData() const; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp index 27128e92ab971..6b7830147ebf9 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp @@ -17,6 +17,7 @@ #include "behavior_path_planner/data_manager.hpp" #include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp" +#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "behavior_path_planner/utils/path_safety_checker/safety_check.hpp" #include @@ -28,10 +29,10 @@ namespace behavior_path_planner::utils::avoidance { using behavior_path_planner::PlannerData; -using behavior_path_planner::utils::safety_check::ExtendedPredictedObject; -using behavior_path_planner::utils::safety_check::PoseWithVelocityAndPolygonStamped; -using behavior_path_planner::utils::safety_check::PoseWithVelocityStamped; -using behavior_path_planner::utils::safety_check::PredictedPathWithPolygon; +using behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObject; +using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityAndPolygonStamped; +using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped; +using behavior_path_planner::utils::path_safety_checker::PredictedPathWithPolygon; bool isOnRight(const ObjectData & obj); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/freespace_pull_over.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/freespace_pull_over.hpp index 1378be5461a64..b698892907789 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/freespace_pull_over.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/freespace_pull_over.hpp @@ -47,6 +47,7 @@ class FreespacePullOver : public PullOverPlannerBase std::unique_ptr planner_; double velocity_; bool use_back_; + bool left_side_parking_; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp index 7a3e96aa3fa3d..721c5b00f6e33 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp @@ -15,6 +15,7 @@ #define BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__LANE_CHANGE_MODULE_DATA_HPP_ #include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp" +#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "behavior_path_planner/utils/path_safety_checker/safety_check.hpp" #include "lanelet2_core/geometry/Lanelet.h" @@ -132,9 +133,9 @@ struct LaneChangeTargetObjectIndices struct LaneChangeTargetObjects { - std::vector current_lane{}; - std::vector target_lane{}; - std::vector other_lane{}; + std::vector current_lane{}; + std::vector target_lane{}; + std::vector other_lane{}; }; enum class LaneChangeModuleType { diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp index 5fa22a8935bf2..f59e0170bdf14 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp @@ -19,6 +19,7 @@ #include "behavior_path_planner/parameters.hpp" #include "behavior_path_planner/utils/lane_change/lane_change_module_data.hpp" #include "behavior_path_planner/utils/lane_change/lane_change_path.hpp" +#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "behavior_path_planner/utils/path_safety_checker/safety_check.hpp" #include "behavior_path_planner/utils/utils.hpp" @@ -43,10 +44,10 @@ using autoware_auto_perception_msgs::msg::PredictedObject; using autoware_auto_perception_msgs::msg::PredictedObjects; using autoware_auto_perception_msgs::msg::PredictedPath; using autoware_auto_planning_msgs::msg::PathWithLaneId; -using behavior_path_planner::utils::safety_check::ExtendedPredictedObject; -using behavior_path_planner::utils::safety_check::PoseWithVelocityAndPolygonStamped; -using behavior_path_planner::utils::safety_check::PoseWithVelocityStamped; -using behavior_path_planner::utils::safety_check::PredictedPathWithPolygon; +using behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObject; +using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityAndPolygonStamped; +using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped; +using behavior_path_planner::utils::path_safety_checker::PredictedPathWithPolygon; using data::lane_change::PathSafetyStatus; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp new file mode 100644 index 0000000000000..b4575eb4d3b7e --- /dev/null +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp @@ -0,0 +1,179 @@ +// Copyright 2023 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. + +#ifndef BEHAVIOR_PATH_PLANNER__UTILS__PATH_SAFETY_CHECKER__OBJECTS_FILTERING_HPP_ +#define BEHAVIOR_PATH_PLANNER__UTILS__PATH_SAFETY_CHECKER__OBJECTS_FILTERING_HPP_ + +#include "behavior_path_planner/data_manager.hpp" +#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" + +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include + +namespace behavior_path_planner::utils::path_safety_checker +{ + +using autoware_auto_perception_msgs::msg::PredictedObject; +using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_auto_planning_msgs::msg::PathPointWithLaneId; + +/** + * @brief Filters objects based on various criteria. + * + * @param objects The predicted objects to filter. + * @param route_handler + * @param current_lanes + * @param current_pose The current pose of ego vehicle. + * @param params The filtering parameters. + * @return PredictedObjects The filtered objects. + */ +PredictedObjects filterObjects( + const std::shared_ptr & objects, + const std::shared_ptr & route_handler, const lanelet::ConstLanelets & current_lanes, + const geometry_msgs::msg::Point & current_pose, + const std::shared_ptr & params); + +/** + * @brief Filter objects based on their velocity. + * + * @param objects The predicted objects to filter. + * @param lim_v Velocity limit for filtering. + * @return PredictedObjects The filtered objects. + */ +PredictedObjects filterObjectsByVelocity(const PredictedObjects & objects, double lim_v); + +/** + * @brief Filter objects based on a velocity range. + * + * @param objects The predicted objects to filter. + * @param min_v Minimum velocity for filtering. + * @param max_v Maximum velocity for filtering. + * @return PredictedObjects The filtered objects. + */ +PredictedObjects filterObjectsByVelocity( + const PredictedObjects & objects, double min_v, double max_v); + +/** + * @brief Filter objects based on their position relative to a current_pose. + * + * @param objects The predicted objects to filter. + * @param path_points Points on the path. + * @param current_pose Current pose of the reference (e.g., ego vehicle). + * @param forward_distance Maximum forward distance for filtering. + * @param backward_distance Maximum backward distance for filtering. + */ +void filterObjectsByPosition( + PredictedObjects & objects, const std::vector & path_points, + const geometry_msgs::msg::Point & current_pose, const double forward_distance, + const double backward_distance); +/** + * @brief Filters the provided objects based on their classification. + * + * @param objects The predicted objects to be filtered. + * @param target_object_types The types of objects to retain after filtering. + */ +void filterObjectsByClass( + PredictedObjects & objects, const ObjectTypesToCheck & target_object_types); + +/** + * @brief Separate index of the obstacles into two part based on whether the object is within + * lanelet. + * @return Indices of objects pair. first objects are in the lanelet, and second others are out of + * lanelet. + */ +std::pair, std::vector> separateObjectIndicesByLanelets( + const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets); + +/** + * @brief Separate the objects into two part based on whether the object is within lanelet. + * @return Objects pair. first objects are in the lanelet, and second others are out of lanelet. + */ +std::pair separateObjectsByLanelets( + const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets); + +/** + * @brief Get the predicted path from an object. + * + * @param obj The extended predicted object. + * @param is_use_all_predicted_path Flag to determine whether to use all predicted paths or just the + * one with maximum confidence. + * @return std::vector The predicted path(s) from the object. + */ +std::vector getPredictedPathFromObj( + const ExtendedPredictedObject & obj, const bool & is_use_all_predicted_path); + +/** + * @brief Create a predicted path based on ego vehicle parameters. + * + * @param ego_predicted_path_params Parameters for ego's predicted path. + * @param path_points Points on the path. + * @param vehicle_pose Current pose of the ego-vehicle. + * @param current_velocity Current velocity of the vehicle. + * @param ego_seg_idx Index of the ego segment. + * @return std::vector The predicted path. + */ +std::vector createPredictedPath( + const std::shared_ptr & ego_predicted_path_params, + const std::vector & path_points, + const geometry_msgs::msg::Pose & vehicle_pose, const double current_velocity, size_t ego_seg_idx); + +/** + * @brief Checks if the centroid of a given object is within the provided lanelets. + * + * @param object The predicted object to check. + * @param target_lanelets The lanelets to check against. + * @return bool True if the object's centroid is within the lanelets, otherwise false. + */ +bool isCentroidWithinLanelets( + const PredictedObject & object, const lanelet::ConstLanelets & target_lanelets); + +/** + * @brief Transforms a given object into an extended predicted object. + * + * @param object The predicted object to transform. + * @param safety_check_time_horizon The time horizon for safety checks. + * @param safety_check_time_resolution The time resolution for safety checks. + * @return ExtendedPredictedObject The transformed object. + */ +ExtendedPredictedObject transform( + const PredictedObject & object, const double safety_check_time_horizon, + const double safety_check_time_resolution); + +/** + * @brief Creates target objects on a lane based on provided parameters. + * + * @param current_lanes + * @param route_handler + * @param filtered_objects The filtered objects. + * @param params The filtering parameters. + * @return TargetObjectsOnLane The target objects on the lane. + */ +TargetObjectsOnLane createTargetObjectsOnLane( + const lanelet::ConstLanelets & current_lanes, const std::shared_ptr & route_handler, + const PredictedObjects & filtered_objects, + const std::shared_ptr & params); + +} // namespace behavior_path_planner::utils::path_safety_checker + +#endif // BEHAVIOR_PATH_PLANNER__UTILS__PATH_SAFETY_CHECKER__OBJECTS_FILTERING_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp new file mode 100644 index 0000000000000..519f589f561ee --- /dev/null +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp @@ -0,0 +1,174 @@ +// Copyright 2023 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. + +#ifndef BEHAVIOR_PATH_PLANNER__UTILS__PATH_SAFETY_CHECKER__PATH_SAFETY_CHECKER_PARAMETERS_HPP_ +#define BEHAVIOR_PATH_PLANNER__UTILS__PATH_SAFETY_CHECKER__PATH_SAFETY_CHECKER_PARAMETERS_HPP_ + +#include + +#include +#include + +#include + +namespace behavior_path_planner::utils::path_safety_checker +{ + +using geometry_msgs::msg::Pose; +using tier4_autoware_utils::Polygon2d; + +struct PoseWithVelocity +{ + Pose pose; + double velocity{0.0}; + + PoseWithVelocity(const Pose & pose, const double velocity) : pose(pose), velocity(velocity) {} +}; + +struct PoseWithVelocityStamped : public PoseWithVelocity +{ + double time{0.0}; + + PoseWithVelocityStamped(const double time, const Pose & pose, const double velocity) + : PoseWithVelocity(pose, velocity), time(time) + { + } +}; + +struct PoseWithVelocityAndPolygonStamped : public PoseWithVelocityStamped +{ + Polygon2d poly; + + PoseWithVelocityAndPolygonStamped( + const double time, const Pose & pose, const double velocity, const Polygon2d & poly) + : PoseWithVelocityStamped(time, pose, velocity), poly(poly) + { + } +}; + +struct PredictedPathWithPolygon +{ + float confidence{0.0}; + std::vector path; +}; + +struct ExtendedPredictedObject +{ + unique_identifier_msgs::msg::UUID uuid; + geometry_msgs::msg::PoseWithCovariance initial_pose; + geometry_msgs::msg::TwistWithCovariance initial_twist; + geometry_msgs::msg::AccelWithCovariance initial_acceleration; + autoware_auto_perception_msgs::msg::Shape shape; + std::vector predicted_paths; +}; + +/** + * @brief Specifies which object class should be checked. + */ +struct ObjectTypesToCheck +{ + bool check_car{true}; ///< Check for cars. + bool check_truck{true}; ///< Check for trucks. + bool check_bus{true}; ///< Check for buses. + bool check_trailer{true}; ///< Check for trailers. + bool check_unknown{true}; ///< Check for unknown object types. + bool check_bicycle{true}; ///< Check for bicycles. + bool check_motorcycle{true}; ///< Check for motorcycles. + bool check_pedestrian{true}; ///< Check for pedestrians. +}; + +/** + * @brief Configuration for which lanes should be checked for objects. + */ +struct ObjectLaneConfiguration +{ + bool check_current_lane{}; ///< Check the current lane. + bool check_right_lane{}; ///< Check the right lane. + bool check_left_lane{}; ///< Check the left lane. + bool check_shoulder_lane{}; ///< Check the shoulder lane. + bool check_other_lane{}; ///< Check other lanes. +}; + +/** + * @brief Contains objects on lanes type. + */ +struct TargetObjectsOnLane +{ + std::vector on_current_lane{}; ///< Objects on the current lane. + std::vector on_right_lane{}; ///< Objects on the right lane. + std::vector on_left_lane{}; ///< Objects on the left lane. + std::vector on_shoulder_lane{}; ///< Objects on the shoulder lane. + std::vector on_other_lane{}; ///< Objects on other lanes. +}; + +/** + * @brief Parameters related to the RSS (Responsibility-Sensitive Safety) model. + */ +struct RSSparams +{ + double rear_vehicle_reaction_time{0.0}; ///< Reaction time of the rear vehicle. + double rear_vehicle_safety_time_margin{0.0}; ///< Safety time margin for the rear vehicle. + double lateral_distance_max_threshold{0.0}; ///< Maximum threshold for lateral distance. + double longitudinal_distance_min_threshold{ + 0.0}; ///< Minimum threshold for longitudinal distance. + double longitudinal_velocity_delta_time{0.0}; ///< Delta time for longitudinal velocity. +}; + +/** + * @brief Parameters for generating the ego vehicle's predicted path. + */ +struct EgoPredictedPathParams +{ + double acceleration; ///< Acceleration value. + double time_horizon; ///< Time horizon for prediction. + double time_resolution; ///< Time resolution for prediction. + double min_slow_speed; ///< Minimum slow speed. + double delay_until_departure; ///< Delay before departure. + double target_velocity; ///< Target velocity. +}; + +/** + * @brief Parameters for filtering objects. + */ +struct ObjectsFilteringParams +{ + double safety_check_time_horizon; ///< Time horizon for object's prediction. + double safety_check_time_resolution; ///< Time resolution for object's prediction. + double object_check_forward_distance; ///< Forward distance for object checks. + double object_check_backward_distance; ///< Backward distance for object checks. + double ignore_object_velocity_threshold; ///< Velocity threshold for ignoring objects. + ObjectTypesToCheck object_types_to_check; ///< Specifies which object types to check. + ObjectLaneConfiguration object_lane_configuration; ///< Configuration for which lanes to check. + bool include_opposite_lane; ///< Include the opposite lane in checks. + bool invert_opposite_lane; ///< Invert the opposite lane in checks. + bool check_all_predicted_path; ///< Check all predicted paths. + bool use_all_predicted_path; ///< Use all predicted paths. + bool use_predicted_path_outside_lanelet; ///< Use predicted paths outside of lanelets. +}; + +/** + * @brief Parameters for safety checks. + */ +struct SafetyCheckParams +{ + bool enable_safety_check; ///< Enable safety checks. + double backward_lane_length; ///< Length of the backward lane for path generation. + double forward_path_length; ///< Length of the forward path lane for path generation. + RSSparams rss_params; ///< Parameters related to the RSS model. + bool publish_debug_marker{false}; ///< Option to publish debug markers. +}; + +} // namespace behavior_path_planner::utils::path_safety_checker + +#endif // BEHAVIOR_PATH_PLANNER__UTILS__PATH_SAFETY_CHECKER__PATH_SAFETY_CHECKER_PARAMETERS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp index 55bac83442e84..edfec6a7db579 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp @@ -17,6 +17,7 @@ #include "behavior_path_planner/data_manager.hpp" #include "behavior_path_planner/marker_utils/utils.hpp" +#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include @@ -36,7 +37,7 @@ #include #include -namespace behavior_path_planner::utils::safety_check +namespace behavior_path_planner::utils::path_safety_checker { using autoware_auto_perception_msgs::msg::PredictedObject; @@ -49,51 +50,6 @@ using tier4_autoware_utils::Point2d; using tier4_autoware_utils::Polygon2d; using vehicle_info_util::VehicleInfo; -struct PoseWithVelocity -{ - Pose pose; - double velocity{0.0}; - - PoseWithVelocity(const Pose & pose, const double velocity) : pose(pose), velocity(velocity) {} -}; - -struct PoseWithVelocityStamped : public PoseWithVelocity -{ - double time{0.0}; - - PoseWithVelocityStamped(const double time, const Pose & pose, const double velocity) - : PoseWithVelocity(pose, velocity), time(time) - { - } -}; - -struct PoseWithVelocityAndPolygonStamped : public PoseWithVelocityStamped -{ - Polygon2d poly; - - PoseWithVelocityAndPolygonStamped( - const double time, const Pose & pose, const double velocity, const Polygon2d & poly) - : PoseWithVelocityStamped(time, pose, velocity), poly(poly) - { - } -}; - -struct PredictedPathWithPolygon -{ - float confidence{0.0}; - std::vector path; -}; - -struct ExtendedPredictedObject -{ - unique_identifier_msgs::msg::UUID uuid; - geometry_msgs::msg::PoseWithCovariance initial_pose; - geometry_msgs::msg::TwistWithCovariance initial_twist; - geometry_msgs::msg::AccelWithCovariance initial_acceleration; - autoware_auto_perception_msgs::msg::Shape shape; - std::vector predicted_paths; -}; - namespace bg = boost::geometry; bool isTargetObjectFront( @@ -145,6 +101,6 @@ bool checkCollision( const BehaviorPathPlannerParameters & common_parameters, const double front_object_deceleration, const double rear_object_deceleration, CollisionCheckDebug & debug); -} // namespace behavior_path_planner::utils::safety_check +} // namespace behavior_path_planner::utils::path_safety_checker #endif // BEHAVIOR_PATH_PLANNER__UTILS__PATH_SAFETY_CHECKER__SAFETY_CHECK_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_utils.hpp index bdca6a2fdf085..191cd3e10def5 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_utils.hpp @@ -75,7 +75,8 @@ std::pair getPathTurnSignal( const BehaviorPathPlannerParameters & common_parameter); PathWithLaneId convertWayPointsToPathWithLaneId( - const freespace_planning_algorithms::PlannerWaypoints & waypoints, const double velocity); + const freespace_planning_algorithms::PlannerWaypoints & waypoints, const double velocity, + const lanelet::ConstLanelets & lanelets); std::vector getReversingIndices(const PathWithLaneId & path); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/freespace_pull_out.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/freespace_pull_out.hpp new file mode 100644 index 0000000000000..abdf17c9c6cfe --- /dev/null +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/freespace_pull_out.hpp @@ -0,0 +1,53 @@ +// Copyright 2023 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. + +#ifndef BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__FREESPACE_PULL_OUT_HPP_ +#define BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__FREESPACE_PULL_OUT_HPP_ + +#include "behavior_path_planner/utils/start_planner/pull_out_planner_base.hpp" + +#include +#include +#include + +#include + +#include +#include + +namespace behavior_path_planner +{ +using freespace_planning_algorithms::AbstractPlanningAlgorithm; +using freespace_planning_algorithms::AstarSearch; +using freespace_planning_algorithms::RRTStar; + +class FreespacePullOut : public PullOutPlannerBase +{ +public: + FreespacePullOut( + rclcpp::Node & node, const StartPlannerParameters & parameters, + const vehicle_info_util::VehicleInfo & vehicle_info); + + PlannerType getPlannerType() override { return PlannerType::FREESPACE; } + + boost::optional plan(Pose start_pose, Pose end_pose) override; + +protected: + std::unique_ptr planner_; + double velocity_; + bool use_back_; +}; +} // namespace behavior_path_planner + +#endif // BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__FREESPACE_PULL_OUT_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_planner_base.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_planner_base.hpp index 3a2087083b427..6e6e5111e5dd9 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_planner_base.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_planner_base.hpp @@ -40,6 +40,7 @@ enum class PlannerType { SHIFT = 1, GEOMETRIC = 2, STOP = 3, + FREESPACE = 4, }; class PullOutPlannerBase diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp index 9abad1a2fbec6..1e26bef0c85be 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp @@ -18,11 +18,20 @@ #include "behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp" +#include +#include +#include + #include #include namespace behavior_path_planner { + +using freespace_planning_algorithms::AstarParam; +using freespace_planning_algorithms::PlannerCommonParam; +using freespace_planning_algorithms::RRTStarParam; + struct StartPlannerParameters { double th_arrived_distance; @@ -56,6 +65,17 @@ struct StartPlannerParameters double backward_search_resolution; double backward_path_update_duration; double ignore_distance_from_lane_end; + // freespace planner + bool enable_freespace_planner; + std::string freespace_planner_algorithm; + double end_pose_search_start_distance; + double end_pose_search_end_distance; + double end_pose_search_interval; + double freespace_planner_velocity; + double vehicle_shape_margin; + PlannerCommonParam freespace_planner_common_parameters; + AstarParam astar_parameters; + RRTStarParam rrt_star_parameters; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp index 088a0f0bd795b..d27d9ebbdadec 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp @@ -19,6 +19,7 @@ #include "behavior_path_planner/marker_utils/utils.hpp" #include "behavior_path_planner/utils/lane_change/lane_change_module_data.hpp" #include "behavior_path_planner/utils/lane_following/module_data.hpp" +#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "behavior_path_planner/utils/path_safety_checker/safety_check.hpp" #include "behavior_path_planner/utils/start_planner/pull_out_path.hpp" #include "motion_utils/motion_utils.hpp" @@ -66,16 +67,19 @@ using autoware_auto_perception_msgs::msg::Shape; using autoware_auto_planning_msgs::msg::Path; using autoware_auto_planning_msgs::msg::PathPointWithLaneId; using autoware_auto_planning_msgs::msg::PathWithLaneId; -using behavior_path_planner::utils::safety_check::ExtendedPredictedObject; -using behavior_path_planner::utils::safety_check::PoseWithVelocityAndPolygonStamped; -using behavior_path_planner::utils::safety_check::PoseWithVelocityStamped; -using behavior_path_planner::utils::safety_check::PredictedPathWithPolygon; using drivable_area_expansion::DrivableAreaExpansionParameters; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::PoseArray; using geometry_msgs::msg::Twist; using geometry_msgs::msg::Vector3; +using path_safety_checker::ExtendedPredictedObject; +using path_safety_checker::ObjectTypesToCheck; +using path_safety_checker::PoseWithVelocityAndPolygonStamped; +using path_safety_checker::PoseWithVelocityStamped; +using path_safety_checker::PredictedPathWithPolygon; +using path_safety_checker::SafetyCheckParams; +using path_safety_checker::TargetObjectsOnLane; using route_handler::RouteHandler; using tier4_autoware_utils::LinearRing2d; using tier4_autoware_utils::LineString2d; @@ -199,27 +203,6 @@ double calcLongitudinalDistanceFromEgoToObjects( const Pose & ego_pose, double base_link2front, double base_link2rear, const PredictedObjects & dynamic_objects); -/** - * @brief Separate index of the obstacles into two part based on whether the object is within - * lanelet. - * @return Indices of objects pair. first objects are in the lanelet, and second others are out of - * lanelet. - */ -std::pair, std::vector> separateObjectIndicesByLanelets( - const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets); - -/** - * @brief Separate the objects into two part based on whether the object is within lanelet. - * @return Objects pair. first objects are in the lanelet, and second others are out of lanelet. - */ -std::pair separateObjectsByLanelets( - const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets); - -PredictedObjects filterObjectsByVelocity(const PredictedObjects & objects, double lim_v); - -PredictedObjects filterObjectsByVelocity( - const PredictedObjects & objects, double min_v, double max_v); - // drivable area generation lanelet::ConstLanelets transformToLanelets(const DrivableLanes & drivable_lanes); lanelet::ConstLanelets transformToLanelets(const std::vector & drivable_lanes); @@ -391,9 +374,6 @@ lanelet::ConstLanelets calcLaneAroundPose( const double dist_threshold = std::numeric_limits::max(), const double yaw_threshold = std::numeric_limits::max()); -std::vector getPredictedPathFromObj( - const ExtendedPredictedObject & obj, const bool & is_use_all_predicted_path); - bool checkPathRelativeAngle(const PathWithLaneId & path, const double angle_threshold); double calcMinimumLaneChangeLength( @@ -422,6 +402,9 @@ void makeBoundLongitudinallyMonotonic( std::optional getPolygonByPoint( const std::shared_ptr & route_handler, const lanelet::ConstPoint3d & point, const std::string & polygon_name); + +lanelet::ConstLanelets combineLanelets( + const lanelet::ConstLanelets & base_lanes, const lanelet::ConstLanelets & added_lanes); } // namespace behavior_path_planner::utils #endif // BEHAVIOR_PATH_PLANNER__UTILS__UTILS_HPP_ 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 88bc583345966..3550b2035248a 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 @@ -17,6 +17,7 @@ #include "behavior_path_planner/marker_utils/avoidance/debug.hpp" #include "behavior_path_planner/scene_module/scene_module_visitor.hpp" #include "behavior_path_planner/utils/avoidance/utils.hpp" +#include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/utils.hpp" @@ -116,30 +117,30 @@ bool AvoidanceModule::isExecutionRequested() const DEBUG_PRINT("AVOIDANCE isExecutionRequested"); // Check ego is in preferred lane - updateInfoMarker(avoidance_data_); - updateDebugMarker(avoidance_data_, path_shifter_, debug_data_); + updateInfoMarker(avoid_data_); + updateDebugMarker(avoid_data_, path_shifter_, debug_data_); // there is object that should be avoid. return true. - if (!!avoidance_data_.stop_target_object) { + if (!!avoid_data_.stop_target_object) { return true; } - if (avoidance_data_.unapproved_new_sl.empty()) { + if (avoid_data_.unapproved_new_sl.empty()) { return false; } - return !avoidance_data_.target_objects.empty(); + return !avoid_data_.target_objects.empty(); } bool AvoidanceModule::isExecutionReady() const { DEBUG_PRINT("AVOIDANCE isExecutionReady"); - return avoidance_data_.safe && avoidance_data_.comfortable; + return avoid_data_.safe && avoid_data_.comfortable; } bool AvoidanceModule::canTransitSuccessState() { - const auto & data = avoidance_data_; + const auto & data = avoid_data_; // Change input lane. -> EXIT. if (!isDrivingSameLane(helper_.getPreviousDrivingLanes(), data.current_lanelets)) { @@ -189,73 +190,51 @@ bool AvoidanceModule::canTransitSuccessState() return false; // Keep current state. } -AvoidancePlanningData AvoidanceModule::calcAvoidancePlanningData(DebugData & debug) const +void AvoidanceModule::fillFundamentalData(AvoidancePlanningData & data, DebugData & debug) { - AvoidancePlanningData data; - // reference pose - const auto reference_pose = + data.reference_pose = utils::getUnshiftedEgoPose(getEgoPose(), helper_.getPreviousSplineShiftPath()); - data.reference_pose = reference_pose; - - // special for avoidance: take behind distance upt ot shift-start-point if it exist. - const auto longest_dist_to_shift_line = [&]() { - double max_dist = 0.0; - for (const auto & pnt : path_shifter_.getShiftLines()) { - max_dist = std::max(max_dist, calcDistance2d(getEgoPose(), pnt.start)); - } - for (const auto & sl : registered_raw_shift_lines_) { - max_dist = std::max(max_dist, calcDistance2d(getEgoPose(), sl.start)); - } - return max_dist; - }(); - // center line path (output of this function must have size > 1) - const auto center_path = utils::calcCenterLinePath( - planner_data_, reference_pose, longest_dist_to_shift_line, - *getPreviousModuleOutput().reference_path); - - debug.center_line = center_path; - if (center_path.points.size() < 2) { - RCLCPP_WARN_THROTTLE( - getLogger(), *clock_, 5000, "calcCenterLinePath() must return path which size > 1"); - return data; - } + // lanelet info + data.current_lanelets = utils::avoidance::getCurrentLanesFromPath( + *getPreviousModuleOutput().reference_path, planner_data_); // reference path - data.reference_path_rough = extendBackwardLength(*getPreviousModuleOutput().path); + if (isDrivingSameLane(helper_.getPreviousDrivingLanes(), data.current_lanelets)) { + data.reference_path_rough = extendBackwardLength(*getPreviousModuleOutput().path); + } else { + data.reference_path_rough = *getPreviousModuleOutput().path; + RCLCPP_WARN(getLogger(), "Previous module lane is updated. Don't use latest reference path."); + } + // resampled reference path data.reference_path = utils::resamplePathWithSpline( data.reference_path_rough, parameters_->resample_interval_for_planning); - if (data.reference_path.points.size() < 2) { - // if the resampled path has only 1 point, use original path. - data.reference_path = center_path; - } - - const size_t nearest_segment_index = - findNearestSegmentIndex(data.reference_path.points, data.reference_pose.position); - data.ego_closest_path_index = - std::min(nearest_segment_index + 1, data.reference_path.points.size() - 1); + // closest index + data.ego_closest_path_index = planner_data_->findEgoIndex(data.reference_path.points); // arclength from ego pose (used in many functions) data.arclength_from_ego = utils::calcPathArcLengthArray( data.reference_path, 0, data.reference_path.points.size(), calcSignedArcLength(data.reference_path.points, getEgoPosition(), 0)); - // lanelet info - data.current_lanelets = utils::avoidance::getCurrentLanesFromPath( - *getPreviousModuleOutput().reference_path, planner_data_); - - // keep avoidance state - data.state = avoidance_data_.state; - // target objects for avoidance fillAvoidanceTargetObjects(data, debug); - DEBUG_PRINT("target object size = %lu", data.target_objects.size()); + // lost object compensation + utils::avoidance::updateRegisteredObject(registered_objects_, data.target_objects, parameters_); + utils::avoidance::compensateDetectionLost( + registered_objects_, data.target_objects, data.other_objects); + + // sort object order by longitudinal distance + std::sort(data.target_objects.begin(), data.target_objects.end(), [](auto a, auto b) { + return a.longitudinal < b.longitudinal; + }); - return data; + // set base path + path_shifter_.setPath(data.reference_path); } void AvoidanceModule::fillAvoidanceTargetObjects( @@ -647,7 +626,7 @@ void AvoidanceModule::updateEgoBehavior(const AvoidancePlanningData & data, Shif void AvoidanceModule::updateRegisteredRawShiftLines() { - const auto & data = avoidance_data_; + const auto & data = avoid_data_; utils::avoidance::fillAdditionalInfoFromPoint(data, registered_raw_shift_lines_); @@ -770,7 +749,7 @@ void AvoidanceModule::registerRawShiftLines(const AvoidLineArray & future) const auto old_size = registered_raw_shift_lines_.size(); auto future_with_info = future; - utils::avoidance::fillAdditionalInfoFromPoint(avoidance_data_, future_with_info); + utils::avoidance::fillAdditionalInfoFromPoint(avoid_data_, future_with_info); printShiftLines(future_with_info, "future_with_info"); printShiftLines(registered_raw_shift_lines_, "registered_raw_shift_lines_"); printShiftLines(current_raw_shift_lines_, "current_raw_shift_lines_"); @@ -965,13 +944,13 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects( { const auto offset = object_parameter.safety_buffer_longitudinal + base_link2front; const auto path_front_to_ego = - avoidance_data_.arclength_from_ego.at(avoidance_data_.ego_closest_path_index); + avoid_data_.arclength_from_ego.at(avoid_data_.ego_closest_path_index); al_avoid.start_longitudinal = std::max(o.longitudinal - offset - feasible_avoid_distance, 1e-3); al_avoid.start_idx = utils::avoidance::findPathIndexFromArclength( - avoidance_data_.arclength_from_ego, al_avoid.start_longitudinal + path_front_to_ego); - al_avoid.start = avoidance_data_.reference_path.points.at(al_avoid.start_idx).point.pose; + avoid_data_.arclength_from_ego, al_avoid.start_longitudinal + path_front_to_ego); + al_avoid.start = avoid_data_.reference_path.points.at(al_avoid.start_idx).point.pose; al_avoid.start_shift_length = helper_.getLinearShift(al_avoid.start.position); al_avoid.end_shift_length = feasible_shift_length.get(); @@ -1037,8 +1016,8 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects( void AvoidanceModule::generateTotalShiftLine( const AvoidLineArray & avoid_lines, ShiftLineData & shift_line_data) const { - const auto & path = avoidance_data_.reference_path; - const auto & arcs = avoidance_data_.arclength_from_ego; + const auto & path = avoid_data_.reference_path; + const auto & arcs = avoid_data_.arclength_from_ego; const auto N = path.points.size(); auto & sl = shift_line_data; @@ -1088,7 +1067,7 @@ void AvoidanceModule::generateTotalShiftLine( // overwrite shift with current_ego_shift until ego pose. const auto current_shift = helper_.getEgoLinearShift(); for (size_t i = 0; i < sl.shift_line.size(); ++i) { - if (avoidance_data_.ego_closest_path_index < i) { + if (avoid_data_.ego_closest_path_index < i) { break; } sl.shift_line.at(i) = current_shift; @@ -1118,7 +1097,7 @@ void AvoidanceModule::generateTotalShiftLine( const auto grad_first_shift_line = (avoid_lines.front().start_shift_length - current_shift) / avoid_lines.front().start_longitudinal; - for (size_t i = avoidance_data_.ego_closest_path_index; i <= avoid_lines.front().start_idx; ++i) { + for (size_t i = avoid_data_.ego_closest_path_index; i <= avoid_lines.front().start_idx; ++i) { sl.shift_line.at(i) = helper_.getLinearShift(getPoint(path.points.at(i))); sl.shift_line_grad.at(i) = grad_first_shift_line; } @@ -1131,8 +1110,8 @@ AvoidLineArray AvoidanceModule::extractShiftLinesFromLine(ShiftLineData & shift_ using utils::avoidance::setEndData; using utils::avoidance::setStartData; - const auto & path = avoidance_data_.reference_path; - const auto & arcs = avoidance_data_.arclength_from_ego; + const auto & path = avoid_data_.reference_path; + const auto & arcs = avoid_data_.arclength_from_ego; const auto N = path.points.size(); auto & sl = shift_line_data; @@ -1173,7 +1152,7 @@ AvoidLineArray AvoidanceModule::extractShiftLinesFromLine(ShiftLineData & shift_ bool found_first_start = false; constexpr auto CREATE_SHIFT_GRAD_THR = 0.001; constexpr auto IS_ALREADY_SHIFTING_THR = 0.001; - for (size_t i = avoidance_data_.ego_closest_path_index; i < N - 1; ++i) { + for (size_t i = avoid_data_.ego_closest_path_index; i < N - 1; ++i) { const auto & p = path.points.at(i).point.pose; const auto shift = sl.shift_line.at(i); @@ -1244,7 +1223,7 @@ AvoidLineArray AvoidanceModule::getFillGapShiftLines(const AvoidLineArray & shif ret.push_back(calc_gap_shift_line(shift_lines.at(i), shift_lines.at(i + 1))); } - utils::avoidance::fillAdditionalInfoFromLongitudinal(avoidance_data_, ret); + utils::avoidance::fillAdditionalInfoFromLongitudinal(avoid_data_, ret); return ret; } @@ -1257,7 +1236,7 @@ void AvoidanceModule::fillShiftLineGap(AvoidLineArray & shift_lines) const return; } - const auto & data = avoidance_data_; + const auto & data = avoid_data_; helper_.alignShiftLinesOrder(shift_lines, false); @@ -1327,8 +1306,8 @@ AvoidLineArray AvoidanceModule::mergeShiftLines( // debug print { - const auto & arc = avoidance_data_.arclength_from_ego; - const auto & closest = avoidance_data_.ego_closest_path_index; + const auto & arc = avoid_data_.arclength_from_ego; + const auto & closest = avoid_data_.ego_closest_path_index; const auto & sl = shift_line_data.shift_line; const auto & sg = shift_line_data.shift_line_grad; const auto & fg = shift_line_data.forward_grad; @@ -1667,7 +1646,7 @@ void AvoidanceModule::trimSharpReturn(AvoidLineArray & shift_lines, const double void AvoidanceModule::addReturnShiftLineFromEgo(AvoidLineArray & sl_candidates) const { constexpr double ep = 1.0e-3; - const auto & data = avoidance_data_; + const auto & data = avoid_data_; const bool has_candidate_point = !sl_candidates.empty(); const bool has_registered_point = !path_shifter_.getShiftLines().empty(); @@ -1715,8 +1694,8 @@ void AvoidanceModule::addReturnShiftLineFromEgo(AvoidLineArray & sl_candidates) // avoidance points: No, shift points: No -> set the ego position to the last shift point // so that the return-shift will be generated from ego position. if (!has_candidate_point && !has_registered_point) { - last_sl.end_idx = avoidance_data_.ego_closest_path_index; - last_sl.end = avoidance_data_.reference_path.points.at(last_sl.end_idx).point.pose; + last_sl.end_idx = avoid_data_.ego_closest_path_index; + last_sl.end = avoid_data_.reference_path.points.at(last_sl.end_idx).point.pose; last_sl.end_shift_length = getCurrentBaseShift(); } } @@ -1757,12 +1736,12 @@ void AvoidanceModule::addReturnShiftLineFromEgo(AvoidLineArray & sl_candidates) // set the return-shift from ego. DEBUG_PRINT( "return shift already exists, but they are all candidates. Add return shift for overwrite."); - last_sl.end_idx = avoidance_data_.ego_closest_path_index; - last_sl.end = avoidance_data_.reference_path.points.at(last_sl.end_idx).point.pose; + last_sl.end_idx = avoid_data_.ego_closest_path_index; + last_sl.end = avoid_data_.reference_path.points.at(last_sl.end_idx).point.pose; last_sl.end_shift_length = current_base_shift; } - const auto & arclength_from_ego = avoidance_data_.arclength_from_ego; + const auto & arclength_from_ego = avoid_data_.arclength_from_ego; const auto nominal_prepare_distance = helper_.getNominalPrepareDistance(); const auto nominal_avoid_distance = helper_.getMaxAvoidanceDistance(last_sl.end_shift_length); @@ -1774,7 +1753,7 @@ void AvoidanceModule::addReturnShiftLineFromEgo(AvoidLineArray & sl_candidates) const auto remaining_distance = arclength_from_ego.back() - parameters_->remain_buffer_distance; // If the avoidance point has already been set, the return shift must be set after the point. - const auto last_sl_distance = avoidance_data_.arclength_from_ego.at(last_sl.end_idx); + const auto last_sl_distance = avoid_data_.arclength_from_ego.at(last_sl.end_idx); // check if there is enough distance for return. if (last_sl_distance > remaining_distance) { // tmp: add some small number (+1.0) @@ -1827,7 +1806,7 @@ void AvoidanceModule::addReturnShiftLineFromEgo(AvoidLineArray & sl_candidates) al.start_longitudinal = arclength_from_ego.at(al.start_idx); al.end_idx = utils::avoidance::findPathIndexFromArclength(arclength_from_ego, prepare_distance_scaled); - al.end = avoidance_data_.reference_path.points.at(al.end_idx).point.pose; + al.end = avoid_data_.reference_path.points.at(al.end_idx).point.pose; al.end_longitudinal = arclength_from_ego.at(al.end_idx); al.end_shift_length = last_sl.end_shift_length; al.start_shift_length = last_sl.end_shift_length; @@ -1842,11 +1821,11 @@ void AvoidanceModule::addReturnShiftLineFromEgo(AvoidLineArray & sl_candidates) al.id = getOriginalShiftLineUniqueId(); al.start_idx = utils::avoidance::findPathIndexFromArclength(arclength_from_ego, prepare_distance_scaled); - al.start = avoidance_data_.reference_path.points.at(al.start_idx).point.pose; + al.start = avoid_data_.reference_path.points.at(al.start_idx).point.pose; al.start_longitudinal = arclength_from_ego.at(al.start_idx); al.end_idx = utils::avoidance::findPathIndexFromArclength( arclength_from_ego, prepare_distance_scaled + avoid_distance_scaled); - al.end = avoidance_data_.reference_path.points.at(al.end_idx).point.pose; + al.end = avoid_data_.reference_path.points.at(al.end_idx).point.pose; al.end_longitudinal = arclength_from_ego.at(al.end_idx); al.end_shift_length = 0.0; al.start_shift_length = last_sl.end_shift_length; @@ -1892,14 +1871,14 @@ bool AvoidanceModule::isSafePath( } const auto safety_check_target_objects = utils::avoidance::getSafetyCheckTargetObjects( - avoidance_data_, planner_data_, parameters_, is_right_shift.value()); + avoid_data_, planner_data_, parameters_, is_right_shift.value()); for (const auto & object : safety_check_target_objects) { - const auto obj_predicted_paths = - utils::getPredictedPathFromObj(object, parameters_->check_all_predicted_path); + const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj( + object, parameters_->check_all_predicted_path); for (const auto & obj_path : obj_predicted_paths) { CollisionCheckDebug collision{}; - if (!utils::safety_check::checkCollision( + if (!utils::path_safety_checker::checkCollision( shifted_path.path, ego_predicted_path, object, obj_path, p, p.expected_front_deceleration, p.expected_rear_deceleration, collision)) { return false; @@ -1920,7 +1899,7 @@ void AvoidanceModule::generateExtendedDrivableArea(BehaviorModuleOutput & output }; const auto & route_handler = planner_data_->route_handler; - const auto & current_lanes = avoidance_data_.current_lanelets; + const auto & current_lanes = avoid_data_.current_lanelets; const auto & enable_opposite = parameters_->use_opposite_lane; std::vector drivable_lanes; @@ -2061,7 +2040,7 @@ void AvoidanceModule::generateExtendedDrivableArea(BehaviorModuleOutput & output // generate obstacle polygons current_drivable_area_info.obstacles = utils::avoidance::generateObstaclePolygonsForDrivableArea( - avoidance_data_.target_objects, parameters_, planner_data_->parameters.vehicle_width / 2.0); + avoid_data_.target_objects, parameters_, planner_data_->parameters.vehicle_width / 2.0); // expand hatched road markings current_drivable_area_info.enable_expanding_hatched_road_markings = parameters_->use_hatched_road_markings; @@ -2129,7 +2108,7 @@ PathWithLaneId AvoidanceModule::extendBackwardLength(const PathWithLaneId & orig BehaviorModuleOutput AvoidanceModule::plan() { - const auto & data = avoidance_data_; + const auto & data = avoid_data_; resetPathCandidate(); resetPathReference(); @@ -2177,16 +2156,22 @@ BehaviorModuleOutput AvoidanceModule::plan() spline_shift_path.path, parameters_->resample_interval_for_output); } - avoidance_data_.state = updateEgoState(data); + avoid_data_.state = updateEgoState(data); // update output data { updateEgoBehavior(data, spline_shift_path); - updateInfoMarker(avoidance_data_); - updateDebugMarker(avoidance_data_, path_shifter_, debug_data_); + updateInfoMarker(avoid_data_); + updateDebugMarker(avoid_data_, path_shifter_, debug_data_); + } + + if (isDrivingSameLane(helper_.getPreviousDrivingLanes(), data.current_lanelets)) { + output.path = std::make_shared(spline_shift_path.path); + } else { + output.path = getPreviousModuleOutput().path; + RCLCPP_WARN(getLogger(), "Previous module lane is updated. Do nothing."); } - output.path = std::make_shared(spline_shift_path.path); output.reference_path = getPreviousModuleOutput().reference_path; path_reference_ = getPreviousModuleOutput().reference_path; @@ -2202,7 +2187,7 @@ BehaviorModuleOutput AvoidanceModule::plan() CandidateOutput AvoidanceModule::planCandidate() const { - const auto & data = avoidance_data_; + const auto & data = avoid_data_; CandidateOutput output; @@ -2272,7 +2257,7 @@ void AvoidanceModule::updatePathShifter(const AvoidLineArray & shift_lines) addNewShiftLines(path_shifter_, shift_lines); - current_raw_shift_lines_ = avoidance_data_.unapproved_raw_sl; + current_raw_shift_lines_ = avoid_data_.unapproved_raw_sl; registerRawShiftLines(shift_lines); @@ -2347,9 +2332,8 @@ void AvoidanceModule::addNewShiftLines( future.push_back(sl); } - const double road_velocity = - avoidance_data_.reference_path.points.at(front_new_shift_line.start_idx) - .point.longitudinal_velocity_mps; + const double road_velocity = avoid_data_.reference_path.points.at(front_new_shift_line.start_idx) + .point.longitudinal_velocity_mps; const double shift_time = PathShifter::calcShiftTimeFromJerk( front_new_shift_line.getRelativeLength(), helper_.getLateralMaxJerkLimit(), helper_.getLateralMaxAccelLimit()); @@ -2494,30 +2478,29 @@ void AvoidanceModule::updateData() } debug_data_ = DebugData(); - avoidance_data_ = calcAvoidancePlanningData(debug_data_); - if (avoidance_data_.reference_path.points.empty()) { - // an empty path will kill further processing - return; - } - - utils::avoidance::updateRegisteredObject( - registered_objects_, avoidance_data_.target_objects, parameters_); - utils::avoidance::compensateDetectionLost( - registered_objects_, avoidance_data_.target_objects, avoidance_data_.other_objects); + avoid_data_ = AvoidancePlanningData(); - std::sort( - avoidance_data_.target_objects.begin(), avoidance_data_.target_objects.end(), - [](auto a, auto b) { return a.longitudinal < b.longitudinal; }); + // update base path and target objects. + fillFundamentalData(avoid_data_, debug_data_); - path_shifter_.setPath(avoidance_data_.reference_path); + // an empty path will kill further processing + if (avoid_data_.reference_path.points.empty()) { + return; + } // update registered shift point for new reference path & remove past objects updateRegisteredRawShiftLines(); - fillShiftLine(avoidance_data_, debug_data_); - fillEgoStatus(avoidance_data_, debug_data_); - fillDebugData(avoidance_data_, debug_data_); + // update shift line and check path safety. + fillShiftLine(avoid_data_, debug_data_); + + // update ego behavior. + fillEgoStatus(avoid_data_, debug_data_); + + // update debug data. + fillDebugData(avoid_data_, debug_data_); + // update rtc status. updateRTCData(); } @@ -2559,7 +2542,7 @@ void AvoidanceModule::initRTCStatus() void AvoidanceModule::updateRTCData() { - const auto & data = avoidance_data_; + const auto & data = avoid_data_; updateRegisteredRTCStatus(helper_.getPreviousSplineShiftPath().path); @@ -2879,7 +2862,7 @@ double AvoidanceModule::calcDistanceToStopLine(const ObjectData & object) const void AvoidanceModule::insertWaitPoint( const bool use_constraints_for_decel, ShiftedPath & shifted_path) const { - const auto & data = avoidance_data_; + const auto & data = avoid_data_; if (!data.stop_target_object) { return; @@ -2922,7 +2905,7 @@ void AvoidanceModule::insertWaitPoint( void AvoidanceModule::insertStopPoint( const bool use_constraints_for_decel, ShiftedPath & shifted_path) const { - const auto & data = avoidance_data_; + const auto & data = avoid_data_; if (data.safe) { return; @@ -2970,7 +2953,7 @@ void AvoidanceModule::insertStopPoint( void AvoidanceModule::insertYieldVelocity(ShiftedPath & shifted_path) const { const auto & p = parameters_; - const auto & data = avoidance_data_; + const auto & data = avoid_data_; if (data.target_objects.empty()) { return; @@ -2987,7 +2970,7 @@ void AvoidanceModule::insertYieldVelocity(ShiftedPath & shifted_path) const void AvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_path) const { - const auto & data = avoidance_data_; + const auto & data = avoid_data_; // do nothing if there is no avoidance target. if (data.target_objects.empty()) { diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp index 0db16c4c1d4ca..841fc9092e279 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp @@ -15,6 +15,7 @@ #include "behavior_path_planner/scene_module/lane_change/avoidance_by_lane_change.hpp" #include "behavior_path_planner/utils/avoidance/utils.hpp" +#include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner/utils/path_utils.hpp" #include @@ -136,7 +137,8 @@ void AvoidanceByLaneChange::fillAvoidanceTargetObjects( const auto p = std::dynamic_pointer_cast(avoidance_parameters_); const auto [object_within_target_lane, object_outside_target_lane] = - utils::separateObjectsByLanelets(*planner_data_->dynamic_object, data.current_lanelets); + utils::path_safety_checker::separateObjectsByLanelets( + *planner_data_->dynamic_object, data.current_lanelets); // Assume that the maximum allocation for data.other object is the sum of // objects_within_target_lane and object_outside_target_lane. The maximum allocation for diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index 29cefe110d9e4..77c5e78d3db50 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -16,6 +16,7 @@ #include "behavior_path_planner/scene_module/scene_module_visitor.hpp" #include "behavior_path_planner/utils/lane_change/utils.hpp" +#include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/utils.hpp" @@ -1372,18 +1373,19 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( for (const auto & obj : collision_check_objects) { auto current_debug_data = assignDebugData(obj); current_debug_data.second.ego_predicted_path.push_back(debug_predicted_path); - const auto obj_predicted_paths = - utils::getPredictedPathFromObj(obj, lane_change_parameters_->use_all_predicted_path); + const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj( + obj, lane_change_parameters_->use_all_predicted_path); for (const auto & obj_path : obj_predicted_paths) { - if (!utils::safety_check::checkCollision( + if (!utils::path_safety_checker::checkCollision( path, ego_predicted_path, obj, obj_path, common_parameters, front_decel, rear_decel, current_debug_data.second)) { path_safety_status.is_safe = false; updateDebugInfo(current_debug_data, path_safety_status.is_safe); const auto & obj_pose = obj.initial_pose.pose; const auto obj_polygon = tier4_autoware_utils::toPolygon2d(obj_pose, obj.shape); - path_safety_status.is_object_coming_from_rear |= !utils::safety_check::isTargetObjectFront( - path, current_pose, common_parameters.vehicle_info, obj_polygon); + path_safety_status.is_object_coming_from_rear |= + !utils::path_safety_checker::isTargetObjectFront( + path, current_pose, common_parameters.vehicle_info, obj_polygon); } } updateDebugInfo(current_debug_data, path_safety_status.is_safe); diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp index b2e872416672e..9d2da0d13f56b 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp @@ -78,6 +78,75 @@ StartPlannerModuleManager::StartPlannerModuleManager( node->declare_parameter(ns + "backward_path_update_duration"); p.ignore_distance_from_lane_end = node->declare_parameter(ns + "ignore_distance_from_lane_end"); + // freespace planner general params + { + std::string ns = "start_planner.freespace_planner."; + p.enable_freespace_planner = node->declare_parameter(ns + "enable_freespace_planner"); + p.freespace_planner_algorithm = + node->declare_parameter(ns + "freespace_planner_algorithm"); + p.end_pose_search_start_distance = + node->declare_parameter(ns + "end_pose_search_start_distance"); + p.end_pose_search_end_distance = + node->declare_parameter(ns + "end_pose_search_end_distance"); + p.end_pose_search_interval = node->declare_parameter(ns + "end_pose_search_interval"); + p.freespace_planner_velocity = node->declare_parameter(ns + "velocity"); + p.vehicle_shape_margin = node->declare_parameter(ns + "vehicle_shape_margin"); + p.freespace_planner_common_parameters.time_limit = + node->declare_parameter(ns + "time_limit"); + p.freespace_planner_common_parameters.minimum_turning_radius = + node->declare_parameter(ns + "minimum_turning_radius"); + p.freespace_planner_common_parameters.maximum_turning_radius = + node->declare_parameter(ns + "maximum_turning_radius"); + p.freespace_planner_common_parameters.turning_radius_size = + node->declare_parameter(ns + "turning_radius_size"); + p.freespace_planner_common_parameters.maximum_turning_radius = std::max( + p.freespace_planner_common_parameters.maximum_turning_radius, + p.freespace_planner_common_parameters.minimum_turning_radius); + p.freespace_planner_common_parameters.turning_radius_size = + std::max(p.freespace_planner_common_parameters.turning_radius_size, 1); + } + // freespace planner search config + { + std::string ns = "start_planner.freespace_planner.search_configs."; + p.freespace_planner_common_parameters.theta_size = + node->declare_parameter(ns + "theta_size"); + p.freespace_planner_common_parameters.angle_goal_range = + node->declare_parameter(ns + "angle_goal_range"); + p.freespace_planner_common_parameters.curve_weight = + node->declare_parameter(ns + "curve_weight"); + p.freespace_planner_common_parameters.reverse_weight = + node->declare_parameter(ns + "reverse_weight"); + p.freespace_planner_common_parameters.lateral_goal_range = + node->declare_parameter(ns + "lateral_goal_range"); + p.freespace_planner_common_parameters.longitudinal_goal_range = + node->declare_parameter(ns + "longitudinal_goal_range"); + } + // freespace planner costmap configs + { + std::string ns = "start_planner.freespace_planner.costmap_configs."; + p.freespace_planner_common_parameters.obstacle_threshold = + node->declare_parameter(ns + "obstacle_threshold"); + } + // freespace planner astar + { + std::string ns = "start_planner.freespace_planner.astar."; + p.astar_parameters.only_behind_solutions = + node->declare_parameter(ns + "only_behind_solutions"); + p.astar_parameters.use_back = node->declare_parameter(ns + "use_back"); + p.astar_parameters.distance_heuristic_weight = + node->declare_parameter(ns + "distance_heuristic_weight"); + } + // freespace planner rrtstar + { + std::string ns = "start_planner.freespace_planner.rrtstar."; + p.rrt_star_parameters.enable_update = node->declare_parameter(ns + "enable_update"); + p.rrt_star_parameters.use_informed_sampling = + node->declare_parameter(ns + "use_informed_sampling"); + p.rrt_star_parameters.max_planning_time = + node->declare_parameter(ns + "max_planning_time"); + p.rrt_star_parameters.neighbor_radius = node->declare_parameter(ns + "neighbor_radius"); + p.rrt_star_parameters.margin = node->declare_parameter(ns + "margin"); + } // validation of parameters if (p.lateral_acceleration_sampling_num < 1) { @@ -128,6 +197,11 @@ bool StartPlannerModuleManager::isSimultaneousExecutableAsApprovedModule() const return false; } + // Other modules are not needed when freespace planning + if (start_planner_ptr->isFreespacePlanning()) { + return false; + } + return enable_simultaneous_execution_as_approved_module_; }; @@ -151,6 +225,12 @@ bool StartPlannerModuleManager::isSimultaneousExecutableAsCandidateModule() cons if (!start_planner_ptr->isBackFinished()) { return false; } + + // Other modules are not needed when freespace planning + if (start_planner_ptr->isFreespacePlanning()) { + return false; + } + return enable_simultaneous_execution_as_candidate_module_; }; diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp index 4e27f57b39e7b..c7541dc8af544 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp @@ -15,9 +15,9 @@ #include "behavior_path_planner/scene_module/start_planner/start_planner_module.hpp" #include "behavior_path_planner/utils/create_vehicle_footprint.hpp" +#include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/start_planner/util.hpp" -#include "behavior_path_planner/utils/utils.hpp" #include #include @@ -58,6 +58,34 @@ StartPlannerModule::StartPlannerModule( if (start_planners_.empty()) { RCLCPP_ERROR(getLogger(), "Not found enabled planner"); } + + if (parameters_->enable_freespace_planner) { + freespace_planner_ = std::make_unique(node, *parameters, vehicle_info_); + const auto freespace_planner_period_ns = rclcpp::Rate(1.0).period(); + freespace_planner_timer_cb_group_ = + node.create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + freespace_planner_timer_ = rclcpp::create_timer( + &node, clock_, freespace_planner_period_ns, + std::bind(&StartPlannerModule::onFreespacePlannerTimer, this), + freespace_planner_timer_cb_group_); + } +} + +void StartPlannerModule::onFreespacePlannerTimer() +{ + if (!planner_data_) { + return; + } + + if (!planner_data_->costmap) { + return; + } + + const bool is_new_costmap = + (clock_->now() - planner_data_->costmap->header.stamp).seconds() < 1.0; + if (isStuck() && is_new_costmap) { + planFreespacePath(); + } } BehaviorModuleOutput StartPlannerModule::run() @@ -200,20 +228,14 @@ BehaviorModuleOutput StartPlannerModule::plan() path = status_.backward_path; } - const auto target_drivable_lanes = utils::getNonOverlappingExpandedLanes( - path, generateDrivableLanes(path), planner_data_->drivable_area_expansion_parameters); - - DrivableAreaInfo current_drivable_area_info; - current_drivable_area_info.drivable_lanes = target_drivable_lanes; - output.drivable_area_info = utils::combineDrivableAreaInfo( - current_drivable_area_info, getPreviousModuleOutput().drivable_area_info); - output.path = std::make_shared(path); output.reference_path = getPreviousModuleOutput().reference_path; output.turn_signal_info = calcTurnSignalInfo(); path_candidate_ = std::make_shared(getFullPath()); path_reference_ = getPreviousModuleOutput().reference_path; + setDrivableAreaInfo(output); + const uint16_t steering_factor_direction = std::invoke([&output]() { if (output.turn_signal_info.turn_signal.command == TurnIndicatorsCommand::ENABLE_LEFT) { return SteeringFactor::LEFT; @@ -257,23 +279,8 @@ CandidateOutput StartPlannerModule::planCandidate() const return CandidateOutput{}; } -std::shared_ptr StartPlannerModule::getCurrentPlanner() const -{ - for (const auto & planner : start_planners_) { - if (status_.planner_type == planner->getPlannerType()) { - return planner; - } - } - return nullptr; -} - PathWithLaneId StartPlannerModule::getFullPath() const { - const auto pull_out_planner = getCurrentPlanner(); - if (pull_out_planner == nullptr) { - return PathWithLaneId{}; - } - // combine partial pull out path PathWithLaneId pull_out_path; for (const auto & partial_path : status_.pull_out_path.partial_paths) { @@ -334,17 +341,14 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval() p.point.longitudinal_velocity_mps = 0.0; } - DrivableAreaInfo current_drivable_area_info; - current_drivable_area_info.drivable_lanes = expanded_lanes; - output.drivable_area_info = utils::combineDrivableAreaInfo( - current_drivable_area_info, getPreviousModuleOutput().drivable_area_info); - output.path = std::make_shared(stop_path); output.reference_path = getPreviousModuleOutput().reference_path; output.turn_signal_info = calcTurnSignalInfo(); path_candidate_ = std::make_shared(getFullPath()); path_reference_ = getPreviousModuleOutput().reference_path; + setDrivableAreaInfo(output); + const uint16_t steering_factor_direction = std::invoke([&output]() { if (output.turn_signal_info.turn_signal.command == TurnIndicatorsCommand::ENABLE_LEFT) { return SteeringFactor::LEFT; @@ -405,18 +409,12 @@ void StartPlannerModule::planWithPriority( const std::vector & start_pose_candidates, const Pose & goal_pose, const std::string search_priority) { - status_.is_safe = false; - status_.planner_type = PlannerType::NONE; - // check if start pose candidates are valid if (start_pose_candidates.empty()) { return; } const auto is_safe_with_pose_planner = [&](const size_t i, const auto & planner) { - // Set back_finished flag based on the current index - status_.back_finished = i == 0; - // Get the pull_out_start_pose for the current index const auto & pull_out_start_pose = start_pose_candidates.at(i); @@ -428,6 +426,7 @@ void StartPlannerModule::planWithPriority( } // use current path if back is not needed if (status_.back_finished) { + const std::lock_guard lock(mutex_); status_.is_safe = true; status_.pull_out_path = *pull_out_path; status_.pull_out_start_pose = pull_out_start_pose; @@ -447,10 +446,13 @@ void StartPlannerModule::planWithPriority( } // Update status variables with the next path information - status_.is_safe = true; - status_.pull_out_path = *pull_out_path_next; - status_.pull_out_start_pose = pull_out_start_pose_next; - status_.planner_type = planner->getPlannerType(); + { + const std::lock_guard lock(mutex_); + status_.is_safe = true; + status_.pull_out_path = *pull_out_path_next; + status_.pull_out_start_pose = pull_out_start_pose_next; + status_.planner_type = planner->getPlannerType(); + } return true; }; @@ -490,7 +492,19 @@ void StartPlannerModule::planWithPriority( } for (const auto & p : order_priority) { - if (is_safe_with_pose_planner(p.first, p.second)) break; + if (is_safe_with_pose_planner(p.first, p.second)) { + const std::lock_guard lock(mutex_); + // Set back_finished flag based on the current index + status_.back_finished = p.first == 0; + return; + } + } + + // not found safe path + if (status_.planner_type != PlannerType::FREESPACE) { + const std::lock_guard lock(mutex_); + status_.is_safe = false; + status_.planner_type = PlannerType::NONE; } } @@ -515,10 +529,6 @@ PathWithLaneId StartPlannerModule::generateStopPath() const path.points.push_back(toPathPointWithLaneId(current_pose)); path.points.push_back(toPathPointWithLaneId(moved_pose)); - // generate drivable area - const auto target_drivable_lanes = utils::getNonOverlappingExpandedLanes( - path, generateDrivableLanes(path), planner_data_->drivable_area_expansion_parameters); - return path; } @@ -631,9 +641,10 @@ std::vector StartPlannerModule::searchPullOutStartPoses() // filter pull out lanes stop objects const auto [pull_out_lane_objects, others] = - utils::separateObjectsByLanelets(*planner_data_->dynamic_object, status_.pull_out_lanes); - const auto pull_out_lane_stop_objects = - utils::filterObjectsByVelocity(pull_out_lane_objects, parameters_->th_moving_object_velocity); + utils::path_safety_checker::separateObjectsByLanelets( + *planner_data_->dynamic_object, status_.pull_out_lanes); + const auto pull_out_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity( + pull_out_lane_objects, parameters_->th_moving_object_velocity); // lateral shift to current_pose const double distance_from_center_line = arc_position_pose.distance; @@ -707,6 +718,10 @@ bool StartPlannerModule::hasFinishedPullOut() const } const auto current_pose = planner_data_->self_odometry->pose.pose; + if (status_.planner_type == PlannerType::FREESPACE) { + return tier4_autoware_utils::calcDistance2d(current_pose, status_.pull_out_path.end_pose) < + parameters_->th_arrived_distance; + } // check that ego has passed pull out end point const double backward_path_length = @@ -774,6 +789,24 @@ bool StartPlannerModule::isStopped() return is_stopped; } +bool StartPlannerModule::isStuck() +{ + if (!isStopped()) { + return false; + } + + if (status_.planner_type == PlannerType::STOP) { + return true; + } + + // not found safe path + if (!status_.is_safe) { + return true; + } + + return false; +} + bool StartPlannerModule::hasFinishedCurrentPath() { const auto current_path = getCurrentPath(); @@ -920,21 +953,21 @@ BehaviorModuleOutput StartPlannerModule::generateStopOutput() const PathWithLaneId stop_path = generateStopPath(); output.path = std::make_shared(stop_path); - DrivableAreaInfo current_drivable_area_info; - current_drivable_area_info.drivable_lanes = generateDrivableLanes(*output.path); - output.drivable_area_info = utils::combineDrivableAreaInfo( - current_drivable_area_info, getPreviousModuleOutput().drivable_area_info); + setDrivableAreaInfo(output); output.reference_path = getPreviousModuleOutput().reference_path; - status_.back_finished = true; - status_.planner_type = PlannerType::STOP; - status_.pull_out_path.partial_paths.clear(); - status_.pull_out_path.partial_paths.push_back(stop_path); - const Pose & current_pose = planner_data_->self_odometry->pose.pose; - status_.pull_out_start_pose = current_pose; - status_.pull_out_path.start_pose = current_pose; - status_.pull_out_path.end_pose = current_pose; + { + const std::lock_guard lock(mutex_); + status_.back_finished = true; + status_.planner_type = PlannerType::STOP; + status_.pull_out_path.partial_paths.clear(); + status_.pull_out_path.partial_paths.push_back(stop_path); + const Pose & current_pose = planner_data_->self_odometry->pose.pose; + status_.pull_out_start_pose = current_pose; + status_.pull_out_path.start_pose = current_pose; + status_.pull_out_path.end_pose = current_pose; + } path_candidate_ = std::make_shared(stop_path); path_reference_ = getPreviousModuleOutput().reference_path; @@ -942,6 +975,68 @@ BehaviorModuleOutput StartPlannerModule::generateStopOutput() return output; } +bool StartPlannerModule::planFreespacePath() +{ + const Pose & current_pose = planner_data_->self_odometry->pose.pose; + const auto & route_handler = planner_data_->route_handler; + + const double end_pose_search_start_distance = parameters_->end_pose_search_start_distance; + const double end_pose_search_end_distance = parameters_->end_pose_search_end_distance; + const double end_pose_search_interval = parameters_->end_pose_search_interval; + + const double backward_path_length = + planner_data_->parameters.backward_path_length + parameters_->max_back_distance; + const auto current_lanes = utils::getExtendedCurrentLanes( + planner_data_, backward_path_length, std::numeric_limits::max(), + /*forward_only_in_route*/ true); + + const auto current_arc_coords = lanelet::utils::getArcCoordinates(current_lanes, current_pose); + + const double s_start = std::max(0.0, current_arc_coords.length + end_pose_search_start_distance); + const double s_end = current_arc_coords.length + end_pose_search_end_distance; + + auto center_line_path = utils::resamplePathWithSpline( + route_handler->getCenterLinePath(current_lanes, s_start, s_end), end_pose_search_interval); + + for (const auto & p : center_line_path.points) { + const Pose end_pose = p.point.pose; + freespace_planner_->setPlannerData(planner_data_); + auto freespace_path = freespace_planner_->plan(current_pose, end_pose); + + if (!freespace_path) { + continue; + } + + const std::lock_guard lock(mutex_); + status_.pull_out_path = *freespace_path; + status_.pull_out_start_pose = current_pose; + status_.planner_type = freespace_planner_->getPlannerType(); + status_.is_safe = true; + status_.back_finished = true; + return true; + } + + return false; +} + +void StartPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) const +{ + if (status_.planner_type == PlannerType::FREESPACE) { + const double drivable_area_margin = planner_data_->parameters.vehicle_width; + output.drivable_area_info.drivable_margin = + planner_data_->parameters.vehicle_width / 2.0 + drivable_area_margin; + } else { + const auto target_drivable_lanes = utils::getNonOverlappingExpandedLanes( + *output.path, generateDrivableLanes(*output.path), + planner_data_->drivable_area_expansion_parameters); + + DrivableAreaInfo current_drivable_area_info; + current_drivable_area_info.drivable_lanes = target_drivable_lanes; + output.drivable_area_info = utils::combineDrivableAreaInfo( + current_drivable_area_info, getPreviousModuleOutput().drivable_area_info); + } +} + void StartPlannerModule::setDebugData() const { using marker_utils::createPathMarkerArray; diff --git a/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp b/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp index 617c3cd633cda..2b62d5e7c3b53 100644 --- a/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp +++ b/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp @@ -200,11 +200,39 @@ void updateDrivableAreaBounds(PathWithLaneId & path, const polygon_t & expanded_ end_right.update(*inter_end, it, dist); } } - if ( // ill-formed expanded drivable area -> keep the original bounds - start_left.segment_it == da.end() || start_right.segment_it == da.end() || - end_left.segment_it == da.end() || end_right.segment_it == da.end()) { - return; + if (start_left.segment_it == da.end()) { + const auto closest_it = + std::min_element(da.begin(), da.end(), [&](const auto & a, const auto & b) { + return boost::geometry::distance(a, start_segment.first) < + boost::geometry::distance(b, start_segment.first); + }); + start_left.update(*closest_it, closest_it, 0.0); } + if (start_right.segment_it == da.end()) { + const auto closest_it = + std::min_element(da.begin(), da.end(), [&](const auto & a, const auto & b) { + return boost::geometry::distance(a, start_segment.second) < + boost::geometry::distance(b, start_segment.second); + }); + start_right.update(*closest_it, closest_it, 0.0); + } + if (end_left.segment_it == da.end()) { + const auto closest_it = + std::min_element(da.begin(), da.end(), [&](const auto & a, const auto & b) { + return boost::geometry::distance(a, end_segment.first) < + boost::geometry::distance(b, end_segment.first); + }); + end_left.update(*closest_it, closest_it, 0.0); + } + if (end_right.segment_it == da.end()) { + const auto closest_it = + std::min_element(da.begin(), da.end(), [&](const auto & a, const auto & b) { + return boost::geometry::distance(a, end_segment.second) < + boost::geometry::distance(b, end_segment.second); + }); + end_right.update(*closest_it, closest_it, 0.0); + } + // extract the expanded left and right bound from the expanded drivable area path.left_bound.clear(); path.right_bound.clear(); @@ -238,8 +266,11 @@ void updateDrivableAreaBounds(PathWithLaneId & path, const polygon_t & expanded_ const auto point_cmp = [](const auto & p1, const auto & p2) { return p1.x == p2.x && p1.y == p2.y; }; - std::unique(path.left_bound.begin(), path.left_bound.end(), point_cmp); - std::unique(path.right_bound.begin(), path.right_bound.end(), point_cmp); + path.left_bound.erase( + std::unique(path.left_bound.begin(), path.left_bound.end(), point_cmp), path.left_bound.end()); + path.right_bound.erase( + std::unique(path.right_bound.begin(), path.right_bound.end(), point_cmp), + path.right_bound.end()); copy_z_over_arc_length(original_left_bound, path.left_bound); copy_z_over_arc_length(original_right_bound, path.right_bound); } diff --git a/planning/behavior_path_planner/src/utils/goal_planner/freespace_pull_over.cpp b/planning/behavior_path_planner/src/utils/goal_planner/freespace_pull_over.cpp index ebfac8a347134..f4aabb13b8916 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/freespace_pull_over.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/freespace_pull_over.cpp @@ -25,7 +25,9 @@ namespace behavior_path_planner FreespacePullOver::FreespacePullOver( rclcpp::Node & node, const GoalPlannerParameters & parameters, const vehicle_info_util::VehicleInfo & vehicle_info) -: PullOverPlannerBase{node, parameters}, velocity_{parameters.freespace_parking_velocity} +: PullOverPlannerBase{node, parameters}, + velocity_{parameters.freespace_parking_velocity}, + left_side_parking_{parameters.parking_policy == ParkingPolicy::LEFT_SIDE} { freespace_planning_algorithms::VehicleShape vehicle_shape( vehicle_info, parameters.vehicle_shape_margin); @@ -58,8 +60,18 @@ boost::optional FreespacePullOver::plan(const Pose & goal_pose) return {}; } + const auto road_lanes = utils::getExtendedCurrentLanes( + planner_data_, parameters_.backward_goal_search_length, parameters_.forward_goal_search_length, + /*forward_only_in_route*/ false); + const auto pull_over_lanes = + goal_planner_utils::getPullOverLanes(*(planner_data_->route_handler), left_side_parking_); + if (road_lanes.empty() || pull_over_lanes.empty()) { + return {}; + } + const auto lanes = utils::combineLanelets(road_lanes, pull_over_lanes); + PathWithLaneId path = - utils::convertWayPointsToPathWithLaneId(planner_->getWaypoints(), velocity_); + utils::convertWayPointsToPathWithLaneId(planner_->getWaypoints(), velocity_, lanes); const auto reverse_indices = utils::getReversingIndices(path); std::vector partial_paths = utils::dividePath(path, reverse_indices); diff --git a/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp b/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp index 9daee39433d97..1e57dcc319dc5 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp @@ -15,6 +15,7 @@ #include "behavior_path_planner/utils/goal_planner/goal_searcher.hpp" #include "behavior_path_planner/utils/goal_planner/util.hpp" +#include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner/utils/path_utils.hpp" #include "lanelet2_extension/utility/utilities.hpp" @@ -69,7 +70,8 @@ GoalCandidates GoalSearcher::search(const Pose & original_goal_pose) parameters_.goal_search_interval); const auto [shoulder_lane_objects, others] = - utils::separateObjectsByLanelets(*(planner_data_->dynamic_object), pull_over_lanes); + utils::path_safety_checker::separateObjectsByLanelets( + *(planner_data_->dynamic_object), pull_over_lanes); std::vector original_search_poses{}; // for search area visualizing size_t goal_id = 0; @@ -151,7 +153,8 @@ void GoalSearcher::update(GoalCandidates & goal_candidates) const const auto pull_over_lanes = goal_planner_utils::getPullOverLanes(*(planner_data_->route_handler), left_side_parking_); const auto [shoulder_lane_objects, others] = - utils::separateObjectsByLanelets(*(planner_data_->dynamic_object), pull_over_lanes); + utils::path_safety_checker::separateObjectsByLanelets( + *(planner_data_->dynamic_object), pull_over_lanes); constexpr bool filter_inside = true; const auto target_objects = goal_planner_utils::filterObjectsByLateralDistance( goal_pose, planner_data_->parameters.vehicle_width, shoulder_lane_objects, diff --git a/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp b/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp new file mode 100644 index 0000000000000..33afe0fe6642f --- /dev/null +++ b/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp @@ -0,0 +1,353 @@ +// Copyright 2023 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. + +#include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" + +#include "behavior_path_planner/utils/utils.hpp" + +namespace behavior_path_planner::utils::path_safety_checker +{ + +PredictedObjects filterObjects( + const std::shared_ptr & objects, + const std::shared_ptr & route_handler, const lanelet::ConstLanelets & current_lanes, + const geometry_msgs::msg::Point & current_pose, + const std::shared_ptr & params) +{ + // Guard + if (objects->objects.empty()) { + return PredictedObjects(); + } + + const double ignore_object_velocity_threshold = params->ignore_object_velocity_threshold; + const double object_check_forward_distance = params->object_check_forward_distance; + const double object_check_backward_distance = params->object_check_backward_distance; + const ObjectTypesToCheck & target_object_types = params->object_types_to_check; + + PredictedObjects filtered_objects; + + filtered_objects = filterObjectsByVelocity(*objects, ignore_object_velocity_threshold); + + filterObjectsByClass(filtered_objects, target_object_types); + + const auto path = route_handler->getCenterLinePath( + current_lanes, object_check_backward_distance, object_check_forward_distance); + + filterObjectsByPosition( + filtered_objects, path.points, current_pose, object_check_forward_distance, + object_check_backward_distance); + + return filtered_objects; +} + +PredictedObjects filterObjectsByVelocity(const PredictedObjects & objects, double lim_v) +{ + return filterObjectsByVelocity(objects, -lim_v, lim_v); +} + +PredictedObjects filterObjectsByVelocity( + const PredictedObjects & objects, double min_v, double max_v) +{ + PredictedObjects filtered; + filtered.header = objects.header; + for (const auto & obj : objects.objects) { + const auto v_norm = std::hypot( + obj.kinematics.initial_twist_with_covariance.twist.linear.x, + obj.kinematics.initial_twist_with_covariance.twist.linear.y); + if (min_v < v_norm && v_norm < max_v) { + filtered.objects.push_back(obj); + } + } + return filtered; +} + +void filterObjectsByPosition( + PredictedObjects & objects, const std::vector & path_points, + const geometry_msgs::msg::Point & current_pose, const double forward_distance, + const double backward_distance) +{ + // Create a new container to hold the filtered objects + PredictedObjects filtered; + filtered.header = objects.header; + + // Reserve space in the vector to avoid reallocations + filtered.objects.reserve(objects.objects.size()); + + for (const auto & obj : objects.objects) { + const double dist_ego_to_obj = motion_utils::calcSignedArcLength( + path_points, current_pose, obj.kinematics.initial_pose_with_covariance.pose.position); + + if (-backward_distance < dist_ego_to_obj && dist_ego_to_obj < forward_distance) { + filtered.objects.push_back(obj); + } + } + + // Replace the original objects with the filtered list + objects.objects = std::move(filtered.objects); + return; +} + +void filterObjectsByClass( + PredictedObjects & objects, const ObjectTypesToCheck & target_object_types) +{ + using autoware_auto_perception_msgs::msg::ObjectClassification; + + PredictedObjects filtered_objects; + + for (auto & object : objects.objects) { + const auto t = utils::getHighestProbLabel(object.classification); + const auto is_object_type = + ((t == ObjectClassification::CAR && target_object_types.check_car) || + (t == ObjectClassification::TRUCK && target_object_types.check_truck) || + (t == ObjectClassification::BUS && target_object_types.check_bus) || + (t == ObjectClassification::TRAILER && target_object_types.check_trailer) || + (t == ObjectClassification::UNKNOWN && target_object_types.check_unknown) || + (t == ObjectClassification::BICYCLE && target_object_types.check_bicycle) || + (t == ObjectClassification::MOTORCYCLE && target_object_types.check_motorcycle) || + (t == ObjectClassification::PEDESTRIAN && target_object_types.check_pedestrian)); + + // If the object type matches any of the target types, add it to the filtered list + if (is_object_type) { + filtered_objects.objects.push_back(object); + } + } + + // Replace the original objects with the filtered list + objects = std::move(filtered_objects); + + return; +} + +std::pair, std::vector> separateObjectIndicesByLanelets( + const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets) +{ + if (target_lanelets.empty()) { + return {}; + } + + std::vector target_indices; + std::vector other_indices; + + for (size_t i = 0; i < objects.objects.size(); i++) { + // create object polygon + const auto & obj = objects.objects.at(i); + // create object polygon + const auto obj_polygon = tier4_autoware_utils::toPolygon2d(obj); + bool is_filtered_object = false; + for (const auto & llt : target_lanelets) { + // create lanelet polygon + const auto polygon2d = llt.polygon2d().basicPolygon(); + if (polygon2d.empty()) { + // no lanelet polygon + continue; + } + Polygon2d lanelet_polygon; + for (const auto & lanelet_point : polygon2d) { + lanelet_polygon.outer().emplace_back(lanelet_point.x(), lanelet_point.y()); + } + lanelet_polygon.outer().push_back(lanelet_polygon.outer().front()); + // check the object does not intersect the lanelet + if (!boost::geometry::disjoint(lanelet_polygon, obj_polygon)) { + target_indices.push_back(i); + is_filtered_object = true; + break; + } + } + + if (!is_filtered_object) { + other_indices.push_back(i); + } + } + + return std::make_pair(target_indices, other_indices); +} + +std::pair separateObjectsByLanelets( + const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets) +{ + PredictedObjects target_objects; + PredictedObjects other_objects; + + const auto [target_indices, other_indices] = + separateObjectIndicesByLanelets(objects, target_lanelets); + + target_objects.objects.reserve(target_indices.size()); + other_objects.objects.reserve(other_indices.size()); + + for (const size_t i : target_indices) { + target_objects.objects.push_back(objects.objects.at(i)); + } + + for (const size_t i : other_indices) { + other_objects.objects.push_back(objects.objects.at(i)); + } + + return std::make_pair(target_objects, other_objects); +} + +std::vector getPredictedPathFromObj( + const ExtendedPredictedObject & obj, const bool & is_use_all_predicted_path) +{ + if (!is_use_all_predicted_path) { + const auto max_confidence_path = std::max_element( + obj.predicted_paths.begin(), obj.predicted_paths.end(), + [](const auto & path1, const auto & path2) { return path1.confidence < path2.confidence; }); + if (max_confidence_path != obj.predicted_paths.end()) { + return {*max_confidence_path}; + } + } + + return obj.predicted_paths; +} + +// TODO(Sugahara): should consider delay before departure +std::vector createPredictedPath( + const std::shared_ptr & ego_predicted_path_params, + const std::vector & path_points, + const geometry_msgs::msg::Pose & vehicle_pose, const double current_velocity, size_t ego_seg_idx) +{ + if (path_points.empty()) { + return {}; + } + + const double min_slow_down_speed = ego_predicted_path_params->min_slow_speed; + const double acceleration = ego_predicted_path_params->acceleration; + const double time_horizon = ego_predicted_path_params->time_horizon; + const double time_resolution = ego_predicted_path_params->time_resolution; + + std::vector predicted_path; + const auto vehicle_pose_frenet = + convertToFrenetPoint(path_points, vehicle_pose.position, ego_seg_idx); + + for (double t = 0.0; t < time_horizon + 1e-3; t += time_resolution) { + const double velocity = std::max(current_velocity + acceleration * t, min_slow_down_speed); + const double length = current_velocity * t + 0.5 * acceleration * t * t; + const auto pose = + motion_utils::calcInterpolatedPose(path_points, vehicle_pose_frenet.length + length); + predicted_path.emplace_back(t, pose, velocity); + } + + return predicted_path; +} + +bool isCentroidWithinLanelets( + const PredictedObject & object, const lanelet::ConstLanelets & target_lanelets) +{ + if (target_lanelets.empty()) { + return false; + } + + const auto & object_pos = object.kinematics.initial_pose_with_covariance.pose.position; + lanelet::BasicPoint2d object_centroid(object_pos.x, object_pos.y); + + for (const auto & llt : target_lanelets) { + if (boost::geometry::within(object_centroid, llt.polygon2d().basicPolygon())) { + return true; + } + } + + return false; +} + +ExtendedPredictedObject transform( + const PredictedObject & object, const double safety_check_time_horizon, + const double safety_check_time_resolution) +{ + ExtendedPredictedObject extended_object; + extended_object.uuid = object.object_id; + extended_object.initial_pose = object.kinematics.initial_pose_with_covariance; + extended_object.initial_twist = object.kinematics.initial_twist_with_covariance; + extended_object.initial_acceleration = object.kinematics.initial_acceleration_with_covariance; + extended_object.shape = object.shape; + + const auto obj_velocity = extended_object.initial_twist.twist.linear.x; + + extended_object.predicted_paths.resize(object.kinematics.predicted_paths.size()); + for (size_t i = 0; i < object.kinematics.predicted_paths.size(); ++i) { + const auto & path = object.kinematics.predicted_paths[i]; + extended_object.predicted_paths[i].confidence = path.confidence; + + // Create path based on time horizon and resolution + for (double t = 0.0; t < safety_check_time_horizon + 1e-3; t += safety_check_time_resolution) { + const auto obj_pose = object_recognition_utils::calcInterpolatedPose(path, t); + if (obj_pose) { + const auto obj_polygon = tier4_autoware_utils::toPolygon2d(*obj_pose, object.shape); + extended_object.predicted_paths[i].path.emplace_back( + t, *obj_pose, obj_velocity, obj_polygon); + } + } + } + + return extended_object; +} + +TargetObjectsOnLane createTargetObjectsOnLane( + const lanelet::ConstLanelets & current_lanes, const std::shared_ptr & route_handler, + const PredictedObjects & filtered_objects, const std::shared_ptr & params) +{ + const auto & object_lane_configuration = params->object_lane_configuration; + const bool include_opposite = params->include_opposite_lane; + const bool invert_opposite = params->invert_opposite_lane; + const double safety_check_time_horizon = params->safety_check_time_horizon; + const double safety_check_time_resolution = params->safety_check_time_resolution; + + lanelet::ConstLanelets all_left_lanelets; + lanelet::ConstLanelets all_right_lanelets; + + // Define lambda functions to update left and right lanelets + const auto update_left_lanelets = [&](const lanelet::ConstLanelet & target_lane) { + const auto left_lanelets = route_handler->getAllLeftSharedLinestringLanelets( + target_lane, include_opposite, invert_opposite); + all_left_lanelets.insert(all_left_lanelets.end(), left_lanelets.begin(), left_lanelets.end()); + }; + + const auto update_right_lanelets = [&](const lanelet::ConstLanelet & target_lane) { + const auto right_lanelets = route_handler->getAllRightSharedLinestringLanelets( + target_lane, include_opposite, invert_opposite); + all_right_lanelets.insert( + all_right_lanelets.end(), right_lanelets.begin(), right_lanelets.end()); + }; + + // Update left and right lanelets for each current lane + for (const auto & current_lane : current_lanes) { + update_left_lanelets(current_lane); + update_right_lanelets(current_lane); + } + + TargetObjectsOnLane target_objects_on_lane{}; + const auto append_objects_on_lane = [&](auto & lane_objects, const auto & check_lanes) { + std::for_each( + filtered_objects.objects.begin(), filtered_objects.objects.end(), [&](const auto & object) { + if (isCentroidWithinLanelets(object, check_lanes)) { + lane_objects.push_back( + transform(object, safety_check_time_horizon, safety_check_time_resolution)); + } + }); + }; + + // TODO(Sugahara): Consider shoulder and other lane objects + if (object_lane_configuration.check_current_lane) { + append_objects_on_lane(target_objects_on_lane.on_current_lane, current_lanes); + } + if (object_lane_configuration.check_left_lane) { + append_objects_on_lane(target_objects_on_lane.on_left_lane, all_left_lanelets); + } + if (object_lane_configuration.check_right_lane) { + append_objects_on_lane(target_objects_on_lane.on_right_lane, all_right_lanelets); + } + + return target_objects_on_lane; +} + +} // namespace behavior_path_planner::utils::path_safety_checker diff --git a/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp b/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp index 23c2951d30a38..ee23ecc005142 100644 --- a/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp +++ b/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp @@ -19,7 +19,7 @@ #include "motion_utils/trajectory/trajectory.hpp" #include "object_recognition_utils/predicted_path_utils.hpp" -namespace behavior_path_planner::utils::safety_check +namespace behavior_path_planner::utils::path_safety_checker { void appendPointToPolygon(Polygon2d & polygon, const geometry_msgs::msg::Point & geom_point) { @@ -303,4 +303,4 @@ bool checkCollision( return true; } -} // namespace behavior_path_planner::utils::safety_check +} // namespace behavior_path_planner::utils::path_safety_checker diff --git a/planning/behavior_path_planner/src/utils/path_utils.cpp b/planning/behavior_path_planner/src/utils/path_utils.cpp index d2d70b0a46dfc..ec14a064bf51b 100644 --- a/planning/behavior_path_planner/src/utils/path_utils.cpp +++ b/planning/behavior_path_planner/src/utils/path_utils.cpp @@ -336,14 +336,28 @@ std::pair getPathTurnSignal( } PathWithLaneId convertWayPointsToPathWithLaneId( - const freespace_planning_algorithms::PlannerWaypoints & waypoints, const double velocity) + const freespace_planning_algorithms::PlannerWaypoints & waypoints, const double velocity, + const lanelet::ConstLanelets & lanelets) { PathWithLaneId path; path.header = waypoints.header; - for (const auto & waypoint : waypoints.waypoints) { + for (size_t i = 0; i < waypoints.waypoints.size(); ++i) { + const auto & waypoint = waypoints.waypoints.at(i); PathPointWithLaneId point{}; point.point.pose = waypoint.pose.pose; - // point.lane_id = // todo + // put the lane that contain waypoints in lane_ids. + bool is_in_lanes = false; + for (const auto & lane : lanelets) { + if (lanelet::utils::isInLanelet(point.point.pose, lane)) { + point.lane_ids.push_back(lane.id()); + is_in_lanes = true; + } + } + // If none of them corresponds, assign the previous lane_ids. + if (!is_in_lanes && i > 0) { + point.lane_ids = path.points.at(i - 1).lane_ids; + } + point.point.longitudinal_velocity_mps = (waypoint.is_back ? -1 : 1) * velocity; path.points.push_back(point); } diff --git a/planning/behavior_path_planner/src/utils/start_planner/freespace_pull_out.cpp b/planning/behavior_path_planner/src/utils/start_planner/freespace_pull_out.cpp new file mode 100644 index 0000000000000..ba34901d9df6a --- /dev/null +++ b/planning/behavior_path_planner/src/utils/start_planner/freespace_pull_out.cpp @@ -0,0 +1,115 @@ +// Copyright 2023 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. + +#include "behavior_path_planner/utils/start_planner/freespace_pull_out.hpp" + +#include "behavior_path_planner/utils/path_utils.hpp" +#include "behavior_path_planner/utils/start_planner/util.hpp" +#include "behavior_path_planner/utils/utils.hpp" + +#include + +#include +#include + +namespace behavior_path_planner +{ +FreespacePullOut::FreespacePullOut( + rclcpp::Node & node, const StartPlannerParameters & parameters, + const vehicle_info_util::VehicleInfo & vehicle_info) +: PullOutPlannerBase{node, parameters}, velocity_{parameters.freespace_planner_velocity} +{ + freespace_planning_algorithms::VehicleShape vehicle_shape( + vehicle_info, parameters.vehicle_shape_margin); + if (parameters.freespace_planner_algorithm == "astar") { + use_back_ = parameters.astar_parameters.use_back; + planner_ = std::make_unique( + parameters.freespace_planner_common_parameters, vehicle_shape, parameters.astar_parameters); + } else if (parameters.freespace_planner_algorithm == "rrtstar") { + use_back_ = true; // no option for disabling back in rrtstar + planner_ = std::make_unique( + parameters.freespace_planner_common_parameters, vehicle_shape, + parameters.rrt_star_parameters); + } +} + +boost::optional FreespacePullOut::plan(const Pose start_pose, const Pose end_pose) +{ + const auto & route_handler = planner_data_->route_handler; + const double backward_path_length = planner_data_->parameters.backward_path_length; + const double forward_path_length = planner_data_->parameters.forward_path_length; + + planner_->setMap(*planner_data_->costmap); + + const bool found_path = planner_->makePlan(start_pose, end_pose); + if (!found_path) { + return {}; + } + + const auto road_lanes = utils::getExtendedCurrentLanes( + planner_data_, backward_path_length, std::numeric_limits::max(), + /*forward_only_in_route*/ true); + // find candidate paths + const auto pull_out_lanes = + start_planner_utils::getPullOutLanes(planner_data_, backward_path_length); + const auto lanes = utils::combineLanelets(road_lanes, pull_out_lanes); + + const PathWithLaneId path = + utils::convertWayPointsToPathWithLaneId(planner_->getWaypoints(), velocity_, lanes); + + const auto reverse_indices = utils::getReversingIndices(path); + std::vector partial_paths = utils::dividePath(path, reverse_indices); + + // remove points which are near the end pose + PathWithLaneId & last_path = partial_paths.back(); + const double th_end_distance = 1.0; + for (auto it = last_path.points.begin(); it != last_path.points.end(); ++it) { + const size_t index = std::distance(last_path.points.begin(), it); + if (index == 0) continue; + const double distance = + tier4_autoware_utils::calcDistance2d(end_pose.position, it->point.pose.position); + if (distance < th_end_distance) { + last_path.points.erase(it, last_path.points.end()); + break; + } + } + + // push back generate road lane path between end pose and goal pose to last path + const auto & goal_pose = route_handler->getGoalPose(); + constexpr double offset_from_end_pose = 1.0; + const auto arc_position_end = lanelet::utils::getArcCoordinates(road_lanes, end_pose); + const double s_start = std::max(arc_position_end.length + offset_from_end_pose, 0.0); + const auto path_end_info = + start_planner_utils::calcEndArcLength(s_start, forward_path_length, road_lanes, goal_pose); + const double s_end = path_end_info.first; + const bool path_terminal_is_goal = path_end_info.second; + + const auto road_center_line_path = route_handler->getCenterLinePath(road_lanes, s_start, s_end); + last_path = start_planner_utils::combineReferencePath(last_path, road_center_line_path); + + const double original_terminal_velocity = last_path.points.back().point.longitudinal_velocity_mps; + utils::correctDividedPathVelocity(partial_paths); + if (!path_terminal_is_goal) { + // not need to stop at terminal + last_path.points.back().point.longitudinal_velocity_mps = original_terminal_velocity; + } + + PullOutPath pull_out_path{}; + pull_out_path.partial_paths = partial_paths; + pull_out_path.start_pose = start_pose; + pull_out_path.end_pose = end_pose; + + return pull_out_path; +} +} // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp b/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp index 5eac8291f0e1a..721ffd3064840 100644 --- a/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp +++ b/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp @@ -14,6 +14,7 @@ #include "behavior_path_planner/utils/start_planner/geometric_pull_out.hpp" +#include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner/utils/start_planner/util.hpp" #include "behavior_path_planner/utils/utils.hpp" @@ -69,9 +70,10 @@ boost::optional GeometricPullOut::plan(Pose start_pose, Pose goal_p // collision check with stop objects in pull out lanes const auto arc_path = planner_.getArcPath(); const auto [pull_out_lane_objects, others] = - utils::separateObjectsByLanelets(*(planner_data_->dynamic_object), pull_out_lanes); - const auto pull_out_lane_stop_objects = - utils::filterObjectsByVelocity(pull_out_lane_objects, parameters_.th_moving_object_velocity); + utils::path_safety_checker::separateObjectsByLanelets( + *(planner_data_->dynamic_object), pull_out_lanes); + const auto pull_out_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity( + pull_out_lane_objects, parameters_.th_moving_object_velocity); if (utils::checkCollisionBetweenPathFootprintsAndObjects( vehicle_footprint_, arc_path, pull_out_lane_stop_objects, diff --git a/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp b/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp index cbe06fa66c306..08dcc0254deb1 100644 --- a/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp +++ b/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp @@ -14,6 +14,7 @@ #include "behavior_path_planner/utils/start_planner/shift_pull_out.hpp" +#include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/start_planner/util.hpp" #include "behavior_path_planner/utils/utils.hpp" @@ -64,9 +65,9 @@ boost::optional ShiftPullOut::plan(Pose start_pose, Pose goal_pose) // extract stop objects in pull out lane for collision check const auto [pull_out_lane_objects, others] = - utils::separateObjectsByLanelets(*dynamic_objects, pull_out_lanes); - const auto pull_out_lane_stop_objects = - utils::filterObjectsByVelocity(pull_out_lane_objects, parameters_.th_moving_object_velocity); + utils::path_safety_checker::separateObjectsByLanelets(*dynamic_objects, pull_out_lanes); + const auto pull_out_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity( + pull_out_lane_objects, parameters_.th_moving_object_velocity); // get safe path for (auto & pull_out_path : pull_out_paths) { diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index 8cc94bd6b46fa..dbbab9f040cfa 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -711,73 +711,6 @@ double calcLongitudinalDistanceFromEgoToObjects( return min_distance; } -std::pair, std::vector> separateObjectIndicesByLanelets( - const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets) -{ - if (target_lanelets.empty()) { - return {}; - } - - std::vector target_indices; - std::vector other_indices; - - for (size_t i = 0; i < objects.objects.size(); i++) { - // create object polygon - const auto & obj = objects.objects.at(i); - // create object polygon - const auto obj_polygon = tier4_autoware_utils::toPolygon2d(obj); - bool is_filtered_object = false; - for (const auto & llt : target_lanelets) { - // create lanelet polygon - const auto polygon2d = llt.polygon2d().basicPolygon(); - if (polygon2d.empty()) { - // no lanelet polygon - continue; - } - Polygon2d lanelet_polygon; - for (const auto & lanelet_point : polygon2d) { - lanelet_polygon.outer().emplace_back(lanelet_point.x(), lanelet_point.y()); - } - lanelet_polygon.outer().push_back(lanelet_polygon.outer().front()); - // check the object does not intersect the lanelet - if (!boost::geometry::disjoint(lanelet_polygon, obj_polygon)) { - target_indices.push_back(i); - is_filtered_object = true; - break; - } - } - - if (!is_filtered_object) { - other_indices.push_back(i); - } - } - - return std::make_pair(target_indices, other_indices); -} - -std::pair separateObjectsByLanelets( - const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets) -{ - PredictedObjects target_objects; - PredictedObjects other_objects; - - const auto [target_indices, other_indices] = - separateObjectIndicesByLanelets(objects, target_lanelets); - - target_objects.objects.reserve(target_indices.size()); - other_objects.objects.reserve(other_indices.size()); - - for (const size_t i : target_indices) { - target_objects.objects.push_back(objects.objects.at(i)); - } - - for (const size_t i : other_indices) { - other_objects.objects.push_back(objects.objects.at(i)); - } - - return std::make_pair(target_objects, other_objects); -} - std::vector calcObjectsDistanceToPath( const PredictedObjects & objects, const PathWithLaneId & ego_path) { @@ -2661,25 +2594,6 @@ std::vector expandLanelets( return expanded_drivable_lanes; } -PredictedObjects filterObjectsByVelocity(const PredictedObjects & objects, double lim_v) -{ - return filterObjectsByVelocity(objects, -lim_v, lim_v); -} - -PredictedObjects filterObjectsByVelocity( - const PredictedObjects & objects, double min_v, double max_v) -{ - PredictedObjects filtered; - filtered.header = objects.header; - for (const auto & obj : objects.objects) { - const auto v = std::abs(obj.kinematics.initial_twist_with_covariance.twist.linear.x); - if (min_v < v && v < max_v) { - filtered.objects.push_back(obj); - } - } - return filtered; -} - PathWithLaneId getCenterLinePathFromRootLanelet( const lanelet::ConstLanelet & root_lanelet, const std::shared_ptr & planner_data) @@ -3051,21 +2965,6 @@ lanelet::ConstLanelets calcLaneAroundPose( return current_lanes; } -std::vector getPredictedPathFromObj( - const ExtendedPredictedObject & obj, const bool & is_use_all_predicted_path) -{ - if (!is_use_all_predicted_path) { - const auto max_confidence_path = std::max_element( - obj.predicted_paths.begin(), obj.predicted_paths.end(), - [](const auto & path1, const auto & path2) { return path1.confidence < path2.confidence; }); - if (max_confidence_path != obj.predicted_paths.end()) { - return {*max_confidence_path}; - } - } - - return obj.predicted_paths; -} - bool checkPathRelativeAngle(const PathWithLaneId & path, const double angle_threshold) { // We need at least three points to compute relative angle @@ -3347,4 +3246,21 @@ void extractObstaclesFromDrivableArea( bound = drivable_area_processing::updateBoundary(bound, unique_polygons); } } + +lanelet::ConstLanelets combineLanelets( + const lanelet::ConstLanelets & base_lanes, const lanelet::ConstLanelets & added_lanes) +{ + lanelet::ConstLanelets combined_lanes = base_lanes; + for (const auto & added_lane : added_lanes) { + const auto it = std::find_if( + combined_lanes.begin(), combined_lanes.end(), + [&added_lane](const lanelet::ConstLanelet & lane) { return lane.id() == added_lane.id(); }); + if (it == combined_lanes.end()) { + combined_lanes.push_back(added_lane); + } + } + + return combined_lanes; +} + } // namespace behavior_path_planner::utils diff --git a/planning/behavior_path_planner/test/test_drivable_area_expansion.cpp b/planning/behavior_path_planner/test/test_drivable_area_expansion.cpp index b02df9753e0b5..605b3b96ee9b9 100644 --- a/planning/behavior_path_planner/test/test_drivable_area_expansion.cpp +++ b/planning/behavior_path_planner/test/test_drivable_area_expansion.cpp @@ -266,7 +266,6 @@ TEST(DrivableAreaExpansionProjection, expandDrivableArea) params.ego_right_offset = -2.0; } // we expect the drivable area to be expanded by 1m on each side - // BUT short paths, due to pruning at the edge of the driving area, there is no expansion drivable_area_expansion::expandDrivableArea( path, params, dynamic_objects, route_handler, path_lanes); // unchanged path points @@ -280,16 +279,16 @@ TEST(DrivableAreaExpansionProjection, expandDrivableArea) ASSERT_EQ(path.left_bound.size(), 3ul); EXPECT_NEAR(path.left_bound[0].x, 0.0, eps); EXPECT_NEAR(path.left_bound[0].y, 1.0, eps); - EXPECT_NEAR(path.left_bound[1].x, 1.0, eps); - EXPECT_NEAR(path.left_bound[1].y, 1.0, eps); + EXPECT_NEAR(path.left_bound[1].x, 0.0, eps); + EXPECT_NEAR(path.left_bound[1].y, 2.0, eps); EXPECT_NEAR(path.left_bound[2].x, 2.0, eps); - EXPECT_NEAR(path.left_bound[2].y, 1.0, eps); + EXPECT_NEAR(path.left_bound[2].y, 2.0, eps); // expanded right bound ASSERT_EQ(path.right_bound.size(), 3ul); EXPECT_NEAR(path.right_bound[0].x, 0.0, eps); - EXPECT_NEAR(path.right_bound[0].y, -1.0, eps); - EXPECT_NEAR(path.right_bound[1].x, 1.0, eps); - EXPECT_NEAR(path.right_bound[1].y, -1.0, eps); + EXPECT_NEAR(path.right_bound[0].y, -2.0, eps); + EXPECT_NEAR(path.right_bound[1].x, 2.0, eps); + EXPECT_NEAR(path.right_bound[1].y, -2.0, eps); EXPECT_NEAR(path.right_bound[2].x, 2.0, eps); EXPECT_NEAR(path.right_bound[2].y, -1.0, eps); } diff --git a/planning/behavior_path_planner/test/test_safety_check.cpp b/planning/behavior_path_planner/test/test_safety_check.cpp index 4058956ceff3f..e24f3274179fe 100644 --- a/planning/behavior_path_planner/test/test_safety_check.cpp +++ b/planning/behavior_path_planner/test/test_safety_check.cpp @@ -13,6 +13,7 @@ // limitations under the License. #include "behavior_path_planner/marker_utils/utils.hpp" +#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "behavior_path_planner/utils/path_safety_checker/safety_check.hpp" #include "tier4_autoware_utils/tier4_autoware_utils.hpp" @@ -35,7 +36,7 @@ using tier4_autoware_utils::Polygon2d; TEST(BehaviorPathPlanningSafetyUtilsTest, createExtendedEgoPolygon) { - using behavior_path_planner::utils::safety_check::createExtendedPolygon; + using behavior_path_planner::utils::path_safety_checker::createExtendedPolygon; vehicle_info_util::VehicleInfo vehicle_info; vehicle_info.max_longitudinal_offset_m = 4.0; @@ -128,7 +129,7 @@ TEST(BehaviorPathPlanningSafetyUtilsTest, createExtendedEgoPolygon) TEST(BehaviorPathPlanningSafetyUtilsTest, createExtendedObjPolygon) { - using behavior_path_planner::utils::safety_check::createExtendedPolygon; + using behavior_path_planner::utils::path_safety_checker::createExtendedPolygon; using tier4_autoware_utils::createPoint; using tier4_autoware_utils::createQuaternionFromYaw; @@ -176,7 +177,7 @@ TEST(BehaviorPathPlanningSafetyUtilsTest, createExtendedObjPolygon) TEST(BehaviorPathPlanningSafetyUtilsTest, calcRssDistance) { - using behavior_path_planner::utils::safety_check::calcRssDistance; + using behavior_path_planner::utils::path_safety_checker::calcRssDistance; { const double front_vel = 5.0; diff --git a/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp b/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp index bc3e07cf8dd74..fb1e93185f738 100644 --- a/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp +++ b/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp @@ -414,23 +414,27 @@ bool AdaptiveCruiseController::estimatePointVelocityFromObject( /* get object velocity, and current yaw */ bool get_obj = false; - double obj_vel; - double obj_yaw; + double obj_vel_norm; + double obj_vel_yaw; const Point collision_point_2d = convertPointRosToBoost(nearest_collision_p_ros); for (const auto & obj : object_ptr->objects) { const Polygon obj_poly = getPolygon( obj.kinematics.initial_pose_with_covariance.pose, obj.shape.dimensions, 0.0, param_.object_polygon_length_margin, param_.object_polygon_width_margin); if (boost::geometry::distance(obj_poly, collision_point_2d) <= 0) { - obj_vel = obj.kinematics.initial_twist_with_covariance.twist.linear.x; - obj_yaw = tf2::getYaw(obj.kinematics.initial_pose_with_covariance.pose.orientation); + obj_vel_norm = std::hypot( + obj.kinematics.initial_twist_with_covariance.twist.linear.x, + obj.kinematics.initial_twist_with_covariance.twist.linear.y); + obj_vel_yaw = std::atan2( + obj.kinematics.initial_twist_with_covariance.twist.linear.y, + obj.kinematics.initial_twist_with_covariance.twist.linear.x); get_obj = true; break; } } if (get_obj) { - *velocity = obj_vel * std::cos(obj_yaw - traj_yaw); + *velocity = obj_vel_norm * std::cos(obj_vel_yaw - traj_yaw); debug_values_.data.at(DBGVAL::ESTIMATED_VEL_OBJ) = *velocity; return true; } else { @@ -442,10 +446,15 @@ void AdaptiveCruiseController::calculateProjectedVelocityFromObject( const PredictedObject & object, const double traj_yaw, double * velocity) { /* get object velocity, and current yaw */ - double obj_vel = object.kinematics.initial_twist_with_covariance.twist.linear.x; - double obj_yaw = tf2::getYaw(object.kinematics.initial_pose_with_covariance.pose.orientation); - - *velocity = obj_vel * std::cos(tier4_autoware_utils::normalizeRadian(obj_yaw - traj_yaw)); + double obj_vel_norm = std::hypot( + object.kinematics.initial_twist_with_covariance.twist.linear.x, + object.kinematics.initial_twist_with_covariance.twist.linear.y); + double obj_vel_yaw = std::atan2( + object.kinematics.initial_twist_with_covariance.twist.linear.y, + object.kinematics.initial_twist_with_covariance.twist.linear.x); + + *velocity = + obj_vel_norm * std::cos(tier4_autoware_utils::normalizeRadian(obj_vel_yaw - traj_yaw)); debug_values_.data.at(DBGVAL::ESTIMATED_VEL_OBJ) = *velocity; }