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

Minor optimizations of cases with no subscribers (#755)

master
Juraj Oršulić 2018-03-06 15:42:15 +01:00 committed by Wally B. Feed
parent b274743eb7
commit 8dc0352d43
1 changed files with 20 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
cartographer_ros/cartographer_ros/node.cc
View File

@ -186,10 +186,12 @@ void Node::PublishTrajectoryStates(const ::ros::WallTimerEvent& timer_event) {
// frequency, and republishing it would be computationally wasteful. // frequency, and republishing it would be computationally wasteful.
if (trajectory_state.local_slam_data->time != if (trajectory_state.local_slam_data->time !=
extrapolator.GetLastPoseTime()) { extrapolator.GetLastPoseTime()) {
// TODO(gaschler): Consider using other message without time information. if (scan_matched_point_cloud_publisher_.getNumSubscribers() > 0) {
// TODO(gaschler): Consider using other message without time
// information.
carto::sensor::TimedPointCloud point_cloud; carto::sensor::TimedPointCloud point_cloud;
point_cloud.reserve( point_cloud.reserve(trajectory_state.local_slam_data
trajectory_state.local_slam_data->range_data_in_local.returns.size()); ->range_data_in_local.returns.size());
for (const Eigen::Vector3f point : for (const Eigen::Vector3f point :
trajectory_state.local_slam_data->range_data_in_local.returns) { trajectory_state.local_slam_data->range_data_in_local.returns) {
Eigen::Vector4f point_time; Eigen::Vector4f point_time;
@ -201,6 +203,7 @@ void Node::PublishTrajectoryStates(const ::ros::WallTimerEvent& timer_event) {
node_options_.map_frame, node_options_.map_frame,
carto::sensor::TransformTimedPointCloud( carto::sensor::TransformTimedPointCloud(
point_cloud, trajectory_state.local_to_map.cast<float>()))); point_cloud, trajectory_state.local_to_map.cast<float>())));
}
extrapolator.AddPose(trajectory_state.local_slam_data->time, extrapolator.AddPose(trajectory_state.local_slam_data->time,
trajectory_state.local_slam_data->local_pose); trajectory_state.local_slam_data->local_pose);
} }
@ -257,8 +260,8 @@ void Node::PublishTrajectoryStates(const ::ros::WallTimerEvent& timer_event) {
void Node::PublishTrajectoryNodeList( void Node::PublishTrajectoryNodeList(
const ::ros::WallTimerEvent& unused_timer_event) { const ::ros::WallTimerEvent& unused_timer_event) {
carto::common::MutexLocker lock(&mutex_);
if (trajectory_node_list_publisher_.getNumSubscribers() > 0) { if (trajectory_node_list_publisher_.getNumSubscribers() > 0) {
carto::common::MutexLocker lock(&mutex_);
trajectory_node_list_publisher_.publish( trajectory_node_list_publisher_.publish(
map_builder_bridge_.GetTrajectoryNodeList()); map_builder_bridge_.GetTrajectoryNodeList());
} }
@ -266,8 +269,8 @@ void Node::PublishTrajectoryNodeList(
void Node::PublishLandmarkPosesList( void Node::PublishLandmarkPosesList(
const ::ros::WallTimerEvent& unused_timer_event) { const ::ros::WallTimerEvent& unused_timer_event) {
carto::common::MutexLocker lock(&mutex_);
if (landmark_poses_list_publisher_.getNumSubscribers() > 0) { if (landmark_poses_list_publisher_.getNumSubscribers() > 0) {
carto::common::MutexLocker lock(&mutex_);
landmark_poses_list_publisher_.publish( landmark_poses_list_publisher_.publish(
map_builder_bridge_.GetLandmarkPosesList()); map_builder_bridge_.GetLandmarkPosesList());
} }
@ -275,8 +278,8 @@ void Node::PublishLandmarkPosesList(
void Node::PublishConstraintList( void Node::PublishConstraintList(
const ::ros::WallTimerEvent& unused_timer_event) { const ::ros::WallTimerEvent& unused_timer_event) {
carto::common::MutexLocker lock(&mutex_);
if (constraint_list_publisher_.getNumSubscribers() > 0) { if (constraint_list_publisher_.getNumSubscribers() > 0) {
carto::common::MutexLocker lock(&mutex_);
constraint_list_publisher_.publish(map_builder_bridge_.GetConstraintList()); constraint_list_publisher_.publish(map_builder_bridge_.GetConstraintList());
} }
} }