diff --git a/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp b/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp index 8c9982b0384d5..d7d7fa8b60513 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp @@ -166,7 +166,7 @@ void AvoidanceByLaneChange::fillAvoidanceTargetObjects( [](const auto & object) { ObjectData other_object; other_object.object = object; - other_object.reason = "OutOfTargetArea"; + other_object.info = ObjectInfo::OUT_OF_TARGET_AREA; return other_object; }); } diff --git a/planning/behavior_path_avoidance_module/config/avoidance.param.yaml b/planning/behavior_path_avoidance_module/config/avoidance.param.yaml index dc42af0412254..884812d0db716 100644 --- a/planning/behavior_path_avoidance_module/config/avoidance.param.yaml +++ b/planning/behavior_path_avoidance_module/config/avoidance.param.yaml @@ -293,4 +293,11 @@ # for debug debug: - marker: false + enable_other_objects_marker: false + enable_other_objects_info: false + enable_detection_area_marker: false + enable_drivable_bound_marker: false + enable_safety_check_marker: false + enable_shift_line_marker: false + enable_lane_marker: false + enable_misc_marker: false diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp index 58ada91df9e96..7958a8a2dcbd4 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp @@ -15,16 +15,12 @@ #ifndef BEHAVIOR_PATH_AVOIDANCE_MODULE__DATA_STRUCTS_HPP_ #define BEHAVIOR_PATH_AVOIDANCE_MODULE__DATA_STRUCTS_HPP_ +#include "behavior_path_avoidance_module/type_alias.hpp" #include "behavior_path_planner_common/data_manager.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" #include -#include - -#include -#include -#include #include #include @@ -38,21 +34,38 @@ namespace behavior_path_planner { -using autoware_auto_perception_msgs::msg::PredictedObject; -using autoware_auto_perception_msgs::msg::PredictedPath; -using autoware_auto_planning_msgs::msg::PathWithLaneId; - -using tier4_autoware_utils::Point2d; -using tier4_autoware_utils::Polygon2d; -using tier4_planning_msgs::msg::AvoidanceDebugMsgArray; - -using geometry_msgs::msg::Point; -using geometry_msgs::msg::Pose; using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebug; - using route_handler::Direction; +enum class ObjectInfo { + NONE = 0, + // ignore reasons + OUT_OF_TARGET_AREA, + FURTHER_THAN_THRESHOLD, + FURTHER_THAN_GOAL, + IS_NOT_TARGET_OBJECT, + IS_NOT_PARKING_OBJECT, + TOO_NEAR_TO_CENTERLINE, + TOO_NEAR_TO_GOAL, + MOVING_OBJECT, + UNSTABLE_OBJECT, + CROSSWALK_USER, + ENOUGH_LATERAL_DISTANCE, + LESS_THAN_EXECUTION_THRESHOLD, + PARALLEL_TO_EGO_LANE, + MERGING_TO_EGO_LANE, + DEVIATING_FROM_EGO_LANE, + // unavoidable reasons + NEED_DECELERATION, + SAME_DIRECTION_SHIFT, + INSUFFICIENT_DRIVABLE_SPACE, + INSUFFICIENT_LONGITUDINAL_DISTANCE, + INVALID_SHIFT_LINE, + // others + AMBIGUOUS_STOPPED_VEHICLE, +}; + struct ObjectParameter { bool is_avoidance_target{false}; @@ -324,7 +337,14 @@ struct AvoidanceParameters bool enable_bound_clipping{false}; // debug - bool publish_debug_marker = false; + bool enable_other_objects_marker{false}; + bool enable_other_objects_info{false}; + bool enable_detection_area_marker{false}; + bool enable_drivable_bound_marker{false}; + bool enable_safety_check_marker{false}; + bool enable_shift_line_marker{false}; + bool enable_lane_marker{false}; + bool enable_misc_marker{false}; }; struct ObjectData // avoidance target @@ -423,8 +443,8 @@ struct ObjectData // avoidance target // overhang points (sort by distance) std::vector> overhang_points{}; - // unavoidable reason - std::string reason{}; + // object detail info + ObjectInfo info{ObjectInfo::NONE}; // lateral avoid margin std::optional avoid_margin{std::nullopt}; diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/debug.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/debug.hpp index f49f457ddd066..6a4f206ddf309 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/debug.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/debug.hpp @@ -16,25 +16,22 @@ #define BEHAVIOR_PATH_AVOIDANCE_MODULE__DEBUG_HPP_ #include "behavior_path_avoidance_module/data_structs.hpp" +#include "behavior_path_avoidance_module/type_alias.hpp" -#include - -#include -#include - +#include #include -namespace marker_utils::avoidance_marker +namespace behavior_path_planner::utils::avoidance { -using autoware_auto_planning_msgs::msg::PathWithLaneId; + +using behavior_path_planner::AvoidanceParameters; using behavior_path_planner::AvoidancePlanningData; using behavior_path_planner::AvoidLineArray; using behavior_path_planner::DebugData; using behavior_path_planner::ObjectDataArray; +using behavior_path_planner::ObjectInfo; using behavior_path_planner::PathShifter; using behavior_path_planner::ShiftLineArray; -using geometry_msgs::msg::Pose; -using visualization_msgs::msg::MarkerArray; MarkerArray createEgoStatusMarkerArray( const AvoidancePlanningData & data, const Pose & p_ego, std::string && ns); @@ -47,11 +44,13 @@ MarkerArray createPredictedVehiclePositions(const PathWithLaneId & path, std::st MarkerArray createTargetObjectsMarkerArray(const ObjectDataArray & objects, const std::string & ns); -MarkerArray createOtherObjectsMarkerArray(const ObjectDataArray & objects, const std::string & ns); +MarkerArray createOtherObjectsMarkerArray( + const ObjectDataArray & objects, const ObjectInfo & info, const bool verbose); MarkerArray createDebugMarkerArray( - const AvoidancePlanningData & data, const PathShifter & shifter, const DebugData & debug); -} // namespace marker_utils::avoidance_marker + const AvoidancePlanningData & data, const PathShifter & shifter, const DebugData & debug, + const std::shared_ptr & parameters); +} // namespace behavior_path_planner::utils::avoidance std::string toStrInfo(const behavior_path_planner::ShiftLineArray & sl_arr); diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp index 8d95e724045ba..8de39d00af130 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp @@ -16,11 +16,10 @@ #define BEHAVIOR_PATH_AVOIDANCE_MODULE__HELPER_HPP_ #include "behavior_path_avoidance_module/data_structs.hpp" +#include "behavior_path_avoidance_module/type_alias.hpp" #include "behavior_path_avoidance_module/utils.hpp" #include "behavior_path_planner_common/utils/utils.hpp" -#include - #include #include #include @@ -32,11 +31,6 @@ namespace behavior_path_planner::helper::avoidance using behavior_path_planner::PathShifter; using behavior_path_planner::PlannerData; -using motion_utils::calcDecelDistWithJerkAndAccConstraints; -using motion_utils::calcLongitudinalOffsetPose; -using motion_utils::calcSignedArcLength; -using motion_utils::findNearestIndex; -using tier4_autoware_utils::getPose; class AvoidanceHelper { @@ -181,14 +175,14 @@ class AvoidanceHelper double getShift(const Point & p) const { validate(); - const auto idx = findNearestIndex(prev_reference_path_.points, p); + const auto idx = motion_utils::findNearestIndex(prev_reference_path_.points, p); return prev_spline_shift_path_.shift_length.at(idx); } double getLinearShift(const Point & p) const { validate(); - const auto idx = findNearestIndex(prev_reference_path_.points, p); + const auto idx = motion_utils::findNearestIndex(prev_reference_path_.points, p); return prev_linear_shift_path_.shift_length.at(idx); } @@ -282,8 +276,8 @@ class AvoidanceHelper } const auto start_idx = data_->findEgoIndex(path.points); - const auto distance = - calcSignedArcLength(path.points, start_idx, max_v_point_.value().first.position); + const auto distance = motion_utils::calcSignedArcLength( + path.points, start_idx, max_v_point_.value().first.position); return std::make_pair(distance, max_v_point_.value().second); } @@ -294,7 +288,7 @@ class AvoidanceHelper const auto & a_now = data_->self_acceleration->accel.accel.linear.x; const auto & a_lim = use_hard_constraints ? p->max_deceleration : p->nominal_deceleration; const auto & j_lim = use_hard_constraints ? p->max_jerk : p->nominal_jerk; - const auto ret = calcDecelDistWithJerkAndAccConstraints( + const auto ret = motion_utils::calcDecelDistWithJerkAndAccConstraints( getEgoSpeed(), target_velocity, a_now, a_lim, j_lim, -1.0 * j_lim); if (!!ret) { @@ -343,6 +337,20 @@ class AvoidanceHelper parameters_->closest_distance_to_wait_and_see_for_ambiguous_vehicle; } + static bool isAbsolutelyNotAvoidable(const ObjectData & object) + { + if (object.is_avoidable) { + return false; + } + + // can avoid it after deceleration. + if (object.info == ObjectInfo::NEED_DECELERATION) { + return false; + } + + return true; + } + bool isReady(const AvoidLineArray & new_shift_lines, const double current_shift_length) const { if (std::abs(current_shift_length) < 1e-3) { @@ -427,14 +435,15 @@ class AvoidanceHelper const auto x_max_accel = v_neg_jerk * t_max_accel + p->max_acceleration * std::pow(t_max_accel, 2.0) / 2.0; - const auto point = - calcLongitudinalOffsetPose(path.points, getEgoPosition(), x_neg_jerk + x_max_accel); + const auto point = motion_utils::calcLongitudinalOffsetPose( + path.points, getEgoPosition(), x_neg_jerk + x_max_accel); if (point.has_value()) { max_v_point_ = std::make_pair(point.value(), v_max); return; } - const auto x_end = calcSignedArcLength(path.points, getEgoPosition(), path.points.size() - 1); + const auto x_end = + motion_utils::calcSignedArcLength(path.points, getEgoPosition(), path.points.size() - 1); const auto t_end = (std::sqrt(v0 * v0 + 2.0 * p->max_acceleration * x_end) - v0) / p->max_acceleration; const auto v_end = v0 + p->max_acceleration * t_end; diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp index 25101537850ae..461084c085ca7 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp @@ -383,7 +383,20 @@ AvoidanceParameters getParameter(rclcpp::Node * node) // debug { const std::string ns = "avoidance.debug."; - p.publish_debug_marker = getOrDeclareParameter(*node, ns + "marker"); + p.enable_other_objects_marker = + getOrDeclareParameter(*node, ns + "enable_other_objects_marker"); + p.enable_other_objects_info = + getOrDeclareParameter(*node, ns + "enable_other_objects_info"); + p.enable_detection_area_marker = + getOrDeclareParameter(*node, ns + "enable_detection_area_marker"); + p.enable_drivable_bound_marker = + getOrDeclareParameter(*node, ns + "enable_drivable_bound_marker"); + p.enable_safety_check_marker = + getOrDeclareParameter(*node, ns + "enable_safety_check_marker"); + p.enable_shift_line_marker = + getOrDeclareParameter(*node, ns + "enable_shift_line_marker"); + p.enable_lane_marker = getOrDeclareParameter(*node, ns + "enable_lane_marker"); + p.enable_misc_marker = getOrDeclareParameter(*node, ns + "enable_misc_marker"); } return p; diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp index 298406c44cf27..d7c101f885cdf 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp @@ -18,6 +18,7 @@ #include "behavior_path_avoidance_module/data_structs.hpp" #include "behavior_path_avoidance_module/helper.hpp" #include "behavior_path_avoidance_module/shift_line_generator.hpp" +#include "behavior_path_avoidance_module/type_alias.hpp" #include "behavior_path_planner_common/interface/scene_module_interface.hpp" #include "behavior_path_planner_common/interface/scene_module_visitor.hpp" @@ -25,12 +26,6 @@ #include #include -#include -#include -#include -#include -#include - #include #include #include @@ -39,12 +34,8 @@ namespace behavior_path_planner { -using motion_utils::calcSignedArcLength; -using motion_utils::findNearestIndex; - -using tier4_planning_msgs::msg::AvoidanceDebugMsg; - using helper::avoidance::AvoidanceHelper; +using tier4_planning_msgs::msg::AvoidanceDebugMsg; class AvoidanceModule : public SceneModuleInterface { @@ -114,7 +105,7 @@ class AvoidanceModule : public SceneModuleInterface for (const auto & left_shift : left_shift_array_) { const double start_distance = - calcSignedArcLength(path.points, ego_idx, left_shift.start_pose.position); + motion_utils::calcSignedArcLength(path.points, ego_idx, left_shift.start_pose.position); const double finish_distance = start_distance + left_shift.relative_longitudinal; rtc_interface_ptr_map_.at("left")->updateCooperateStatus( left_shift.uuid, true, start_distance, finish_distance, clock_->now()); @@ -127,7 +118,7 @@ class AvoidanceModule : public SceneModuleInterface for (const auto & right_shift : right_shift_array_) { const double start_distance = - calcSignedArcLength(path.points, ego_idx, right_shift.start_pose.position); + motion_utils::calcSignedArcLength(path.points, ego_idx, right_shift.start_pose.position); const double finish_distance = start_distance + right_shift.relative_longitudinal; rtc_interface_ptr_map_.at("right")->updateCooperateStatus( right_shift.uuid, true, start_distance, finish_distance, clock_->now()); diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/shift_line_generator.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/shift_line_generator.hpp index efdda622a664c..e721401207830 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/shift_line_generator.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/shift_line_generator.hpp @@ -17,6 +17,7 @@ #include "behavior_path_avoidance_module/data_structs.hpp" #include "behavior_path_avoidance_module/helper.hpp" +#include "behavior_path_avoidance_module/type_alias.hpp" #include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" #include diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/type_alias.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/type_alias.hpp new file mode 100644 index 0000000000000..8e7c7820a3650 --- /dev/null +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/type_alias.hpp @@ -0,0 +1,72 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef BEHAVIOR_PATH_AVOIDANCE_MODULE__TYPE_ALIAS_HPP_ +#define BEHAVIOR_PATH_AVOIDANCE_MODULE__TYPE_ALIAS_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace behavior_path_planner +{ +// auto msgs +using autoware_auto_perception_msgs::msg::PredictedObject; +using autoware_auto_perception_msgs::msg::PredictedPath; +using autoware_auto_planning_msgs::msg::PathWithLaneId; + +// ROS 2 general msgs +using geometry_msgs::msg::Point; +using geometry_msgs::msg::Pose; +using geometry_msgs::msg::TransformStamped; +using geometry_msgs::msg::Vector3; +using std_msgs::msg::ColorRGBA; +using visualization_msgs::msg::Marker; +using visualization_msgs::msg::MarkerArray; + +// tier4 msgs +using tier4_planning_msgs::msg::AvoidanceDebugMsg; +using tier4_planning_msgs::msg::AvoidanceDebugMsgArray; + +// tier4 utils functions +using tier4_autoware_utils::appendMarkerArray; +using tier4_autoware_utils::calcDistance2d; +using tier4_autoware_utils::calcLateralDeviation; +using tier4_autoware_utils::calcOffsetPose; +using tier4_autoware_utils::calcYawDeviation; +using tier4_autoware_utils::createDefaultMarker; +using tier4_autoware_utils::createMarkerColor; +using tier4_autoware_utils::createMarkerScale; +using tier4_autoware_utils::createPoint; +using tier4_autoware_utils::createQuaternionFromRPY; +using tier4_autoware_utils::getPoint; +using tier4_autoware_utils::getPose; +using tier4_autoware_utils::Point2d; +using tier4_autoware_utils::Polygon2d; +using tier4_autoware_utils::pose2transform; +using tier4_autoware_utils::toHexString; +} // namespace behavior_path_planner + +#endif // BEHAVIOR_PATH_AVOIDANCE_MODULE__TYPE_ALIAS_HPP_ diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp index d120aab4d6943..cf68f577ed7c0 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp @@ -25,6 +25,7 @@ namespace behavior_path_planner::utils::avoidance { + using behavior_path_planner::PlannerData; using behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObject; using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityAndPolygonStamped; diff --git a/planning/behavior_path_avoidance_module/schema/avoidance.schema.json b/planning/behavior_path_avoidance_module/schema/avoidance.schema.json index 7c0205d2473e5..e0f1156172932 100644 --- a/planning/behavior_path_avoidance_module/schema/avoidance.schema.json +++ b/planning/behavior_path_avoidance_module/schema/avoidance.schema.json @@ -1388,13 +1388,57 @@ "debug": { "type": "object", "properties": { - "marker": { + "enable_other_objects_marker": { "type": "boolean", - "description": "Publish debug marker.", + "description": "Publish other objects marker.", + "default": "false" + }, + "enable_other_objects_info": { + "type": "boolean", + "description": "Publish other objects detail information.", + "default": "false" + }, + "enable_detection_area_marker": { + "type": "boolean", + "description": "Publish detection area.", + "default": "false" + }, + "enable_drivable_bound_marker": { + "type": "boolean", + "description": "Publish drivable area boundary.", + "default": "false" + }, + "enable_safety_check_marker": { + "type": "boolean", + "description": "Publish safety check information.", + "default": "false" + }, + "enable_shift_line_marker": { + "type": "boolean", + "description": "Publish shift line information.", + "default": "false" + }, + "enable_lane_marker": { + "type": "boolean", + "description": "Publish lane information.", + "default": "false" + }, + "enable_misc_marker": { + "type": "boolean", + "description": "Publish misc markers.", "default": "false" } }, - "required": ["marker"], + "required": [ + "enable_other_objects_marker", + "enable_other_objects_info", + "enable_detection_area_marker", + "enable_drivable_bound_marker", + "enable_safety_check_marker", + "enable_shift_line_marker", + "enable_lane_marker", + "enable_misc_marker" + ], "additionalProperties": false } }, diff --git a/planning/behavior_path_avoidance_module/src/debug.cpp b/planning/behavior_path_avoidance_module/src/debug.cpp index 4595a77a84b3b..4d3b4605ac956 100644 --- a/planning/behavior_path_avoidance_module/src/debug.cpp +++ b/planning/behavior_path_avoidance_module/src/debug.cpp @@ -19,26 +19,12 @@ #include #include -#include - -#include #include #include -namespace marker_utils::avoidance_marker +namespace behavior_path_planner::utils::avoidance { - -using tier4_autoware_utils::appendMarkerArray; -using tier4_autoware_utils::calcDistance2d; -using tier4_autoware_utils::calcOffsetPose; -using tier4_autoware_utils::createDefaultMarker; -using tier4_autoware_utils::createMarkerColor; -using tier4_autoware_utils::createMarkerScale; -using tier4_autoware_utils::createPoint; -using tier4_autoware_utils::getPose; -using visualization_msgs::msg::Marker; - namespace { @@ -144,7 +130,8 @@ MarkerArray createToDrivableBoundDistance(const ObjectDataArray & objects, std:: return msg; } -MarkerArray createObjectInfoMarkerArray(const ObjectDataArray & objects, std::string && ns) +MarkerArray createObjectInfoMarkerArray( + const ObjectDataArray & objects, std::string && ns, const bool verbose) { MarkerArray msg; @@ -153,7 +140,7 @@ MarkerArray createObjectInfoMarkerArray(const ObjectDataArray & objects, std::st createMarkerScale(0.5, 0.5, 0.5), createMarkerColor(1.0, 1.0, 0.0, 1.0)); for (const auto & object : objects) { - { + if (verbose) { marker.id = uuidToInt32(object.object.object_id); marker.pose = object.object.kinematics.initial_pose_with_covariance.pose; std::ostringstream string_stream; @@ -174,9 +161,11 @@ MarkerArray createObjectInfoMarkerArray(const ObjectDataArray & objects, std::st { marker.id = uuidToInt32(object.object.object_id); + marker.pose = object.object.kinematics.initial_pose_with_covariance.pose; marker.pose.position.z += 2.0; std::ostringstream string_stream; - string_stream << object.reason << (object.is_parked ? "(PARKED)" : ""); + string_stream << magic_enum::enum_name(object.info) << (object.is_parked ? "(PARKED)" : ""); + string_stream << (object.is_ambiguous ? "(WAIT AND SEE)" : ""); marker.text = string_stream.str(); marker.color = createMarkerColor(1.0, 1.0, 1.0, 0.999); marker.scale = createMarkerScale(0.6, 0.6, 0.6); @@ -214,7 +203,7 @@ MarkerArray avoidableObjectsMarkerArray(const ObjectDataArray & objects, std::st createMarkerColor(1.0, 1.0, 0.0, 0.8)), &msg); - appendMarkerArray(createObjectInfoMarkerArray(objects, ns + "_info"), &msg); + appendMarkerArray(createObjectInfoMarkerArray(objects, ns + "_info", true), &msg); appendMarkerArray(createObjectPolygonMarkerArray(objects, ns + "_envelope_polygon"), &msg); appendMarkerArray(createToDrivableBoundDistance(objects, ns + "_to_drivable_bound"), &msg); appendMarkerArray(createOverhangLaneletMarkerArray(objects, ns + "_overhang_lanelet"), &msg); @@ -233,7 +222,7 @@ MarkerArray unAvoidableObjectsMarkerArray(const ObjectDataArray & objects, std:: createMarkerColor(1.0, 0.0, 0.0, 0.8)), &msg); - appendMarkerArray(createObjectInfoMarkerArray(objects, ns + "_info"), &msg); + appendMarkerArray(createObjectInfoMarkerArray(objects, ns + "_info", true), &msg); appendMarkerArray(createObjectPolygonMarkerArray(objects, ns + "_envelope_polygon"), &msg); appendMarkerArray(createToDrivableBoundDistance(objects, ns + "_to_drivable_bound"), &msg); appendMarkerArray(createOverhangLaneletMarkerArray(objects, ns + "_overhang_lanelet"), &msg); @@ -441,14 +430,13 @@ MarkerArray createTargetObjectsMarkerArray(const ObjectDataArray & objects, cons return msg; } -MarkerArray createOtherObjectsMarkerArray(const ObjectDataArray & objects, const std::string & ns) +MarkerArray createOtherObjectsMarkerArray( + const ObjectDataArray & objects, const ObjectInfo & info, const bool verbose) { - using behavior_path_planner::utils::convertToSnakeCase; - - const auto filtered_objects = [&objects, &ns]() { + const auto filtered_objects = [&objects, &info]() { ObjectDataArray ret{}; for (const auto & o : objects) { - if (o.reason != ns) { + if (o.info != info) { continue; } ret.push_back(o); @@ -460,18 +448,21 @@ MarkerArray createOtherObjectsMarkerArray(const ObjectDataArray & objects, const MarkerArray msg; msg.markers.reserve(filtered_objects.size() * 2); + std::ostringstream string_stream; + string_stream << magic_enum::enum_name(info); + + std::string ns = string_stream.str(); + transform(ns.begin(), ns.end(), ns.begin(), tolower); + appendMarkerArray( createObjectsCubeMarkerArray( - filtered_objects, "others_" + convertToSnakeCase(ns) + "_cube", - createMarkerScale(3.0, 1.5, 1.5), createMarkerColor(0.0, 1.0, 0.0, 0.8)), + filtered_objects, "others_" + ns + "_cube", createMarkerScale(3.0, 1.5, 1.5), + createMarkerColor(0.0, 1.0, 0.0, 0.8)), &msg); appendMarkerArray( - createObjectInfoMarkerArray(filtered_objects, "others_" + convertToSnakeCase(ns) + "_info"), - &msg); + createObjectInfoMarkerArray(filtered_objects, "others_" + ns + "_info", verbose), &msg); appendMarkerArray( - createOverhangLaneletMarkerArray( - filtered_objects, "others_" + convertToSnakeCase(ns) + "_overhang_lanelet"), - &msg); + createOverhangLaneletMarkerArray(filtered_objects, "others_" + ns + "_overhang_lanelet"), &msg); return msg; } @@ -513,7 +504,8 @@ MarkerArray createDrivableBounds( } MarkerArray createDebugMarkerArray( - const AvoidancePlanningData & data, const PathShifter & shifter, const DebugData & debug) + const AvoidancePlanningData & data, const PathShifter & shifter, const DebugData & debug, + const std::shared_ptr & parameters) { using behavior_path_planner::utils::transformToLanelets; using lanelet::visualization::laneletsAsTriangleMarkerArray; @@ -528,7 +520,6 @@ MarkerArray createDebugMarkerArray( using marker_utils::showPolygon; using marker_utils::showPredictedPath; using marker_utils::showSafetyCheckInfo; - using tier4_planning_msgs::msg::AvoidanceDebugFactor; const auto current_time = rclcpp::Clock{RCL_ROS_TIME}.now(); MarkerArray msg; @@ -548,7 +539,7 @@ MarkerArray createDebugMarkerArray( }; const auto addObjects = [&](const ObjectDataArray & objects, const auto & ns) { - add(createOtherObjectsMarkerArray(objects, ns)); + add(createOtherObjectsMarkerArray(objects, ns, parameters->enable_other_objects_info)); }; const auto addShiftLength = @@ -563,29 +554,27 @@ MarkerArray createDebugMarkerArray( }; // ignore objects - { - addObjects(data.other_objects, AvoidanceDebugFactor::OBJECT_IS_BEHIND_THRESHOLD); - addObjects(data.other_objects, AvoidanceDebugFactor::OBJECT_IS_IN_FRONT_THRESHOLD); - addObjects(data.other_objects, AvoidanceDebugFactor::OBJECT_IS_NOT_TYPE); - addObjects(data.other_objects, AvoidanceDebugFactor::OBJECT_BEHIND_PATH_GOAL); - addObjects(data.other_objects, AvoidanceDebugFactor::TOO_NEAR_TO_CENTERLINE); - addObjects(data.other_objects, AvoidanceDebugFactor::NOT_PARKING_OBJECT); - addObjects(data.other_objects, std::string("MovingObject")); - addObjects(data.other_objects, std::string("CrosswalkUser")); - addObjects(data.other_objects, std::string("OutOfTargetArea")); - addObjects(data.other_objects, std::string("NotNeedAvoidance")); - addObjects(data.other_objects, std::string("LessThanExecutionThreshold")); - addObjects(data.other_objects, std::string("TooNearToGoal")); - addObjects(data.other_objects, std::string("ParallelToEgoLane")); - addObjects(data.other_objects, std::string("MergingToEgoLane")); - addObjects(data.other_objects, std::string("DeviatingFromEgoLane")); - addObjects(data.other_objects, std::string("UnstableObject")); - addObjects(data.other_objects, std::string("AmbiguousStoppedVehicle")); - addObjects(data.other_objects, std::string("AmbiguousStoppedVehicle(wait-and-see)")); + if (parameters->enable_other_objects_marker) { + addObjects(data.other_objects, ObjectInfo::FURTHER_THAN_THRESHOLD); + addObjects(data.other_objects, ObjectInfo::IS_NOT_TARGET_OBJECT); + addObjects(data.other_objects, ObjectInfo::FURTHER_THAN_GOAL); + addObjects(data.other_objects, ObjectInfo::TOO_NEAR_TO_CENTERLINE); + addObjects(data.other_objects, ObjectInfo::IS_NOT_PARKING_OBJECT); + addObjects(data.other_objects, ObjectInfo::MOVING_OBJECT); + addObjects(data.other_objects, ObjectInfo::CROSSWALK_USER); + addObjects(data.other_objects, ObjectInfo::OUT_OF_TARGET_AREA); + addObjects(data.other_objects, ObjectInfo::ENOUGH_LATERAL_DISTANCE); + addObjects(data.other_objects, ObjectInfo::LESS_THAN_EXECUTION_THRESHOLD); + addObjects(data.other_objects, ObjectInfo::TOO_NEAR_TO_GOAL); + addObjects(data.other_objects, ObjectInfo::PARALLEL_TO_EGO_LANE); + addObjects(data.other_objects, ObjectInfo::MERGING_TO_EGO_LANE); + addObjects(data.other_objects, ObjectInfo::DEVIATING_FROM_EGO_LANE); + addObjects(data.other_objects, ObjectInfo::UNSTABLE_OBJECT); + addObjects(data.other_objects, ObjectInfo::AMBIGUOUS_STOPPED_VEHICLE); } // shift line pre-process - { + if (parameters->enable_shift_line_marker) { addAvoidLine(debug.step1_registered_shift_line, "step1_registered_shift_line", 0.2, 0.2, 1.0); addAvoidLine(debug.step1_current_shift_line, "step1_current_shift_line", 0.2, 0.4, 0.8, 0.3); addAvoidLine(debug.step1_merged_shift_line, "step1_merged_shift_line", 0.2, 0.6, 0.6, 0.3); @@ -594,39 +583,39 @@ MarkerArray createDebugMarkerArray( } // merge process - { + if (parameters->enable_shift_line_marker) { addAvoidLine(debug.step2_merged_shift_line, "step2_merged_shift_line", 0.2, 1.0, 0.0, 0.3); } // trimming process - { + if (parameters->enable_shift_line_marker) { addAvoidLine(debug.step3_grad_filtered_1st, "step3_grad_filtered_1st", 0.2, 0.8, 0.0, 0.3); addAvoidLine(debug.step3_grad_filtered_2nd, "step3_grad_filtered_2nd", 0.4, 0.6, 0.0, 0.3); addAvoidLine(debug.step3_grad_filtered_3rd, "step3_grad_filtered_3rd", 0.6, 0.4, 0.0, 0.3); } // registering process - { + if (parameters->enable_shift_line_marker) { addShiftLine(shifter.getShiftLines(), "step4_old_shift_line", 1.0, 1.0, 0.0, 0.3); addAvoidLine(data.new_shift_line, "step4_new_shift_line", 1.0, 0.0, 0.0, 0.3); } // safety check - { + if (parameters->enable_safety_check_marker) { add(showSafetyCheckInfo(debug.collision_check, "object_debug_info")); add(showPredictedPath(debug.collision_check, "ego_predicted_path")); add(showPolygon(debug.collision_check, "ego_and_target_polygon_relation")); } // shift length - { + if (parameters->enable_shift_line_marker) { addShiftLength(debug.pos_shift, "merged_length_pos", 0.0, 0.7, 0.5); addShiftLength(debug.neg_shift, "merged_length_neg", 0.0, 0.5, 0.7); addShiftLength(debug.total_shift, "merged_length_total", 0.99, 0.4, 0.2); } // shift grad - { + if (parameters->enable_shift_line_marker) { addShiftGrad(debug.pos_shift_grad, debug.pos_shift, "merged_grad_pos", 0.0, 0.7, 0.5); addShiftGrad(debug.neg_shift_grad, debug.neg_shift, "merged_grad_neg", 0.0, 0.5, 0.7); addShiftGrad(debug.total_forward_grad, debug.total_shift, "grad_forward", 0.99, 0.4, 0.2); @@ -634,15 +623,20 @@ MarkerArray createDebugMarkerArray( } // detection area - size_t i = 0; - for (const auto & detection_area : debug.detection_areas) { - add(createPolygonMarkerArray(detection_area, "detection_area", i++, 0.16, 1.0, 0.69, 0.1)); + if (parameters->enable_detection_area_marker) { + size_t i = 0; + for (const auto & detection_area : debug.detection_areas) { + add(createPolygonMarkerArray(detection_area, "detection_area", i++, 0.16, 1.0, 0.69, 0.1)); + } } - // misc - { - add(createPathMarkerArray(path, "centerline_resampled", 0, 0.0, 0.9, 0.5)); + // drivable bound + if (parameters->enable_drivable_bound_marker) { add(createDrivableBounds(data, "drivable_bound", 1.0, 0.0, 0.42)); + } + + // lane + if (parameters->enable_lane_marker) { add(laneletsAsTriangleMarkerArray( "drivable_lanes", transformToLanelets(data.drivable_lanes), createMarkerColor(0.16, 1.0, 0.69, 0.2))); @@ -652,9 +646,14 @@ MarkerArray createDebugMarkerArray( "safety_check_lanes", debug.safety_check_lanes, createMarkerColor(1.0, 0.0, 0.42, 0.2))); } + // misc + if (parameters->enable_misc_marker) { + add(createPathMarkerArray(path, "centerline_resampled", 0, 0.0, 0.9, 0.5)); + } + return msg; } -} // namespace marker_utils::avoidance_marker +} // namespace behavior_path_planner::utils::avoidance std::string toStrInfo(const behavior_path_planner::ShiftLineArray & sl_arr) { diff --git a/planning/behavior_path_avoidance_module/src/manager.cpp b/planning/behavior_path_avoidance_module/src/manager.cpp index 7772bd9c2ad35..bdf6f4e822f7b 100644 --- a/planning/behavior_path_avoidance_module/src/manager.cpp +++ b/planning/behavior_path_avoidance_module/src/manager.cpp @@ -261,7 +261,17 @@ void AvoidanceModuleManager::updateModuleParams(const std::vector(parameters, ns + "marker", p->publish_debug_marker); + updateParam( + parameters, ns + "enable_other_objects_marker", p->enable_other_objects_marker); + updateParam(parameters, ns + "enable_other_objects_info", p->enable_other_objects_info); + updateParam( + parameters, ns + "enable_detection_area_marker", p->enable_detection_area_marker); + updateParam( + parameters, ns + "enable_drivable_bound_marker", p->enable_drivable_bound_marker); + updateParam(parameters, ns + "enable_safety_check_marker", p->enable_safety_check_marker); + updateParam(parameters, ns + "enable_shift_line_marker", p->enable_shift_line_marker); + updateParam(parameters, ns + "enable_lane_marker", p->enable_lane_marker); + updateParam(parameters, ns + "enable_misc_marker", p->enable_misc_marker); } std::for_each(observers_.begin(), observers_.end(), [&p](const auto & observer) { diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp index b70b606d8f279..d0afe25949bcb 100644 --- a/planning/behavior_path_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_module/src/scene.cpp @@ -26,12 +26,6 @@ #include #include -#include -#include -#include - -#include "tier4_planning_msgs/msg/detail/avoidance_debug_msg_array__struct.hpp" -#include #include #include @@ -42,18 +36,6 @@ namespace behavior_path_planner { - -using motion_utils::calcLongitudinalOffsetPose; -using motion_utils::calcSignedArcLength; -using motion_utils::findNearestIndex; -using tier4_autoware_utils::appendMarkerArray; -using tier4_autoware_utils::calcDistance2d; -using tier4_autoware_utils::calcLateralDeviation; -using tier4_autoware_utils::getPoint; -using tier4_autoware_utils::getPose; -using tier4_autoware_utils::toHexString; -using tier4_planning_msgs::msg::AvoidanceDebugFactor; - namespace { bool isDrivingSameLane( @@ -119,9 +101,8 @@ bool AvoidanceModule::isExecutionRequested() const } return std::any_of( - avoid_data_.target_objects.begin(), avoid_data_.target_objects.end(), [](const auto & o) { - return o.is_avoidable || o.reason == AvoidanceDebugFactor::TOO_LARGE_JERK; - }); + avoid_data_.target_objects.begin(), avoid_data_.target_objects.end(), + [this](const auto & o) { return !helper_->isAbsolutelyNotAvoidable(o); }); } bool AvoidanceModule::isExecutionReady() const @@ -136,10 +117,9 @@ bool AvoidanceModule::isExecutionReady() const bool AvoidanceModule::isSatisfiedSuccessCondition(const AvoidancePlanningData & data) const { - const bool has_avoidance_target = - std::any_of(data.target_objects.begin(), data.target_objects.end(), [](const auto & o) { - return o.is_avoidable || o.reason == AvoidanceDebugFactor::TOO_LARGE_JERK; - }); + const bool has_avoidance_target = std::any_of( + data.target_objects.begin(), data.target_objects.end(), + [this](const auto & o) { return !helper_->isAbsolutelyNotAvoidable(o); }); if (has_avoidance_target) { return false; @@ -285,7 +265,7 @@ void AvoidanceModule::fillFundamentalData(AvoidancePlanningData & data, DebugDat // arclength from ego pose (used in many functions) data.arclength_from_ego = utils::calcPathArcLengthArray( data.reference_path, 0, data.reference_path.points.size(), - calcSignedArcLength(data.reference_path.points, getEgoPosition(), 0)); + motion_utils::calcSignedArcLength(data.reference_path.points, getEgoPosition(), 0)); data.to_return_point = utils::avoidance::calcDistanceToReturnDeadLine( data.current_lanelets, data.reference_path_rough, planner_data_, parameters_); @@ -337,7 +317,7 @@ void AvoidanceModule::fillAvoidanceTargetObjects( for (const auto & object : object_outside_target_lane.objects) { ObjectData other_object = createObjectData(data, object); - other_object.reason = "OutOfTargetArea"; + other_object.info = ObjectInfo::OUT_OF_TARGET_AREA; data.other_objects.push_back(other_object); } @@ -367,8 +347,6 @@ void AvoidanceModule::fillAvoidanceTargetObjects( debug_info.object_id = toHexString(o.object.object_id); debug_info.longitudinal_distance = o.longitudinal; debug_info.lateral_distance_from_centerline = o.to_centerline; - debug_info.allow_avoidance = o.reason == ""; - debug_info.failed_reason = o.reason; debug_info_array.push_back(debug_info); }; @@ -385,7 +363,8 @@ ObjectData AvoidanceModule::createObjectData( const auto & path_points = data.reference_path.points; const auto & object_pose = object.kinematics.initial_pose_with_covariance.pose; - const auto object_closest_index = findNearestIndex(path_points, object_pose.position); + const auto object_closest_index = + motion_utils::findNearestIndex(path_points, object_pose.position); const auto object_closest_pose = path_points.at(object_closest_index).point.pose; const auto object_type = utils::getHighestProbLabel(object.classification); const auto object_parameter = parameters_->object_parameters.at(object_type); @@ -430,7 +409,7 @@ bool AvoidanceModule::canYieldManeuver(const AvoidancePlanningData & data) const const auto registered_lines = path_shifter_.getShiftLines(); if (!registered_lines.empty()) { const size_t idx = planner_data_->findEgoIndex(path_shifter_.getReferencePath().points); - const auto to_shift_start_point = calcSignedArcLength( + const auto to_shift_start_point = motion_utils::calcSignedArcLength( path_shifter_.getReferencePath().points, idx, registered_lines.front().start_idx); if (to_shift_start_point < helper_->getMinimumPrepareDistance()) { RCLCPP_DEBUG( @@ -530,12 +509,11 @@ void AvoidanceModule::fillEgoStatus( * if the both of following two conditions are satisfied, the module surely avoid the object. * Condition1: there is risk to collide with object without avoidance. * Condition2: there is enough space to avoid. - * In TOO_LARGE_JERK condition, it is possible to avoid object by deceleration even if the flag + * In NEED_DECELERATION condition, it is possible to avoid object by deceleration even if the flag * is_avoidable is FALSE. So, the module inserts stop point for such a object. */ for (const auto & o : data.target_objects) { - const auto enough_space = o.is_avoidable || o.reason == AvoidanceDebugFactor::TOO_LARGE_JERK; - if (o.avoid_required && enough_space) { + if (o.avoid_required && !helper_->isAbsolutelyNotAvoidable(o)) { data.avoid_required = true; data.stop_target_object = o; data.to_stop_line = o.to_stop_line; @@ -647,7 +625,7 @@ void AvoidanceModule::fillDebugData( const auto prepare_distance = helper_->getNominalPrepareDistance(); const auto total_avoid_distance = prepare_distance + avoidance_distance + constant_distance; - dead_pose_ = calcLongitudinalOffsetPose( + dead_pose_ = motion_utils::calcLongitudinalOffsetPose( data.reference_path.points, getEgoPosition(), o_front.longitudinal - total_avoid_distance); if (!dead_pose_) { @@ -834,8 +812,8 @@ PathWithLaneId AvoidanceModule::extendBackwardLength(const PathWithLaneId & orig return a.start_idx < b.start_idx; }); return std::max( - max_dist, - calcSignedArcLength(previous_path.points, lines.front().start.position, getEgoPosition())); + max_dist, motion_utils::calcSignedArcLength( + previous_path.points, lines.front().start.position, getEgoPosition())); }(); const auto extra_margin = 10.0; // Since distance does not consider arclength, but just line. @@ -852,7 +830,9 @@ PathWithLaneId AvoidanceModule::extendBackwardLength(const PathWithLaneId & orig size_t clip_idx = 0; for (size_t i = 0; i < prev_ego_idx; ++i) { - if (backward_length > calcSignedArcLength(previous_path.points, clip_idx, *prev_ego_idx)) { + if ( + backward_length > + motion_utils::calcSignedArcLength(previous_path.points, clip_idx, *prev_ego_idx)) { break; } clip_idx = i; @@ -1370,7 +1350,7 @@ void AvoidanceModule::updateRTCData() void AvoidanceModule::updateInfoMarker(const AvoidancePlanningData & data) const { - using marker_utils::avoidance_marker::createTargetObjectsMarkerArray; + using utils::avoidance::createTargetObjectsMarkerArray; info_marker_.markers.clear(); appendMarkerArray( @@ -1381,12 +1361,7 @@ void AvoidanceModule::updateDebugMarker( const AvoidancePlanningData & data, const PathShifter & shifter, const DebugData & debug) const { debug_marker_.markers.clear(); - - if (!parameters_->publish_debug_marker) { - return; - } - - debug_marker_ = marker_utils::avoidance_marker::createDebugMarkerArray(data, shifter, debug); + debug_marker_ = utils::avoidance::createDebugMarkerArray(data, shifter, debug, parameters_); } void AvoidanceModule::updateAvoidanceDebugData( @@ -1465,7 +1440,7 @@ void AvoidanceModule::insertReturnDeadLine( // Consider the difference in path length between the shifted path and original path (the path // that is shifted inward has a shorter distance to the end of the path than the other one.) const auto & to_reference_path_end = data.arclength_from_ego.back(); - const auto to_shifted_path_end = calcSignedArcLength( + const auto to_shifted_path_end = motion_utils::calcSignedArcLength( shifted_path.path.points, getEgoPosition(), shifted_path.path.points.size() - 1); const auto buffer = std::max(0.0, to_shifted_path_end - to_reference_path_end); @@ -1502,7 +1477,8 @@ void AvoidanceModule::insertReturnDeadLine( const auto start_idx = planner_data_->findEgoIndex(shifted_path.path.points); for (size_t i = start_idx; i < shifted_path.path.points.size(); ++i) { - const auto distance_from_ego = calcSignedArcLength(shifted_path.path.points, start_idx, i); + const auto distance_from_ego = + motion_utils::calcSignedArcLength(shifted_path.path.points, start_idx, i); // slow down speed is inserted only in front of the object. const auto shift_longitudinal_distance = to_stop_line - distance_from_ego; @@ -1596,7 +1572,7 @@ void AvoidanceModule::insertStopPoint( }(); const auto stop_distance = - calcSignedArcLength(shifted_path.path.points, getEgoPosition(), stop_idx); + motion_utils::calcSignedArcLength(shifted_path.path.points, getEgoPosition(), stop_idx); // If we don't need to consider deceleration constraints, insert a deceleration point // and return immediately @@ -1667,9 +1643,7 @@ void AvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_path) const return; } - const auto enough_space = - object.value().is_avoidable || object.value().reason == AvoidanceDebugFactor::TOO_LARGE_JERK; - if (!enough_space) { + if (helper_->isAbsolutelyNotAvoidable(object.value())) { return; } @@ -1709,7 +1683,8 @@ void AvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_path) const const auto start_idx = planner_data_->findEgoIndex(shifted_path.path.points); for (size_t i = start_idx; i < shifted_path.path.points.size(); ++i) { - const auto distance_from_ego = calcSignedArcLength(shifted_path.path.points, start_idx, i); + const auto distance_from_ego = + motion_utils::calcSignedArcLength(shifted_path.path.points, start_idx, i); // slow down speed is inserted only in front of the object. const auto shift_longitudinal_distance = distance_to_object - distance_from_ego; @@ -1752,7 +1727,8 @@ void AvoidanceModule::insertAvoidanceVelocity(ShiftedPath & shifted_path) const const auto start_idx = planner_data_->findEgoIndex(shifted_path.path.points); for (size_t i = start_idx; i < shifted_path.path.points.size(); ++i) { - const auto distance_from_ego = calcSignedArcLength(shifted_path.path.points, start_idx, i); + const auto distance_from_ego = + motion_utils::calcSignedArcLength(shifted_path.path.points, start_idx, i); // slow down speed is inserted only in front of the object. const auto accel_distance = distance_to_accel_end_point - distance_from_ego; diff --git a/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp b/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp index 9137048945fa1..84b2b9b61a702 100644 --- a/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp +++ b/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp @@ -17,17 +17,9 @@ #include "behavior_path_avoidance_module/utils.hpp" #include "behavior_path_planner_common/utils/utils.hpp" -#include - -#include - namespace behavior_path_planner::utils::avoidance { -using tier4_autoware_utils::generateUUID; -using tier4_autoware_utils::getPoint; -using tier4_planning_msgs::msg::AvoidanceDebugFactor; - namespace { bool isBestEffort(const std::string & policy) @@ -182,7 +174,7 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( // prepare distance is not enough. unavoidable. if (avoidance_distance < 1e-3) { - object.reason = AvoidanceDebugFactor::REMAINING_DISTANCE_LESS_THAN_ZERO; + object.info = ObjectInfo::INSUFFICIENT_LONGITUDINAL_DISTANCE; return std::nullopt; } @@ -200,10 +192,10 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( // avoidance distance is not enough. unavoidable. if (!isBestEffort(parameters_->policy_deceleration)) { if (avoidance_distance < helper_->getMinAvoidanceDistance(avoiding_shift) + LON_DIST_BUFFER) { - object.reason = AvoidanceDebugFactor::REMAINING_DISTANCE_LESS_THAN_ZERO; + object.info = ObjectInfo::INSUFFICIENT_LONGITUDINAL_DISTANCE; return std::nullopt; } else { - object.reason = AvoidanceDebugFactor::TOO_LARGE_JERK; + object.info = ObjectInfo::NEED_DECELERATION; return std::nullopt; } } @@ -213,7 +205,7 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( avoidance_distance, helper_->getLateralMaxJerkLimit(), helper_->getAvoidanceEgoSpeed()); if (std::abs(feasible_relative_shift_length) < parameters_->lateral_execution_threshold) { - object.reason = "LessThanExecutionThreshold"; + object.info = ObjectInfo::LESS_THAN_EXECUTION_THRESHOLD; return std::nullopt; } @@ -224,7 +216,7 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( if ( avoidance_distance < helper_->getMinAvoidanceDistance(feasible_shift_length) + LON_DIST_BUFFER) { - object.reason = AvoidanceDebugFactor::REMAINING_DISTANCE_LESS_THAN_ZERO; + object.info = ObjectInfo::INSUFFICIENT_LONGITUDINAL_DISTANCE; return std::nullopt; } @@ -238,7 +230,7 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( 0.5 * data_->parameters.vehicle_width + lateral_hard_margin; if (infeasible) { RCLCPP_DEBUG(rclcpp::get_logger(""), "feasible shift length is not enough to avoid. "); - object.reason = AvoidanceDebugFactor::TOO_LARGE_JERK; + object.info = ObjectInfo::NEED_DECELERATION; return std::nullopt; } @@ -254,7 +246,7 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( AvoidOutlines outlines; for (auto & o : data.target_objects) { if (!o.avoid_margin.has_value()) { - o.reason = AvoidanceDebugFactor::INSUFFICIENT_LATERAL_MARGIN; + o.info = ObjectInfo::INSUFFICIENT_DRIVABLE_SPACE; if (o.avoid_required && is_forward_object(o)) { break; } else { @@ -266,7 +258,7 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( const auto desire_shift_length = helper_->getShiftLength(o, is_object_on_right, o.avoid_margin.value()); if (utils::avoidance::isSameDirectionShift(is_object_on_right, desire_shift_length)) { - o.reason = AvoidanceDebugFactor::SAME_DIRECTION_SHIFT; + o.info = ObjectInfo::SAME_DIRECTION_SHIFT; if (o.avoid_required && is_forward_object(o)) { break; } else { @@ -377,7 +369,7 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( } else if (is_valid_shift_line(al_avoid) && is_valid_shift_line(al_return)) { outlines.emplace_back(al_avoid, al_return); } else { - o.reason = "InvalidShiftLine"; + o.info = ObjectInfo::INVALID_SHIFT_LINE; continue; } diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp index b711dcb91236d..1e83769dde599 100644 --- a/planning/behavior_path_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_avoidance_module/src/utils.cpp @@ -20,14 +20,8 @@ #include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner_common/utils/path_utils.hpp" #include "behavior_path_planner_common/utils/traffic_light_utils.hpp" -#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" #include -#include -#include - -#include -#include #include #include @@ -49,21 +43,6 @@ namespace behavior_path_planner::utils::avoidance using autoware_perception_msgs::msg::TrafficSignalElement; using behavior_path_planner::utils::traffic_light::calcDistanceToRedTrafficLight; using behavior_path_planner::utils::traffic_light::getDistanceToNextTrafficLight; -using geometry_msgs::msg::TransformStamped; -using motion_utils::calcLongitudinalOffsetPoint; -using motion_utils::calcSignedArcLength; -using motion_utils::findNearestIndex; -using motion_utils::findNearestSegmentIndex; -using motion_utils::insertTargetPoint; -using tier4_autoware_utils::calcDistance2d; -using tier4_autoware_utils::calcLateralDeviation; -using tier4_autoware_utils::calcOffsetPose; -using tier4_autoware_utils::calcYawDeviation; -using tier4_autoware_utils::createQuaternionFromRPY; -using tier4_autoware_utils::getPose; -using tier4_autoware_utils::pose2transform; -using tier4_planning_msgs::msg::AvoidanceDebugFactor; -using tier4_planning_msgs::msg::AvoidanceDebugMsg; namespace { @@ -569,14 +548,14 @@ bool isNeverAvoidanceTarget( { if (object.is_within_intersection) { if (object.behavior == ObjectData::Behavior::NONE) { - object.reason = "ParallelToEgoLane"; + object.info = ObjectInfo::PARALLEL_TO_EGO_LANE; RCLCPP_DEBUG( rclcpp::get_logger(logger_namespace), "object belongs to ego lane. never avoid it."); return true; } if (object.behavior == ObjectData::Behavior::MERGING) { - object.reason = "MergingToEgoLane"; + object.info = ObjectInfo::MERGING_TO_EGO_LANE; RCLCPP_DEBUG( rclcpp::get_logger(logger_namespace), "object belongs to ego lane. never avoid it."); return true; @@ -584,7 +563,7 @@ bool isNeverAvoidanceTarget( } if (object.behavior == ObjectData::Behavior::MERGING) { - object.reason = "MergingToEgoLane"; + object.info = ObjectInfo::MERGING_TO_EGO_LANE; if ( isOnRight(object) && !object.is_parked && object.overhang_points.front().first > parameters->th_overhang_distance) { @@ -604,7 +583,7 @@ bool isNeverAvoidanceTarget( } if (object.behavior == ObjectData::Behavior::DEVIATING) { - object.reason = "DeviatingFromEgoLane"; + object.info = ObjectInfo::DEVIATING_FROM_EGO_LANE; if ( isOnRight(object) && !object.is_parked && object.overhang_points.front().first > parameters->th_overhang_distance) { @@ -629,7 +608,7 @@ bool isNeverAvoidanceTarget( const auto left_lane = planner_data->route_handler->getLeftLanelet(object.overhang_lanelet, true, false); if (right_lane.has_value() && left_lane.has_value()) { - object.reason = AvoidanceDebugFactor::NOT_PARKING_OBJECT; + object.info = ObjectInfo::IS_NOT_PARKING_OBJECT; RCLCPP_DEBUG( rclcpp::get_logger(logger_namespace), "object isn't on the edge lane. never avoid it."); return true; @@ -638,7 +617,7 @@ bool isNeverAvoidanceTarget( if (isCloseToStopFactor(object, data, planner_data, parameters)) { if (object.is_on_ego_lane && !object.is_parked) { - object.reason = AvoidanceDebugFactor::NOT_PARKING_OBJECT; + object.info = ObjectInfo::IS_NOT_PARKING_OBJECT; RCLCPP_DEBUG( rclcpp::get_logger(logger_namespace), "object is close to stop factor. never avoid it."); return true; @@ -666,11 +645,11 @@ bool isObviousAvoidanceTarget( } if (!object.is_parked) { - object.reason = AvoidanceDebugFactor::NOT_PARKING_OBJECT; + object.info = ObjectInfo::IS_NOT_PARKING_OBJECT; } if (object.behavior == ObjectData::Behavior::MERGING) { - object.reason = "MergingToEgoLane"; + object.info = ObjectInfo::MERGING_TO_EGO_LANE; } return false; @@ -683,13 +662,13 @@ bool isSatisfiedWithCommonCondition( { // Step1. filtered by target object type. if (!isAvoidanceTargetObjectType(object.object, parameters)) { - object.reason = AvoidanceDebugFactor::OBJECT_IS_NOT_TYPE; + object.info = ObjectInfo::IS_NOT_TARGET_OBJECT; return false; } // Step2. filtered stopped objects. if (filtering_utils::isMovingObject(object, parameters)) { - object.reason = AvoidanceDebugFactor::MOVING_OBJECT; + object.info = ObjectInfo::MOVING_OBJECT; return false; } @@ -698,12 +677,12 @@ bool isSatisfiedWithCommonCondition( fillLongitudinalAndLengthByClosestEnvelopeFootprint(data.reference_path_rough, ego_pos, object); if (object.longitudinal < -parameters->object_check_backward_distance) { - object.reason = AvoidanceDebugFactor::OBJECT_IS_BEHIND_THRESHOLD; + object.info = ObjectInfo::FURTHER_THAN_THRESHOLD; return false; } if (object.longitudinal > forward_detection_range) { - object.reason = AvoidanceDebugFactor::OBJECT_IS_IN_FRONT_THRESHOLD; + object.info = ObjectInfo::FURTHER_THAN_THRESHOLD; return false; } @@ -714,12 +693,12 @@ bool isSatisfiedWithCommonCondition( const auto ego_idx = planner_data->findEgoIndex(data.reference_path_rough.points); const auto to_goal_distance = rh->isInGoalRouteSection(data.current_lanelets.back()) - ? calcSignedArcLength( + ? motion_utils::calcSignedArcLength( data.reference_path_rough.points, ego_idx, data.reference_path_rough.points.size() - 1) : std::numeric_limits::max(); if (object.longitudinal > to_goal_distance) { - object.reason = AvoidanceDebugFactor::OBJECT_BEHIND_PATH_GOAL; + object.info = ObjectInfo::FURTHER_THAN_GOAL; return false; } @@ -727,7 +706,7 @@ bool isSatisfiedWithCommonCondition( if ( object.longitudinal + object.length / 2 + parameters->object_check_goal_distance > to_goal_distance) { - object.reason = "TooNearToGoal"; + object.info = ObjectInfo::TOO_NEAR_TO_GOAL; return false; } } @@ -742,7 +721,7 @@ bool isSatisfiedWithNonVehicleCondition( { // avoidance module ignore pedestrian and bicycle around crosswalk if (isWithinCrosswalk(object, planner_data->route_handler->getOverallGraphPtr())) { - object.reason = "CrosswalkUser"; + object.info = ObjectInfo::CROSSWALK_USER; return false; } @@ -751,7 +730,7 @@ bool isSatisfiedWithNonVehicleCondition( object.to_centerline = lanelet::utils::getArcCoordinates(data.current_lanelets, object_pose).distance; if (std::abs(object.to_centerline) < parameters->threshold_distance_object_is_on_center) { - object.reason = AvoidanceDebugFactor::TOO_NEAR_TO_CENTERLINE; + object.info = ObjectInfo::TOO_NEAR_TO_CENTERLINE; return false; } @@ -788,14 +767,14 @@ bool isSatisfiedWithVehicleCondition( // from here, filtering for ambiguous vehicle. if (!parameters->enable_avoidance_for_ambiguous_vehicle) { - object.reason = "AmbiguousStoppedVehicle"; + object.info = ObjectInfo::AMBIGUOUS_STOPPED_VEHICLE; return false; } const auto stop_time_longer_than_threshold = object.stop_time > parameters->time_threshold_for_ambiguous_vehicle; if (!stop_time_longer_than_threshold) { - object.reason = "AmbiguousStoppedVehicle(wait-and-see)"; + object.info = ObjectInfo::AMBIGUOUS_STOPPED_VEHICLE; return false; } @@ -804,25 +783,25 @@ bool isSatisfiedWithVehicleCondition( calcDistance2d(object.init_pose, current_pose) > parameters->distance_threshold_for_ambiguous_vehicle; if (is_moving_distance_longer_than_threshold) { - object.reason = "AmbiguousStoppedVehicle"; + object.info = ObjectInfo::AMBIGUOUS_STOPPED_VEHICLE; return false; } if (object.is_within_intersection) { if (object.behavior == ObjectData::Behavior::DEVIATING) { - object.reason = "AmbiguousStoppedVehicle(wait-and-see)"; + object.info = ObjectInfo::AMBIGUOUS_STOPPED_VEHICLE; object.is_ambiguous = true; return true; } } else { if (object.behavior == ObjectData::Behavior::MERGING) { - object.reason = "AmbiguousStoppedVehicle(wait-and-see)"; + object.info = ObjectInfo::AMBIGUOUS_STOPPED_VEHICLE; object.is_ambiguous = true; return true; } if (object.behavior == ObjectData::Behavior::DEVIATING) { - object.reason = "AmbiguousStoppedVehicle(wait-and-see)"; + object.info = ObjectInfo::AMBIGUOUS_STOPPED_VEHICLE; object.is_ambiguous = true; return true; } @@ -833,7 +812,7 @@ bool isSatisfiedWithVehicleCondition( } } - object.reason = AvoidanceDebugFactor::NOT_PARKING_OBJECT; + object.info = ObjectInfo::IS_NOT_PARKING_OBJECT; return false; } @@ -847,12 +826,12 @@ bool isNoNeedAvoidanceBehavior( const auto shift_length = calcShiftLength( isOnRight(object), object.overhang_points.front().first, object.avoid_margin.value()); if (!isShiftNecessary(isOnRight(object), shift_length)) { - object.reason = "NotNeedAvoidance"; + object.info = ObjectInfo::ENOUGH_LATERAL_DISTANCE; return true; } if (std::abs(shift_length) < parameters->lateral_execution_threshold) { - object.reason = "LessThanExecutionThreshold"; + object.info = ObjectInfo::LESS_THAN_EXECUTION_THRESHOLD; return true; } @@ -901,7 +880,7 @@ double getRoadShoulderDistance( const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose; const auto object_closest_index = - findNearestIndex(data.reference_path.points, object_pose.position); + motion_utils::findNearestIndex(data.reference_path.points, object_pose.position); const auto object_closest_pose = data.reference_path.points.at(object_closest_index).point.pose; const auto rh = planner_data->route_handler; @@ -933,8 +912,14 @@ double getRoadShoulderDistance( } } + const auto envelope_polygon_width = + boost::geometry::area(object.envelope_poly) / + std::max(object.length, 1e-3); // prevent division by zero + { - const auto p2 = calcOffsetPose(p_tmp, 0.0, (isOnRight(object) ? -1.0 : 1.0), 0.0).position; + const auto p2 = + calcOffsetPose(p_tmp, 0.0, (isOnRight(object) ? -0.5 : 0.5) * envelope_polygon_width, 0.0) + .position; const auto opt_intersect = tier4_autoware_utils::intersect(p1.second, p2, bound.at(i - 1), bound.at(i)); @@ -1144,7 +1129,7 @@ void fillLongitudinalAndLengthByClosestEnvelopeFootprint( for (const auto & p : obj.envelope_poly.outer()) { const auto point = tier4_autoware_utils::createPoint(p.x(), p.y(), 0.0); // TODO(someone): search around first position where the ego should avoid the object. - const double arc_length = calcSignedArcLength(path.points, ego_pos, point); + const double arc_length = motion_utils::calcSignedArcLength(path.points, ego_pos, point); min_distance = std::min(min_distance, arc_length); max_distance = std::max(max_distance, arc_length); } @@ -1161,7 +1146,7 @@ std::vector> calcEnvelopeOverhangDistance( for (const auto & p : object_data.envelope_poly.outer()) { const auto point = tier4_autoware_utils::createPoint(p.x(), p.y(), 0.0); // TODO(someone): search around first position where the ego should avoid the object. - const auto idx = findNearestIndex(path.points, point); + const auto idx = motion_utils::findNearestIndex(path.points, point); const auto lateral = calcLateralDeviation(getPose(path.points.at(idx)), point); overhang_points.emplace_back(lateral, point); } @@ -1332,15 +1317,16 @@ void insertDecelPoint( const Point & p_src, const double offset, const double velocity, PathWithLaneId & path, std::optional & p_out) { - const auto decel_point = calcLongitudinalOffsetPoint(path.points, p_src, offset); + const auto decel_point = motion_utils::calcLongitudinalOffsetPoint(path.points, p_src, offset); if (!decel_point) { // TODO(Satoshi OTA) Think later the process in the case of no decel point found. return; } - const auto seg_idx = findNearestSegmentIndex(path.points, decel_point.value()); - const auto insert_idx = insertTargetPoint(seg_idx, decel_point.value(), path.points); + const auto seg_idx = motion_utils::findNearestSegmentIndex(path.points, decel_point.value()); + const auto insert_idx = + motion_utils::insertTargetPoint(seg_idx, decel_point.value(), path.points); if (!insert_idx) { // TODO(Satoshi OTA) Think later the process in the case of no decel point found. @@ -1702,7 +1688,7 @@ void filterTargetObjects( constexpr double STOP_TIME_THRESHOLD = 3.0; // [s] if (filtering_utils::isUnknownTypeObject(o)) { if (o.stop_time < STOP_TIME_THRESHOLD) { - o.reason = "UnstableObject"; + o.info = ObjectInfo::UNSTABLE_OBJECT; data.other_objects.push_back(o); continue; } @@ -1761,9 +1747,9 @@ void fillAdditionalInfoFromPoint(const AvoidancePlanningData & data, AvoidLineAr // calc longitudinal for (auto & sl : lines) { - sl.start_idx = findNearestIndex(path.points, sl.start.position); + sl.start_idx = motion_utils::findNearestIndex(path.points, sl.start.position); sl.start_longitudinal = arc.at(sl.start_idx); - sl.end_idx = findNearestIndex(path.points, sl.end.position); + sl.end_idx = motion_utils::findNearestIndex(path.points, sl.end.position); sl.end_longitudinal = arc.at(sl.end_idx); } } @@ -2060,7 +2046,8 @@ std::pair separateObjectsByPath( Pose p_spline_ego_front = spline_path.points.front().point.pose; double next_longitudinal_distance = parameters->resample_interval_for_output; for (size_t i = 0; i < points_size; ++i) { - const auto distance_from_ego = calcSignedArcLength(reference_path.points, ego_idx, i); + const auto distance_from_ego = + motion_utils::calcSignedArcLength(reference_path.points, ego_idx, i); if (distance_from_ego > object_check_forward_distance) { break; } @@ -2308,7 +2295,7 @@ double calcDistanceToReturnDeadLine( if (planner_data->route_handler->isInGoalRouteSection(lanelets.back())) { const auto & ego_pos = planner_data->self_odometry->pose.pose.position; const auto to_goal_distance = - calcSignedArcLength(path.points, ego_pos, path.points.size() - 1); + motion_utils::calcSignedArcLength(path.points, ego_pos, path.points.size() - 1); distance_to_return_dead_line = std::min( distance_to_return_dead_line, to_goal_distance - parameters->dead_line_buffer_for_goal); }