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

37
cartographer_ros/cartographer_ros/node.cc
View File

@ -186,21 +186,24 @@ 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) {
carto::sensor::TimedPointCloud point_cloud; // TODO(gaschler): Consider using other message without time
point_cloud.reserve( // information.
trajectory_state.local_slam_data->range_data_in_local.returns.size()); carto::sensor::TimedPointCloud point_cloud;
for (const Eigen::Vector3f point : point_cloud.reserve(trajectory_state.local_slam_data
trajectory_state.local_slam_data->range_data_in_local.returns) { ->range_data_in_local.returns.size());
Eigen::Vector4f point_time; for (const Eigen::Vector3f point :
point_time << point, 0.f; trajectory_state.local_slam_data->range_data_in_local.returns) {
point_cloud.push_back(point_time); 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<float>())));
} }
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<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());
} }
} }