Skip to content

Commit

Permalink
chore(autoware_costmap_generator): suppress Could not find a connecti…
Browse files Browse the repository at this point in the history
…on between 'map' and 'base_link' (autowarefoundation#9655)

Signed-off-by: Y.Hisaki <[email protected]>
  • Loading branch information
yhisaki authored Dec 16, 2024
1 parent 51a74c6 commit 932e87f
Showing 1 changed file with 5 additions and 3 deletions.
8 changes: 5 additions & 3 deletions planning/autoware_costmap_generator/src/costmap_generator.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -51,6 +51,7 @@
#include <autoware_lanelet2_extension/utility/utilities.hpp>
#include <autoware_lanelet2_extension/visualization/visualization.hpp>
#include <pcl_ros/transforms.hpp>
#include <rclcpp/clock.hpp>
#include <rclcpp/logging.hpp>
#include <tf2_eigen/tf2_eigen.hpp>

Expand All @@ -68,14 +69,15 @@ namespace

// Copied from scenario selector
geometry_msgs::msg::PoseStamped::ConstSharedPtr getCurrentPose(
const tf2_ros::Buffer & tf_buffer, const rclcpp::Logger & logger)
const tf2_ros::Buffer & tf_buffer, const rclcpp::Logger & logger,
const rclcpp::Clock::SharedPtr clock)
{
geometry_msgs::msg::TransformStamped tf_current_pose;

try {
tf_current_pose = tf_buffer.lookupTransform("map", "base_link", tf2::TimePointZero);
} catch (tf2::TransformException & ex) {
RCLCPP_ERROR(logger, "%s", ex.what());
RCLCPP_ERROR_THROTTLE(logger, *clock, 5000, "%s", ex.what());
return nullptr;
}

Expand Down Expand Up @@ -253,7 +255,7 @@ void CostmapGenerator::update_data()

void CostmapGenerator::set_current_pose()
{
current_pose_ = getCurrentPose(tf_buffer_, this->get_logger());
current_pose_ = getCurrentPose(tf_buffer_, this->get_logger(), this->get_clock());
}

void CostmapGenerator::onTimer()
Expand Down

0 comments on commit 932e87f

Please sign in to comment.