From 424e702289a1df6fda5427fd641407ec3f4235fb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juraj=20Or=C5=A1uli=C4=87?= Date: Thu, 14 Jun 2018 12:43:10 +0200 Subject: [PATCH] Revert timers other than PublishTrajectoryStates back to being WallTimers. (#898) --- cartographer_ros/cartographer_ros/node.cc | 31 ++++++++++--------- cartographer_ros/cartographer_ros/node.h | 17 ++++++---- .../occupancy_grid_node_main.cc | 11 ++++--- 3 files changed, 33 insertions(+), 26 deletions(-) diff --git a/cartographer_ros/cartographer_ros/node.cc b/cartographer_ros/cartographer_ros/node.cc index 15b1d59..cf18a71 100644 --- a/cartographer_ros/cartographer_ros/node.cc +++ b/cartographer_ros/cartographer_ros/node.cc @@ -114,21 +114,21 @@ Node::Node( node_handle_.advertise( kScanMatchedPointCloudTopic, kLatestOnlyPublisherQueueSize); - timers_.push_back(node_handle_.createTimer( - ::ros::Duration(node_options_.submap_publish_period_sec), + wall_timers_.push_back(node_handle_.createWallTimer( + ::ros::WallDuration(node_options_.submap_publish_period_sec), &Node::PublishSubmapList, this)); - timers_.push_back(node_handle_.createTimer( + publish_trajectory_states_timer_ = node_handle_.createTimer( ::ros::Duration(node_options_.pose_publish_period_sec), - &Node::PublishTrajectoryStates, this)); - timers_.push_back(node_handle_.createTimer( - ::ros::Duration(node_options_.trajectory_publish_period_sec), + &Node::PublishTrajectoryStates, this); + wall_timers_.push_back(node_handle_.createWallTimer( + ::ros::WallDuration(node_options_.trajectory_publish_period_sec), &Node::PublishTrajectoryNodeList, this)); - timers_.push_back(node_handle_.createTimer( - ::ros::Duration(node_options_.trajectory_publish_period_sec), + wall_timers_.push_back(node_handle_.createWallTimer( + ::ros::WallDuration(node_options_.trajectory_publish_period_sec), &Node::PublishLandmarkPosesList, this)); - timers_.push_back( - node_handle_.createTimer(::ros::Duration(kConstraintPublishPeriodSec), - &Node::PublishConstraintList, this)); + wall_timers_.push_back(node_handle_.createWallTimer( + ::ros::WallDuration(kConstraintPublishPeriodSec), + &Node::PublishConstraintList, this)); } Node::~Node() { FinishAllTrajectories(); } @@ -143,7 +143,7 @@ bool Node::HandleSubmapQuery( return true; } -void Node::PublishSubmapList(const ::ros::TimerEvent& unused_timer_event) { +void Node::PublishSubmapList(const ::ros::WallTimerEvent& unused_timer_event) { carto::common::MutexLocker lock(&mutex_); submap_list_publisher_.publish(map_builder_bridge_.GetSubmapList()); } @@ -261,7 +261,7 @@ void Node::PublishTrajectoryStates(const ::ros::TimerEvent& timer_event) { } void Node::PublishTrajectoryNodeList( - const ::ros::TimerEvent& unused_timer_event) { + const ::ros::WallTimerEvent& unused_timer_event) { if (trajectory_node_list_publisher_.getNumSubscribers() > 0) { carto::common::MutexLocker lock(&mutex_); trajectory_node_list_publisher_.publish( @@ -270,7 +270,7 @@ void Node::PublishTrajectoryNodeList( } void Node::PublishLandmarkPosesList( - const ::ros::TimerEvent& unused_timer_event) { + const ::ros::WallTimerEvent& unused_timer_event) { if (landmark_poses_list_publisher_.getNumSubscribers() > 0) { carto::common::MutexLocker lock(&mutex_); landmark_poses_list_publisher_.publish( @@ -278,7 +278,8 @@ void Node::PublishLandmarkPosesList( } } -void Node::PublishConstraintList(const ::ros::TimerEvent& unused_timer_event) { +void Node::PublishConstraintList( + const ::ros::WallTimerEvent& unused_timer_event) { if (constraint_list_publisher_.getNumSubscribers() > 0) { carto::common::MutexLocker lock(&mutex_); constraint_list_publisher_.publish(map_builder_bridge_.GetConstraintList()); diff --git a/cartographer_ros/cartographer_ros/node.h b/cartographer_ros/cartographer_ros/node.h index 2c6df52..9a41a22 100644 --- a/cartographer_ros/cartographer_ros/node.h +++ b/cartographer_ros/cartographer_ros/node.h @@ -149,13 +149,13 @@ class Node { void LaunchSubscribers(const TrajectoryOptions& options, const cartographer_ros_msgs::SensorTopics& topics, int trajectory_id); - void PublishSubmapList(const ::ros::TimerEvent& timer_event); + void PublishSubmapList(const ::ros::WallTimerEvent& timer_event); void AddExtrapolator(int trajectory_id, const TrajectoryOptions& options); void AddSensorSamplers(int trajectory_id, const TrajectoryOptions& options); void PublishTrajectoryStates(const ::ros::TimerEvent& timer_event); - void PublishTrajectoryNodeList(const ::ros::TimerEvent& timer_event); - void PublishLandmarkPosesList(const ::ros::TimerEvent& timer_event); - void PublishConstraintList(const ::ros::TimerEvent& timer_event); + void PublishTrajectoryNodeList(const ::ros::WallTimerEvent& timer_event); + void PublishLandmarkPosesList(const ::ros::WallTimerEvent& timer_event); + void PublishConstraintList(const ::ros::WallTimerEvent& timer_event); void SpinOccupancyGridThreadForever(); bool ValidateTrajectoryOptions(const TrajectoryOptions& options); bool ValidateTopicNames(const ::cartographer_ros_msgs::SensorTopics& topics, @@ -205,9 +205,14 @@ class Node { std::unordered_set subscribed_topics_; std::unordered_map is_active_trajectory_ GUARDED_BY(mutex_); - // We have to keep the timer handles of ::ros::Timers around, otherwise + // We have to keep the timer handles of ::ros::WallTimers around, otherwise // they do not fire. - std::vector<::ros::Timer> timers_; + std::vector<::ros::WallTimer> wall_timers_; + // The timer for publishing trajectory states (i.e. pose transforms) is a + // regular timer which is not triggered when simulation time is standing + // still. This prevents overflowing the transform listener buffer by + // publishing the same transforms over and over again. + ::ros::Timer publish_trajectory_states_timer_; }; } // namespace cartographer_ros diff --git a/cartographer_ros/cartographer_ros/occupancy_grid_node_main.cc b/cartographer_ros/cartographer_ros/occupancy_grid_node_main.cc index 3c4aaa6..8385fad 100644 --- a/cartographer_ros/cartographer_ros/occupancy_grid_node_main.cc +++ b/cartographer_ros/cartographer_ros/occupancy_grid_node_main.cc @@ -58,7 +58,7 @@ class Node { private: void HandleSubmapList(const cartographer_ros_msgs::SubmapList::ConstPtr& msg); - void DrawAndPublish(const ::ros::TimerEvent& timer_event); + void DrawAndPublish(const ::ros::WallTimerEvent& timer_event); void PublishOccupancyGrid(const std::string& frame_id, const ros::Time& time, const Eigen::Array2f& origin, cairo_surface_t* surface); @@ -71,7 +71,7 @@ class Node { ::ros::Subscriber submap_list_subscriber_ GUARDED_BY(mutex_); ::ros::Publisher occupancy_grid_publisher_ GUARDED_BY(mutex_); std::map submap_slices_ GUARDED_BY(mutex_); - ::ros::Timer occupancy_grid_publisher_timer_; + ::ros::WallTimer occupancy_grid_publisher_timer_; std::string last_frame_id_; ros::Time last_timestamp_; }; @@ -91,8 +91,9 @@ Node::Node(const double resolution, const double publish_period_sec) node_handle_.advertise<::nav_msgs::OccupancyGrid>( kOccupancyGridTopic, kLatestOnlyPublisherQueueSize, true /* latched */)), - occupancy_grid_publisher_timer_(node_handle_.createTimer( - ::ros::Duration(publish_period_sec), &Node::DrawAndPublish, this)) {} + occupancy_grid_publisher_timer_( + node_handle_.createWallTimer(::ros::WallDuration(publish_period_sec), + &Node::DrawAndPublish, this)) {} void Node::HandleSubmapList( const cartographer_ros_msgs::SubmapList::ConstPtr& msg) { @@ -152,7 +153,7 @@ void Node::HandleSubmapList( last_frame_id_ = msg->header.frame_id; } -void Node::DrawAndPublish(const ::ros::TimerEvent& unused_timer_event) { +void Node::DrawAndPublish(const ::ros::WallTimerEvent& unused_timer_event) { if (submap_slices_.empty() || last_frame_id_.empty()) { return; }