void callback(const sensor_msgs::PointCloud2ConstPtr& input)
{
// Converting from PointCloud2 msg to pcl::PointCloud
pcl::PCLPointCloud2 pcl_pc2;
pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_out(new pcl::PointCloud<pcl::PointXYZ>);
pcl_conversions::toPCL(*input,pcl_pc2);
pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_in(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromPCLPointCloud2(pcl_pc2,*pcl_in);
// waiting for the transform and transform point cloud from its frame to the /camera_link frame
// You might need to change camera_link frame to what ever frame you would like your
// pointcloud to be transformed to...
tf_listener->waitForTransform("/camera_link", (*pcl_in).header.frame_id, input->header.stamp, ros::Duration(5.0));
pcl_ros::transformPointCloud("/camera_link", *pcl_in, *pcl_out, *tf_listener);
sensor_msgs::PointCloud2 output;
pcl::toROSMsg (*pcl_out, output);
tf_pub.publish(output);
}
void CloudAssembler::process(const sensor_msgs::PointCloud2::ConstPtr &cloud)
{
ROS_INFO_STREAM_ONCE("CloudAssembler::process(): Point cloud received");
geometry_msgs::PoseStamped p;
if (!getRobotPose(cloud->header.stamp, p)) return;
bool update = false;
double dist = sqrt(pow(robot_pose_.pose.position.x - p.pose.position.x, 2) + pow(robot_pose_.pose.position.y - p.pose.position.y, 2));
if (dist > dist_th_)
{
robot_pose_ = p;
if (dist > max_dist_th_)
{
cloud_buff_->clear();
}
else update = true;
}
VPointCloud vpcl;
TPointCloudPtr tpcl(new TPointCloud());
// Retrieve the input point cloud
pcl::fromROSMsg(*cloud, vpcl);
pcl::copyPointCloud(vpcl, *tpcl);
pcl::PassThrough< TPoint > pass;
pass.setInputCloud(tpcl);
pass.setFilterFieldName("x");
pass.setFilterLimits(min_x_, max_x_);
pass.filter(*tpcl);
pass.setInputCloud(tpcl);
pass.setFilterFieldName("y");
pass.setFilterLimits(min_y_, max_y_);
pass.filter(*tpcl);
pass.setInputCloud(tpcl);
pass.setFilterFieldName("z");
pass.setFilterLimits(min_z_, max_z_);
pass.filter(*tpcl);
pcl::ApproximateVoxelGrid<TPoint> psor;
psor.setInputCloud(tpcl);
psor.setDownsampleAllData(false);
psor.setLeafSize(filter_cloud_res_, filter_cloud_res_, filter_cloud_res_);
psor.filter(*tpcl);
pcl::StatisticalOutlierRemoval< TPoint > foutl;
foutl.setInputCloud(tpcl);
foutl.setMeanK(filter_cloud_k_);
foutl.setStddevMulThresh(filter_cloud_th_);
foutl.filter(*tpcl);
pcl_ros::transformPointCloud("odom", *tpcl, *tpcl, listener_);
// get accumulated cloud
TPointCloudPtr pcl_out(new TPointCloud());
for (unsigned int i = 0; i < cloud_buff_->size(); i++)
{
*pcl_out += cloud_buff_->at(i);
}
// registration
if (cloud_buff_->size() > 0)
{
pcl::IterativeClosestPoint< TPoint, TPoint> icp;
icp.setInputCloud(tpcl);
icp.setInputTarget(pcl_out);
pcl::PointCloud<TPoint> aligned;
icp.align(aligned);
if (icp.hasConverged())
{
*tpcl = aligned;
std::cout << "ICP score: " << icp.getFitnessScore() << std::endl;
}
}
if (update) cloud_buff_->push_back(*tpcl);
if (points_pub_.getNumSubscribers() == 0)
{
return;
}
*pcl_out += *tpcl;
pcl::ApproximateVoxelGrid<TPoint> sor;
sor.setInputCloud(pcl_out);
sor.setDownsampleAllData(false);
sor.setLeafSize(filter_cloud_res_, filter_cloud_res_, filter_cloud_res_);
TPointCloudPtr pcl_filt(new TPointCloud());
sor.filter(*pcl_filt);
sensor_msgs::PointCloud2::Ptr cloud_out(new sensor_msgs::PointCloud2());
pcl::toROSMsg(*pcl_filt, *cloud_out);
//std::cout << "points: " << pcl_out->points.size() << std::endl;
cloud_out->header.stamp = cloud->header.stamp;
cloud_out->header.frame_id = fixed_frame_;
points_pub_.publish(cloud_out);
}
