From 3bf596ee7f5892ba4996c64b5fd56341159871f0 Mon Sep 17 00:00:00 2001 From: Ryuta Kambe Date: Thu, 5 Jan 2023 11:08:56 +0900 Subject: [PATCH] perf(behavior_velocity_planner): remove pose.orientation from occlusion_spot (#2591) Signed-off-by: kminoda --- .../occlusion_spot/occlusion_spot_utils.hpp | 3 +++ .../risk_predictive_braking.hpp | 2 +- .../src/scene_module/occlusion_spot/debug.cpp | 13 +++++----- .../occlusion_spot/occlusion_spot_utils.cpp | 25 +++++++++---------- .../risk_predictive_braking.cpp | 6 +++-- .../occlusion_spot/scene_occlusion_spot.cpp | 4 ++- 6 files changed, 29 insertions(+), 24 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 bfe03cced0a77..47ea7b5532278 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 @@ -183,6 +183,7 @@ struct PossibleCollisionInfo struct DebugData { double z; + double baselink_to_front; std::string road_type = ""; std::string detection_type = ""; Polygons2d detection_area_polygons; @@ -190,8 +191,10 @@ struct DebugData std::vector parked_vehicle_point; std::vector possible_collisions; std::vector occlusion_points; + std::vector debug_poses; void resetData() { + debug_poses.clear(); close_partition.clear(); detection_area_polygons.clear(); parked_vehicle_point.clear(); diff --git a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/risk_predictive_braking.hpp b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/risk_predictive_braking.hpp index c201cc4158907..15eac76e35c48 100644 --- a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/risk_predictive_braking.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/risk_predictive_braking.hpp @@ -28,7 +28,7 @@ namespace occlusion_spot_utils { void applySafeVelocityConsideringPossibleCollision( PathWithLaneId * inout_path, std::vector & possible_collisions, - const PlannerParam & param); + std::vector & debug_poses, const PlannerParam & param); /** * @param: v: ego velocity config 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 76e71b67a2d03..73c8b12b79162 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 @@ -219,13 +219,12 @@ MarkerArray OcclusionSpotModule::createVirtualWallMarkerArray() MarkerArray wall_marker; std::string module_name = "occlusion_spot"; - if (!debug_data_.possible_collisions.empty()) { - for (size_t id = 0; id < debug_data_.possible_collisions.size(); id++) { - const auto & pose = debug_data_.possible_collisions.at(id).intersection_pose; - appendMarkerArray( - motion_utils::createSlowDownVirtualWallMarker(pose, module_name, current_time, id), - &wall_marker, current_time); - } + for (size_t id = 0; id < debug_data_.debug_poses.size(); id++) { + const auto p_front = + calcOffsetPose(debug_data_.debug_poses.at(id), debug_data_.baselink_to_front, 0.0, 0.0); + appendMarkerArray( + motion_utils::createSlowDownVirtualWallMarker(p_front, module_name, current_time, id), + &wall_marker, current_time); } return wall_marker; } 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 ec20be3a2457c..d8507cb5a77c8 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 @@ -283,16 +283,13 @@ PossibleCollisionInfo calculateCollisionPathPointFromOcclusionSpot( const ArcCoordinates & arc_coord_occlusion_with_offset, const lanelet::ConstLanelet & path_lanelet, const PlannerParam & param) { - auto calcPose = [](const lanelet::ConstLanelet & pl, const ArcCoordinates & arc) { - const auto & ll = pl.centerline2d(); + auto calcPosition = [](const ConstLineString2d & ll, const ArcCoordinates & arc) { BasicPoint2d bp = fromArcCoordinates(ll, arc); - Pose pose; - pose.position.x = bp[0]; - pose.position.y = bp[1]; - pose.position.z = 0.0; - pose.orientation = tier4_autoware_utils::createQuaternionFromYaw( - lanelet::utils::getLaneletAngle(pl, pose.position)); - return pose; + Point position; + position.x = bp[0]; + position.y = bp[1]; + position.z = 0.0; + return position; }; /** * @brief calculate obstacle collision intersection from arc coordinate info. @@ -316,11 +313,13 @@ 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.ttv = ttv; - pc.obstacle_info.position = calcPose(path_lanelet, arc_coord_occlusion).position; + + const auto & ll = path_lanelet.centerline2d(); + pc.obstacle_info.position = calcPosition(ll, arc_coord_occlusion); pc.obstacle_info.max_velocity = param.pedestrian_vel; - 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}); + pc.collision_pose.position = calcPosition(ll, {arc_coord_occlusion_with_offset.length, 0.0}); + pc.collision_with_margin.pose.position = calcPosition(ll, {distance_to_stop, 0.0}); + pc.intersection_pose.position = calcPosition(ll, {arc_coord_occlusion.length, 0.0}); return pc; } 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 3e7ff2c9e281a..f4817428af445 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 @@ -25,7 +25,7 @@ namespace occlusion_spot_utils { void applySafeVelocityConsideringPossibleCollision( PathWithLaneId * inout_path, std::vector & possible_collisions, - const PlannerParam & param) + std::vector & debug_poses, const PlannerParam & param) { // return nullptr or too few points if (!inout_path || inout_path->points.size() < 2) { @@ -60,7 +60,9 @@ void applySafeVelocityConsideringPossibleCollision( safe_velocity = std::max(safe_velocity, v_min); possible_collision.obstacle_info.safe_motion.safe_velocity = safe_velocity; const auto & pose = possible_collision.collision_with_margin.pose; - planning_utils::insertDecelPoint(pose.position, *inout_path, safe_velocity); + const auto & decel_pose = + planning_utils::insertDecelPoint(pose.position, *inout_path, safe_velocity); + if (decel_pose) debug_poses.push_back(decel_pose.get()); } } 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 9bafd9e707741..1d09cac0bceeb 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 @@ -183,7 +183,9 @@ bool OcclusionSpotModule::modifyPathVelocity( // 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_); + utils::applySafeVelocityConsideringPossibleCollision( + path, possible_collisions, debug_data_.debug_poses, param_); + debug_data_.baselink_to_front = param_.baselink_to_front; // these debug topics needs computation resource debug_data_.z = path->points.front().point.pose.position.z; debug_data_.possible_collisions = possible_collisions;