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

Revert timers other than PublishTrajectoryStates back to being WallTimers. (#898)

master
Juraj Oršulić 2018-06-14 12:43:10 +02:00 committed by Wally B. Feed
parent 4f32f88717
commit 424e702289
3 changed files with 33 additions and 26 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,21 +114,21 @@ Node::Node(
node_handle_.advertise<sensor_msgs::PointCloud2>( node_handle_.advertise<sensor_msgs::PointCloud2>(
kScanMatchedPointCloudTopic, kLatestOnlyPublisherQueueSize); kScanMatchedPointCloudTopic, kLatestOnlyPublisherQueueSize);
timers_.push_back(node_handle_.createTimer( wall_timers_.push_back(node_handle_.createWallTimer(
::ros::Duration(node_options_.submap_publish_period_sec), ::ros::WallDuration(node_options_.submap_publish_period_sec),
&Node::PublishSubmapList, this)); &Node::PublishSubmapList, this));
timers_.push_back(node_handle_.createTimer( publish_trajectory_states_timer_ = node_handle_.createTimer(
::ros::Duration(node_options_.pose_publish_period_sec), ::ros::Duration(node_options_.pose_publish_period_sec),
&Node::PublishTrajectoryStates, this)); &Node::PublishTrajectoryStates, this);
timers_.push_back(node_handle_.createTimer( wall_timers_.push_back(node_handle_.createWallTimer(
::ros::Duration(node_options_.trajectory_publish_period_sec), ::ros::WallDuration(node_options_.trajectory_publish_period_sec),
&Node::PublishTrajectoryNodeList, this)); &Node::PublishTrajectoryNodeList, this));
timers_.push_back(node_handle_.createTimer( wall_timers_.push_back(node_handle_.createWallTimer(
::ros::Duration(node_options_.trajectory_publish_period_sec), ::ros::WallDuration(node_options_.trajectory_publish_period_sec),
&Node::PublishLandmarkPosesList, this)); &Node::PublishLandmarkPosesList, this));
timers_.push_back( wall_timers_.push_back(node_handle_.createWallTimer(
node_handle_.createTimer(::ros::Duration(kConstraintPublishPeriodSec), ::ros::WallDuration(kConstraintPublishPeriodSec),
&Node::PublishConstraintList, this)); &Node::PublishConstraintList, this));
} }
Node::~Node() { FinishAllTrajectories(); } Node::~Node() { FinishAllTrajectories(); }
@ -143,7 +143,7 @@ bool Node::HandleSubmapQuery(
return true; 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_); carto::common::MutexLocker lock(&mutex_);
submap_list_publisher_.publish(map_builder_bridge_.GetSubmapList()); submap_list_publisher_.publish(map_builder_bridge_.GetSubmapList());
} }
@ -261,7 +261,7 @@ void Node::PublishTrajectoryStates(const ::ros::TimerEvent& timer_event) {
} }
void Node::PublishTrajectoryNodeList( void Node::PublishTrajectoryNodeList(
const ::ros::TimerEvent& unused_timer_event) { const ::ros::WallTimerEvent& 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::TimerEvent& unused_timer_event) { const ::ros::WallTimerEvent& 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,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) { 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());

17
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::TimerEvent& timer_event); void PublishSubmapList(const ::ros::WallTimerEvent& 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::TimerEvent& timer_event); void PublishTrajectoryStates(const ::ros::TimerEvent& timer_event);
void PublishTrajectoryNodeList(const ::ros::TimerEvent& timer_event); void PublishTrajectoryNodeList(const ::ros::WallTimerEvent& timer_event);
void PublishLandmarkPosesList(const ::ros::TimerEvent& timer_event); void PublishLandmarkPosesList(const ::ros::WallTimerEvent& timer_event);
void PublishConstraintList(const ::ros::TimerEvent& timer_event); void PublishConstraintList(const ::ros::WallTimerEvent& 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,14 @@ 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::Timers around, otherwise // We have to keep the timer handles of ::ros::WallTimers around, otherwise
// they do not fire. // 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 } // 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::TimerEvent& timer_event); void DrawAndPublish(const ::ros::WallTimerEvent& 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::Timer occupancy_grid_publisher_timer_; ::ros::WallTimer occupancy_grid_publisher_timer_;
std::string last_frame_id_; std::string last_frame_id_;
ros::Time last_timestamp_; ros::Time last_timestamp_;
}; };
@ -91,8 +91,9 @@ 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_(node_handle_.createTimer( occupancy_grid_publisher_timer_(
::ros::Duration(publish_period_sec), &Node::DrawAndPublish, this)) {} node_handle_.createWallTimer(::ros::WallDuration(publish_period_sec),
&Node::DrawAndPublish, this)) {}
void Node::HandleSubmapList( void Node::HandleSubmapList(
const cartographer_ros_msgs::SubmapList::ConstPtr& msg) { const cartographer_ros_msgs::SubmapList::ConstPtr& msg) {
@ -152,7 +153,7 @@ void Node::HandleSubmapList(
last_frame_id_ = msg->header.frame_id; 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()) { if (submap_slices_.empty() || last_frame_id_.empty()) {
return; return;
} }