diff --git a/src/ros_filter.cpp b/src/ros_filter.cpp index 54a29277..be89e1af 100644 --- a/src/ros_filter.cpp +++ b/src/ros_filter.cpp @@ -348,7 +348,7 @@ void RosFilter::controlStampedCallback( filter_.setControl(latest_control_, msg->header.stamp); } else { RCLCPP_WARN_STREAM_THROTTLE( - get_logger(), *get_clock(), 5.0, "Commanded velocities " + get_logger(), *get_clock(), 5000, "Commanded velocities " " must be given in the robot's body frame (" << base_link_frame_id_ << "). Message frame was " << msg->header.frame_id); } @@ -2221,7 +2221,7 @@ void RosFilter::periodicUpdate() RCLCPP_ERROR_STREAM_SKIPFIRST_THROTTLE( get_logger(), *get_clock(), - 5.0, + 5000, "Could not obtain transform from " << odom_frame_id_ << "->" << base_link_frame_id_); } } else { diff --git a/src/ros_filter_utilities.cpp b/src/ros_filter_utilities.cpp index 24e95dbf..1c547a11 100644 --- a/src/ros_filter_utilities.cpp +++ b/src/ros_filter_utilities.cpp @@ -49,15 +49,6 @@ #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "tf2_ros/buffer.h" -#define THROTTLE(clock, duration, thing) do { \ - static rclcpp::Time _last_output_time ## __LINE__(0, 0, (clock)->get_clock_type()); \ - auto _now = (clock)->now(); \ - if (_now - _last_output_time ## __LINE__ > (duration)) { \ - _last_output_time ## __LINE__ = _now; \ - thing; \ - } \ -} while (0) - std::ostream & operator<<(std::ostream & os, const tf2::Vector3 & vec) { os << "(" << std::setprecision(20) << vec.getX() << " " << vec.getY() << " " << diff --git a/src/ros_robot_localization_listener.cpp b/src/ros_robot_localization_listener.cpp index e745670b..e5083708 100644 --- a/src/ros_robot_localization_listener.cpp +++ b/src/ros_robot_localization_listener.cpp @@ -51,15 +51,6 @@ #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "yaml-cpp/yaml.h" -#define THROTTLE(clock, duration, thing) do { \ - static rclcpp::Time _last_output_time ## __LINE__(0, 0, (clock)->get_clock_type()); \ - auto _now = (clock)->now(); \ - if (_now - _last_output_time ## __LINE__ > (duration)) { \ - _last_output_time ## __LINE__ = _now; \ - thing; \ - } \ -} while (0) - namespace robot_localization { FilterTypes::FilterType filterTypeFromString( @@ -165,14 +156,11 @@ RosRobotLocalizationListener::RosRobotLocalizationListener( // Wait until the base and world frames are set by the incoming messages while (rclcpp::ok() && base_frame_id_.empty()) { rclcpp::spin_some(node); - // TODO(ros2/rclcpp#879) RCLCPP_THROTTLE_INFO() when released - THROTTLE( - node->get_clock(), std::chrono::seconds(1), - RCLCPP_INFO( - node_logger_->get_logger(), - "Ros Robot Localization Listener: Waiting for incoming messages on " - "topics %s and %s", - odom_sub_.getTopic().c_str(), accel_sub_.getTopic().c_str())); + RCLCPP_INFO_THROTTLE( + node_logger_->get_logger(), *node->get_clock(), 1000, + "Ros Robot Localization Listener: Waiting for incoming messages on " + "topics %s and %s", + odom_sub_.getTopic().c_str(), accel_sub_.getTopic().c_str()); rclcpp::Rate(10).sleep(); } }