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.
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<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,
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());
}
}