12345qiupeng
/
cartographer_ros
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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.
master
Michael Grupp 2018-06-11 13:43:58 +02:00 committed by Wally B. Feed
parent 6b473e3339
commit 5d5ce68a11
3 changed files with 28 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
cartographer_ros/cartographer_ros/node.cc
View File

@ -114,20 +114,20 @@ Node::Node(
node_handle_.advertise<sensor_msgs::PointCloud2>( node_handle_.advertise<sensor_msgs::PointCloud2>(
kScanMatchedPointCloudTopic, kLatestOnlyPublisherQueueSize); kScanMatchedPointCloudTopic, kLatestOnlyPublisherQueueSize);
wall_timers_.push_back(node_handle_.createWallTimer( timers_.push_back(node_handle_.createTimer(
::ros::WallDuration(node_options_.submap_publish_period_sec), ::ros::Duration(node_options_.submap_publish_period_sec),
&Node::PublishSubmapList, this)); &Node::PublishSubmapList, this));
wall_timers_.push_back(node_handle_.createWallTimer( timers_.push_back(node_handle_.createTimer(
::ros::WallDuration(node_options_.pose_publish_period_sec), ::ros::Duration(node_options_.pose_publish_period_sec),
&Node::PublishTrajectoryStates, this)); &Node::PublishTrajectoryStates, this));
wall_timers_.push_back(node_handle_.createWallTimer( timers_.push_back(node_handle_.createTimer(
::ros::WallDuration(node_options_.trajectory_publish_period_sec), ::ros::Duration(node_options_.trajectory_publish_period_sec),
&Node::PublishTrajectoryNodeList, this)); &Node::PublishTrajectoryNodeList, this));
wall_timers_.push_back(node_handle_.createWallTimer( timers_.push_back(node_handle_.createTimer(
::ros::WallDuration(node_options_.trajectory_publish_period_sec), ::ros::Duration(node_options_.trajectory_publish_period_sec),
&Node::PublishLandmarkPosesList, this)); &Node::PublishLandmarkPosesList, this));
wall_timers_.push_back(node_handle_.createWallTimer( timers_.push_back(
::ros::WallDuration(kConstraintPublishPeriodSec), node_handle_.createTimer(::ros::Duration(kConstraintPublishPeriodSec),
&Node::PublishConstraintList, this)); &Node::PublishConstraintList, this));
} }
@ -143,7 +143,7 @@ bool Node::HandleSubmapQuery(
return true; 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_); carto::common::MutexLocker lock(&mutex_);
submap_list_publisher_.publish(map_builder_bridge_.GetSubmapList()); submap_list_publisher_.publish(map_builder_bridge_.GetSubmapList());
} }
@ -176,7 +176,7 @@ void Node::AddSensorSamplers(const int trajectory_id,
options.landmarks_sampling_ratio)); 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_); carto::common::MutexLocker lock(&mutex_);
for (const auto& entry : map_builder_bridge_.GetTrajectoryStates()) { for (const auto& entry : map_builder_bridge_.GetTrajectoryStates()) {
const auto& trajectory_state = entry.second; const auto& trajectory_state = entry.second;
@ -261,7 +261,7 @@ void Node::PublishTrajectoryStates(const ::ros::WallTimerEvent& timer_event) {
} }
void Node::PublishTrajectoryNodeList( void Node::PublishTrajectoryNodeList(
const ::ros::WallTimerEvent& unused_timer_event) { const ::ros::TimerEvent& unused_timer_event) {
if (trajectory_node_list_publisher_.getNumSubscribers() > 0) { if (trajectory_node_list_publisher_.getNumSubscribers() > 0) {
carto::common::MutexLocker lock(&mutex_); carto::common::MutexLocker lock(&mutex_);
trajectory_node_list_publisher_.publish( trajectory_node_list_publisher_.publish(
@ -270,7 +270,7 @@ void Node::PublishTrajectoryNodeList(
} }
void Node::PublishLandmarkPosesList( void Node::PublishLandmarkPosesList(
const ::ros::WallTimerEvent& unused_timer_event) { const ::ros::TimerEvent& unused_timer_event) {
if (landmark_poses_list_publisher_.getNumSubscribers() > 0) { if (landmark_poses_list_publisher_.getNumSubscribers() > 0) {
carto::common::MutexLocker lock(&mutex_); carto::common::MutexLocker lock(&mutex_);
landmark_poses_list_publisher_.publish( landmark_poses_list_publisher_.publish(
@ -278,8 +278,7 @@ void Node::PublishLandmarkPosesList(
} }
} }
void Node::PublishConstraintList( void Node::PublishConstraintList(const ::ros::TimerEvent& unused_timer_event) {
const ::ros::WallTimerEvent& unused_timer_event) {
if (constraint_list_publisher_.getNumSubscribers() > 0) { if (constraint_list_publisher_.getNumSubscribers() > 0) {
carto::common::MutexLocker lock(&mutex_); carto::common::MutexLocker lock(&mutex_);
constraint_list_publisher_.publish(map_builder_bridge_.GetConstraintList()); constraint_list_publisher_.publish(map_builder_bridge_.GetConstraintList());

14
cartographer_ros/cartographer_ros/node.h
View File

@ -149,13 +149,13 @@ class Node {
void LaunchSubscribers(const TrajectoryOptions& options, void LaunchSubscribers(const TrajectoryOptions& options,
const cartographer_ros_msgs::SensorTopics& topics, const cartographer_ros_msgs::SensorTopics& topics,
int trajectory_id); 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 AddExtrapolator(int trajectory_id, const TrajectoryOptions& options);
void AddSensorSamplers(int trajectory_id, const TrajectoryOptions& options); void AddSensorSamplers(int trajectory_id, const TrajectoryOptions& options);
void PublishTrajectoryStates(const ::ros::WallTimerEvent& timer_event); void PublishTrajectoryStates(const ::ros::TimerEvent& timer_event);
void PublishTrajectoryNodeList(const ::ros::WallTimerEvent& timer_event); void PublishTrajectoryNodeList(const ::ros::TimerEvent& timer_event);
void PublishLandmarkPosesList(const ::ros::WallTimerEvent& timer_event); void PublishLandmarkPosesList(const ::ros::TimerEvent& timer_event);
void PublishConstraintList(const ::ros::WallTimerEvent& timer_event); void PublishConstraintList(const ::ros::TimerEvent& timer_event);
void SpinOccupancyGridThreadForever(); void SpinOccupancyGridThreadForever();
bool ValidateTrajectoryOptions(const TrajectoryOptions& options); bool ValidateTrajectoryOptions(const TrajectoryOptions& options);
bool ValidateTopicNames(const ::cartographer_ros_msgs::SensorTopics& topics, bool ValidateTopicNames(const ::cartographer_ros_msgs::SensorTopics& topics,
@ -205,9 +205,9 @@ class Node {
std::unordered_set<std::string> subscribed_topics_; std::unordered_set<std::string> subscribed_topics_;
std::unordered_map<int, bool> is_active_trajectory_ GUARDED_BY(mutex_); std::unordered_map<int, bool> 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. // they do not fire.
std::vector<::ros::WallTimer> wall_timers_; std::vector<::ros::Timer> timers_;
}; };
} // namespace cartographer_ros } // namespace cartographer_ros

11
cartographer_ros/cartographer_ros/occupancy_grid_node_main.cc
View File

@ -58,7 +58,7 @@ class Node {
private: private:
void HandleSubmapList(const cartographer_ros_msgs::SubmapList::ConstPtr& msg); 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, void PublishOccupancyGrid(const std::string& frame_id, const ros::Time& time,
const Eigen::Array2f& origin, const Eigen::Array2f& origin,
cairo_surface_t* surface); cairo_surface_t* surface);
@ -71,7 +71,7 @@ class Node {
::ros::Subscriber submap_list_subscriber_ GUARDED_BY(mutex_); ::ros::Subscriber submap_list_subscriber_ GUARDED_BY(mutex_);
::ros::Publisher occupancy_grid_publisher_ GUARDED_BY(mutex_); ::ros::Publisher occupancy_grid_publisher_ GUARDED_BY(mutex_);
std::map<SubmapId, SubmapSlice> submap_slices_ GUARDED_BY(mutex_); std::map<SubmapId, SubmapSlice> submap_slices_ GUARDED_BY(mutex_);
::ros::WallTimer occupancy_grid_publisher_timer_; ::ros::Timer occupancy_grid_publisher_timer_;
std::string last_frame_id_; std::string last_frame_id_;
ros::Time last_timestamp_; ros::Time last_timestamp_;
}; };
@ -91,9 +91,8 @@ Node::Node(const double resolution, const double publish_period_sec)
node_handle_.advertise<::nav_msgs::OccupancyGrid>( node_handle_.advertise<::nav_msgs::OccupancyGrid>(
kOccupancyGridTopic, kLatestOnlyPublisherQueueSize, kOccupancyGridTopic, kLatestOnlyPublisherQueueSize,
true /* latched */)), true /* latched */)),
occupancy_grid_publisher_timer_( occupancy_grid_publisher_timer_(node_handle_.createTimer(
node_handle_.createWallTimer(::ros::WallDuration(publish_period_sec), ::ros::Duration(publish_period_sec), &Node::DrawAndPublish, this)) {}
&Node::DrawAndPublish, this)) {}
void Node::HandleSubmapList( void Node::HandleSubmapList(
const cartographer_ros_msgs::SubmapList::ConstPtr& msg) { const cartographer_ros_msgs::SubmapList::ConstPtr& msg) {
@ -153,7 +152,7 @@ void Node::HandleSubmapList(
last_frame_id_ = msg->header.frame_id; 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()) { if (submap_slices_.empty() || last_frame_id_.empty()) {
return; return;
} }