From 575b21d43dc724fd5e8621816ac0177dc62e1182 Mon Sep 17 00:00:00 2001 From: Shumpei Wakabayashi <42209144+shmpwk@users.noreply.github.com> Date: Mon, 24 Apr 2023 09:47:56 +0900 Subject: [PATCH 01/16] fix(static_centerline_optimizer): show unsafe footprints with vector map builder (#3472) fix: unsafe footprints Signed-off-by: Shumpei Wakabayashi --- .../src/static_centerline_optimizer_node.cpp | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/planning/static_centerline_optimizer/src/static_centerline_optimizer_node.cpp b/planning/static_centerline_optimizer/src/static_centerline_optimizer_node.cpp index f2a7b1885198c..ab2788a7c755b 100644 --- a/planning/static_centerline_optimizer/src/static_centerline_optimizer_node.cpp +++ b/planning/static_centerline_optimizer/src/static_centerline_optimizer_node.cpp @@ -233,7 +233,6 @@ void StaticCenterlineOptimizerNode::run() load_map(lanelet2_input_file_path); const auto route_lane_ids = plan_route(start_lanelet_id, end_lanelet_id); const auto optimized_traj_points = plan_path(route_lane_ids); - evaluate(route_lane_ids, optimized_traj_points); save_map(lanelet2_output_file_path, route_lane_ids, optimized_traj_points); } @@ -445,6 +444,9 @@ void StaticCenterlineOptimizerNode::on_plan_path( return; } + // publish unsafe_footprints + evaluate(route_lane_ids, optimized_traj_points); + // create output data auto target_traj_point = optimized_traj_points.cbegin(); bool is_end_lanelet = false; @@ -486,9 +488,13 @@ void StaticCenterlineOptimizerNode::evaluate( const std::vector & optimized_traj_points) { const auto route_lanelets = get_lanelets_from_ids(*route_handler_ptr_, route_lane_ids); - - const auto dist_thresh_vec = declare_parameter>("marker_color_dist_thresh"); - const auto marker_color_vec = declare_parameter>("marker_color"); + const auto dist_thresh_vec = + has_parameter("marker_color_dist_thresh") + ? get_parameter("marker_color_dist_thresh").as_double_array() + : declare_parameter>("marker_color_dist_thresh"); + const auto marker_color_vec = has_parameter("marker_color") + ? get_parameter("marker_color").as_string_array() + : declare_parameter>("marker_color"); const auto get_marker_color = [&](const double dist) -> boost::optional> { for (size_t i = 0; i < dist_thresh_vec.size(); ++i) { const double dist_thresh = dist_thresh_vec.at(i); From 1d4bf8e3fd0bb0f906f73f7bf40a9a70414e6db0 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Mon, 24 Apr 2023 10:53:19 +0900 Subject: [PATCH 02/16] feat(behavior_path_planner): make drivable area expansion parameters common (#3499) * feat(behavior_path_planner): make drivable area expansion parameters common Signed-off-by: Takayuki Murooka * fix Signed-off-by: Takayuki Murooka * Nonoverlap -> NonOverlap Signed-off-by: Takayuki Murooka * fix Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka --- .../config/drivable_area_expansion.param.yaml | 23 ++------- .../behavior_path_planner/parameters.hpp | 5 -- .../scene_module/scene_module_interface.hpp | 12 +++++ .../utils/avoidance/avoidance_module_data.hpp | 5 -- .../drivable_area_expansion/parameters.hpp | 12 +++++ .../lane_change/lane_change_module_data.hpp | 5 -- .../utils/lane_following/module_data.hpp | 4 -- .../utils/pull_out/pull_out_parameters.hpp | 4 -- .../utils/pull_over/pull_over_parameters.hpp | 4 -- .../side_shift/side_shift_parameters.hpp | 4 -- .../src/behavior_path_planner_node.cpp | 51 ++++--------------- .../avoidance/avoidance_module.cpp | 6 --- .../scene_module/avoidance_by_lc/module.cpp | 20 +++----- .../scene_module/lane_change/bt_normal.cpp | 6 ++- .../external_request_lane_change_module.cpp | 18 +++---- .../src/scene_module/lane_change/normal.cpp | 15 +++--- .../scene_module/pull_out/pull_out_module.cpp | 22 +++----- .../pull_over/pull_over_module.cpp | 21 +++----- .../side_shift/side_shift_module.cpp | 11 ++-- .../src/utils/pull_out/shift_pull_out.cpp | 5 +- .../src/utils/pull_over/shift_pull_over.cpp | 5 +- .../behavior_path_planner/src/utils/utils.cpp | 7 +-- 22 files changed, 93 insertions(+), 172 deletions(-) diff --git a/planning/behavior_path_planner/config/drivable_area_expansion.param.yaml b/planning/behavior_path_planner/config/drivable_area_expansion.param.yaml index 57147dcbd23cf..1609cdbc60b7a 100644 --- a/planning/behavior_path_planner/config/drivable_area_expansion.param.yaml +++ b/planning/behavior_path_planner/config/drivable_area_expansion.param.yaml @@ -1,26 +1,9 @@ /**: ros__parameters: # Static expansion - avoidance: - drivable_area_right_bound_offset: 0.0 - drivable_area_left_bound_offset: 0.0 - drivable_area_types_to_skip: [road_border] - lane_change: - drivable_area_right_bound_offset: 0.0 - drivable_area_left_bound_offset: 0.0 - drivable_area_types_to_skip: [road_border] - pull_out: - drivable_area_right_bound_offset: 0.0 - drivable_area_left_bound_offset: 0.0 - drivable_area_types_to_skip: [road_border] - pull_over: - drivable_area_right_bound_offset: 0.0 - drivable_area_left_bound_offset: 0.0 - drivable_area_types_to_skip: [road_border] - side_shift: - drivable_area_right_bound_offset: 0.0 - drivable_area_left_bound_offset: 0.0 - drivable_area_types_to_skip: [road_border] + drivable_area_right_bound_offset: 0.0 + drivable_area_left_bound_offset: 0.0 + drivable_area_types_to_skip: [road_border] # Dynamic expansion by projecting the ego footprint along the path dynamic_expansion: diff --git a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp index 4b2bc67d6febe..b8e911d8dc8b6 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp @@ -107,11 +107,6 @@ struct BehaviorPathPlannerParameters double rear_vehicle_reaction_time; double rear_vehicle_safety_time_margin; - // lane following - double drivable_area_right_bound_offset; - double drivable_area_left_bound_offset; - std::vector drivable_area_types_to_skip; - // lane change double lane_change_prepare_duration; }; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp index 4ec88e4024be3..28b55aa3c196d 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp @@ -354,6 +354,18 @@ class SceneModuleInterface return std::abs(planner_data_->self_odometry->twist.twist.linear.x); } + std::vector getNonOverlappingExpandedLanes( + PathWithLaneId & path, const std::vector & lanes) const + { + const auto & dp = planner_data_->drivable_area_expansion_parameters; + + const auto shorten_lanes = utils::cutOverlappedLanes(path, lanes); + const auto expanded_lanes = utils::expandLanelets( + shorten_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset, + dp.drivable_area_types_to_skip); + return expanded_lanes; + } + rclcpp::Clock::SharedPtr clock_; std::shared_ptr planner_data_; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp index aa2eef71384fc..f0f5067611e0d 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp @@ -247,11 +247,6 @@ struct AvoidanceParameters // parameters depend on object class std::unordered_map object_parameters; - // drivable area expansion - double drivable_area_right_bound_offset{}; - double drivable_area_left_bound_offset{}; - std::vector drivable_area_types_to_skip{}; - // clip left and right bounds for objects bool enable_bound_clipping{false}; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/parameters.hpp index b8d8a235580e4..a4c11c68d13ec 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/parameters.hpp @@ -28,6 +28,9 @@ namespace drivable_area_expansion struct DrivableAreaExpansionParameters { + static constexpr auto DRIVABLE_AREA_RIGHT_BOUND_OFFSET_PARAM = "drivable_area_right_bound_offset"; + static constexpr auto DRIVABLE_AREA_LEFT_BOUND_OFFSET_PARAM = "drivable_area_left_bound_offset"; + static constexpr auto DRIVABLE_AREA_TYPES_TO_SKIP_PARAM = "drivable_area_types_to_skip"; static constexpr auto ENABLED_PARAM = "dynamic_expansion.enabled"; static constexpr auto EGO_EXTRA_OFFSET_FRONT = "dynamic_expansion.ego.extra_footprint_offset.front"; @@ -55,6 +58,9 @@ struct DrivableAreaExpansionParameters static constexpr auto EXTRA_COMPENSATE_PARAM = "dynamic_expansion.avoid_linestring.compensate.extra_distance"; + double drivable_area_right_bound_offset; + double drivable_area_left_bound_offset; + std::vector drivable_area_types_to_skip; bool enabled = false; std::string expansion_method{}; double avoid_linestring_dist{}; @@ -83,6 +89,12 @@ struct DrivableAreaExpansionParameters void init(rclcpp::Node & node) { + drivable_area_right_bound_offset = + node.declare_parameter(DRIVABLE_AREA_RIGHT_BOUND_OFFSET_PARAM); + drivable_area_left_bound_offset = + node.declare_parameter(DRIVABLE_AREA_LEFT_BOUND_OFFSET_PARAM); + drivable_area_types_to_skip = + node.declare_parameter>(DRIVABLE_AREA_TYPES_TO_SKIP_PARAM); enabled = node.declare_parameter(ENABLED_PARAM); max_expansion_distance = node.declare_parameter(MAX_EXP_DIST_PARAM); max_path_arc_length = node.declare_parameter(MAX_PATH_ARC_LENGTH_PARAM); 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 52682206176f0..a7bab00979c9e 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 @@ -55,11 +55,6 @@ struct LaneChangeParameters double abort_delta_time{3.0}; double abort_max_lateral_jerk{10.0}; - // drivable area expansion - double drivable_area_right_bound_offset{0.0}; - double drivable_area_left_bound_offset{0.0}; - std::vector drivable_area_types_to_skip{}; - // debug marker bool publish_debug_marker{false}; }; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_following/module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_following/module_data.hpp index d4eab4835dddb..118fb39606012 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_following/module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_following/module_data.hpp @@ -24,10 +24,6 @@ namespace behavior_path_planner struct LaneFollowingParameters { double lane_change_prepare_duration; - // drivable area expansion - double drivable_area_right_bound_offset; - double drivable_area_left_bound_offset; - std::vector drivable_area_types_to_skip{}; // finding closest lanelet double distance_threshold; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_out/pull_out_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_out/pull_out_parameters.hpp index 49a72d4b79524..ce4629bf9da19 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_out/pull_out_parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_out/pull_out_parameters.hpp @@ -51,10 +51,6 @@ struct PullOutParameters double backward_search_resolution; double backward_path_update_duration; double ignore_distance_from_lane_end; - // drivable area expansion - double drivable_area_right_bound_offset; - double drivable_area_left_bound_offset; - std::vector drivable_area_types_to_skip; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_over/pull_over_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_over/pull_over_parameters.hpp index 63b8eea71977c..477f6dae46562 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_over/pull_over_parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_over/pull_over_parameters.hpp @@ -99,10 +99,6 @@ struct PullOverParameters bool enable_collision_check_at_prepare_phase; bool use_predicted_path_outside_lanelet; bool use_all_predicted_path; - // drivable area expansion - double drivable_area_right_bound_offset; - double drivable_area_left_bound_offset; - std::vector drivable_area_types_to_skip; // debug bool print_debug_info; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/side_shift/side_shift_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/side_shift/side_shift_parameters.hpp index ee139b0774803..1087a1226d711 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/side_shift/side_shift_parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/side_shift/side_shift_parameters.hpp @@ -37,10 +37,6 @@ struct SideShiftParameters double drivable_area_width; double drivable_area_height; double shift_request_time_limit; - // drivable area expansion - double drivable_area_right_bound_offset; - double drivable_area_left_bound_offset; - std::vector drivable_area_types_to_skip; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index a48a4572ef2fc..bd995f556da53 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -432,14 +432,6 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() p.rear_vehicle_reaction_time = declare_parameter("rear_vehicle_reaction_time"); p.rear_vehicle_safety_time_margin = declare_parameter("rear_vehicle_safety_time_margin"); - // lane following - p.drivable_area_right_bound_offset = - declare_parameter("lane_following.drivable_area_right_bound_offset"); - p.drivable_area_left_bound_offset = - declare_parameter("lane_following.drivable_area_left_bound_offset"); - p.drivable_area_types_to_skip = - declare_parameter>("lane_following.drivable_area_types_to_skip"); - // lane change p.lane_change_prepare_duration = declare_parameter("lane_change.lane_change_prepare_duration"); @@ -463,13 +455,7 @@ SideShiftParameters BehaviorPathPlannerNode::getSideShiftParam() p.shifting_lateral_jerk = declare_parameter(ns + "shifting_lateral_jerk"); p.min_shifting_distance = declare_parameter(ns + "min_shifting_distance"); p.min_shifting_speed = declare_parameter(ns + "min_shifting_speed"); - p.drivable_area_right_bound_offset = - declare_parameter(ns + "drivable_area_right_bound_offset"); p.shift_request_time_limit = declare_parameter(ns + "shift_request_time_limit"); - p.drivable_area_left_bound_offset = - declare_parameter(ns + "drivable_area_left_bound_offset"); - p.drivable_area_types_to_skip = - declare_parameter>(ns + "drivable_area_types_to_skip"); return p; } @@ -509,12 +495,6 @@ AvoidanceParameters BehaviorPathPlannerNode::getAvoidanceParam() declare_parameter(ns + "detection_area_right_expand_dist"); p.detection_area_left_expand_dist = declare_parameter(ns + "detection_area_left_expand_dist"); - p.drivable_area_right_bound_offset = - declare_parameter(ns + "drivable_area_right_bound_offset"); - p.drivable_area_left_bound_offset = - declare_parameter(ns + "drivable_area_left_bound_offset"); - p.drivable_area_types_to_skip = - declare_parameter>(ns + "drivable_area_types_to_skip"); p.enable_bound_clipping = declare_parameter(ns + "enable_bound_clipping"); p.enable_avoidance_over_same_direction = declare_parameter(ns + "enable_avoidance_over_same_direction"); @@ -704,14 +684,6 @@ LaneChangeParameters BehaviorPathPlannerNode::getLaneChangeParam() p.abort_delta_time = declare_parameter(parameter("abort_delta_time")); p.abort_max_lateral_jerk = declare_parameter(parameter("abort_max_lateral_jerk")); - // drivable area expansion - p.drivable_area_right_bound_offset = - declare_parameter(parameter("drivable_area_right_bound_offset")); - p.drivable_area_left_bound_offset = - declare_parameter(parameter("drivable_area_left_bound_offset")); - p.drivable_area_types_to_skip = - declare_parameter>(parameter("drivable_area_types_to_skip")); - // debug marker p.publish_debug_marker = declare_parameter(parameter("publish_debug_marker")); @@ -829,13 +801,6 @@ PullOverParameters BehaviorPathPlannerNode::getPullOverParam() p.use_predicted_path_outside_lanelet = declare_parameter(ns + "use_predicted_path_outside_lanelet"); p.use_all_predicted_path = declare_parameter(ns + "use_all_predicted_path"); - // drivable area - p.drivable_area_right_bound_offset = - declare_parameter(ns + "drivable_area_right_bound_offset"); - p.drivable_area_left_bound_offset = - declare_parameter(ns + "drivable_area_left_bound_offset"); - p.drivable_area_types_to_skip = - declare_parameter>(ns + "drivable_area_types_to_skip"); // debug p.print_debug_info = declare_parameter(ns + "print_debug_info"); @@ -955,13 +920,6 @@ PullOutParameters BehaviorPathPlannerNode::getPullOutParam() p.backward_search_resolution = declare_parameter(ns + "backward_search_resolution"); p.backward_path_update_duration = declare_parameter(ns + "backward_path_update_duration"); p.ignore_distance_from_lane_end = declare_parameter(ns + "ignore_distance_from_lane_end"); - // drivable area - p.drivable_area_right_bound_offset = - declare_parameter(ns + "drivable_area_right_bound_offset"); - p.drivable_area_left_bound_offset = - declare_parameter(ns + "drivable_area_left_bound_offset"); - p.drivable_area_types_to_skip = - declare_parameter>(ns + "drivable_area_types_to_skip"); // validation of parameters if (p.pull_out_sampling_num < 1) { @@ -1536,6 +1494,15 @@ SetParametersResult BehaviorPathPlannerNode::onSetParam( // Drivable area expansion parameters using drivable_area_expansion::DrivableAreaExpansionParameters; const std::lock_guard lock(mutex_pd_); // for planner_data_ + updateParam( + parameters, DrivableAreaExpansionParameters::DRIVABLE_AREA_RIGHT_BOUND_OFFSET_PARAM, + planner_data_->drivable_area_expansion_parameters.drivable_area_right_bound_offset); + updateParam( + parameters, DrivableAreaExpansionParameters::DRIVABLE_AREA_LEFT_BOUND_OFFSET_PARAM, + planner_data_->drivable_area_expansion_parameters.drivable_area_left_bound_offset); + updateParam( + parameters, DrivableAreaExpansionParameters::DRIVABLE_AREA_TYPES_TO_SKIP_PARAM, + planner_data_->drivable_area_expansion_parameters.drivable_area_types_to_skip); updateParam( parameters, DrivableAreaExpansionParameters::ENABLED_PARAM, planner_data_->drivable_area_expansion_parameters.enabled); 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 60351778802eb..9ac37adf1dc43 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 @@ -2514,12 +2514,6 @@ void AvoidanceModule::generateExtendedDrivableArea(BehaviorModuleOutput & output output.drivable_lanes.push_back(current_drivable_lanes); } - const auto shorten_lanes = utils::cutOverlappedLanes(path, output.drivable_lanes); - - const auto extended_lanes = utils::expandLanelets( - shorten_lanes, parameters_->drivable_area_left_bound_offset, - parameters_->drivable_area_right_bound_offset, parameters_->drivable_area_types_to_skip); - { const auto & p = planner_data_->parameters; generateDrivableArea( diff --git a/planning/behavior_path_planner/src/scene_module/avoidance_by_lc/module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance_by_lc/module.cpp index ac824c8492b65..320520cef5ba2 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance_by_lc/module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance_by_lc/module.cpp @@ -552,13 +552,9 @@ PathWithLaneId AvoidanceByLCModule::getReferencePath() const lane_change_buffer); const auto drivable_lanes = utils::generateDrivableLanes(current_lanes); - const auto shorten_lanes = utils::cutOverlappedLanes(reference_path, drivable_lanes); - const auto expanded_lanes = utils::expandLanelets( - shorten_lanes, parameters_->lane_change->drivable_area_left_bound_offset, - parameters_->lane_change->drivable_area_right_bound_offset, - parameters_->lane_change->drivable_area_types_to_skip); + const auto target_drivable_lanes = getNonOverlappingExpandedLanes(reference_path, drivable_lanes); utils::generateDrivableArea( - reference_path, expanded_lanes, common_parameters.vehicle_length, planner_data_); + reference_path, target_drivable_lanes, common_parameters.vehicle_length, planner_data_); return reference_path; } @@ -695,9 +691,10 @@ bool AvoidanceByLCModule::isValidPath(const PathWithLaneId & path) const const auto drivable_lanes = utils::lane_change::generateDrivableLanes( *route_handler, utils::extendLanes(route_handler, status_.current_lanes), utils::extendLanes(route_handler, status_.lane_change_lanes)); + const auto & dp = planner_data_->drivable_area_expansion_parameters; const auto expanded_lanes = utils::expandLanelets( - drivable_lanes, parameters_->lane_change->drivable_area_left_bound_offset, - parameters_->lane_change->drivable_area_right_bound_offset); + drivable_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset, + dp.drivable_area_types_to_skip); const auto lanelets = utils::transformToLanelets(expanded_lanes); // check path points are in any lanelets @@ -937,12 +934,9 @@ void AvoidanceByLCModule::generateExtendedDrivableArea(PathWithLaneId & path) const auto & route_handler = planner_data_->route_handler; const auto drivable_lanes = utils::lane_change::generateDrivableLanes( *route_handler, status_.current_lanes, status_.lane_change_lanes); - const auto shorten_lanes = utils::cutOverlappedLanes(path, drivable_lanes); - const auto expanded_lanes = utils::expandLanelets( - shorten_lanes, parameters_->lane_change->drivable_area_left_bound_offset, - parameters_->lane_change->drivable_area_right_bound_offset); + const auto target_drivable_lanes = getNonOverlappingExpandedLanes(path, drivable_lanes); utils::generateDrivableArea( - path, expanded_lanes, common_parameters.vehicle_length, planner_data_); + path, target_drivable_lanes, common_parameters.vehicle_length, planner_data_); } bool AvoidanceByLCModule::isApprovedPathSafe(Pose & ego_pose_before_collision) const diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/bt_normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/bt_normal.cpp index a4e7c9f8baaec..ac4643dfbee2f 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/bt_normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/bt_normal.cpp @@ -68,11 +68,13 @@ PathWithLaneId NormalLaneChangeBT::getReferencePath() const *route_handler, reference_path, current_lanes, parameters_->prepare_duration, lane_change_buffer); + const auto & dp = planner_data_->drivable_area_expansion_parameters; + const auto drivable_lanes = utils::generateDrivableLanes(current_lanes); const auto shorten_lanes = utils::cutOverlappedLanes(reference_path, drivable_lanes); const auto expanded_lanes = utils::expandLanelets( - shorten_lanes, parameters_->drivable_area_left_bound_offset, - parameters_->drivable_area_right_bound_offset, parameters_->drivable_area_types_to_skip); + shorten_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset, + dp.drivable_area_types_to_skip); utils::generateDrivableArea( reference_path, expanded_lanes, common_parameters.vehicle_length, planner_data_); diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/external_request_lane_change_module.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/external_request_lane_change_module.cpp index db5854633f0b3..a6266e836ebf5 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/external_request_lane_change_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/external_request_lane_change_module.cpp @@ -295,12 +295,9 @@ PathWithLaneId ExternalRequestLaneChangeModule::getReferencePath() const lane_change_buffer); const auto drivable_lanes = utils::generateDrivableLanes(current_lanes); - const auto shorten_lanes = utils::cutOverlappedLanes(reference_path, drivable_lanes); - const auto expanded_lanes = utils::expandLanelets( - drivable_lanes, parameters_->drivable_area_left_bound_offset, - parameters_->drivable_area_right_bound_offset); + const auto target_drivable_lanes = getNonOverlappingExpandedLanes(reference_path, drivable_lanes); utils::generateDrivableArea( - reference_path, expanded_lanes, common_parameters.vehicle_length, planner_data_); + reference_path, target_drivable_lanes, common_parameters.vehicle_length, planner_data_); return reference_path; } @@ -397,14 +394,15 @@ bool ExternalRequestLaneChangeModule::isSafe() const bool ExternalRequestLaneChangeModule::isValidPath(const PathWithLaneId & path) const { const auto & route_handler = planner_data_->route_handler; + const auto & dp = planner_data_->drivable_area_expansion_parameters; // check lane departure const auto drivable_lanes = utils::lane_change::generateDrivableLanes( *route_handler, utils::extendLanes(route_handler, status_.current_lanes), utils::extendLanes(route_handler, status_.lane_change_lanes)); const auto expanded_lanes = utils::expandLanelets( - drivable_lanes, parameters_->drivable_area_left_bound_offset, - parameters_->drivable_area_right_bound_offset); + drivable_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset, + dp.drivable_area_types_to_skip); const auto lanelets = utils::transformToLanelets(expanded_lanes); // check path points are in any lanelets @@ -625,12 +623,12 @@ void ExternalRequestLaneChangeModule::generateExtendedDrivableArea(PathWithLaneI { const auto & common_parameters = planner_data_->parameters; const auto & route_handler = planner_data_->route_handler; + const auto & dp = planner_data_->drivable_area_expansion_parameters; const auto drivable_lanes = utils::lane_change::generateDrivableLanes( *route_handler, status_.current_lanes, status_.lane_change_lanes); const auto expanded_lanes = utils::expandLanelets( - drivable_lanes, parameters_->drivable_area_left_bound_offset, - parameters_->drivable_area_right_bound_offset); - + drivable_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset, + dp.drivable_area_types_to_skip); utils::generateDrivableArea( path, expanded_lanes, common_parameters.vehicle_length, planner_data_); } 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 453ec9d96b6d7..61f4cb1e620b2 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 @@ -103,13 +103,14 @@ PathWithLaneId NormalLaneChange::generatePlannedPath() void NormalLaneChange::generateExtendedDrivableArea(PathWithLaneId & path) { + const auto & common_parameters = planner_data_->parameters; + const auto & dp = planner_data_->drivable_area_expansion_parameters; + const auto drivable_lanes = getDrivableLanes(); const auto shorten_lanes = utils::cutOverlappedLanes(path, drivable_lanes); - - const auto & common_parameters = planner_data_->parameters; const auto expanded_lanes = utils::expandLanelets( - shorten_lanes, parameters_->drivable_area_left_bound_offset, - parameters_->drivable_area_right_bound_offset, parameters_->drivable_area_types_to_skip); + shorten_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset, + dp.drivable_area_types_to_skip); utils::generateDrivableArea( path, expanded_lanes, common_parameters.vehicle_length, planner_data_); } @@ -575,14 +576,16 @@ void NormalLaneChange::calcTurnSignalInfo() bool NormalLaneChange::isValidPath(const PathWithLaneId & path) const { const auto & route_handler = planner_data_->route_handler; + const auto & dp = planner_data_->drivable_area_expansion_parameters; // check lane departure const auto drivable_lanes = utils::lane_change::generateDrivableLanes( *route_handler, utils::extendLanes(route_handler, status_.current_lanes), utils::extendLanes(route_handler, status_.lane_change_lanes)); const auto expanded_lanes = utils::expandLanelets( - drivable_lanes, parameters_->drivable_area_left_bound_offset, - parameters_->drivable_area_right_bound_offset); + drivable_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset, + dp.drivable_area_types_to_skip); + const auto lanelets = utils::transformToLanelets(expanded_lanes); // check path points are in any lanelets diff --git a/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp b/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp index 963352a3a13ac..c56059eb1258a 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp @@ -198,12 +198,9 @@ BehaviorModuleOutput PullOutModule::plan() path = status_.backward_path; } - const auto shorten_lanes = utils::cutOverlappedLanes(path, status_.lanes); - const auto expanded_lanes = utils::expandLanelets( - shorten_lanes, parameters_->drivable_area_left_bound_offset, - parameters_->drivable_area_right_bound_offset, parameters_->drivable_area_types_to_skip); + const auto target_drivable_lanes = getNonOverlappingExpandedLanes(path, status_.lanes); utils::generateDrivableArea( - path, expanded_lanes, planner_data_->parameters.vehicle_length, planner_data_); + path, target_drivable_lanes, planner_data_->parameters.vehicle_length, planner_data_); output.path = std::make_shared(path); output.reference_path = getPreviousModuleOutput().reference_path; @@ -310,13 +307,13 @@ BehaviorModuleOutput PullOutModule::planWaitingApproval() const auto current_lanes = utils::getExtendedCurrentLanes(planner_data_); const auto pull_out_lanes = pull_out_utils::getPullOutLanes(planner_data_); + auto stop_path = status_.back_finished ? getCurrentPath() : status_.backward_path; const auto drivable_lanes = utils::generateDrivableLanesWithShoulderLanes(current_lanes, pull_out_lanes); + const auto & dp = planner_data_->drivable_area_expansion_parameters; const auto expanded_lanes = utils::expandLanelets( - drivable_lanes, parameters_->drivable_area_left_bound_offset, - parameters_->drivable_area_right_bound_offset, parameters_->drivable_area_types_to_skip); - - auto stop_path = status_.back_finished ? getCurrentPath() : status_.backward_path; + drivable_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset, + dp.drivable_area_types_to_skip); utils::generateDrivableArea( stop_path, expanded_lanes, planner_data_->parameters.vehicle_length, planner_data_); for (auto & p : stop_path.points) { @@ -524,12 +521,9 @@ PathWithLaneId PullOutModule::generateStopPath() const path.points.push_back(toPathPointWithLaneId(moved_pose)); // generate drivable area - const auto shorten_lanes = utils::cutOverlappedLanes(path, status_.lanes); - const auto expanded_lanes = utils::expandLanelets( - shorten_lanes, parameters_->drivable_area_left_bound_offset, - parameters_->drivable_area_right_bound_offset, parameters_->drivable_area_types_to_skip); + const auto target_drivable_lanes = getNonOverlappingExpandedLanes(path, status_.lanes); utils::generateDrivableArea( - path, expanded_lanes, planner_data_->parameters.vehicle_length, planner_data_); + path, target_drivable_lanes, planner_data_->parameters.vehicle_length, planner_data_); return path; } diff --git a/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp b/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp index 1d6e263ca2bcc..6920dcb0e9220 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp @@ -618,12 +618,9 @@ BehaviorModuleOutput PullOverModule::plan() for (auto & path : status_.pull_over_path->partial_paths) { const size_t ego_idx = planner_data_->findEgoIndex(path.points); utils::clipPathLength(path, ego_idx, planner_data_->parameters); - const auto shorten_lanes = utils::cutOverlappedLanes(path, status_.lanes); - const auto expanded_lanes = utils::expandLanelets( - shorten_lanes, parameters_->drivable_area_left_bound_offset, - parameters_->drivable_area_right_bound_offset, parameters_->drivable_area_types_to_skip); + const auto target_drivable_lanes = getNonOverlappingExpandedLanes(path, status_.lanes); utils::generateDrivableArea( - path, expanded_lanes, planner_data_->parameters.vehicle_length, planner_data_); + path, target_drivable_lanes, planner_data_->parameters.vehicle_length, planner_data_); } } @@ -857,12 +854,9 @@ PathWithLaneId PullOverModule::generateStopPath() // generate drivable area const auto drivable_lanes = utils::generateDrivableLanes(status_.current_lanes); - const auto shorten_lanes = utils::cutOverlappedLanes(reference_path, drivable_lanes); - const auto expanded_lanes = utils::expandLanelets( - shorten_lanes, parameters_->drivable_area_left_bound_offset, - parameters_->drivable_area_right_bound_offset, parameters_->drivable_area_types_to_skip); + const auto target_drivable_lanes = getNonOverlappingExpandedLanes(reference_path, drivable_lanes); utils::generateDrivableArea( - reference_path, expanded_lanes, common_parameters.vehicle_length, planner_data_); + reference_path, target_drivable_lanes, common_parameters.vehicle_length, planner_data_); return reference_path; } @@ -895,12 +889,9 @@ PathWithLaneId PullOverModule::generateFeasibleStopPath() // generate drivable area const auto drivable_lanes = utils::generateDrivableLanes(status_.current_lanes); - const auto shorten_lanes = utils::cutOverlappedLanes(stop_path, drivable_lanes); - const auto expanded_lanes = utils::expandLanelets( - shorten_lanes, parameters_->drivable_area_left_bound_offset, - parameters_->drivable_area_right_bound_offset, parameters_->drivable_area_types_to_skip); + const auto target_drivable_lanes = getNonOverlappingExpandedLanes(stop_path, drivable_lanes); utils::generateDrivableArea( - stop_path, expanded_lanes, common_parameters.vehicle_length, planner_data_); + stop_path, target_drivable_lanes, common_parameters.vehicle_length, planner_data_); return stop_path; } diff --git a/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp b/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp index 9624dfc904e93..1d5a875c3ada9 100644 --- a/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp @@ -419,21 +419,20 @@ double SideShiftModule::getClosestShiftLength() const void SideShiftModule::adjustDrivableArea(ShiftedPath * path) const { + const auto & dp = planner_data_->drivable_area_expansion_parameters; const auto itr = std::minmax_element(path->shift_length.begin(), path->shift_length.end()); constexpr double threshold = 0.1; constexpr double margin = 0.5; const double left_offset = std::max( - *itr.second + (*itr.first > threshold ? margin : 0.0), - parameters_->drivable_area_left_bound_offset); + *itr.second + (*itr.first > threshold ? margin : 0.0), dp.drivable_area_left_bound_offset); const double right_offset = -std::min( - *itr.first - (*itr.first < -threshold ? margin : 0.0), - -parameters_->drivable_area_right_bound_offset); + *itr.first - (*itr.first < -threshold ? margin : 0.0), -dp.drivable_area_right_bound_offset); const auto drivable_lanes = utils::generateDrivableLanes(current_lanelets_); const auto shorten_lanes = utils::cutOverlappedLanes(path->path, drivable_lanes); - const auto expanded_lanes = utils::expandLanelets( - shorten_lanes, left_offset, right_offset, parameters_->drivable_area_types_to_skip); + const auto expanded_lanes = + utils::expandLanelets(shorten_lanes, left_offset, right_offset, dp.drivable_area_types_to_skip); { const auto & p = planner_data_->parameters; diff --git a/planning/behavior_path_planner/src/utils/pull_out/shift_pull_out.cpp b/planning/behavior_path_planner/src/utils/pull_out/shift_pull_out.cpp index 319cfeacd76ad..996b54fff5752 100644 --- a/planning/behavior_path_planner/src/utils/pull_out/shift_pull_out.cpp +++ b/planning/behavior_path_planner/src/utils/pull_out/shift_pull_out.cpp @@ -92,9 +92,10 @@ boost::optional ShiftPullOut::plan(Pose start_pose, Pose goal_pose) // check lane departure const auto drivable_lanes = utils::generateDrivableLanesWithShoulderLanes(road_lanes, shoulder_lanes); + const auto & dp = planner_data_->drivable_area_expansion_parameters; const auto expanded_lanes = utils::expandLanelets( - drivable_lanes, parameters_.drivable_area_left_bound_offset, - parameters_.drivable_area_right_bound_offset); + drivable_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset, + dp.drivable_area_types_to_skip); if (lane_departure_checker_->checkPathWillLeaveLane( utils::transformToLanelets(expanded_lanes), path_start_to_end)) { continue; diff --git a/planning/behavior_path_planner/src/utils/pull_over/shift_pull_over.cpp b/planning/behavior_path_planner/src/utils/pull_over/shift_pull_over.cpp index dcb2c3aa26fc0..5fb58aa6cc443 100644 --- a/planning/behavior_path_planner/src/utils/pull_over/shift_pull_over.cpp +++ b/planning/behavior_path_planner/src/utils/pull_over/shift_pull_over.cpp @@ -207,9 +207,10 @@ boost::optional ShiftPullOver::generatePullOverPath( // check if the parking path will leave lanes const auto drivable_lanes = utils::generateDrivableLanesWithShoulderLanes(road_lanes, shoulder_lanes); + const auto & dp = planner_data_->drivable_area_expansion_parameters; const auto expanded_lanes = utils::expandLanelets( - drivable_lanes, parameters_.drivable_area_left_bound_offset, - parameters_.drivable_area_right_bound_offset); + drivable_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset, + dp.drivable_area_types_to_skip); if (lane_departure_checker_.checkPathWillLeaveLane( utils::transformToLanelets(expanded_lanes), pull_over_path.getParkingPath())) { return {}; diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index 66e4a2ed3f0e3..60b09bba0b117 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -1799,11 +1799,12 @@ BehaviorModuleOutput getReferencePath( const auto drivable_lanelets = getLaneletsFromPath(reference_path, route_handler); const auto drivable_lanes = generateDrivableLanes(drivable_lanelets); - const auto shorten_lanes = cutOverlappedLanes(reference_path, drivable_lanes); + const auto & dp = planner_data->drivable_area_expansion_parameters; + const auto shorten_lanes = cutOverlappedLanes(reference_path, drivable_lanes); const auto expanded_lanes = expandLanelets( - shorten_lanes, p.drivable_area_left_bound_offset, p.drivable_area_right_bound_offset, - p.drivable_area_types_to_skip); + shorten_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset, + dp.drivable_area_types_to_skip); generateDrivableArea(reference_path, expanded_lanes, p.vehicle_length, planner_data); From a3a653de17753181c245d173b3dae6d905307a6c Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Mon, 24 Apr 2023 11:47:22 +0900 Subject: [PATCH 03/16] feat(behavior_velocity_planner::intersection): add occlusion detection feature (#3458) * migrated Signed-off-by: Mamoru Sobue * WIP Signed-off-by: Mamoru Sobue * IntersectionModuleManager own one rtc_interface for intersection_occlusion Signed-off-by: Mamoru Sobue * divide occlusion safety and activated Signed-off-by: Mamoru Sobue * fixed to update occlusion cooperate status at construction Signed-off-by: Mamoru Sobue * fixed getOcclusionFirstStopSafety() Signed-off-by: Mamoru Sobue * fixed not to show both intersection and intersection_occlusion after passed 1st stop line Signed-off-by: Mamoru Sobue * fixed the intersection_occlusion/inersection stop position afte r CLEARED Signed-off-by: Mamoru Sobue * if occlusion cleared when eog is before 1st stop line, set stop line to 1st stop line and clear prev_occlusion_stop_line_pose_ Signed-off-by: Mamoru Sobue * (misc) fix viz, sync param Signed-off-by: Mamoru Sobue * fixed vehicle footprint offset calculation Signed-off-by: Mamoru Sobue * add occcupancy_grid_map method/param var to launcher Signed-off-by: Mamoru Sobue * migrated latest Signed-off-by: Mamoru Sobue * use static pass judge line Signed-off-by: Mamoru Sobue * removed some params Signed-off-by: Mamoru Sobue * organized param Signed-off-by: Mamoru Sobue * add occlusion enable flag Signed-off-by: Mamoru Sobue * revert occupancy grid settings in this PR Signed-off-by: Mamoru Sobue * remove comment Signed-off-by: Mamoru Sobue * fixed pass judge line collision detection to original Signed-off-by: Mamoru Sobue * style(pre-commit): autofix * use vehicle_length for static pass judge line Signed-off-by: Mamoru Sobue * fixed virtual wall marker Signed-off-by: Mamoru Sobue --------- Signed-off-by: Mamoru Sobue Signed-off-by: Mamoru Sobue Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../src/rtc_manager_panel.cpp | 5 +- .../config/intersection.param.yaml | 11 + .../scene_module/intersection/manager.hpp | 10 + .../intersection/scene_intersection.hpp | 80 ++- .../scene_module/intersection/util.hpp | 27 +- .../scene_module/intersection/util_type.hpp | 15 +- .../scene_module/scene_module_interface.hpp | 12 +- .../behavior_velocity_planner/package.xml | 3 + .../src/scene_module/intersection/debug.cpp | 49 +- .../src/scene_module/intersection/manager.cpp | 211 +++++-- .../intersection/scene_intersection.cpp | 582 +++++++++++++++--- .../scene_merge_from_private_road.cpp | 8 +- .../src/scene_module/intersection/util.cpp | 293 ++++++--- .../config/rtc_auto_mode_manager.param.yaml | 2 + .../src/rtc_auto_mode_manager_interface.cpp | 2 + planning/rtc_interface/src/rtc_interface.cpp | 2 + 16 files changed, 1074 insertions(+), 238 deletions(-) diff --git a/common/rtc_manager_rviz_plugin/src/rtc_manager_panel.cpp b/common/rtc_manager_rviz_plugin/src/rtc_manager_panel.cpp index 3f6724e64f1f2..5adb1bf385f4b 100644 --- a/common/rtc_manager_rviz_plugin/src/rtc_manager_panel.cpp +++ b/common/rtc_manager_rviz_plugin/src/rtc_manager_panel.cpp @@ -90,6 +90,9 @@ std::string getModuleName(const uint8_t module_type) case Module::OCCLUSION_SPOT: { return "occlusion_spot"; } + case Module::INTERSECTION_OCCLUSION: { + return "intersection_occlusion"; + } } return "NONE"; } @@ -111,7 +114,7 @@ bool isPathChangeModule(const uint8_t module_type) RTCManagerPanel::RTCManagerPanel(QWidget * parent) : rviz_common::Panel(parent) { // TODO(tanaka): replace this magic number to Module::SIZE - const size_t module_size = 18; + const size_t module_size = 19; auto_modes_.reserve(module_size); auto * v_layout = new QVBoxLayout; auto vertical_header = new QHeaderView(Qt::Vertical); diff --git a/planning/behavior_velocity_planner/config/intersection.param.yaml b/planning/behavior_velocity_planner/config/intersection.param.yaml index 8a69d80394a68..927a27a745b3f 100644 --- a/planning/behavior_velocity_planner/config/intersection.param.yaml +++ b/planning/behavior_velocity_planner/config/intersection.param.yaml @@ -28,5 +28,16 @@ collision_end_margin_time: 6.0 # [s] this + state_transit_margin_time should be higher to account for collision with slow/decelerating object keep_detection_vel_thr: 0.833 # == 3.0km/h. keep detection if ego is ego.vel < keep_detection_vel_thr + occlusion: + enable: true + occlusion_detection_area_length: 70.0 # [m] + occlusion_creep_velocity: 0.8333 # the creep velocity to occlusion limit stop line + free_space_max: 43 + occupied_min: 58 + do_dp: true + before_creep_stop_time: 1.0 # [s] + min_vehicle_brake_for_rss: -2.5 # [m/s^2] + max_vehicle_velocity_for_rss: 16.66 # [m/s] == 60kmph + merge_from_private: stop_duration_sec: 1.0 diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/manager.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/manager.hpp index 7e22a2be65d71..c55f7e7f3f58d 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/manager.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/manager.hpp @@ -26,6 +26,7 @@ #include #include #include +#include namespace behavior_velocity_planner { @@ -38,6 +39,9 @@ class IntersectionModuleManager : public SceneModuleManagerInterfaceWithRTC private: IntersectionModule::PlannerParam intersection_param_; + // additional for INTERSECTION_OCCLUSION + RTCInterface occlusion_rtc_interface_; + std::unordered_map occlusion_map_uuid_; void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; @@ -45,6 +49,12 @@ class IntersectionModuleManager : public SceneModuleManagerInterfaceWithRTC const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; bool hasSameParentLaneletAndTurnDirectionWithRegistered(const lanelet::ConstLanelet & lane) const; + + /* called from SceneModuleInterfaceWithRTC::plan */ + void sendRTC(const Time & stamp) override; + void setActivation() override; + /* called from SceneModuleInterface::updateSceneModuleInstances */ + void deleteExpiredModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; }; class MergeFromPrivateModuleManager : public SceneModuleManagerInterface diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp index 6d96bd204a9ce..e4d1c016fa49f 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp @@ -15,6 +15,7 @@ #ifndef SCENE_MODULE__INTERSECTION__SCENE_INTERSECTION_HPP_ #define SCENE_MODULE__INTERSECTION__SCENE_INTERSECTION_HPP_ +#include #include #include #include @@ -27,6 +28,7 @@ #include #include #include +#include #include #include @@ -48,9 +50,9 @@ class IntersectionModule : public SceneModuleInterface public: struct DebugData { - bool stop_required = false; - - geometry_msgs::msg::Pose stop_wall_pose; + geometry_msgs::msg::Pose collision_stop_wall_pose; + geometry_msgs::msg::Pose occlusion_stop_wall_pose; + geometry_msgs::msg::Pose occlusion_first_stop_wall_pose; geometry_msgs::msg::Polygon stuck_vehicle_detect_area; geometry_msgs::msg::Polygon candidate_collision_ego_lane_polygon; std::vector candidate_collision_object_polygons; @@ -62,6 +64,8 @@ class IntersectionModule : public SceneModuleInterface autoware_auto_perception_msgs::msg::PredictedObjects conflicting_targets; autoware_auto_perception_msgs::msg::PredictedObjects stuck_targets; geometry_msgs::msg::Pose predicted_obj_pose; + geometry_msgs::msg::Point nearest_occlusion_point; + geometry_msgs::msg::Point nearest_occlusion_projection_point; }; public: @@ -102,12 +106,33 @@ class IntersectionModule : public SceneModuleInterface double collision_end_margin_time; //! end margin time to check collision double keep_detection_vel_thr; //! keep detection if ego is ego.vel < keep_detection_vel_thr } collision_detection; + struct Occlusion + { + bool enable; + double occlusion_detection_area_length; //! used for occlusion detection + double occlusion_creep_velocity; //! the creep velocity to occlusion limit stop lline + int free_space_max; + int occupied_min; + bool do_dp; + double before_creep_stop_time; + double min_vehicle_brake_for_rss; + double max_vehicle_velocity_for_rss; + } occlusion; + }; + + enum OcclusionState { + NONE, + BEFORE_FIRST_STOP_LINE, + WAIT_FIRST_STOP_LINE, + CREEP_SECOND_STOP_LINE, + CLEARED, }; IntersectionModule( const int64_t module_id, const int64_t lane_id, std::shared_ptr planner_data, const PlannerParam & planner_param, const std::set & assoc_ids, - const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock); + const bool enable_occlusion_detection, rclcpp::Node & node, const rclcpp::Logger logger, + const rclcpp::Clock::SharedPtr clock); /** * @brief plan go-stop velocity at traffic crossing with collision check between reference path @@ -120,11 +145,23 @@ class IntersectionModule : public SceneModuleInterface const std::set & getAssocIds() const { return assoc_ids_; } + UUID getOcclusionUUID() const { return occlusion_uuid_; } + bool getOcclusionSafety() const { return occlusion_safety_; } + double getOcclusionDistance() const { return occlusion_stop_distance_; } + UUID getOcclusionFirstStopUUID() const { return occlusion_first_stop_uuid_; } + bool getOcclusionFirstStopSafety() const { return occlusion_first_stop_safety_; } + double getOcclusionFirstStopDistance() const { return occlusion_first_stop_distance_; } + void setOcclusionActivation(const bool activation) { occlusion_activated_ = activation; } + void setOcclusionFirstStopActivation(const bool activation) + { + occlusion_first_stop_activated_ = activation; + } + private: + rclcpp::Node & node_; const int64_t lane_id_; std::string turn_direction_; - bool has_traffic_light_; - bool is_go_out_; + bool is_go_out_ = false; // Parameter PlannerParam planner_param_; std::optional intersection_lanelets_; @@ -132,6 +169,26 @@ class IntersectionModule : public SceneModuleInterface // have same turn_direction const std::set assoc_ids_; + // for occlusion detection + const bool enable_occlusion_detection_; + std::optional> detection_divisions_; + std::optional prev_occlusion_stop_line_pose_; + OcclusionState occlusion_state_; + // NOTE: uuid_ is base member + // for occlusion clearance decision + const UUID occlusion_uuid_; + bool occlusion_safety_; + double occlusion_stop_distance_; + bool occlusion_activated_; + // for first stop in two-phase stop + const UUID occlusion_first_stop_uuid_; // TODO(Mamoru Sobue): replace with uuid_ + bool occlusion_first_stop_safety_; + double occlusion_first_stop_distance_; + bool occlusion_first_stop_activated_; + + StateMachine collision_state_machine_; //! for stable collision checking + StateMachine before_creep_state_machine_; //! for two phase stop + /** * @brief check collision for all lanelet area & predicted objects (call checkPathCollision() as * actual collision check algorithm inside this function) @@ -258,13 +315,22 @@ class IntersectionModule : public SceneModuleInterface lanelet::ConstLanelets & ego_lane_with_next_lane, lanelet::ConstLanelet & closest_lanelet, const Polygon2d & stuck_vehicle_detect_area, const autoware_auto_perception_msgs::msg::PredictedObject & object) const; - StateMachine state_machine_; //! for state + + std::optional findNearestOcclusionProjectedPosition( + const nav_msgs::msg::OccupancyGrid & occ_grid, + const std::vector & detection_areas, + const lanelet::CompoundPolygon3d & first_detection_area, + const autoware_auto_planning_msgs::msg::PathWithLaneId & path_ip, const double interval, + const std::pair & lane_interval, + const std::vector & lane_divisions, + const double occlusion_dist_thr) const; // Debug mutable DebugData debug_data_; std::shared_ptr virtual_wall_marker_creator_ = std::make_shared(); + rclcpp::Publisher::SharedPtr occlusion_grid_pub_; }; } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp index b703147d1fab5..b80277309fe06 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp @@ -60,7 +60,7 @@ IntersectionLanelets getObjectiveLanelets( const int lane_id, const std::set & assoc_ids, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const std::pair lane_interval, const double detection_area_length, - const bool tl_arrow_solid_on = false); + const double occlusion_detection_area_length, const bool tl_arrow_solid_on = false); /** * @brief Generate a stop line and insert it into the path. If the stop line is defined in the map, @@ -72,7 +72,7 @@ IntersectionLanelets getObjectiveLanelets( " @param use_stuck_stopline if true, a stop line is generated at the beginning of intersection lane * @return nullopt if path is not intersecting with detection areas */ -std::optional generateStopLine( +std::optional generateCollisionStopLine( const int lane_id, const lanelet::CompoundPolygon3d & first_detection_area, const std::shared_ptr & planner_data, const double stop_line_margin, autoware_auto_planning_msgs::msg::PathWithLaneId * original_path, @@ -94,6 +94,25 @@ std::optional generateStuckStopLine( const autoware_auto_planning_msgs::msg::PathWithLaneId & path_ip, const double ip_interval, const std::pair lane_interval, const rclcpp::Logger logger); +std::optional> generateOcclusionStopLines( + const int lane_id, const std::vector & detection_areas, + const std::shared_ptr & planner_data, const double collision_stop_line_margin, + const size_t occlusion_projection_index, const double occlusion_extra_margin, + autoware_auto_planning_msgs::msg::PathWithLaneId * original_path, + const autoware_auto_planning_msgs::msg::PathWithLaneId & path_ip, const double interval, + const std::pair lane_interaval, const rclcpp::Logger logger); + +std::optional generateStaticPassJudgeLine( + const lanelet::CompoundPolygon3d & first_detection_area, + autoware_auto_planning_msgs::msg::PathWithLaneId * original_path, + const autoware_auto_planning_msgs::msg::PathWithLaneId & path_ip, const double ip_interval, + const std::pair lane_interval, + const std::shared_ptr & planner_data); + +std::optional getFirstPointInsidePolygon( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const std::pair lane_interval, const lanelet::CompoundPolygon3d & polygon); + /** * @brief Calculate first path index that is in the polygon. * @param path target path @@ -160,6 +179,10 @@ bool isTrafficLightArrowActivated( lanelet::ConstLanelet lane, const std::map & tl_infos); +std::vector generateDetectionLaneDivisions( + lanelet::ConstLanelets detection_lanelets, + const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution); + } // namespace util } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/util_type.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/util_type.hpp index dc6cbd12847e3..bf3a906cc40ca 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/util_type.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/util_type.hpp @@ -29,9 +29,11 @@ struct IntersectionLanelets lanelet::ConstLanelets attention; lanelet::ConstLanelets conflicting; lanelet::ConstLanelets adjacent; + lanelet::ConstLanelets occlusion_attention; // for occlusion detection std::vector attention_area; std::vector conflicting_area; std::vector adjacent_area; + std::vector occlusion_attention_area; // the first area intersecting with the path // even if lane change/re-routing happened on the intersection, these areas area are supposed to // be invariant under the 'associative' lanes. @@ -39,10 +41,17 @@ struct IntersectionLanelets std::optional first_detection_area; }; -struct StopLineIdx +enum class StopReason : int { + STUCK, + COLLISION, + OCCLUSION, +}; + +struct DetectionLaneDivision { - size_t pass_judge_line = 0; - size_t collision_stop_line = 0; + int lane_id; + // discrete fine lines from left to right + std::vector divisions; }; } // namespace behavior_velocity_planner::util diff --git a/planning/behavior_velocity_planner/include/scene_module/scene_module_interface.hpp b/planning/behavior_velocity_planner/include/scene_module/scene_module_interface.hpp index bd786a00b655d..844cbc5f41d44 100644 --- a/planning/behavior_velocity_planner/include/scene_module/scene_module_interface.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/scene_module_interface.hpp @@ -58,6 +58,7 @@ using tier4_autoware_utils::StopWatch; using tier4_debug_msgs::msg::Float64Stamped; using tier4_planning_msgs::msg::StopFactor; using tier4_planning_msgs::msg::StopReason; +using tier4_rtc_msgs::msg::Module; using unique_identifier_msgs::msg::UUID; class SceneModuleInterface @@ -66,6 +67,7 @@ class SceneModuleInterface explicit SceneModuleInterface( const int64_t module_id, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock) : module_id_(module_id), + activated_(false), safe_(false), distance_(std::numeric_limits::lowest()), logger_(logger), @@ -126,8 +128,7 @@ class SceneModuleInterface { const auto & p = planner_data_; return motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - points, p->current_odometry->pose, p->ego_nearest_dist_threshold, - p->ego_nearest_yaw_threshold); + points, p->current_odometry->pose, p->ego_nearest_dist_threshold); } }; @@ -135,7 +136,7 @@ class SceneModuleManagerInterface { public: SceneModuleManagerInterface(rclcpp::Node & node, [[maybe_unused]] const char * module_name) - : clock_(node.get_clock()), logger_(node.get_logger()) + : node_(node), clock_(node.get_clock()), logger_(node.get_logger()) { const auto ns = std::string("~/debug/") + module_name; pub_debug_ = node.create_publisher(ns, 1); @@ -307,6 +308,7 @@ class SceneModuleManagerInterface std::shared_ptr planner_data_; boost::optional first_stop_path_point_index_; + rclcpp::Node & node_; rclcpp::Clock::SharedPtr clock_; // Debug bool is_publish_debug_path_ = {false}; // note : this is very heavy debug topic option @@ -342,7 +344,7 @@ class SceneModuleManagerInterfaceWithRTC : public SceneModuleManagerInterface RTCInterface rtc_interface_; std::unordered_map map_uuid_; - void sendRTC(const Time & stamp) + virtual void sendRTC(const Time & stamp) { for (const auto & scene_module : scene_modules_) { const UUID uuid = getUUID(scene_module->getModuleId()); @@ -351,7 +353,7 @@ class SceneModuleManagerInterfaceWithRTC : public SceneModuleManagerInterface publishRTCStatus(stamp); } - void setActivation() + virtual void setActivation() { for (const auto & scene_module : scene_modules_) { const UUID uuid = getUUID(scene_module->getModuleId()); diff --git a/planning/behavior_velocity_planner/package.xml b/planning/behavior_velocity_planner/package.xml index f2de760a3e0f8..bc023491a1975 100644 --- a/planning/behavior_velocity_planner/package.xml +++ b/planning/behavior_velocity_planner/package.xml @@ -55,15 +55,18 @@ autoware_auto_perception_msgs autoware_auto_planning_msgs autoware_auto_tf2 + cv_bridge diagnostic_msgs eigen geometry_msgs + grid_map_cv grid_map_ros grid_map_utils interpolation lanelet2_extension libboost-dev libopencv-dev + magic_enum message_filters motion_utils motion_velocity_smoother diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp index abc50b4057f70..88e861be51db4 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp @@ -103,6 +103,25 @@ visualization_msgs::msg::MarkerArray createPoseMarkerArray( return msg; } +visualization_msgs::msg::Marker createPointMarkerArray( + const geometry_msgs::msg::Point & point, const std::string & ns, const int64_t id, const double r, + const double g, const double b) +{ + visualization_msgs::msg::Marker marker_point{}; + marker_point.header.frame_id = "map"; + marker_point.ns = ns + "_point"; + marker_point.id = id; + marker_point.lifetime = rclcpp::Duration::from_seconds(0.3); + marker_point.type = visualization_msgs::msg::Marker::SPHERE; + marker_point.action = visualization_msgs::msg::Marker::ADD; + marker_point.scale = createMarkerScale(2.0, 2.0, 2.0); + marker_point.color = createMarkerColor(r, g, b, 0.999); + + marker_point.pose.position = point; + + return marker_point; +} + } // namespace visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray() @@ -143,6 +162,14 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( module_id_, now, 0.3, 0.0, 0.0, 0.5, 0.0, 0.0), &debug_marker_array, now); + if (!occlusion_safety_) { + debug_marker_array.markers.push_back(createPointMarkerArray( + debug_data_.nearest_occlusion_point, "nearest_occlusion", module_id_, 0.5, 0.5, 0.0)); + debug_marker_array.markers.push_back(createPointMarkerArray( + debug_data_.nearest_occlusion_projection_point, "nearest_occluison_projection", module_id_, + 0.5, 0.5, 0.0)); + } + size_t i{0}; for (const auto & p : debug_data_.candidate_collision_object_polygons) { appendMarkerArray( @@ -176,10 +203,24 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createVirtualWallMarker const auto now = this->clock_->now(); - if (debug_data_.stop_required) { + // int32_t uid = planning_utils::bitShift(module_id_); + // TODO(Mamoru Sobue): collision stop pose depends on before/after occlusion clearance + if (!activated_) { + appendMarkerArray( + virtual_wall_marker_creator_->createStopVirtualWallMarker( + {debug_data_.collision_stop_wall_pose}, "intersection", now), + &wall_marker, now); + } + if (!occlusion_first_stop_activated_) { + appendMarkerArray( + virtual_wall_marker_creator_->createStopVirtualWallMarker( + {debug_data_.occlusion_first_stop_wall_pose}, "intersection", now), + &wall_marker, now); + } + if (!occlusion_activated_) { appendMarkerArray( virtual_wall_marker_creator_->createStopVirtualWallMarker( - {debug_data_.stop_wall_pose}, "intersection", now), + {debug_data_.occlusion_stop_wall_pose}, "intersection_occlusion", now), &wall_marker, now); } return wall_marker; @@ -191,11 +232,11 @@ visualization_msgs::msg::MarkerArray MergeFromPrivateRoadModule::createDebugMark const auto state = state_machine_.getState(); + int32_t uid = planning_utils::bitShift(module_id_); const auto now = this->clock_->now(); if (state == StateMachine::State::STOP) { appendMarkerArray( - createPoseMarkerArray( - debug_data_.stop_point_pose, "stop_point_pose", module_id_, 1.0, 0.0, 0.0), + createPoseMarkerArray(debug_data_.stop_point_pose, "stop_point_pose", uid, 1.0, 0.0, 0.0), &debug_marker_array, now); } diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp index 78a103a8878ec..1d7ab5c4f7c0c 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp @@ -27,7 +27,8 @@ namespace behavior_velocity_planner { IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) -: SceneModuleManagerInterfaceWithRTC(node, getModuleName()) +: SceneModuleManagerInterfaceWithRTC(node, getModuleName()), + occlusion_rtc_interface_(&node, "intersection_occlusion") { const std::string ns(getModuleName()); auto & ip = intersection_param_; @@ -76,21 +77,21 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) node.declare_parameter(ns + ".collision_detection.collision_end_margin_time"); ip.collision_detection.keep_detection_vel_thr = node.declare_parameter(ns + ".collision_detection.keep_detection_vel_thr"); -} -MergeFromPrivateModuleManager::MergeFromPrivateModuleManager(rclcpp::Node & node) -: SceneModuleManagerInterface(node, getModuleName()) -{ - const std::string ns(getModuleName()); - auto & mp = merge_from_private_area_param_; - mp.stop_duration_sec = node.declare_parameter(ns + ".stop_duration_sec"); - mp.detection_area_length = - node.get_parameter("intersection.common.detection_area_length").as_double(); - mp.detection_area_right_margin = - node.get_parameter("intersection.common.detection_area_right_margin").as_double(); - mp.detection_area_left_margin = - node.get_parameter("intersection.common.detection_area_left_margin").as_double(); - mp.stop_line_margin = node.get_parameter("intersection.common.stop_line_margin").as_double(); + ip.occlusion.enable = node.declare_parameter(ns + ".occlusion.enable"); + ip.occlusion.occlusion_detection_area_length = + node.declare_parameter(ns + ".occlusion.occlusion_detection_area_length"); + ip.occlusion.occlusion_creep_velocity = + node.declare_parameter(ns + ".occlusion.occlusion_creep_velocity"); + ip.occlusion.free_space_max = node.declare_parameter(ns + ".occlusion.free_space_max"); + ip.occlusion.occupied_min = node.declare_parameter(ns + ".occlusion.occupied_min"); + ip.occlusion.do_dp = node.declare_parameter(ns + ".occlusion.do_dp"); + ip.occlusion.before_creep_stop_time = + node.declare_parameter(ns + ".occlusion.before_creep_stop_time"); + ip.occlusion.min_vehicle_brake_for_rss = + node.declare_parameter(ns + ".occlusion.min_vehicle_brake_for_rss"); + ip.occlusion.max_vehicle_velocity_for_rss = + node.declare_parameter(ns + ".occlusion.max_vehicle_velocity_for_rss"); } void IntersectionModuleManager::launchNewModules( @@ -101,6 +102,8 @@ void IntersectionModuleManager::launchNewModules( const auto lanelets = planning_utils::getLaneletsOnPath(path, lanelet_map, planner_data_->current_odometry->pose); + // run occlusion detection only in the first intersection + bool enable_occlusion_detection = intersection_param_.occlusion.enable; for (size_t i = 0; i < lanelets.size(); i++) { const auto ll = lanelets.at(i); const auto lane_id = ll.id(); @@ -120,15 +123,148 @@ void IntersectionModuleManager::launchNewModules( const auto assoc_ids = planning_utils::getAssociativeIntersectionLanelets(ll, lanelet_map, routing_graph); - registerModule(std::make_shared( - module_id, lane_id, planner_data_, intersection_param_, assoc_ids, - logger_.get_child("intersection_module"), clock_)); + const auto new_module = std::make_shared( + module_id, lane_id, planner_data_, intersection_param_, assoc_ids, enable_occlusion_detection, + node_, logger_.get_child("intersection_module"), clock_); + const auto occlusion_uuid = new_module->getOcclusionUUID(); + const auto occlusion_first_stop_uuid = new_module->getOcclusionFirstStopUUID(); + registerModule(std::move(new_module)); + // default generateUUID(module_id); updateRTCStatus( getUUID(module_id), true, std::numeric_limits::lowest(), path.header.stamp); + // occlusion + occlusion_rtc_interface_.updateCooperateStatus( + occlusion_uuid, true, std::numeric_limits::lowest(), + std::numeric_limits::lowest(), path.header.stamp); + rtc_interface_.updateCooperateStatus( + occlusion_first_stop_uuid, true, std::numeric_limits::lowest(), + std::numeric_limits::lowest(), path.header.stamp); + + enable_occlusion_detection = false; } } +std::function &)> +IntersectionModuleManager::getModuleExpiredFunction( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path) +{ + const auto lane_set = planning_utils::getLaneletsOnPath( + path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); + + return [this, lane_set](const std::shared_ptr & scene_module) { + const auto intersection_module = std::dynamic_pointer_cast(scene_module); + const auto & assoc_ids = intersection_module->getAssocIds(); + for (const auto & lane : lane_set) { + const std::string turn_direction = lane.attributeOr("turn_direction", "else"); + const auto is_intersection = + turn_direction == "right" || turn_direction == "left" || turn_direction == "straight"; + if (!is_intersection) { + continue; + } + + if (assoc_ids.find(lane.id()) != assoc_ids.end() /* contains */) { + return false; + } + } + return true; + }; +} + +bool IntersectionModuleManager::hasSameParentLaneletAndTurnDirectionWithRegistered( + const lanelet::ConstLanelet & lane) const +{ + for (const auto & scene_module : scene_modules_) { + const auto intersection_module = std::dynamic_pointer_cast(scene_module); + const auto & assoc_ids = intersection_module->getAssocIds(); + if (assoc_ids.find(lane.id()) != assoc_ids.end()) { + return true; + } + } + return false; +} + +void IntersectionModuleManager::sendRTC(const Time & stamp) +{ + for (const auto & scene_module : scene_modules_) { + // default + const UUID uuid = getUUID(scene_module->getModuleId()); + updateRTCStatus(uuid, scene_module->isSafe(), scene_module->getDistance(), stamp); + // occlusion + const auto intersection_module = std::dynamic_pointer_cast(scene_module); + const auto occlusion_uuid = intersection_module->getOcclusionUUID(); + const auto occlusion_safety = intersection_module->getOcclusionSafety(); + const auto occlusion_distance = intersection_module->getOcclusionDistance(); + occlusion_rtc_interface_.updateCooperateStatus( + occlusion_uuid, occlusion_safety, occlusion_distance, occlusion_distance, stamp); + + const auto occlusion_first_stop_uuid = intersection_module->getOcclusionFirstStopUUID(); + const auto occlusion_first_stop_safety = intersection_module->getOcclusionFirstStopSafety(); + const auto occlusion_first_stop_distance = intersection_module->getOcclusionFirstStopDistance(); + rtc_interface_.updateCooperateStatus( + occlusion_first_stop_uuid, occlusion_first_stop_safety, occlusion_first_stop_distance, + occlusion_first_stop_distance, stamp); + } + rtc_interface_.publishCooperateStatus(stamp); // publishRTCStatus() + occlusion_rtc_interface_.publishCooperateStatus(stamp); +} + +void IntersectionModuleManager::setActivation() +{ + for (const auto & scene_module : scene_modules_) { + // default + scene_module->setActivation(rtc_interface_.isActivated(getUUID(scene_module->getModuleId()))); + // occlusion + const auto intersection_module = std::dynamic_pointer_cast(scene_module); + const auto occlusion_uuid = intersection_module->getOcclusionUUID(); + intersection_module->setOcclusionActivation( + occlusion_rtc_interface_.isActivated(occlusion_uuid)); + const auto occlusion_first_stop_uuid = intersection_module->getOcclusionFirstStopUUID(); + intersection_module->setOcclusionFirstStopActivation( + rtc_interface_.isActivated(occlusion_first_stop_uuid)); + } +} + +void IntersectionModuleManager::deleteExpiredModules( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path) +{ + const auto isModuleExpired = getModuleExpiredFunction(path); + + // Copy container to avoid iterator corruption + // due to scene_modules_.erase() in unregisterModule() + const auto copied_scene_modules = scene_modules_; + + for (const auto & scene_module : copied_scene_modules) { + if (isModuleExpired(scene_module)) { + // default + removeRTCStatus(getUUID(scene_module->getModuleId())); + removeUUID(scene_module->getModuleId()); + // occlusion + const auto intersection_module = std::dynamic_pointer_cast(scene_module); + const auto occlusion_uuid = intersection_module->getOcclusionUUID(); + const auto occlusion_first_uuid = intersection_module->getOcclusionFirstStopUUID(); + occlusion_rtc_interface_.removeCooperateStatus(occlusion_uuid); + rtc_interface_.removeCooperateStatus(occlusion_first_uuid); + unregisterModule(scene_module); + } + } +} + +MergeFromPrivateModuleManager::MergeFromPrivateModuleManager(rclcpp::Node & node) +: SceneModuleManagerInterface(node, getModuleName()) +{ + const std::string ns(getModuleName()); + auto & mp = merge_from_private_area_param_; + mp.stop_duration_sec = node.declare_parameter(ns + ".stop_duration_sec"); + mp.detection_area_length = + node.get_parameter("intersection.common.detection_area_length").as_double(); + mp.detection_area_right_margin = + node.get_parameter("intersection.common.detection_area_right_margin").as_double(); + mp.detection_area_left_margin = + node.get_parameter("intersection.common.detection_area_left_margin").as_double(); + mp.stop_line_margin = node.get_parameter("intersection.common.stop_line_margin").as_double(); +} + void MergeFromPrivateModuleManager::launchNewModules( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) { @@ -195,32 +331,6 @@ void MergeFromPrivateModuleManager::launchNewModules( } } -std::function &)> -IntersectionModuleManager::getModuleExpiredFunction( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) -{ - const auto lane_set = planning_utils::getLaneletsOnPath( - path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); - - return [this, lane_set](const std::shared_ptr & scene_module) { - const auto intersection_module = std::dynamic_pointer_cast(scene_module); - const auto & assoc_ids = intersection_module->getAssocIds(); - for (const auto & lane : lane_set) { - const std::string turn_direction = lane.attributeOr("turn_direction", "else"); - const auto is_intersection = - turn_direction == "right" || turn_direction == "left" || turn_direction == "straight"; - if (!is_intersection) { - continue; - } - - if (assoc_ids.find(lane.id()) != assoc_ids.end() /* contains */) { - return false; - } - } - return true; - }; -} - std::function &)> MergeFromPrivateModuleManager::getModuleExpiredFunction( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) @@ -248,19 +358,6 @@ MergeFromPrivateModuleManager::getModuleExpiredFunction( }; } -bool IntersectionModuleManager::hasSameParentLaneletAndTurnDirectionWithRegistered( - const lanelet::ConstLanelet & lane) const -{ - for (const auto & scene_module : scene_modules_) { - const auto intersection_module = std::dynamic_pointer_cast(scene_module); - const auto & assoc_ids = intersection_module->getAssocIds(); - if (assoc_ids.find(lane.id()) != assoc_ids.end()) { - return true; - } - } - return false; -} - bool MergeFromPrivateModuleManager::hasSameParentLaneletAndTurnDirectionWithRegistered( const lanelet::ConstLanelet & lane) const { diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp index 9009f507883fb..4304f4059fac6 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp @@ -12,10 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include +#include #include #include #include #include + +#include +// #include +// #include +#include +#include #include #include #include @@ -53,12 +61,20 @@ static geometry_msgs::msg::Pose getObjectPoseWithVelocityDirection( IntersectionModule::IntersectionModule( const int64_t module_id, const int64_t lane_id, std::shared_ptr planner_data, - const PlannerParam & planner_param, const std::set & assoc_ids, const rclcpp::Logger logger, + const PlannerParam & planner_param, const std::set & assoc_ids, + const bool enable_occlusion_detection, rclcpp::Node & node, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock) : SceneModuleInterface(module_id, logger, clock), + node_(node), lane_id_(lane_id), is_go_out_(false), - assoc_ids_(assoc_ids) + assoc_ids_(assoc_ids), + enable_occlusion_detection_(enable_occlusion_detection), + detection_divisions_(std::nullopt), + prev_occlusion_stop_line_pose_(std::nullopt), + occlusion_state_(OcclusionState::NONE), + occlusion_uuid_(tier4_autoware_utils::generateUUID()), + occlusion_first_stop_uuid_(tier4_autoware_utils::generateUUID()) { velocity_factor_.init(VelocityFactor::INTERSECTION); planner_param_ = planner_param; @@ -66,9 +82,15 @@ IntersectionModule::IntersectionModule( const auto & assigned_lanelet = planner_data->route_handler_->getLaneletMapPtr()->laneletLayer.get(lane_id); turn_direction_ = assigned_lanelet.attributeOr("turn_direction", "else"); - has_traffic_light_ = - !(assigned_lanelet.regulatoryElementsAs().empty()); - state_machine_.setMarginTime(planner_param_.collision_detection.state_transit_margin_time); + collision_state_machine_.setMarginTime( + planner_param_.collision_detection.state_transit_margin_time); + before_creep_state_machine_.setMarginTime(planner_param_.occlusion.before_creep_stop_time); + // TODO(Mamoru Sobue): maybe optional is better + before_creep_state_machine_.setState(StateMachine::State::STOP); + if (enable_occlusion_detection) { + occlusion_grid_pub_ = node_.create_publisher( + "~/debug/intersection/occlusion_grid", rclcpp::QoS(1).transient_local()); + } } bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) @@ -76,11 +98,17 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * RCLCPP_DEBUG(logger_, "===== plan start ====="); debug_data_ = DebugData(); - const StateMachine::State current_state = state_machine_.getState(); *stop_reason = planning_utils::initializeStopReason(StopReason::INTERSECTION); - RCLCPP_DEBUG( - logger_, "lane_id = %ld, state = %s", lane_id_, StateMachine::toString(current_state).c_str()); + /* set default RTC */ + // safe_, distance_ + setSafe(true); + setDistance(std::numeric_limits::lowest()); + // occlusion + occlusion_safety_ = true; + occlusion_stop_distance_ = std::numeric_limits::lowest(); + occlusion_first_stop_safety_ = true; + occlusion_first_stop_distance_ = std::numeric_limits::lowest(); /* get current pose */ const geometry_msgs::msg::Pose current_pose = planner_data_->current_odometry->pose; @@ -98,16 +126,12 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * if (!splineInterpolate(*path, interval, path_ip, logger_)) { RCLCPP_DEBUG_SKIPFIRST_THROTTLE(logger_, *clock_, 1000 /* ms */, "splineInterpolate failed"); RCLCPP_DEBUG(logger_, "===== plan end ====="); - setSafe(true); - setDistance(std::numeric_limits::lowest()); return false; } const auto lane_interval_ip_opt = util::findLaneIdsInterval(path_ip, assoc_ids_); if (!lane_interval_ip_opt) { RCLCPP_WARN(logger_, "Path has no interval on intersection lane %ld", lane_id_); RCLCPP_DEBUG(logger_, "===== plan end ====="); - setSafe(true); - setDistance(std::numeric_limits::lowest()); return false; } const auto lane_interval_ip = lane_interval_ip_opt.value(); @@ -125,9 +149,12 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * } const auto & detection_lanelets = intersection_lanelets_.value().attention; const auto & adjacent_lanelets = intersection_lanelets_.value().adjacent; + const auto & occlusion_attention_lanelets = intersection_lanelets_.value().occlusion_attention; const auto & detection_area = intersection_lanelets_.value().attention_area; + const auto & occlusion_attention_area = intersection_lanelets_.value().occlusion_attention_area; const auto & first_conflicting_area = intersection_lanelets_.value().first_conflicting_area; const auto & first_detection_area = intersection_lanelets_.value().first_detection_area; + const auto & conflicting_lanelets = intersection_lanelets_.value().conflicting; debug_data_.detection_area = detection_area; debug_data_.adjacent_area = intersection_lanelets_.value().adjacent_area; @@ -138,21 +165,16 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * debug_data_.intersection_area = toGeomPoly(intersection_area_2d); } - const std::optional stuck_line_idx_opt = - first_conflicting_area - ? util::generateStuckStopLine( - first_conflicting_area.value(), planner_data_, planner_param_.common.stop_line_margin, - planner_param_.stuck_vehicle.use_stuck_stopline, path, path_ip, interval, - lane_interval_ip, logger_.get_child("util")) - : std::nullopt; + if (conflicting_lanelets.empty()) { + RCLCPP_DEBUG(logger_, "conflicting area is empty"); + return false; + } - /* set stop lines for base_link */ - const auto stop_lines_idx_opt = first_detection_area - ? util::generateStopLine( - lane_id_, first_detection_area.value(), planner_data_, - planner_param_.common.stop_line_margin, path, path_ip, - interval, lane_interval_ip, logger_.get_child("util")) - : std::nullopt; + if (!detection_divisions_.has_value()) { + detection_divisions_ = util::generateDetectionLaneDivisions( + occlusion_attention_lanelets, routing_graph_ptr, + planner_data_->occupancy_grid->info.resolution / std::sqrt(2.0)); + } /* calc closest index */ const auto closest_idx_opt = @@ -161,68 +183,171 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * RCLCPP_WARN_SKIPFIRST_THROTTLE( logger_, *clock_, 1000 /* ms */, "motion_utils::findNearestIndex fail"); RCLCPP_DEBUG(logger_, "===== plan end ====="); - setSafe(true); - setDistance(std::numeric_limits::lowest()); return false; } const size_t closest_idx = closest_idx_opt.get(); - if (stop_lines_idx_opt) { - const auto stop_line_idx = stop_lines_idx_opt.value().collision_stop_line; - const auto pass_judge_line_idx = stop_lines_idx_opt.value().pass_judge_line; + const auto static_pass_judge_line_opt = + first_detection_area + ? util::generateStaticPassJudgeLine( + first_detection_area.value(), path, path_ip, interval, lane_interval_ip, planner_data_) + : std::nullopt; + if (static_pass_judge_line_opt) { + const auto pass_judge_line_idx = static_pass_judge_line_opt.value(); const bool is_over_pass_judge_line = util::isOverTargetIndex(*path, closest_idx, current_pose, pass_judge_line_idx); - - /* if ego is over the pass judge line before collision is detected, keep going */ - const double current_velocity = planner_data_->current_velocity->twist.linear.x; - if ( - is_over_pass_judge_line && is_go_out_ && - current_velocity > planner_param_.collision_detection.keep_detection_vel_thr) { + const double vel = std::fabs(planner_data_->current_velocity->twist.linear.x); + const bool keep_detection = (vel < planner_param_.collision_detection.keep_detection_vel_thr); + // if ego is over the pass judge line and not stopped + if (is_over_pass_judge_line && is_go_out_ && !keep_detection) { RCLCPP_DEBUG(logger_, "over the pass judge line. no plan needed."); RCLCPP_DEBUG(logger_, "===== plan end ====="); - setSafe(true); - setDistance(motion_utils::calcSignedArcLength( - path->points, planner_data_->current_odometry->pose.position, - path->points.at(stop_line_idx).point.pose.position)); return true; } } - /* collision checking */ - bool is_entry_prohibited = false; - - /* get dynamic object */ - const auto objects_ptr = planner_data_->predicted_objects; - /* considering lane change in the intersection, these lanelets are generated from the path */ const auto ego_lane_with_next_lane = getEgoLaneWithNextLane(*path, planner_data_->vehicle_info_.vehicle_width_m); const auto ego_lane = ego_lane_with_next_lane.front(); debug_data_.ego_lane = ego_lane.polygon3d(); + /* get dynamic object */ + // TODO(Mamoru Sobue): filter objects on detection area here + const auto objects_ptr = planner_data_->predicted_objects; + /* check stuck vehicle */ const auto stuck_vehicle_detect_area = generateStuckVehicleDetectAreaPolygon(*path, ego_lane_with_next_lane, closest_idx); const bool is_stuck = checkStuckVehicleInIntersection(objects_ptr, stuck_vehicle_detect_area); debug_data_.stuck_vehicle_detect_area = toGeomPoly(stuck_vehicle_detect_area); + const std::optional stuck_line_idx_opt = + first_conflicting_area + ? util::generateStuckStopLine( + first_conflicting_area.value(), planner_data_, planner_param_.common.stop_line_margin, + planner_param_.stuck_vehicle.use_stuck_stopline, path, path_ip, interval, + lane_interval_ip, logger_.get_child("util")) + : std::nullopt; /* calculate dynamic collision around detection area */ - const double baselink2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; + /* set stop lines for base_link */ + const auto default_stop_line_idx_opt = + first_detection_area ? util::generateCollisionStopLine( + lane_id_, first_detection_area.value(), planner_data_, + planner_param_.common.stop_line_margin, path, path_ip, interval, + lane_interval_ip, logger_.get_child("util")) + : std::nullopt; const double time_delay = is_go_out_ ? 0.0 : (planner_param_.collision_detection.state_transit_margin_time - - state_machine_.getDuration()); + collision_state_machine_.getDuration()); const bool has_collision = checkCollision( lanelet_map_ptr, *path, detection_lanelets, adjacent_lanelets, intersection_area, ego_lane, ego_lane_with_next_lane, objects_ptr, closest_idx, time_delay); + /* check occlusion on detection lane */ + const auto first_inside_detection_idx_ip_opt = + first_detection_area ? util::getFirstPointInsidePolygon( + path_ip, lane_interval_ip_opt.value(), first_detection_area.value()) + : std::nullopt; + const std::pair lane_detection_interval_ip = + first_inside_detection_idx_ip_opt + ? std::make_pair( + first_inside_detection_idx_ip_opt.value(), std::get<1>(lane_interval_ip_opt.value())) + : lane_interval_ip_opt.value(); + const double occlusion_dist_thr = std::fabs( + std::pow(planner_param_.occlusion.max_vehicle_velocity_for_rss, 2) / + (2 * planner_param_.occlusion.min_vehicle_brake_for_rss)); + const auto occlusion_stop_line_idx_ip_opt = + (enable_occlusion_detection_ && first_detection_area && !occlusion_attention_lanelets.empty()) + ? findNearestOcclusionProjectedPosition( + *planner_data_->occupancy_grid, occlusion_attention_area, first_detection_area.value(), + path_ip, interval, lane_detection_interval_ip, detection_divisions_.value(), + occlusion_dist_thr) + : std::nullopt; + const std::optional occlusion_stop_line_idx_opt = + occlusion_stop_line_idx_ip_opt + ? util::insertPoint( + path_ip.points.at(occlusion_stop_line_idx_ip_opt.value()).point.pose, path) + : std::nullopt; + + /* a flag if front stop line is not occlusion */ + bool stuck_stop_required = false; + bool collision_stop_required = false; + bool first_phase_stop_required = false; + bool occlusion_stop_required = false; + /* calculate final stop lines */ - std::optional stop_line_idx = - stop_lines_idx_opt ? std::make_optional(stop_lines_idx_opt.value().collision_stop_line) - : std::nullopt; - if (is_stuck && stuck_line_idx_opt) { - is_entry_prohibited = true; + std::optional stop_line_idx = default_stop_line_idx_opt; + std::optional occlusion_stop_line_idx = + default_stop_line_idx_opt; // TODO(Mamoru Sobue): maybe different position depending on the + // flag + std::optional occlusion_first_stop_line_idx = default_stop_line_idx_opt; + std::optional> insert_creep_during_occlusion = std::nullopt; + if (occlusion_stop_line_idx_opt) { + if (!default_stop_line_idx_opt) { + occlusion_stop_required = true; + stop_line_idx = occlusion_stop_line_idx = occlusion_stop_line_idx_opt; + prev_occlusion_stop_line_pose_ = + path_ip.points.at(occlusion_stop_line_idx_opt.value()).point.pose; + occlusion_state_ = OcclusionState::CREEP_SECOND_STOP_LINE; + RCLCPP_WARN(logger_, "directly stop at occlusion stop line because collision line not found"); + } else if (before_creep_state_machine_.getState() == StateMachine::State::GO) { + occlusion_stop_required = true; + stop_line_idx = occlusion_stop_line_idx = occlusion_stop_line_idx_opt; + // clear first stop line + // insert creep velocity [closest_idx, occlusion_stop_line) + insert_creep_during_occlusion = + std::make_pair(closest_idx, occlusion_stop_line_idx_opt.value()); + prev_occlusion_stop_line_pose_ = + path_ip.points.at(occlusion_stop_line_idx_ip_opt.value()).point.pose; + occlusion_state_ = OcclusionState::CREEP_SECOND_STOP_LINE; + } else { + const double dist_default_stop_line = motion_utils::calcSignedArcLength( + path_ip.points, current_pose.position, + path->points.at(default_stop_line_idx_opt.value()).point.pose.position); + if (dist_default_stop_line < planner_param_.common.stop_overshoot_margin) { + // start waiting at the first stop line + before_creep_state_machine_.setStateWithMarginTime( + StateMachine::State::GO, logger_.get_child("occlusion state_machine"), *clock_); + occlusion_state_ = OcclusionState::WAIT_FIRST_STOP_LINE; + } + first_phase_stop_required = true; + occlusion_stop_required = true; + occlusion_stop_line_idx = occlusion_stop_line_idx_opt; + stop_line_idx = occlusion_first_stop_line_idx; + // insert creep velocity [default_stop_line, occlusion_stop_line) + insert_creep_during_occlusion = + std::make_pair(default_stop_line_idx_opt.value(), occlusion_stop_line_idx_opt.value()); + prev_occlusion_stop_line_pose_ = + path_ip.points.at(occlusion_stop_line_idx_ip_opt.value()).point.pose; + occlusion_state_ = OcclusionState::BEFORE_FIRST_STOP_LINE; + } + } else if (prev_occlusion_stop_line_pose_) { + // previously occlusion existed, but now it is clear + const auto prev_occlusion_stop_pose_idx = motion_utils::findNearestIndex( + path->points, prev_occlusion_stop_line_pose_.value(), 3.0, M_PI_4); + if (!util::isOverTargetIndex( + *path, closest_idx, current_pose, default_stop_line_idx_opt.value())) { + stop_line_idx = default_stop_line_idx_opt.value(); + prev_occlusion_stop_line_pose_ = std::nullopt; + } else if (!util::isOverTargetIndex( + *path, closest_idx, current_pose, prev_occlusion_stop_pose_idx.get())) { + stop_line_idx = prev_occlusion_stop_pose_idx.get(); + } else { + // TODO(Mamoru Sobue): consider static occlusion limit stop line + prev_occlusion_stop_line_pose_ = std::nullopt; + } + occlusion_state_ = OcclusionState::CLEARED; + if (stop_line_idx && has_collision) { + // do collision checking at previous occlusion stop line + collision_stop_required = true; + } else { + collision_stop_required = false; + } + } else if (is_stuck && stuck_line_idx_opt) { + stuck_stop_required = true; const size_t stuck_line_idx = stuck_line_idx_opt.value(); const double dist_stuck_stopline = motion_utils::calcSignedArcLength( path->points, path->points.at(stuck_line_idx).point.pose.position, @@ -232,40 +357,87 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * (dist_stuck_stopline > planner_param_.common.stop_overshoot_margin); if (!is_over_stuck_stopline) { stop_line_idx = stuck_line_idx; - } else if (is_over_stuck_stopline && stop_lines_idx_opt) { - stop_line_idx = stop_lines_idx_opt.value().collision_stop_line; + } else if (is_over_stuck_stopline && default_stop_line_idx_opt) { + stop_line_idx = default_stop_line_idx_opt.value(); } } else if (has_collision) { - is_entry_prohibited = true; - stop_line_idx = stop_lines_idx_opt - ? std::make_optional(stop_lines_idx_opt.value().collision_stop_line) - : std::nullopt; + collision_stop_required = true; + stop_line_idx = default_stop_line_idx_opt; } - state_machine_.setStateWithMarginTime( - is_entry_prohibited ? StateMachine::State::STOP : StateMachine::State::GO, - logger_.get_child("state_machine"), *clock_); - setSafe(state_machine_.getState() == StateMachine::State::GO); - if (!stop_line_idx) { - RCLCPP_DEBUG(logger_, "detection_area is empty, no plan needed"); + RCLCPP_DEBUG(logger_, "detection_area is empty"); RCLCPP_DEBUG(logger_, "===== plan end ====="); - setSafe(true); - setDistance(std::numeric_limits::lowest()); return false; } - setDistance(motion_utils::calcSignedArcLength( - path->points, planner_data_->current_odometry->pose.position, - path->points.at(stop_line_idx.value()).point.pose.position)); + const std::string occlusion_state = std::string(magic_enum::enum_name(occlusion_state_)); + RCLCPP_DEBUG(logger_, "occlusion state: OcclusionState::%s", occlusion_state.c_str()); + + /* for collision and stuck state */ + collision_state_machine_.setStateWithMarginTime( + (collision_stop_required || stuck_stop_required) ? StateMachine::State::STOP + : StateMachine::State::GO, + logger_.get_child("collision state_machine"), *clock_); + + /* set RTC request respectively */ + if (occlusion_stop_required) { + if (first_phase_stop_required) { + occlusion_first_stop_safety_ = false; + occlusion_first_stop_distance_ = motion_utils::calcSignedArcLength( + path->points, planner_data_->current_odometry->pose.position, + path->points.at(stop_line_idx.value()).point.pose.position); + } + occlusion_safety_ = false; + occlusion_stop_distance_ = motion_utils::calcSignedArcLength( + path->points, planner_data_->current_odometry->pose.position, + path->points.at(occlusion_stop_line_idx.value()).point.pose.position); + } else { + /* collision */ + setSafe(collision_state_machine_.getState() == StateMachine::State::GO); + setDistance(motion_utils::calcSignedArcLength( + path->points, planner_data_->current_odometry->pose.position, + path->points.at(stop_line_idx.value()).point.pose.position)); + } - if (!isActivated()) { - // if RTC says intersection entry is 'dangerous', insert stop_line(v == 0.0) in this block + /* make decision */ + const double baselink2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; + if (!occlusion_activated_) { + is_go_out_ = false; + /* in case of creeping */ + if (insert_creep_during_occlusion) { + const auto [start, end] = insert_creep_during_occlusion.value(); + for (size_t i = start; i < end; ++i) { + planning_utils::setVelocityFromIndex( + i, planner_param_.occlusion.occlusion_creep_velocity /* [m/s] */, path); + } + } + + if (!occlusion_first_stop_activated_) { + planning_utils::setVelocityFromIndex( + occlusion_first_stop_line_idx.value(), 0.0 /* [m/s] */, path); + debug_data_.occlusion_first_stop_wall_pose = + planning_utils::getAheadPose(occlusion_first_stop_line_idx.value(), baselink2front, *path); + } + + const auto reconciled_occlusion_stop_line_idx = + occlusion_stop_required + ? occlusion_stop_line_idx.value() + : stop_line_idx.value(); // because intersection module may miss real occlusion + planning_utils::setVelocityFromIndex(reconciled_occlusion_stop_line_idx, 0.0 /* [m/s] */, path); + debug_data_.occlusion_stop_wall_pose = + planning_utils::getAheadPose(reconciled_occlusion_stop_line_idx, baselink2front, *path); + + RCLCPP_DEBUG(logger_, "not activated. stop at the line."); + RCLCPP_DEBUG(logger_, "===== plan end ====="); + return true; + } + + if (!isActivated() /* collision*/) { is_go_out_ = false; planning_utils::setVelocityFromIndex(stop_line_idx.value(), 0.0 /* [m/s] */, path); - debug_data_.stop_required = true; // dangerous or disabled by RTC - debug_data_.stop_wall_pose = + debug_data_.collision_stop_wall_pose = planning_utils::getAheadPose(stop_line_idx.value(), baselink2front, *path); // Get stop point and stop factor @@ -780,4 +952,264 @@ bool IntersectionModule::checkFrontVehicleDeceleration( return false; } +std::optional IntersectionModule::findNearestOcclusionProjectedPosition( + const nav_msgs::msg::OccupancyGrid & occ_grid, + const std::vector & detection_areas, + const lanelet::CompoundPolygon3d & first_detection_area, + const autoware_auto_planning_msgs::msg::PathWithLaneId & path_ip, const double interval, + const std::pair & lane_interval, + const std::vector & lane_divisions, + const double occlusion_dist_thr) const +{ + const auto first_detection_area_idx = + util::getFirstPointInsidePolygon(path_ip, lane_interval, first_detection_area); + if (!first_detection_area_idx) { + return std::nullopt; + } + + const double baselink2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; + const double vehicle_width = planner_data_->vehicle_info_.vehicle_width_m; + const int width = occ_grid.info.width; + const int height = occ_grid.info.height; + const double reso = occ_grid.info.resolution; + const auto & origin = occ_grid.info.origin.position; + + // NOTE: interesting area is set to 0 for later masking + cv::Mat detection_mask(width, height, CV_8UC1, cv::Scalar(0)); + cv::Mat unknown_mask(width, height, CV_8UC1, cv::Scalar(0)); + + // (1) prepare detection area mask + Polygon2d grid_poly; + grid_poly.outer().emplace_back(origin.x, origin.y); + grid_poly.outer().emplace_back(origin.x + (width - 1) * reso, origin.y); + grid_poly.outer().emplace_back(origin.x + (width - 1) * reso, origin.y + (height - 1) * reso); + grid_poly.outer().emplace_back(origin.x, origin.y + (height - 1) * reso); + grid_poly.outer().emplace_back(origin.x, origin.y); + bg::correct(grid_poly); + + std::vector> detection_area_cv_polygons; + for (const auto & detection_area : detection_areas) { + const auto area2d = lanelet::utils::to2D(detection_area); + Polygon2d area2d_poly; + for (const auto & p : area2d) { + area2d_poly.outer().emplace_back(p.x(), p.y()); + } + area2d_poly.outer().push_back(area2d_poly.outer().front()); + bg::correct(area2d_poly); + std::vector common_areas; + bg::intersection(area2d_poly, grid_poly, common_areas); + if (common_areas.empty()) { + continue; + } + for (size_t i = 0; i < common_areas.size(); ++i) { + common_areas[i].outer().push_back(common_areas[i].outer().front()); + bg::correct(common_areas[i]); + } + for (const auto & common_area : common_areas) { + std::vector detection_area_cv_polygon; + for (const auto & p : common_area.outer()) { + const int idx_x = static_cast((p.x() - origin.x) / reso); + const int idx_y = static_cast((p.y() - origin.y) / reso); + detection_area_cv_polygon.emplace_back(idx_x, height - 1 - idx_y); + } + detection_area_cv_polygons.push_back(detection_area_cv_polygon); + } + } + for (const auto & poly : detection_area_cv_polygons) { + cv::fillPoly(detection_mask, poly, cv::Scalar(255), cv::LINE_AA); + } + + // (2) prepare unknown mask + // In OpenCV the pixel at (X=x, Y=y) (with left-upper origin) is accesed by img[y, x] + for (int x = 0; x < width; x++) { + for (int y = 0; y < height; y++) { + const int idx = y * width + x; + const unsigned char intensity = occ_grid.data.at(idx); + if ( + planner_param_.occlusion.free_space_max <= intensity && + intensity < planner_param_.occlusion.occupied_min) { + unknown_mask.at(height - 1 - y, x) = 255; + } + } + } + + // (3) occlusion mask + cv::Mat occlusion_mask(width, height, CV_8UC1, cv::Scalar(0)); + cv::bitwise_and(detection_mask, unknown_mask, occlusion_mask); + + // (4) create distance grid + // value: 0 - 254: signed distance representing [distamce_min, distance_max] + // 255: undefined value + const double distance_max = std::hypot(width * reso / 2, height * reso / 2); + const double distance_min = -distance_max; + const int undef_pixel = 255; + const int max_cost_pixel = 254; + + auto dist2pixel = [=](const double dist) { + return std::min( + max_cost_pixel, + static_cast((dist - distance_min) / (distance_max - distance_min) * max_cost_pixel)); + }; + auto pixel2dist = [=](const int pixel) { + return pixel * 1.0 / max_cost_pixel * (distance_max - distance_min) + distance_min; + }; + + const int zero_dist_pixel = dist2pixel(0.0); + + auto coord2index = [&](const double x, const double y) { + const int idx_x = (x - origin.x) / reso; + const int idx_y = (y - origin.y) / reso; + if (idx_x < 0 || idx_x >= width) return std::make_tuple(false, -1, -1); + if (idx_y < 0 || idx_y >= height) return std::make_tuple(false, -1, -1); + return std::make_tuple(true, idx_x, idx_y); + }; + + cv::Mat distance_grid(width, height, CV_8UC1, cv::Scalar(undef_pixel)); + cv::Mat projection_ind_grid(width, height, CV_32S, cv::Scalar(-1)); + + const auto [lane_start, lane_end] = lane_interval; + for (int i = static_cast(lane_end); i >= static_cast(lane_start); i--) { + const auto & path_pos = path_ip.points.at(i).point.pose.position; + const int idx_x = (path_pos.x - origin.x) / reso; + const int idx_y = (path_pos.y - origin.y) / reso; + if (idx_x < 0 || idx_x >= width) continue; + if (idx_y < 0 || idx_y >= height) continue; + distance_grid.at(height - 1 - idx_y, idx_x) = zero_dist_pixel; + projection_ind_grid.at(height - 1 - idx_y, idx_x) = i; + } + + for (const auto & lane_division : lane_divisions) { + const auto & divisions = lane_division.divisions; + for (const auto & division : divisions) { + bool is_in_grid = false; + bool zero_dist_cell_found = false; + int projection_ind = -1; + std::optional> cost_prev_checkpoint = + std::nullopt; // cost, x, y, projection_ind + for (const auto & point : division) { + const auto [valid, idx_x, idx_y] = coord2index(point.x(), point.y()); + // exited grid just now + if (is_in_grid && !valid) break; + + // still not entering grid + if (!is_in_grid && !valid) continue; + + // From here, "valid" + const int pixel = distance_grid.at(height - 1 - idx_y, idx_x); + + // entered grid for 1st time + if (!is_in_grid) { + assert(pixel == undef_pixel || pixel == zero_dist_pixel); + is_in_grid = true; + if (pixel == undef_pixel) { + continue; + } + } + + if (pixel == zero_dist_pixel) { + zero_dist_cell_found = true; + projection_ind = projection_ind_grid.at(height - 1 - idx_y, idx_x); + assert(projection_ind >= 0); + cost_prev_checkpoint = std::make_optional>( + 0.0, point.x(), point.y(), projection_ind); + continue; + } + + if (zero_dist_cell_found) { + // finally traversed to defined cell (first half) + const auto [prev_cost, prev_checkpoint_x, prev_checkpoint_y, prev_projection_ind] = + cost_prev_checkpoint.value(); + const double dy = point.y() - prev_checkpoint_y, dx = point.x() - prev_checkpoint_x; + double new_dist = prev_cost + std::hypot(dy, dx); + const int new_projection_ind = projection_ind_grid.at(height - 1 - idx_y, idx_x); + const double cur_dist = pixel2dist(pixel); + if (planner_param_.occlusion.do_dp && cur_dist < new_dist) { + new_dist = cur_dist; + if (new_projection_ind > 0) { + projection_ind = std::min(prev_projection_ind, new_projection_ind); + } + } + projection_ind_grid.at(height - 1 - idx_y, idx_x) = projection_ind; + distance_grid.at(height - 1 - idx_y, idx_x) = dist2pixel(new_dist); + cost_prev_checkpoint = std::make_optional>( + new_dist, point.x(), point.y(), projection_ind); + } + } + } + } + + // clean-up and find nearest risk + const int min_cost_thr = dist2pixel(occlusion_dist_thr); + int min_cost = undef_pixel - 1; + int max_cost = 0; + std::optional min_cost_projection_ind = std::nullopt; + geometry_msgs::msg::Point nearest_occlusion_point; + for (int i = 0; i < width; ++i) { + for (int j = 0; j < height; ++j) { + const int pixel = static_cast(distance_grid.at(height - 1 - j, i)); + const bool occluded = (occlusion_mask.at(height - 1 - j, i) == 255); + if (pixel == undef_pixel || !occluded) { + // ignore outside of cropped + // some cell maybe undef still + distance_grid.at(height - 1 - j, i) = 0; + continue; + } + if (max_cost < pixel) { + max_cost = pixel; + } + const int projection_ind = projection_ind_grid.at(height - 1 - j, i); + if (pixel < min_cost) { + assert(projection_ind >= 0); + min_cost = pixel; + min_cost_projection_ind = projection_ind; + nearest_occlusion_point.x = origin.x + i * reso; + nearest_occlusion_point.y = origin.y + j * reso; + nearest_occlusion_point.z = origin.z + distance_max * pixel / max_cost_pixel; + } + } + } + debug_data_.nearest_occlusion_point = nearest_occlusion_point; + + cv::Mat distance_grid_heatmap; + cv::applyColorMap(distance_grid, distance_grid_heatmap, cv::COLORMAP_JET); + /* + cv::namedWindow("distance_grid_viz" + std::to_string(lane_id_), cv::WINDOW_NORMAL); + cv::imshow("distance_grid_viz" + std::to_string(lane_id_), distance_grid_heatmap); + cv::waitKey(1); + */ + grid_map::GridMap occlusion_grid({"elevation"}); + occlusion_grid.setFrameId("map"); + occlusion_grid.setGeometry( + grid_map::Length(width * reso, height * reso), reso, + grid_map::Position(origin.x + width * reso / 2, origin.y + height * reso / 2)); + cv::rotate(distance_grid, distance_grid, cv::ROTATE_90_COUNTERCLOCKWISE); + cv::rotate(distance_grid_heatmap, distance_grid_heatmap, cv::ROTATE_90_COUNTERCLOCKWISE); + grid_map::GridMapCvConverter::addLayerFromImage( + distance_grid, "elevation", occlusion_grid, origin.z /* elevation for 0 */, + origin.z + distance_max /* elevation for 255 */); + grid_map::GridMapCvConverter::addColorLayerFromImage( + distance_grid_heatmap, "color", occlusion_grid); + occlusion_grid_pub_->publish(grid_map::GridMapRosConverter::toMessage(occlusion_grid)); + if (min_cost > min_cost_thr || !min_cost_projection_ind.has_value()) { + return std::nullopt; + } + + const auto nearest_occlusion_projection_pose = + path_ip.points.at(min_cost_projection_ind.value()).point.pose; + debug_data_.nearest_occlusion_projection_point = nearest_occlusion_projection_pose.position; + const double tan_wall_ang = std::tan( + tf2::getYaw(path_ip.points.at(min_cost_projection_ind.value()).point.pose.orientation) - + M_PI / 2.0); + const double tan_projection_ang = std::atan2( + nearest_occlusion_projection_pose.position.y - nearest_occlusion_point.y, + nearest_occlusion_projection_pose.position.x - nearest_occlusion_point.x); + const double tan_diff_ang = + std::fabs((tan_wall_ang - tan_projection_ang) / (1 + tan_wall_ang * tan_projection_ang)); + const double footprint_offset = vehicle_width / 2.0 * tan_diff_ang; + const size_t baselink_max_entry_ind = static_cast(std::max( + 0, + first_detection_area_idx.value() - std::ceil((baselink2front + footprint_offset) / interval))); + return std::make_optional(baselink_max_entry_ind); +} + } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp index 8297e95c1939e..973b88ae08e59 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp @@ -93,18 +93,18 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity(PathWithLaneId * path, StopR const auto & first_conflicting_area = intersection_lanelets_.value().first_conflicting_area; /* set stop-line and stop-judgement-line for base_link */ - const auto stop_lines_idx_opt = + const auto stop_line_idx_opt = first_conflicting_area - ? util::generateStopLine( + ? util::generateCollisionStopLine( lane_id_, first_conflicting_area.value(), planner_data_, planner_param_.stop_line_margin, path, path_ip, interval, lane_interval_ip, logger_.get_child("util")) : std::nullopt; - if (!stop_lines_idx_opt.has_value()) { + if (!stop_line_idx_opt.has_value()) { RCLCPP_WARN_SKIPFIRST_THROTTLE(logger_, *clock_, 1000 /* ms */, "setStopLineIdx fail"); return false; } - const size_t stop_line_idx = stop_lines_idx_opt.value().collision_stop_line; + const size_t stop_line_idx = stop_line_idx_opt.value(); if (stop_line_idx == 0) { RCLCPP_DEBUG(logger_, "stop line is at path[0], ignore planning."); return true; diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp index d6440d931717d..a8c4cb574a330 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp @@ -25,9 +25,11 @@ #include #include +#include #include #include #include +#include #include namespace behavior_velocity_planner @@ -123,39 +125,66 @@ std::optional getDuplicatedPointIdx( return std::nullopt; } -std::optional> getFirstPointInsidePolygons( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, +std::optional generateStaticPassJudgeLine( + const lanelet::CompoundPolygon3d & first_detection_area, + autoware_auto_planning_msgs::msg::PathWithLaneId * original_path, + const autoware_auto_planning_msgs::msg::PathWithLaneId & path_ip, const double ip_interval, const std::pair lane_interval, - const std::vector & polygons) + const std::shared_ptr & planner_data) +{ + const auto pass_judge_line_idx_ip = + util::getFirstPointInsidePolygon(path_ip, lane_interval, first_detection_area); + if (!pass_judge_line_idx_ip) { + return std::nullopt; + } + const int base2front_idx_dist = + std::ceil(planner_data->vehicle_info_.vehicle_length_m / ip_interval); + const int idx = static_cast(pass_judge_line_idx_ip.value()) - base2front_idx_dist; + if (idx < 0) { + return std::nullopt; + } + const auto & insert_point = path_ip.points.at(static_cast(idx)).point.pose; + const auto duplicate_idx_opt = util::getDuplicatedPointIdx(*original_path, insert_point.position); + if (duplicate_idx_opt) { + return duplicate_idx_opt; + } else { + const auto insert_idx_opt = util::insertPoint(insert_point, original_path); + if (!insert_idx_opt) { + return std::nullopt; + } + return insert_idx_opt; + } +} + +std::optional getFirstPointInsidePolygon( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const std::pair lane_interval, const lanelet::CompoundPolygon3d & polygon) { + const auto polygon_2d = lanelet::utils::to2D(polygon); for (size_t i = lane_interval.first; i <= lane_interval.second; ++i) { - bool is_in_lanelet = false; auto p = path.points.at(i).point.pose.position; - for (const auto & polygon : polygons) { - const auto polygon_2d = lanelet::utils::to2D(polygon); - is_in_lanelet = bg::within(to_bg2d(p), polygon_2d); - if (is_in_lanelet) { - return std::make_optional>(i, polygon); - } - } + const auto is_in_lanelet = bg::within(to_bg2d(p), polygon_2d); if (is_in_lanelet) { - break; + return std::make_optional(i); } } return std::nullopt; } -static std::optional getFirstPointInsidePolygon( +std::optional> getFirstPointInsidePolygons( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const std::pair lane_interval, const lanelet::CompoundPolygon3d & polygon) + const std::pair lane_interval, + const std::vector & polygons) { for (size_t i = lane_interval.first; i <= lane_interval.second; ++i) { bool is_in_lanelet = false; auto p = path.points.at(i).point.pose.position; - const auto polygon_2d = lanelet::utils::to2D(polygon); - is_in_lanelet = bg::within(to_bg2d(p), polygon_2d); - if (is_in_lanelet) { - return std::make_optional(i); + for (const auto & polygon : polygons) { + const auto polygon_2d = lanelet::utils::to2D(polygon); + is_in_lanelet = bg::within(to_bg2d(p), polygon_2d); + if (is_in_lanelet) { + return std::make_optional>(i, polygon); + } } if (is_in_lanelet) { break; @@ -164,29 +193,16 @@ static std::optional getFirstPointInsidePolygon( return std::nullopt; } -std::optional generateStopLine( +std::optional generateCollisionStopLine( const int lane_id, const lanelet::CompoundPolygon3d & detection_area, const std::shared_ptr & planner_data, const double stop_line_margin, autoware_auto_planning_msgs::msg::PathWithLaneId * original_path, const autoware_auto_planning_msgs::msg::PathWithLaneId & path_ip, const double interval, const std::pair lane_interval_ip, const rclcpp::Logger logger) { - /* set judge line dist */ - const double current_vel = planner_data->current_velocity->twist.linear.x; - const double current_acc = planner_data->current_acceleration->accel.accel.linear.x; - const double max_acc = planner_data->max_stop_acceleration_threshold; - const double max_jerk = planner_data->max_stop_jerk_threshold; - const double delay_response_time = planner_data->delay_response_time; - const double pass_judge_line_dist = planning_utils::calcJudgeLineDistWithJerkLimit( - current_vel, current_acc, max_acc, max_jerk, delay_response_time); - const int stop_line_margin_idx_dist = std::ceil(stop_line_margin / interval); const int base2front_idx_dist = std::ceil(planner_data->vehicle_info_.max_longitudinal_offset_m / interval); - const int pass_judge_idx_dist = std::ceil(pass_judge_line_dist / interval); - - /* generate stop points */ - util::StopLineIdx idxs; // If a stop_line tag is defined on lanelet_map, use it. // else generate a stop_line behind the intersection of path and detection area @@ -218,59 +234,25 @@ std::optional generateStopLine( return std::nullopt; } - { - /* insert stop_point */ - const auto & insert_point = path_ip.points.at(stop_idx_ip).point.pose; - const auto duplicate_idx_opt = - util::getDuplicatedPointIdx(*original_path, insert_point.position); - if (duplicate_idx_opt) { - idxs.collision_stop_line = duplicate_idx_opt.value(); - } else { - const auto insert_idx_opt = util::insertPoint(insert_point, original_path); - if (!insert_idx_opt) { - RCLCPP_WARN(logger, "insertPoint failed for stop line"); - return std::nullopt; - } - idxs.collision_stop_line = insert_idx_opt.value(); - } - } - - const bool has_prior_stopline = std::any_of( - original_path->points.begin(), original_path->points.begin() + idxs.collision_stop_line, - [](const auto & p) { return std::fabs(p.point.longitudinal_velocity_mps) < 0.1; }); - - /* insert judge point */ - const size_t pass_judge_idx_ip = static_cast(std::min( - static_cast(path_ip.points.size()) - 1, - std::max(static_cast(stop_idx_ip) - pass_judge_idx_dist, 0))); - /* if another stop point exist before intersection stop_line, disable judge_line. */ - if (has_prior_stopline || pass_judge_idx_ip == stop_idx_ip) { - idxs.pass_judge_line = idxs.collision_stop_line; + /* insert stop_point */ + std::optional collision_stop_line = std::nullopt; + const auto & insert_point = path_ip.points.at(stop_idx_ip).point.pose; + const auto duplicate_idx_opt = util::getDuplicatedPointIdx(*original_path, insert_point.position); + if (duplicate_idx_opt) { + collision_stop_line = duplicate_idx_opt.value(); } else { - const auto & insert_point = path_ip.points.at(pass_judge_idx_ip).point.pose; - const auto duplicate_idx_opt = - util::getDuplicatedPointIdx(*original_path, insert_point.position); - if (duplicate_idx_opt) { - idxs.pass_judge_line = duplicate_idx_opt.value(); + const auto insert_idx_opt = util::insertPoint(insert_point, original_path); + if (!insert_idx_opt) { + RCLCPP_WARN(logger, "insertPoint failed for stop line"); + return std::nullopt; } else { - const auto insert_idx_opt = util::insertPoint(insert_point, original_path); - if (!insert_idx_opt) { - RCLCPP_WARN(logger, "insertPoint failed to pass judge line"); - return std::nullopt; - } - idxs.pass_judge_line = insert_idx_opt.value(); - idxs.collision_stop_line = - std::min(idxs.collision_stop_line + 1, original_path->points.size() - 1); + collision_stop_line = insert_idx_opt.value(); } } - RCLCPP_DEBUG( - logger, - "generateStopLine() : stop_idx = %ld, pass_judge_idx = %ld" - ", has_prior_stopline = %d", - idxs.collision_stop_line, idxs.pass_judge_line, has_prior_stopline); + RCLCPP_DEBUG(logger, "generateCollisionStopLine() : stop_idx = %ld", collision_stop_line.value()); - return std::make_optional(idxs); + return collision_stop_line; } std::optional generateStuckStopLine( @@ -389,7 +371,7 @@ IntersectionLanelets getObjectiveLanelets( const int lane_id, const std::set & assoc_ids, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const std::pair lane_interval, const double detection_area_length, - const bool tl_arrow_solid_on) + const double occlusion_detection_area_length, const bool tl_arrow_solid_on) { const auto & assigned_lanelet = lanelet_map_ptr->laneletLayer.get(lane_id); const auto turn_direction = assigned_lanelet.attributeOr("turn_direction", "else"); @@ -482,6 +464,27 @@ IntersectionLanelets getObjectiveLanelets( } } + lanelet::ConstLanelets occlusion_detection_and_preceding_lanelets; + { + const double length = occlusion_detection_area_length; + std::set detection_ids; + for (const auto & ll : detection_lanelets) { + // Preceding lanes does not include detection_lane so add them at the end + const auto & inserted = detection_ids.insert(ll.id()); + if (inserted.second) occlusion_detection_and_preceding_lanelets.push_back(ll); + // get preceding lanelets without ego_lanelets + // to prevent the detection area from including the ego lanes and its' preceding lanes. + const auto lanelet_sequences = lanelet::utils::query::getPrecedingLaneletSequences( + routing_graph_ptr, ll, length, ego_lanelets); + for (const auto & ls : lanelet_sequences) { + for (const auto & l : ls) { + const auto & inserted = detection_ids.insert(l.id()); + if (inserted.second) occlusion_detection_and_preceding_lanelets.push_back(l); + } + } + } + } + IntersectionLanelets result; if (!tl_arrow_solid_on) { result.attention = std::move(detection_and_preceding_lanelets); @@ -490,10 +493,12 @@ IntersectionLanelets getObjectiveLanelets( } result.conflicting = std::move(conflicting_ex_ego_lanelets); result.adjacent = planning_utils::getConstLaneletsFromIds(lanelet_map_ptr, assoc_ids); + result.occlusion_attention = std::move(occlusion_detection_and_preceding_lanelets); // compoundPolygon3d result.attention_area = getPolygon3dFromLanelets(result.attention); result.conflicting_area = getPolygon3dFromLanelets(result.conflicting); result.adjacent_area = getPolygon3dFromLanelets(result.adjacent); + result.occlusion_attention_area = getPolygon3dFromLanelets(result.occlusion_attention); // find the first conflicting/detection area polygon intersecting the path { @@ -698,5 +703,133 @@ bool isTrafficLightArrowActivated( return false; } +std::vector generateDetectionLaneDivisions( + lanelet::ConstLanelets detection_lanelets_all, + [[maybe_unused]] const lanelet::routing::RoutingGraphPtr routing_graph_ptr, + const double resolution) +{ + using lanelet::utils::getCenterlineWithOffset; + using lanelet::utils::to2D; + + // (0) remove left/right lanelet + lanelet::ConstLanelets detection_lanelets; + for (const auto & detection_lanelet : detection_lanelets_all) { + const auto turn_direction = getTurnDirection(detection_lanelet); + if (turn_direction.compare("left") == 0 || turn_direction.compare("right") == 0) { + continue; + } + detection_lanelets.push_back(detection_lanelet); + } + + // (1) tsort detection_lanelets + // generate adjacency matrix + // if lanelet1 => lanelet2; then adjacency[lanelet2][lanelet1] = true + const int n_node = detection_lanelets.size(); + std::vector> adjacency(n_node); + for (int dst = 0; dst < n_node; ++dst) { + adjacency[dst].resize(n_node); + for (int src = 0; src < n_node; ++src) { + adjacency[dst][src] = false; + } + } + std::set detection_lanelet_ids; + std::unordered_map id2ind, ind2id; + std::unordered_map id2lanelet; + int ind = 0; + for (const auto & detection_lanelet : detection_lanelets) { + detection_lanelet_ids.insert(detection_lanelet.id()); + const int id = detection_lanelet.id(); + id2ind[id] = ind; + ind2id[ind] = id; + id2lanelet[id] = detection_lanelet; + ind++; + } + for (const auto & detection_lanelet : detection_lanelets) { + const auto & followings = routing_graph_ptr->following(detection_lanelet); + const int dst = detection_lanelet.id(); + for (const auto & following : followings) { + if (const int src = following.id(); + detection_lanelet_ids.find(src) != detection_lanelet_ids.end()) { + adjacency[(id2ind[src])][(id2ind[dst])] = true; + } + } + } + // terminal node + std::map> branches; + auto has_no_previous = [&](const int node) { + for (int dst = 0; dst < n_node; dst++) { + if (adjacency[dst][node]) { + return false; + } + } + return true; + }; + for (int src = 0; src < n_node; src++) { + if (!has_no_previous(src)) { + continue; + } + branches[(ind2id[src])] = std::vector{}; + auto & branch = branches[(ind2id[src])]; + int node_iter = ind2id[src]; + while (true) { + const auto & dsts = adjacency[(id2ind[node_iter])]; + // NOTE: assuming detection lanelets have only one previous lanelet + const auto next = std::find(dsts.begin(), dsts.end(), true); + if (next == dsts.end()) { + branch.push_back(node_iter); + break; + } + branch.push_back(node_iter); + node_iter = ind2id[std::distance(dsts.begin(), next)]; + } + } + for (decltype(branches)::iterator it = branches.begin(); it != branches.end(); it++) { + auto & branch = it->second; + std::reverse(branch.begin(), branch.end()); + } + + // (2) merge each branch to one lanelet + // NOTE: somehow bg::area() for merged lanelet does not work, so calculate it here + std::unordered_map> merged_branches; + for (const auto & [src, branch] : branches) { + lanelet::Points3d lefts; + lanelet::Points3d rights; + double area = 0; + for (const auto & lane_id : branch) { + const auto lane = id2lanelet[lane_id]; + for (const auto & left_point : lane.leftBound()) { + lefts.push_back(lanelet::Point3d(left_point)); + } + for (const auto & right_point : lane.rightBound()) { + rights.push_back(lanelet::Point3d(right_point)); + } + area += bg::area(lane.polygon2d().basicPolygon()); + } + lanelet::LineString3d right = lanelet::LineString3d(lanelet::InvalId, lefts).invert(); + lanelet::LineString3d left = lanelet::LineString3d(lanelet::InvalId, rights).invert(); + lanelet::Lanelet merged = lanelet::Lanelet(lanelet::InvalId, left, right); + merged_branches[src] = std::make_pair(merged, area); + } + + // (3) descritize each merged lanelet + std::vector detection_divisions; + for (const auto & [last_lane_id, branch] : merged_branches) { + DetectionLaneDivision detection_division; + detection_division.lane_id = last_lane_id; + const auto detection_lanelet = branch.first; + const double area = branch.second; + const double length = bg::length(detection_lanelet.centerline()); + const double width = area / length; + auto & divisions = detection_division.divisions; + for (int i = 0; i < static_cast(width / resolution); ++i) { + const double offset = resolution * i - width / 2; + divisions.push_back(to2D(getCenterlineWithOffset(detection_lanelet, offset, resolution))); + } + divisions.push_back(to2D(getCenterlineWithOffset(detection_lanelet, width / 2, resolution))); + detection_divisions.push_back(detection_division); + } + return detection_divisions; +} + } // namespace util } // namespace behavior_velocity_planner diff --git a/planning/rtc_auto_mode_manager/config/rtc_auto_mode_manager.param.yaml b/planning/rtc_auto_mode_manager/config/rtc_auto_mode_manager.param.yaml index afda1b04f9add..6dd6755da27d8 100644 --- a/planning/rtc_auto_mode_manager/config/rtc_auto_mode_manager.param.yaml +++ b/planning/rtc_auto_mode_manager/config/rtc_auto_mode_manager.param.yaml @@ -15,6 +15,7 @@ - "avoidance_right" - "pull_over" - "pull_out" + - "intersection_occlusion" default_enable_list: - "blind_spot" @@ -31,3 +32,4 @@ - "avoidance_right" - "pull_over" - "pull_out" + - "intersection_occlusion" diff --git a/planning/rtc_auto_mode_manager/src/rtc_auto_mode_manager_interface.cpp b/planning/rtc_auto_mode_manager/src/rtc_auto_mode_manager_interface.cpp index c1769a5f500fa..b942a6013e2e2 100644 --- a/planning/rtc_auto_mode_manager/src/rtc_auto_mode_manager_interface.cpp +++ b/planning/rtc_auto_mode_manager/src/rtc_auto_mode_manager_interface.cpp @@ -55,6 +55,8 @@ Module getModuleType(const std::string & module_name) module.type = Module::PULL_OVER; } else if (module_name == "pull_out") { module.type = Module::PULL_OUT; + } else if (module_name == "intersection_occlusion") { + module.type = Module::INTERSECTION_OCCLUSION; } else { module.type = Module::NONE; } diff --git a/planning/rtc_interface/src/rtc_interface.cpp b/planning/rtc_interface/src/rtc_interface.cpp index af1f3da4331bd..fefa0b3812c20 100644 --- a/planning/rtc_interface/src/rtc_interface.cpp +++ b/planning/rtc_interface/src/rtc_interface.cpp @@ -68,6 +68,8 @@ Module getModuleType(const std::string & module_name) module.type = Module::PULL_OVER; } else if (module_name == "pull_out") { module.type = Module::PULL_OUT; + } else if (module_name == "intersection_occlusion") { + module.type = Module::INTERSECTION_OCCLUSION; } return module; } From aba954f30124155afe15ba4f216076101a125265 Mon Sep 17 00:00:00 2001 From: OHMAE Takugo Date: Mon, 24 Apr 2023 12:31:10 +0900 Subject: [PATCH 04/16] feat(lidar_centerpoint_tvm): single inference lidar centerpoint node (#3130) * add signle_inference_node to lidar_centerpoint_tvm * style(pre-commit): autofix * change python3 to python * changes model_name in lidar_centerpoint_tvm.launch.xml from centerpoint_tiny to centerpoint * remove unnecessary parameters in single_inference_lidar_centerpoint_tvm.launch.xml --------- Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Xinyu Wang <93699235+angry-crab@users.noreply.github.com> --- .../lidar_centerpoint_tvm/CMakeLists.txt | 22 ++ .../single_inference_node.hpp | 66 +++++ ...inference_lidar_centerpoint_tvm.launch.xml | 31 +++ .../preprocess/pointcloud_densification.cpp | 20 +- .../scripts/lidar_centerpoint_visualizer.py | 55 ++++ .../src/single_inference_node.cpp | 239 ++++++++++++++++++ 6 files changed, 425 insertions(+), 8 deletions(-) create mode 100644 perception/lidar_centerpoint_tvm/include/lidar_centerpoint_tvm/single_inference_node.hpp create mode 100644 perception/lidar_centerpoint_tvm/launch/single_inference_lidar_centerpoint_tvm.launch.xml create mode 100755 perception/lidar_centerpoint_tvm/scripts/lidar_centerpoint_visualizer.py create mode 100644 perception/lidar_centerpoint_tvm/src/single_inference_node.cpp diff --git a/perception/lidar_centerpoint_tvm/CMakeLists.txt b/perception/lidar_centerpoint_tvm/CMakeLists.txt index 744450d5733b1..e9da98d4ae3c4 100644 --- a/perception/lidar_centerpoint_tvm/CMakeLists.txt +++ b/perception/lidar_centerpoint_tvm/CMakeLists.txt @@ -68,6 +68,28 @@ if((NOT NN_DEPENDENCY_ENCODER STREQUAL "") AND (NOT NN_DEPENDENCY_BACKBONE STREQ EXECUTABLE lidar_centerpoint_tvm_node ) + ## single inference node ## + ament_auto_add_library(single_inference_lidar_centerpoint_tvm_component SHARED + src/single_inference_node.cpp + ) + + target_link_libraries(single_inference_lidar_centerpoint_tvm_component + ${tvm_runtime_LIBRARIES} + ) + + + rclcpp_components_register_node(single_inference_lidar_centerpoint_tvm_component + PLUGIN "autoware::perception::lidar_centerpoint_tvm::SingleInferenceLidarCenterPointNode" + EXECUTABLE single_inference_lidar_centerpoint_tvm_node + ) + + install(PROGRAMS + scripts/lidar_centerpoint_visualizer.py + DESTINATION lib/${PROJECT_NAME} + ) + + ament_export_dependencies(ament_cmake_python) + ament_auto_package(INSTALL_TO_SHARE launch config diff --git a/perception/lidar_centerpoint_tvm/include/lidar_centerpoint_tvm/single_inference_node.hpp b/perception/lidar_centerpoint_tvm/include/lidar_centerpoint_tvm/single_inference_node.hpp new file mode 100644 index 0000000000000..4c3a7a2c9ecf3 --- /dev/null +++ b/perception/lidar_centerpoint_tvm/include/lidar_centerpoint_tvm/single_inference_node.hpp @@ -0,0 +1,66 @@ +// Copyright 2022 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 LIDAR_CENTERPOINT_TVM__SINGLE_INFERENCE_NODE_HPP_ +#define LIDAR_CENTERPOINT_TVM__SINGLE_INFERENCE_NODE_HPP_ + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include + +namespace autoware +{ +namespace perception +{ +namespace lidar_centerpoint_tvm +{ +class SingleInferenceLidarCenterPointNode : public rclcpp::Node +{ +public: + explicit SingleInferenceLidarCenterPointNode(const rclcpp::NodeOptions & node_options); + +private: + void detect(const std::string & pcd_path, const std::string & detections_path); + std::vector getVertices( + const autoware_auto_perception_msgs::msg::Shape & shape, const Eigen::Affine3d & pose) const; + void dumpDetectionsAsMesh( + const autoware_auto_perception_msgs::msg::DetectedObjects & objects_msg, + const std::string & output_path) const; + + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_{tf_buffer_}; + + float score_threshold_{0.0}; + std::vector class_names_; + bool rename_car_to_truck_and_bus_{false}; + bool has_twist_{false}; + + std::unique_ptr detector_ptr_{nullptr}; +}; + +} // namespace lidar_centerpoint_tvm +} // namespace perception +} // namespace autoware + +#endif // LIDAR_CENTERPOINT_TVM__SINGLE_INFERENCE_NODE_HPP_ diff --git a/perception/lidar_centerpoint_tvm/launch/single_inference_lidar_centerpoint_tvm.launch.xml b/perception/lidar_centerpoint_tvm/launch/single_inference_lidar_centerpoint_tvm.launch.xml new file mode 100644 index 0000000000000..e62808804ca07 --- /dev/null +++ b/perception/lidar_centerpoint_tvm/launch/single_inference_lidar_centerpoint_tvm.launch.xml @@ -0,0 +1,31 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/perception/lidar_centerpoint_tvm/lib/preprocess/pointcloud_densification.cpp b/perception/lidar_centerpoint_tvm/lib/preprocess/pointcloud_densification.cpp index 299a7eeb98044..5562e963d177b 100644 --- a/perception/lidar_centerpoint_tvm/lib/preprocess/pointcloud_densification.cpp +++ b/perception/lidar_centerpoint_tvm/lib/preprocess/pointcloud_densification.cpp @@ -45,7 +45,7 @@ boost::optional getTransform( target_frame_id, source_frame_id, time, rclcpp::Duration::from_seconds(0.5)); return transform_stamped.transform; } catch (tf2::TransformException & ex) { - RCLCPP_WARN_STREAM(rclcpp::get_logger("lidar_centerpoint"), ex.what()); + RCLCPP_WARN_STREAM(rclcpp::get_logger("lidar_centerpoint_tvm"), ex.what()); return boost::none; } } @@ -77,14 +77,18 @@ bool PointCloudDensification::enqueuePointCloud( { const auto header = pointcloud_msg.header; - auto transform_world2current = - getTransform(tf_buffer, header.frame_id, param_.world_frame_id(), header.stamp); - if (!transform_world2current) { - return false; + if (param_.pointcloud_cache_size() > 1) { + auto transform_world2current = + getTransform(tf_buffer, header.frame_id, param_.world_frame_id(), header.stamp); + if (!transform_world2current) { + return false; + } + auto affine_world2current = transformToEigen(transform_world2current.get()); + + enqueue(pointcloud_msg, affine_world2current); + } else { + enqueue(pointcloud_msg, Eigen::Affine3f::Identity()); } - auto affine_world2current = transformToEigen(transform_world2current.get()); - - enqueue(pointcloud_msg, affine_world2current); dequeue(); return true; diff --git a/perception/lidar_centerpoint_tvm/scripts/lidar_centerpoint_visualizer.py b/perception/lidar_centerpoint_tvm/scripts/lidar_centerpoint_visualizer.py new file mode 100755 index 0000000000000..5a1135379615f --- /dev/null +++ b/perception/lidar_centerpoint_tvm/scripts/lidar_centerpoint_visualizer.py @@ -0,0 +1,55 @@ +#!/usr/bin/env python +# Copyright 2022 TIER IV, Inc. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os +import time + +import open3d as o3d +import rclpy +from rclpy.node import Node + + +def main(args=None): + rclpy.init(args=args) + + node = Node("lidar_centerpoint_visualizer") + node.declare_parameter("pcd_path", rclpy.Parameter.Type.STRING) + node.declare_parameter("detections_path", rclpy.Parameter.Type.STRING) + + pcd_path = node.get_parameter("pcd_path").get_parameter_value().string_value + detections_path = node.get_parameter("detections_path").get_parameter_value().string_value + + while not os.path.exists(pcd_path) and not os.path.exists(detections_path): + time.sleep(1.0) + + if not rclpy.ok(): + rclpy.shutdown() + return + + mesh = o3d.io.read_triangle_mesh(detections_path) + pcd = o3d.io.read_point_cloud(pcd_path) + + mesh_frame = o3d.geometry.TriangleMesh.create_coordinate_frame(size=1.0, origin=[0, 0, 0]) + + detection_lines = o3d.geometry.LineSet.create_from_triangle_mesh(mesh) + detection_lines.paint_uniform_color([1.0, 0.0, 1.0]) + + o3d.visualization.draw_geometries([mesh_frame, pcd, detection_lines]) + + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/perception/lidar_centerpoint_tvm/src/single_inference_node.cpp b/perception/lidar_centerpoint_tvm/src/single_inference_node.cpp new file mode 100644 index 0000000000000..8b810c5a7d759 --- /dev/null +++ b/perception/lidar_centerpoint_tvm/src/single_inference_node.cpp @@ -0,0 +1,239 @@ +// Copyright 2022 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 "lidar_centerpoint_tvm/single_inference_node.hpp" + +#include +#include +#include +#include +#include + +#include + +#include +#include +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif + +#include +#include +#include + +namespace autoware +{ +namespace perception +{ +namespace lidar_centerpoint_tvm +{ + +SingleInferenceLidarCenterPointNode::SingleInferenceLidarCenterPointNode( + const rclcpp::NodeOptions & node_options) +: Node("lidar_center_point_tvm", node_options), tf_buffer_(this->get_clock()) +{ + const float score_threshold = + static_cast(this->declare_parameter("score_threshold", 0.35)); + const float circle_nms_dist_threshold = + static_cast(this->declare_parameter("circle_nms_dist_threshold", 1.5)); + const float yaw_norm_threshold = + static_cast(this->declare_parameter("yaw_norm_threshold", 0.0)); + const std::string densification_world_frame_id = + this->declare_parameter("densification_world_frame_id", "map"); + const int densification_num_past_frames = + this->declare_parameter("densification_num_past_frames", 0); + + class_names_ = this->declare_parameter>("class_names"); + has_twist_ = this->declare_parameter("has_twist", false); + const std::size_t point_feature_size = + static_cast(this->declare_parameter("point_feature_size")); + const std::size_t max_voxel_size = + static_cast(this->declare_parameter("max_voxel_size")); + const auto point_cloud_range = this->declare_parameter>("point_cloud_range"); + const auto voxel_size = this->declare_parameter>("voxel_size"); + const std::size_t downsample_factor = + static_cast(this->declare_parameter("downsample_factor")); + const std::size_t encoder_in_feature_size = + static_cast(this->declare_parameter("encoder_in_feature_size")); + const auto pcd_path = this->declare_parameter("pcd_path"); + const auto detections_path = this->declare_parameter("detections_path"); + + DensificationParam densification_param( + densification_world_frame_id, densification_num_past_frames); + + if (point_cloud_range.size() != 6) { + RCLCPP_WARN_STREAM( + rclcpp::get_logger("single_inference_lidar_centerpoint"), + "The size of point_cloud_range != 6: use the default parameters."); + } + if (voxel_size.size() != 3) { + RCLCPP_WARN_STREAM( + rclcpp::get_logger("single_inference_lidar_centerpoint"), + "The size of voxel_size != 3: use the default parameters."); + } + CenterPointConfig config( + class_names_.size(), point_feature_size, max_voxel_size, point_cloud_range, voxel_size, + downsample_factor, encoder_in_feature_size, score_threshold, circle_nms_dist_threshold, + yaw_norm_threshold); + detector_ptr_ = std::make_unique(densification_param, config); + + detect(pcd_path, detections_path); + exit(0); +} + +std::vector SingleInferenceLidarCenterPointNode::getVertices( + const autoware_auto_perception_msgs::msg::Shape & shape, const Eigen::Affine3d & pose) const +{ + std::vector vertices; + Eigen::Vector3d vertex; + + vertex.x() = -shape.dimensions.x / 2.0; + vertex.y() = -shape.dimensions.y / 2.0; + vertex.z() = shape.dimensions.z / 2.0; + vertices.push_back(pose * vertex); + + vertex.x() = -shape.dimensions.x / 2.0; + vertex.y() = shape.dimensions.y / 2.0; + vertex.z() = shape.dimensions.z / 2.0; + vertices.push_back(pose * vertex); + + vertex.x() = -shape.dimensions.x / 2.0; + vertex.y() = shape.dimensions.y / 2.0; + vertex.z() = -shape.dimensions.z / 2.0; + vertices.push_back(pose * vertex); + + vertex.x() = shape.dimensions.x / 2.0; + vertex.y() = shape.dimensions.y / 2.0; + vertex.z() = shape.dimensions.z / 2.0; + vertices.push_back(pose * vertex); + + vertex.x() = shape.dimensions.x / 2.0; + vertex.y() = shape.dimensions.y / 2.0; + vertex.z() = -shape.dimensions.z / 2.0; + vertices.push_back(pose * vertex); + + vertex.x() = shape.dimensions.x / 2.0; + vertex.y() = -shape.dimensions.y / 2.0; + vertex.z() = shape.dimensions.z / 2.0; + vertices.push_back(pose * vertex); + + vertex.x() = shape.dimensions.x / 2.0; + vertex.y() = -shape.dimensions.y / 2.0; + vertex.z() = -shape.dimensions.z / 2.0; + vertices.push_back(pose * vertex); + + vertex.x() = -shape.dimensions.x / 2.0; + vertex.y() = -shape.dimensions.y / 2.0; + vertex.z() = -shape.dimensions.z / 2.0; + vertices.push_back(pose * vertex); + + return vertices; +} + +void SingleInferenceLidarCenterPointNode::detect( + const std::string & pcd_path, const std::string & detections_path) +{ + sensor_msgs::msg::PointCloud2 msg; + pcl::PointCloud::Ptr pc_ptr(new pcl::PointCloud()); + + pcl::io::loadPCDFile(pcd_path, *pc_ptr); + pcl::toROSMsg(*pc_ptr, msg); + msg.header.frame_id = "lidar_frame"; + + std::vector det_boxes3d; + bool is_success = detector_ptr_->detect(msg, tf_buffer_, det_boxes3d); + if (!is_success) { + return; + } + + autoware_auto_perception_msgs::msg::DetectedObjects output_msg; + output_msg.header = msg.header; + for (const auto & box3d : det_boxes3d) { + autoware_auto_perception_msgs::msg::DetectedObject obj; + box3DToDetectedObject(box3d, class_names_, rename_car_to_truck_and_bus_, has_twist_, obj); + output_msg.objects.emplace_back(obj); + } + + dumpDetectionsAsMesh(output_msg, detections_path); + + RCLCPP_INFO( + rclcpp::get_logger("single_inference_lidar_centerpoint_tvm"), + "The detection results were saved as meshes in %s", detections_path.c_str()); +} + +void SingleInferenceLidarCenterPointNode::dumpDetectionsAsMesh( + const autoware_auto_perception_msgs::msg::DetectedObjects & objects_msg, + const std::string & output_path) const +{ + std::ofstream ofs(output_path, std::ofstream::out); + std::stringstream vertices_stream; + std::stringstream faces_stream; + int index = 0; + int num_detections = static_cast(objects_msg.objects.size()); + + ofs << "ply" << std::endl; + ofs << "format ascii 1.0" << std::endl; + ofs << "comment created by lidar_centerpoint" << std::endl; + ofs << "element vertex " << 8 * num_detections << std::endl; + ofs << "property float x" << std::endl; + ofs << "property float y" << std::endl; + ofs << "property float z" << std::endl; + ofs << "element face " << 12 * num_detections << std::endl; + ofs << "property list uchar uint vertex_indices" << std::endl; + ofs << "end_header" << std::endl; + + auto streamFace = [&faces_stream](int v1, int v2, int v3) { + faces_stream << std::to_string(3) << " " << std::to_string(v1) << " " << std::to_string(v2) + << " " << std::to_string(v3) << std::endl; + }; + + for (const auto & object : objects_msg.objects) { + Eigen::Affine3d pose_affine; + tf2::fromMsg(object.kinematics.pose_with_covariance.pose, pose_affine); + + std::vector vertices = getVertices(object.shape, pose_affine); + + for (const auto & vertex : vertices) { + vertices_stream << vertex.x() << " " << vertex.y() << " " << vertex.z() << std::endl; + } + + streamFace(index + 1, index + 3, index + 4); + streamFace(index + 3, index + 5, index + 6); + streamFace(index + 0, index + 7, index + 5); + streamFace(index + 7, index + 2, index + 4); + streamFace(index + 5, index + 3, index + 1); + streamFace(index + 7, index + 0, index + 2); + streamFace(index + 2, index + 1, index + 4); + streamFace(index + 4, index + 3, index + 6); + streamFace(index + 5, index + 7, index + 6); + streamFace(index + 6, index + 7, index + 4); + streamFace(index + 0, index + 5, index + 1); + index += 8; + } + + ofs << vertices_stream.str(); + ofs << faces_stream.str(); + + ofs.close(); +} + +} // namespace lidar_centerpoint_tvm +} // namespace perception +} // namespace autoware + +#include +RCLCPP_COMPONENTS_REGISTER_NODE( + autoware::perception::lidar_centerpoint_tvm::SingleInferenceLidarCenterPointNode) From 2a0f760e78749c92c7c333f1f358d6bee549a23a Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Mon, 24 Apr 2023 14:40:29 +0900 Subject: [PATCH 05/16] feat(intersection): add flag to enable creep towards intersection occlusion (#3510) * feat(intersection): add flag to enable creep towards intersection occlusion Signed-off-by: Takayuki Murooka * fix Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka --- .../behavior_velocity_planner/config/intersection.param.yaml | 1 + .../include/scene_module/intersection/scene_intersection.hpp | 3 ++- .../src/scene_module/intersection/manager.cpp | 1 + .../src/scene_module/intersection/scene_intersection.cpp | 2 +- 4 files changed, 5 insertions(+), 2 deletions(-) diff --git a/planning/behavior_velocity_planner/config/intersection.param.yaml b/planning/behavior_velocity_planner/config/intersection.param.yaml index 927a27a745b3f..d350ed2a759e6 100644 --- a/planning/behavior_velocity_planner/config/intersection.param.yaml +++ b/planning/behavior_velocity_planner/config/intersection.param.yaml @@ -31,6 +31,7 @@ occlusion: enable: true occlusion_detection_area_length: 70.0 # [m] + enable_creeping: false # flag to use the creep velocity when reaching occlusion limit stop line occlusion_creep_velocity: 0.8333 # the creep velocity to occlusion limit stop line free_space_max: 43 occupied_min: 58 diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp index e4d1c016fa49f..19c7145073051 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp @@ -110,7 +110,8 @@ class IntersectionModule : public SceneModuleInterface { bool enable; double occlusion_detection_area_length; //! used for occlusion detection - double occlusion_creep_velocity; //! the creep velocity to occlusion limit stop lline + bool enable_creeping; + double occlusion_creep_velocity; //! the creep velocity to occlusion limit stop lline int free_space_max; int occupied_min; bool do_dp; diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp index 1d7ab5c4f7c0c..0605486da3ea8 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp @@ -81,6 +81,7 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) ip.occlusion.enable = node.declare_parameter(ns + ".occlusion.enable"); ip.occlusion.occlusion_detection_area_length = node.declare_parameter(ns + ".occlusion.occlusion_detection_area_length"); + ip.occlusion.enable_creeping = node.declare_parameter(ns + ".occlusion.enable_creeping"); ip.occlusion.occlusion_creep_velocity = node.declare_parameter(ns + ".occlusion.occlusion_creep_velocity"); ip.occlusion.free_space_max = node.declare_parameter(ns + ".occlusion.free_space_max"); diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp index 4304f4059fac6..186c03bcce854 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp @@ -405,7 +405,7 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * if (!occlusion_activated_) { is_go_out_ = false; /* in case of creeping */ - if (insert_creep_during_occlusion) { + if (insert_creep_during_occlusion && planner_param_.occlusion.enable_creeping) { const auto [start, end] = insert_creep_during_occlusion.value(); for (size_t i = start; i < end; ++i) { planning_utils::setVelocityFromIndex( From bc8ab77a8358d893846715a06077c19bde96072c Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Mon, 24 Apr 2023 15:19:39 +0900 Subject: [PATCH 06/16] chore(lane_change): use BehaviorModuleOuput as output of generatePlannedPath (#3508) * chore(lane_change): use BehaviorModuleOuput as output of generatePlannedPath Signed-off-by: Takayuki Murooka * generatePlannedPath->generateOutput and generateExtendedDrivableArea->extendOutputDrivableArea Signed-off-by: Takayuki Murooka * move assignment in plan to generateOutput Signed-off-by: Muhammad Zulfaqar Azmi * fix pointer error in new module Signed-off-by: Muhammad Zulfaqar Azmi --------- Signed-off-by: Takayuki Murooka Signed-off-by: Muhammad Zulfaqar Azmi Co-authored-by: Muhammad Zulfaqar Azmi --- .../scene_module/lane_change/base_class.hpp | 4 ++-- .../external_request_lane_change_module.hpp | 2 +- .../scene_module/lane_change/normal.hpp | 4 ++-- .../external_request_lane_change_module.cpp | 10 ++++---- .../scene_module/lane_change/interface.cpp | 14 +++-------- .../lane_change/lane_change_module.cpp | 8 ++----- .../src/scene_module/lane_change/normal.cpp | 23 +++++++++++-------- 7 files changed, 30 insertions(+), 35 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp index 6959fcb91616f..99dca7f3efa97 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp @@ -66,9 +66,9 @@ class LaneChangeBase virtual std::pair getSafePath(LaneChangePath & safe_path) const = 0; - virtual PathWithLaneId generatePlannedPath() = 0; + virtual BehaviorModuleOutput generateOutput() = 0; - virtual void generateExtendedDrivableArea(PathWithLaneId & path) = 0; + virtual void extendOutputDrivableArea(BehaviorModuleOutput & output) = 0; virtual bool hasFinishedLaneChange() const = 0; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/external_request_lane_change_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/external_request_lane_change_module.hpp index 157aeb9352bd8..0ae334abe5c93 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/external_request_lane_change_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/external_request_lane_change_module.hpp @@ -110,7 +110,7 @@ class ExternalRequestLaneChangeModule : public SceneModuleInterface LaneChangePath & safe_path) const; void updateLaneChangeStatus(); - void generateExtendedDrivableArea(PathWithLaneId & path); + void extendOutputDrivableArea(BehaviorModuleOutput & output); void updateOutputTurnSignal(BehaviorModuleOutput & output); void updateSteeringFactorPtr(const BehaviorModuleOutput & output); void updateSteeringFactorPtr( 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 097db1ea6e065..eca898f9a59c1 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 @@ -43,9 +43,9 @@ class NormalLaneChange : public LaneChangeBase std::pair getSafePath(LaneChangePath & safe_path) const override; - PathWithLaneId generatePlannedPath() override; + BehaviorModuleOutput generateOutput() override; - void generateExtendedDrivableArea(PathWithLaneId & path) override; + void extendOutputDrivableArea(BehaviorModuleOutput & output) override; bool hasFinishedLaneChange() const override; diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/external_request_lane_change_module.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/external_request_lane_change_module.cpp index a6266e836ebf5..6238f9bb80b57 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/external_request_lane_change_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/external_request_lane_change_module.cpp @@ -141,12 +141,13 @@ BehaviorModuleOutput ExternalRequestLaneChangeModule::plan() } } - generateExtendedDrivableArea(path); - prev_approved_path_ = path; BehaviorModuleOutput output; output.path = std::make_shared(path); + + extendOutputDrivableArea(output); + updateOutputTurnSignal(output); updateSteeringFactorPtr(output); @@ -619,7 +620,7 @@ std_msgs::msg::Header ExternalRequestLaneChangeModule::getRouteHeader() const { return planner_data_->route_handler->getRouteHeader(); } -void ExternalRequestLaneChangeModule::generateExtendedDrivableArea(PathWithLaneId & path) +void ExternalRequestLaneChangeModule::extendOutputDrivableArea(BehaviorModuleOutput & output) { const auto & common_parameters = planner_data_->parameters; const auto & route_handler = planner_data_->route_handler; @@ -629,8 +630,9 @@ void ExternalRequestLaneChangeModule::generateExtendedDrivableArea(PathWithLaneI const auto expanded_lanes = utils::expandLanelets( drivable_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset, dp.drivable_area_types_to_skip); + utils::generateDrivableArea( - path, expanded_lanes, common_parameters.vehicle_length, planner_data_); + *output.path, expanded_lanes, common_parameters.vehicle_length, planner_data_); } bool ExternalRequestLaneChangeModule::isApprovedPathSafe(Pose & ego_pose_before_collision) const diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp index 2d3f43866319a..726f88743be80 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp @@ -116,9 +116,6 @@ BehaviorModuleOutput LaneChangeInterface::plan() resetPathCandidate(); resetPathReference(); - module_type_->setPreviousDrivableLanes(getPreviousModuleOutput().drivable_lanes); - const auto path = module_type_->generatePlannedPath(); - if (!module_type_->isValidPath()) { return {}; } @@ -127,14 +124,9 @@ BehaviorModuleOutput LaneChangeInterface::plan() resetPathIfAbort(); } - const auto reference_path = module_type_->getReferencePath(); - - BehaviorModuleOutput output; - output.path = std::make_shared(path); - output.reference_path = std::make_shared(reference_path); - output.turn_signal_info = module_type_->updateOutputTurnSignal(); - - path_reference_ = std::make_shared(reference_path); + module_type_->setPreviousDrivableLanes(getPreviousModuleOutput().drivable_lanes); + auto output = module_type_->generateOutput(); + path_reference_ = output.reference_path; *prev_approved_path_ = *getPreviousModuleOutput().path; updateSteeringFactorPtr(output); diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp index 0e89367c48a95..73b7f6d823292 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp @@ -113,8 +113,6 @@ BehaviorModuleOutput LaneChangeModule::plan() resetPathReference(); is_activated_ = isActivated(); - const auto path = module_type_->generatePlannedPath(); - if (!module_type_->isValidPath()) { return {}; } @@ -123,12 +121,10 @@ BehaviorModuleOutput LaneChangeModule::plan() resetPathIfAbort(); } - BehaviorModuleOutput output; - output.path = std::make_shared(path); - output.turn_signal_info = module_type_->updateOutputTurnSignal(); + auto output = module_type_->generateOutput(); path_reference_ = getPreviousModuleOutput().reference_path; - prev_approved_path_ = path; + prev_approved_path_ = *output.path; updateSteeringFactorPtr(output); clearWaitingApproval(); 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 61f4cb1e620b2..ebf95cef2b0b8 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 @@ -85,34 +85,39 @@ std::pair NormalLaneChange::getSafePath(LaneChangePath & safe_path) return {true, found_safe_path}; } -PathWithLaneId NormalLaneChange::generatePlannedPath() +BehaviorModuleOutput NormalLaneChange::generateOutput() { - auto path = getLaneChangePath().path; - generateExtendedDrivableArea(path); + BehaviorModuleOutput output; + output.path = std::make_shared(getLaneChangePath().path); + + extendOutputDrivableArea(output); if (isAbortState()) { - return path; + return output; } if (isStopState()) { - const auto stop_point = utils::insertStopPoint(0.1, path); + const auto stop_point = utils::insertStopPoint(0.1, *output.path); } - return path; + output.reference_path = std::make_shared(getReferencePath()); + output.turn_signal_info = updateOutputTurnSignal(); + + return output; } -void NormalLaneChange::generateExtendedDrivableArea(PathWithLaneId & path) +void NormalLaneChange::extendOutputDrivableArea(BehaviorModuleOutput & output) { const auto & common_parameters = planner_data_->parameters; const auto & dp = planner_data_->drivable_area_expansion_parameters; const auto drivable_lanes = getDrivableLanes(); - const auto shorten_lanes = utils::cutOverlappedLanes(path, drivable_lanes); + const auto shorten_lanes = utils::cutOverlappedLanes(*output.path, drivable_lanes); const auto expanded_lanes = utils::expandLanelets( shorten_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset, dp.drivable_area_types_to_skip); utils::generateDrivableArea( - path, expanded_lanes, common_parameters.vehicle_length, planner_data_); + *output.path, expanded_lanes, common_parameters.vehicle_length, planner_data_); } bool NormalLaneChange::hasFinishedLaneChange() const { From 16f64342fe0a2be5647d77699caf094538ed5a19 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Mon, 24 Apr 2023 17:06:07 +0900 Subject: [PATCH 07/16] feat(intersection): send intersection/intersection_occlusion rtc status (#3511) * Merge feat/intersection-occlusion-latest (#8) Signed-off-by: Mamoru Sobue * fixed virtual wall marker Signed-off-by: Mamoru Sobue * use is_occluded flag Signed-off-by: Mamoru Sobue * 3 rtc intersection appears Signed-off-by: Mamoru Sobue * clearCooperateStatus before sendRTC not to duplicate default/occlusion_first_stop_uuid Signed-off-by: Mamoru Sobue --------- Signed-off-by: Mamoru Sobue --- .../intersection/scene_intersection.hpp | 13 ++-- .../behavior_velocity_planner/package.xml | 1 + .../src/scene_module/intersection/debug.cpp | 1 - .../src/scene_module/intersection/manager.cpp | 73 ++++++++++--------- .../intersection/scene_intersection.cpp | 35 ++++----- 5 files changed, 63 insertions(+), 60 deletions(-) diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp index 19c7145073051..45bc12049ba33 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp @@ -157,6 +157,7 @@ class IntersectionModule : public SceneModuleInterface { occlusion_first_stop_activated_ = activation; } + bool isOccluded() const { return is_occluded_; } private: rclcpp::Node & node_; @@ -173,19 +174,19 @@ class IntersectionModule : public SceneModuleInterface // for occlusion detection const bool enable_occlusion_detection_; std::optional> detection_divisions_; - std::optional prev_occlusion_stop_line_pose_; - OcclusionState occlusion_state_; + bool is_occluded_ = false; + OcclusionState occlusion_state_ = OcclusionState::NONE; // NOTE: uuid_ is base member // for occlusion clearance decision const UUID occlusion_uuid_; - bool occlusion_safety_; + bool occlusion_safety_ = true; double occlusion_stop_distance_; - bool occlusion_activated_; + bool occlusion_activated_ = true; // for first stop in two-phase stop const UUID occlusion_first_stop_uuid_; // TODO(Mamoru Sobue): replace with uuid_ - bool occlusion_first_stop_safety_; + bool occlusion_first_stop_safety_ = true; double occlusion_first_stop_distance_; - bool occlusion_first_stop_activated_; + bool occlusion_first_stop_activated_ = true; StateMachine collision_state_machine_; //! for stable collision checking StateMachine before_creep_state_machine_; //! for two phase stop diff --git a/planning/behavior_velocity_planner/package.xml b/planning/behavior_velocity_planner/package.xml index bc023491a1975..4f6903ded6767 100644 --- a/planning/behavior_velocity_planner/package.xml +++ b/planning/behavior_velocity_planner/package.xml @@ -88,6 +88,7 @@ tier4_v2x_msgs vehicle_info_util visualization_msgs + grid_map_rviz_plugin ament_cmake_ros ament_lint_auto diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp index 88e861be51db4..5eb57f06ec372 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp @@ -203,7 +203,6 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createVirtualWallMarker const auto now = this->clock_->now(); - // int32_t uid = planning_utils::bitShift(module_id_); // TODO(Mamoru Sobue): collision stop pose depends on before/after occlusion clearance if (!activated_) { appendMarkerArray( diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp index 0605486da3ea8..66acc4d92e013 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp @@ -127,20 +127,8 @@ void IntersectionModuleManager::launchNewModules( const auto new_module = std::make_shared( module_id, lane_id, planner_data_, intersection_param_, assoc_ids, enable_occlusion_detection, node_, logger_.get_child("intersection_module"), clock_); - const auto occlusion_uuid = new_module->getOcclusionUUID(); - const auto occlusion_first_stop_uuid = new_module->getOcclusionFirstStopUUID(); registerModule(std::move(new_module)); - // default generateUUID(module_id); - updateRTCStatus( - getUUID(module_id), true, std::numeric_limits::lowest(), path.header.stamp); - // occlusion - occlusion_rtc_interface_.updateCooperateStatus( - occlusion_uuid, true, std::numeric_limits::lowest(), - std::numeric_limits::lowest(), path.header.stamp); - rtc_interface_.updateCooperateStatus( - occlusion_first_stop_uuid, true, std::numeric_limits::lowest(), - std::numeric_limits::lowest(), path.header.stamp); enable_occlusion_detection = false; } @@ -188,23 +176,36 @@ bool IntersectionModuleManager::hasSameParentLaneletAndTurnDirectionWithRegister void IntersectionModuleManager::sendRTC(const Time & stamp) { for (const auto & scene_module : scene_modules_) { - // default - const UUID uuid = getUUID(scene_module->getModuleId()); - updateRTCStatus(uuid, scene_module->isSafe(), scene_module->getDistance(), stamp); - // occlusion const auto intersection_module = std::dynamic_pointer_cast(scene_module); + const bool is_occluded = intersection_module->isOccluded(); + const UUID uuid = getUUID(scene_module->getModuleId()); const auto occlusion_uuid = intersection_module->getOcclusionUUID(); - const auto occlusion_safety = intersection_module->getOcclusionSafety(); - const auto occlusion_distance = intersection_module->getOcclusionDistance(); - occlusion_rtc_interface_.updateCooperateStatus( - occlusion_uuid, occlusion_safety, occlusion_distance, occlusion_distance, stamp); - const auto occlusion_first_stop_uuid = intersection_module->getOcclusionFirstStopUUID(); - const auto occlusion_first_stop_safety = intersection_module->getOcclusionFirstStopSafety(); - const auto occlusion_first_stop_distance = intersection_module->getOcclusionFirstStopDistance(); - rtc_interface_.updateCooperateStatus( - occlusion_first_stop_uuid, occlusion_first_stop_safety, occlusion_first_stop_distance, - occlusion_first_stop_distance, stamp); + if (!is_occluded) { + rtc_interface_.clearCooperateStatus(); + // default + updateRTCStatus(uuid, scene_module->isSafe(), scene_module->getDistance(), stamp); + occlusion_rtc_interface_.updateCooperateStatus( + occlusion_uuid, true, std::numeric_limits::lowest(), + std::numeric_limits::lowest(), stamp); + // send {default, occlusion} RTC status + } else { + rtc_interface_.clearCooperateStatus(); + // occlusion + const auto occlusion_safety = intersection_module->getOcclusionSafety(); + const auto occlusion_distance = intersection_module->getOcclusionDistance(); + occlusion_rtc_interface_.updateCooperateStatus( + occlusion_uuid, occlusion_safety, occlusion_distance, occlusion_distance, stamp); + + const auto occlusion_first_stop_safety = intersection_module->getOcclusionFirstStopSafety(); + const auto occlusion_first_stop_distance = + intersection_module->getOcclusionFirstStopDistance(); + rtc_interface_.updateCooperateStatus( + occlusion_first_stop_uuid, occlusion_first_stop_safety, occlusion_first_stop_distance, + occlusion_first_stop_distance, stamp); + // send {first_stop, occlusion} RTC status + // rtc_interface_.removeCooperateStatus(uuid); + } } rtc_interface_.publishCooperateStatus(stamp); // publishRTCStatus() occlusion_rtc_interface_.publishCooperateStatus(stamp); @@ -213,16 +214,22 @@ void IntersectionModuleManager::sendRTC(const Time & stamp) void IntersectionModuleManager::setActivation() { for (const auto & scene_module : scene_modules_) { - // default - scene_module->setActivation(rtc_interface_.isActivated(getUUID(scene_module->getModuleId()))); - // occlusion const auto intersection_module = std::dynamic_pointer_cast(scene_module); const auto occlusion_uuid = intersection_module->getOcclusionUUID(); - intersection_module->setOcclusionActivation( - occlusion_rtc_interface_.isActivated(occlusion_uuid)); const auto occlusion_first_stop_uuid = intersection_module->getOcclusionFirstStopUUID(); - intersection_module->setOcclusionFirstStopActivation( - rtc_interface_.isActivated(occlusion_first_stop_uuid)); + const bool is_occluded = intersection_module->isOccluded(); + if (!is_occluded) { + // default + scene_module->setActivation(rtc_interface_.isActivated(getUUID(scene_module->getModuleId()))); + intersection_module->setOcclusionActivation( + occlusion_rtc_interface_.isActivated(occlusion_uuid)); + } else { + // occlusion + intersection_module->setOcclusionActivation( + occlusion_rtc_interface_.isActivated(occlusion_uuid)); + intersection_module->setOcclusionFirstStopActivation( + rtc_interface_.isActivated(occlusion_first_stop_uuid)); + } } } diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp index 186c03bcce854..1c9e235547c30 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp @@ -67,12 +67,9 @@ IntersectionModule::IntersectionModule( : SceneModuleInterface(module_id, logger, clock), node_(node), lane_id_(lane_id), - is_go_out_(false), assoc_ids_(assoc_ids), enable_occlusion_detection_(enable_occlusion_detection), detection_divisions_(std::nullopt), - prev_occlusion_stop_line_pose_(std::nullopt), - occlusion_state_(OcclusionState::NONE), occlusion_uuid_(tier4_autoware_utils::generateUUID()), occlusion_first_stop_uuid_(tier4_autoware_utils::generateUUID()) { @@ -289,8 +286,7 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * if (!default_stop_line_idx_opt) { occlusion_stop_required = true; stop_line_idx = occlusion_stop_line_idx = occlusion_stop_line_idx_opt; - prev_occlusion_stop_line_pose_ = - path_ip.points.at(occlusion_stop_line_idx_opt.value()).point.pose; + is_occluded_ = true; occlusion_state_ = OcclusionState::CREEP_SECOND_STOP_LINE; RCLCPP_WARN(logger_, "directly stop at occlusion stop line because collision line not found"); } else if (before_creep_state_machine_.getState() == StateMachine::State::GO) { @@ -300,8 +296,7 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * // insert creep velocity [closest_idx, occlusion_stop_line) insert_creep_during_occlusion = std::make_pair(closest_idx, occlusion_stop_line_idx_opt.value()); - prev_occlusion_stop_line_pose_ = - path_ip.points.at(occlusion_stop_line_idx_ip_opt.value()).point.pose; + is_occluded_ = true; occlusion_state_ = OcclusionState::CREEP_SECOND_STOP_LINE; } else { const double dist_default_stop_line = motion_utils::calcSignedArcLength( @@ -320,32 +315,32 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * // insert creep velocity [default_stop_line, occlusion_stop_line) insert_creep_during_occlusion = std::make_pair(default_stop_line_idx_opt.value(), occlusion_stop_line_idx_opt.value()); - prev_occlusion_stop_line_pose_ = - path_ip.points.at(occlusion_stop_line_idx_ip_opt.value()).point.pose; + is_occluded_ = true; occlusion_state_ = OcclusionState::BEFORE_FIRST_STOP_LINE; } - } else if (prev_occlusion_stop_line_pose_) { + } else if (is_occluded_) { // previously occlusion existed, but now it is clear - const auto prev_occlusion_stop_pose_idx = motion_utils::findNearestIndex( - path->points, prev_occlusion_stop_line_pose_.value(), 3.0, M_PI_4); if (!util::isOverTargetIndex( *path, closest_idx, current_pose, default_stop_line_idx_opt.value())) { stop_line_idx = default_stop_line_idx_opt.value(); - prev_occlusion_stop_line_pose_ = std::nullopt; - } else if (!util::isOverTargetIndex( - *path, closest_idx, current_pose, prev_occlusion_stop_pose_idx.get())) { - stop_line_idx = prev_occlusion_stop_pose_idx.get(); - } else { - // TODO(Mamoru Sobue): consider static occlusion limit stop line - prev_occlusion_stop_line_pose_ = std::nullopt; + } else if ( + static_pass_judge_line_opt && + !util::isOverTargetIndex( + *path, closest_idx, current_pose, static_pass_judge_line_opt.value())) { + stop_line_idx = static_pass_judge_line_opt; } occlusion_state_ = OcclusionState::CLEARED; + is_occluded_ = false; if (stop_line_idx && has_collision) { // do collision checking at previous occlusion stop line collision_stop_required = true; } else { collision_stop_required = false; } + if (is_stuck && stuck_line_idx_opt) { + stuck_stop_required = true; + stop_line_idx = static_pass_judge_line_opt; + } } else if (is_stuck && stuck_line_idx_opt) { stuck_stop_required = true; const size_t stuck_line_idx = stuck_line_idx_opt.value(); @@ -401,7 +396,7 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * } /* make decision */ - const double baselink2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; + const double baselink2front = planner_data_->vehicle_info_.vehicle_length_m; if (!occlusion_activated_) { is_go_out_ = false; /* in case of creeping */ From cc5599628c5755f1bb7cbbd13b86621935b412e4 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" <43976882+isamu-takagi@users.noreply.github.com> Date: Mon, 24 Apr 2023 18:11:23 +0900 Subject: [PATCH 08/16] feat(default_ad_api): add route change api (#3197) * feat: add route change api Signed-off-by: Takagi, Isamu * fix: reroute Signed-off-by: Takagi, Isamu --------- Signed-off-by: Takagi, Isamu --- .../include/autoware_ad_api_specs/routing.hpp | 12 ++++++++++++ system/default_ad_api/src/routing.cpp | 2 ++ system/default_ad_api/src/routing.hpp | 4 ++++ .../default_ad_api_helpers/ad_api_adaptors/README.md | 1 + .../ad_api_adaptors/launch/rviz_adaptors.launch.xml | 1 + .../ad_api_adaptors/src/routing_adaptor.cpp | 11 +++++++++++ .../ad_api_adaptors/src/routing_adaptor.hpp | 4 ++++ 7 files changed, 35 insertions(+) diff --git a/common/autoware_ad_api_specs/include/autoware_ad_api_specs/routing.hpp b/common/autoware_ad_api_specs/include/autoware_ad_api_specs/routing.hpp index 76fac74f667f9..62f99a4a8d8e9 100644 --- a/common/autoware_ad_api_specs/include/autoware_ad_api_specs/routing.hpp +++ b/common/autoware_ad_api_specs/include/autoware_ad_api_specs/routing.hpp @@ -38,6 +38,18 @@ struct SetRoute static constexpr char name[] = "/api/routing/set_route"; }; +struct ChangeRoutePoints +{ + using Service = autoware_adapi_v1_msgs::srv::SetRoutePoints; + static constexpr char name[] = "/api/routing/change_route_points"; +}; + +struct ChangeRoute +{ + using Service = autoware_adapi_v1_msgs::srv::SetRoute; + static constexpr char name[] = "/api/routing/change_route"; +}; + struct ClearRoute { using Service = autoware_adapi_v1_msgs::srv::ClearRoute; diff --git a/system/default_ad_api/src/routing.cpp b/system/default_ad_api/src/routing.cpp index f2181eea5fb74..5204146201c8a 100644 --- a/system/default_ad_api/src/routing.cpp +++ b/system/default_ad_api/src/routing.cpp @@ -33,6 +33,8 @@ RoutingNode::RoutingNode(const rclcpp::NodeOptions & options) : Node("routing", adaptor.init_srv(srv_clear_route_, this, &RoutingNode::on_clear_route); adaptor.relay_service(cli_set_route_, srv_set_route_, group_cli_); adaptor.relay_service(cli_set_route_points_, srv_set_route_points_, group_cli_); + adaptor.relay_service(cli_change_route_, srv_change_route_, group_cli_); + adaptor.relay_service(cli_change_route_points_, srv_change_route_points_, group_cli_); adaptor.init_cli(cli_operation_mode_, group_cli_); adaptor.init_sub(sub_operation_mode_, this, &RoutingNode::on_operation_mode); diff --git a/system/default_ad_api/src/routing.hpp b/system/default_ad_api/src/routing.hpp index a0b1626d42807..7c15f6d64608f 100644 --- a/system/default_ad_api/src/routing.hpp +++ b/system/default_ad_api/src/routing.hpp @@ -38,11 +38,15 @@ class RoutingNode : public rclcpp::Node Pub pub_route_; Srv srv_set_route_points_; Srv srv_set_route_; + Srv srv_change_route_points_; + Srv srv_change_route_; Srv srv_clear_route_; Sub sub_state_; Sub sub_route_; Cli cli_set_route_points_; Cli cli_set_route_; + Cli cli_change_route_points_; + Cli cli_change_route_; Cli cli_clear_route_; Cli cli_operation_mode_; Sub sub_operation_mode_; diff --git a/system/default_ad_api_helpers/ad_api_adaptors/README.md b/system/default_ad_api_helpers/ad_api_adaptors/README.md index b0dd92df3a95b..6cb43c5475828 100644 --- a/system/default_ad_api_helpers/ad_api_adaptors/README.md +++ b/system/default_ad_api_helpers/ad_api_adaptors/README.md @@ -24,6 +24,7 @@ The clear API is called automatically before setting the route. | ------------ | ------------------ | ------------------------------------- | -------------------------------------------------- | | Subscription | ~/input/fixed_goal | /planning/mission_planning/goal | The goal pose of route. Disable goal modification. | | Subscription | ~/input/rough_goal | /rviz/routing/rough_goal | The goal pose of route. Enable goal modification. | +| Subscription | ~/input/reroute | /rviz/routing/reroute | The goal pose of reroute. | | Subscription | ~/input/waypoint | /planning/mission_planning/checkpoint | The waypoint pose of route. | | Client | - | /api/routing/clear_route | The route clear API. | | Client | - | /api/routing/set_route_points | The route points set API. | diff --git a/system/default_ad_api_helpers/ad_api_adaptors/launch/rviz_adaptors.launch.xml b/system/default_ad_api_helpers/ad_api_adaptors/launch/rviz_adaptors.launch.xml index 02f873813520c..4ed9d134d2183 100644 --- a/system/default_ad_api_helpers/ad_api_adaptors/launch/rviz_adaptors.launch.xml +++ b/system/default_ad_api_helpers/ad_api_adaptors/launch/rviz_adaptors.launch.xml @@ -11,6 +11,7 @@ + diff --git a/system/default_ad_api_helpers/ad_api_adaptors/src/routing_adaptor.cpp b/system/default_ad_api_helpers/ad_api_adaptors/src/routing_adaptor.cpp index 4823955474d06..7151902a972d6 100644 --- a/system/default_ad_api_helpers/ad_api_adaptors/src/routing_adaptor.cpp +++ b/system/default_ad_api_helpers/ad_api_adaptors/src/routing_adaptor.cpp @@ -27,10 +27,13 @@ RoutingAdaptor::RoutingAdaptor() : Node("routing_adaptor") "~/input/fixed_goal", 3, std::bind(&RoutingAdaptor::on_fixed_goal, this, _1)); sub_rough_goal_ = create_subscription( "~/input/rough_goal", 3, std::bind(&RoutingAdaptor::on_rough_goal, this, _1)); + sub_reroute_ = create_subscription( + "~/input/reroute", 3, std::bind(&RoutingAdaptor::on_reroute, this, _1)); sub_waypoint_ = create_subscription( "~/input/waypoint", 10, std::bind(&RoutingAdaptor::on_waypoint, this, _1)); const auto adaptor = component_interface_utils::NodeAdaptor(this); + adaptor.init_cli(cli_reroute_); adaptor.init_cli(cli_route_); adaptor.init_cli(cli_clear_); adaptor.init_sub( @@ -97,6 +100,14 @@ void RoutingAdaptor::on_waypoint(const PoseStamped::ConstSharedPtr pose) route_->waypoints.push_back(pose->pose); } +void RoutingAdaptor::on_reroute(const PoseStamped::ConstSharedPtr pose) +{ + const auto route = std::make_shared(); + route->header = pose->header; + route->goal = pose->pose; + cli_reroute_->async_send_request(route); +} + } // namespace ad_api_adaptors int main(int argc, char ** argv) diff --git a/system/default_ad_api_helpers/ad_api_adaptors/src/routing_adaptor.hpp b/system/default_ad_api_helpers/ad_api_adaptors/src/routing_adaptor.hpp index 6c0bbf72e40a7..4040ee37ead3e 100644 --- a/system/default_ad_api_helpers/ad_api_adaptors/src/routing_adaptor.hpp +++ b/system/default_ad_api_helpers/ad_api_adaptors/src/routing_adaptor.hpp @@ -34,14 +34,17 @@ class RoutingAdaptor : public rclcpp::Node private: using PoseStamped = geometry_msgs::msg::PoseStamped; using SetRoutePoints = autoware_ad_api::routing::SetRoutePoints; + using ChangeRoutePoints = autoware_ad_api::routing::ChangeRoutePoints; using ClearRoute = autoware_ad_api::routing::ClearRoute; using RouteState = autoware_ad_api::routing::RouteState; + component_interface_utils::Client::SharedPtr cli_reroute_; component_interface_utils::Client::SharedPtr cli_route_; component_interface_utils::Client::SharedPtr cli_clear_; component_interface_utils::Subscription::SharedPtr sub_state_; rclcpp::Subscription::SharedPtr sub_fixed_goal_; rclcpp::Subscription::SharedPtr sub_rough_goal_; rclcpp::Subscription::SharedPtr sub_waypoint_; + rclcpp::Subscription::SharedPtr sub_reroute_; rclcpp::TimerBase::SharedPtr timer_; bool calling_service_ = false; @@ -53,6 +56,7 @@ class RoutingAdaptor : public rclcpp::Node void on_fixed_goal(const PoseStamped::ConstSharedPtr pose); void on_rough_goal(const PoseStamped::ConstSharedPtr pose); void on_waypoint(const PoseStamped::ConstSharedPtr pose); + void on_reroute(const PoseStamped::ConstSharedPtr pose); }; } // namespace ad_api_adaptors From bf5998600ab7512d10ed7824acc8fe3b6b14f427 Mon Sep 17 00:00:00 2001 From: badai nguyen <94814556+badai-nguyen@users.noreply.github.com> Date: Mon, 24 Apr 2023 18:26:34 +0900 Subject: [PATCH 09/16] fix(compare_map_segmentation): update remaining compare map filters for dynamic map loader (#3365) * initialize dynamic loader class Signed-off-by: badai-nguyen * fix: static and dynamic loader Signed-off-by: badai-nguyen * fix(voxel_distance_based): add processing time Signed-off-by: badai-nguyen * feat: add map loader for voxel based approximatef filter Signed-off-by: badai-nguyen * fix: add debug processing time Signed-off-by: badai-nguyen * feat: add dml for distance based compare map filter Signed-off-by: badai-nguyen * fix: dyanmic map loader for distance based Signed-off-by: badai-nguyen * refactoring Signed-off-by: badai-nguyen * fix: voxel_distance_base bug Signed-off-by: badai-nguyen * fix: bug Signed-off-by: badai-nguyen * docs: update readme Signed-off-by: badai-nguyen * chore: typo Signed-off-by: badai-nguyen * chore: typo Signed-off-by: badai-nguyen * chore: remove excessive comments Signed-off-by: badai-nguyen --------- Signed-off-by: badai-nguyen --- perception/compare_map_segmentation/README.md | 6 +- ...tance_based_compare_map_filter_nodelet.hpp | 88 +++++++-- ...approximate_compare_map_filter_nodelet.hpp | 43 +++-- ...tance_based_compare_map_filter_nodelet.hpp | 110 ++++++++++-- .../voxel_grid_map_loader.hpp | 23 +-- .../compare_map_segmentation/package.xml | 2 +- ...tance_based_compare_map_filter_nodelet.cpp | 164 ++++++++++++----- ...approximate_compare_map_filter_nodelet.cpp | 119 +++++++------ ...tance_based_compare_map_filter_nodelet.cpp | 167 +++++++++++------- .../src/voxel_grid_map_loader.cpp | 22 ++- 10 files changed, 517 insertions(+), 227 deletions(-) diff --git a/perception/compare_map_segmentation/README.md b/perception/compare_map_segmentation/README.md index df3c8600f5a67..b70b71789163b 100644 --- a/perception/compare_map_segmentation/README.md +++ b/perception/compare_map_segmentation/README.md @@ -16,11 +16,11 @@ Compare the z of the input points with the value of elevation_map. The height di ### Distance Based Compare Map Filter -WIP +This filter compares the input pointcloud with the map pointcloud using the `nearestKSearch` function of `kdtree` and removes points that are close to the map point cloud. The map pointcloud can be loaded statically at once at the beginning or dynamically as the vehicle moves. ### Voxel Based Approximate Compare Map Filter -WIP +The filter loads the map point cloud, which can be loaded statically at the beginning or dynamically during vehicle movement, and creates a voxel grid of the map point cloud. The filter uses the getCentroidIndexAt function in combination with the getGridCoordinates function from the VoxelGrid class to find input points that are inside the voxel grid and removes them. ### Voxel Based Compare Map Filter @@ -30,7 +30,7 @@ For each point of input pointcloud, the filter use `getCentroidIndexAt` combine ### Voxel Distance based Compare Map Filter -WIP +This filter is a combination of the distance_based_compare_map_filter and voxel_based_approximate_compare_map_filter. The filter loads the map point cloud, which can be loaded statically at the beginning or dynamically during vehicle movement, and creates a voxel grid and a k-d tree of the map point cloud. The filter uses the getCentroidIndexAt function in combination with the getGridCoordinates function from the VoxelGrid class to find input points that are inside the voxel grid and removes them. For points that do not belong to any voxel grid, they are compared again with the map point cloud using the radiusSearch function of the k-d tree and are removed if they are close enough to the map. ## Inputs / Outputs diff --git a/perception/compare_map_segmentation/include/compare_map_segmentation/distance_based_compare_map_filter_nodelet.hpp b/perception/compare_map_segmentation/include/compare_map_segmentation/distance_based_compare_map_filter_nodelet.hpp index 613cfd2b177e1..8090ea2e4221a 100644 --- a/perception/compare_map_segmentation/include/compare_map_segmentation/distance_based_compare_map_filter_nodelet.hpp +++ b/perception/compare_map_segmentation/include/compare_map_segmentation/distance_based_compare_map_filter_nodelet.hpp @@ -16,33 +16,99 @@ #define COMPARE_MAP_SEGMENTATION__DISTANCE_BASED_COMPARE_MAP_FILTER_NODELET_HPP_ #include "pointcloud_preprocessor/filter.hpp" +#include "voxel_grid_map_loader.hpp" +#include // for pcl::isFinite #include #include +#include +#include #include namespace compare_map_segmentation { + +typedef typename pcl::Filter::PointCloud PointCloud; +typedef typename PointCloud::Ptr PointCloudPtr; +typedef typename PointCloud::ConstPtr PointCloudConstPtr; + +class DistanceBasedStaticMapLoader : public VoxelGridStaticMapLoader +{ +private: + PointCloudConstPtr map_ptr_; + pcl::search::Search::Ptr tree_; + +public: + DistanceBasedStaticMapLoader( + rclcpp::Node * node, double leaf_size, std::string * tf_map_input_frame, std::mutex * mutex) + : VoxelGridStaticMapLoader(node, leaf_size, tf_map_input_frame, mutex) + { + RCLCPP_INFO(logger_, "DistanceBasedStaticMapLoader initialized.\n"); + } + + void onMapCallback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr map) override; + bool is_close_to_map(const pcl::PointXYZ & point, const double distance_threshold) override; +}; + +class DistanceBasedDynamicMapLoader : public VoxelGridDynamicMapLoader +{ +public: + DistanceBasedDynamicMapLoader( + rclcpp::Node * node, double leaf_size, std::string * tf_map_input_frame, std::mutex * mutex, + rclcpp::CallbackGroup::SharedPtr main_callback_group) + : VoxelGridDynamicMapLoader(node, leaf_size, tf_map_input_frame, mutex, main_callback_group) + { + RCLCPP_INFO(logger_, "DistanceBasedDynamicMapLoader initialized.\n"); + } + bool is_close_to_map(const pcl::PointXYZ & point, const double distance_threshold) override; + + inline void addMapCellAndFilter( + const autoware_map_msgs::msg::PointCloudMapCellWithID & map_cell_to_add) override + { + map_grid_size_x_ = map_cell_to_add.metadata.max_x - map_cell_to_add.metadata.min_x; + map_grid_size_y_ = map_cell_to_add.metadata.max_y - map_cell_to_add.metadata.min_y; + + pcl::PointCloud map_cell_pc_tmp; + pcl::fromROSMsg(map_cell_to_add.pointcloud, map_cell_pc_tmp); + + auto map_cell_voxel_input_tmp_ptr = + std::make_shared>(map_cell_pc_tmp); + + MapGridVoxelInfo current_voxel_grid_list_item; + current_voxel_grid_list_item.min_b_x = map_cell_to_add.metadata.min_x; + current_voxel_grid_list_item.min_b_y = map_cell_to_add.metadata.min_y; + current_voxel_grid_list_item.max_b_x = map_cell_to_add.metadata.max_x; + current_voxel_grid_list_item.max_b_y = map_cell_to_add.metadata.max_y; + + // add kdtree + pcl::search::Search::Ptr tree_tmp; + if (!tree_tmp) { + if (map_cell_voxel_input_tmp_ptr->isOrganized()) { + tree_tmp.reset(new pcl::search::OrganizedNeighbor()); + } else { + tree_tmp.reset(new pcl::search::KdTree(false)); + } + } + tree_tmp->setInputCloud(map_cell_voxel_input_tmp_ptr); + current_voxel_grid_list_item.map_cell_kdtree = tree_tmp; + + // add + (*mutex_ptr_).lock(); + current_voxel_grid_dict_.insert({map_cell_to_add.cell_id, current_voxel_grid_list_item}); + (*mutex_ptr_).unlock(); + } +}; + class DistanceBasedCompareMapFilterComponent : public pointcloud_preprocessor::Filter { protected: virtual void filter( const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output); - void input_target_callback(const PointCloud2ConstPtr map); - private: - rclcpp::Subscription::SharedPtr sub_map_; - PointCloudConstPtr map_ptr_; double distance_threshold_; - pcl::search::Search::Ptr tree_; - - /** \brief Parameter service callback result : needed to be hold */ - OnSetParametersCallbackHandle::SharedPtr set_param_res_; - - /** \brief Parameter service callback */ - rcl_interfaces::msg::SetParametersResult paramCallback(const std::vector & p); + std::unique_ptr distance_based_map_loader_; public: PCL_MAKE_ALIGNED_OPERATOR_NEW diff --git a/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_based_approximate_compare_map_filter_nodelet.hpp b/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_based_approximate_compare_map_filter_nodelet.hpp index 53edda0852cf6..f05f8100873cd 100644 --- a/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_based_approximate_compare_map_filter_nodelet.hpp +++ b/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_based_approximate_compare_map_filter_nodelet.hpp @@ -16,35 +16,52 @@ #define COMPARE_MAP_SEGMENTATION__VOXEL_BASED_APPROXIMATE_COMPARE_MAP_FILTER_NODELET_HPP_ // NOLINT #include "pointcloud_preprocessor/filter.hpp" +#include "voxel_grid_map_loader.hpp" #include #include +#include +#include #include namespace compare_map_segmentation { + +class VoxelBasedApproximateStaticMapLoader : public VoxelGridStaticMapLoader +{ +public: + explicit VoxelBasedApproximateStaticMapLoader( + rclcpp::Node * node, double leaf_size, std::string * tf_map_input_frame, std::mutex * mutex) + : VoxelGridStaticMapLoader(node, leaf_size, tf_map_input_frame, mutex) + { + RCLCPP_INFO(logger_, "VoxelBasedApproximateStaticMapLoader initialized.\n"); + } + bool is_close_to_map(const pcl::PointXYZ & point, const double distance_threshold) override; +}; + +class VoxelBasedApproximateDynamicMapLoader : public VoxelGridDynamicMapLoader +{ +public: + VoxelBasedApproximateDynamicMapLoader( + rclcpp::Node * node, double leaf_size, std::string * tf_map_input_frame, std::mutex * mutex, + rclcpp::CallbackGroup::SharedPtr main_callback_group) + : VoxelGridDynamicMapLoader(node, leaf_size, tf_map_input_frame, mutex, main_callback_group) + { + RCLCPP_INFO(logger_, "VoxelBasedApproximateDynamicMapLoader initialized.\n"); + } + bool is_close_to_map(const pcl::PointXYZ & point, const double distance_threshold) override; +}; + class VoxelBasedApproximateCompareMapFilterComponent : public pointcloud_preprocessor::Filter { protected: virtual void filter( const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output); - void input_target_callback(const PointCloud2ConstPtr map); - private: - // pcl::SegmentDifferences impl_; - rclcpp::Subscription::SharedPtr sub_map_; - PointCloudPtr voxel_map_ptr_; double distance_threshold_; - pcl::VoxelGrid voxel_grid_; - bool set_map_in_voxel_grid_; - - /** \brief Parameter service callback result : needed to be hold */ - OnSetParametersCallbackHandle::SharedPtr set_param_res_; - - /** \brief Parameter service callback */ - rcl_interfaces::msg::SetParametersResult paramCallback(const std::vector & p); + std::unique_ptr voxel_based_approximate_map_loader_; public: PCL_MAKE_ALIGNED_OPERATOR_NEW diff --git a/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_distance_based_compare_map_filter_nodelet.hpp b/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_distance_based_compare_map_filter_nodelet.hpp index e16f1230a9608..d60e4af75e89c 100644 --- a/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_distance_based_compare_map_filter_nodelet.hpp +++ b/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_distance_based_compare_map_filter_nodelet.hpp @@ -16,14 +16,110 @@ #define COMPARE_MAP_SEGMENTATION__VOXEL_DISTANCE_BASED_COMPARE_MAP_FILTER_NODELET_HPP_ // NOLINT #include "pointcloud_preprocessor/filter.hpp" +#include "voxel_grid_map_loader.hpp" #include #include +#include +#include +#include +#include #include namespace compare_map_segmentation { + +typedef typename pcl::Filter::PointCloud PointCloud; +typedef typename PointCloud::Ptr PointCloudPtr; +typedef typename PointCloud::ConstPtr PointCloudConstPtr; + +class VoxelDistanceBasedStaticMapLoader : public VoxelGridStaticMapLoader +{ +private: + PointCloudConstPtr map_ptr_; + pcl::search::Search::Ptr tree_; + +public: + explicit VoxelDistanceBasedStaticMapLoader( + rclcpp::Node * node, double leaf_size, std::string * tf_map_input_frame, std::mutex * mutex) + : VoxelGridStaticMapLoader(node, leaf_size, tf_map_input_frame, mutex) + { + RCLCPP_INFO(logger_, "VoxelDistanceBasedStaticMapLoader initialized.\n"); + } + bool is_close_to_map(const pcl::PointXYZ & point, const double distance_threshold) override; + void onMapCallback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr map) override; +}; + +class VoxelDistanceBasedDynamicMapLoader : public VoxelGridDynamicMapLoader +{ +protected: +private: + PointCloudConstPtr map_ptr_; + /* data */ +public: + explicit VoxelDistanceBasedDynamicMapLoader( + rclcpp::Node * node, double leaf_size, std::string * tf_map_input_frame, std::mutex * mutex, + rclcpp::CallbackGroup::SharedPtr main_callback_group) + : VoxelGridDynamicMapLoader(node, leaf_size, tf_map_input_frame, mutex, main_callback_group) + { + RCLCPP_INFO(logger_, "VoxelDistanceBasedDynamicMapLoader initialized.\n"); + } + bool is_close_to_map(const pcl::PointXYZ & point, const double distance_threshold) override; + + inline void addMapCellAndFilter( + const autoware_map_msgs::msg::PointCloudMapCellWithID & map_cell_to_add) override + { + map_grid_size_x_ = map_cell_to_add.metadata.max_x - map_cell_to_add.metadata.min_x; + map_grid_size_y_ = map_cell_to_add.metadata.max_y - map_cell_to_add.metadata.min_y; + + pcl::PointCloud map_cell_pc_tmp; + pcl::fromROSMsg(map_cell_to_add.pointcloud, map_cell_pc_tmp); + + VoxelGridPointXYZ map_cell_voxel_grid_tmp; + PointCloudPtr map_cell_downsampled_pc_ptr_tmp; + + auto map_cell_voxel_input_tmp_ptr = + std::make_shared>(map_cell_pc_tmp); + map_cell_voxel_grid_tmp.setLeafSize(voxel_leaf_size_, voxel_leaf_size_, voxel_leaf_size_); + map_cell_downsampled_pc_ptr_tmp.reset(new pcl::PointCloud); + map_cell_voxel_grid_tmp.setInputCloud(map_cell_voxel_input_tmp_ptr); + map_cell_voxel_grid_tmp.setSaveLeafLayout(true); + map_cell_voxel_grid_tmp.filter(*map_cell_downsampled_pc_ptr_tmp); + + MapGridVoxelInfo current_voxel_grid_list_item; + current_voxel_grid_list_item.min_b_x = map_cell_to_add.metadata.min_x; + current_voxel_grid_list_item.min_b_y = map_cell_to_add.metadata.min_y; + current_voxel_grid_list_item.max_b_x = map_cell_to_add.metadata.max_x; + current_voxel_grid_list_item.max_b_y = map_cell_to_add.metadata.max_y; + + current_voxel_grid_list_item.map_cell_voxel_grid.set_voxel_grid( + &(map_cell_voxel_grid_tmp.leaf_layout_), map_cell_voxel_grid_tmp.get_min_b(), + map_cell_voxel_grid_tmp.get_max_b(), map_cell_voxel_grid_tmp.get_div_b(), + map_cell_voxel_grid_tmp.get_divb_mul(), map_cell_voxel_grid_tmp.get_inverse_leaf_size()); + + current_voxel_grid_list_item.map_cell_pc_ptr.reset(new pcl::PointCloud); + current_voxel_grid_list_item.map_cell_pc_ptr = std::move(map_cell_downsampled_pc_ptr_tmp); + + // add kdtree + pcl::search::Search::Ptr tree_tmp; + if (!tree_tmp) { + if (map_cell_voxel_input_tmp_ptr->isOrganized()) { + tree_tmp.reset(new pcl::search::OrganizedNeighbor()); + } else { + tree_tmp.reset(new pcl::search::KdTree(false)); + } + } + tree_tmp->setInputCloud(map_cell_voxel_input_tmp_ptr); + current_voxel_grid_list_item.map_cell_kdtree = tree_tmp; + + // add + (*mutex_ptr_).lock(); + current_voxel_grid_dict_.insert({map_cell_to_add.cell_id, current_voxel_grid_list_item}); + (*mutex_ptr_).unlock(); + } +}; + class VoxelDistanceBasedCompareMapFilterComponent : public pointcloud_preprocessor::Filter { protected: @@ -33,20 +129,8 @@ class VoxelDistanceBasedCompareMapFilterComponent : public pointcloud_preprocess void input_target_callback(const PointCloud2ConstPtr map); private: - // pcl::SegmentDifferences impl_; - rclcpp::Subscription::SharedPtr sub_map_; - PointCloudPtr voxel_map_ptr_; - PointCloudConstPtr map_ptr_; + std::unique_ptr voxel_distance_based_map_loader_; double distance_threshold_; - pcl::search::Search::Ptr tree_; - pcl::VoxelGrid voxel_grid_; - bool set_map_in_voxel_grid_; - - /** \brief Parameter service callback result : needed to be hold */ - OnSetParametersCallbackHandle::SharedPtr set_param_res_; - - /** \brief Parameter service callback */ - rcl_interfaces::msg::SetParametersResult paramCallback(const std::vector & p); public: PCL_MAKE_ALIGNED_OPERATOR_NEW diff --git a/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_grid_map_loader.hpp b/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_grid_map_loader.hpp index 522f9aefbe34a..b9e424eb9b613 100644 --- a/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_grid_map_loader.hpp +++ b/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_grid_map_loader.hpp @@ -24,6 +24,7 @@ #include #include +#include #include #include @@ -105,6 +106,9 @@ class VoxelGridMapLoader explicit VoxelGridMapLoader( rclcpp::Node * node, double leaf_size, std::string * tf_map_input_frame, std::mutex * mutex); virtual bool is_close_to_map(const pcl::PointXYZ & point, const double distance_threshold) = 0; + bool is_close_to_neighbor_voxels( + const pcl::PointXYZ & point, const double distance_threshold, VoxelGridPointXYZ & voxel, + pcl::search::Search::Ptr tree) const; bool is_close_to_neighbor_voxels( const pcl::PointXYZ & point, const double distance_threshold, const PointCloudPtr & map, VoxelGridPointXYZ & voxel) const; @@ -119,10 +123,9 @@ class VoxelGridMapLoader std::string * tf_map_input_frame_; }; -//*************************** for Static Map loader Voxel Grid Filter ************* class VoxelGridStaticMapLoader : public VoxelGridMapLoader { -private: +protected: rclcpp::Subscription::SharedPtr sub_map_; VoxelGridPointXYZ voxel_grid_; PointCloudPtr voxel_map_ptr_; @@ -130,23 +133,22 @@ class VoxelGridStaticMapLoader : public VoxelGridMapLoader public: explicit VoxelGridStaticMapLoader( rclcpp::Node * node, double leaf_size, std::string * tf_map_input_frame, std::mutex * mutex); - void onMapCallback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr map); - bool is_close_to_map(const pcl::PointXYZ & point, const double distance_threshold); + virtual void onMapCallback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr map); + virtual bool is_close_to_map(const pcl::PointXYZ & point, const double distance_threshold); }; -// *************** for Dynamic and Differential Map loader Voxel Grid Filter ************* class VoxelGridDynamicMapLoader : public VoxelGridMapLoader { +protected: struct MapGridVoxelInfo { VoxelGridPointXYZ map_cell_voxel_grid; PointCloudPtr map_cell_pc_ptr; float min_b_x, min_b_y, max_b_x, max_b_y; + pcl::search::Search::Ptr map_cell_kdtree; }; typedef typename std::map VoxelGridDict; - -private: using InitializationState = localization_interface::InitializationState; /** \brief Map to hold loaded map grid id and it's voxel filter */ @@ -195,7 +197,7 @@ class VoxelGridDynamicMapLoader : public VoxelGridMapLoader void timer_callback(); bool should_update_map() const; void request_update_map(const geometry_msgs::msg::Point & position); - bool is_close_to_map(const pcl::PointXYZ & point, const double distance_threshold); + virtual bool is_close_to_map(const pcl::PointXYZ & point, const double distance_threshold); /** \brief Check if point close to map pointcloud in the */ bool is_close_to_next_map_grid( const pcl::PointXYZ & point, const int current_map_grid_index, const double distance_threshold); @@ -231,7 +233,7 @@ class VoxelGridDynamicMapLoader : public VoxelGridMapLoader } /** Update loaded map grid array for fast searching*/ - inline void updateVoxelGridArray() + virtual inline void updateVoxelGridArray() { origin_x_ = std::floor((current_position_.value().x - map_loader_radius_) / map_grid_size_x_) * map_grid_size_x_; @@ -269,12 +271,11 @@ class VoxelGridDynamicMapLoader : public VoxelGridMapLoader (*mutex_ptr_).unlock(); } - inline void addMapCellAndFilter( + virtual inline void addMapCellAndFilter( const autoware_map_msgs::msg::PointCloudMapCellWithID & map_cell_to_add) { map_grid_size_x_ = map_cell_to_add.metadata.max_x - map_cell_to_add.metadata.min_x; map_grid_size_y_ = map_cell_to_add.metadata.max_y - map_cell_to_add.metadata.min_y; - pcl::PointCloud map_cell_pc_tmp; pcl::fromROSMsg(map_cell_to_add.pointcloud, map_cell_pc_tmp); diff --git a/perception/compare_map_segmentation/package.xml b/perception/compare_map_segmentation/package.xml index 37dac621a61ad..bd2bccf69ebf5 100644 --- a/perception/compare_map_segmentation/package.xml +++ b/perception/compare_map_segmentation/package.xml @@ -3,7 +3,7 @@ compare_map_segmentation 0.1.0 - The ROS2 compare_map_segmentation package + The ROS 2 compare_map_segmentation package amc-nu Yukihiro Saito badai nguyen diff --git a/perception/compare_map_segmentation/src/distance_based_compare_map_filter_nodelet.cpp b/perception/compare_map_segmentation/src/distance_based_compare_map_filter_nodelet.cpp index b96b8faa98c99..519afa4145bcf 100644 --- a/perception/compare_map_segmentation/src/distance_based_compare_map_filter_nodelet.cpp +++ b/perception/compare_map_segmentation/src/distance_based_compare_map_filter_nodelet.cpp @@ -22,52 +22,17 @@ namespace compare_map_segmentation { -using pointcloud_preprocessor::get_param; -DistanceBasedCompareMapFilterComponent::DistanceBasedCompareMapFilterComponent( - const rclcpp::NodeOptions & options) -: Filter("DistanceBasedCompareMapFilter", options) -{ - distance_threshold_ = static_cast(declare_parameter("distance_threshold", 0.3)); - - using std::placeholders::_1; - sub_map_ = this->create_subscription( - "map", rclcpp::QoS{1}.transient_local(), - std::bind(&DistanceBasedCompareMapFilterComponent::input_target_callback, this, _1)); - - set_param_res_ = this->add_on_set_parameters_callback( - std::bind(&DistanceBasedCompareMapFilterComponent::paramCallback, this, _1)); -} - -void DistanceBasedCompareMapFilterComponent::filter( - const PointCloud2ConstPtr & input, [[maybe_unused]] const IndicesPtr & indices, - PointCloud2 & output) -{ - std::scoped_lock lock(mutex_); - if (map_ptr_ == NULL || tree_ == NULL) { - output = *input; - return; - } - pcl::PointCloud::Ptr pcl_input(new pcl::PointCloud); - pcl::PointCloud::Ptr pcl_output(new pcl::PointCloud); - pcl::fromROSMsg(*input, *pcl_input); - pcl_output->points.reserve(pcl_input->points.size()); - pcl::getPointCloudDifference( - *pcl_input, distance_threshold_ * distance_threshold_, tree_, *pcl_output); - - pcl::toROSMsg(*pcl_output, output); - output.header = input->header; -} - -void DistanceBasedCompareMapFilterComponent::input_target_callback(const PointCloud2ConstPtr map) +void DistanceBasedStaticMapLoader::onMapCallback( + const sensor_msgs::msg::PointCloud2::ConstSharedPtr map) { pcl::PointCloud map_pcl; pcl::fromROSMsg(*map, map_pcl); const auto map_pcl_ptr = pcl::make_shared>(map_pcl); - std::scoped_lock lock(mutex_); + (*mutex_ptr_).lock(); map_ptr_ = map_pcl_ptr; - tf_input_frame_ = map_ptr_->header.frame_id; + *tf_map_input_frame_ = map_ptr_->header.frame_id; if (!tree_) { if (map_ptr_->isOrganized()) { tree_.reset(new pcl::search::OrganizedNeighbor()); @@ -76,23 +41,128 @@ void DistanceBasedCompareMapFilterComponent::input_target_callback(const PointCl } } tree_->setInputCloud(map_ptr_); + + (*mutex_ptr_).unlock(); +} + +bool DistanceBasedStaticMapLoader::is_close_to_map( + const pcl::PointXYZ & point, const double distance_threshold) +{ + if (map_ptr_ == NULL) { + return false; + } + if (tree_ == NULL) { + return false; + } + + std::vector nn_indices(1); + std::vector nn_distances(1); + if (!isFinite(point)) { + return false; + } + if (!tree_->nearestKSearch(point, 1, nn_indices, nn_distances)) { + return false; + } + if (nn_distances[0] > distance_threshold) { + return false; + } + return true; } -rcl_interfaces::msg::SetParametersResult DistanceBasedCompareMapFilterComponent::paramCallback( - const std::vector & p) +bool DistanceBasedDynamicMapLoader::is_close_to_map( + const pcl::PointXYZ & point, const double distance_threshold) +{ + if (current_voxel_grid_dict_.size() == 0) { + return false; + } + if (!isFinite(point)) { + return false; + } + + const int map_grid_index = static_cast( + std::floor((point.x - origin_x_) / map_grid_size_x_) + + map_grids_x_ * std::floor((point.y - origin_y_) / map_grid_size_y_)); + + if (static_cast(map_grid_index) >= current_voxel_grid_array_.size()) { + return false; + } + if (current_voxel_grid_array_.at(map_grid_index) != NULL) { + if (current_voxel_grid_array_.at(map_grid_index)->map_cell_kdtree == NULL) { + return false; + } + std::vector nn_indices(1); + std::vector nn_distances(1); + if (!current_voxel_grid_array_.at(map_grid_index) + ->map_cell_kdtree->nearestKSearch(point, 1, nn_indices, nn_distances)) { + return false; + } + if (nn_distances[0] <= distance_threshold) { + return true; + } + } + return false; +} + +DistanceBasedCompareMapFilterComponent::DistanceBasedCompareMapFilterComponent( + const rclcpp::NodeOptions & options) +: Filter("DistanceBasedCompareMapFilter", options) +{ + // initialize debug tool + { + using tier4_autoware_utils::DebugPublisher; + using tier4_autoware_utils::StopWatch; + stop_watch_ptr_ = std::make_unique>(); + debug_publisher_ = std::make_unique(this, "distance_based_compare_map_filter"); + stop_watch_ptr_->tic("cyclic_time"); + stop_watch_ptr_->tic("processing_time"); + } + + distance_threshold_ = declare_parameter("distance_threshold"); + bool use_dynamic_map_loading = declare_parameter("use_dynamic_map_loading"); + if (use_dynamic_map_loading) { + rclcpp::CallbackGroup::SharedPtr main_callback_group; + main_callback_group = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + distance_based_map_loader_ = std::make_unique( + this, distance_threshold_, &tf_input_frame_, &mutex_, main_callback_group); + } else { + distance_based_map_loader_ = std::make_unique( + this, distance_threshold_, &tf_input_frame_, &mutex_); + } +} + +void DistanceBasedCompareMapFilterComponent::filter( + const PointCloud2ConstPtr & input, [[maybe_unused]] const IndicesPtr & indices, + PointCloud2 & output) { std::scoped_lock lock(mutex_); + stop_watch_ptr_->toc("processing_time", true); - if (get_param(p, "distance_threshold", distance_threshold_)) { - RCLCPP_DEBUG(get_logger(), "Setting new distance threshold to: %f.", distance_threshold_); + pcl::PointCloud::Ptr pcl_input(new pcl::PointCloud); + pcl::PointCloud::Ptr pcl_output(new pcl::PointCloud); + pcl::fromROSMsg(*input, *pcl_input); + pcl_output->points.reserve(pcl_input->points.size()); + + for (size_t i = 0; i < pcl_input->points.size(); ++i) { + if (distance_based_map_loader_->is_close_to_map(pcl_input->points.at(i), distance_threshold_)) { + continue; + } + pcl_output->points.push_back(pcl_input->points.at(i)); } - rcl_interfaces::msg::SetParametersResult result; - result.successful = true; - result.reason = "success"; + pcl::toROSMsg(*pcl_output, output); + output.header = input->header; - return result; + // add processing time for debug + if (debug_publisher_) { + const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); + const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); + debug_publisher_->publish( + "debug/cyclic_time_ms", cyclic_time_ms); + debug_publisher_->publish( + "debug/processing_time_ms", processing_time_ms); + } } + } // namespace compare_map_segmentation #include diff --git a/perception/compare_map_segmentation/src/voxel_based_approximate_compare_map_filter_nodelet.cpp b/perception/compare_map_segmentation/src/voxel_based_approximate_compare_map_filter_nodelet.cpp index 3ffed162b65e9..830503467a02a 100644 --- a/perception/compare_map_segmentation/src/voxel_based_approximate_compare_map_filter_nodelet.cpp +++ b/perception/compare_map_segmentation/src/voxel_based_approximate_compare_map_filter_nodelet.cpp @@ -22,7 +22,49 @@ namespace compare_map_segmentation { -using pointcloud_preprocessor::get_param; + +bool VoxelBasedApproximateStaticMapLoader::is_close_to_map( + const pcl::PointXYZ & point, [[maybe_unused]] const double distance_threshold) +{ + if (voxel_map_ptr_ == NULL) { + return false; + } + const int index = + voxel_grid_.getCentroidIndexAt(voxel_grid_.getGridCoordinates(point.x, point.y, point.z)); + if (index == -1) { + return false; + } else { + return true; + } +} + +bool VoxelBasedApproximateDynamicMapLoader::is_close_to_map( + const pcl::PointXYZ & point, [[maybe_unused]] const double distance_threshold) +{ + if (current_voxel_grid_dict_.size() == 0) { + return false; + } + + const int map_grid_index = static_cast( + std::floor((point.x - origin_x_) / map_grid_size_x_) + + map_grids_x_ * std::floor((point.y - origin_y_) / map_grid_size_y_)); + + if (static_cast(map_grid_index) >= current_voxel_grid_array_.size()) { + return false; + } + if (current_voxel_grid_array_.at(map_grid_index) != NULL) { + const int index = current_voxel_grid_array_.at(map_grid_index) + ->map_cell_voxel_grid.getCentroidIndexAt( + current_voxel_grid_array_.at(map_grid_index) + ->map_cell_voxel_grid.getGridCoordinates(point.x, point.y, point.z)); + if (index == -1) { + return false; + } else { + return true; + } + } + return false; +} VoxelBasedApproximateCompareMapFilterComponent::VoxelBasedApproximateCompareMapFilterComponent( const rclcpp::NodeOptions & options) @@ -39,17 +81,17 @@ VoxelBasedApproximateCompareMapFilterComponent::VoxelBasedApproximateCompareMapF stop_watch_ptr_->tic("processing_time"); } - distance_threshold_ = static_cast(declare_parameter("distance_threshold", 0.3)); - - set_map_in_voxel_grid_ = false; - - using std::placeholders::_1; - sub_map_ = this->create_subscription( - "map", rclcpp::QoS{1}.transient_local(), - std::bind(&VoxelBasedApproximateCompareMapFilterComponent::input_target_callback, this, _1)); - - set_param_res_ = this->add_on_set_parameters_callback( - std::bind(&VoxelBasedApproximateCompareMapFilterComponent::paramCallback, this, _1)); + distance_threshold_ = declare_parameter("distance_threshold"); + bool use_dynamic_map_loading = declare_parameter("use_dynamic_map_loading"); + if (use_dynamic_map_loading) { + rclcpp::CallbackGroup::SharedPtr main_callback_group; + main_callback_group = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + voxel_based_approximate_map_loader_ = std::make_unique( + this, distance_threshold_, &tf_input_frame_, &mutex_, main_callback_group); + } else { + voxel_based_approximate_map_loader_ = std::make_unique( + this, distance_threshold_, &tf_input_frame_, &mutex_); + } } void VoxelBasedApproximateCompareMapFilterComponent::filter( @@ -57,43 +99,21 @@ void VoxelBasedApproximateCompareMapFilterComponent::filter( PointCloud2 & output) { std::scoped_lock lock(mutex_); - if (voxel_map_ptr_ == NULL) { - output = *input; - return; - } + stop_watch_ptr_->toc("processing_time", true); pcl::PointCloud::Ptr pcl_input(new pcl::PointCloud); pcl::PointCloud::Ptr pcl_output(new pcl::PointCloud); pcl::fromROSMsg(*input, *pcl_input); pcl_output->points.reserve(pcl_input->points.size()); for (size_t i = 0; i < pcl_input->points.size(); ++i) { - const int index = voxel_grid_.getCentroidIndexAt(voxel_grid_.getGridCoordinates( - pcl_input->points.at(i).x, pcl_input->points.at(i).y, pcl_input->points.at(i).z)); - if (index == -1) { // empty voxel - // map_ptr_->points.at(index) - pcl_output->points.push_back(pcl_input->points.at(i)); + if (voxel_based_approximate_map_loader_->is_close_to_map( + pcl_input->points.at(i), distance_threshold_)) { + continue; } + pcl_output->points.push_back(pcl_input->points.at(i)); } - pcl::toROSMsg(*pcl_output, output); output.header = input->header; -} - -void VoxelBasedApproximateCompareMapFilterComponent::input_target_callback( - const PointCloud2ConstPtr map) -{ - stop_watch_ptr_->toc("processing_time", true); - pcl::PointCloud map_pcl; - pcl::fromROSMsg(*map, map_pcl); - const auto map_pcl_ptr = pcl::make_shared>(map_pcl); - std::scoped_lock lock(mutex_); - set_map_in_voxel_grid_ = true; - tf_input_frame_ = map_pcl_ptr->header.frame_id; - voxel_map_ptr_.reset(new pcl::PointCloud); - voxel_grid_.setLeafSize(distance_threshold_, distance_threshold_, distance_threshold_); - voxel_grid_.setInputCloud(map_pcl_ptr); - voxel_grid_.setSaveLeafLayout(true); - voxel_grid_.filter(*voxel_map_ptr_); // add processing time for debug if (debug_publisher_) { const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); @@ -105,27 +125,6 @@ void VoxelBasedApproximateCompareMapFilterComponent::input_target_callback( } } -rcl_interfaces::msg::SetParametersResult -VoxelBasedApproximateCompareMapFilterComponent::paramCallback( - const std::vector & p) -{ - std::scoped_lock lock(mutex_); - - if (get_param(p, "distance_threshold", distance_threshold_)) { - voxel_grid_.setLeafSize(distance_threshold_, distance_threshold_, distance_threshold_); - voxel_grid_.setSaveLeafLayout(true); - if (set_map_in_voxel_grid_) { - voxel_grid_.filter(*voxel_map_ptr_); - } - RCLCPP_DEBUG(get_logger(), "Setting new distance threshold to: %f.", distance_threshold_); - } - - rcl_interfaces::msg::SetParametersResult result; - result.successful = true; - result.reason = "success"; - - return result; -} } // namespace compare_map_segmentation #include diff --git a/perception/compare_map_segmentation/src/voxel_distance_based_compare_map_filter_nodelet.cpp b/perception/compare_map_segmentation/src/voxel_distance_based_compare_map_filter_nodelet.cpp index 39330a9641143..5a6ebc401c3d3 100644 --- a/perception/compare_map_segmentation/src/voxel_distance_based_compare_map_filter_nodelet.cpp +++ b/perception/compare_map_segmentation/src/voxel_distance_based_compare_map_filter_nodelet.cpp @@ -22,71 +22,26 @@ namespace compare_map_segmentation { -using pointcloud_preprocessor::get_param; -VoxelDistanceBasedCompareMapFilterComponent::VoxelDistanceBasedCompareMapFilterComponent( - const rclcpp::NodeOptions & options) -: Filter("VoxelDistanceBasedCompareMapFilter", options) -{ - distance_threshold_ = static_cast(declare_parameter("distance_threshold", 0.3)); - - using std::placeholders::_1; - sub_map_ = this->create_subscription( - "map", rclcpp::QoS{1}.transient_local(), - std::bind(&VoxelDistanceBasedCompareMapFilterComponent::input_target_callback, this, _1)); - - set_param_res_ = this->add_on_set_parameters_callback( - std::bind(&VoxelDistanceBasedCompareMapFilterComponent::paramCallback, this, _1)); -} - -void VoxelDistanceBasedCompareMapFilterComponent::filter( - const PointCloud2ConstPtr & input, [[maybe_unused]] const IndicesPtr & indices, - PointCloud2 & output) -{ - std::scoped_lock lock(mutex_); - if (voxel_map_ptr_ == NULL || map_ptr_ == NULL || tree_ == NULL) { - output = *input; - return; - } - pcl::PointCloud::Ptr pcl_input(new pcl::PointCloud); - pcl::PointCloud::Ptr pcl_output(new pcl::PointCloud); - pcl::fromROSMsg(*input, *pcl_input); - pcl_output->points.reserve(pcl_input->points.size()); - for (size_t i = 0; i < pcl_input->points.size(); ++i) { - const int index = voxel_grid_.getCentroidIndexAt(voxel_grid_.getGridCoordinates( - pcl_input->points.at(i).x, pcl_input->points.at(i).y, pcl_input->points.at(i).z)); - if (index == -1) { // empty voxel - std::vector nn_indices(1); // nn means nearest neighbor - std::vector nn_distances(1); - if ( - tree_->radiusSearch( - pcl_input->points.at(i), distance_threshold_, nn_indices, nn_distances, 1) == 0) { - pcl_output->points.push_back(pcl_input->points.at(i)); - } - } - } - - pcl::toROSMsg(*pcl_output, output); - output.header = input->header; -} - -void VoxelDistanceBasedCompareMapFilterComponent::input_target_callback( - const PointCloud2ConstPtr map) +void VoxelDistanceBasedStaticMapLoader::onMapCallback( + const sensor_msgs::msg::PointCloud2::ConstSharedPtr map) { pcl::PointCloud map_pcl; pcl::fromROSMsg(*map, map_pcl); const auto map_pcl_ptr = pcl::make_shared>(map_pcl); - std::scoped_lock lock(mutex_); - tf_input_frame_ = map_pcl_ptr->header.frame_id; + (*mutex_ptr_).lock(); + map_ptr_ = map_pcl_ptr; + *tf_map_input_frame_ = map_ptr_->header.frame_id; // voxel voxel_map_ptr_.reset(new pcl::PointCloud); - voxel_grid_.setLeafSize(distance_threshold_, distance_threshold_, distance_threshold_); + voxel_grid_.setLeafSize(voxel_leaf_size_, voxel_leaf_size_, voxel_leaf_size_); voxel_grid_.setInputCloud(map_pcl_ptr); voxel_grid_.setSaveLeafLayout(true); voxel_grid_.filter(*voxel_map_ptr_); // kdtree map_ptr_ = map_pcl_ptr; + if (!tree_) { if (map_ptr_->isOrganized()) { tree_.reset(new pcl::search::OrganizedNeighbor()); @@ -95,27 +50,109 @@ void VoxelDistanceBasedCompareMapFilterComponent::input_target_callback( } } tree_->setInputCloud(map_ptr_); + (*mutex_ptr_).unlock(); } -rcl_interfaces::msg::SetParametersResult VoxelDistanceBasedCompareMapFilterComponent::paramCallback( - const std::vector & p) +bool VoxelDistanceBasedStaticMapLoader::is_close_to_map( + const pcl::PointXYZ & point, const double distance_threshold) { - std::scoped_lock lock(mutex_); + if (voxel_map_ptr_ == NULL) { + return false; + } + if (map_ptr_ == NULL) { + return false; + } + if (tree_ == NULL) { + return false; + } + if (is_close_to_neighbor_voxels(point, distance_threshold, voxel_grid_, tree_)) { + return true; + } + return false; +} + +bool VoxelDistanceBasedDynamicMapLoader::is_close_to_map( + const pcl::PointXYZ & point, const double distance_threshold) +{ + if (current_voxel_grid_dict_.size() == 0) { + return false; + } + + const int map_grid_index = static_cast( + std::floor((point.x - origin_x_) / map_grid_size_x_) + + map_grids_x_ * std::floor((point.y - origin_y_) / map_grid_size_y_)); + + if (static_cast(map_grid_index) >= current_voxel_grid_array_.size()) { + return false; + } + if ( + current_voxel_grid_array_.at(map_grid_index) != NULL && + is_close_to_neighbor_voxels( + point, distance_threshold, current_voxel_grid_array_.at(map_grid_index)->map_cell_voxel_grid, + current_voxel_grid_array_.at(map_grid_index)->map_cell_kdtree)) { + return true; + } + return false; +} + +VoxelDistanceBasedCompareMapFilterComponent::VoxelDistanceBasedCompareMapFilterComponent( + const rclcpp::NodeOptions & options) +: Filter("VoxelDistanceBasedCompareMapFilter", options) +{ + // initialize debug tool + { + using tier4_autoware_utils::DebugPublisher; + using tier4_autoware_utils::StopWatch; + stop_watch_ptr_ = std::make_unique>(); + debug_publisher_ = + std::make_unique(this, "voxel_distance_based_compare_map_filter"); + stop_watch_ptr_->tic("cyclic_time"); + stop_watch_ptr_->tic("processing_time"); + } - if (get_param(p, "distance_threshold", distance_threshold_)) { - voxel_grid_.setLeafSize(distance_threshold_, distance_threshold_, distance_threshold_); - voxel_grid_.setSaveLeafLayout(true); - if (set_map_in_voxel_grid_) { - voxel_grid_.filter(*voxel_map_ptr_); + distance_threshold_ = declare_parameter("distance_threshold"); + bool use_dynamic_map_loading = declare_parameter("use_dynamic_map_loading"); + if (use_dynamic_map_loading) { + rclcpp::CallbackGroup::SharedPtr main_callback_group; + main_callback_group = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + voxel_distance_based_map_loader_ = std::make_unique( + this, distance_threshold_, &tf_input_frame_, &mutex_, main_callback_group); + } else { + voxel_distance_based_map_loader_ = std::make_unique( + this, distance_threshold_, &tf_input_frame_, &mutex_); + } +} + +void VoxelDistanceBasedCompareMapFilterComponent::filter( + const PointCloud2ConstPtr & input, [[maybe_unused]] const IndicesPtr & indices, + PointCloud2 & output) +{ + std::scoped_lock lock(mutex_); + stop_watch_ptr_->toc("processing_time", true); + pcl::PointCloud::Ptr pcl_input(new pcl::PointCloud); + pcl::PointCloud::Ptr pcl_output(new pcl::PointCloud); + pcl::fromROSMsg(*input, *pcl_input); + pcl_output->points.reserve(pcl_input->points.size()); + for (size_t i = 0; i < pcl_input->points.size(); ++i) { + if (voxel_distance_based_map_loader_->is_close_to_map( + pcl_input->points.at(i), distance_threshold_)) { + continue; } - RCLCPP_DEBUG(get_logger(), "Setting new distance threshold to: %f.", distance_threshold_); + pcl_output->points.push_back(pcl_input->points.at(i)); } - rcl_interfaces::msg::SetParametersResult result; - result.successful = true; - result.reason = "success"; + pcl::toROSMsg(*pcl_output, output); + output.header = input->header; - return result; + // add processing time for debug + if (debug_publisher_) { + const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); + const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); + debug_publisher_->publish( + "debug/cyclic_time_ms", cyclic_time_ms); + debug_publisher_->publish( + "debug/processing_time_ms", processing_time_ms); + } } } // namespace compare_map_segmentation diff --git a/perception/compare_map_segmentation/src/voxel_grid_map_loader.cpp b/perception/compare_map_segmentation/src/voxel_grid_map_loader.cpp index 029f9cdb435dc..dc9f965e0696a 100644 --- a/perception/compare_map_segmentation/src/voxel_grid_map_loader.cpp +++ b/perception/compare_map_segmentation/src/voxel_grid_map_loader.cpp @@ -45,6 +45,25 @@ void VoxelGridMapLoader::publish_downsampled_map( downsampled_map_pub_->publish(downsampled_map_msg); } +bool VoxelGridMapLoader::is_close_to_neighbor_voxels( + const pcl::PointXYZ & point, const double distance_threshold, VoxelGridPointXYZ & voxel, + pcl::search::Search::Ptr tree) const +{ + const int index = voxel.getCentroidIndexAt(voxel.getGridCoordinates(point.x, point.y, point.z)); + if (index != -1) { + return true; + } + if (tree == NULL) { + return false; + } + std::vector nn_indices(1); + std::vector nn_distances(1); + if (tree->radiusSearch(point, distance_threshold, nn_indices, nn_distances, 1) == 0) { + return false; + } + return true; +} + bool VoxelGridMapLoader::is_close_to_neighbor_voxels( const pcl::PointXYZ & point, const double distance_threshold, const PointCloudPtr & map, VoxelGridPointXYZ & voxel) const @@ -219,8 +238,6 @@ bool VoxelGridMapLoader::is_in_voxel( return false; } -//*************************** for Static Map loader Voxel Grid Filter ************* - VoxelGridStaticMapLoader::VoxelGridStaticMapLoader( rclcpp::Node * node, double leaf_size, std::string * tf_map_input_frame, std::mutex * mutex) : VoxelGridMapLoader(node, leaf_size, tf_map_input_frame, mutex) @@ -258,7 +275,6 @@ bool VoxelGridStaticMapLoader::is_close_to_map( } return false; } -//*************** for Dynamic and Differential Map loader Voxel Grid Filter ************* VoxelGridDynamicMapLoader::VoxelGridDynamicMapLoader( rclcpp::Node * node, double leaf_size, std::string * tf_map_input_frame, std::mutex * mutex, From de4b01b9ba9ea7fae81301e962ccdc19b90aabfd Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Mon, 24 Apr 2023 18:33:53 +0900 Subject: [PATCH 10/16] feat(intersection): send intersection/intersection_occlusion rtc status(fix #3511) (#3512) * Merge feat/intersection-occlusion-latest (#8) Signed-off-by: Mamoru Sobue * fixed virtual wall marker Signed-off-by: Mamoru Sobue * use is_occluded flag Signed-off-by: Mamoru Sobue * 3 rtc intersection appears Signed-off-by: Mamoru Sobue * clearCooperateStatus before sendRTC not to duplicate default/occlusion_first_stop_uuid Signed-off-by: Mamoru Sobue * clearCooperateStatus at the beginning of send RTC Signed-off-by: Mamoru Sobue --------- Signed-off-by: Mamoru Sobue --- .../src/scene_module/intersection/manager.cpp | 13 +++---------- 1 file changed, 3 insertions(+), 10 deletions(-) diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp index 66acc4d92e013..e2058b3f393e9 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp @@ -104,7 +104,6 @@ void IntersectionModuleManager::launchNewModules( const auto lanelets = planning_utils::getLaneletsOnPath(path, lanelet_map, planner_data_->current_odometry->pose); // run occlusion detection only in the first intersection - bool enable_occlusion_detection = intersection_param_.occlusion.enable; for (size_t i = 0; i < lanelets.size(); i++) { const auto ll = lanelets.at(i); const auto lane_id = ll.id(); @@ -125,12 +124,10 @@ void IntersectionModuleManager::launchNewModules( const auto assoc_ids = planning_utils::getAssociativeIntersectionLanelets(ll, lanelet_map, routing_graph); const auto new_module = std::make_shared( - module_id, lane_id, planner_data_, intersection_param_, assoc_ids, enable_occlusion_detection, - node_, logger_.get_child("intersection_module"), clock_); + module_id, lane_id, planner_data_, intersection_param_, assoc_ids, true, node_, + logger_.get_child("intersection_module"), clock_); registerModule(std::move(new_module)); generateUUID(module_id); - - enable_occlusion_detection = false; } } @@ -175,6 +172,7 @@ bool IntersectionModuleManager::hasSameParentLaneletAndTurnDirectionWithRegister void IntersectionModuleManager::sendRTC(const Time & stamp) { + rtc_interface_.clearCooperateStatus(); for (const auto & scene_module : scene_modules_) { const auto intersection_module = std::dynamic_pointer_cast(scene_module); const bool is_occluded = intersection_module->isOccluded(); @@ -182,15 +180,12 @@ void IntersectionModuleManager::sendRTC(const Time & stamp) const auto occlusion_uuid = intersection_module->getOcclusionUUID(); const auto occlusion_first_stop_uuid = intersection_module->getOcclusionFirstStopUUID(); if (!is_occluded) { - rtc_interface_.clearCooperateStatus(); // default updateRTCStatus(uuid, scene_module->isSafe(), scene_module->getDistance(), stamp); occlusion_rtc_interface_.updateCooperateStatus( occlusion_uuid, true, std::numeric_limits::lowest(), std::numeric_limits::lowest(), stamp); - // send {default, occlusion} RTC status } else { - rtc_interface_.clearCooperateStatus(); // occlusion const auto occlusion_safety = intersection_module->getOcclusionSafety(); const auto occlusion_distance = intersection_module->getOcclusionDistance(); @@ -203,8 +198,6 @@ void IntersectionModuleManager::sendRTC(const Time & stamp) rtc_interface_.updateCooperateStatus( occlusion_first_stop_uuid, occlusion_first_stop_safety, occlusion_first_stop_distance, occlusion_first_stop_distance, stamp); - // send {first_stop, occlusion} RTC status - // rtc_interface_.removeCooperateStatus(uuid); } } rtc_interface_.publishCooperateStatus(stamp); // publishRTCStatus() From 784919d001c8a3e6b318557df8ffd3fff6baa97d Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Mon, 24 Apr 2023 18:47:57 +0900 Subject: [PATCH 11/16] feat(avoidance): separate debug marker namespace for each ignore reason (#3507) Signed-off-by: satoshi-ota --- .../marker_util/avoidance/debug.hpp | 2 +- .../src/marker_util/avoidance/debug.cpp | 27 ++++++++++++++----- .../avoidance/avoidance_module.cpp | 15 ++++++++++- 3 files changed, 36 insertions(+), 8 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/marker_util/avoidance/debug.hpp b/planning/behavior_path_planner/include/behavior_path_planner/marker_util/avoidance/debug.hpp index 51de1d2d6318d..9b676ecafa2c3 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/marker_util/avoidance/debug.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/marker_util/avoidance/debug.hpp @@ -64,7 +64,7 @@ MarkerArray createAvoidableTargetObjectsMarkerArray( MarkerArray createUnavoidableTargetObjectsMarkerArray( const ObjectDataArray & objects, std::string && ns); -MarkerArray createOtherObjectsMarkerArray(const ObjectDataArray & objects, std::string && ns); +MarkerArray createOtherObjectsMarkerArray(const ObjectDataArray & objects, const std::string & ns); MarkerArray createUnsafeObjectsMarkerArray(const ObjectDataArray & objects, std::string && ns); diff --git a/planning/behavior_path_planner/src/marker_util/avoidance/debug.cpp b/planning/behavior_path_planner/src/marker_util/avoidance/debug.cpp index 7b5309c1d622a..cbc6a383899b4 100644 --- a/planning/behavior_path_planner/src/marker_util/avoidance/debug.cpp +++ b/planning/behavior_path_planner/src/marker_util/avoidance/debug.cpp @@ -412,18 +412,33 @@ MarkerArray createUnavoidableTargetObjectsMarkerArray( } MarkerArray createOtherObjectsMarkerArray( - const behavior_path_planner::ObjectDataArray & objects, std::string && ns) + const behavior_path_planner::ObjectDataArray & objects, const std::string & ns) { + using behavior_path_planner::utils::convertToSnakeCase; + + const auto filtered_objects = [&objects, &ns]() { + ObjectDataArray ret{}; + for (const auto & o : objects) { + if (o.reason != ns) { + continue; + } + ret.push_back(o); + } + + return ret; + }(); + MarkerArray msg; - msg.markers.reserve(objects.size() * 2); + msg.markers.reserve(filtered_objects.size() * 2); appendMarkerArray( createObjectsCubeMarkerArray( - objects, ns + "_cube", createMarkerScale(3.0, 1.5, 1.5), - createMarkerColor(0.0, 1.0, 0.0, 0.8)), + filtered_objects, "others_" + convertToSnakeCase(ns) + "_cube", + createMarkerScale(3.0, 1.5, 1.5), createMarkerColor(0.0, 1.0, 0.0, 0.8)), + &msg); + appendMarkerArray( + createObjectInfoMarkerArray(filtered_objects, "others_" + convertToSnakeCase(ns) + "_info"), &msg); - - appendMarkerArray(createObjectInfoMarkerArray(objects, ns + "_info"), &msg); return msg; } 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 9ac37adf1dc43..23628721590e8 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 @@ -3310,7 +3310,20 @@ void AvoidanceModule::setDebugData( createAvoidableTargetObjectsMarkerArray(avoidable_target_objects, "avoidable_target_objects")); add(createUnavoidableTargetObjectsMarkerArray( unavoidable_target_objects, "unavoidable_target_objects")); - add(createOtherObjectsMarkerArray(data.other_objects, "other_objects")); + + add(createOtherObjectsMarkerArray( + data.other_objects, AvoidanceDebugFactor::OBJECT_IS_BEHIND_THRESHOLD)); + add(createOtherObjectsMarkerArray( + data.other_objects, AvoidanceDebugFactor::OBJECT_IS_IN_FRONT_THRESHOLD)); + add(createOtherObjectsMarkerArray( + data.other_objects, AvoidanceDebugFactor::OBJECT_BEHIND_PATH_GOAL)); + add(createOtherObjectsMarkerArray( + data.other_objects, AvoidanceDebugFactor::TOO_NEAR_TO_CENTERLINE)); + add(createOtherObjectsMarkerArray(data.other_objects, AvoidanceDebugFactor::OBJECT_IS_NOT_TYPE)); + add(createOtherObjectsMarkerArray(data.other_objects, AvoidanceDebugFactor::NOT_PARKING_OBJECT)); + add(createOtherObjectsMarkerArray(data.other_objects, std::string("MovingObject"))); + add(createOtherObjectsMarkerArray(data.other_objects, std::string("OutOfTargetArea"))); + add(makeOverhangToRoadShoulderMarkerArray(data.target_objects, "overhang")); add(createOverhangFurthestLineStringMarkerArray( debug.bounds, "farthest_linestring_from_overhang", 1.0, 0.0, 1.0)); From 1c15bcfe2a05bf57ef46fc8012a389146e6a00fc Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Mon, 24 Apr 2023 18:48:36 +0900 Subject: [PATCH 12/16] fix(avoidance): use infinity to show that next traffic light doesn't exist explicitly (#3506) Signed-off-by: satoshi-ota --- .../utils/avoidance/avoidance_module_data.hpp | 2 +- .../behavior_path_planner/src/marker_util/avoidance/debug.cpp | 3 +-- planning/behavior_path_planner/src/utils/avoidance/util.cpp | 2 +- planning/behavior_path_planner/src/utils/utils.cpp | 4 ++-- 4 files changed, 5 insertions(+), 6 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp index f0f5067611e0d..9e8d81afe12b7 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp @@ -308,7 +308,7 @@ struct ObjectData // avoidance target double to_road_shoulder_distance{0.0}; // to intersection - double to_stop_factor_distance{std::numeric_limits::max()}; + double to_stop_factor_distance{std::numeric_limits::infinity()}; // if lateral margin is NOT enough, the ego must avoid the object. bool avoid_required{false}; diff --git a/planning/behavior_path_planner/src/marker_util/avoidance/debug.cpp b/planning/behavior_path_planner/src/marker_util/avoidance/debug.cpp index cbc6a383899b4..336c1c8d0a051 100644 --- a/planning/behavior_path_planner/src/marker_util/avoidance/debug.cpp +++ b/planning/behavior_path_planner/src/marker_util/avoidance/debug.cpp @@ -78,14 +78,13 @@ MarkerArray createObjectInfoMarkerArray(const ObjectDataArray & objects, std::st for (const auto & object : objects) { { - const auto to_stop_factor_distance = std::min(object.to_stop_factor_distance, 1000.0); marker.id = uuidToInt32(object.object.object_id); marker.pose = object.object.kinematics.initial_pose_with_covariance.pose; std::ostringstream string_stream; string_stream << std::fixed << std::setprecision(2); string_stream << "ratio:" << object.shiftable_ratio << " [-]\n" << "lateral: " << object.lateral << " [-]\n" - << "stop_factor:" << to_stop_factor_distance << " [m]\n" + << "stop_factor:" << object.to_stop_factor_distance << " [m]\n" << "move_time:" << object.move_time << " [s]\n" << "stop_time:" << object.stop_time << " [s]\n"; marker.text = string_stream.str(); diff --git a/planning/behavior_path_planner/src/utils/avoidance/util.cpp b/planning/behavior_path_planner/src/utils/avoidance/util.cpp index f9506b9d75955..566eda2643de1 100644 --- a/planning/behavior_path_planner/src/utils/avoidance/util.cpp +++ b/planning/behavior_path_planner/src/utils/avoidance/util.cpp @@ -819,7 +819,7 @@ void fillObjectMovingTime( } if (is_new_object) { - object_data.move_time = std::numeric_limits::max(); + object_data.move_time = std::numeric_limits::infinity(); object_data.stop_time = 0.0; object_data.last_move = now; return; diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index 60b09bba0b117..4aacafeb2dab3 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -1118,7 +1118,7 @@ double getDistanceToNextTrafficLight( { lanelet::ConstLanelet current_lanelet; if (!lanelet::utils::query::getClosestLanelet(lanelets, current_pose, ¤t_lanelet)) { - return std::numeric_limits::max(); + return std::numeric_limits::infinity(); } const auto lanelet_point = lanelet::utils::conversion::toLaneletPoint(current_pose.position); @@ -1166,7 +1166,7 @@ double getDistanceToNextTrafficLight( distance += lanelet::utils::getLaneletLength3d(llt); } - return std::numeric_limits::max(); + return std::numeric_limits::infinity(); } double getDistanceToNextIntersection( From 345315eaab3340d2b25992cbb49f15a338c6ee49 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" <43976882+isamu-takagi@users.noreply.github.com> Date: Mon, 24 Apr 2023 19:56:01 +0900 Subject: [PATCH 13/16] chore: update codeowners (#3513) Signed-off-by: Takagi, Isamu --- common/autoware_ad_api_specs/package.xml | 2 +- common/component_interface_utils/package.xml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/common/autoware_ad_api_specs/package.xml b/common/autoware_ad_api_specs/package.xml index 0659a3c923923..ca3d998617d9d 100644 --- a/common/autoware_ad_api_specs/package.xml +++ b/common/autoware_ad_api_specs/package.xml @@ -7,7 +7,7 @@ Takagi, Isamu yabuta Kah Hooi Tan - Kenji Miyake + Ryohsuke Mitsudome Apache License 2.0 ament_cmake_auto diff --git a/common/component_interface_utils/package.xml b/common/component_interface_utils/package.xml index 90602a65bd1b0..b1f49fe40f407 100644 --- a/common/component_interface_utils/package.xml +++ b/common/component_interface_utils/package.xml @@ -7,7 +7,7 @@ Takagi, Isamu yabuta Kah Hooi Tan - Kenji Miyake + Yukihiro Saito Apache License 2.0 ament_cmake_auto From 21bd777bd858809f8a275f1126543ff98870fae4 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Mon, 24 Apr 2023 21:23:21 +0900 Subject: [PATCH 14/16] chore(intersection): add visualization dependency (#3509) * chore(intersection): add visualization dependency Signed-off-by: Mamoru Sobue * style(pre-commit): autofix --------- Signed-off-by: Mamoru Sobue Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> From d93b622da3f8ce75dd62cae6c053b6b26746b49f Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Mon, 24 Apr 2023 22:11:38 +0900 Subject: [PATCH 15/16] feat(intersection): fixed virtual wall visualization (#3514) * fixed virtual wall visualization Signed-off-by: Mamoru Sobue * fixed occlusion.enable param Signed-off-by: Mamoru Sobue * reflected comments Signed-off-by: Mamoru Sobue --------- Signed-off-by: Mamoru Sobue --- .../src/scene_module/intersection/debug.cpp | 8 ++++++++ .../src/scene_module/intersection/manager.cpp | 5 +++-- 2 files changed, 11 insertions(+), 2 deletions(-) diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp index 5eb57f06ec372..a28b15493be56 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp @@ -222,6 +222,14 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createVirtualWallMarker {debug_data_.occlusion_stop_wall_pose}, "intersection_occlusion", now), &wall_marker, now); } + + auto id = 0; + for (auto & marker : wall_marker.markers) { + if (marker.action == visualization_msgs::msg::Marker::ADD) { + marker.id = id; + id++; + } + } return wall_marker; } diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp index e2058b3f393e9..6fdeca5614058 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp @@ -104,6 +104,7 @@ void IntersectionModuleManager::launchNewModules( const auto lanelets = planning_utils::getLaneletsOnPath(path, lanelet_map, planner_data_->current_odometry->pose); // run occlusion detection only in the first intersection + const bool enable_occlusion_detection = intersection_param_.occlusion.enable; for (size_t i = 0; i < lanelets.size(); i++) { const auto ll = lanelets.at(i); const auto lane_id = ll.id(); @@ -124,8 +125,8 @@ void IntersectionModuleManager::launchNewModules( const auto assoc_ids = planning_utils::getAssociativeIntersectionLanelets(ll, lanelet_map, routing_graph); const auto new_module = std::make_shared( - module_id, lane_id, planner_data_, intersection_param_, assoc_ids, true, node_, - logger_.get_child("intersection_module"), clock_); + module_id, lane_id, planner_data_, intersection_param_, assoc_ids, enable_occlusion_detection, + node_, logger_.get_child("intersection_module"), clock_); registerModule(std::move(new_module)); generateUUID(module_id); } From f2b7687ab9bc9ad0ffe9806b827c7c81bacc69b8 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Tue, 25 Apr 2023 01:07:29 +0900 Subject: [PATCH 16/16] feat(pull_over): minor change with drivable area expansion (#3515) * feat(pull_over): can use different offsets for right left drivable area expansion Signed-off-by: Takayuki Murooka * fix Signed-off-by: Takayuki Murooka * fix Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka --- .../behavior_path_planner/utils/utils.hpp | 4 ++-- .../utils/pull_over/freespace_pull_over.cpp | 5 ++-- .../behavior_path_planner/src/utils/utils.cpp | 24 +++++++++---------- 3 files changed, 17 insertions(+), 16 deletions(-) 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 3bcd01d058dba..d625613259d56 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 @@ -216,8 +216,8 @@ void generateDrivableArea( const std::shared_ptr planner_data, const bool is_driving_forward = true); void generateDrivableArea( - PathWithLaneId & path, const double vehicle_length, const double vehicle_width, - const double margin, const bool is_driving_forward = true); + PathWithLaneId & path, const double vehicle_length, const double offset, + const bool is_driving_forward = true); lanelet::ConstLineStrings3d getMaximumDrivableArea( const std::shared_ptr & planner_data); diff --git a/planning/behavior_path_planner/src/utils/pull_over/freespace_pull_over.cpp b/planning/behavior_path_planner/src/utils/pull_over/freespace_pull_over.cpp index 8bb450f60e5fb..783398bca524a 100644 --- a/planning/behavior_path_planner/src/utils/pull_over/freespace_pull_over.cpp +++ b/planning/behavior_path_planner/src/utils/pull_over/freespace_pull_over.cpp @@ -109,9 +109,10 @@ boost::optional FreespacePullOver::plan(const Pose & goal_pose) // path points is less than 2 return {}; } + + const double offset = planner_data_->parameters.vehicle_width / 2.0 + drivable_area_margin; utils::generateDrivableArea( - path, planner_data_->parameters.vehicle_length, planner_data_->parameters.vehicle_width, - drivable_area_margin, *is_driving_forward); + path, planner_data_->parameters.vehicle_length, offset, *is_driving_forward); } PullOverPath pull_over_path{}; diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index 4aacafeb2dab3..acce7ffa1c4c3 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -971,8 +971,8 @@ void generateDrivableArea( // generate drivable area by expanding path for freespace void generateDrivableArea( - PathWithLaneId & path, const double vehicle_length, const double vehicle_width, - const double margin, const bool is_driving_forward) + PathWithLaneId & path, const double vehicle_length, const double offset, + const bool is_driving_forward) { using tier4_autoware_utils::calcOffsetPose; @@ -1007,8 +1007,8 @@ void generateDrivableArea( for (const auto & point : resampled_path.points) { const auto & pose = point.point.pose; - const auto left_point = calcOffsetPose(pose, 0, vehicle_width / 2.0 + margin, 0); - const auto right_point = calcOffsetPose(pose, 0, -vehicle_width / 2.0 - margin, 0); + const auto left_point = calcOffsetPose(pose, 0, offset, 0); + const auto right_point = calcOffsetPose(pose, 0, -offset, 0); left_bound.push_back(left_point.position); right_bound.push_back(right_point.position); @@ -1018,32 +1018,32 @@ void generateDrivableArea( // add backward offset point to bound const Pose first_point = calcOffsetPose(resampled_path.points.front().point.pose, -vehicle_length, 0, 0); - const Pose left_first_point = calcOffsetPose(first_point, 0, vehicle_width / 2.0 + margin, 0); - const Pose right_first_point = calcOffsetPose(first_point, 0, -vehicle_width / 2.0 - margin, 0); + const Pose left_first_point = calcOffsetPose(first_point, 0, offset, 0); + const Pose right_first_point = calcOffsetPose(first_point, 0, -offset, 0); left_bound.insert(left_bound.begin(), left_first_point.position); right_bound.insert(right_bound.begin(), right_first_point.position); // add forward offset point to bound const Pose last_point = calcOffsetPose(resampled_path.points.back().point.pose, vehicle_length, 0, 0); - const Pose left_last_point = calcOffsetPose(last_point, 0, vehicle_width / 2.0 + margin, 0); - const Pose right_last_point = calcOffsetPose(last_point, 0, -vehicle_width / 2.0 - margin, 0); + const Pose left_last_point = calcOffsetPose(last_point, 0, offset, 0); + const Pose right_last_point = calcOffsetPose(last_point, 0, -offset, 0); left_bound.push_back(left_last_point.position); right_bound.push_back(right_last_point.position); } else { // add forward offset point to bound const Pose first_point = calcOffsetPose(resampled_path.points.front().point.pose, vehicle_length, 0, 0); - const Pose left_first_point = calcOffsetPose(first_point, 0, vehicle_width / 2.0 + margin, 0); - const Pose right_first_point = calcOffsetPose(first_point, 0, -vehicle_width / 2.0 - margin, 0); + const Pose left_first_point = calcOffsetPose(first_point, 0, offset, 0); + const Pose right_first_point = calcOffsetPose(first_point, 0, -offset, 0); left_bound.insert(left_bound.begin(), left_first_point.position); right_bound.insert(right_bound.begin(), right_first_point.position); // add backward offset point to bound const Pose last_point = calcOffsetPose(resampled_path.points.back().point.pose, -vehicle_length, 0, 0); - const Pose left_last_point = calcOffsetPose(last_point, 0, vehicle_width / 2.0 + margin, 0); - const Pose right_last_point = calcOffsetPose(last_point, 0, -vehicle_width / 2.0 - margin, 0); + const Pose left_last_point = calcOffsetPose(last_point, 0, offset, 0); + const Pose right_last_point = calcOffsetPose(last_point, 0, -offset, 0); left_bound.push_back(left_last_point.position); right_bound.push_back(right_last_point.position); }