From 5145bb5e6a3c9542130cc0b04a2aecae54a7239d Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Thu, 23 May 2024 12:52:19 +0900 Subject: [PATCH] feat(avoidance): make it selectable output debug marker from yaml (#7097) Signed-off-by: satoshi-ota --- .../config/avoidance.param.yaml | 9 ++- .../data_structs.hpp | 9 ++- .../behavior_path_avoidance_module/debug.hpp | 8 ++- .../parameter_helper.hpp | 15 ++++- .../schema/avoidance.schema.json | 50 +++++++++++++++- .../src/debug.cpp | 59 ++++++++++++------- .../src/manager.cpp | 12 +++- .../src/scene.cpp | 7 +-- 8 files changed, 132 insertions(+), 37 deletions(-) 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 2e1daed38488f..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 @@ -337,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 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 63cb54d2e3fa1..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 @@ -18,11 +18,13 @@ #include "behavior_path_avoidance_module/data_structs.hpp" #include "behavior_path_avoidance_module/type_alias.hpp" +#include #include namespace behavior_path_planner::utils::avoidance { +using behavior_path_planner::AvoidanceParameters; using behavior_path_planner::AvoidancePlanningData; using behavior_path_planner::AvoidLineArray; using behavior_path_planner::DebugData; @@ -42,10 +44,12 @@ MarkerArray createPredictedVehiclePositions(const PathWithLaneId & path, std::st MarkerArray createTargetObjectsMarkerArray(const ObjectDataArray & objects, const std::string & ns); -MarkerArray createOtherObjectsMarkerArray(const ObjectDataArray & objects, const ObjectInfo & info); +MarkerArray createOtherObjectsMarkerArray( + const ObjectDataArray & objects, const ObjectInfo & info, const bool verbose); 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); } // 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/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/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 65450c143ccda..4d3b4605ac956 100644 --- a/planning/behavior_path_avoidance_module/src/debug.cpp +++ b/planning/behavior_path_avoidance_module/src/debug.cpp @@ -130,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; @@ -139,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; @@ -160,6 +161,7 @@ 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 << magic_enum::enum_name(object.info) << (object.is_parked ? "(PARKED)" : ""); @@ -201,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); @@ -220,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); @@ -428,7 +430,8 @@ MarkerArray createTargetObjectsMarkerArray(const ObjectDataArray & objects, cons return msg; } -MarkerArray createOtherObjectsMarkerArray(const ObjectDataArray & objects, const ObjectInfo & info) +MarkerArray createOtherObjectsMarkerArray( + const ObjectDataArray & objects, const ObjectInfo & info, const bool verbose) { const auto filtered_objects = [&objects, &info]() { ObjectDataArray ret{}; @@ -456,7 +459,8 @@ MarkerArray createOtherObjectsMarkerArray(const ObjectDataArray & objects, const 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_" + ns + "_info"), &msg); + appendMarkerArray( + createObjectInfoMarkerArray(filtered_objects, "others_" + ns + "_info", verbose), &msg); appendMarkerArray( createOverhangLaneletMarkerArray(filtered_objects, "others_" + ns + "_overhang_lanelet"), &msg); @@ -500,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; @@ -534,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 = @@ -549,7 +554,7 @@ MarkerArray createDebugMarkerArray( }; // ignore objects - { + 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); @@ -569,7 +574,7 @@ MarkerArray createDebugMarkerArray( } // 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); @@ -578,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); @@ -618,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))); @@ -636,6 +646,11 @@ 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 behavior_path_planner::utils::avoidance 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 51fe19e4b3237..15b3e970df4b1 100644 --- a/planning/behavior_path_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_module/src/scene.cpp @@ -1360,12 +1360,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_ = utils::avoidance::createDebugMarkerArray(data, shifter, debug); + debug_marker_ = utils::avoidance::createDebugMarkerArray(data, shifter, debug, parameters_); } void AvoidanceModule::updateAvoidanceDebugData(