From c4ee2eb391739e24392058cc552b6470150d037b Mon Sep 17 00:00:00 2001 From: tanaka3 Date: Mon, 28 Mar 2022 14:10:08 +0900 Subject: [PATCH 1/6] refactor(behavior_velocity): refactor codes Signed-off-by: tanaka3 --- .../occlusion_spot/occlusion_spot_utils.hpp | 2 +- .../occlusion_spot/scene_occlusion_spot.hpp | 1 + .../src/scene_module/occlusion_spot/debug.cpp | 4 +- .../occlusion_spot/occlusion_spot_utils.cpp | 38 +++++++------------ .../risk_predictive_braking.cpp | 2 - .../occlusion_spot/scene_occlusion_spot.cpp | 6 +-- 6 files changed, 20 insertions(+), 33 deletions(-) diff --git a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp index 2effa9e9ce55a..10783cbe5414e 100644 --- a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp @@ -189,7 +189,6 @@ struct DebugData std::string detection_type = ""; Polygons2d detection_area_polygons; std::vector close_partition; - std::vector partition_lanelets; std::vector parked_vehicle_point; std::vector possible_collisions; std::vector occlusion_points; @@ -197,6 +196,7 @@ struct DebugData PathWithLaneId interp_path; void resetData() { + close_partition.clear(); detection_area_polygons.clear(); parked_vehicle_point.clear(); possible_collisions.clear(); diff --git a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot.hpp b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot.hpp index 60424f15e19d2..4eda27f559336 100644 --- a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot.hpp @@ -61,6 +61,7 @@ class OcclusionSpotModule : public SceneModuleInterface // Parameter PlannerParam param_; tier4_autoware_utils::StopWatch stop_watch_; + std::vector partition_lanelets_; protected: int64_t module_id_{}; diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/debug.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/debug.cpp index b688641a9bd0b..83d60cf3d350e 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/debug.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/debug.cpp @@ -295,9 +295,9 @@ visualization_msgs::msg::MarkerArray OcclusionSpotModule::createDebugMarkerArray debug_data_.detection_area_polygons, "detection_area", module_id_, debug_data_.z), current_time, &debug_marker_array); } - if (!debug_data_.partition_lanelets.empty()) { + if (!debug_data_.close_partition.empty()) { appendMarkerArray( - makePolygonMarker(debug_data_.close_partition, "partition", module_id_, debug_data_.z), + makePolygonMarker(debug_data_.close_partition, "close_partition", module_id_, debug_data_.z), current_time, &debug_marker_array); } if (!debug_data_.interp_path.points.empty()) { diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp index 8b471ae929861..c5a51f2d2c92a 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp @@ -275,7 +275,7 @@ PossibleCollisionInfo calculateCollisionPathPointFromOcclusionSpot( const ArcCoordinates & arc_coord_occlusion_with_offset, const lanelet::ConstLanelet & path_lanelet, const PlannerParam & param) { - auto setPose = [](const lanelet::ConstLanelet & pl, const ArcCoordinates & arc) { + auto calcPose = [](const lanelet::ConstLanelet & pl, const ArcCoordinates & arc) { const auto & ll = pl.centerline2d(); BasicPoint2d bp = fromArcCoordinates(ll, arc); Pose pose; @@ -306,11 +306,11 @@ PossibleCollisionInfo calculateCollisionPathPointFromOcclusionSpot( pc.arc_lane_dist_at_collision = {distance_to_stop, arc_coord_occlusion_with_offset.distance}; pc.obstacle_info.safe_motion = sm; pc.obstacle_info.ttc = ttc; - pc.obstacle_info.position = setPose(path_lanelet, arc_coord_occlusion).position; + pc.obstacle_info.position = calcPose(path_lanelet, arc_coord_occlusion).position; pc.obstacle_info.max_velocity = param.pedestrian_vel; - pc.collision_pose = setPose(path_lanelet, {arc_coord_occlusion_with_offset.length, 0.0}); - pc.collision_with_margin.pose = setPose(path_lanelet, {distance_to_stop, 0.0}); - pc.intersection_pose = setPose(path_lanelet, {arc_coord_occlusion.length, 0.0}); + pc.collision_pose = calcPose(path_lanelet, {arc_coord_occlusion_with_offset.length, 0.0}); + pc.collision_with_margin.pose = calcPose(path_lanelet, {distance_to_stop, 0.0}); + pc.intersection_pose = calcPose(path_lanelet, {arc_coord_occlusion.length, 0.0}); return pc; } @@ -339,12 +339,6 @@ bool generatePossibleCollisionBehindParkedVehicle( arc_coord_occlusion, arc_coord_occlusion_with_offset, path_lanelet, param); possible_collisions.emplace_back(pc); } - // sort by arc length - std::sort( - possible_collisions.begin(), possible_collisions.end(), - [](PossibleCollisionInfo pc1, PossibleCollisionInfo pc2) { - return pc1.arc_lane_dist_at_collision.length < pc2.arc_lane_dist_at_collision.length; - }); if (possible_collisions.empty()) return false; return true; } @@ -395,28 +389,22 @@ bool createPossibleCollisionsInDetectionArea( const auto pc = generateOneNotableCollisionFromOcclusionSpot( grid, occlusion_spot_positions, offset_from_start_to_ego, base_point, path_lanelet, param, debug_data); - if (!pc) continue; + if (pc == boost::none) continue; const double lateral_distance = std::abs(pc.get().arc_lane_dist_at_collision.distance); if (lateral_distance > distance_lower_bound) continue; distance_lower_bound = lateral_distance; possible_collisions.emplace_back(pc.get()); } - // sort by arc length - std::sort( - possible_collisions.begin(), possible_collisions.end(), - [](PossibleCollisionInfo pc1, PossibleCollisionInfo pc2) { - return pc1.arc_lane_dist_at_collision.length < pc2.arc_lane_dist_at_collision.length; - }); if (possible_collisions.empty()) return false; return true; } -bool isNotBlockedByPartition(const LineString2d & direction, const BasicPolygons2d & partitions) +bool isBlockedByPartition(const LineString2d & direction, const BasicPolygons2d & partitions) { for (const auto & p : partitions) { - if (bg::intersects(direction, p)) return false; + if (bg::intersects(direction, p)) return true; } - return true; + return false; } boost::optional generateOneNotableCollisionFromOcclusionSpot( @@ -453,14 +441,14 @@ boost::optional generateOneNotableCollisionFromOcclusionS const auto & ip = pc.intersection_pose.position; bool collision_free_at_intersection = grid_utils::isCollisionFree( grid, occlusion_spot_position, grid_map::Position(ip.x, ip.y), param.pedestrian_radius); - bool obstacle_not_blocked_by_partition = true; + bool is_obstacle_blocked_by_partition = false; if (!collision_free_at_intersection) continue; if (param.use_partition_lanelet) { const auto & op = obstacle_point; const LineString2d obstacle_vec = {{op[0], op[1]}, {ip.x, ip.y}}; - obstacle_not_blocked_by_partition = isNotBlockedByPartition(obstacle_vec, partition_lanelets); + is_obstacle_blocked_by_partition = isBlockedByPartition(obstacle_vec, partition_lanelets); } - if (!obstacle_not_blocked_by_partition) continue; + if (is_obstacle_blocked_by_partition) continue; distance_lower_bound = dist; candidate = pc; has_collision = true; @@ -468,7 +456,7 @@ boost::optional generateOneNotableCollisionFromOcclusionS if (has_collision) { return candidate; } else { - return {}; + return boost::none; } } diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/risk_predictive_braking.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/risk_predictive_braking.cpp index 5b642d80df942..709d2f75c5a90 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/risk_predictive_braking.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/risk_predictive_braking.cpp @@ -27,8 +27,6 @@ void applySafeVelocityConsideringPossibleCollision( PathWithLaneId * inout_path, std::vector & possible_collisions, const PlannerParam & param) { - const auto logger{rclcpp::get_logger("behavior_velocity_planner").get_child("occlusion_spot")}; - rclcpp::Clock clock{RCL_ROS_TIME}; // return nullptr or too few points if (!inout_path || inout_path->points.size() < 2) { return; diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp index f3f510445d7ed..79e541be5da3c 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp @@ -51,7 +51,7 @@ OcclusionSpotModule::OcclusionSpotModule( } if (param_.use_partition_lanelet) { const lanelet::LaneletMapConstPtr & ll = planner_data->route_handler_->getLaneletMapPtr(); - planning_utils::getAllPartitionLanelets(ll, debug_data_.partition_lanelets); + planning_utils::getAllPartitionLanelets(ll, partition_lanelets_); } } @@ -88,7 +88,6 @@ bool OcclusionSpotModule::modifyPathVelocity( predicted_path = utils::applyVelocityToPath(interp_path, param_.v.v_ego); } else if (param_.pass_judge == utils::PASS_JUDGE::SMOOTH_VELOCITY) { } - debug_data_.interp_path = interp_path; const geometry_msgs::msg::Point start_point = interp_path.points.at(0).point.pose.position; const auto offset = tier4_autoware_utils::calcSignedArcLength( interp_path.points, ego_pose, start_point, param_.dist_thr, param_.angle_thr); @@ -103,7 +102,7 @@ bool OcclusionSpotModule::modifyPathVelocity( // extract only close lanelet if (param_.use_partition_lanelet) { planning_utils::extractClosePartition( - ego_pose.position, debug_data_.partition_lanelets, debug_data_.close_partition); + ego_pose.position, partition_lanelets_, debug_data_.close_partition); } if (param_.is_show_processing_time) stop_watch_.tic("processing_time"); if (param_.detection_method == utils::DETECTION_METHOD::OCCUPANCY_GRID) { @@ -150,6 +149,7 @@ bool OcclusionSpotModule::modifyPathVelocity( debug_data_.z = path->points.front().point.pose.position.z; debug_data_.possible_collisions = possible_collisions; + debug_data_.interp_path = interp_path; debug_data_.path_raw = clipped_path; return true; } From fa71d7e7f955b74281e49fe2102fb85ef429b955 Mon Sep 17 00:00:00 2001 From: tanaka3 Date: Mon, 28 Mar 2022 14:36:18 +0900 Subject: [PATCH 2/6] refactor(behavior_velocity): fix typo Signed-off-by: tanaka3 --- .../src/scene_module/occlusion_spot/scene_occlusion_spot.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp index 79e541be5da3c..2fcecdf779b35 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp @@ -107,7 +107,7 @@ bool OcclusionSpotModule::modifyPathVelocity( if (param_.is_show_processing_time) stop_watch_.tic("processing_time"); if (param_.detection_method == utils::DETECTION_METHOD::OCCUPANCY_GRID) { const auto & occ_grid_ptr = planner_data_->occupancy_grid; - if (!occ_grid_ptr) return true; // mo data + if (!occ_grid_ptr) return true; // no data grid_map::GridMap grid_map; grid_utils::denoiseOccupancyGridCV( occ_grid_ptr, grid_map, param_.grid, param_.is_show_cv_window, param_.filter_occupancy_grid); @@ -125,7 +125,7 @@ bool OcclusionSpotModule::modifyPathVelocity( } } else if (param_.detection_method == utils::DETECTION_METHOD::PREDICTED_OBJECT) { const auto & dynamic_obj_arr_ptr = planner_data_->predicted_objects; - if (!dynamic_obj_arr_ptr) return true; // mo data + if (!dynamic_obj_arr_ptr) return true; // no data std::vector obj = utils::getParkedVehicles(*dynamic_obj_arr_ptr, param_, debug_data_.parked_vehicle_point); const auto filtered_obj = From b985d0db00ac41b15df4ee8ed040139e210f9396 Mon Sep 17 00:00:00 2001 From: tanaka3 Date: Mon, 28 Mar 2022 16:46:05 +0900 Subject: [PATCH 3/6] refactor(behavior_velocity): refactor debuggings Signed-off-by: tanaka3 --- .../config/occlusion_spot.param.yaml | 2 +- .../occlusion_spot/scene_occlusion_spot.hpp | 1 + .../occlusion_spot/occlusion_spot_utils.cpp | 1 + .../occlusion_spot/scene_occlusion_spot.cpp | 50 +++++++++++-------- 4 files changed, 32 insertions(+), 22 deletions(-) diff --git a/planning/behavior_velocity_planner/config/occlusion_spot.param.yaml b/planning/behavior_velocity_planner/config/occlusion_spot.param.yaml index a0b9e9d3243ea..09c06acbfae54 100644 --- a/planning/behavior_velocity_planner/config/occlusion_spot.param.yaml +++ b/planning/behavior_velocity_planner/config/occlusion_spot.param.yaml @@ -1,7 +1,7 @@ /**: ros__parameters: occlusion_spot: - detection_method: "predicted_object" # [-] candidate is "occupancy_grid" or "predicted_object" + detection_method: "occupancy_grid" # [-] candidate is "occupancy_grid" or "predicted_object" pass_judge: "current_velocity" # [-] candidate is "current_velocity" filter_occupancy_grid: true # [-] whether to filter occupancy grid by morphologyEx or not use_object_info: true # [-] whether to reflect object info to occupancy grid map or not diff --git a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot.hpp b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot.hpp index 4eda27f559336..dde073cdde3f0 100644 --- a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot.hpp @@ -62,6 +62,7 @@ class OcclusionSpotModule : public SceneModuleInterface PlannerParam param_; tier4_autoware_utils::StopWatch stop_watch_; std::vector partition_lanelets_; + std::vector close_partition_; protected: int64_t module_id_{}; diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp index c5a51f2d2c92a..8b65d42aab85c 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp @@ -73,6 +73,7 @@ bool buildDetectionAreaPolygon( da_range.min_longitudinal_distance; da_range.min_lateral_distance = p.half_vehicle_width; da_range.max_lateral_distance = p.detection_area.max_lateral_distance; + slices.clear(); return planning_utils::createDetectionAreaPolygons(slices, path, da_range, p.pedestrian_vel); } diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp index 2fcecdf779b35..326619e930cee 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp @@ -29,8 +29,11 @@ #include // turn on only when debugging. -#define DEBUG_PRINT(enable, x) \ - if (enable) RCLCPP_INFO_STREAM_THROTTLE(logger_, *clock_, 3000, x); +#define DEBUG_PRINT(enable, n, x) \ + if (enable) { \ + const double time = x; \ + RCLCPP_INFO_STREAM_THROTTLE(logger_, *clock_, 3000, n << time); \ + } namespace behavior_velocity_planner { @@ -45,7 +48,8 @@ OcclusionSpotModule::OcclusionSpotModule( if (param_.detection_method == utils::DETECTION_METHOD::OCCUPANCY_GRID) { debug_data_.detection_type = "occupancy"; //! occupancy grid limitation( 100 * 100 ) - param_.detection_area_length = std::min(50.0, param_.detection_area_length); + const double max_length = 35.0; // current available length + param_.detection_area_length = std::min(max_length, param_.detection_area_length); } else if (param_.detection_method == utils::DETECTION_METHOD::PREDICTED_OBJECT) { debug_data_.detection_type = "object"; } @@ -59,6 +63,7 @@ bool OcclusionSpotModule::modifyPathVelocity( autoware_auto_planning_msgs::msg::PathWithLaneId * path, [[maybe_unused]] tier4_planning_msgs::msg::StopReason * stop_reason) { + if (param_.is_show_processing_time) stop_watch_.tic("total_processing_time"); debug_data_.resetData(); if (path->points.size() < 2) { return true; @@ -83,28 +88,31 @@ bool OcclusionSpotModule::modifyPathVelocity( PathWithLaneId interp_path; //! never change this interpolation interval(will affect module accuracy) splineInterpolate(clipped_path, 1.0, &interp_path, logger_); - PathWithLaneId predicted_path; - if (param_.pass_judge == utils::PASS_JUDGE::CURRENT_VELOCITY) { - predicted_path = utils::applyVelocityToPath(interp_path, param_.v.v_ego); - } else if (param_.pass_judge == utils::PASS_JUDGE::SMOOTH_VELOCITY) { - } const geometry_msgs::msg::Point start_point = interp_path.points.at(0).point.pose.position; const auto offset = tier4_autoware_utils::calcSignedArcLength( interp_path.points, ego_pose, start_point, param_.dist_thr, param_.angle_thr); if (offset == boost::none) return true; const double offset_from_start_to_ego = -offset.get(); - auto & detection_area_polygons = debug_data_.detection_area_polygons; + const bool show_time = param_.is_show_processing_time; + if (show_time) stop_watch_.tic("processing_time"); + PathWithLaneId predicted_path; + if (param_.pass_judge == utils::PASS_JUDGE::CURRENT_VELOCITY) { + predicted_path = utils::applyVelocityToPath(interp_path, param_.v.v_ego); + } else if (param_.pass_judge == utils::PASS_JUDGE::SMOOTH_VELOCITY) { + } + DEBUG_PRINT(show_time, "apply velocity [ms]: ", stop_watch_.toc("processing_time", true)); if (!utils::buildDetectionAreaPolygon( - detection_area_polygons, predicted_path, offset_from_start_to_ego, param_)) { + debug_data_.detection_area_polygons, predicted_path, offset_from_start_to_ego, param_)) { return true; // path point is not enough } + DEBUG_PRINT(show_time, "generate poly[ms]: ", stop_watch_.toc("processing_time", true)); std::vector possible_collisions; // extract only close lanelet if (param_.use_partition_lanelet) { - planning_utils::extractClosePartition( - ego_pose.position, partition_lanelets_, debug_data_.close_partition); + planning_utils::extractClosePartition(ego_pose.position, partition_lanelets_, close_partition_); } - if (param_.is_show_processing_time) stop_watch_.tic("processing_time"); + DEBUG_PRINT(show_time, "extract[ms]: ", stop_watch_.toc("processing_time", true)); + std::vector parked_vehicle_point; if (param_.detection_method == utils::DETECTION_METHOD::OCCUPANCY_GRID) { const auto & occ_grid_ptr = planner_data_->occupancy_grid; if (!occ_grid_ptr) return true; // no data @@ -114,8 +122,7 @@ bool OcclusionSpotModule::modifyPathVelocity( if (param_.use_object_info) { grid_utils::addObjectsToGridMap(*planner_data_->predicted_objects, grid_map); } - DEBUG_PRINT( - param_.is_show_processing_time, "grid [ms]: " << stop_watch_.toc("processing_time", true)); + DEBUG_PRINT(show_time, "grid [ms]: ", stop_watch_.toc("processing_time", true)); // Note: Don't consider offset from path start to ego here if (!utils::createPossibleCollisionsInDetectionArea( possible_collisions, grid_map, interp_path, offset_from_start_to_ego, param_, @@ -127,9 +134,9 @@ bool OcclusionSpotModule::modifyPathVelocity( const auto & dynamic_obj_arr_ptr = planner_data_->predicted_objects; if (!dynamic_obj_arr_ptr) return true; // no data std::vector obj = - utils::getParkedVehicles(*dynamic_obj_arr_ptr, param_, debug_data_.parked_vehicle_point); + utils::getParkedVehicles(*dynamic_obj_arr_ptr, param_, parked_vehicle_point); const auto filtered_obj = - utils::filterDynamicObjectByDetectionArea(obj, detection_area_polygons); + utils::filterDynamicObjectByDetectionArea(obj, debug_data_.detection_area_polygons); // Note: Don't consider offset from path start to ego here if (!utils::generatePossibleCollisionBehindParkedVehicle( possible_collisions, interp_path, param_, offset_from_start_to_ego, filtered_obj)) { @@ -137,20 +144,21 @@ bool OcclusionSpotModule::modifyPathVelocity( return true; } } - DEBUG_PRINT( - param_.is_show_processing_time, "occlusion [ms]: " << stop_watch_.toc("processing_time", true)); - DEBUG_PRINT(param_.is_show_occlusion, "num collision:" << possible_collisions.size()); + DEBUG_PRINT(show_time, "occlusion [ms]: ", stop_watch_.toc("processing_time", true)); + DEBUG_PRINT(show_time, "num collision:", possible_collisions.size()); utils::calcSlowDownPointsForPossibleCollision(0, interp_path, 0.0, possible_collisions); // Note: Consider offset from path start to ego here utils::handleCollisionOffset(possible_collisions, offset_from_start_to_ego); // apply safe velocity using ebs and pbs deceleration utils::applySafeVelocityConsideringPossibleCollision(path, possible_collisions, param_); // these debug topics needs computation resource - + debug_data_.parked_vehicle_point = parked_vehicle_point; + debug_data_.close_partition = close_partition_; debug_data_.z = path->points.front().point.pose.position.z; debug_data_.possible_collisions = possible_collisions; debug_data_.interp_path = interp_path; debug_data_.path_raw = clipped_path; + DEBUG_PRINT(show_time, "total [ms]: ", stop_watch_.toc("total_processing_time", true)); return true; } From 59e31801f66d063987af79b153808a0182960b38 Mon Sep 17 00:00:00 2001 From: tanaka3 Date: Tue, 29 Mar 2022 12:13:33 +0900 Subject: [PATCH 4/6] refactor(behavior_velocity): unite to generate Signed-off-by: tanaka3 --- .../occlusion_spot/occlusion_spot_utils.hpp | 4 ++-- .../occlusion_spot/occlusion_spot_utils.cpp | 14 +++++--------- .../occlusion_spot/scene_occlusion_spot.cpp | 12 ++++++------ 3 files changed, 13 insertions(+), 17 deletions(-) diff --git a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp index 10783cbe5414e..356212191c7c5 100644 --- a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp @@ -220,7 +220,7 @@ std::vector filterDynamicObjectByDetectionArea( std::vector getParkedVehicles( const PredictedObjects & dyn_objects, const PlannerParam & param, std::vector & debug_point); -bool generatePossibleCollisionBehindParkedVehicle( +bool generatePossibleCollisionsFromObjects( std::vector & possible_collisions, const PathWithLaneId & path, const PlannerParam & param, const double offset_from_start_to_ego, const std::vector & dyn_objects); @@ -246,7 +246,7 @@ boost::optional generateOneNotableCollisionFromOcclusionS const double offset_from_start_to_ego, const Point2d base_point, const lanelet::ConstLanelet & path_lanelet, const PlannerParam & param, DebugData & debug_data); //!< @brief generate possible collisions coming from occlusion spots on the side of the path -bool createPossibleCollisionsInDetectionArea( +bool generatePossibleCollisionsFromGridMap( std::vector & possible_collisions, const grid_map::GridMap & grid, const PathWithLaneId & path, const double offset_from_start_to_ego, const PlannerParam & param, DebugData & debug_data); diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp index 8b65d42aab85c..951a91638adae 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp @@ -315,7 +315,7 @@ PossibleCollisionInfo calculateCollisionPathPointFromOcclusionSpot( return pc; } -bool generatePossibleCollisionBehindParkedVehicle( +bool generatePossibleCollisionsFromObjects( std::vector & possible_collisions, const PathWithLaneId & path, const PlannerParam & param, const double offset_from_start_to_ego, const std::vector & dyn_objects) @@ -361,7 +361,7 @@ std::vector filterDynamicObjectByDetectionArea( return filtered_obj; } -bool createPossibleCollisionsInDetectionArea( +bool generatePossibleCollisionsFromGridMap( std::vector & possible_collisions, const grid_map::GridMap & grid, const PathWithLaneId & path, const double offset_from_start_to_ego, const PlannerParam & param, DebugData & debug_data) @@ -396,8 +396,7 @@ bool createPossibleCollisionsInDetectionArea( distance_lower_bound = lateral_distance; possible_collisions.emplace_back(pc.get()); } - if (possible_collisions.empty()) return false; - return true; + return !possible_collisions.empty(); } bool isBlockedByPartition(const LineString2d & direction, const BasicPolygons2d & partitions) @@ -454,11 +453,8 @@ boost::optional generateOneNotableCollisionFromOcclusionS candidate = pc; has_collision = true; } - if (has_collision) { - return candidate; - } else { - return boost::none; - } + if (has_collision) return candidate; + return boost::none; } } // namespace occlusion_spot_utils diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp index 326619e930cee..93cb1f4dce78a 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp @@ -29,10 +29,10 @@ #include // turn on only when debugging. -#define DEBUG_PRINT(enable, n, x) \ - if (enable) { \ - const double time = x; \ - RCLCPP_INFO_STREAM_THROTTLE(logger_, *clock_, 3000, n << time); \ +#define DEBUG_PRINT(enable, n, x) \ + if (enable) { \ + const double time = x; \ + RCLCPP_INFO_STREAM_THROTTLE(logger_, *clock_, 3000, (n << time)); \ } namespace behavior_velocity_planner @@ -124,7 +124,7 @@ bool OcclusionSpotModule::modifyPathVelocity( } DEBUG_PRINT(show_time, "grid [ms]: ", stop_watch_.toc("processing_time", true)); // Note: Don't consider offset from path start to ego here - if (!utils::createPossibleCollisionsInDetectionArea( + if (!utils::generatePossibleCollisionsFromGridMap( possible_collisions, grid_map, interp_path, offset_from_start_to_ego, param_, debug_data_)) { // no occlusion spot @@ -138,7 +138,7 @@ bool OcclusionSpotModule::modifyPathVelocity( const auto filtered_obj = utils::filterDynamicObjectByDetectionArea(obj, debug_data_.detection_area_polygons); // Note: Don't consider offset from path start to ego here - if (!utils::generatePossibleCollisionBehindParkedVehicle( + if (!utils::generatePossibleCollisionsFromObjects( possible_collisions, interp_path, param_, offset_from_start_to_ego, filtered_obj)) { // no occlusion spot return true; From 0ecb73cc1e0e2c3e7a0c9f3a6803bb7c54c2850e Mon Sep 17 00:00:00 2001 From: tanaka3 Date: Tue, 29 Mar 2022 12:25:23 +0900 Subject: [PATCH 5/6] refactor(behavior_velocity): apply clang tidy Signed-off-by: tanaka3 --- .../scene_module/occlusion_spot/occlusion_spot_utils.cpp | 3 +-- .../scene_module/occlusion_spot/scene_occlusion_spot.cpp | 8 ++++---- 2 files changed, 5 insertions(+), 6 deletions(-) diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp index 951a91638adae..55f7047654749 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp @@ -340,8 +340,7 @@ bool generatePossibleCollisionsFromObjects( arc_coord_occlusion, arc_coord_occlusion_with_offset, path_lanelet, param); possible_collisions.emplace_back(pc); } - if (possible_collisions.empty()) return false; - return true; + return !possible_collisions.empty(); } std::vector filterDynamicObjectByDetectionArea( diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp index 93cb1f4dce78a..6dd75b302387c 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp @@ -29,10 +29,10 @@ #include // turn on only when debugging. -#define DEBUG_PRINT(enable, n, x) \ - if (enable) { \ - const double time = x; \ - RCLCPP_INFO_STREAM_THROTTLE(logger_, *clock_, 3000, (n << time)); \ +#define DEBUG_PRINT(enable, n, x) \ + if (enable) { \ + const std::string time_msg = n + std::to_string(x); \ + RCLCPP_INFO_STREAM_THROTTLE(logger_, *clock_, 3000, time_msg); \ } namespace behavior_velocity_planner From 60a17c94d9ec14a2de688f5d26a670ef5867d096 Mon Sep 17 00:00:00 2001 From: tanaka3 Date: Tue, 29 Mar 2022 13:21:29 +0900 Subject: [PATCH 6/6] chore(behavior_velocity): retun to default settting Signed-off-by: tanaka3 --- .../behavior_velocity_planner/config/occlusion_spot.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/behavior_velocity_planner/config/occlusion_spot.param.yaml b/planning/behavior_velocity_planner/config/occlusion_spot.param.yaml index 09c06acbfae54..a0b9e9d3243ea 100644 --- a/planning/behavior_velocity_planner/config/occlusion_spot.param.yaml +++ b/planning/behavior_velocity_planner/config/occlusion_spot.param.yaml @@ -1,7 +1,7 @@ /**: ros__parameters: occlusion_spot: - detection_method: "occupancy_grid" # [-] candidate is "occupancy_grid" or "predicted_object" + detection_method: "predicted_object" # [-] candidate is "occupancy_grid" or "predicted_object" pass_judge: "current_velocity" # [-] candidate is "current_velocity" filter_occupancy_grid: true # [-] whether to filter occupancy grid by morphologyEx or not use_object_info: true # [-] whether to reflect object info to occupancy grid map or not