Skip to content

Commit

Permalink
adjust publishCloud()
Browse files Browse the repository at this point in the history
  • Loading branch information
@TixiaoShan
TixiaoShan committed Feb 4, 2022
1 parent 20fdc6d commit 4f2ccdb
Show file tree
Hide file tree
Showing 5 changed files with 17 additions and 19 deletions.
6 changes: 3 additions & 3 deletions include/utility.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -279,14 +279,14 @@ class ParamServer
};

template<typename T>
sensor_msgs::PointCloud2 publishCloud(ros::Publisher *thisPub, const T& thisCloud, ros::Time thisStamp, std::string thisFrame)
sensor_msgs::PointCloud2 publishCloud(const ros::Publisher& thisPub, const T& thisCloud, ros::Time thisStamp, std::string thisFrame)
{
sensor_msgs::PointCloud2 tempCloud;
pcl::toROSMsg(*thisCloud, tempCloud);
tempCloud.header.stamp = thisStamp;
tempCloud.header.frame_id = thisFrame;
if (thisPub->getNumSubscribers() != 0)
thisPub->publish(tempCloud);
if (thisPub.getNumSubscribers() != 0)
thisPub.publish(tempCloud);
return tempCloud;
}

Expand Down
1 change: 1 addition & 0 deletions msg/cloud_info.msg
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,5 +30,6 @@ sensor_msgs/PointCloud2 cloud_surface # extracted surface feature

# 3rd party messages
sensor_msgs/PointCloud2 key_frame_cloud
sensor_msgs/PointCloud2 key_frame_color
sensor_msgs/PointCloud2 key_frame_poses
sensor_msgs/PointCloud2 key_frame_map
4 changes: 2 additions & 2 deletions src/featureExtraction.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -250,8 +250,8 @@ class FeatureExtraction : public ParamServer
// free cloud info memory
freeCloudInfoMemory();
// save newly extracted features
cloudInfo.cloud_corner = publishCloud(&pubCornerPoints, cornerCloud, cloudHeader.stamp, lidarFrame);
cloudInfo.cloud_surface = publishCloud(&pubSurfacePoints, surfaceCloud, cloudHeader.stamp, lidarFrame);
cloudInfo.cloud_corner = publishCloud(pubCornerPoints, cornerCloud, cloudHeader.stamp, lidarFrame);
cloudInfo.cloud_surface = publishCloud(pubSurfacePoints, surfaceCloud, cloudHeader.stamp, lidarFrame);
// publish to mapOptimization
pubLaserCloudInfo.publish(cloudInfo);
}
Expand Down
2 changes: 1 addition & 1 deletion src/imageProjection.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -588,7 +588,7 @@ class ImageProjection : public ParamServer
void publishClouds()
{
cloudInfo.header = cloudHeader;
cloudInfo.cloud_deskewed = publishCloud(&pubExtractedCloud, extractedCloud, cloudHeader.stamp, lidarFrame);
cloudInfo.cloud_deskewed = publishCloud(pubExtractedCloud, extractedCloud, cloudHeader.stamp, lidarFrame);
pubLaserCloudInfo.publish(cloudInfo);
}
};
Expand Down
23 changes: 10 additions & 13 deletions src/mapOptmization.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -74,7 +74,6 @@ class mapOptimization : public ParamServer
ros::Publisher pubLoopConstraintEdge;

ros::Publisher pubSLAMInfo;
ros::Publisher pubSLAMDummy;

ros::Subscriber subCloud;
ros::Subscriber subGPS;
Expand Down Expand Up @@ -183,7 +182,6 @@ class mapOptimization : public ParamServer
pubCloudRegisteredRaw = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/mapping/cloud_registered_raw", 1);

pubSLAMInfo = nh.advertise<lio_sam::cloud_info>("lio_sam/mapping/slam_info", 1);
pubSLAMDummy = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/mapping/slam_dummy", 1);

downSizeFilterCorner.setLeafSize(mappingCornerLeafSize, mappingCornerLeafSize, mappingCornerLeafSize);
downSizeFilterSurf.setLeafSize(mappingSurfLeafSize, mappingSurfLeafSize, mappingSurfLeafSize);
Expand Down Expand Up @@ -488,7 +486,7 @@ class mapOptimization : public ParamServer
downSizeFilterGlobalMapKeyFrames.setLeafSize(globalMapVisualizationLeafSize, globalMapVisualizationLeafSize, globalMapVisualizationLeafSize); // for global map visualization
downSizeFilterGlobalMapKeyFrames.setInputCloud(globalMapKeyFrames);
downSizeFilterGlobalMapKeyFrames.filter(*globalMapKeyFramesDS);
publishCloud(&pubLaserCloudSurround, globalMapKeyFramesDS, timeLaserInfoStamp, odometryFrame);
publishCloud(pubLaserCloudSurround, globalMapKeyFramesDS, timeLaserInfoStamp, odometryFrame);
}


Expand Down Expand Up @@ -554,7 +552,7 @@ class mapOptimization : public ParamServer
if (cureKeyframeCloud->size() < 300 || prevKeyframeCloud->size() < 1000)
return;
if (pubHistoryKeyFrames.getNumSubscribers() != 0)
publishCloud(&pubHistoryKeyFrames, prevKeyframeCloud, timeLaserInfoStamp, odometryFrame);
publishCloud(pubHistoryKeyFrames, prevKeyframeCloud, timeLaserInfoStamp, odometryFrame);
}

// ICP Settings
Expand All @@ -579,7 +577,7 @@ class mapOptimization : public ParamServer
{
pcl::PointCloud<PointType>::Ptr closed_cloud(new pcl::PointCloud<PointType>());
pcl::transformPointCloud(*cureKeyframeCloud, *closed_cloud, icp.getFinalTransformation());
publishCloud(&pubIcpKeyFrames, closed_cloud, timeLaserInfoStamp, odometryFrame);
publishCloud(pubIcpKeyFrames, closed_cloud, timeLaserInfoStamp, odometryFrame);
}

// Get pose transformation
Expand Down Expand Up @@ -1702,17 +1700,17 @@ class mapOptimization : public ParamServer
if (cloudKeyPoses3D->points.empty())
return;
// publish key poses
publishCloud(&pubKeyPoses, cloudKeyPoses3D, timeLaserInfoStamp, odometryFrame);
publishCloud(pubKeyPoses, cloudKeyPoses3D, timeLaserInfoStamp, odometryFrame);
// Publish surrounding key frames
publishCloud(&pubRecentKeyFrames, laserCloudSurfFromMapDS, timeLaserInfoStamp, odometryFrame);
publishCloud(pubRecentKeyFrames, laserCloudSurfFromMapDS, timeLaserInfoStamp, odometryFrame);
// publish registered key frame
if (pubRecentKeyFrame.getNumSubscribers() != 0)
{
pcl::PointCloud<PointType>::Ptr cloudOut(new pcl::PointCloud<PointType>());
PointTypePose thisPose6D = trans2PointTypePose(transformTobeMapped);
*cloudOut += *transformPointCloud(laserCloudCornerLastDS, &thisPose6D);
*cloudOut += *transformPointCloud(laserCloudSurfLastDS, &thisPose6D);
publishCloud(&pubRecentKeyFrame, cloudOut, timeLaserInfoStamp, odometryFrame);
publishCloud(pubRecentKeyFrame, cloudOut, timeLaserInfoStamp, odometryFrame);
}
// publish registered high-res raw cloud
if (pubCloudRegisteredRaw.getNumSubscribers() != 0)
Expand All @@ -1721,7 +1719,7 @@ class mapOptimization : public ParamServer
pcl::fromROSMsg(cloudInfo.cloud_deskewed, *cloudOut);
PointTypePose thisPose6D = trans2PointTypePose(transformTobeMapped);
*cloudOut = *transformPointCloud(cloudOut, &thisPose6D);
publishCloud(&pubCloudRegisteredRaw, cloudOut, timeLaserInfoStamp, odometryFrame);
publishCloud(pubCloudRegisteredRaw, cloudOut, timeLaserInfoStamp, odometryFrame);
}
// publish path
if (pubPath.getNumSubscribers() != 0)
Expand All @@ -1741,16 +1739,15 @@ class mapOptimization : public ParamServer
pcl::PointCloud<PointType>::Ptr cloudOut(new pcl::PointCloud<PointType>());
*cloudOut += *laserCloudCornerLastDS;
*cloudOut += *laserCloudSurfLastDS;
slamInfo.key_frame_cloud = publishCloud(&pubSLAMDummy, cloudOut, timeLaserInfoStamp, lidarFrame);
slamInfo.key_frame_poses = publishCloud(&pubSLAMDummy, cloudKeyPoses6D, timeLaserInfoStamp, odometryFrame);
slamInfo.key_frame_cloud = publishCloud(ros::Publisher(), cloudOut, timeLaserInfoStamp, lidarFrame);
slamInfo.key_frame_poses = publishCloud(ros::Publisher(), cloudKeyPoses6D, timeLaserInfoStamp, odometryFrame);
pcl::PointCloud<PointType>::Ptr localMapOut(new pcl::PointCloud<PointType>());
*localMapOut += *laserCloudCornerFromMapDS;
*localMapOut += *laserCloudSurfFromMapDS;
slamInfo.key_frame_map = publishCloud(&pubSLAMDummy, localMapOut, timeLaserInfoStamp, odometryFrame);
slamInfo.key_frame_map = publishCloud(ros::Publisher(), localMapOut, timeLaserInfoStamp, odometryFrame);
pubSLAMInfo.publish(slamInfo);
lastSLAMInfoPubSize = cloudKeyPoses6D->size();
}

}
}
};
Expand Down

0 comments on commit 4f2ccdb

Please sign in to comment.