From 8dc0352d43e2a91826e0aa3dbf835217529a8af8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juraj=20Or=C5=A1uli=C4=87?= Date: Tue, 6 Mar 2018 15:42:15 +0100 Subject: [PATCH] Minor optimizations of cases with no subscribers (#755) --- cartographer_ros/cartographer_ros/node.cc | 37 ++++++++++++----------- 1 file changed, 20 insertions(+), 17 deletions(-) diff --git a/cartographer_ros/cartographer_ros/node.cc b/cartographer_ros/cartographer_ros/node.cc index b39f98a..f2e874c 100644 --- a/cartographer_ros/cartographer_ros/node.cc +++ b/cartographer_ros/cartographer_ros/node.cc @@ -186,21 +186,24 @@ void Node::PublishTrajectoryStates(const ::ros::WallTimerEvent& timer_event) { // frequency, and republishing it would be computationally wasteful. if (trajectory_state.local_slam_data->time != extrapolator.GetLastPoseTime()) { - // TODO(gaschler): Consider using other message without time information. - carto::sensor::TimedPointCloud point_cloud; - point_cloud.reserve( - trajectory_state.local_slam_data->range_data_in_local.returns.size()); - for (const Eigen::Vector3f point : - trajectory_state.local_slam_data->range_data_in_local.returns) { - Eigen::Vector4f point_time; - point_time << point, 0.f; - point_cloud.push_back(point_time); + if (scan_matched_point_cloud_publisher_.getNumSubscribers() > 0) { + // TODO(gaschler): Consider using other message without time + // information. + carto::sensor::TimedPointCloud point_cloud; + point_cloud.reserve(trajectory_state.local_slam_data + ->range_data_in_local.returns.size()); + for (const Eigen::Vector3f point : + trajectory_state.local_slam_data->range_data_in_local.returns) { + Eigen::Vector4f point_time; + point_time << point, 0.f; + point_cloud.push_back(point_time); + } + scan_matched_point_cloud_publisher_.publish(ToPointCloud2Message( + carto::common::ToUniversal(trajectory_state.local_slam_data->time), + node_options_.map_frame, + carto::sensor::TransformTimedPointCloud( + point_cloud, trajectory_state.local_to_map.cast()))); } - scan_matched_point_cloud_publisher_.publish(ToPointCloud2Message( - carto::common::ToUniversal(trajectory_state.local_slam_data->time), - node_options_.map_frame, - carto::sensor::TransformTimedPointCloud( - point_cloud, trajectory_state.local_to_map.cast()))); extrapolator.AddPose(trajectory_state.local_slam_data->time, trajectory_state.local_slam_data->local_pose); } @@ -257,8 +260,8 @@ void Node::PublishTrajectoryStates(const ::ros::WallTimerEvent& timer_event) { void Node::PublishTrajectoryNodeList( const ::ros::WallTimerEvent& unused_timer_event) { - carto::common::MutexLocker lock(&mutex_); if (trajectory_node_list_publisher_.getNumSubscribers() > 0) { + carto::common::MutexLocker lock(&mutex_); trajectory_node_list_publisher_.publish( map_builder_bridge_.GetTrajectoryNodeList()); } @@ -266,8 +269,8 @@ void Node::PublishTrajectoryNodeList( void Node::PublishLandmarkPosesList( const ::ros::WallTimerEvent& unused_timer_event) { - carto::common::MutexLocker lock(&mutex_); if (landmark_poses_list_publisher_.getNumSubscribers() > 0) { + carto::common::MutexLocker lock(&mutex_); landmark_poses_list_publisher_.publish( map_builder_bridge_.GetLandmarkPosesList()); } @@ -275,8 +278,8 @@ void Node::PublishLandmarkPosesList( void Node::PublishConstraintList( const ::ros::WallTimerEvent& unused_timer_event) { - carto::common::MutexLocker lock(&mutex_); if (constraint_list_publisher_.getNumSubscribers() > 0) { + carto::common::MutexLocker lock(&mutex_); constraint_list_publisher_.publish(map_builder_bridge_.GetConstraintList()); } }