autowarefoundation · xmfcx · Feb 12, 2024 · Dec 22, 2023 · Dec 22, 2023 · Dec 22, 2023
diff --git 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
@@ -75,7 +75,6 @@ class ControlValidator : public rclcpp::Node
   rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::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
@@ -46,14 +46,11 @@
 
   setupParameters();
 
-  if (publish_diag_) {
-    setupDiag();
-  }
+  setupDiag();
 }
 
 void ControlValidator::setupParameters()
 {
-  publish_diag_ = declare_parameter<bool>("publish_diag");
   diag_error_count_threshold_ = declare_parameter<int>("diag_error_count_threshold");
   display_on_terminal_ = declare_parameter<bool>("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
@@ -626,7 +626,7 @@
     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 @@
   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
@@ -96,8 +96,10 @@
   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<lanelet::ConstLineString3d> stop_lines =
     lanelet::utils::query::stopLinesLanelets(road_lanelets);
@@ -179,8 +181,12 @@
     &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
@@ -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
@@ -102,7 +102,7 @@ std::map<std::string, PCDFileMetadata> 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<pcl::PointXYZ> 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
@@ -102,7 +102,7 @@
 
 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::LaneletMap>();
   lanelet::utils::conversion::fromBinMsg(
     *msg, lanelet_map_ptr_, &traffic_rules_ptr_, &routing_graph_ptr_);
@@ -117,7 +117,7 @@
   lanelet::routing::RoutingGraphContainer overall_graphs({vehicle_graph, pedestrian_graph});
   overall_graphs_ptr_ =
     std::make_shared<const lanelet::routing::RoutingGraphContainer>(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
@@ -870,11 +870,11 @@
 
 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::LaneletMap>();
   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
@@ -203,7 +203,7 @@
   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
@@ -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
@@ -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
@@ -199,7 +199,9 @@
         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<double>::lowest());
     return false;
@@ -356,9 +358,9 @@
   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
@@ -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
@@ -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
@@ -65,22 +65,22 @@ 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;
     }
 
     // ego pose is lost or new ego pose is designated in simulation
     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
@@ -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
@@ -67,22 +67,22 @@ 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;
     }
 
     // ego pose is lost or new ego pose is designated in simulation
     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
@@ -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
@@ -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
@@ -197,7 +197,7 @@
   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/...imple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp b/...imple_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
@@ -358,7 +358,7 @@
     throw std::runtime_error(msg);
   };
 
-  RCLCPP_INFO(
+  RCLCPP_DEBUG(
     this->get_logger(), "MRM State changed: %s -> %s", state2string(mrm_state_.state),
     state2string(new_state));