chore(behavior_path_planner): change logger throttle (tier4#1157)

* chore(behavior_path_planner): change logger throttle Signed-off-by: Shumpei Wakabayashi <shumpei.wakabayashi@tier4.jp> * chore(behavior_path_planner): change error throttle Signed-off-by: Shumpei Wakabayashi <shumpei.wakabayashi@tier4.jp> * chore(behavior_path_planner): from get_logger to getLogger Signed-off-by: Shumpei Wakabayashi <shumpei.wakabayashi@tier4.jp> * chore(behavior_path_planner): fix bug Signed-off-by: Shumpei Wakabayashi <shumpei.wakabayashi@tier4.jp> * ci(pre-commit): autofix Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Tomoya Kimura <tomoya.kimura@tier4.jp>
boyali · Sep 28, 2022 · 491888e · 491888e
1 parent 87df1c4
commit 491888e
Show file tree

Hide file tree

Showing 6 changed files with 14 additions and 8 deletions.
diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp
@@ -541,7 +541,8 @@ void BehaviorPathPlannerNode::run()
   if (!clipped_path.points.empty()) {
     path_publisher_->publish(clipped_path);
   } else {
-    RCLCPP_ERROR(get_logger(), "behavior path output is empty! Stop publish.");
+    RCLCPP_ERROR_THROTTLE(
+      get_logger(), *get_clock(), 5000, "behavior path output is empty! Stop publish.");
   }
   path_candidate_publisher_->publish(util::toPath(*path_candidate));
 

diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp
@@ -301,7 +301,8 @@ lanelet::ConstLanelets LaneChangeModule::getCurrentLanes() const
 
   lanelet::ConstLanelet current_lane;
   if (!route_handler->getClosestLaneletWithinRoute(current_pose, &current_lane)) {
-    RCLCPP_ERROR(getLogger(), "failed to find closest lanelet within route!!!");
+    RCLCPP_ERROR_THROTTLE(
+      getLogger(), *clock_, 5000, "failed to find closest lanelet within route!!!");
     return {};  // TODO(Horibe) what should be returned?
   }
 

diff --git a/planning/behavior_path_planner/src/scene_module/lane_following/lane_following_module.cpp b/planning/behavior_path_planner/src/scene_module/lane_following/lane_following_module.cpp
@@ -84,7 +84,8 @@ PathWithLaneId LaneFollowingModule::getReferencePath() const
 
   lanelet::ConstLanelet current_lane;
   if (!planner_data_->route_handler->getClosestLaneletWithinRoute(current_pose, &current_lane)) {
-    RCLCPP_ERROR(getLogger(), "failed to find closest lanelet within route!!!");
+    RCLCPP_ERROR_THROTTLE(
+      getLogger(), *clock_, 5000, "failed to find closest lanelet within route!!!");
     return reference_path;  // TODO(Horibe)
   }
 

diff --git a/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp b/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp
@@ -359,7 +359,8 @@ lanelet::ConstLanelets PullOutModule::getCurrentLanes() const
 
   lanelet::ConstLanelet current_lane;
   if (!route_handler->getClosestLaneletWithinRoute(current_pose, &current_lane)) {
-    RCLCPP_ERROR(getLogger(), "failed to find closest lanelet within route!!!");
+    RCLCPP_ERROR_THROTTLE(
+      getLogger(), *clock_, 5000, "failed to find closest lanelet within route!!!");
     return {};  // TODO(Horibe) what should be returned?
   }
 
@@ -394,7 +395,7 @@ lanelet::ConstLanelets PullOutModule::getPullOutLanes(
       shoulder_lane, current_pose, pull_out_lane_length_, pull_out_lane_length_);
 
   } else {
-    RCLCPP_ERROR(getLogger(), "getPullOverTarget didn't work");
+    RCLCPP_ERROR_THROTTLE(getLogger(), *clock_, 5000, "getPullOverTarget didn't work");
     shoulder_lanes.clear();
   }
 

diff --git a/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp b/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp
@@ -228,7 +228,7 @@ void PullOverModule::updatePullOverStatus()
     route_handler->setPullOverGoalPose(
       target_shoulder_lane, common_parameters.vehicle_width, parameters_.margin_from_boundary);
   } else {
-    RCLCPP_ERROR(getLogger(), "failed to get shoulder lane!!!");
+    RCLCPP_ERROR_THROTTLE(getLogger(), *clock_, 5000, "failed to get shoulder lane!!!");
   }
 
   // Get pull_over lanes
@@ -309,7 +309,8 @@ lanelet::ConstLanelets PullOverModule::getCurrentLanes() const
 
   lanelet::ConstLanelet current_lane;
   if (!route_handler->getClosestLaneletWithinRoute(current_pose, &current_lane)) {
-    RCLCPP_ERROR(getLogger(), "failed to find closest lanelet within route!!!");
+    RCLCPP_ERROR_THROTTLE(
+      getLogger(), *clock_, 5000, "failed to find closest lanelet within route!!!");
     return {};  // TODO(Horibe) what should be returned?
   }
 

diff --git a/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp b/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp
@@ -191,7 +191,8 @@ void SideShiftModule::updateData()
 
   lanelet::ConstLanelet current_lane;
   if (!route_handler->getClosestLaneletWithinRoute(reference_pose.pose, &current_lane)) {
-    RCLCPP_ERROR(getLogger(), "failed to find closest lanelet within route!!!");
+    RCLCPP_ERROR_THROTTLE(
+      getLogger(), *clock_, 5000, "failed to find closest lanelet within route!!!");
   }
 
   // For current_lanes with desired length