From 99c8b320933b83f0158f31e2f94bc4ca373f0e2d Mon Sep 17 00:00:00 2001 From: Ahmed Ebrahim <36835765+ahmeddesokyebrahim@users.noreply.github.com> Date: Mon, 12 Feb 2024 19:40:01 +0200 Subject: [PATCH] fix(log-messages): reduce excessive log messages (#5971) Signed-off-by: AhmedEbrahim --- .../config/control_validator.param.yaml | 2 -- .../control_validator/control_validator.hpp | 1 - .../control_validator/src/control_validator.cpp | 5 +---- .../src/pid_longitudinal_controller.cpp | 6 +++--- .../lanelet2_map_visualization_node.cpp | 14 ++++++++++---- .../pointcloud_map_loader_module.cpp | 2 +- .../pointcloud_map_loader_node.cpp | 2 +- .../crosswalk_traffic_light_estimator/src/node.cpp | 4 ++-- .../src/map_based_prediction_node.cpp | 4 ++-- .../src/traffic_light_map_visualizer/node.cpp | 2 +- .../behavior_path_planner/src/planner_manager.cpp | 2 +- .../src/planner_manager.cpp | 2 +- .../src/scene.cpp | 10 ++++++---- .../freespace_planner/freespace_planner_node.cpp | 6 +++--- planning/obstacle_avoidance_planner/src/node.cpp | 2 +- .../src/replan_checker.cpp | 6 +++--- .../path_smoother/src/elastic_band_smoother.cpp | 2 +- planning/path_smoother/src/replan_checker.cpp | 6 +++--- planning/route_handler/src/route_handler.cpp | 2 +- .../scenario_selector_node.cpp | 2 +- simulator/dummy_perception_publisher/src/node.cpp | 2 +- .../simple_planning_simulator_core.cpp | 4 ++-- .../emergency_handler/emergency_handler_core.cpp | 2 +- 23 files changed, 46 insertions(+), 44 deletions(-) diff --git a/control/control_validator/config/control_validator.param.yaml b/control/control_validator/config/control_validator.param.yaml index 7bbe6a466799b..12709b18b7932 100644 --- a/control/control_validator/config/control_validator.param.yaml +++ b/control/control_validator/config/control_validator.param.yaml @@ -1,8 +1,6 @@ /**: ros__parameters: - publish_diag: true # if true, diagnostic msg is published - # If the number of consecutive invalid trajectory exceeds this threshold, the Diag will be set to ERROR. # (For example, threshold = 1 means, even if the trajectory is invalid, Diag will not be ERROR if # the next trajectory is valid.) diff --git a/control/control_validator/include/control_validator/control_validator.hpp b/control/control_validator/include/control_validator/control_validator.hpp index 48b7eba7412a2..eba9bf700ba08 100644 --- a/control/control_validator/include/control_validator/control_validator.hpp +++ b/control/control_validator/include/control_validator/control_validator.hpp @@ -75,7 +75,6 @@ class ControlValidator : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_markers_; // system parameters - bool publish_diag_ = true; int diag_error_count_threshold_ = 0; bool display_on_terminal_ = true; diff --git a/control/control_validator/src/control_validator.cpp b/control/control_validator/src/control_validator.cpp index 03285a284453d..5282e31fef898 100644 --- a/control/control_validator/src/control_validator.cpp +++ b/control/control_validator/src/control_validator.cpp @@ -46,14 +46,11 @@ ControlValidator::ControlValidator(const rclcpp::NodeOptions & options) setupParameters(); - if (publish_diag_) { - setupDiag(); - } + setupDiag(); } void ControlValidator::setupParameters() { - publish_diag_ = declare_parameter("publish_diag"); diag_error_count_threshold_ = declare_parameter("diag_error_count_threshold"); display_on_terminal_ = declare_parameter("display_on_terminal"); diff --git a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp index b700ac7f29f8a..010019011e44a 100644 --- a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp +++ b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp @@ -626,7 +626,7 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d return; }; - const auto info_once = [this](const auto & s) { RCLCPP_INFO_ONCE(logger_, "%s", s); }; + const auto debug_msg_once = [this](const auto & s) { RCLCPP_DEBUG_ONCE(logger_, "%s", s); }; const bool is_under_control = m_current_operation_mode.is_autoware_control_enabled && m_current_operation_mode.mode == OperationModeState::AUTONOMOUS; @@ -691,10 +691,10 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d if (m_control_state == ControlState::STOPPED) { // -- debug print -- if (has_nonzero_target_vel && !departure_condition_from_stopped) { - info_once("target speed > 0, but departure condition is not met. Keep STOPPED."); + debug_msg_once("target speed > 0, but departure condition is not met. Keep STOPPED."); } if (has_nonzero_target_vel && keep_stopped_condition) { - info_once("target speed > 0, but keep stop condition is met. Keep STOPPED."); + debug_msg_once("target speed > 0, but keep stop condition is met. Keep STOPPED."); } // --------------- diff --git a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp index 870f8d06d90ea..bdfbcf904cf36 100644 --- a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp +++ b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp @@ -96,8 +96,10 @@ void Lanelet2MapVisualizationNode::onMapBin( lanelet::ConstLanelets crosswalk_lanelets = lanelet::utils::query::crosswalkLanelets(all_lanelets); lanelet::ConstLineStrings3d partitions = lanelet::utils::query::getAllPartitions(viz_lanelet_map); - lanelet::ConstLineStrings3d pedestrian_markings = - lanelet::utils::query::getAllPedestrianMarkings(viz_lanelet_map); + lanelet::ConstLineStrings3d pedestrian_polygon_markings = + lanelet::utils::query::getAllPedestrianPolygonMarkings(viz_lanelet_map); + lanelet::ConstLineStrings3d pedestrian_line_markings = + lanelet::utils::query::getAllPedestrianLineMarkings(viz_lanelet_map); lanelet::ConstLanelets walkway_lanelets = lanelet::utils::query::walkwayLanelets(all_lanelets); std::vector stop_lines = lanelet::utils::query::stopLinesLanelets(road_lanelets); @@ -179,8 +181,12 @@ void Lanelet2MapVisualizationNode::onMapBin( &map_marker_array, lanelet::visualization::laneletsAsTriangleMarkerArray( "crosswalk_lanelets", crosswalk_lanelets, cl_cross)); insertMarkerArray( - &map_marker_array, lanelet::visualization::pedestrianMarkingsAsMarkerArray( - pedestrian_markings, cl_pedestrian_markings)); + &map_marker_array, lanelet::visualization::pedestrianPolygonMarkingsAsMarkerArray( + pedestrian_polygon_markings, cl_pedestrian_markings)); + + insertMarkerArray( + &map_marker_array, lanelet::visualization::pedestrianLineMarkingsAsMarkerArray( + pedestrian_line_markings, cl_pedestrian_markings)); insertMarkerArray( &map_marker_array, lanelet::visualization::laneletsAsTriangleMarkerArray( "walkway_lanelets", walkway_lanelets, cl_cross)); diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.cpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.cpp index b4aa930831a04..a66f9ee99534c 100644 --- a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.cpp +++ b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.cpp @@ -74,7 +74,7 @@ sensor_msgs::msg::PointCloud2 PointcloudMapLoaderModule::loadPCDFiles( for (size_t i = 0; i < pcd_paths.size(); ++i) { auto & path = pcd_paths[i]; if (i % 50 == 0) { - RCLCPP_INFO_STREAM( + RCLCPP_DEBUG_STREAM( logger_, fmt::format("Load {} ({} out of {})", path, i + 1, pcd_paths.size())); } diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp index 5f4b3e311e6e9..cde3d459b3687 100644 --- a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp +++ b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp @@ -102,7 +102,7 @@ std::map PointCloudMapLoaderNode::getPCDMetadata( // a metadata file. // Note that this should ideally be avoided and thus eventually be removed by someone, until // Autoware users get used to handling the PCD file(s) with metadata. - RCLCPP_INFO_STREAM(get_logger(), "Create PCD metadata, as the pointcloud is a single file."); + RCLCPP_DEBUG_STREAM(get_logger(), "Create PCD metadata, as the pointcloud is a single file."); pcl::PointCloud single_pcd; if (pcl::io::loadPCDFile(pcd_paths[0], single_pcd) == -1) { throw std::runtime_error("PCD load failed: " + pcd_paths[0]); diff --git a/perception/crosswalk_traffic_light_estimator/src/node.cpp b/perception/crosswalk_traffic_light_estimator/src/node.cpp index 904a365786755..e29a80de283ce 100644 --- a/perception/crosswalk_traffic_light_estimator/src/node.cpp +++ b/perception/crosswalk_traffic_light_estimator/src/node.cpp @@ -102,7 +102,7 @@ CrosswalkTrafficLightEstimatorNode::CrosswalkTrafficLightEstimatorNode( void CrosswalkTrafficLightEstimatorNode::onMap(const HADMapBin::ConstSharedPtr msg) { - RCLCPP_INFO(get_logger(), "[CrosswalkTrafficLightEstimatorNode]: Start loading lanelet"); + RCLCPP_DEBUG(get_logger(), "[CrosswalkTrafficLightEstimatorNode]: Start loading lanelet"); lanelet_map_ptr_ = std::make_shared(); lanelet::utils::conversion::fromBinMsg( *msg, lanelet_map_ptr_, &traffic_rules_ptr_, &routing_graph_ptr_); @@ -117,7 +117,7 @@ void CrosswalkTrafficLightEstimatorNode::onMap(const HADMapBin::ConstSharedPtr m lanelet::routing::RoutingGraphContainer overall_graphs({vehicle_graph, pedestrian_graph}); overall_graphs_ptr_ = std::make_shared(overall_graphs); - RCLCPP_INFO(get_logger(), "[CrosswalkTrafficLightEstimatorNode]: Map is loaded"); + RCLCPP_DEBUG(get_logger(), "[CrosswalkTrafficLightEstimatorNode]: Map is loaded"); } void CrosswalkTrafficLightEstimatorNode::onRoute(const LaneletRoute::ConstSharedPtr msg) diff --git a/perception/map_based_prediction/src/map_based_prediction_node.cpp b/perception/map_based_prediction/src/map_based_prediction_node.cpp index fffaede3a904d..2436da9925254 100644 --- a/perception/map_based_prediction/src/map_based_prediction_node.cpp +++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp @@ -870,11 +870,11 @@ PredictedObject MapBasedPredictionNode::convertToPredictedObject( void MapBasedPredictionNode::mapCallback(const HADMapBin::ConstSharedPtr msg) { - RCLCPP_INFO(get_logger(), "[Map Based Prediction]: Start loading lanelet"); + RCLCPP_DEBUG(get_logger(), "[Map Based Prediction]: Start loading lanelet"); lanelet_map_ptr_ = std::make_shared(); lanelet::utils::conversion::fromBinMsg( *msg, lanelet_map_ptr_, &traffic_rules_ptr_, &routing_graph_ptr_); - RCLCPP_INFO(get_logger(), "[Map Based Prediction]: Map is loaded"); + RCLCPP_DEBUG(get_logger(), "[Map Based Prediction]: Map is loaded"); const auto all_lanelets = lanelet::utils::query::laneletLayer(lanelet_map_ptr_); const auto crosswalks = lanelet::utils::query::crosswalkLanelets(all_lanelets); diff --git a/perception/traffic_light_visualization/src/traffic_light_map_visualizer/node.cpp b/perception/traffic_light_visualization/src/traffic_light_map_visualizer/node.cpp index e621a20450bbf..48c6f7dd0f2bb 100644 --- a/perception/traffic_light_visualization/src/traffic_light_map_visualizer/node.cpp +++ b/perception/traffic_light_visualization/src/traffic_light_map_visualizer/node.cpp @@ -203,7 +203,7 @@ void TrafficLightMapVisualizerNode::binMapCallback( lanelet::LaneletMapPtr viz_lanelet_map(new lanelet::LaneletMap); lanelet::utils::conversion::fromBinMsg(*input_map_msg, viz_lanelet_map); - RCLCPP_INFO(get_logger(), "Map is loaded\n"); + RCLCPP_DEBUG(get_logger(), "Map is loaded\n"); // get lanelets etc to visualize lanelet::ConstLanelets all_lanelets = lanelet::utils::query::laneletLayer(viz_lanelet_map); diff --git a/planning/behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/src/planner_manager.cpp index c60bc53a1129e..5ec34ebbab6e3 100644 --- a/planning/behavior_path_planner/src/planner_manager.cpp +++ b/planning/behavior_path_planner/src/planner_manager.cpp @@ -60,7 +60,7 @@ void PlannerManager::launchScenePlugin(rclcpp::Node & node, const std::string & // register manager_ptrs_.push_back(plugin); processing_time_.emplace(plugin->name(), 0.0); - RCLCPP_INFO_STREAM(node.get_logger(), "The scene plugin '" << name << "' is loaded."); + RCLCPP_DEBUG_STREAM(node.get_logger(), "The scene plugin '" << name << "' is loaded."); } else { RCLCPP_ERROR_STREAM(node.get_logger(), "The scene plugin '" << name << "' is not available."); } diff --git a/planning/behavior_velocity_planner/src/planner_manager.cpp b/planning/behavior_velocity_planner/src/planner_manager.cpp index b4cb87d0b5258..cb491920c87d4 100644 --- a/planning/behavior_velocity_planner/src/planner_manager.cpp +++ b/planning/behavior_velocity_planner/src/planner_manager.cpp @@ -71,7 +71,7 @@ void BehaviorVelocityPlannerManager::launchScenePlugin( // register scene_manager_plugins_.push_back(plugin); - RCLCPP_INFO_STREAM(node.get_logger(), "The scene plugin '" << name << "' is loaded."); + RCLCPP_DEBUG_STREAM(node.get_logger(), "The scene plugin '" << name << "' is loaded."); } else { RCLCPP_ERROR_STREAM(node.get_logger(), "The scene plugin '" << name << "' is not available."); } diff --git a/planning/behavior_velocity_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_traffic_light_module/src/scene.cpp index 9982ec34c4bca..96cb2c4e1c790 100644 --- a/planning/behavior_velocity_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/scene.cpp @@ -199,7 +199,9 @@ bool TrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopReason * input_path, lanelet_stop_lines, planner_param_.stop_margin + planner_data_->vehicle_info_.max_longitudinal_offset_m, planner_data_->stop_line_extend_length, stop_line_point, stop_line_point_idx)) { - RCLCPP_WARN_THROTTLE(logger_, *clock_, 5000, "Failed to calculate stop point and insert index"); + RCLCPP_WARN_STREAM_ONCE( + logger_, "Failed to calculate stop point and insert index for regulatory element id " + << traffic_light_reg_elem_.id()); setSafe(true); setDistance(std::numeric_limits::lowest()); return false; @@ -356,9 +358,9 @@ bool TrafficLightModule::findValidTrafficSignal(TrafficSignalStamped & valid_tra const auto traffic_signal_stamped_opt = planner_data_->getTrafficSignal( traffic_light_reg_elem_.id(), true /* traffic light module keeps last observation */); if (!traffic_signal_stamped_opt) { - RCLCPP_WARN_THROTTLE( - logger_, *clock_, 5000 /* ms */, - "the traffic signal data associated with regulatory element id is not received"); + RCLCPP_WARN_STREAM_ONCE( + logger_, "the traffic signal data associated with regulatory element id " + << traffic_light_reg_elem_.id() << " is not received"); return false; } valid_traffic_signal = traffic_signal_stamped_opt.value(); diff --git a/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp b/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp index 6400ae6d135ba..c4ba1424cf45e 100644 --- a/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp +++ b/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp @@ -372,7 +372,7 @@ bool FreespacePlannerNode::isPlanRequired() const bool is_obstacle_found = algo_->hasObstacleOnTrajectory(trajectory2PoseArray(forward_trajectory)); if (is_obstacle_found) { - RCLCPP_INFO(get_logger(), "Found obstacle"); + RCLCPP_DEBUG(get_logger(), "Found obstacle"); return true; } } @@ -493,10 +493,10 @@ void FreespacePlannerNode::planTrajectory() const bool result = algo_->makePlan(current_pose_in_costmap_frame, goal_pose_in_costmap_frame); const rclcpp::Time end = get_clock()->now(); - RCLCPP_INFO(get_logger(), "Freespace planning: %f [s]", (end - start).seconds()); + RCLCPP_DEBUG(get_logger(), "Freespace planning: %f [s]", (end - start).seconds()); if (result) { - RCLCPP_INFO(get_logger(), "Found goal!"); + RCLCPP_DEBUG(get_logger(), "Found goal!"); trajectory_ = createTrajectory(current_pose_, algo_->getWaypoints(), node_param_.waypoints_velocity); reversing_indices_ = getReversingIndices(trajectory_); diff --git a/planning/obstacle_avoidance_planner/src/node.cpp b/planning/obstacle_avoidance_planner/src/node.cpp index 23b146a3e5280..1dcabf202970c 100644 --- a/planning/obstacle_avoidance_planner/src/node.cpp +++ b/planning/obstacle_avoidance_planner/src/node.cpp @@ -206,7 +206,7 @@ rcl_interfaces::msg::SetParametersResult ObstacleAvoidancePlanner::onParam( void ObstacleAvoidancePlanner::initializePlanning() { - RCLCPP_INFO(get_logger(), "Initialize planning"); + RCLCPP_DEBUG(get_logger(), "Initialize planning"); mpt_optimizer_ptr_->initialize(enable_debug_info_, traj_param_); diff --git a/planning/obstacle_avoidance_planner/src/replan_checker.cpp b/planning/obstacle_avoidance_planner/src/replan_checker.cpp index 6d7c6e702471a..5f495051cc2bd 100644 --- a/planning/obstacle_avoidance_planner/src/replan_checker.cpp +++ b/planning/obstacle_avoidance_planner/src/replan_checker.cpp @@ -65,14 +65,14 @@ bool ReplanChecker::isResetRequired(const PlannerData & planner_data) const // path shape changes if (isPathAroundEgoChanged(planner_data, prev_traj_points)) { - RCLCPP_INFO( + RCLCPP_DEBUG( logger_, "Replan with resetting optimization since path shape around ego changed."); return true; } // path goal changes if (isPathGoalChanged(planner_data, prev_traj_points)) { - RCLCPP_INFO(logger_, "Replan with resetting optimization since path goal changed."); + RCLCPP_DEBUG(logger_, "Replan with resetting optimization since path goal changed."); return true; } @@ -80,7 +80,7 @@ bool ReplanChecker::isResetRequired(const PlannerData & planner_data) const const double delta_dist = tier4_autoware_utils::calcDistance2d(p.ego_pose, prev_ego_pose_ptr_->position); if (max_ego_moving_dist_ < delta_dist) { - RCLCPP_INFO( + RCLCPP_DEBUG( logger_, "Replan with resetting optimization since current ego pose is far from previous ego pose."); return true; diff --git a/planning/path_smoother/src/elastic_band_smoother.cpp b/planning/path_smoother/src/elastic_band_smoother.cpp index 8227882cbb61c..a81a92f3fcc96 100644 --- a/planning/path_smoother/src/elastic_band_smoother.cpp +++ b/planning/path_smoother/src/elastic_band_smoother.cpp @@ -136,7 +136,7 @@ rcl_interfaces::msg::SetParametersResult ElasticBandSmoother::onParam( void ElasticBandSmoother::initializePlanning() { - RCLCPP_INFO(get_logger(), "Initialize planning"); + RCLCPP_DEBUG(get_logger(), "Initialize planning"); eb_path_smoother_ptr_->initialize(false, common_param_); resetPreviousData(); diff --git a/planning/path_smoother/src/replan_checker.cpp b/planning/path_smoother/src/replan_checker.cpp index 11a4a3d5f4dad..f451a05a8f835 100644 --- a/planning/path_smoother/src/replan_checker.cpp +++ b/planning/path_smoother/src/replan_checker.cpp @@ -67,14 +67,14 @@ bool ReplanChecker::isResetRequired(const PlannerData & planner_data) const // path shape changes if (isPathAroundEgoChanged(planner_data, prev_traj_points)) { - RCLCPP_INFO( + RCLCPP_DEBUG( logger_, "Replan with resetting optimization since path shape around ego changed."); return true; } // path goal changes if (isPathGoalChanged(planner_data, prev_traj_points)) { - RCLCPP_INFO(logger_, "Replan with resetting optimization since path goal changed."); + RCLCPP_DEBUG(logger_, "Replan with resetting optimization since path goal changed."); return true; } @@ -82,7 +82,7 @@ bool ReplanChecker::isResetRequired(const PlannerData & planner_data) const const double delta_dist = tier4_autoware_utils::calcDistance2d(p.ego_pose, prev_ego_pose_ptr_->position); if (max_ego_moving_dist_ < delta_dist) { - RCLCPP_INFO( + RCLCPP_DEBUG( logger_, "Replan with resetting optimization since current ego pose is far from previous ego pose."); return true; diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp index 0e6464a81f354..90c44d9eaecd0 100644 --- a/planning/route_handler/src/route_handler.cpp +++ b/planning/route_handler/src/route_handler.cpp @@ -377,7 +377,7 @@ Header RouteHandler::getRouteHeader() const UUID RouteHandler::getRouteUuid() const { if (!route_ptr_) { - RCLCPP_WARN(logger_, "[Route Handler] getRouteUuid: Route has not been set yet"); + RCLCPP_WARN_SKIPFIRST(logger_, "[Route Handler] getRouteUuid: Route has not been set yet"); return UUID(); } return route_ptr_->uuid; diff --git a/planning/scenario_selector/src/scenario_selector_node/scenario_selector_node.cpp b/planning/scenario_selector/src/scenario_selector_node/scenario_selector_node.cpp index 1d5c7b44dcc45..734ff11aaf4a1 100644 --- a/planning/scenario_selector/src/scenario_selector_node/scenario_selector_node.cpp +++ b/planning/scenario_selector/src/scenario_selector_node/scenario_selector_node.cpp @@ -180,7 +180,7 @@ void ScenarioSelectorNode::updateCurrentScenario() } if (current_scenario_ != prev_scenario) { - RCLCPP_INFO_STREAM( + RCLCPP_DEBUG_STREAM( this->get_logger(), "scenario changed: " << prev_scenario << " -> " << current_scenario_); } } diff --git a/simulator/dummy_perception_publisher/src/node.cpp b/simulator/dummy_perception_publisher/src/node.cpp index f4cb6063a9bf6..3ac663d2c8647 100644 --- a/simulator/dummy_perception_publisher/src/node.cpp +++ b/simulator/dummy_perception_publisher/src/node.cpp @@ -197,7 +197,7 @@ void DummyPerceptionPublisherNode::timerCallback() std::string error; if (!tf_buffer_.canTransform("base_link", /*src*/ "map", tf2::TimePointZero, &error)) { failed_tf_time = this->now(); - RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "map->base_link is not available yet"); + RCLCPP_DEBUG_THROTTLE(get_logger(), *get_clock(), 5000, "map->base_link is not available yet"); return; } diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp index 26be4841e2f2a..e99f6d5f2cf00 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp @@ -182,7 +182,7 @@ SimplePlanningSimulator::SimplePlanningSimulator(const rclcpp::NodeOptions & opt // set initialize source const auto initialize_source = declare_parameter("initialize_source", "INITIAL_POSE_TOPIC"); - RCLCPP_INFO(this->get_logger(), "initialize_source : %s", initialize_source.c_str()); + RCLCPP_DEBUG(this->get_logger(), "initialize_source : %s", initialize_source.c_str()); if (initialize_source == "ORIGIN") { Pose p; p.orientation.w = 1.0; // yaw = 0 @@ -218,7 +218,7 @@ void SimplePlanningSimulator::initialize_vehicle_model() { const auto vehicle_model_type_str = declare_parameter("vehicle_model_type", "IDEAL_STEER_VEL"); - RCLCPP_INFO(this->get_logger(), "vehicle_model_type = %s", vehicle_model_type_str.c_str()); + RCLCPP_DEBUG(this->get_logger(), "vehicle_model_type = %s", vehicle_model_type_str.c_str()); const double vel_lim = declare_parameter("vel_lim", 50.0); const double vel_rate_lim = declare_parameter("vel_rate_lim", 7.0); diff --git a/system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp b/system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp index db6d11f98b5ba..17fc92f3d36c7 100644 --- a/system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp +++ b/system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp @@ -358,7 +358,7 @@ void EmergencyHandler::transitionTo(const int new_state) throw std::runtime_error(msg); }; - RCLCPP_INFO( + RCLCPP_DEBUG( this->get_logger(), "MRM State changed: %s -> %s", state2string(mrm_state_.state), state2string(new_state));