From aad386962821bf4493c96f35f26ba9a169ee256a Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Wed, 10 Jul 2024 08:13:26 +0900 Subject: [PATCH 1/7] fix(static_obstacle_avoidance): stop position is unstable (#7880) fix(static_obstacle_avoidance): fix stop position Signed-off-by: satoshi-ota --- .../src/scene.cpp | 6 +- .../debug.hpp | 2 + .../scene.hpp | 6 + .../utils.hpp | 11 +- .../src/debug.cpp | 17 +++ .../src/scene.cpp | 46 ++++--- .../src/utils.cpp | 117 +++++++++--------- 7 files changed, 119 insertions(+), 86 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp index 7571a1f61d1c1..a7a7b30e4eec0 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp @@ -114,10 +114,8 @@ void AvoidanceByLaneChange::updateSpecialData() : Direction::RIGHT; } - utils::static_obstacle_avoidance::updateRegisteredObject( - registered_objects_, avoidance_data_.target_objects, p); - utils::static_obstacle_avoidance::compensateDetectionLost( - registered_objects_, avoidance_data_.target_objects, avoidance_data_.other_objects); + utils::static_obstacle_avoidance::compensateLostTargetObjects( + registered_objects_, avoidance_data_, clock_.now(), planner_data_, p); std::sort( avoidance_data_.target_objects.begin(), avoidance_data_.target_objects.end(), diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/debug.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/debug.hpp index 316501fbbd37f..63985e574f3b2 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/debug.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/debug.hpp @@ -47,6 +47,8 @@ MarkerArray createTargetObjectsMarkerArray(const ObjectDataArray & objects, cons MarkerArray createOtherObjectsMarkerArray( const ObjectDataArray & objects, const ObjectInfo & info, const bool verbose); +MarkerArray createStopTargetObjectMarkerArray(const AvoidancePlanningData & data); + MarkerArray createDebugMarkerArray( const AvoidancePlanningData & data, const PathShifter & shifter, const DebugData & debug, const std::shared_ptr & parameters); diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/scene.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/scene.hpp index 635edb7c84f40..34d06a46d9ac8 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/scene.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/scene.hpp @@ -266,6 +266,12 @@ class StaticObstacleAvoidanceModule : public SceneModuleInterface */ void fillAvoidanceTargetObjects(AvoidancePlanningData & data, DebugData & debug) const; + /** + * @brief fill additional data which are necessary to plan avoidance path/velocity. + * @param avoidance target objects. + */ + void fillAvoidanceTargetData(ObjectDataArray & objects) const; + /** * @brief fill candidate shift lines. * @param avoidance data. diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/utils.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/utils.hpp index 1f68ef7d49c47..909d28ed4bab6 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/utils.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/utils.hpp @@ -117,15 +117,12 @@ void fillObjectStoppableJudge( ObjectData & object_data, const ObjectDataArray & registered_objects, const double feasible_stop_distance, const std::shared_ptr & parameters); -void updateRegisteredObject( - ObjectDataArray & registered_objects, const ObjectDataArray & now_objects, - const std::shared_ptr & parameters); - void updateClipObject(ObjectDataArray & clip_objects, AvoidancePlanningData & data); -void compensateDetectionLost( - const ObjectDataArray & registered_objects, ObjectDataArray & now_objects, - ObjectDataArray & other_objects); +void compensateLostTargetObjects( + ObjectDataArray & stored_objects, AvoidancePlanningData & data, const rclcpp::Time & now, + const std::shared_ptr & planner_data, + const std::shared_ptr & parameters); void filterTargetObjects( ObjectDataArray & objects, AvoidancePlanningData & data, const double forward_detection_range, diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp index e9838eb4a2cfc..8618a1c340004 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp @@ -466,6 +466,23 @@ MarkerArray createOtherObjectsMarkerArray( return msg; } +MarkerArray createStopTargetObjectMarkerArray(const AvoidancePlanningData & data) +{ + MarkerArray msg; + + if (!data.stop_target_object.has_value()) { + return msg; + } + + appendMarkerArray( + createObjectsCubeMarkerArray( + {data.stop_target_object.value()}, "stop_target", createMarkerScale(3.4, 1.9, 1.9), + createMarkerColor(1.0, 0.0, 0.42, 0.5)), + &msg); + + return msg; +} + MarkerArray createDrivableBounds( const AvoidancePlanningData & data, std::string && ns, const float & r, const float & g, const float & b) diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp index b7ab41ec99abd..8452912989c43 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp @@ -288,16 +288,22 @@ void StaticObstacleAvoidanceModule::fillFundamentalData( data.to_start_point = utils::static_obstacle_avoidance::calcDistanceToAvoidStartLine( data.current_lanelets, data.reference_path_rough, planner_data_, parameters_); - // target objects for avoidance + // filter only for the latest detected objects. fillAvoidanceTargetObjects(data, debug); - // lost object compensation - utils::static_obstacle_avoidance::updateRegisteredObject( - registered_objects_, data.target_objects, parameters_); - utils::static_obstacle_avoidance::compensateDetectionLost( - registered_objects_, data.target_objects, data.other_objects); + // compensate lost object which was avoidance target. if the time hasn't passed more than + // threshold since perception module lost the target yet, this module keeps it as avoidance + // target. + utils::static_obstacle_avoidance::compensateLostTargetObjects( + registered_objects_, data, clock_->now(), planner_data_, parameters_); + + // once an object filtered for boundary clipping, this module keeps the information until the end + // of execution. utils::static_obstacle_avoidance::updateClipObject(clip_objects_, data); + // calculate various data for each target objects. + fillAvoidanceTargetData(data.target_objects); + // sort object order by longitudinal distance std::sort(data.target_objects.begin(), data.target_objects.end(), [](auto a, auto b) { return a.longitudinal < b.longitudinal; @@ -310,8 +316,6 @@ void StaticObstacleAvoidanceModule::fillFundamentalData( void StaticObstacleAvoidanceModule::fillAvoidanceTargetObjects( AvoidancePlanningData & data, DebugData & debug) const { - using utils::static_obstacle_avoidance::fillAvoidanceNecessity; - using utils::static_obstacle_avoidance::fillObjectStoppableJudge; using utils::static_obstacle_avoidance::filterTargetObjects; using utils::static_obstacle_avoidance::separateObjectsByPath; using utils::static_obstacle_avoidance::updateRoadShoulderDistance; @@ -347,15 +351,6 @@ void StaticObstacleAvoidanceModule::fillAvoidanceTargetObjects( filterTargetObjects(objects, data, forward_detection_range, planner_data_, parameters_); updateRoadShoulderDistance(data, planner_data_, parameters_); - // Calculate the distance needed to safely decelerate the ego vehicle to a stop line. - const auto & vehicle_width = planner_data_->parameters.vehicle_width; - const auto feasible_stop_distance = helper_->getFeasibleDecelDistance(0.0, false); - std::for_each(data.target_objects.begin(), data.target_objects.end(), [&, this](auto & o) { - fillAvoidanceNecessity(o, registered_objects_, vehicle_width, parameters_); - o.to_stop_line = calcDistanceToStopLine(o); - fillObjectStoppableJudge(o, registered_objects_, feasible_stop_distance, parameters_); - }); - // debug { std::vector debug_info_array; @@ -373,6 +368,21 @@ void StaticObstacleAvoidanceModule::fillAvoidanceTargetObjects( } } +void StaticObstacleAvoidanceModule::fillAvoidanceTargetData(ObjectDataArray & objects) const +{ + using utils::static_obstacle_avoidance::fillAvoidanceNecessity; + using utils::static_obstacle_avoidance::fillObjectStoppableJudge; + + // Calculate the distance needed to safely decelerate the ego vehicle to a stop line. + const auto & vehicle_width = planner_data_->parameters.vehicle_width; + const auto feasible_stop_distance = helper_->getFeasibleDecelDistance(0.0, false); + std::for_each(objects.begin(), objects.end(), [&, this](auto & o) { + fillAvoidanceNecessity(o, registered_objects_, vehicle_width, parameters_); + o.to_stop_line = calcDistanceToStopLine(o); + fillObjectStoppableJudge(o, registered_objects_, feasible_stop_distance, parameters_); + }); +} + ObjectData StaticObstacleAvoidanceModule::createObjectData( const AvoidancePlanningData & data, const PredictedObject & object) const { @@ -1392,11 +1402,13 @@ void StaticObstacleAvoidanceModule::updateRTCData() void StaticObstacleAvoidanceModule::updateInfoMarker(const AvoidancePlanningData & data) const { + using utils::static_obstacle_avoidance::createStopTargetObjectMarkerArray; using utils::static_obstacle_avoidance::createTargetObjectsMarkerArray; info_marker_.markers.clear(); appendMarkerArray( createTargetObjectsMarkerArray(data.target_objects, "target_objects"), &info_marker_); + appendMarkerArray(createStopTargetObjectMarkerArray(data), &info_marker_); } void StaticObstacleAvoidanceModule::updateDebugMarker( diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp index 016972dccb38e..64831e3b24fff 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp @@ -1645,31 +1645,39 @@ void fillObjectStoppableJudge( object_data.is_stoppable = same_id_obj->is_stoppable; } -void updateRegisteredObject( - ObjectDataArray & registered_objects, const ObjectDataArray & now_objects, +void compensateLostTargetObjects( + ObjectDataArray & stored_objects, AvoidancePlanningData & data, const rclcpp::Time & now, + const std::shared_ptr & planner_data, const std::shared_ptr & parameters) { - const auto updateIfDetectedNow = [&now_objects](auto & registered_object) { - const auto & n = now_objects; - const auto r_id = registered_object.object.object_id; - const auto same_id_obj = std::find_if( - n.begin(), n.end(), [&r_id](const auto & o) { return o.object.object_id == r_id; }); + const auto include = [](const auto & objects, const auto & search_id) { + return std::all_of(objects.begin(), objects.end(), [&search_id](const auto & o) { + return o.object.object_id != search_id; + }); + }; + + // STEP.1 UPDATE STORED OBJECTS. + const auto match = [&data](auto & object) { + const auto & search_id = object.object.object_id; + const auto same_id_object = std::find_if( + data.target_objects.begin(), data.target_objects.end(), + [&search_id](const auto & o) { return o.object.object_id == search_id; }); // same id object is detected. update registered. - if (same_id_obj != n.end()) { - registered_object = *same_id_obj; + if (same_id_object != data.target_objects.end()) { + object = *same_id_object; return true; } - constexpr auto POS_THR = 1.5; - const auto r_pos = registered_object.getPose(); - const auto similar_pos_obj = std::find_if(n.begin(), n.end(), [&](const auto & o) { - return calcDistance2d(r_pos, o.getPose()) < POS_THR; - }); + const auto similar_pos_obj = std::find_if( + data.target_objects.begin(), data.target_objects.end(), [&object](const auto & o) { + constexpr auto POS_THR = 1.5; + return calcDistance2d(object.getPose(), o.getPose()) < POS_THR; + }); // same id object is not detected, but object is found around registered. update registered. - if (similar_pos_obj != n.end()) { - registered_object = *similar_pos_obj; + if (similar_pos_obj != data.target_objects.end()) { + object = *similar_pos_obj; return true; } @@ -1677,61 +1685,54 @@ void updateRegisteredObject( return false; }; - const rclcpp::Time now = rclcpp::Clock(RCL_ROS_TIME).now(); - - // -- check registered_objects, remove if lost_count exceeds limit. -- - for (int i = static_cast(registered_objects.size()) - 1; i >= 0; --i) { - auto & r = registered_objects.at(i); - - // registered object is not detected this time. lost count up. - if (!updateIfDetectedNow(r)) { - r.lost_time = (now - r.last_seen).seconds(); - } else { - r.last_seen = now; - r.lost_time = 0.0; - } + // STEP1-1: REMOVE EXPIRED OBJECTS. + const auto itr = std::remove_if( + stored_objects.begin(), stored_objects.end(), [&now, &match, ¶meters](auto & o) { + if (!match(o)) { + o.lost_time = (now - o.last_seen).seconds(); + } else { + o.last_seen = now; + o.lost_time = 0.0; + } - // lost count exceeds threshold. remove object from register. - if (r.lost_time > parameters->object_last_seen_threshold) { - registered_objects.erase(registered_objects.begin() + i); - } - } + return o.lost_time > parameters->object_last_seen_threshold; + }); - const auto isAlreadyRegistered = [&](const auto & n_id) { - const auto & r = registered_objects; - return std::any_of( - r.begin(), r.end(), [&n_id](const auto & o) { return o.object.object_id == n_id; }); - }; + stored_objects.erase(itr, stored_objects.end()); - // -- check now_objects, add it if it has new object id -- - for (const auto & now_obj : now_objects) { - if (!isAlreadyRegistered(now_obj.object.object_id)) { - registered_objects.push_back(now_obj); + // STEP1-2: UPDATE STORED OBJECTS IF THERE ARE NEW OBJECTS. + for (const auto & current_object : data.target_objects) { + if (!include(stored_objects, current_object.object.object_id)) { + stored_objects.push_back(current_object); } } -} -void compensateDetectionLost( - const ObjectDataArray & registered_objects, ObjectDataArray & now_objects, - ObjectDataArray & other_objects) -{ - const auto isDetectedNow = [&](const auto & r_id) { - const auto & n = now_objects; + // STEP2: COMPENSATE CURRENT TARGET OBJECTS + const auto is_detected = [&](const auto & object_id) { return std::any_of( - n.begin(), n.end(), [&r_id](const auto & o) { return o.object.object_id == r_id; }); + data.target_objects.begin(), data.target_objects.end(), + [&object_id](const auto & o) { return o.object.object_id == object_id; }); }; - const auto isIgnoreObject = [&](const auto & r_id) { - const auto & n = other_objects; + const auto is_ignored = [&](const auto & object_id) { return std::any_of( - n.begin(), n.end(), [&r_id](const auto & o) { return o.object.object_id == r_id; }); + data.other_objects.begin(), data.other_objects.end(), + [&object_id](const auto & o) { return o.object.object_id == object_id; }); }; - for (const auto & registered : registered_objects) { - if ( - !isDetectedNow(registered.object.object_id) && !isIgnoreObject(registered.object.object_id)) { - now_objects.push_back(registered); + for (auto & stored_object : stored_objects) { + if (is_detected(stored_object.object.object_id)) { + continue; + } + if (is_ignored(stored_object.object.object_id)) { + continue; } + + const auto & ego_pos = planner_data->self_odometry->pose.pose.position; + fillLongitudinalAndLengthByClosestEnvelopeFootprint( + data.reference_path_rough, ego_pos, stored_object); + + data.target_objects.push_back(stored_object); } } From eb123deec9d39a2c035deac4f64fbdbb1f471101 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Wed, 10 Jul 2024 19:18:30 +0900 Subject: [PATCH 2/7] fix(static_obstacle_avoidance): don't automatically avoid ambiguous vehicle (#7851) * fix(static_obstacle_avoidance): don't automatically avoid ambiguous vehicle Signed-off-by: satoshi-ota * chore(schema): update schema Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota --- .../src/manager.cpp | 8 ++-- .../static_obstacle_avoidance.param.yaml | 10 +++- .../data_structs.hpp | 5 +- .../helper.hpp | 47 +++++++++++++++++-- .../parameter_helper.hpp | 8 ++-- .../static_obstacle_avoidance.schema.json | 31 ++++++++---- .../src/manager.cpp | 5 +- .../src/utils.cpp | 23 +++------ 8 files changed, 95 insertions(+), 42 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp index 02b90186d9b2f..8096d2944ee2b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp @@ -137,9 +137,11 @@ void AvoidanceByLaneChangeModuleManager::init(rclcpp::Node * node) { const std::string ns = "avoidance.target_filtering.avoidance_for_ambiguous_vehicle."; - p.enable_avoidance_for_ambiguous_vehicle = getOrDeclareParameter(*node, ns + "enable"); - p.closest_distance_to_wait_and_see_for_ambiguous_vehicle = - getOrDeclareParameter(*node, ns + "closest_distance_to_wait_and_see"); + p.policy_ambiguous_vehicle = getOrDeclareParameter(*node, ns + "policy"); + p.wait_and_see_target_behaviors = + getOrDeclareParameter>(*node, ns + "wait_and_see.target_behaviors"); + p.wait_and_see_th_closest_distance = + getOrDeclareParameter(*node, ns + "wait_and_see.th_closest_distance"); p.time_threshold_for_ambiguous_vehicle = getOrDeclareParameter(*node, ns + "condition.th_stopped_time"); p.distance_threshold_for_ambiguous_vehicle = diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/config/static_obstacle_avoidance.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/config/static_obstacle_avoidance.param.yaml index 3087ccc93934b..6bd5e0faf1938 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/config/static_obstacle_avoidance.param.yaml +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/config/static_obstacle_avoidance.param.yaml @@ -138,8 +138,11 @@ # params for avoidance of vehicle type objects that are ambiguous as to whether they are parked. avoidance_for_ambiguous_vehicle: - enable: true # [-] - closest_distance_to_wait_and_see: 10.0 # [m] + # policy for ego behavior for ambiguous vehicle. + # "auto" : generate candidate path. if RTC is running as AUTO mode, the ego avoids it automatically. + # "manual" : generate candidate path and wait operator approval even if RTC is running as AUTO mode. + # "ignore" : never avoid it. + policy: "auto" # [-] condition: th_stopped_time: 3.0 # [s] th_moving_distance: 1.0 # [m] @@ -149,6 +152,9 @@ crosswalk: front_distance: 30.0 # [m] behind_distance: 30.0 # [m] + wait_and_see: + target_behaviors: ["MERGING", "DEVIATING"] # [-] + th_closest_distance: 10.0 # [m] # params for filtering objects that are in intersection intersection: diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp index 49599004e0952..079928a58bd9f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp @@ -108,7 +108,7 @@ struct AvoidanceParameters bool enable_cancel_maneuver{false}; // enable avoidance for all parking vehicle - bool enable_avoidance_for_ambiguous_vehicle{false}; + std::string policy_ambiguous_vehicle{"ignore"}; // enable yield maneuver. bool enable_yield_maneuver{false}; @@ -192,7 +192,8 @@ struct AvoidanceParameters double object_check_min_road_shoulder_width{0.0}; // force avoidance - double closest_distance_to_wait_and_see_for_ambiguous_vehicle{0.0}; + std::vector wait_and_see_target_behaviors{"NONE", "MERGING", "DEVIATING"}; + double wait_and_see_th_closest_distance{0.0}; double time_threshold_for_ambiguous_vehicle{0.0}; double distance_threshold_for_ambiguous_vehicle{0.0}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/helper.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/helper.hpp index 8c46affbc64e3..bfeb942c82be3 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/helper.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/helper.hpp @@ -23,6 +23,7 @@ #include #include #include +#include #include #include @@ -325,10 +326,27 @@ class AvoidanceHelper const auto object = objects.front(); + // if the object is NOT ambiguous, this module doesn't wait operator approval if RTC is running + // as AUTO mode. if (!object.is_ambiguous) { return true; } + // check only front objects. + if (object.longitudinal < 0.0) { + return true; + } + + // if the policy is "manual", this module generates candidate path and waits approval. + if (parameters_->policy_ambiguous_vehicle == "manual") { + return false; + } + + // don't delay avoidance start position if it's not MERGING or DEVIATING vehicle. + if (!isWaitAndSeeTarget(object)) { + return true; + } + if (!object.avoid_margin.has_value()) { return true; } @@ -341,9 +359,32 @@ class AvoidanceHelper const auto constant_distance = getFrontConstantDistance(object); const auto avoidance_distance = getMinAvoidanceDistance(desire_shift_length); - return object.longitudinal < - prepare_distance + constant_distance + avoidance_distance + - parameters_->closest_distance_to_wait_and_see_for_ambiguous_vehicle; + return object.longitudinal < prepare_distance + constant_distance + avoidance_distance + + parameters_->wait_and_see_th_closest_distance; + } + + bool isWaitAndSeeTarget(const ObjectData & object) const + { + const auto & behaviors = parameters_->wait_and_see_target_behaviors; + if (object.behavior == ObjectData::Behavior::MERGING) { + return std::any_of(behaviors.begin(), behaviors.end(), [](const std::string & behavior) { + return behavior == "MERGING"; + }); + } + + if (object.behavior == ObjectData::Behavior::DEVIATING) { + return std::any_of(behaviors.begin(), behaviors.end(), [](const std::string & behavior) { + return behavior == "DEVIATING"; + }); + } + + if (object.behavior == ObjectData::Behavior::NONE) { + return std::any_of(behaviors.begin(), behaviors.end(), [](const std::string & behavior) { + return behavior == "NONE"; + }); + } + + return false; } static bool isAbsolutelyNotAvoidable(const ObjectData & object) diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp index 84cf7c4e33d26..0cea2a4e633c3 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp @@ -137,9 +137,11 @@ AvoidanceParameters getParameter(rclcpp::Node * node) { const std::string ns = "avoidance.target_filtering.avoidance_for_ambiguous_vehicle."; - p.enable_avoidance_for_ambiguous_vehicle = getOrDeclareParameter(*node, ns + "enable"); - p.closest_distance_to_wait_and_see_for_ambiguous_vehicle = - getOrDeclareParameter(*node, ns + "closest_distance_to_wait_and_see"); + p.policy_ambiguous_vehicle = getOrDeclareParameter(*node, ns + "policy"); + p.wait_and_see_target_behaviors = + getOrDeclareParameter>(*node, ns + "wait_and_see.target_behaviors"); + p.wait_and_see_th_closest_distance = + getOrDeclareParameter(*node, ns + "wait_and_see.th_closest_distance"); p.time_threshold_for_ambiguous_vehicle = getOrDeclareParameter(*node, ns + "condition.th_stopped_time"); p.distance_threshold_for_ambiguous_vehicle = diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json index db3215fa8d238..246b96ec5440e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json @@ -716,9 +716,10 @@ "avoidance_for_ambiguous_vehicle": { "type": "object", "properties": { - "enable": { - "type": "boolean", - "description": "Enable avoidance maneuver for ambiguous vehicles.", + "policy": { + "type": "string", + "enum": ["auto", "manual", "ignore"], + "description": "Ego behavior policy for ambiguous vehicle.", "default": "true" }, "closest_distance_to_wait_and_see": { @@ -778,14 +779,26 @@ }, "required": ["traffic_light", "crosswalk"], "additionalProperties": false + }, + "wait_and_see": { + "type": "object", + "properties": { + "target_behaviors": { + "type": "array", + "default": ["MERGING", "DEVIATING"], + "description": "This module doesn't avoid these behaviors vehicle until it gets closer than threshold." + }, + "th_closest_distance": { + "type": "number", + "default": 10.0, + "description": "Threshold to check whether the ego gets close enough the ambiguous vehicle." + } + }, + "required": ["target_behaviors", "th_closest_distance"], + "additionalProperties": false } }, - "required": [ - "enable", - "closest_distance_to_wait_and_see", - "condition", - "ignore_area" - ], + "required": ["policy", "condition", "ignore_area", "wait_and_see"], "additionalProperties": false }, "intersection": { diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/manager.cpp index 2d02f33e19870..c5f338e91acbd 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/manager.cpp @@ -132,10 +132,9 @@ void StaticObstacleAvoidanceModuleManager::updateModuleParams( { const std::string ns = "avoidance.avoidance.lateral.avoidance_for_ambiguous_vehicle."; - updateParam(parameters, ns + "enable", p->enable_avoidance_for_ambiguous_vehicle); + updateParam(parameters, ns + "policy", p->policy_ambiguous_vehicle); updateParam( - parameters, ns + "closest_distance_to_wait_and_see", - p->closest_distance_to_wait_and_see_for_ambiguous_vehicle); + parameters, ns + "wait_and_see.th_closest_distance", p->wait_and_see_th_closest_distance); updateParam( parameters, ns + "condition.th_stopped_time", p->time_threshold_for_ambiguous_vehicle); updateParam( diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp index 64831e3b24fff..cfd304f1908a1 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp @@ -880,7 +880,7 @@ bool isSatisfiedWithVehicleCondition( // from here, filtering for ambiguous vehicle. - if (!parameters->enable_avoidance_for_ambiguous_vehicle) { + if (parameters->policy_ambiguous_vehicle == "ignore") { object.info = ObjectInfo::AMBIGUOUS_STOPPED_VEHICLE; return false; } @@ -889,6 +889,7 @@ bool isSatisfiedWithVehicleCondition( object.stop_time > parameters->time_threshold_for_ambiguous_vehicle; if (!stop_time_longer_than_threshold) { object.info = ObjectInfo::AMBIGUOUS_STOPPED_VEHICLE; + object.is_ambiguous = false; return false; } @@ -897,6 +898,7 @@ bool isSatisfiedWithVehicleCondition( parameters->distance_threshold_for_ambiguous_vehicle; if (is_moving_distance_longer_than_threshold) { object.info = ObjectInfo::AMBIGUOUS_STOPPED_VEHICLE; + object.is_ambiguous = false; return false; } @@ -907,22 +909,9 @@ bool isSatisfiedWithVehicleCondition( return true; } } else { - if (object.behavior == ObjectData::Behavior::MERGING) { - object.info = ObjectInfo::AMBIGUOUS_STOPPED_VEHICLE; - object.is_ambiguous = true; - return true; - } - - if (object.behavior == ObjectData::Behavior::DEVIATING) { - object.info = ObjectInfo::AMBIGUOUS_STOPPED_VEHICLE; - object.is_ambiguous = true; - return true; - } - - if (object.behavior == ObjectData::Behavior::NONE) { - object.is_ambiguous = false; - return true; - } + object.info = ObjectInfo::AMBIGUOUS_STOPPED_VEHICLE; + object.is_ambiguous = true; + return true; } object.info = ObjectInfo::IS_NOT_PARKING_OBJECT; From 32c41bcefb883c2eccb6a5a3b00973a7d3322406 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Tue, 16 Jul 2024 13:34:39 +0900 Subject: [PATCH 3/7] feat(static_obstacle_avoidance): show markers when system requests operator support (#7994) Signed-off-by: satoshi-ota --- .../debug.hpp | 3 + .../src/debug.cpp | 73 ++++++++++++++++++- .../src/scene.cpp | 5 ++ 3 files changed, 80 insertions(+), 1 deletion(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/debug.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/debug.hpp index 63985e574f3b2..b205838896038 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/debug.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/debug.hpp @@ -47,6 +47,9 @@ MarkerArray createTargetObjectsMarkerArray(const ObjectDataArray & objects, cons MarkerArray createOtherObjectsMarkerArray( const ObjectDataArray & objects, const ObjectInfo & info, const bool verbose); +MarkerArray createAmbiguousObjectsMarkerArray( + const ObjectDataArray & objects, const Pose & ego_pose, const std::string & policy); + MarkerArray createStopTargetObjectMarkerArray(const AvoidancePlanningData & data); MarkerArray createDebugMarkerArray( diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp index 8618a1c340004..a542107e890aa 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp @@ -456,7 +456,7 @@ MarkerArray createOtherObjectsMarkerArray( appendMarkerArray( createObjectsCubeMarkerArray( filtered_objects, "others_" + ns + "_cube", createMarkerScale(3.0, 1.5, 1.5), - createMarkerColor(0.0, 1.0, 0.0, 0.8)), + createMarkerColor(0.5, 0.5, 0.5, 0.8)), &msg); appendMarkerArray( createObjectInfoMarkerArray(filtered_objects, "others_" + ns + "_info", verbose), &msg); @@ -466,6 +466,77 @@ MarkerArray createOtherObjectsMarkerArray( return msg; } +MarkerArray createAmbiguousObjectsMarkerArray( + const ObjectDataArray & objects, const Pose & ego_pose, const std::string & policy) +{ + MarkerArray msg; + + if (policy != "manual") { + return msg; + } + + for (const auto & object : objects) { + if (!object.is_ambiguous || !object.is_avoidable) { + continue; + } + + { + auto marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "ambiguous_target", 0L, Marker::ARROW, + createMarkerScale(0.5, 1.0, 1.0), createMarkerColor(1.0, 1.0, 0.0, 0.999)); + + Point src, dst; + src = object.getPosition(); + src.z += 4.0; + dst = object.getPosition(); + dst.z += 2.0; + + marker.points.push_back(src); + marker.points.push_back(dst); + marker.id = uuidToInt32(object.object.object_id); + + msg.markers.push_back(marker); + } + + { + auto marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "ambiguous_target_text", 0L, + Marker::TEXT_VIEW_FACING, createMarkerScale(0.5, 0.5, 0.5), + createMarkerColor(1.0, 1.0, 0.0, 1.0)); + + marker.id = uuidToInt32(object.object.object_id); + marker.pose = object.getPose(); + marker.pose.position.z += 4.5; + std::ostringstream string_stream; + string_stream << "SHOULD AVOID?"; + marker.text = string_stream.str(); + marker.color = createMarkerColor(1.0, 1.0, 0.0, 0.999); + marker.scale = createMarkerScale(0.8, 0.8, 0.8); + msg.markers.push_back(marker); + } + + { + auto marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "request_text", 0L, Marker::TEXT_VIEW_FACING, + createMarkerScale(0.5, 0.5, 0.5), createMarkerColor(1.0, 1.0, 0.0, 1.0)); + + marker.id = uuidToInt32(object.object.object_id); + marker.pose = ego_pose; + marker.pose.position.z += 2.0; + std::ostringstream string_stream; + string_stream << "SYSTEM REQUESTS OPERATOR SUPPORT."; + marker.text = string_stream.str(); + marker.color = createMarkerColor(1.0, 1.0, 0.0, 0.999); + marker.scale = createMarkerScale(0.8, 0.8, 0.8); + msg.markers.push_back(marker); + } + + return msg; + } + + return msg; +} + MarkerArray createStopTargetObjectMarkerArray(const AvoidancePlanningData & data) { MarkerArray msg; diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp index 8452912989c43..324bfdff8d777 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp @@ -1402,6 +1402,7 @@ void StaticObstacleAvoidanceModule::updateRTCData() void StaticObstacleAvoidanceModule::updateInfoMarker(const AvoidancePlanningData & data) const { + using utils::static_obstacle_avoidance::createAmbiguousObjectsMarkerArray; using utils::static_obstacle_avoidance::createStopTargetObjectMarkerArray; using utils::static_obstacle_avoidance::createTargetObjectsMarkerArray; @@ -1409,6 +1410,10 @@ void StaticObstacleAvoidanceModule::updateInfoMarker(const AvoidancePlanningData appendMarkerArray( createTargetObjectsMarkerArray(data.target_objects, "target_objects"), &info_marker_); appendMarkerArray(createStopTargetObjectMarkerArray(data), &info_marker_); + appendMarkerArray( + createAmbiguousObjectsMarkerArray( + data.target_objects, getEgoPose(), parameters_->policy_ambiguous_vehicle), + &info_marker_); } void StaticObstacleAvoidanceModule::updateDebugMarker( From ed97aff8fe87c17f1f89a060ffd79549156fddf5 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Tue, 16 Jul 2024 14:34:57 +0900 Subject: [PATCH 4/7] fix(static_obstacle_avoidance): fix issues in target filtiering logic (#7954) * fix: unknown filtering flow Signed-off-by: satoshi-ota * fix: relax target filtering logic for object which is in freespace Signed-off-by: satoshi-ota * docs: update flowchart Signed-off-by: satoshi-ota * fix: check stopped time in freespace Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota --- .../README.md | 21 +++++++++ .../static_obstacle_avoidance.param.yaml | 4 ++ .../data_structs.hpp | 3 ++ .../parameter_helper.hpp | 6 +++ .../static_obstacle_avoidance.schema.json | 22 ++++++++- .../src/manager.cpp | 6 +++ .../src/utils.cpp | 46 +++++++++++++++++-- 7 files changed, 103 insertions(+), 5 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/README.md b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/README.md index ddd0d473d2cbf..d32ab36301810 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/README.md +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/README.md @@ -401,6 +401,15 @@ else (\n no) #00FFB1 :IGNORE; stop endif +if(Is UNKNOWN objects?) then (yes) +if(isSatisfiedWithUnknownTypeObjectCodition) then (yes) +#FF006C :AVOID; +stop +else (\n no) +#00FFB1 :IGNORE; +stop +endif +else (\n no) if(Is vehicle type?) then (yes) if(isSatisfiedWithVehicleCodition()) then (yes) else (\n no) @@ -618,6 +627,18 @@ title Filter vehicle which is obviously an avoidance target start partition isObviousAvoidanceTarget() { +if(Is object within freespace?) then (yes) +if(Is object on ego lane?) then (no) +if(Is object stopping longer than threshold?) then (yes) +#FF006C :return true; +stop +else (\n no) +endif +else (\n yes) +endif +else (\n no) +endif + if(Is object within intersection?) then (yes) else (\n no) if(Is object parallel to ego lane? AND Is object a parked vehicle?) then (yes) diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/config/static_obstacle_avoidance.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/config/static_obstacle_avoidance.param.yaml index 6bd5e0faf1938..0e60e3216361d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/config/static_obstacle_avoidance.param.yaml +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/config/static_obstacle_avoidance.param.yaml @@ -160,6 +160,10 @@ intersection: yaw_deviation: 0.349 # [rad] (default 20.0deg) + freespace: + condition: + th_stopped_time: 5.0 # [-] + # For safety check safety_check: # safety check target type diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp index 079928a58bd9f..4e14dc4886768 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp @@ -191,6 +191,9 @@ struct AvoidanceParameters // minimum road shoulder width. maybe 0.5 [m] double object_check_min_road_shoulder_width{0.0}; + // time threshold for vehicle in freespace. + double freespace_condition_th_stopped_time{0.0}; + // force avoidance std::vector wait_and_see_target_behaviors{"NONE", "MERGING", "DEVIATING"}; double wait_and_see_th_closest_distance{0.0}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp index 0cea2a4e633c3..56ac3d7f4f1bb 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp @@ -154,6 +154,12 @@ AvoidanceParameters getParameter(rclcpp::Node * node) getOrDeclareParameter(*node, ns + "ignore_area.crosswalk.behind_distance"); } + { + const std::string ns = "avoidance.target_filtering.freespace."; + p.freespace_condition_th_stopped_time = + getOrDeclareParameter(*node, ns + "condition.th_stopped_time"); + } + { const std::string ns = "avoidance.target_filtering.detection_area."; p.use_static_detection_area = getOrDeclareParameter(*node, ns + "static"); diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json index 246b96ec5440e..38a91ac39525b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json @@ -812,6 +812,25 @@ }, "required": ["yaw_deviation"], "additionalProperties": false + }, + "freespace": { + "type": "object", + "properties": { + "condition": { + "type": "object", + "properties": { + "th_stopped_time": { + "type": "number", + "description": "This module delays avoidance maneuver to see vehicle behavior in freespace.", + "default": 5.0 + } + }, + "required": ["th_stopped_time"], + "additionalProperties": false + } + }, + "required": ["condition"], + "additionalProperties": false } }, "required": [ @@ -823,7 +842,8 @@ "merging_vehicle", "parked_vehicle", "avoidance_for_ambiguous_vehicle", - "intersection" + "intersection", + "freespace" ], "additionalProperties": false }, diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/manager.cpp index c5f338e91acbd..5655696ff086d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/manager.cpp @@ -150,6 +150,12 @@ void StaticObstacleAvoidanceModuleManager::updateModuleParams( p->object_ignore_section_crosswalk_behind_distance); } + { + const std::string ns = "avoidance.target_filtering.freespace."; + updateParam( + parameters, ns + "condition.th_stopped_time", p->freespace_condition_th_stopped_time); + } + { const std::string ns = "avoidance.target_filtering.intersection."; updateParam(parameters, ns + "yaw_deviation", p->object_check_yaw_deviation); diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp index cfd304f1908a1..0830c0f10dd4e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp @@ -339,6 +339,32 @@ bool isWithinIntersection( lanelet::utils::to2D(polygon.basicPolygon())); } +bool isWithinFreespace( + const ObjectData & object, const std::shared_ptr & route_handler) +{ + auto polygons = lanelet::utils::query::getAllParkingLots(route_handler->getLaneletMapPtr()); + if (polygons.empty()) { + return false; + } + + std::sort(polygons.begin(), polygons.end(), [&object](const auto & a, const auto & b) { + const double a_distance = boost::geometry::distance( + lanelet::utils::to2D(a).basicPolygon(), + lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(object.getPosition())) + .basicPoint()); + const double b_distance = boost::geometry::distance( + lanelet::utils::to2D(b).basicPolygon(), + lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(object.getPosition())) + .basicPoint()); + return a_distance < b_distance; + }); + + return boost::geometry::within( + lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(object.getPosition())) + .basicPoint(), + lanelet::utils::to2D(polygons.front().basicPolygon())); +} + bool isOnEgoLane(const ObjectData & object, const std::shared_ptr & route_handler) { if (boost::geometry::within( @@ -708,6 +734,14 @@ bool isObviousAvoidanceTarget( [[maybe_unused]] const std::shared_ptr & planner_data, [[maybe_unused]] const std::shared_ptr & parameters) { + if (isWithinFreespace(object, planner_data->route_handler)) { + if (!object.is_on_ego_lane) { + if (object.stop_time > parameters->freespace_condition_th_stopped_time) { + return true; + } + } + } + if (!object.is_within_intersection) { if (object.is_parked && object.behavior == ObjectData::Behavior::NONE) { RCLCPP_DEBUG(rclcpp::get_logger(logger_namespace), "object is obvious parked vehicle."); @@ -1842,17 +1876,19 @@ void filterTargetObjects( utils::static_obstacle_avoidance::calcEnvelopeOverhangDistance(o, data.reference_path); o.to_road_shoulder_distance = filtering_utils::getRoadShoulderDistance(o, data, planner_data); - // TODO(Satoshi Ota) parametrize stop time threshold if need. - constexpr double STOP_TIME_THRESHOLD = 3.0; // [s] if (filtering_utils::isUnknownTypeObject(o)) { + // TARGET: UNKNOWN + + // TODO(Satoshi Ota) parametrize stop time threshold if need. + constexpr double STOP_TIME_THRESHOLD = 3.0; // [s] if (o.stop_time < STOP_TIME_THRESHOLD) { o.info = ObjectInfo::UNSTABLE_OBJECT; data.other_objects.push_back(o); continue; } - } + } else if (filtering_utils::isVehicleTypeObject(o)) { + // TARGET: CAR, TRUCK, BUS, TRAILER, MOTORCYCLE - if (filtering_utils::isVehicleTypeObject(o)) { o.is_within_intersection = filtering_utils::isWithinIntersection(o, planner_data->route_handler); o.is_parked = @@ -1869,6 +1905,8 @@ void filterTargetObjects( continue; } } else { + // TARGET: PEDESTRIAN, BICYCLE + o.is_within_intersection = filtering_utils::isWithinIntersection(o, planner_data->route_handler); o.is_parked = false; From 433937f8e5d867c74923eb835dc08ec3ac2d56e0 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Thu, 18 Jul 2024 15:49:25 +0900 Subject: [PATCH 5/7] fix(static_obstacle_avoidance): avoid object behind unavoidance object if unavoidable is not on the path (#8066) Signed-off-by: satoshi-ota --- .../src/shift_line_generator.cpp | 34 +++++++++++++++---- 1 file changed, 27 insertions(+), 7 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp index c3a67eb074d73..5e58466fa4229 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp @@ -239,10 +239,18 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( const auto is_forward_object = [](const auto & object) { return object.longitudinal > 0.0; }; + const auto is_on_path = [this](const auto & object) { + const auto [overhang, point] = object.overhang_points.front(); + return std::abs(overhang) < 0.5 * data_->parameters.vehicle_width; + }; + const auto is_valid_shift_line = [](const auto & s) { return s.start_longitudinal > 0.0 && s.start_longitudinal < s.end_longitudinal; }; + ObjectDataArray unavoidable_objects; + + // target objects are sorted by longitudinal distance. AvoidOutlines outlines; for (auto & o : data.target_objects) { if (!o.avoid_margin.has_value()) { @@ -253,22 +261,22 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( } else { o.info = ObjectInfo::INSUFFICIENT_DRIVABLE_SPACE; } - if (o.avoid_required && is_forward_object(o)) { + if (o.avoid_required && is_forward_object(o) && is_on_path(o)) { break; } else { + unavoidable_objects.push_back(o); continue; } } - const auto is_object_on_right = utils::static_obstacle_avoidance::isOnRight(o); const auto desire_shift_length = - helper_->getShiftLength(o, is_object_on_right, o.avoid_margin.value()); - if (utils::static_obstacle_avoidance::isSameDirectionShift( - is_object_on_right, desire_shift_length)) { + helper_->getShiftLength(o, isOnRight(o), o.avoid_margin.value()); + if (utils::static_obstacle_avoidance::isSameDirectionShift(isOnRight(o), desire_shift_length)) { o.info = ObjectInfo::SAME_DIRECTION_SHIFT; - if (o.avoid_required && is_forward_object(o)) { + if (o.avoid_required && is_forward_object(o) && is_on_path(o)) { break; } else { + unavoidable_objects.push_back(o); continue; } } @@ -276,13 +284,25 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( // calculate feasible shift length based on behavior policy const auto feasible_shift_profile = get_shift_profile(o, desire_shift_length); if (!feasible_shift_profile.has_value()) { - if (o.avoid_required && is_forward_object(o)) { + if (o.avoid_required && is_forward_object(o) && is_on_path(o)) { break; } else { + unavoidable_objects.push_back(o); continue; } } + // If there is an object that cannot be avoided, this module only avoids object on the same side + // as unavoidable object. + if (!unavoidable_objects.empty()) { + if (isOnRight(unavoidable_objects.front()) && !isOnRight(o)) { + break; + } + if (!isOnRight(unavoidable_objects.front()) && isOnRight(o)) { + break; + } + } + // use absolute dist for return-to-center, relative dist from current for avoiding. const auto feasible_return_distance = helper_->getMaxAvoidanceDistance(feasible_shift_profile.value().first); From d8d9c19f773a7ad1f8a56637fda61aaad6b00482 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Fri, 19 Jul 2024 17:42:38 +0900 Subject: [PATCH 6/7] fix(bpp, rtc_interface): fix state transition (#7743) * fix(rtc_interface): check rtc state Signed-off-by: satoshi-ota * fix(bpp_interface): check rtc state Signed-off-by: satoshi-ota * feat(rtc_interface): print Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota --- .../autoware/rtc_interface/rtc_interface.hpp | 2 + .../src/rtc_interface.cpp | 68 +++++++++++++++++-- .../interface/scene_module_interface.hpp | 14 +++- 3 files changed, 75 insertions(+), 9 deletions(-) diff --git a/planning/autoware_rtc_interface/include/autoware/rtc_interface/rtc_interface.hpp b/planning/autoware_rtc_interface/include/autoware/rtc_interface/rtc_interface.hpp index 62f9a55c75455..23f7f0b4b36c5 100644 --- a/planning/autoware_rtc_interface/include/autoware/rtc_interface/rtc_interface.hpp +++ b/planning/autoware_rtc_interface/include/autoware/rtc_interface/rtc_interface.hpp @@ -61,8 +61,10 @@ class RTCInterface bool isActivated(const UUID & uuid) const; bool isRegistered(const UUID & uuid) const; bool isRTCEnabled(const UUID & uuid) const; + bool isTerminated(const UUID & uuid) const; void lockCommandUpdate(); void unlockCommandUpdate(); + void print() const; private: void onCooperateCommandService( diff --git a/planning/autoware_rtc_interface/src/rtc_interface.cpp b/planning/autoware_rtc_interface/src/rtc_interface.cpp index 985e38b64f2bd..44678077d5dd1 100644 --- a/planning/autoware_rtc_interface/src/rtc_interface.cpp +++ b/planning/autoware_rtc_interface/src/rtc_interface.cpp @@ -20,7 +20,7 @@ namespace { using tier4_rtc_msgs::msg::Module; -std::string to_string(const unique_identifier_msgs::msg::UUID & uuid) +std::string uuid_to_string(const unique_identifier_msgs::msg::UUID & uuid) { std::stringstream ss; for (auto i = 0; i < 16; ++i) { @@ -29,6 +29,34 @@ std::string to_string(const unique_identifier_msgs::msg::UUID & uuid) return ss.str(); } +std::string command_to_string(const uint8_t type) +{ + if (type == tier4_rtc_msgs::msg::Command::ACTIVATE) { + return "ACTIVATE"; + } else if (type == tier4_rtc_msgs::msg::Command::DEACTIVATE) { + return "DEACTIVATE"; + } + + throw std::domain_error("invalid rtc command."); +} + +std::string state_to_string(const uint8_t type) +{ + if (type == tier4_rtc_msgs::msg::State::WAITING_FOR_EXECUTION) { + return "WAITING_FOR_EXECUTION"; + } else if (type == tier4_rtc_msgs::msg::State::RUNNING) { + return "RUNNING"; + } else if (type == tier4_rtc_msgs::msg::State::ABORTING) { + return "ABORTING"; + } else if (type == tier4_rtc_msgs::msg::State::SUCCEEDED) { + return "SUCCEEDED"; + } else if (type == tier4_rtc_msgs::msg::State::FAILED) { + return "FAILED"; + } + + throw std::domain_error("invalid rtc state."); +} + Module getModuleType(const std::string & module_name) { Module module; @@ -158,14 +186,14 @@ std::vector RTCInterface::validateCooperateCommands( } else { RCLCPP_WARN_STREAM( getLogger(), "[validateCooperateCommands] uuid : " - << to_string(command.uuid) + << uuid_to_string(command.uuid) << " state is not WAITING_FOR_EXECUTION or RUNNING. state : " << itr->state.type << std::endl); response.success = false; } } else { RCLCPP_WARN_STREAM( - getLogger(), "[validateCooperateCommands] uuid : " << to_string(command.uuid) + getLogger(), "[validateCooperateCommands] uuid : " << uuid_to_string(command.uuid) << " is not found." << std::endl); response.success = false; } @@ -262,7 +290,7 @@ void RTCInterface::removeCooperateStatus(const UUID & uuid) RCLCPP_WARN_STREAM( getLogger(), - "[removeCooperateStatus] uuid : " << to_string(uuid) << " is not found." << std::endl); + "[removeCooperateStatus] uuid : " << uuid_to_string(uuid) << " is not found." << std::endl); } void RTCInterface::removeStoredCommand(const UUID & uuid) @@ -313,7 +341,7 @@ bool RTCInterface::isActivated(const UUID & uuid) const } RCLCPP_WARN_STREAM( - getLogger(), "[isActivated] uuid : " << to_string(uuid) << " is not found." << std::endl); + getLogger(), "[isActivated] uuid : " << uuid_to_string(uuid) << " is not found." << std::endl); return false; } @@ -338,7 +366,23 @@ bool RTCInterface::isRTCEnabled(const UUID & uuid) const } RCLCPP_WARN_STREAM( - getLogger(), "[isRTCEnabled] uuid : " << to_string(uuid) << " is not found." << std::endl); + getLogger(), "[isRTCEnabled] uuid : " << uuid_to_string(uuid) << " is not found." << std::endl); + return is_auto_mode_enabled_; +} + +bool RTCInterface::isTerminated(const UUID & uuid) const +{ + std::lock_guard lock(mutex_); + const auto itr = std::find_if( + registered_status_.statuses.begin(), registered_status_.statuses.end(), + [uuid](auto & s) { return s.uuid == uuid; }); + + if (itr != registered_status_.statuses.end()) { + return itr->state.type == State::SUCCEEDED || itr->state.type == State::FAILED; + } + + RCLCPP_WARN_STREAM( + getLogger(), "[isTerminated] uuid : " << uuid_to_string(uuid) << " is not found." << std::endl); return is_auto_mode_enabled_; } @@ -363,4 +407,16 @@ bool RTCInterface::isLocked() const return is_locked_; } +void RTCInterface::print() const +{ + RCLCPP_INFO_STREAM(getLogger(), "---print rtc cooperate statuses---" << std::endl); + for (const auto status : registered_status_.statuses) { + RCLCPP_INFO_STREAM( + getLogger(), "uuid:" << uuid_to_string(status.uuid) + << " command:" << command_to_string(status.command_status.type) + << std::boolalpha << " safe:" << status.safe + << " state:" << state_to_string(status.state.type) << std::endl); + } +} + } // namespace autoware::rtc_interface diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp index 95eb82e0b4c23..c4a05d171654d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp @@ -335,8 +335,15 @@ class SceneModuleInterface { return std::any_of( rtc_interface_ptr_map_.begin(), rtc_interface_ptr_map_.end(), [&](const auto & rtc) { - return rtc.second->isRegistered(uuid_map_.at(rtc.first)) && - rtc.second->isActivated(uuid_map_.at(rtc.first)); + if (!rtc.second->isRegistered(uuid_map_.at(rtc.first))) { + return false; + } + + if (rtc.second->isTerminated(uuid_map_.at(rtc.first))) { + return true; + } + + return rtc.second->isActivated(uuid_map_.at(rtc.first)); }); } @@ -345,7 +352,8 @@ class SceneModuleInterface return std::any_of( rtc_interface_ptr_map_.begin(), rtc_interface_ptr_map_.end(), [&](const auto & rtc) { return rtc.second->isRegistered(uuid_map_.at(rtc.first)) && - !rtc.second->isActivated(uuid_map_.at(rtc.first)); + !rtc.second->isActivated(uuid_map_.at(rtc.first)) && + !rtc.second->isTerminated(uuid_map_.at(rtc.first)); }); } From 3f7060a0c4a0919b0be7127afdbdf8162bbee949 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Tue, 16 Jul 2024 19:30:24 +0900 Subject: [PATCH 7/7] feat(static_obstacle_avoidance): integrate time keeper to major functions (#8044) Signed-off-by: satoshi-ota --- .../src/scene.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp index 324bfdff8d777..d125f043f477b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp @@ -316,6 +316,7 @@ void StaticObstacleAvoidanceModule::fillFundamentalData( void StaticObstacleAvoidanceModule::fillAvoidanceTargetObjects( AvoidancePlanningData & data, DebugData & debug) const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); using utils::static_obstacle_avoidance::filterTargetObjects; using utils::static_obstacle_avoidance::separateObjectsByPath; using utils::static_obstacle_avoidance::updateRoadShoulderDistance; @@ -370,6 +371,7 @@ void StaticObstacleAvoidanceModule::fillAvoidanceTargetObjects( void StaticObstacleAvoidanceModule::fillAvoidanceTargetData(ObjectDataArray & objects) const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); using utils::static_obstacle_avoidance::fillAvoidanceNecessity; using utils::static_obstacle_avoidance::fillObjectStoppableJudge; @@ -1402,6 +1404,7 @@ void StaticObstacleAvoidanceModule::updateRTCData() void StaticObstacleAvoidanceModule::updateInfoMarker(const AvoidancePlanningData & data) const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); using utils::static_obstacle_avoidance::createAmbiguousObjectsMarkerArray; using utils::static_obstacle_avoidance::createStopTargetObjectMarkerArray; using utils::static_obstacle_avoidance::createTargetObjectsMarkerArray;