51
51
#include < autoware_lanelet2_extension/utility/utilities.hpp>
52
52
#include < autoware_lanelet2_extension/visualization/visualization.hpp>
53
53
#include < pcl_ros/transforms.hpp>
54
+ #include < rclcpp/clock.hpp>
54
55
#include < rclcpp/logging.hpp>
55
56
#include < tf2_eigen/tf2_eigen.hpp>
56
57
@@ -68,14 +69,15 @@ namespace
68
69
69
70
// Copied from scenario selector
70
71
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)
72
74
{
73
75
geometry_msgs::msg::TransformStamped tf_current_pose;
74
76
75
77
try {
76
78
tf_current_pose = tf_buffer.lookupTransform (" map" , " base_link" , tf2::TimePointZero);
77
79
} catch (tf2::TransformException & ex) {
78
- RCLCPP_ERROR (logger, " %s" , ex.what ());
80
+ RCLCPP_ERROR_THROTTLE (logger, * clock , 5000 , " %s" , ex.what ());
79
81
return nullptr ;
80
82
}
81
83
@@ -253,7 +255,7 @@ void CostmapGenerator::update_data()
253
255
254
256
void CostmapGenerator::set_current_pose ()
255
257
{
256
- current_pose_ = getCurrentPose (tf_buffer_, this ->get_logger ());
258
+ current_pose_ = getCurrentPose (tf_buffer_, this ->get_logger (), this -> get_clock () );
257
259
}
258
260
259
261
void CostmapGenerator::onTimer ()
0 commit comments