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."); } } }; diff --git a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml b/planning/behavior_path_planner/config/behavior_path_planner.param.yaml index f3f6870085e02..2c181489ba4c2 100644 --- a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml +++ b/planning/behavior_path_planner/config/behavior_path_planner.param.yaml @@ -1,6 +1,5 @@ /**: ros__parameters: - verbose: false max_iteration_num: 100 traffic_light_signal_timeout: 1.0 planning_hz: 10.0 diff --git a/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp index 9054435a33912..51a4cddabb941 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp @@ -96,7 +96,7 @@ struct SceneModuleStatus class PlannerManager { public: - PlannerManager(rclcpp::Node & node, const size_t max_iteration_num, const bool verbose); + PlannerManager(rclcpp::Node & node, const size_t max_iteration_num); /** * @brief run all candidate and approved modules. @@ -471,8 +471,6 @@ class PlannerManager mutable std::shared_ptr debug_msg_ptr_; size_t max_iteration_num_{100}; - - bool verbose_{false}; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index bc1140d627206..b56d1a207f76d 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -133,7 +133,7 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod const std::lock_guard lock(mutex_manager_); // for planner_manager_ const auto & p = planner_data_->parameters; - planner_manager_ = std::make_shared(*this, p.max_iteration_num, p.verbose); + planner_manager_ = std::make_shared(*this, p.max_iteration_num); for (const auto & name : declare_parameter>("launch_modules")) { // workaround: Since ROS 2 can't get empty list, launcher set [''] on the parameter. @@ -208,7 +208,6 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() { BehaviorPathPlannerParameters p{}; - p.verbose = declare_parameter("verbose"); p.max_iteration_num = declare_parameter("max_iteration_num"); p.traffic_light_signal_timeout = declare_parameter("traffic_light_signal_timeout"); diff --git a/planning/behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/src/planner_manager.cpp index 6ac67605839e1..418a70cb269d6 100644 --- a/planning/behavior_path_planner/src/planner_manager.cpp +++ b/planning/behavior_path_planner/src/planner_manager.cpp @@ -30,13 +30,11 @@ namespace behavior_path_planner { -PlannerManager::PlannerManager( - rclcpp::Node & node, const size_t max_iteration_num, const bool verbose) +PlannerManager::PlannerManager(rclcpp::Node & node, const size_t max_iteration_num) : plugin_loader_("behavior_path_planner", "behavior_path_planner::SceneModuleManagerInterface"), logger_(node.get_logger().get_child("planner_manager")), clock_(*node.get_clock()), - max_iteration_num_{max_iteration_num}, - verbose_{verbose} + max_iteration_num_{max_iteration_num} { processing_time_.emplace("total_time", 0.0); debug_publisher_ptr_ = std::make_unique(&node, "~/debug"); @@ -946,11 +944,7 @@ void PlannerManager::print() const state_publisher_ptr_->publish("internal_state", string_stream.str()); - if (!verbose_) { - return; - } - - RCLCPP_INFO_STREAM(logger_, string_stream.str()); + RCLCPP_DEBUG_STREAM(logger_, string_stream.str()); } void PlannerManager::publishProcessingTime() const diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/parameters.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/parameters.hpp index 1eae4a1c4c345..1241f98daa747 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/parameters.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/parameters.hpp @@ -30,7 +30,6 @@ struct ModuleConfigParameters struct BehaviorPathPlannerParameters { - bool verbose; size_t max_iteration_num{100}; double traffic_light_signal_timeout{1.0};