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); }