diff --git a/common/tier4_logging_level_configure_rviz_plugin/config/logger_config.yaml b/common/tier4_logging_level_configure_rviz_plugin/config/logger_config.yaml index a298967a31af9..f54d991a9d852 100644 --- a/common/tier4_logging_level_configure_rviz_plugin/config/logger_config.yaml +++ b/common/tier4_logging_level_configure_rviz_plugin/config/logger_config.yaml @@ -37,6 +37,8 @@ Planning: behavior_path_planner_avoidance: - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner logger_name: planning.scenario_planning.lane_driving.behavior_planning.behavior_path_planner.avoidance + - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner + logger_name: planning.scenario_planning.lane_driving.behavior_planning.behavior_path_planner.avoidance.utils behavior_path_planner_goal_planner: - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner diff --git a/planning/behavior_path_avoidance_module/config/avoidance.param.yaml b/planning/behavior_path_avoidance_module/config/avoidance.param.yaml index 5d6044df64b2f..c85fdd5a2351a 100644 --- a/planning/behavior_path_avoidance_module/config/avoidance.param.yaml +++ b/planning/behavior_path_avoidance_module/config/avoidance.param.yaml @@ -290,4 +290,3 @@ # for debug debug: marker: false - console: 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 e74b546f31fc4..8516cc61f860b 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 @@ -322,7 +322,6 @@ struct AvoidanceParameters // debug bool publish_debug_marker = false; - bool print_debug_info = false; }; struct ObjectData // avoidance target 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 6dbf52c0425fb..e115e676b1dbf 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 @@ -379,7 +379,6 @@ AvoidanceParameters getParameter(rclcpp::Node * node) { const std::string ns = "avoidance.debug."; p.publish_debug_marker = getOrDeclareParameter(*node, ns + "marker"); - p.print_debug_info = getOrDeclareParameter(*node, ns + "console"); } return p; 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 3f1d3495e51cb..92e9da61c4ada 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 @@ -31,6 +31,9 @@ using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityAndPoly using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped; using behavior_path_planner::utils::path_safety_checker::PredictedPathWithPolygon; +static constexpr const char * logger_namespace = + "planning.scenario_planning.lane_driving.behavior_planning.behavior_path_planner.avoidance.utils"; + bool isOnRight(const ObjectData & obj); double calcShiftLength( diff --git a/planning/behavior_path_avoidance_module/schema/avoidance.schema.json b/planning/behavior_path_avoidance_module/schema/avoidance.schema.json index 79882beb805f8..eee34edbc0e0c 100644 --- a/planning/behavior_path_avoidance_module/schema/avoidance.schema.json +++ b/planning/behavior_path_avoidance_module/schema/avoidance.schema.json @@ -1379,14 +1379,9 @@ "type": "boolean", "description": "Publish debug marker.", "default": "false" - }, - "console": { - "type": "boolean", - "description": "Output debug info on console.", - "default": "false" } }, - "required": ["marker", "console"], + "required": ["marker"], "additionalProperties": false } }, diff --git a/planning/behavior_path_avoidance_module/src/manager.cpp b/planning/behavior_path_avoidance_module/src/manager.cpp index e3391f251e55f..dbff6f60365f3 100644 --- a/planning/behavior_path_avoidance_module/src/manager.cpp +++ b/planning/behavior_path_avoidance_module/src/manager.cpp @@ -257,7 +257,6 @@ void AvoidanceModuleManager::updateModuleParams(const std::vector(parameters, ns + "marker", p->publish_debug_marker); - updateParam(parameters, ns + "console", p->print_debug_info); } 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 b16395f825385..1d46e4e1f9d5d 100644 --- a/planning/behavior_path_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_module/src/scene.cpp @@ -40,12 +40,6 @@ #include #include -// set as macro so that calling function name will be printed. -// debug print is heavy. turn on only when debugging. -#define DEBUG_PRINT(...) \ - RCLCPP_DEBUG_EXPRESSION(getLogger(), parameters_->print_debug_info, __VA_ARGS__) -#define printShiftLines(p, msg) DEBUG_PRINT("[%s] %s", msg, toStrInfo(p).c_str()) - namespace behavior_path_planner { @@ -109,7 +103,7 @@ AvoidanceModule::AvoidanceModule( bool AvoidanceModule::isExecutionRequested() const { - DEBUG_PRINT("AVOIDANCE isExecutionRequested"); + RCLCPP_DEBUG(getLogger(), "AVOIDANCE isExecutionRequested"); // Check ego is in preferred lane updateInfoMarker(avoid_data_); @@ -132,7 +126,11 @@ bool AvoidanceModule::isExecutionRequested() const bool AvoidanceModule::isExecutionReady() const { - DEBUG_PRINT("AVOIDANCE isExecutionReady"); + RCLCPP_DEBUG_STREAM(getLogger(), "---Avoidance GO/NO-GO status---"); + RCLCPP_DEBUG_STREAM(getLogger(), std::boolalpha << "SAFE:" << avoid_data_.safe); + RCLCPP_DEBUG_STREAM(getLogger(), std::boolalpha << "COMFORTABLE:" << avoid_data_.comfortable); + RCLCPP_DEBUG_STREAM(getLogger(), std::boolalpha << "VALID:" << avoid_data_.valid); + RCLCPP_DEBUG_STREAM(getLogger(), std::boolalpha << "READY:" << avoid_data_.ready); return avoid_data_.safe && avoid_data_.comfortable && avoid_data_.valid && avoid_data_.ready; } @@ -1139,7 +1137,7 @@ void AvoidanceModule::addNewShiftLines( const auto new_shift_length = front_new_shift_line.end_shift_length; const auto new_shift_end_idx = front_new_shift_line.end_idx; - DEBUG_PRINT("min_start_idx = %lu", min_start_idx); + RCLCPP_DEBUG(getLogger(), "min_start_idx = %lu", min_start_idx); // Remove shift points that starts later than the new_shift_line from path_shifter. // @@ -1152,8 +1150,9 @@ void AvoidanceModule::addNewShiftLines( // farther avoidance. for (const auto & sl : current_shift_lines) { if (sl.start_idx >= min_start_idx) { - DEBUG_PRINT( - "sl.start_idx = %lu, this sl starts after new proposal. remove this one.", sl.start_idx); + RCLCPP_DEBUG( + getLogger(), "sl.start_idx = %lu, this sl starts after new proposal. remove this one.", + sl.start_idx); continue; } @@ -1171,7 +1170,7 @@ void AvoidanceModule::addNewShiftLines( } } - DEBUG_PRINT("sl.start_idx = %lu, no conflict. keep this one.", sl.start_idx); + RCLCPP_DEBUG(getLogger(), "sl.start_idx = %lu, no conflict. keep this one.", sl.start_idx); future.push_back(sl); } diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp index bef3976be7e1e..33718ae16a038 100644 --- a/planning/behavior_path_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_avoidance_module/src/utils.cpp @@ -563,13 +563,15 @@ bool isNeverAvoidanceTarget( if (object.is_within_intersection) { if (object.behavior == ObjectData::Behavior::NONE) { object.reason = "ParallelToEgoLane"; - RCLCPP_DEBUG(rclcpp::get_logger(__func__), "object belongs to ego lane. never avoid it."); + 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"; - RCLCPP_DEBUG(rclcpp::get_logger(__func__), "object belongs to ego lane. never avoid it."); + RCLCPP_DEBUG( + rclcpp::get_logger(logger_namespace), "object belongs to ego lane. never avoid it."); return true; } } @@ -581,7 +583,8 @@ bool isNeverAvoidanceTarget( 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; - RCLCPP_DEBUG(rclcpp::get_logger(__func__), "object isn't on the edge lane. never avoid it."); + RCLCPP_DEBUG( + rclcpp::get_logger(logger_namespace), "object isn't on the edge lane. never avoid it."); return true; } } @@ -589,7 +592,8 @@ bool isNeverAvoidanceTarget( if (isCloseToStopFactor(object, data, planner_data, parameters)) { if (object.is_on_ego_lane && !object.is_parked) { object.reason = AvoidanceDebugFactor::NOT_PARKING_OBJECT; - RCLCPP_DEBUG(rclcpp::get_logger(__func__), "object is close to stop factor. never avoid it."); + RCLCPP_DEBUG( + rclcpp::get_logger(logger_namespace), "object is close to stop factor. never avoid it."); return true; } } @@ -604,12 +608,12 @@ bool isObviousAvoidanceTarget( { if (!object.is_within_intersection) { if (object.is_parked && object.behavior == ObjectData::Behavior::NONE) { - RCLCPP_DEBUG(rclcpp::get_logger(__func__), "object is obvious parked vehicle."); + RCLCPP_DEBUG(rclcpp::get_logger(logger_namespace), "object is obvious parked vehicle."); return true; } if (!object.is_on_ego_lane && object.behavior == ObjectData::Behavior::NONE) { - RCLCPP_DEBUG(rclcpp::get_logger(__func__), "object is adjacent vehicle."); + RCLCPP_DEBUG(rclcpp::get_logger(logger_namespace), "object is adjacent vehicle."); return true; } } @@ -2187,8 +2191,7 @@ DrivableLanes generateExpandedDrivableLanes( } if (i == max_recursive_search_num - 1) { RCLCPP_ERROR( - rclcpp::get_logger("behavior_path_planner").get_child("avoidance"), - "Drivable area expansion reaches max iteration."); + rclcpp::get_logger(logger_namespace), "Drivable area expansion reaches max iteration."); } } };