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 303d7449e40dd..501554b0a2b02 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 @@ -113,10 +113,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 3797fe0d55147..a3b1b7e10b885 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 @@ -118,15 +118,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 eb0d903f2e398..e1f8bd4ad2734 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 b0e7d68c75d44..641ae9547b712 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 @@ -286,16 +286,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; @@ -308,8 +314,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; @@ -345,15 +349,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; @@ -371,6 +366,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 { @@ -1376,11 +1386,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 30d1aad16374b..ddc544553f30e 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); } }