diff --git a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp index efd158fd9f2a0..a9db9f51faa4b 100644 --- a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp +++ b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp @@ -1107,17 +1107,27 @@ inline boost::optional insertTargetPoint( { validateNonEmpty(points); - if (insert_point_length < 0.0 || src_segment_idx >= points.size() - 1) { + if (src_segment_idx >= points.size() - 1) { return boost::none; } // Get Nearest segment index boost::optional segment_idx = boost::none; - for (size_t i = src_segment_idx + 1; i < points.size(); ++i) { - const double length = calcSignedArcLength(points, src_segment_idx, i); - if (insert_point_length <= length) { - segment_idx = i - 1; - break; + if (0.0 <= insert_point_length) { + for (size_t i = src_segment_idx + 1; i < points.size(); ++i) { + const double length = calcSignedArcLength(points, src_segment_idx, i); + if (insert_point_length <= length) { + segment_idx = i - 1; + break; + } + } + } else { + for (int i = src_segment_idx - 1; 0 <= i; --i) { + const double length = calcSignedArcLength(points, src_segment_idx, i); + if (length <= insert_point_length) { + segment_idx = i; + break; + } } } @@ -1128,7 +1138,7 @@ inline boost::optional insertTargetPoint( // Get Target Point const double segment_length = calcSignedArcLength(points, *segment_idx, *segment_idx + 1); const double target_length = - std::max(0.0, insert_point_length - calcSignedArcLength(points, src_segment_idx, *segment_idx)); + insert_point_length - calcSignedArcLength(points, src_segment_idx, *segment_idx); const double ratio = std::clamp(target_length / segment_length, 0.0, 1.0); const auto p_target = tier4_autoware_utils::calcInterpolatedPoint( tier4_autoware_utils::getPoint(points.at(*segment_idx)), diff --git a/common/motion_utils/test/src/trajectory/test_trajectory.cpp b/common/motion_utils/test/src/trajectory/test_trajectory.cpp index a1e0ec0768c80..4b2b6118c74ac 100644 --- a/common/motion_utils/test/src/trajectory/test_trajectory.cpp +++ b/common/motion_utils/test/src/trajectory/test_trajectory.cpp @@ -2750,6 +2750,193 @@ TEST(trajectory, insertTargetPoint_Length_Without_Target_Point_Non_Zero_Start_Id } } +TEST(trajectory, insertTargetPoint_Negative_Length_Without_Target_Point_Non_Zero_Start_Idx) +{ + using motion_utils::calcArcLength; + using motion_utils::findNearestSegmentIndex; + using motion_utils::insertTargetPoint; + using tier4_autoware_utils::calcDistance2d; + using tier4_autoware_utils::createPoint; + using tier4_autoware_utils::deg2rad; + using tier4_autoware_utils::getPose; + + const auto traj = generateTestTrajectory(10, 1.0); + + // Insert + for (double x_start = -0.5; x_start < -5.0; x_start -= 1.0) { + auto traj_out = traj; + + const size_t start_idx = 7; + const auto p_target = createPoint(7.0 + x_start, 0.0, 0.0); + const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); + const auto insert_idx = insertTargetPoint(start_idx, x_start, traj_out.points); + + EXPECT_NE(insert_idx, boost::none); + EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); + + for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { + EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + } + + { + const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_EQ(p_insert.position.x, p_target.x); + EXPECT_EQ(p_insert.position.y, p_target.y); + EXPECT_EQ(p_insert.position.z, p_target.z); + EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); + } + + { + const auto p_base = getPose(traj_out.points.at(base_idx)); + const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); + } + } + + // Boundary Condition(Before End Point) + { + auto traj_out = traj; + const double x_start = -1.0 - 1e-2; + + const size_t start_idx = 8; + const auto p_target = createPoint(7.0 - 1e-2, 0.0, 0.0); + const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); + const auto insert_idx = insertTargetPoint(start_idx, x_start, traj_out.points); + + EXPECT_NE(insert_idx, boost::none); + EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); + + for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { + EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + } + + { + const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_EQ(p_insert.position.x, p_target.x); + EXPECT_EQ(p_insert.position.y, p_target.y); + EXPECT_EQ(p_insert.position.z, p_target.z); + EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); + } + + { + const auto p_base = getPose(traj_out.points.at(base_idx)); + const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); + } + } + + // Boundary Condition(Right on End Point) + { + auto traj_out = traj; + const double x_start = -1.0; + + const size_t start_idx = 8; + const auto p_target = createPoint(7.0, 0.0, 0.0); + const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); + const auto insert_idx = insertTargetPoint(start_idx, x_start, traj_out.points); + + EXPECT_NE(insert_idx, boost::none); + EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_EQ(traj_out.points.size(), traj.points.size()); + + for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { + EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + } + + { + const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_EQ(p_insert.position.x, p_target.x); + EXPECT_EQ(p_insert.position.y, p_target.y); + EXPECT_EQ(p_insert.position.z, p_target.z); + EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); + } + + { + const auto p_base = getPose(traj_out.points.at(base_idx)); + const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); + } + } + + // Boundary Condition(start point) + { + auto traj_out = traj; + const double x_start = -5.0; + + const size_t start_idx = 5; + const auto p_target = createPoint(0.0, 0.0, 0.0); + const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); + const auto insert_idx = insertTargetPoint(start_idx, x_start, traj_out.points); + + EXPECT_NE(insert_idx, boost::none); + EXPECT_EQ(insert_idx.get(), base_idx); + EXPECT_EQ(traj_out.points.size(), traj.points.size()); + + for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { + EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + } + + { + const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_EQ(p_insert.position.x, p_target.x); + EXPECT_EQ(p_insert.position.y, p_target.y); + EXPECT_EQ(p_insert.position.z, p_target.z); + EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); + } + + { + const auto p_base = getPose(traj_out.points.at(base_idx)); + const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); + } + } + + // No Insert (Index Out of range) + { + auto traj_out = traj; + EXPECT_EQ(insertTargetPoint(0, -1.0, traj_out.points), boost::none); + EXPECT_EQ(insertTargetPoint(0, -1.0, traj_out.points), boost::none); + } + + // No Insert (Length out of range) + { + auto traj_out = traj; + EXPECT_EQ(insertTargetPoint(9, -10.0, traj_out.points), boost::none); + EXPECT_EQ(insertTargetPoint(9, -9.0001, traj_out.points), boost::none); + EXPECT_EQ(insertTargetPoint(1, -1.0001, traj_out.points), boost::none); + } +} + TEST(trajectory, insertTargetPoint_Length_from_a_pose) { using motion_utils::calcArcLength; diff --git a/planning/behavior_path_planner/CMakeLists.txt b/planning/behavior_path_planner/CMakeLists.txt index 78383f6146414..8cad60065540b 100644 --- a/planning/behavior_path_planner/CMakeLists.txt +++ b/planning/behavior_path_planner/CMakeLists.txt @@ -109,6 +109,14 @@ if(BUILD_TESTING) behavior_path_planner_node ) + ament_add_ros_isolated_gmock(test_${CMAKE_PROJECT_NAME}_lane_change_module + test/test_lane_change_utils.cpp + ) + + target_link_libraries(test_${CMAKE_PROJECT_NAME}_lane_change_module + behavior_path_planner_node + ) + ament_add_ros_isolated_gmock(test_${CMAKE_PROJECT_NAME}_turn_signal test/test_turn_signal.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 index 98e2b21b8cc22..c26cfaa208277 100644 --- a/planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml +++ b/planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml @@ -15,6 +15,7 @@ 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] + lat_offset_from_obstacle: 2.0 # [m] + time_to_avoid_same_directional_object: 5.0 # [s] + time_to_avoid_opposite_directional_object: 6.0 # [s] + max_lat_offset_to_avoid: 2.0 # [m] 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 index c4e458311b093..b428eadf3e7fd 100644 --- 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 @@ -11,12 +11,13 @@ Then the motion planner, in detail obstacle_avoidance_planner, will generate an ### 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 | +| 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_same_directional_object | [s] | double | Elapsed time for avoiding the same directional obstacle | 5.0 | +| drivable_area_generation.time_to_avoid_opposite_directional_object | [s] | double | Elapsed time for avoiding the opposite directional obstacle | 6.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/data_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp index 686a3233ade82..601612b11a570 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 @@ -81,8 +81,13 @@ struct DrivableLanes // quite messy. Needs to be refactored. struct DrivableAreaInfo { + struct Obstacle + { + geometry_msgs::msg::Pose pose; + tier4_autoware_utils::Polygon2d poly; + }; std::vector drivable_lanes; - std::vector obstacle_polys; + std::vector obstacles; // obstacles to extract from the drivable area // temporary only for pull over's freespace planning double drivable_margin{0.0}; 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 50a2d2ce5aa45..744be5348111d 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 @@ -50,7 +50,8 @@ struct DynamicAvoidanceParameters // drivable area generation double lat_offset_from_obstacle{0.0}; - double time_to_avoid{0.0}; + double time_to_avoid_same_directional_object{0.0}; + double time_to_avoid_opposite_directional_object{0.0}; double max_lat_offset_to_avoid{0.0}; }; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp index 5dde792453cbe..7873e39dc8de9 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp @@ -65,7 +65,7 @@ void setStartData( Polygon2d createEnvelopePolygon( const ObjectData & object_data, const Pose & closest_pose, const double envelope_buffer); -std::vector generateObstaclePolygonsForDrivableArea( +std::vector generateObstaclePolygonsForDrivableArea( const ObjectDataArray & objects, const std::shared_ptr & parameters, const double vehicle_width); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp index ea952fdc7e209..cd005a7cd335d 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp @@ -377,7 +377,7 @@ std::vector combineDrivableLanes( const std::vector & new_drivable_lanes_vec); void extractObstaclesFromDrivableArea( - PathWithLaneId & path, const std::vector & obj_polys); + PathWithLaneId & path, const std::vector & obstacles); } // namespace behavior_path_planner::utils #endif // BEHAVIOR_PATH_PLANNER__UTILS__UTILS_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 4f66e46c8d0b1..8a98a6f4b84ae 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -676,7 +676,10 @@ DynamicAvoidanceParameters BehaviorPathPlannerNode::getDynamicAvoidanceParam() { // 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.time_to_avoid_same_directional_object = + declare_parameter(ns + "time_to_avoid_same_directional_object"); + p.time_to_avoid_opposite_directional_object = + declare_parameter(ns + "time_to_avoid_opposite_directional_object"); p.max_lat_offset_to_avoid = declare_parameter(ns + "max_lat_offset_to_avoid"); } @@ -1490,12 +1493,17 @@ SetParametersResult BehaviorPathPlannerNode::onSetParam( return result; } +#ifndef USE_OLD_ARCHITECTURE + { + const std::lock_guard lock(mutex_manager_); // for planner_manager_ + planner_manager_->updateModuleParams(parameters); + } +#endif + result.successful = true; result.reason = "success"; try { - updateParam( - parameters, "avoidance.publish_debug_marker", avoidance_param_ptr_->publish_debug_marker); updateParam( parameters, "lane_change.publish_debug_marker", lane_change_param_ptr_->publish_debug_marker); // Drivable area expansion parameters diff --git a/planning/behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/src/planner_manager.cpp index feb8c08320624..e396aa7846132 100644 --- a/planning/behavior_path_planner/src/planner_manager.cpp +++ b/planning/behavior_path_planner/src/planner_manager.cpp @@ -136,7 +136,7 @@ void PlannerManager::generateCombinedDrivableArea( } // extract obstacles from drivable area - utils::extractObstaclesFromDrivableArea(*output.path, output.drivable_area_info.obstacle_polys); + utils::extractObstaclesFromDrivableArea(*output.path, output.drivable_area_info.obstacles); } std::vector PlannerManager::getRequestModules( 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 a8e09dae9e703..4420525bfa14e 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 @@ -2532,9 +2532,8 @@ void AvoidanceModule::generateExtendedDrivableArea(BehaviorModuleOutput & output output.drivable_area_info.drivable_lanes = utils::combineDrivableLanes( getPreviousModuleOutput().drivable_area_info.drivable_lanes, drivable_lanes); // generate obstacle polygons - output.drivable_area_info.obstacle_polys = - utils::avoidance::generateObstaclePolygonsForDrivableArea( - avoidance_data_.target_objects, parameters_, planner_data_->parameters.vehicle_width / 2.0); + output.drivable_area_info.obstacles = utils::avoidance::generateObstaclePolygonsForDrivableArea( + avoidance_data_.target_objects, parameters_, planner_data_->parameters.vehicle_width / 2.0); } { // for old architecture diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp index fba08bd39c8d5..993219ebfbb62 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp @@ -32,14 +32,68 @@ AvoidanceModuleManager::AvoidanceModuleManager( void AvoidanceModuleManager::updateModuleParams(const std::vector & parameters) { + using autoware_auto_perception_msgs::msg::ObjectClassification; using tier4_autoware_utils::updateParam; auto p = parameters_; - std::string ns = "avoidance."; - updateParam(parameters, ns + "enable_safety_check", p->enable_safety_check); - updateParam(parameters, ns + "publish_debug_marker", p->publish_debug_marker); - updateParam(parameters, ns + "print_debug_info", p->print_debug_info); + { + const std::string ns = "avoidance."; + updateParam(parameters, ns + "enable_safety_check", p->enable_safety_check); + updateParam(parameters, ns + "publish_debug_marker", p->publish_debug_marker); + updateParam(parameters, ns + "print_debug_info", p->print_debug_info); + } + + const auto update_object_param = [&p, ¶meters]( + const auto & semantic, const std::string & ns) { + auto & config = p->object_parameters.at(semantic); + updateParam(parameters, ns + "enable", config.enable); + updateParam(parameters, ns + "envelope_buffer_margin", config.envelope_buffer_margin); + updateParam(parameters, ns + "safety_buffer_lateral", config.safety_buffer_lateral); + updateParam( + parameters, ns + "safety_buffer_longitudinal", config.safety_buffer_longitudinal); + }; + + { + const std::string ns = "avoidance.target_object."; + update_object_param(ObjectClassification::MOTORCYCLE, ns + "motorcycle."); + update_object_param(ObjectClassification::CAR, ns + "car."); + update_object_param(ObjectClassification::TRUCK, ns + "truck."); + update_object_param(ObjectClassification::TRAILER, ns + "trailer."); + update_object_param(ObjectClassification::BUS, ns + "bus."); + update_object_param(ObjectClassification::PEDESTRIAN, ns + "pedestrian."); + update_object_param(ObjectClassification::BICYCLE, ns + "bicycle."); + update_object_param(ObjectClassification::UNKNOWN, ns + "unknown."); + } + + { + const std::string ns = "avoidance.avoidance.lateral."; + updateParam( + parameters, ns + "avoidance_execution_lateral_threshold", + p->avoidance_execution_lateral_threshold); + updateParam( + parameters, ns + "lateral_passable_safety_buffer", p->lateral_passable_safety_buffer); + updateParam(parameters, ns + "lateral_collision_margin", p->lateral_collision_margin); + updateParam( + parameters, ns + "road_shoulder_safety_margin", p->road_shoulder_safety_margin); + } + + { + const std::string ns = "avoidance.avoidance.longitudinal."; + updateParam(parameters, ns + "prepare_time", p->prepare_time); + } + + { + const std::string ns = "avoidance.stop."; + updateParam(parameters, ns + "max_distance", p->stop_max_distance); + updateParam(parameters, ns + "min_distance", p->stop_min_distance); + } + + { + const std::string ns = "avoidance.constrains.lateral."; + updateParam(parameters, ns + "nominal_lateral_jerk", p->nominal_lateral_jerk); + updateParam(parameters, ns + "max_lateral_jerk", p->max_lateral_jerk); + } std::for_each(registered_modules_.begin(), registered_modules_.end(), [&p](const auto & m) { m->updateModuleParams(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 index d745b40d4bf39..46d2e73bbb44a 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 @@ -168,12 +168,12 @@ BehaviorModuleOutput DynamicAvoidanceModule::plan() // 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; + // 3. create obstacles to avoid (= extract from the drivable area) + std::vector obstacles_for_drivable_area; 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()); + obstacles_for_drivable_area.push_back({object.pose, obstacle_poly.value()}); } } @@ -182,7 +182,7 @@ BehaviorModuleOutput DynamicAvoidanceModule::plan() 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.drivable_area_info.obstacles = obstacles_for_drivable_area; output.turn_signal_info = getPreviousModuleOutput().turn_signal_info; return output; @@ -307,26 +307,28 @@ std::optional DynamicAvoidanceModule::calcDynam }(); // 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( + const double length_to_avoid = [&]() { + if (0.0 <= object.path_projected_vel) { + return object.path_projected_vel * parameters_->time_to_avoid_same_directional_object; + } + return object.path_projected_vel * parameters_->time_to_avoid_opposite_directional_object; + }(); + 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), + 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; + if (!lon_bound_start_idx_opt && !lon_bound_end_idx_opt) { + // NOTE: The obstacle is longitudinally out of the ego's trajectory. + return std::nullopt; + } + const size_t lon_bound_start_idx = + 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_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 = diff --git a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp index ce601330727af..7e05946d7d29f 100644 --- a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp +++ b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp @@ -270,14 +270,14 @@ Polygon2d createEnvelopePolygon( return expanded_polygon; } -std::vector generateObstaclePolygonsForDrivableArea( +std::vector generateObstaclePolygonsForDrivableArea( const ObjectDataArray & objects, const std::shared_ptr & parameters, const double vehicle_width) { - std::vector obj_polys; + std::vector obstacles_for_drivable_area; if (objects.empty() || !parameters->enable_bound_clipping) { - return obj_polys; + return obstacles_for_drivable_area; } for (const auto & object : objects) { @@ -302,9 +302,10 @@ std::vector generateObstaclePolygonsForDrivable object.avoid_margin.get() - object_parameter.envelope_buffer_margin - vehicle_width / 2.0; const auto obj_poly = tier4_autoware_utils::expandPolygon(object.envelope_poly, diff_poly_buffer); - obj_polys.push_back(obj_poly); + obstacles_for_drivable_area.push_back( + {object.object.kinematics.initial_pose_with_covariance.pose, obj_poly}); } - return obj_polys; + return obstacles_for_drivable_area; } double getLongitudinalVelocity(const Pose & p_ref, const Pose & p_target, const double v) diff --git a/planning/behavior_path_planner/src/utils/safety_check.cpp b/planning/behavior_path_planner/src/utils/safety_check.cpp index 82230e93bb732..dd5db1ad44d96 100644 --- a/planning/behavior_path_planner/src/utils/safety_check.cpp +++ b/planning/behavior_path_planner/src/utils/safety_check.cpp @@ -23,24 +23,18 @@ ProjectedDistancePoint pointToSegment( const Point2d & reference_point, const Point2d & polygon_segment_start, const Point2d & polygon_segment_end) { - auto copied_point_from_object = polygon_segment_end; - auto copied_point_from_reference = reference_point; - bg::subtract_point(copied_point_from_object, polygon_segment_start); - bg::subtract_point(copied_point_from_reference, polygon_segment_start); - - const auto c1 = bg::dot_product(copied_point_from_reference, copied_point_from_object); - if (!(c1 > 0)) { - return {polygon_segment_start, Pythagoras::apply(reference_point, polygon_segment_start)}; - } + auto segment_vec = polygon_segment_end; + auto segment_start_to_reference_vec = reference_point; + bg::subtract_point(segment_vec, polygon_segment_start); + bg::subtract_point(segment_start_to_reference_vec, polygon_segment_start); - const auto c2 = bg::dot_product(copied_point_from_object, copied_point_from_object); - if (!(c2 > c1)) { - return {polygon_segment_end, Pythagoras::apply(reference_point, polygon_segment_end)}; - } + const auto c1 = bg::dot_product(segment_vec, segment_start_to_reference_vec); + const auto c2 = bg::dot_product(segment_vec, segment_vec); + const auto ratio = std::clamp(c1 / c2, 0.0, 1.0); Point2d projected = polygon_segment_start; - bg::multiply_value(copied_point_from_object, c1 / c2); - bg::add_point(projected, copied_point_from_object); + bg::multiply_value(segment_vec, ratio); + bg::add_point(projected, segment_vec); return {projected, Pythagoras::apply(reference_point, projected)}; } diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index de9dba542e409..647715aa1541c 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -427,7 +427,7 @@ std::vector updateBoundary( return updated_bound; } -geometry_msgs::msg::Point calcCenterOfGeometry(const Polygon2d & obj_poly) +[[maybe_unused]] geometry_msgs::msg::Point calcCenterOfGeometry(const Polygon2d & obj_poly) { geometry_msgs::msg::Point center_pos; for (const auto & point : obj_poly.outer()) { @@ -2466,25 +2466,25 @@ std::vector combineDrivableLanes( // NOTE: Assuming that path.right/left_bound is already created. void extractObstaclesFromDrivableArea( - PathWithLaneId & path, const std::vector & obj_polys) + PathWithLaneId & path, const std::vector & obstacles) { - if (obj_polys.empty()) { + if (obstacles.empty()) { return; } std::vector> right_polygons; std::vector> left_polygons; - for (const auto & obj_poly : obj_polys) { - const auto obj_pos = drivable_area_processing::calcCenterOfGeometry(obj_poly); + for (const auto & obstacle : obstacles) { + const auto & obj_pos = obstacle.pose.position; // get edge points of the object const size_t nearest_path_idx = motion_utils::findNearestIndex(path.points, obj_pos); // to get z for object polygon std::vector edge_points; - for (size_t i = 0; i < obj_poly.outer().size() - 1; + for (size_t i = 0; i < obstacle.poly.outer().size() - 1; ++i) { // NOTE: There is a duplicated points edge_points.push_back(tier4_autoware_utils::createPoint( - obj_poly.outer().at(i).x(), obj_poly.outer().at(i).y(), + obstacle.poly.outer().at(i).x(), obstacle.poly.outer().at(i).y(), path.points.at(nearest_path_idx).point.pose.position.z)); } diff --git a/planning/behavior_path_planner/test/test_lane_change_utils.cpp b/planning/behavior_path_planner/test/test_lane_change_utils.cpp index d126fc0607a71..c3ca024a80ba6 100644 --- a/planning/behavior_path_planner/test/test_lane_change_utils.cpp +++ b/planning/behavior_path_planner/test/test_lane_change_utils.cpp @@ -11,27 +11,29 @@ // 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/lane_change/utils.hpp" +#include "behavior_path_planner/utils/safety_check.hpp" +#include "tier4_autoware_utils/tier4_autoware_utils.hpp" #include #include -TEST(BehaviorPathPlanningLaneChangeUtilsTest, testStoppingDistance) +constexpr double epsilon = 1e-6; + +TEST(BehaviorPathPlanningLaneChangeUtilsTest, projectCurrentPoseToTarget) { - const auto vehicle_velocity = 8.333; + geometry_msgs::msg::Pose ego_pose; + const auto ego_yaw = tier4_autoware_utils::deg2rad(0.0); + ego_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(ego_yaw); + ego_pose.position = tier4_autoware_utils::createPoint(0, 0, 0); - const auto negative_accel = -1.5; - const auto distance_when_negative = - behavior_path_planner::utils::lane_change::stoppingDistance(vehicle_velocity, negative_accel); - ASSERT_NEAR(distance_when_negative, 23.1463, 1e-3); + geometry_msgs::msg::Pose obj_pose; + obj_pose.position = tier4_autoware_utils::createPoint(-4, 3, 0); + const auto obj_yaw = tier4_autoware_utils::deg2rad(0.0); + ego_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(obj_yaw); - const auto positive_accel = 1.5; - const auto distance_when_positive = - behavior_path_planner::utils::lane_change::stoppingDistance(vehicle_velocity, positive_accel); - ASSERT_NEAR(distance_when_positive, 34.7194, 1e-3); + const auto result = + behavior_path_planner::utils::safety_check::projectCurrentPoseToTarget(ego_pose, obj_pose); - const auto zero_accel = 0.0; - const auto distance_when_zero = - behavior_path_planner::utils::lane_change::stoppingDistance(vehicle_velocity, zero_accel); - ASSERT_NEAR(distance_when_zero, 34.7194, 1e-3); + EXPECT_NEAR(result.position.x, -4, epsilon); + EXPECT_NEAR(result.position.y, 3, epsilon); } diff --git a/planning/obstacle_avoidance_planner/config/obstacle_avoidance_planner.param.yaml b/planning/obstacle_avoidance_planner/config/obstacle_avoidance_planner.param.yaml index 01a8dfb48307b..3f4655b78229c 100644 --- a/planning/obstacle_avoidance_planner/config/obstacle_avoidance_planner.param.yaml +++ b/planning/obstacle_avoidance_planner/config/obstacle_avoidance_planner.param.yaml @@ -110,6 +110,7 @@ avoidance_cost_margin: 0.0 # [m] avoidance_cost_band_length: 5.0 # [m] avoidance_cost_decrease_rate: 0.05 # decreased cost per point interval + min_drivable_width: 0.2 # [m] The vehicle width and this parameter is guaranteed to keep for collision free constraint. weight: lat_error_weight: 0.0 # weight for lateral error diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/mpt_optimizer.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/mpt_optimizer.hpp index ddc7980f5dca8..b62cf1f9322fd 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/mpt_optimizer.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/mpt_optimizer.hpp @@ -184,6 +184,7 @@ class MPTOptimizer double avoidance_cost_margin; double avoidance_cost_band_length; double avoidance_cost_decrease_rate; + double min_drivable_width; double avoidance_lat_error_weight; double avoidance_yaw_error_weight; double avoidance_steer_input_weight; @@ -251,6 +252,7 @@ class MPTOptimizer const std::vector & left_bound, const std::vector & right_bound, const geometry_msgs::msg::Pose & ego_pose, const double ego_vel) const; + void keepMinimumBoundsWidth(std::vector & ref_points) const; std::vector extendViolatedBounds( const std::vector & ref_points) const; void avoidSuddenSteering( diff --git a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp index 2fdd76bfc8a93..83295b61c42a1 100644 --- a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp +++ b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp @@ -246,6 +246,7 @@ MPTOptimizer::MPTParam::MPTParam( node->declare_parameter("mpt.avoidance.avoidance_cost_band_length"); avoidance_cost_decrease_rate = node->declare_parameter("mpt.avoidance.avoidance_cost_decrease_rate"); + min_drivable_width = node->declare_parameter("mpt.avoidance.min_drivable_width"); avoidance_lat_error_weight = node->declare_parameter("mpt.avoidance.weight.lat_error_weight"); @@ -384,6 +385,7 @@ void MPTOptimizer::MPTParam::onParam(const std::vector & para parameters, "mpt.avoidance.max_longitudinal_margin_for_bound_violation", max_longitudinal_margin_for_bound_violation); updateParam(parameters, "mpt.avoidance.max_bound_fixing_time", max_bound_fixing_time); + updateParam(parameters, "mpt.avoidance.min_drivable_width", min_drivable_width); updateParam(parameters, "mpt.avoidance.max_avoidance_cost", max_avoidance_cost); updateParam(parameters, "mpt.avoidance.avoidance_cost_margin", avoidance_cost_margin); updateParam( @@ -802,10 +804,15 @@ void MPTOptimizer::updateBounds( ref_point_for_bound_search.pose, left_bound, soft_road_clearance, true); const double dist_to_right_bound = calcLateralDistToBounds( ref_point_for_bound_search.pose, right_bound, soft_road_clearance, false); - ref_points.at(i).bounds = Bounds{dist_to_right_bound, dist_to_left_bound}; } + // keep vehicle width + margin + // NOTE: The drivable area's width is sometimes narrower than the vehicle width which means + // infeasible to run especially when obstacles are extracted from the drivable area. + // In this case, the drivable area's width is forced to be wider. + keepMinimumBoundsWidth(ref_points); + // extend violated bounds, where the input path is outside the drivable area ref_points = extendViolatedBounds(ref_points); @@ -833,6 +840,160 @@ void MPTOptimizer::updateBounds( return; } +void MPTOptimizer::keepMinimumBoundsWidth(std::vector & ref_points) const +{ + // 1. calculate start and end sections which are out of bounds + std::vector> out_of_upper_bound_sections; + std::vector> out_of_lower_bound_sections; + std::optional out_of_upper_bound_start_idx = std::nullopt; + std::optional out_of_lower_bound_start_idx = std::nullopt; + for (size_t i = 0; i < ref_points.size(); ++i) { + const auto & b = ref_points.at(i).bounds; + // const double drivable_width = b.upper_bound - b.lower_bound; + // const bool is_infeasible_to_drive = drivable_width < mpt_param_.min_drivable_width; + + // NOTE: The following condition should be uncommented to see obstacles outside the path. + // However, on a narrow road, the ego may go outside the road border with this condition. + // Currently, we cannot distinguish obstacles and road border + if (/*is_infeasible_to_drive ||*/ b.upper_bound < 0.0) { // out of upper bound + if (!out_of_upper_bound_start_idx) { + out_of_upper_bound_start_idx = i; + } + } else { + if (out_of_upper_bound_start_idx) { + out_of_upper_bound_sections.push_back({*out_of_upper_bound_start_idx, i - 1}); + out_of_upper_bound_start_idx = std::nullopt; + } + } + if (/*is_infeasible_to_drive ||*/ 0.0 < b.lower_bound) { // out of lower bound + if (!out_of_lower_bound_start_idx) { + out_of_lower_bound_start_idx = i; + } + } else { + if (out_of_lower_bound_start_idx) { + out_of_lower_bound_sections.push_back({*out_of_lower_bound_start_idx, i - 1}); + out_of_lower_bound_start_idx = std::nullopt; + } + } + } + if (out_of_upper_bound_start_idx) { + out_of_upper_bound_sections.push_back({*out_of_upper_bound_start_idx, ref_points.size() - 1}); + } + if (out_of_lower_bound_start_idx) { + out_of_lower_bound_sections.push_back({*out_of_lower_bound_start_idx, ref_points.size() - 1}); + } + + auto original_ref_points = ref_points; + const auto is_inside_sections = [&](const size_t target_idx, const auto & sections) { + for (const auto & section : sections) { + if (section.first <= target_idx && target_idx <= section.second) { + return true; + } + } + return false; + }; + + // lower bound + for (const auto & out_of_lower_bound_section : out_of_lower_bound_sections) { + std::optional upper_bound_start_idx = std::nullopt; + std::optional upper_bound_end_idx = std::nullopt; + for (size_t p_idx = out_of_lower_bound_section.first; + p_idx <= out_of_lower_bound_section.second; ++p_idx) { + const bool is_out_of_upper_bound = is_inside_sections(p_idx, out_of_upper_bound_sections); + + const auto & original_b = original_ref_points.at(p_idx).bounds; + auto & b = ref_points.at(p_idx).bounds; + if (is_out_of_upper_bound) { + if (!upper_bound_start_idx) { + upper_bound_start_idx = p_idx; + } + upper_bound_end_idx = p_idx; + + // It seems both bounds are cut out. Widen the bounds towards the both side. + const double center_dist_to_bounds = + (original_b.upper_bound + original_b.lower_bound) / 2.0; + b.upper_bound = + std::max(b.upper_bound, center_dist_to_bounds + mpt_param_.min_drivable_width / 2.0); + b.lower_bound = + std::min(b.lower_bound, center_dist_to_bounds - mpt_param_.min_drivable_width / 2.0); + continue; + } + // Only the Lower bound is cut out. Widen the bounds towards the lower bound since cut out too + // much. + b.lower_bound = + std::min(b.lower_bound, original_b.upper_bound + mpt_param_.min_drivable_width); + continue; + } + // extend longitudinal if it overlaps out_of_upper_bound_sections + if (upper_bound_start_idx) { + for (size_t p_idx = out_of_lower_bound_section.first; p_idx < *upper_bound_start_idx; + ++p_idx) { + auto & b = ref_points.at(p_idx).bounds; + b.lower_bound = + std::min(b.lower_bound, ref_points.at(*upper_bound_start_idx).bounds.lower_bound); + } + } + if (upper_bound_end_idx) { + for (size_t p_idx = *upper_bound_end_idx + 1; p_idx <= out_of_lower_bound_section.second; + ++p_idx) { + auto & b = ref_points.at(p_idx).bounds; + b.lower_bound = + std::min(b.lower_bound, ref_points.at(*upper_bound_end_idx).bounds.lower_bound); + } + } + } + + // upper bound + for (const auto & out_of_upper_bound_section : out_of_upper_bound_sections) { + std::optional lower_bound_start_idx = std::nullopt; + std::optional lower_bound_end_idx = std::nullopt; + for (size_t p_idx = out_of_upper_bound_section.first; + p_idx <= out_of_upper_bound_section.second; ++p_idx) { + const bool is_out_of_lower_bound = is_inside_sections(p_idx, out_of_lower_bound_sections); + + const auto & original_b = original_ref_points.at(p_idx).bounds; + auto & b = ref_points.at(p_idx).bounds; + if (is_out_of_lower_bound) { + if (!lower_bound_start_idx) { + lower_bound_start_idx = p_idx; + } + lower_bound_end_idx = p_idx; + + // It seems both bounds are cut out. Widen the bounds towards the both side. + const double center_dist_to_bounds = + (original_b.upper_bound + original_b.lower_bound) / 2.0; + b.upper_bound = + std::max(b.upper_bound, center_dist_to_bounds + mpt_param_.min_drivable_width / 2.0); + b.lower_bound = + std::min(b.lower_bound, center_dist_to_bounds - mpt_param_.min_drivable_width / 2.0); + continue; + } + // Only the Upper bound is cut out. Widen the bounds towards the upper bound since cut out too + // much. + b.upper_bound = + std::max(b.upper_bound, original_b.lower_bound - mpt_param_.min_drivable_width); + continue; + } + // extend longitudinal if it overlaps out_of_lower_bound_sections + if (lower_bound_start_idx) { + for (size_t p_idx = out_of_upper_bound_section.first; p_idx < *lower_bound_start_idx; + ++p_idx) { + auto & b = ref_points.at(p_idx).bounds; + b.upper_bound = + std::max(b.upper_bound, ref_points.at(*lower_bound_start_idx).bounds.upper_bound); + } + } + if (lower_bound_end_idx) { + for (size_t p_idx = *lower_bound_end_idx + 1; p_idx <= out_of_upper_bound_section.second; + ++p_idx) { + auto & b = ref_points.at(p_idx).bounds; + b.upper_bound = + std::max(b.upper_bound, ref_points.at(*lower_bound_end_idx).bounds.upper_bound); + } + } + } +} + std::vector MPTOptimizer::extendViolatedBounds( const std::vector & ref_points) const {