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>(
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());

17
cartographer_ros/cartographer_ros/node.h
View File

@ -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<std::string> subscribed_topics_;
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.
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

11
cartographer_ros/cartographer_ros/occupancy_grid_node_main.cc
View File

@ -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<SubmapId, SubmapSlice> 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;
}