00001 #include <iostream> 00002 #include <pcl/io/pcd_io.h> 00003 #include <pcl/point_types.h> 00004 #include <pcl/registration/icp.h> 00005 00006 #include <ros/ros.h> 00007 #include <pcl_ros/point_cloud.h> 00008 #include <pcl/point_types.h> 00009 00010 #include <ros/ros.h> 00011 #include <laser_assembler/AssembleScans.h> 00012 00013 00014 typedef pcl::PointCloud<pcl::PointXYZ> PointCloud; 00015 using namespace laser_assembler; 00016 00017 ros::ServiceClient client; 00018 00019 PointCloud::Ptr getLaserCloud() { 00020 PointCloud::Ptr cloud (new PointCloud); 00021 AssembleScans srv; 00022 srv.request.begin = ros::Time(0,0); 00023 srv.request.end = ros::Time::now(); 00024 if (client.call(srv)) { 00025 for(int i=0; i<srv.response.cloud.points.size(); i++) { 00026 pcl::PointXYZ p = pcl::PointXYZ(); 00027 p.x = srv.response.cloud.points[i].x; 00028 p.y = srv.response.cloud.points[i].y; 00029 p.z = srv.response.cloud.points[i].z; 00030 00031 cloud->push_back(p); 00032 } 00033 } else ROS_ERROR("Service call failed\n"); 00034 return cloud; 00035 } 00036 00037 void cloudCallback(const PointCloud::ConstPtr& cloud1) { 00038 PointCloud::Ptr cloud2 = getLaserCloud(); 00039 pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp; 00040 icp.setInputCloud(cloud1); 00041 icp.setInputTarget(cloud2); 00042 PointCloud Final; 00043 icp.align(Final); 00044 std::cout << "has converged:" << icp.hasConverged() << " score: " << 00045 icp.getFitnessScore() << std::endl; 00046 std::cout << icp.getFinalTransformation() << std::endl; 00047 } 00048 00049 int main (int argc, char** argv) { 00050 ros::init(argc, argv, "find_transform"); 00051 ros::NodeHandle nh; 00052 ROS_INFO("Waiting for assembled tilt scans"); 00053 ros::service::waitForService("assemble_scans"); 00054 client = nh.serviceClient<AssembleScans>("assemble_scans"); 00055 ros::Subscriber cloud_sub = nh.subscribe<PointCloud>("cloud", 1, cloudCallback); 00056 00057 00058 ros::spin(); 00059 return 0; 00060 }