From 4e9ecf9b9e291c7cb0f3fb6bd4ec73ceef2699e5 Mon Sep 17 00:00:00 2001 From: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com> Date: Fri, 28 Apr 2023 14:45:22 +0900 Subject: [PATCH 1/9] feat(behavior_path_planner): use lanelets for getting backward lanelets (#3567) * feat(behavior_path_planner): use lanelets for getting backward lanelets Signed-off-by: yutaka * update Signed-off-by: yutaka --------- Signed-off-by: yutaka --- .../utils/lane_change/utils.hpp | 4 ++-- .../scene_module/avoidance_by_lc/module.cpp | 8 ++++---- .../src/scene_module/lane_change/normal.cpp | 17 +++++++++-------- .../src/utils/lane_change/utils.cpp | 19 ++++++++++++------- 4 files changed, 27 insertions(+), 21 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp index 33ff4da7d77ff..43514b011c6fb 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp @@ -165,8 +165,8 @@ bool hasEnoughLengthToLaneChangeAfterAbort( const Pose & curent_pose, const double abort_return_dist, const BehaviorPathPlannerParameters & common_param); -lanelet::ConstLanelets getExtendedTargetLanesForCollisionCheck( - const RouteHandler & route_handler, const lanelet::ConstLanelet & target_lanes, +lanelet::ConstLanelets getBackwardLanelets( + const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, const Pose & current_pose, const double backward_length); LaneChangeTargetObjectIndices filterObjectIndices( 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 f7103df9c1647..e3f032d3d0be3 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 @@ -948,15 +948,15 @@ bool AvoidanceByLCModule::isApprovedPathSafe(Pose & ego_pose_before_collision) c const auto & path = status_.lane_change_path; // get lanes used for detection - const auto check_lanes = utils::lane_change::getExtendedTargetLanesForCollisionCheck( - *route_handler, path.target_lanelets.front(), current_pose, check_distance_); + const auto backward_target_lanes_for_object_filtering = utils::lane_change::getBackwardLanelets( + *route_handler, path.target_lanelets, current_pose, check_distance_); std::unordered_map debug_data; const auto lateral_buffer = utils::lane_change::calcLateralBufferForFiltering(common_parameters.vehicle_width); const auto dynamic_object_indices = utils::lane_change::filterObjectIndices( - {path}, *dynamic_objects, check_lanes, current_pose, common_parameters.forward_path_length, - *lane_change_parameters, lateral_buffer); + {path}, *dynamic_objects, backward_target_lanes_for_object_filtering, current_pose, + common_parameters.forward_path_length, *lane_change_parameters, lateral_buffer); return utils::lane_change::isLaneChangePathSafe( path, dynamic_objects, dynamic_object_indices, current_pose, current_twist, common_parameters, 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 c94d051b0551c..3ec87716f8035 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 @@ -492,11 +492,12 @@ bool NormalLaneChange::getLaneChangePaths( if (candidate_paths->empty()) { // only compute dynamic object indices once - const auto backward_lanes = utils::lane_change::getExtendedTargetLanesForCollisionCheck( - route_handler, target_lanelets.front(), getEgoPose(), check_length_); + const auto backward_target_lanes_for_object_filtering = + utils::lane_change::getBackwardLanelets( + route_handler, target_lanelets, getEgoPose(), check_length_); dynamic_object_indices = utils::lane_change::filterObjectIndices( - {*candidate_path}, *dynamic_objects, backward_lanes, getEgoPose(), - common_parameter.forward_path_length, *parameters_, lateral_buffer); + {*candidate_path}, *dynamic_objects, backward_target_lanes_for_object_filtering, + getEgoPose(), common_parameter.forward_path_length, *parameters_, lateral_buffer); } candidate_paths->push_back(*candidate_path); @@ -532,15 +533,15 @@ bool NormalLaneChange::isApprovedPathSafe(Pose & ego_pose_before_collision) cons const auto & path = status_.lane_change_path; // get lanes used for detection - const auto check_lanes = utils::lane_change::getExtendedTargetLanesForCollisionCheck( - *route_handler, path.target_lanelets.front(), current_pose, check_length_); + const auto backward_target_lanes_for_object_filtering = utils::lane_change::getBackwardLanelets( + *route_handler, path.target_lanelets, current_pose, check_length_); CollisionCheckDebugMap debug_data; const auto lateral_buffer = utils::lane_change::calcLateralBufferForFiltering(common_parameters.vehicle_width); const auto dynamic_object_indices = utils::lane_change::filterObjectIndices( - {path}, *dynamic_objects, check_lanes, current_pose, common_parameters.forward_path_length, - lane_change_parameters, lateral_buffer); + {path}, *dynamic_objects, backward_target_lanes_for_object_filtering, current_pose, + common_parameters.forward_path_length, lane_change_parameters, lateral_buffer); return utils::lane_change::isLaneChangePathSafe( path, dynamic_objects, dynamic_object_indices, current_pose, current_twist, common_parameters, diff --git a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp index 4a036738e8421..718b5793bb558 100644 --- a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp +++ b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp @@ -402,10 +402,10 @@ bool getLaneChangePaths( if (candidate_paths->empty()) { // only compute dynamic object indices once - const auto backward_lanes = utils::lane_change::getExtendedTargetLanesForCollisionCheck( - route_handler, target_lanelets.front(), pose, check_length); + const auto backward_target_lanes_for_object_filtering = + utils::lane_change::getBackwardLanelets(route_handler, target_lanelets, pose, check_length); dynamic_object_indices = filterObjectIndices( - {*candidate_path}, *dynamic_objects, backward_lanes, pose, + {*candidate_path}, *dynamic_objects, backward_target_lanes_for_object_filtering, pose, common_parameter.forward_path_length, parameter, lateral_buffer); } candidate_paths->push_back(*candidate_path); @@ -1115,18 +1115,23 @@ bool hasEnoughLengthToLaneChangeAfterAbort( } // TODO(Azu): In the future, get back lanelet within `to_back_dist` [m] from queried lane -lanelet::ConstLanelets getExtendedTargetLanesForCollisionCheck( - const RouteHandler & route_handler, const lanelet::ConstLanelet & target_lane, +lanelet::ConstLanelets getBackwardLanelets( + const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, const Pose & current_pose, const double backward_length) { - const auto arc_length = lanelet::utils::getArcCoordinates({target_lane}, current_pose); + if (target_lanes.empty()) { + return {}; + } + + const auto arc_length = lanelet::utils::getArcCoordinates(target_lanes, current_pose); if (arc_length.length >= backward_length) { return {}; } + const auto & front_lane = target_lanes.front(); const auto preceding_lanes = route_handler.getPrecedingLaneletSequence( - target_lane, std::abs(backward_length - arc_length.length), {target_lane}); + front_lane, std::abs(backward_length - arc_length.length), {front_lane}); lanelet::ConstLanelets backward_lanes{}; const auto num_of_lanes = std::invoke([&preceding_lanes]() { From 393178dc2496fa5d557880399f85283f06dbe5e9 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos Tsunekawa Date: Fri, 28 Apr 2023 15:46:24 +0900 Subject: [PATCH 2/9] fix(lidar_centerpoint): fixed a typo in the documentation related to centerpoint's training (#3571) Fixed a typo in the documentation related to centerpoint Signed-off-by: Kenzo Lobos-Tsunekawa --- perception/lidar_centerpoint/README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/perception/lidar_centerpoint/README.md b/perception/lidar_centerpoint/README.md index b1cb925f5e49f..f5a15916f31a9 100644 --- a/perception/lidar_centerpoint/README.md +++ b/perception/lidar_centerpoint/README.md @@ -56,8 +56,8 @@ You can download the onnx format of trained models by clicking on the links belo - Centerpoint : [pts_voxel_encoder_centerpoint.onnx](https://awf.ml.dev.web.auto/perception/models/centerpoint/v2/pts_voxel_encoder_centerpoint.onnx), [pts_backbone_neck_head_centerpoint.onnx](https://awf.ml.dev.web.auto/perception/models/centerpoint/v2/pts_backbone_neck_head_centerpoint.onnx) - Centerpoint tiny: [pts_voxel_encoder_centerpoint_tiny.onnx](https://awf.ml.dev.web.auto/perception/models/centerpoint/v2/pts_voxel_encoder_centerpoint_tiny.onnx), [pts_backbone_neck_head_centerpoint_tiny.onnx](https://awf.ml.dev.web.auto/perception/models/centerpoint/v2/pts_backbone_neck_head_centerpoint_tiny.onnx) -`Centerpoint` was trained in `nuScenes` (~110k lidar frames) [8] and TIER IV's internal database (~11k lidar frames) for 60 epochs. -`Centerpoint tiny` was trained in `Argoverse 2` (~28k lidar frames) [9] and TIER IV's internal database (~11k lidar frames) for 20 epochs. +`Centerpoint` was trained in `nuScenes` (~28k lidar frames) [8] and TIER IV's internal database (~11k lidar frames) for 60 epochs. +`Centerpoint tiny` was trained in `Argoverse 2` (~110k lidar frames) [9] and TIER IV's internal database (~11k lidar frames) for 20 epochs. ## Standalone inference and visualization From 2f5d8f03606c680834739c31d317c1bd9ed66054 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Fri, 28 Apr 2023 15:54:50 +0900 Subject: [PATCH 3/9] feat(behavior_velocity_planner::intersection): modify module deletion condition (#3572) Signed-off-by: Mamoru Sobue --- planning/behavior_velocity_planner/src/utilization/util.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/planning/behavior_velocity_planner/src/utilization/util.cpp b/planning/behavior_velocity_planner/src/utilization/util.cpp index db5520a9142d5..d3d73927c2727 100644 --- a/planning/behavior_velocity_planner/src/utilization/util.cpp +++ b/planning/behavior_velocity_planner/src/utilization/util.cpp @@ -677,6 +677,7 @@ std::set getAssociativeIntersectionLanelets( for (const auto & neighbor : neighbors) parent_neighbors.insert(neighbor.id()); } std::set assocs; + assocs.insert(lane.id()); for (const auto & parent_neighbor_id : parent_neighbors) { const auto parent_neighbor = lanelet_map->laneletLayer.get(parent_neighbor_id); const auto followings = routing_graph->following(parent_neighbor); From dcf9d3240986f0877242151fabe7061803741499 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Fri, 28 Apr 2023 16:08:25 +0900 Subject: [PATCH 4/9] fix(avoidance): fix bug in debug marker generation (#3547) Signed-off-by: satoshi-ota --- .../behavior_path_planner/src/marker_util/avoidance/debug.cpp | 3 +++ .../src/scene_module/avoidance/avoidance_module.cpp | 1 - 2 files changed, 3 insertions(+), 1 deletion(-) 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 336c1c8d0a051..b4075932b80ab 100644 --- a/planning/behavior_path_planner/src/marker_util/avoidance/debug.cpp +++ b/planning/behavior_path_planner/src/marker_util/avoidance/debug.cpp @@ -88,6 +88,9 @@ MarkerArray createObjectInfoMarkerArray(const ObjectDataArray & objects, std::st << "move_time:" << object.move_time << " [s]\n" << "stop_time:" << object.stop_time << " [s]\n"; marker.text = string_stream.str(); + marker.color = createMarkerColor(1.0, 1.0, 0.0, 0.999); + marker.scale = createMarkerScale(0.5, 0.5, 0.5); + marker.ns = ns; msg.markers.push_back(marker); } 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 8c06b074998c3..a8e09dae9e703 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 @@ -3328,7 +3328,6 @@ void AvoidanceModule::setDebugData( add(createOverhangFurthestLineStringMarkerArray( debug.bounds, "farthest_linestring_from_overhang", 1.0, 0.0, 1.0)); - add(createUnavoidableObjectsMarkerArray(debug.unavoidable_objects, "unavoidable_objects")); add(createUnsafeObjectsMarkerArray(debug.unsafe_objects, "unsafe_objects")); // parent object info From 4360ee1180ee14f1e0ec9f7c8c57bab828351f36 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Fri, 28 Apr 2023 17:35:01 +0900 Subject: [PATCH 5/9] fix(intersection): always send finite distance to occlusion peeking stop line in RTC (#3573) Signed-off-by: Mamoru Sobue --- .../src/scene_module/intersection/manager.cpp | 5 ++--- .../src/scene_module/intersection/scene_intersection.cpp | 6 +++--- 2 files changed, 5 insertions(+), 6 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 85b49217b0fdc..e64e26c1555d7 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp @@ -189,17 +189,16 @@ void IntersectionModuleManager::sendRTC(const Time & stamp) const bool is_occluded = intersection_module->isOccluded(); const UUID uuid = getUUID(scene_module->getModuleId()); const auto occlusion_uuid = intersection_module->getOcclusionUUID(); + const auto occlusion_distance = intersection_module->getOcclusionDistance(); const auto occlusion_first_stop_uuid = intersection_module->getOcclusionFirstStopUUID(); if (!is_occluded) { // 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); + occlusion_uuid, true, occlusion_distance, occlusion_distance, stamp); } else { // 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); 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 059d0ef15d7e4..e32b6695a0cec 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 @@ -385,17 +385,17 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * logger_.get_child("collision state_machine"), *clock_); /* set RTC safety respectively */ + occlusion_first_stop_distance_ = dist_1st_stopline; + occlusion_stop_distance_ = dist_2nd_stopline; + setDistance(dist_1st_stopline); if (occlusion_stop_required) { if (first_phase_stop_required) { occlusion_first_stop_safety_ = false; - occlusion_first_stop_distance_ = dist_1st_stopline; } occlusion_safety_ = is_occlusion_cleared; - occlusion_stop_distance_ = dist_2nd_stopline; } else { /* collision */ setSafe(collision_state_machine_.getState() == StateMachine::State::GO); - setDistance(dist_1st_stopline); } /* make decision */ From 7bd2b7875a1ed58413f9eb57c49fe3ab44dcca0b Mon Sep 17 00:00:00 2001 From: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com> Date: Fri, 28 Apr 2023 20:01:32 +0900 Subject: [PATCH 6/9] refactor(behavior_path_planner): rename some variables (#3570) --- .../src/utils/lane_change/utils.cpp | 10 +++++----- .../src/utils/safety_check.cpp | 13 ++++++------- 2 files changed, 11 insertions(+), 12 deletions(-) diff --git a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp index 718b5793bb558..67cdd72511585 100644 --- a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp +++ b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp @@ -497,7 +497,7 @@ bool isLaneChangePathSafe( path.points, current_pose, common_parameter.ego_nearest_dist_threshold, common_parameter.ego_nearest_yaw_threshold); - const auto vehicle_predicted_path = utils::convertToPredictedPath( + const auto ego_predicted_path = utils::convertToPredictedPath( path, current_twist, current_pose, current_seg_idx, check_end_time, time_resolution, prepare_duration, acceleration); const auto & vehicle_info = common_parameter.vehicle_info; @@ -541,7 +541,7 @@ bool isLaneChangePathSafe( for (double t = check_start_time; t < check_end_time; t += time_resolution) { tier4_autoware_utils::Polygon2d ego_polygon; const auto result = - utils::getEgoExpectedPoseAndConvertToPolygon(vehicle_predicted_path, t, vehicle_info); + utils::getEgoExpectedPoseAndConvertToPolygon(ego_predicted_path, t, vehicle_info); if (!result) { continue; } @@ -552,9 +552,9 @@ bool isLaneChangePathSafe( for (const auto & i : in_lane_object_indices) { const auto & obj = dynamic_objects->objects.at(i); auto current_debug_data = assignDebugData(obj); - const auto predicted_paths = + const auto obj_predicted_paths = utils::getPredictedPathFromObj(obj, lane_change_parameter.use_all_predicted_path); - for (const auto & obj_path : predicted_paths) { + for (const auto & obj_path : obj_predicted_paths) { if (!utils::safety_check::isSafeInLaneletCollisionCheck( interpolated_ego, current_twist, check_durations, lane_change_path.duration.prepare, obj, obj_path, common_parameter, @@ -574,7 +574,7 @@ bool isLaneChangePathSafe( for (const auto & i : dynamic_objects_indices.other_lane) { const auto & obj = dynamic_objects->objects.at(i); auto current_debug_data = assignDebugData(obj); - current_debug_data.second.ego_predicted_path.push_back(vehicle_predicted_path); + current_debug_data.second.ego_predicted_path.push_back(ego_predicted_path); const auto predicted_paths = utils::getPredictedPathFromObj(obj, lane_change_parameter.use_all_predicted_path); diff --git a/planning/behavior_path_planner/src/utils/safety_check.cpp b/planning/behavior_path_planner/src/utils/safety_check.cpp index 0bd83be6d059b..82230e93bb732 100644 --- a/planning/behavior_path_planner/src/utils/safety_check.cpp +++ b/planning/behavior_path_planner/src/utils/safety_check.cpp @@ -199,16 +199,15 @@ bool isSafeInLaneletCollisionCheck( continue; } - const auto found_expected_obj_pose = + auto expected_obj_pose = perception_utils::calcInterpolatedPose(target_object_path, current_time); - if (!found_expected_obj_pose) { + if (!expected_obj_pose) { continue; } - auto expected_obj_pose = *found_expected_obj_pose; const auto & obj_polygon = - tier4_autoware_utils::toPolygon2d(expected_obj_pose, target_object.shape); + tier4_autoware_utils::toPolygon2d(*expected_obj_pose, target_object.shape); const auto & ego_info = interpolated_ego.at(i); auto expected_ego_pose = ego_info.first; const auto & ego_polygon = ego_info.second; @@ -223,12 +222,12 @@ bool isSafeInLaneletCollisionCheck( debug.lerped_path.push_back(expected_ego_pose); getProjectedDistancePointFromPolygons( - ego_polygon, obj_polygon, expected_ego_pose, expected_obj_pose); + ego_polygon, obj_polygon, expected_ego_pose, *expected_obj_pose); debug.expected_ego_pose = expected_ego_pose; - debug.expected_obj_pose = expected_obj_pose; + debug.expected_obj_pose = *expected_obj_pose; if (!hasEnoughDistance( - expected_ego_pose, ego_current_twist, expected_obj_pose, object_twist, common_parameters, + expected_ego_pose, ego_current_twist, *expected_obj_pose, object_twist, common_parameters, front_decel, rear_decel, debug)) { debug.failed_reason = "not_enough_longitudinal"; return false; From 24bce1e3652b25df274e5a3e95e6adf10c198716 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Fri, 28 Apr 2023 21:29:26 +0900 Subject: [PATCH 7/9] feat(behavior_path_planner): add dynamic obstacle avoidance module (#3415) * implement dynamic avoidance module Signed-off-by: Takayuki Murooka * update Signed-off-by: Takayuki Murooka * update Signed-off-by: Takayuki Murooka * fix spell Signed-off-by: Takayuki Murooka * update Signed-off-by: Takayuki Murooka * Update planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> * Update planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> * Update planning/behavior_path_planner/docs/behavior_path_planner_dynamic_avoidance_design.md Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> * dealt with review Signed-off-by: Takayuki Murooka * update test Signed-off-by: Takayuki Murooka * disable dynamic avoidance with old architecture, and pass build CI Signed-off-by: Takayuki Murooka * fix Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> --- .../launch/planning.launch.xml | 1 + .../behavior_planning.launch.py | 3 + planning/behavior_path_planner/CMakeLists.txt | 2 + .../dynamic_avoidance.param.yaml | 20 + .../config/scene_module_manager.param.yaml | 7 + ...r_path_planner_dynamic_avoidance_design.md | 22 + .../behavior_path_planner_node.hpp | 5 + .../behavior_path_planner/data_manager.hpp | 1 + .../behavior_path_planner/parameters.hpp | 1 + .../dynamic_avoidance_module.hpp | 116 ++++++ .../dynamic_avoidance/manager.hpp | 53 +++ .../src/behavior_path_planner_node.cpp | 36 ++ .../dynamic_avoidance_module.cpp | 379 ++++++++++++++++++ .../dynamic_avoidance/manager.cpp | 46 +++ ...t_behavior_path_planner_node_interface.cpp | 26 +- 15 files changed, 706 insertions(+), 12 deletions(-) create mode 100644 planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml create mode 100644 planning/behavior_path_planner/docs/behavior_path_planner_dynamic_avoidance_design.md create mode 100644 planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp create mode 100644 planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/manager.hpp create mode 100644 planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp create mode 100644 planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp diff --git a/launch/tier4_planning_launch/launch/planning.launch.xml b/launch/tier4_planning_launch/launch/planning.launch.xml index 128ed0f9cd0ae..df99552ac4fdc 100644 --- a/launch/tier4_planning_launch/launch/planning.launch.xml +++ b/launch/tier4_planning_launch/launch/planning.launch.xml @@ -21,6 +21,7 @@ + diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py index b37c177dd79e6..85cf24e926190 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py @@ -49,6 +49,8 @@ def launch_setup(context, *args, **kwargs): avoidance_param = yaml.safe_load(f)["/**"]["ros__parameters"] with open(LaunchConfiguration("avoidance_by_lc_param_path").perform(context), "r") as f: avoidance_by_lc_param = yaml.safe_load(f)["/**"]["ros__parameters"] + with open(LaunchConfiguration("dynamic_avoidance_param_path").perform(context), "r") as f: + dynamic_avoidance_param = yaml.safe_load(f)["/**"]["ros__parameters"] with open(LaunchConfiguration("lane_change_param_path").perform(context), "r") as f: lane_change_param = yaml.safe_load(f)["/**"]["ros__parameters"] with open(LaunchConfiguration("goal_planner_param_path").perform(context), "r") as f: @@ -90,6 +92,7 @@ def launch_setup(context, *args, **kwargs): side_shift_param, avoidance_param, avoidance_by_lc_param, + dynamic_avoidance_param, lane_change_param, goal_planner_param, pull_out_param, diff --git a/planning/behavior_path_planner/CMakeLists.txt b/planning/behavior_path_planner/CMakeLists.txt index 0c27e5fd835d8..78383f6146414 100644 --- a/planning/behavior_path_planner/CMakeLists.txt +++ b/planning/behavior_path_planner/CMakeLists.txt @@ -15,6 +15,7 @@ set(common_src src/scene_module/scene_module_visitor.cpp src/scene_module/avoidance/avoidance_module.cpp src/scene_module/avoidance_by_lc/module.cpp + src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp src/scene_module/pull_out/pull_out_module.cpp src/scene_module/goal_planner/goal_planner_module.cpp src/scene_module/side_shift/side_shift_module.cpp @@ -66,6 +67,7 @@ else() src/planner_manager.cpp src/scene_module/avoidance/manager.cpp src/scene_module/avoidance_by_lc/manager.cpp + src/scene_module/dynamic_avoidance/manager.cpp src/scene_module/pull_out/manager.cpp src/scene_module/goal_planner/manager.cpp src/scene_module/side_shift/manager.cpp diff --git a/planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml b/planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml new file mode 100644 index 0000000000000..98e2b21b8cc22 --- /dev/null +++ b/planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml @@ -0,0 +1,20 @@ +/**: + ros__parameters: + dynamic_avoidance: + # avoidance is performed for the object type with true + target_object: + car: true + truck: true + bus: true + trailer: true + unknown: false + bicycle: false + motorcycle: true + pedestrian: false + + min_obstacle_vel: 1.0 # [m/s] + + drivable_area_generation: + lat_offset_from_obstacle: 0.8 # [m] + time_to_avoid: 5.0 # [s] + max_lat_offset_to_avoid: 1.0 # [m] diff --git a/planning/behavior_path_planner/config/scene_module_manager.param.yaml b/planning/behavior_path_planner/config/scene_module_manager.param.yaml index f5f1afe805d9b..30626a4df30c4 100644 --- a/planning/behavior_path_planner/config/scene_module_manager.param.yaml +++ b/planning/behavior_path_planner/config/scene_module_manager.param.yaml @@ -65,3 +65,10 @@ enable_simultaneous_execution_as_candidate_module: false priority: 3 max_module_size: 1 + + dynamic_avoidance: + enable_module: false + enable_simultaneous_execution_as_approved_module: true + enable_simultaneous_execution_as_candidate_module: true + priority: 7 + max_module_size: 1 diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_dynamic_avoidance_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_dynamic_avoidance_design.md new file mode 100644 index 0000000000000..c4e458311b093 --- /dev/null +++ b/planning/behavior_path_planner/docs/behavior_path_planner_dynamic_avoidance_design.md @@ -0,0 +1,22 @@ +# Dynamic avoidance design + +## Purpose / Role + +This is a module designed for avoiding obstacles which are running. +Static obstacles such as parked vehicles are dealt with by the avoidance module. + +This module is under development. +In the current implementation, the dynamic obstacles to avoid is extracted from the drivable area. +Then the motion planner, in detail obstacle_avoidance_planner, will generate an avoiding trajectory. + +### Parameters + +| Name | Unit | Type | Description | Default value | +| :------------------------------------------------ | :---- | :----- | :-------------------------------------- | :------------ | +| target_object.car | [-] | bool | The flag whether to avoid cars or not | true | +| target_object.truck | [-] | bool | The flag whether to avoid trucks or not | true | +| ... | [-] | bool | ... | ... | +| target_object.min_obstacle_vel | [m/s] | double | Minimum obstacle velocity to avoid | 1.0 | +| drivable_area_generation.lat_offset_from_obstacle | [m] | double | Lateral offset to avoid from obstacles | 0.8 | +| drivable_area_generation.time_to_avoid | [s] | double | Elapsed time for avoiding an obstacle | 5.0 | +| drivable_area_generation.max_lat_offset_to_avoid | [m] | double | Maximum lateral offset to avoid | 0.5 | diff --git a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp index 7024a78ae274b..be7f4c69f5c5f 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp @@ -21,6 +21,7 @@ #ifdef USE_OLD_ARCHITECTURE #include "behavior_path_planner/behavior_tree_manager.hpp" #include "behavior_path_planner/scene_module/avoidance/avoidance_module.hpp" +#include "behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp" #include "behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp" #include "behavior_path_planner/scene_module/lane_following/lane_following_module.hpp" #include "behavior_path_planner/scene_module/pull_out/pull_out_module.hpp" @@ -29,6 +30,7 @@ #include "behavior_path_planner/planner_manager.hpp" #include "behavior_path_planner/scene_module/avoidance/manager.hpp" #include "behavior_path_planner/scene_module/avoidance_by_lc/manager.hpp" +#include "behavior_path_planner/scene_module/dynamic_avoidance/manager.hpp" #include "behavior_path_planner/scene_module/goal_planner/manager.hpp" #include "behavior_path_planner/scene_module/lane_change/manager.hpp" #include "behavior_path_planner/scene_module/pull_out/manager.hpp" @@ -74,6 +76,7 @@ namespace behavior_path_planner { using autoware_adapi_v1_msgs::msg::OperationModeState; using autoware_auto_mapping_msgs::msg::HADMapBin; +using autoware_auto_perception_msgs::msg::PredictedObject; using autoware_auto_perception_msgs::msg::PredictedObjects; using autoware_auto_planning_msgs::msg::Path; using autoware_auto_planning_msgs::msg::PathWithLaneId; @@ -148,6 +151,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node // parameters std::shared_ptr avoidance_param_ptr_; std::shared_ptr avoidance_by_lc_param_ptr_; + std::shared_ptr dynamic_avoidance_param_ptr_; std::shared_ptr side_shift_param_ptr_; std::shared_ptr lane_change_param_ptr_; std::shared_ptr pull_out_param_ptr_; @@ -160,6 +164,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node #endif AvoidanceParameters getAvoidanceParam(); + DynamicAvoidanceParameters getDynamicAvoidanceParam(); LaneChangeParameters getLaneChangeParam(); SideShiftParameters getSideShiftParam(); GoalPlannerParameters getGoalPlannerParam(); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp index a7ec6eb0df2ca..686a3233ade82 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp @@ -43,6 +43,7 @@ namespace behavior_path_planner { using autoware_adapi_v1_msgs::msg::OperationModeState; +using autoware_auto_perception_msgs::msg::PredictedObject; using autoware_auto_perception_msgs::msg::PredictedObjects; using autoware_auto_planning_msgs::msg::PathWithLaneId; using autoware_auto_vehicle_msgs::msg::HazardLightsCommand; 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 6a9974fb482ef..2e310b2e9dfb7 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp @@ -35,6 +35,7 @@ struct BehaviorPathPlannerParameters ModuleConfigParameters config_avoidance; ModuleConfigParameters config_avoidance_by_lc; + ModuleConfigParameters config_dynamic_avoidance; ModuleConfigParameters config_pull_out; ModuleConfigParameters config_goal_planner; ModuleConfigParameters config_side_shift; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp new file mode 100644 index 0000000000000..50a2d2ce5aa45 --- /dev/null +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp @@ -0,0 +1,116 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__DYNAMIC_AVOIDANCE__DYNAMIC_AVOIDANCE_MODULE_HPP_ +#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__DYNAMIC_AVOIDANCE__DYNAMIC_AVOIDANCE_MODULE_HPP_ + +#include "behavior_path_planner/scene_module/scene_module_interface.hpp" + +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace behavior_path_planner +{ +struct DynamicAvoidanceParameters +{ + // obstacle types to avoid + bool avoid_car{true}; + bool avoid_truck{true}; + bool avoid_bus{true}; + bool avoid_trailer{true}; + bool avoid_unknown{false}; + bool avoid_bicycle{false}; + bool avoid_motorcycle{false}; + bool avoid_pedestrian{false}; + double min_obstacle_vel{0.0}; + + // drivable area generation + double lat_offset_from_obstacle{0.0}; + double time_to_avoid{0.0}; + double max_lat_offset_to_avoid{0.0}; +}; + +class DynamicAvoidanceModule : public SceneModuleInterface +{ +public: + struct DynamicAvoidanceObject + { + explicit DynamicAvoidanceObject( + const PredictedObject & predicted_object, const double arg_path_projected_vel) + : pose(predicted_object.kinematics.initial_pose_with_covariance.pose), + path_projected_vel(arg_path_projected_vel), + shape(predicted_object.shape) + { + } + + geometry_msgs::msg::Pose pose; + double path_projected_vel; + autoware_auto_perception_msgs::msg::Shape shape; + }; + +#ifdef USE_OLD_ARCHITECTURE + DynamicAvoidanceModule( + const std::string & name, rclcpp::Node & node, + std::shared_ptr parameters); +#else + DynamicAvoidanceModule( + const std::string & name, rclcpp::Node & node, + std::shared_ptr parameters, + const std::unordered_map > & rtc_interface_ptr_map); + + void updateModuleParams(const std::shared_ptr & parameters) + { + parameters_ = parameters; + } +#endif + + bool isExecutionRequested() const override; + bool isExecutionReady() const override; + ModuleStatus updateState() override; + ModuleStatus getNodeStatusWhileWaitingApproval() const override { return ModuleStatus::SUCCESS; } + BehaviorModuleOutput plan() override; + CandidateOutput planCandidate() const override; + BehaviorModuleOutput planWaitingApproval() override; + void updateData() override; + void acceptVisitor( + [[maybe_unused]] const std::shared_ptr & visitor) const override + { + } + +private: + std::vector calcTargetObjects() const; + lanelet::ConstLanelets getAdjacentLanes( + const double forward_distance, const double backward_distance) const; + std::optional calcDynamicObstaclePolygon( + const PathWithLaneId & path, const DynamicAvoidanceObject & object) const; + + std::vector target_objects_; + std::shared_ptr parameters_; +}; +} // namespace behavior_path_planner + +#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__DYNAMIC_AVOIDANCE__DYNAMIC_AVOIDANCE_MODULE_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/manager.hpp new file mode 100644 index 0000000000000..51fe30c8dc7ee --- /dev/null +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/manager.hpp @@ -0,0 +1,53 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__DYNAMIC_AVOIDANCE__MANAGER_HPP_ +#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__DYNAMIC_AVOIDANCE__MANAGER_HPP_ + +#include "behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp" +#include "behavior_path_planner/scene_module/scene_module_manager_interface.hpp" + +#include + +#include +#include +#include +#include + +namespace behavior_path_planner +{ + +class DynamicAvoidanceModuleManager : public SceneModuleManagerInterface +{ +public: + DynamicAvoidanceModuleManager( + rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config, + const std::shared_ptr & parameters); + + std::shared_ptr createNewSceneModuleInstance() override + { + return std::make_shared( + name_, *node_, parameters_, rtc_interface_ptr_map_); + } + + void updateModuleParams(const std::vector & parameters) override; + +private: + std::shared_ptr parameters_; + std::vector> registered_modules_; +}; + +} // namespace behavior_path_planner + +#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__DYNAMIC_AVOIDANCE__MANAGER_HPP_ 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 82c0c73f52b12..4f66e46c8d0b1 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -121,6 +121,8 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod // set parameters { avoidance_param_ptr_ = std::make_shared(getAvoidanceParam()); + dynamic_avoidance_param_ptr_ = + std::make_shared(getDynamicAvoidanceParam()); lane_change_param_ptr_ = std::make_shared(getLaneChangeParam()); pull_out_param_ptr_ = std::make_shared(getPullOutParam()); goal_planner_param_ptr_ = std::make_shared(getGoalPlannerParam()); @@ -294,6 +296,12 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod "avoidance_by_lane_change", create_publisher(path_reference_name_space + "avoidance_by_lane_change", 1)); } + + if (p.config_dynamic_avoidance.enable_module) { + auto manager = std::make_shared( + this, "dynamic_avoidance", p.config_dynamic_avoidance, dynamic_avoidance_param_ptr_); + planner_manager_->registerSceneModuleManager(manager); + } } #endif @@ -349,6 +357,7 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() get_scene_module_manager_param("external_request_lane_change_left."); p.config_avoidance = get_scene_module_manager_param("avoidance."); p.config_avoidance_by_lc = get_scene_module_manager_param("avoidance_by_lc."); + p.config_dynamic_avoidance = get_scene_module_manager_param("dynamic_avoidance."); // vehicle info const auto vehicle_info = VehicleInfoUtil(*this).getVehicleInfo(); @@ -647,6 +656,33 @@ AvoidanceParameters BehaviorPathPlannerNode::getAvoidanceParam() return p; } +DynamicAvoidanceParameters BehaviorPathPlannerNode::getDynamicAvoidanceParam() +{ + DynamicAvoidanceParameters p{}; + + { // target object + std::string ns = "dynamic_avoidance.target_object."; + p.avoid_car = declare_parameter(ns + "car"); + p.avoid_truck = declare_parameter(ns + "truck"); + p.avoid_bus = declare_parameter(ns + "bus"); + p.avoid_trailer = declare_parameter(ns + "trailer"); + p.avoid_unknown = declare_parameter(ns + "unknown"); + p.avoid_bicycle = declare_parameter(ns + "bicycle"); + p.avoid_motorcycle = declare_parameter(ns + "motorcycle"); + p.avoid_pedestrian = declare_parameter(ns + "pedestrian"); + p.min_obstacle_vel = declare_parameter(ns + "min_obstacle_vel"); + } + + { // drivable_area_generation + std::string ns = "dynamic_avoidance.drivable_area_generation."; + p.lat_offset_from_obstacle = declare_parameter(ns + "lat_offset_from_obstacle"); + p.time_to_avoid = declare_parameter(ns + "time_to_avoid"); + p.max_lat_offset_to_avoid = declare_parameter(ns + "max_lat_offset_to_avoid"); + } + + return p; +} + LaneChangeParameters BehaviorPathPlannerNode::getLaneChangeParam() { LaneChangeParameters p{}; diff --git a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp new file mode 100644 index 0000000000000..d745b40d4bf39 --- /dev/null +++ b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp @@ -0,0 +1,379 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp" + +#include "behavior_path_planner/utils/path_utils.hpp" +#include "behavior_path_planner/utils/utils.hpp" + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace behavior_path_planner +{ +namespace +{ +bool isCentroidWithinLanelets( + const PredictedObject & object, const lanelet::ConstLanelets & target_lanelets) +{ + if (target_lanelets.empty()) { + return false; + } + + const auto & object_pos = object.kinematics.initial_pose_with_covariance.pose.position; + lanelet::BasicPoint2d object_centroid(object_pos.x, object_pos.y); + + for (const auto & llt : target_lanelets) { + if (boost::geometry::within(object_centroid, llt.polygon2d().basicPolygon())) { + return true; + } + } + + return false; +} + +std::vector getObjectsInLanes( + const std::vector & objects, const lanelet::ConstLanelets & target_lanes) +{ + std::vector target_objects; + for (const auto & object : objects) { + if (isCentroidWithinLanelets(object, target_lanes)) { + target_objects.push_back(object); + } + } + + return target_objects; +} + +geometry_msgs::msg::Point toGeometryPoint(const tier4_autoware_utils::Point2d & point) +{ + geometry_msgs::msg::Point geom_obj_point; + geom_obj_point.x = point.x(); + geom_obj_point.y = point.y(); + return geom_obj_point; +} + +double calcObstacleProjectedVelocity( + const std::vector & path_points, const PredictedObject & object) +{ + const auto & obj_pose = object.kinematics.initial_pose_with_covariance.pose; + const double obj_vel = object.kinematics.initial_twist_with_covariance.twist.linear.x; + + const size_t obj_idx = motion_utils::findNearestIndex(path_points, obj_pose.position); + + const double obj_yaw = tf2::getYaw(obj_pose.orientation); + const double path_yaw = tf2::getYaw(path_points.at(obj_idx).point.pose.orientation); + + return obj_vel * std::cos(obj_yaw - path_yaw); +} + +std::pair getMinMaxValues(const std::vector & vec) +{ + const size_t min_idx = std::distance(vec.begin(), std::min_element(vec.begin(), vec.end())); + + const size_t max_idx = std::distance(vec.begin(), std::max_element(vec.begin(), vec.end())); + + return std::make_pair(vec.at(min_idx), vec.at(max_idx)); +} +} // namespace + +#ifdef USE_OLD_ARCHITECTURE +DynamicAvoidanceModule::DynamicAvoidanceModule( + const std::string & name, rclcpp::Node & node, + std::shared_ptr parameters) +: SceneModuleInterface{name, node, createRTCInterfaceMap(node, name, {""})}, + parameters_{std::move(parameters)} +#else +DynamicAvoidanceModule::DynamicAvoidanceModule( + const std::string & name, rclcpp::Node & node, + std::shared_ptr parameters, + const std::unordered_map > & rtc_interface_ptr_map) +: SceneModuleInterface{name, node, rtc_interface_ptr_map}, parameters_{std::move(parameters)} +#endif +{ +} + +bool DynamicAvoidanceModule::isExecutionRequested() const +{ + RCLCPP_DEBUG(getLogger(), "DYNAMIC AVOIDANCE isExecutionRequested."); + + const auto prev_module_path = getPreviousModuleOutput().path; + if (!prev_module_path || prev_module_path->points.size() < 2) { + return false; + } + + // check if the ego is driving forward + const auto is_driving_forward = motion_utils::isDrivingForward(prev_module_path->points); + if (!is_driving_forward || !(*is_driving_forward)) { + return false; + } + + // check if the planner is already running + if (current_state_ == ModuleStatus::RUNNING) { + return true; + } + + // check if there is target objects to avoid + return !target_objects_.empty(); +} + +bool DynamicAvoidanceModule::isExecutionReady() const +{ + RCLCPP_DEBUG(getLogger(), "DYNAMIC AVOIDANCE isExecutionReady."); + return true; +} + +void DynamicAvoidanceModule::updateData() +{ + target_objects_ = calcTargetObjects(); +} + +ModuleStatus DynamicAvoidanceModule::updateState() +{ + const bool has_avoidance_target = !target_objects_.empty(); + + if (!has_avoidance_target) { + current_state_ = ModuleStatus::SUCCESS; + } else { + current_state_ = ModuleStatus::RUNNING; + } + + return current_state_; +} + +BehaviorModuleOutput DynamicAvoidanceModule::plan() +{ + // 1. get reference path from previous module + const auto prev_module_path = getPreviousModuleOutput().path; + + // 2. get drivable lanes from previous module + const auto drivable_lanes = getPreviousModuleOutput().drivable_area_info.drivable_lanes; + + // 3. create obstacle polygons to avoid + std::vector obstacle_polys; + for (const auto & object : target_objects_) { + const auto obstacle_poly = calcDynamicObstaclePolygon(*prev_module_path, object); + if (obstacle_poly) { + obstacle_polys.push_back(obstacle_poly.value()); + } + } + + BehaviorModuleOutput output; + output.path = prev_module_path; + output.reference_path = getPreviousModuleOutput().reference_path; + // for new architecture + output.drivable_area_info.drivable_lanes = drivable_lanes; + output.drivable_area_info.obstacle_polys = obstacle_polys; + output.turn_signal_info = getPreviousModuleOutput().turn_signal_info; + + return output; +} + +CandidateOutput DynamicAvoidanceModule::planCandidate() const +{ + auto candidate_path = utils::generateCenterLinePath(planner_data_); + return CandidateOutput(*candidate_path); +} + +BehaviorModuleOutput DynamicAvoidanceModule::planWaitingApproval() +{ + BehaviorModuleOutput out = plan(); + return out; +} + +std::vector +DynamicAvoidanceModule::calcTargetObjects() const +{ + const auto prev_module_path = getPreviousModuleOutput().path; + + // 1. calculate target lanes to filter obstacles + const auto target_lanes = getAdjacentLanes(100.0, 10.0); + + // 2. filter obstacles for dynamic avoidance + const auto & predicted_objects = planner_data_->dynamic_object->objects; + const auto target_predicted_objects = getObjectsInLanes(predicted_objects, target_lanes); + + // 3. convert predicted objects to dynamic avoidance objects + std::vector target_avoidance_objects; + for (const auto & predicted_object : target_predicted_objects) { + const double path_projected_vel = + calcObstacleProjectedVelocity(prev_module_path->points, predicted_object); + // check if velocity is high enough + if (std::abs(path_projected_vel) < parameters_->min_obstacle_vel) { + continue; + } + + target_avoidance_objects.push_back( + DynamicAvoidanceObject(predicted_object, path_projected_vel)); + } + + return target_avoidance_objects; +} + +lanelet::ConstLanelets DynamicAvoidanceModule::getAdjacentLanes( + const double forward_distance, const double backward_distance) const +{ + const auto & rh = planner_data_->route_handler; + + lanelet::ConstLanelet current_lane; + if (!rh->getClosestLaneletWithinRoute(getEgoPose(), ¤t_lane)) { + RCLCPP_ERROR( + rclcpp::get_logger("behavior_path_planner").get_child("avoidance"), + "failed to find closest lanelet within route!!!"); + return {}; + } + + const auto ego_succeeding_lanes = + rh->getLaneletSequence(current_lane, getEgoPose(), backward_distance, forward_distance); + + lanelet::ConstLanelets target_lanes; + for (const auto & lane : ego_succeeding_lanes) { + const auto opt_left_lane = rh->getLeftLanelet(lane); + if (opt_left_lane) { + target_lanes.push_back(opt_left_lane.get()); + } + + const auto opt_right_lane = rh->getRightLanelet(lane); + if (opt_right_lane) { + target_lanes.push_back(opt_right_lane.get()); + } + + const auto right_opposite_lanes = rh->getRightOppositeLanelets(lane); + if (!right_opposite_lanes.empty()) { + target_lanes.push_back(right_opposite_lanes.front()); + } + } + + return target_lanes; +} + +std::optional DynamicAvoidanceModule::calcDynamicObstaclePolygon( + const PathWithLaneId & path, const DynamicAvoidanceObject & object) const +{ + auto path_for_bound = path; + + const size_t obj_seg_idx = + motion_utils::findNearestSegmentIndex(path_for_bound.points, object.pose.position); + const double obj_lat_offset = + motion_utils::calcLateralOffset(path_for_bound.points, object.pose.position, obj_seg_idx); + const bool is_left = 0.0 < obj_lat_offset; + const auto obj_points = tier4_autoware_utils::toPolygon2d(object.pose, object.shape); + + // calculate min/max lateral offset from object to path + const auto [min_obj_lat_abs_offset, max_obj_lat_abs_offset] = [&]() { + std::vector obj_lat_abs_offset_vec; + for (size_t i = 0; i < obj_points.outer().size(); ++i) { + const auto geom_obj_point = toGeometryPoint(obj_points.outer().at(i)); + const size_t obj_point_seg_idx = + motion_utils::findNearestSegmentIndex(path_for_bound.points, geom_obj_point); + const double obj_point_lat_offset = + motion_utils::calcLateralOffset(path_for_bound.points, geom_obj_point, obj_point_seg_idx); + obj_lat_abs_offset_vec.push_back(std::abs(obj_point_lat_offset)); + } + return getMinMaxValues(obj_lat_abs_offset_vec); + }(); + const double min_obj_lat_offset = min_obj_lat_abs_offset * (is_left ? 1.0 : -1.0); + const double max_obj_lat_offset = max_obj_lat_abs_offset * (is_left ? 1.0 : -1.0); + + // calculate min/max longitudinal offset from object to path + const auto [min_obj_lon_offset, max_obj_lon_offset] = [&]() { + std::vector obj_lon_offset_vec; + for (size_t i = 0; i < obj_points.outer().size(); ++i) { + const auto geom_obj_point = toGeometryPoint(obj_points.outer().at(i)); + const double lon_offset = motion_utils::calcLongitudinalOffsetToSegment( + path_for_bound.points, obj_seg_idx, geom_obj_point); + obj_lon_offset_vec.push_back(lon_offset); + } + return getMinMaxValues(obj_lon_offset_vec); + }(); + + // calculate bound start and end index + const double length_to_avoid = object.path_projected_vel * parameters_->time_to_avoid; + [[maybe_unused]] const auto lon_bound_start_idx_opt = motion_utils::insertTargetPoint( + obj_seg_idx, min_obj_lon_offset + (length_to_avoid < 0 ? length_to_avoid : 0.0), + path_for_bound.points); + const auto lon_bound_end_idx_opt = motion_utils::insertTargetPoint( + obj_seg_idx, max_obj_lon_offset + (0 < length_to_avoid ? length_to_avoid : 0.0), + path_for_bound.points); + const size_t lon_bound_start_idx = obj_seg_idx; + const size_t lon_bound_end_idx = lon_bound_end_idx_opt + ? lon_bound_end_idx_opt.value() + : static_cast(path_for_bound.points.size() - 1); + + // TODO(murooka) insertTargetPoint does not work well with a negative offset for now. + // When a offset is negative, the bound will be weird. + if ( + lon_bound_start_idx == 0 && + lon_bound_end_idx == static_cast(path_for_bound.points.size() - 1)) { + return std::nullopt; + } + + // calculate bound min and max lateral offset + const double min_bound_lat_offset = [&]() { + const double raw_min_bound_lat_offset = + min_obj_lat_offset - parameters_->lat_offset_from_obstacle * (is_left ? 1.0 : -1.0); + const double min_bound_lat_abs_offset_limit = + planner_data_->parameters.vehicle_width / 2.0 - parameters_->max_lat_offset_to_avoid; + if (is_left) { + if (raw_min_bound_lat_offset < min_bound_lat_abs_offset_limit) { + return min_bound_lat_abs_offset_limit; + } + } else { + if (-min_bound_lat_abs_offset_limit < raw_min_bound_lat_offset) { + return -min_bound_lat_abs_offset_limit; + } + } + return raw_min_bound_lat_offset; + }(); + const double max_bound_lat_offset = + max_obj_lat_offset + parameters_->lat_offset_from_obstacle * (is_left ? 1.0 : -1.0); + + // create inner/outer bound points + std::vector obj_inner_bound_points; + std::vector obj_outer_bound_points; + for (size_t i = lon_bound_start_idx; i <= lon_bound_end_idx; ++i) { + obj_inner_bound_points.push_back( + tier4_autoware_utils::calcOffsetPose( + path_for_bound.points.at(i).point.pose, 0.0, min_bound_lat_offset, 0.0) + .position); + obj_outer_bound_points.push_back( + tier4_autoware_utils::calcOffsetPose( + path_for_bound.points.at(i).point.pose, 0.0, max_bound_lat_offset, 0.0) + .position); + } + + // create obj_polygon from inner/outer bound points + tier4_autoware_utils::Polygon2d obj_poly; + for (const auto & bound_point : obj_inner_bound_points) { + const auto obj_poly_point = tier4_autoware_utils::Point2d(bound_point.x, bound_point.y); + obj_poly.outer().push_back(obj_poly_point); + } + std::reverse(obj_outer_bound_points.begin(), obj_outer_bound_points.end()); + for (const auto & bound_point : obj_outer_bound_points) { + const auto obj_poly_point = tier4_autoware_utils::Point2d(bound_point.x, bound_point.y); + obj_poly.outer().push_back(obj_poly_point); + } + + boost::geometry::correct(obj_poly); + return obj_poly; +} +} // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp new file mode 100644 index 0000000000000..3b7a74515f2dc --- /dev/null +++ b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp @@ -0,0 +1,46 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "behavior_path_planner/scene_module/dynamic_avoidance/manager.hpp" + +#include + +#include +#include +#include + +namespace behavior_path_planner +{ + +DynamicAvoidanceModuleManager::DynamicAvoidanceModuleManager( + rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config, + const std::shared_ptr & parameters) +: SceneModuleManagerInterface(node, name, config, {""}), parameters_{parameters} +{ +} + +void DynamicAvoidanceModuleManager::updateModuleParams( + [[maybe_unused]] const std::vector & parameters) +{ + using tier4_autoware_utils::updateParam; + + auto & p = parameters_; + + [[maybe_unused]] std::string ns = name_ + "."; + + std::for_each(registered_modules_.begin(), registered_modules_.end(), [&p](const auto & m) { + m->updateModuleParams(p); + }); +} +} // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_planner/test/test_behavior_path_planner_node_interface.cpp index eb1e9e6acede8..28eac75a17e0a 100644 --- a/planning/behavior_path_planner/test/test_behavior_path_planner_node_interface.cpp +++ b/planning/behavior_path_planner/test/test_behavior_path_planner_node_interface.cpp @@ -52,18 +52,20 @@ std::shared_ptr generateNode() "bt_tree_config_path", behavior_path_planner_dir + "/config/behavior_path_planner_tree.xml"); test_utils::updateNodeOptions( - node_options, {planning_test_utils_dir + "/config/test_common.param.yaml", - planning_test_utils_dir + "/config/test_nearest_search.param.yaml", - planning_test_utils_dir + "/config/test_vehicle_info.param.yaml", - behavior_path_planner_dir + "/config/behavior_path_planner.param.yaml", - behavior_path_planner_dir + "/config/drivable_area_expansion.param.yaml", - behavior_path_planner_dir + "/config/scene_module_manager.param.yaml", - behavior_path_planner_dir + "/config/avoidance/avoidance.param.yaml", - behavior_path_planner_dir + "/config/lane_change/lane_change.param.yaml", - behavior_path_planner_dir + "/config/pull_out/pull_out.param.yaml", - behavior_path_planner_dir + "/config/goal_planner/goal_planner.param.yaml", - behavior_path_planner_dir + "/config/avoidance_by_lc/avoidance_by_lc.param.yaml", - behavior_path_planner_dir + "/config/side_shift/side_shift.param.yaml"}); + node_options, + {planning_test_utils_dir + "/config/test_common.param.yaml", + planning_test_utils_dir + "/config/test_nearest_search.param.yaml", + planning_test_utils_dir + "/config/test_vehicle_info.param.yaml", + behavior_path_planner_dir + "/config/behavior_path_planner.param.yaml", + behavior_path_planner_dir + "/config/drivable_area_expansion.param.yaml", + behavior_path_planner_dir + "/config/scene_module_manager.param.yaml", + behavior_path_planner_dir + "/config/avoidance/avoidance.param.yaml", + behavior_path_planner_dir + "/config/dynamic_avoidance/dynamic_avoidance.param.yaml", + behavior_path_planner_dir + "/config/lane_change/lane_change.param.yaml", + behavior_path_planner_dir + "/config/pull_out/pull_out.param.yaml", + behavior_path_planner_dir + "/config/goal_planner/goal_planner.param.yaml", + behavior_path_planner_dir + "/config/avoidance_by_lc/avoidance_by_lc.param.yaml", + behavior_path_planner_dir + "/config/side_shift/side_shift.param.yaml"}); return std::make_shared(node_options); } From 8f7173840a53013b330123914f556458615d6412 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Fri, 28 Apr 2023 23:59:10 +0900 Subject: [PATCH 8/9] fix(tier4_planning_rviz_plugin): suppress warning (#3578) Signed-off-by: Takayuki Murooka --- .../src/path/display.cpp | 15 ++++++--------- 1 file changed, 6 insertions(+), 9 deletions(-) diff --git a/common/tier4_planning_rviz_plugin/src/path/display.cpp b/common/tier4_planning_rviz_plugin/src/path/display.cpp index 53a5777acabf1..f1516512c8993 100644 --- a/common/tier4_planning_rviz_plugin/src/path/display.cpp +++ b/common/tier4_planning_rviz_plugin/src/path/display.cpp @@ -33,9 +33,8 @@ void AutowarePathWithLaneIdDisplay::preProcessMessageDetail() VehicleInfoUtil(*rviz_ros_node_.lock()->get_raw_node()).getVehicleInfo()); updateVehicleInfo(); } catch (const std::exception & e) { - RCLCPP_WARN_THROTTLE( - rviz_ros_node_.lock()->get_raw_node()->get_logger(), - *rviz_ros_node_.lock()->get_raw_node()->get_clock(), 5000, "Failed to get vehicle_info: %s", + RCLCPP_WARN_ONCE( + rviz_ros_node_.lock()->get_raw_node()->get_logger(), "Failed to get vehicle_info: %s", e.what()); } } @@ -113,9 +112,8 @@ void AutowarePathDisplay::preProcessMessageDetail() VehicleInfoUtil(*rviz_ros_node_.lock()->get_raw_node()).getVehicleInfo()); updateVehicleInfo(); } catch (const std::exception & e) { - RCLCPP_WARN_THROTTLE( - rviz_ros_node_.lock()->get_raw_node()->get_logger(), - *rviz_ros_node_.lock()->get_raw_node()->get_clock(), 5000, "Failed to get vehicle_info: %s", + RCLCPP_WARN_ONCE( + rviz_ros_node_.lock()->get_raw_node()->get_logger(), "Failed to get vehicle_info: %s", e.what()); } } @@ -131,9 +129,8 @@ void AutowareTrajectoryDisplay::preProcessMessageDetail() VehicleInfoUtil(*rviz_ros_node_.lock()->get_raw_node()).getVehicleInfo()); updateVehicleInfo(); } catch (const std::exception & e) { - RCLCPP_WARN_THROTTLE( - rviz_ros_node_.lock()->get_raw_node()->get_logger(), - *rviz_ros_node_.lock()->get_raw_node()->get_clock(), 5000, "Failed to get vehicle_info: %s", + RCLCPP_WARN_ONCE( + rviz_ros_node_.lock()->get_raw_node()->get_logger(), "Failed to get vehicle_info: %s", e.what()); } } From 279b64a743eb066c9acb55c32b66ad39af4413bf Mon Sep 17 00:00:00 2001 From: Satoshi Tanaka <16330533+scepter914@users.noreply.github.com> Date: Fri, 28 Apr 2023 22:28:08 +0200 Subject: [PATCH 9/9] fix(radar_converter_outputs): add DetectedObjects output (#3450) * fix(radar_tracks_msgs_converter_node): add DetectedObjects output Signed-off-by: scepter914 * fix typo Signed-off-by: scepter914 * fix(radar_tracks_msgs_converter_node): add DetectedObjects output Signed-off-by: scepter914 * fix typo Signed-off-by: scepter914 * update launcher Signed-off-by: scepter914 --------- Signed-off-by: scepter914 --- .../radar_tracks_msgs_converter/README.md | 5 +- .../radar_tracks_msgs_converter_node.hpp | 9 +- .../radar_tracks_msgs_converter.launch.xml | 6 +- .../radar_tracks_msgs_converter_node.cpp | 93 ++++++++++++++++++- 4 files changed, 106 insertions(+), 7 deletions(-) diff --git a/perception/radar_tracks_msgs_converter/README.md b/perception/radar_tracks_msgs_converter/README.md index 79275858ee0ee..a16cd5945801b 100644 --- a/perception/radar_tracks_msgs_converter/README.md +++ b/perception/radar_tracks_msgs_converter/README.md @@ -1,6 +1,6 @@ # radar_tracks_msgs_converter -This package convert from [radar_msgs/msg/RadarTracks](https://github.com/ros-perception/radar_msgs/blob/ros2/msg/RadarTracks.msg) into [autoware_auto_perception_msgs/msg/TrackedObject](https://gitlab.com/autowarefoundation/autoware.auto/autoware_auto_msgs/-/blob/master/autoware_auto_perception_msgs/msg/TrackedObject.idl). +This package convert from [radar_msgs/msg/RadarTracks](https://github.com/ros-perception/radar_msgs/blob/ros2/msg/RadarTracks.msg) into [autoware_auto_perception_msgs/msg/DetectedObject](https://gitlab.com/autowarefoundation/autoware.auto/autoware_auto_msgs/-/blob/master/autoware_auto_perception_msgs/msg/DetectedObject.idl) and [autoware_auto_perception_msgs/msg/TrackedObject](https://gitlab.com/autowarefoundation/autoware.auto/autoware_auto_msgs/-/blob/master/autoware_auto_perception_msgs/msg/TrackedObject.idl). - Calculation cost is O(n). - n: The number of radar objects @@ -13,7 +13,8 @@ This package convert from [radar_msgs/msg/RadarTracks](https://github.com/ros-pe - `~/input/radar_objects` (radar_msgs/msg/RadarTracks.msg): Input radar topic - `~/input/odometry` (nav_msgs/msg/Odometry.msg): Ego vehicle odometry topic - Output - - `~/output/radar_objects` (autoware_auto_perception_msgs/msg/TrackedObject.msg): The topic converted to Autoware's message + - `~/output/radar_detected_objects` (autoware_auto_perception_msgs/msg/DetectedObject.idl): The topic converted to Autoware's message + - `~/output/radar_tracked_objects` (autoware_auto_perception_msgs/msg/TrackedObject.idl): The topic converted to Autoware's message ### Parameters diff --git a/perception/radar_tracks_msgs_converter/include/radar_tracks_msgs_converter/radar_tracks_msgs_converter_node.hpp b/perception/radar_tracks_msgs_converter/include/radar_tracks_msgs_converter/radar_tracks_msgs_converter_node.hpp index a45d7c8fc8c73..d5738c80dcca9 100644 --- a/perception/radar_tracks_msgs_converter/include/radar_tracks_msgs_converter/radar_tracks_msgs_converter_node.hpp +++ b/perception/radar_tracks_msgs_converter/include/radar_tracks_msgs_converter/radar_tracks_msgs_converter_node.hpp @@ -18,6 +18,7 @@ #include "rclcpp/rclcpp.hpp" #include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "autoware_auto_perception_msgs/msg/detected_objects.hpp" #include "autoware_auto_perception_msgs/msg/object_classification.hpp" #include "autoware_auto_perception_msgs/msg/shape.hpp" #include "autoware_auto_perception_msgs/msg/tracked_object.hpp" @@ -33,6 +34,9 @@ namespace radar_tracks_msgs_converter { +using autoware_auto_perception_msgs::msg::DetectedObject; +using autoware_auto_perception_msgs::msg::DetectedObjectKinematics; +using autoware_auto_perception_msgs::msg::DetectedObjects; using autoware_auto_perception_msgs::msg::ObjectClassification; using autoware_auto_perception_msgs::msg::Shape; using autoware_auto_perception_msgs::msg::TrackedObject; @@ -69,7 +73,8 @@ class RadarTracksMsgsConverterNode : public rclcpp::Node geometry_msgs::msg::TransformStamped::ConstSharedPtr transform_; // Publisher - rclcpp::Publisher::SharedPtr pub_radar_{}; + rclcpp::Publisher::SharedPtr pub_tracked_objects_{}; + rclcpp::Publisher::SharedPtr pub_detected_objects_{}; // Timer rclcpp::TimerBase::SharedPtr timer_{}; @@ -86,7 +91,9 @@ class RadarTracksMsgsConverterNode : public rclcpp::Node NodeParam node_param_{}; // Core + geometry_msgs::msg::PoseWithCovariance convertPoseWithCovariance(); TrackedObjects convertRadarTrackToTrackedObjects(); + DetectedObjects convertRadarTrackToDetectedObjects(); uint8_t convertClassification(const uint16_t classification); }; diff --git a/perception/radar_tracks_msgs_converter/launch/radar_tracks_msgs_converter.launch.xml b/perception/radar_tracks_msgs_converter/launch/radar_tracks_msgs_converter.launch.xml index ba4cc4762ea7d..c9f4a31354247 100644 --- a/perception/radar_tracks_msgs_converter/launch/radar_tracks_msgs_converter.launch.xml +++ b/perception/radar_tracks_msgs_converter/launch/radar_tracks_msgs_converter.launch.xml @@ -1,14 +1,16 @@ - + + - + + diff --git a/perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node/radar_tracks_msgs_converter_node.cpp b/perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node/radar_tracks_msgs_converter_node.cpp index cfe6ca3e4cea0..eb4a497e70495 100644 --- a/perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node/radar_tracks_msgs_converter_node.cpp +++ b/perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node/radar_tracks_msgs_converter_node.cpp @@ -88,7 +88,8 @@ RadarTracksMsgsConverterNode::RadarTracksMsgsConverterNode(const rclcpp::NodeOpt transform_listener_ = std::make_shared(this); // Publisher - pub_radar_ = create_publisher("~/output/radar_objects", 1); + pub_tracked_objects_ = create_publisher("~/output/radar_tracked_objects", 1); + pub_detected_objects_ = create_publisher("~/output/radar_detected_objects", 1); // Timer const auto update_period_ns = rclcpp::Rate(node_param_.update_rate_hz).period(); @@ -152,11 +153,99 @@ void RadarTracksMsgsConverterNode::onTimer() node_param_.new_frame_id, header.frame_id, header.stamp, rclcpp::Duration::from_seconds(0.01)); TrackedObjects tracked_objects = convertRadarTrackToTrackedObjects(); + DetectedObjects detected_objects = convertRadarTrackToDetectedObjects(); if (!tracked_objects.objects.empty()) { - pub_radar_->publish(tracked_objects); + pub_tracked_objects_->publish(tracked_objects); + pub_detected_objects_->publish(detected_objects); } } +DetectedObjects RadarTracksMsgsConverterNode::convertRadarTrackToDetectedObjects() +{ + DetectedObjects detected_objects; + detected_objects.header = radar_data_->header; + detected_objects.header.frame_id = node_param_.new_frame_id; + using POSE_IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + using RADAR_IDX = tier4_autoware_utils::xyz_upper_covariance_index::XYZ_UPPER_COV_IDX; + + for (auto & radar_track : radar_data_->tracks) { + DetectedObject detected_object; + + detected_object.existence_probability = 1.0; + + detected_object.shape.type = Shape::BOUNDING_BOX; + detected_object.shape.dimensions = radar_track.size; + + // kinematics + DetectedObjectKinematics kinematics; + kinematics.has_twist = true; + kinematics.has_twist_covariance = true; + + // convert by tf + geometry_msgs::msg::PoseStamped radar_pose_stamped{}; + radar_pose_stamped.pose.position = radar_track.position; + geometry_msgs::msg::PoseStamped transformed_pose_stamped{}; + tf2::doTransform(radar_pose_stamped, transformed_pose_stamped, *transform_); + kinematics.pose_with_covariance.pose = transformed_pose_stamped.pose; + + { + auto & pose_cov = kinematics.pose_with_covariance.covariance; + auto & radar_position_cov = radar_track.position_covariance; + pose_cov[POSE_IDX::X_X] = radar_position_cov[RADAR_IDX::X_X]; + pose_cov[POSE_IDX::X_Y] = radar_position_cov[RADAR_IDX::X_Y]; + pose_cov[POSE_IDX::X_Z] = radar_position_cov[RADAR_IDX::X_Z]; + pose_cov[POSE_IDX::Y_X] = radar_position_cov[RADAR_IDX::X_Y]; + pose_cov[POSE_IDX::Y_Y] = radar_position_cov[RADAR_IDX::Y_Y]; + pose_cov[POSE_IDX::Y_Z] = radar_position_cov[RADAR_IDX::Y_Z]; + pose_cov[POSE_IDX::Z_X] = radar_position_cov[RADAR_IDX::X_Z]; + pose_cov[POSE_IDX::Z_Y] = radar_position_cov[RADAR_IDX::Y_Z]; + pose_cov[POSE_IDX::Z_Z] = radar_position_cov[RADAR_IDX::Z_Z]; + } + + // convert by tf + geometry_msgs::msg::Vector3Stamped radar_velocity_stamped{}; + radar_velocity_stamped.vector = radar_track.velocity; + geometry_msgs::msg::Vector3Stamped transformed_vector3_stamped{}; + tf2::doTransform(radar_velocity_stamped, transformed_vector3_stamped, *transform_); + kinematics.twist_with_covariance.twist.linear = transformed_vector3_stamped.vector; + + // twist compensation + if (node_param_.use_twist_compensation) { + if (odometry_data_) { + kinematics.twist_with_covariance.twist.linear.x += odometry_data_->twist.twist.linear.x; + kinematics.twist_with_covariance.twist.linear.y += odometry_data_->twist.twist.linear.y; + kinematics.twist_with_covariance.twist.linear.z += odometry_data_->twist.twist.linear.z; + } else { + RCLCPP_INFO(get_logger(), "Odometry data is not coming"); + } + } + + { + auto & twist_cov = kinematics.twist_with_covariance.covariance; + auto & radar_vel_cov = radar_track.velocity_covariance; + twist_cov[POSE_IDX::X_X] = radar_vel_cov[RADAR_IDX::X_X]; + twist_cov[POSE_IDX::X_Y] = radar_vel_cov[RADAR_IDX::X_Y]; + twist_cov[POSE_IDX::X_Z] = radar_vel_cov[RADAR_IDX::X_Z]; + twist_cov[POSE_IDX::Y_X] = radar_vel_cov[RADAR_IDX::X_Y]; + twist_cov[POSE_IDX::Y_Y] = radar_vel_cov[RADAR_IDX::Y_Y]; + twist_cov[POSE_IDX::Y_Z] = radar_vel_cov[RADAR_IDX::Y_Z]; + twist_cov[POSE_IDX::Z_X] = radar_vel_cov[RADAR_IDX::X_Z]; + twist_cov[POSE_IDX::Z_Y] = radar_vel_cov[RADAR_IDX::Y_Z]; + twist_cov[POSE_IDX::Z_Z] = radar_vel_cov[RADAR_IDX::Z_Z]; + } + detected_object.kinematics = kinematics; + + // classification + ObjectClassification classification; + classification.probability = 1.0; + classification.label = convertClassification(radar_track.classification); + detected_object.classification.emplace_back(classification); + + detected_objects.objects.emplace_back(detected_object); + } + return detected_objects; +} + TrackedObjects RadarTracksMsgsConverterNode::convertRadarTrackToTrackedObjects() { TrackedObjects tracked_objects;