From 5d5ce68a11a35c4f313622fbef5e3fd80751b565 Mon Sep 17 00:00:00 2001 From: Michael Grupp Date: Mon, 11 Jun 2018 13:43:58 +0200 Subject: [PATCH] Fix memory leak in simulations by removing wall timers. (#891) Fixes the problem of ever-growing memory after `rosbag play --clock` finishes, as discussed in https://github.com/googlecartographer/cartographer/issues/1182 The wall timers caused the timer callback that publishes TF data to be called even if no simulated `/clock` was published anymore. As the TF buffer cache time of the TF listener seems to be based on the ROS time instead of wall clock, it could grow out of bounds. Now, `ros::Timer` plays nicely with both normal (wall) and simulated time and no callbacks are executed if `/clock` stops in simulation. --- cartographer_ros/cartographer_ros/node.cc | 33 +++++++++---------- cartographer_ros/cartographer_ros/node.h | 14 ++++---- .../occupancy_grid_node_main.cc | 11 +++---- 3 files changed, 28 insertions(+), 30 deletions(-) diff --git a/cartographer_ros/cartographer_ros/node.cc b/cartographer_ros/cartographer_ros/node.cc index 7570451..15b1d59 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); - wall_timers_.push_back(node_handle_.createWallTimer( - ::ros::WallDuration(node_options_.submap_publish_period_sec), + timers_.push_back(node_handle_.createTimer( + ::ros::Duration(node_options_.submap_publish_period_sec), &Node::PublishSubmapList, this)); - wall_timers_.push_back(node_handle_.createWallTimer( - ::ros::WallDuration(node_options_.pose_publish_period_sec), + timers_.push_back(node_handle_.createTimer( + ::ros::Duration(node_options_.pose_publish_period_sec), &Node::PublishTrajectoryStates, this)); - wall_timers_.push_back(node_handle_.createWallTimer( - ::ros::WallDuration(node_options_.trajectory_publish_period_sec), + timers_.push_back(node_handle_.createTimer( + ::ros::Duration(node_options_.trajectory_publish_period_sec), &Node::PublishTrajectoryNodeList, this)); - wall_timers_.push_back(node_handle_.createWallTimer( - ::ros::WallDuration(node_options_.trajectory_publish_period_sec), + timers_.push_back(node_handle_.createTimer( + ::ros::Duration(node_options_.trajectory_publish_period_sec), &Node::PublishLandmarkPosesList, this)); - wall_timers_.push_back(node_handle_.createWallTimer( - ::ros::WallDuration(kConstraintPublishPeriodSec), - &Node::PublishConstraintList, this)); + timers_.push_back( + node_handle_.createTimer(::ros::Duration(kConstraintPublishPeriodSec), + &Node::PublishConstraintList, this)); } Node::~Node() { FinishAllTrajectories(); } @@ -143,7 +143,7 @@ bool Node::HandleSubmapQuery( return true; } -void Node::PublishSubmapList(const ::ros::WallTimerEvent& unused_timer_event) { +void Node::PublishSubmapList(const ::ros::TimerEvent& unused_timer_event) { carto::common::MutexLocker lock(&mutex_); submap_list_publisher_.publish(map_builder_bridge_.GetSubmapList()); } @@ -176,7 +176,7 @@ void Node::AddSensorSamplers(const int trajectory_id, options.landmarks_sampling_ratio)); } -void Node::PublishTrajectoryStates(const ::ros::WallTimerEvent& timer_event) { +void Node::PublishTrajectoryStates(const ::ros::TimerEvent& timer_event) { carto::common::MutexLocker lock(&mutex_); for (const auto& entry : map_builder_bridge_.GetTrajectoryStates()) { const auto& trajectory_state = entry.second; @@ -261,7 +261,7 @@ void Node::PublishTrajectoryStates(const ::ros::WallTimerEvent& timer_event) { } void Node::PublishTrajectoryNodeList( - const ::ros::WallTimerEvent& unused_timer_event) { + const ::ros::TimerEvent& 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::WallTimerEvent& unused_timer_event) { + const ::ros::TimerEvent& unused_timer_event) { if (landmark_poses_list_publisher_.getNumSubscribers() > 0) { carto::common::MutexLocker lock(&mutex_); landmark_poses_list_publisher_.publish( @@ -278,8 +278,7 @@ void Node::PublishLandmarkPosesList( } } -void Node::PublishConstraintList( - const ::ros::WallTimerEvent& unused_timer_event) { +void Node::PublishConstraintList(const ::ros::TimerEvent& 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 1c7f25c..2c6df52 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::WallTimerEvent& timer_event); + void PublishSubmapList(const ::ros::TimerEvent& timer_event); void AddExtrapolator(int trajectory_id, const TrajectoryOptions& options); void AddSensorSamplers(int trajectory_id, const TrajectoryOptions& options); - void PublishTrajectoryStates(const ::ros::WallTimerEvent& timer_event); - void PublishTrajectoryNodeList(const ::ros::WallTimerEvent& timer_event); - void PublishLandmarkPosesList(const ::ros::WallTimerEvent& timer_event); - void PublishConstraintList(const ::ros::WallTimerEvent& timer_event); + 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 SpinOccupancyGridThreadForever(); bool ValidateTrajectoryOptions(const TrajectoryOptions& options); bool ValidateTopicNames(const ::cartographer_ros_msgs::SensorTopics& topics, @@ -205,9 +205,9 @@ 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::WallTimers around, otherwise + // We have to keep the timer handles of ::ros::Timers around, otherwise // they do not fire. - std::vector<::ros::WallTimer> wall_timers_; + std::vector<::ros::Timer> timers_; }; } // 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 8385fad..3c4aaa6 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::WallTimerEvent& timer_event); + void DrawAndPublish(const ::ros::TimerEvent& 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::WallTimer occupancy_grid_publisher_timer_; + ::ros::Timer occupancy_grid_publisher_timer_; std::string last_frame_id_; ros::Time last_timestamp_; }; @@ -91,9 +91,8 @@ 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_.createWallTimer(::ros::WallDuration(publish_period_sec), - &Node::DrawAndPublish, this)) {} + occupancy_grid_publisher_timer_(node_handle_.createTimer( + ::ros::Duration(publish_period_sec), &Node::DrawAndPublish, this)) {} void Node::HandleSubmapList( const cartographer_ros_msgs::SubmapList::ConstPtr& msg) { @@ -153,7 +152,7 @@ void Node::HandleSubmapList( last_frame_id_ = msg->header.frame_id; } -void Node::DrawAndPublish(const ::ros::WallTimerEvent& unused_timer_event) { +void Node::DrawAndPublish(const ::ros::TimerEvent& unused_timer_event) { if (submap_slices_.empty() || last_frame_id_.empty()) { return; }