Skip to content

Commit 932e87f

Browse files
authored
chore(autoware_costmap_generator): suppress Could not find a connection between 'map' and 'base_link' (#9655)
Signed-off-by: Y.Hisaki <yhisaki31@gmail.com>
1 parent 51a74c6 commit 932e87f

File tree

1 file changed

+5
-3
lines changed

1 file changed

+5
-3
lines changed

planning/autoware_costmap_generator/src/costmap_generator.cpp

+5-3
Original file line numberDiff line numberDiff line change
@@ -51,6 +51,7 @@
5151
#include <autoware_lanelet2_extension/utility/utilities.hpp>
5252
#include <autoware_lanelet2_extension/visualization/visualization.hpp>
5353
#include <pcl_ros/transforms.hpp>
54+
#include <rclcpp/clock.hpp>
5455
#include <rclcpp/logging.hpp>
5556
#include <tf2_eigen/tf2_eigen.hpp>
5657

@@ -68,14 +69,15 @@ namespace
6869

6970
// Copied from scenario selector
7071
geometry_msgs::msg::PoseStamped::ConstSharedPtr getCurrentPose(
71-
const tf2_ros::Buffer & tf_buffer, const rclcpp::Logger & logger)
72+
const tf2_ros::Buffer & tf_buffer, const rclcpp::Logger & logger,
73+
const rclcpp::Clock::SharedPtr clock)
7274
{
7375
geometry_msgs::msg::TransformStamped tf_current_pose;
7476

7577
try {
7678
tf_current_pose = tf_buffer.lookupTransform("map", "base_link", tf2::TimePointZero);
7779
} catch (tf2::TransformException & ex) {
78-
RCLCPP_ERROR(logger, "%s", ex.what());
80+
RCLCPP_ERROR_THROTTLE(logger, *clock, 5000, "%s", ex.what());
7981
return nullptr;
8082
}
8183

@@ -253,7 +255,7 @@ void CostmapGenerator::update_data()
253255

254256
void CostmapGenerator::set_current_pose()
255257
{
256-
current_pose_ = getCurrentPose(tf_buffer_, this->get_logger());
258+
current_pose_ = getCurrentPose(tf_buffer_, this->get_logger(), this->get_clock());
257259
}
258260

259261
void CostmapGenerator::onTimer()

0 commit comments

Comments
 (0)