From 9ac8faf5291337e072e9f22679dead91a9a26be8 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Wed, 9 Aug 2023 03:56:29 +0900 Subject: [PATCH] fix(dynamic_avoidance): minor changes with bug fix (#4567) * feat(dynamic_avoidance): use hatched road markings Signed-off-by: Takayuki Murooka * add some ros parameters Signed-off-by: Takayuki Murooka * update config Signed-off-by: Takayuki Murooka * remove comment Signed-off-by: Takayuki Murooka * feat(dynamic_avoidance): add minor changes Signed-off-by: Takayuki Murooka * fix Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka --- .../dynamic_avoidance_module.hpp | 7 ++- .../dynamic_avoidance_module.cpp | 61 ++++++------------- 2 files changed, 21 insertions(+), 47 deletions(-) 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 index de41fe9359bc2..1f54786df83d7 100644 --- 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 @@ -120,8 +120,10 @@ class DynamicAvoidanceModule : public SceneModuleInterface std::optional latest_time_inside_ego_path{std::nullopt}; std::vector predicted_paths{}; - MinMaxValue lon_offset_to_avoid; - MinMaxValue lat_offset_to_avoid; + // NOTE: Previous values of the following are used for low-pass filtering. + // Therefore, they has to be initialized as nullopt. + std::optional lon_offset_to_avoid{std::nullopt}; + std::optional lat_offset_to_avoid{std::nullopt}; bool is_collision_left; bool should_be_avoided{false}; @@ -306,7 +308,6 @@ class DynamicAvoidanceModule : public SceneModuleInterface bool isLabelTargetObstacle(const uint8_t label) const; void updateTargetObjects(); - std::optional> calcPathForObjectPolygon() const; bool willObjectCutIn( const std::vector & ego_path, const PredictedPath & predicted_path, const double obj_tangent_vel, const LatLonOffset & lat_lon_offset) const; 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 index b5ed7cc217a56..7f69397cb3cf5 100644 --- 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 @@ -68,7 +68,7 @@ void appendExtractedPolygonMarker( auto marker = tier4_autoware_utils::createDefaultMarker( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "extracted_polygons", marker_array.markers.size(), visualization_msgs::msg::Marker::LINE_STRIP, - tier4_autoware_utils::createMarkerScale(0.05, 0.0, 0.0), + tier4_autoware_utils::createMarkerScale(0.1, 0.0, 0.0), tier4_autoware_utils::createMarkerColor(1.0, 0.5, 0.6, 0.8)); // NOTE: obj_poly.outer() has already duplicated points to close the polygon. @@ -349,11 +349,7 @@ void DynamicAvoidanceModule::updateTargetObjects() const auto prev_module_path = getPreviousModuleOutput().path; const auto & predicted_objects = planner_data_->dynamic_object->objects; - const auto path_points_for_object_polygon = calcPathForObjectPolygon(); - if (!path_points_for_object_polygon) { - return; - } - + const auto path_points_for_object_polygon = getPreviousModuleOutput().reference_path->points; const auto prev_objects = target_objects_manager_.getValidObjects(); // 1. Rough filtering of target objects @@ -509,9 +505,9 @@ void DynamicAvoidanceModule::updateTargetObjects() // 2.g. calculate longitudinal and lateral offset to avoid const auto obj_points = tier4_autoware_utils::toPolygon2d(object.pose, object.shape); const auto lon_offset_to_avoid = calcMinMaxLongitudinalOffsetToAvoid( - *path_points_for_object_polygon, object.pose, obj_points, object.vel, time_to_collision); + path_points_for_object_polygon, object.pose, obj_points, object.vel, time_to_collision); const auto lat_offset_to_avoid = calcMinMaxLateralOffsetToAvoid( - *path_points_for_object_polygon, obj_points, is_collision_left, object.lat_vel, prev_object); + path_points_for_object_polygon, obj_points, is_collision_left, object.lat_vel, prev_object); const bool should_be_avoided = true; target_objects_manager_.updateObject( @@ -519,30 +515,6 @@ void DynamicAvoidanceModule::updateTargetObjects() } } -std::optional> DynamicAvoidanceModule::calcPathForObjectPolygon() - const -{ - const auto & ego_pose = getEgoPose(); - const auto & rh = planner_data_->route_handler; - - // get path with backward margin - lanelet::ConstLanelet current_lane; - if (!rh->getClosestLaneletWithinRoute(ego_pose, ¤t_lane)) { - RCLCPP_ERROR( - rclcpp::get_logger("behavior_path_planner").get_child("dynamic_avoidance"), - "failed to find closest lanelet within route!!!"); - return std::nullopt; - } - - constexpr double forward_length = 100.0; - const double backward_length = 50.0; - const auto current_lanes = - rh->getLaneletSequence(current_lane, ego_pose, backward_length, forward_length); - const auto path = utils::getCenterLinePath( - *rh, current_lanes, ego_pose, backward_length, forward_length, planner_data_->parameters); - return path.points; -} - [[maybe_unused]] std::optional> DynamicAvoidanceModule::calcCollisionSection( const std::vector & ego_path, const PredictedPath & obj_path) const @@ -849,10 +821,10 @@ MinMaxValue DynamicAvoidanceModule::calcMinMaxLateralOffsetToAvoid( // filter min_bound_lat_offset const auto prev_min_lat_avoid_to_offset = [&]() -> std::optional { - if (!prev_object) { + if (!prev_object || !prev_object->lat_offset_to_avoid) { return std::nullopt; } - return prev_object->lat_offset_to_avoid.min_value; + return prev_object->lat_offset_to_avoid->min_value; }(); const double filtered_min_bound_lat_offset = prev_min_lat_avoid_to_offset ? signal_processing::lowpassFilter( @@ -867,22 +839,23 @@ MinMaxValue DynamicAvoidanceModule::calcMinMaxLateralOffsetToAvoid( std::optional DynamicAvoidanceModule::calcDynamicObstaclePolygon( const DynamicAvoidanceObject & object) const { - auto path_points_for_object_polygon = calcPathForObjectPolygon(); - if (!path_points_for_object_polygon) { + if (!object.lon_offset_to_avoid || !object.lat_offset_to_avoid) { return std::nullopt; } + auto path_points_for_object_polygon = getPreviousModuleOutput().reference_path->points; + const size_t obj_seg_idx = - motion_utils::findNearestSegmentIndex(*path_points_for_object_polygon, object.pose.position); + motion_utils::findNearestSegmentIndex(path_points_for_object_polygon, object.pose.position); const auto obj_points = tier4_autoware_utils::toPolygon2d(object.pose, object.shape); const auto lon_bound_start_idx_opt = motion_utils::insertTargetPoint( - obj_seg_idx, object.lon_offset_to_avoid.min_value, *path_points_for_object_polygon); + obj_seg_idx, object.lon_offset_to_avoid->min_value, path_points_for_object_polygon); const size_t updated_obj_seg_idx = (lon_bound_start_idx_opt && lon_bound_start_idx_opt.value() <= obj_seg_idx) ? obj_seg_idx + 1 : obj_seg_idx; const auto lon_bound_end_idx_opt = motion_utils::insertTargetPoint( - updated_obj_seg_idx, object.lon_offset_to_avoid.max_value, *path_points_for_object_polygon); + updated_obj_seg_idx, object.lon_offset_to_avoid->max_value, path_points_for_object_polygon); if (!lon_bound_start_idx_opt && !lon_bound_end_idx_opt) { // NOTE: The obstacle is longitudinally out of the ego's trajectory. @@ -892,19 +865,19 @@ std::optional DynamicAvoidanceModule::calcDynam lon_bound_start_idx_opt ? lon_bound_start_idx_opt.value() : static_cast(0); const size_t lon_bound_end_idx = lon_bound_end_idx_opt ? lon_bound_end_idx_opt.value() - : static_cast(path_points_for_object_polygon->size() - 1); + : static_cast(path_points_for_object_polygon.size() - 1); // 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_points_for_object_polygon->at(i).point.pose, 0.0, - object.lat_offset_to_avoid.min_value, 0.0) + path_points_for_object_polygon.at(i).point.pose, 0.0, + object.lat_offset_to_avoid->min_value, 0.0) .position); obj_outer_bound_points.push_back(tier4_autoware_utils::calcOffsetPose( - path_points_for_object_polygon->at(i).point.pose, 0.0, - object.lat_offset_to_avoid.max_value, 0.0) + path_points_for_object_polygon.at(i).point.pose, 0.0, + object.lat_offset_to_avoid->max_value, 0.0) .position); }