00001 #include <ros/ros.h>
00002 #include <math.h>
00003 #include <sensor_msgs/PointCloud2.h>
00004 #include <pcl/point_types.h>
00005 #include <pcl_ros/point_cloud.h>
00006 #include <pcl/filters/voxel_grid.h>
00007 #include <pointcloud_snapshot_service/PointCloudSnapShot.h>
00008 #include "pcl_ros/transforms.h"
00009 #include <tf/transform_listener.h>
00010
00011 class PclSnapshot{
00012 public:
00013 ros::NodeHandle nh_;
00014
00015 ros::Publisher snapshot_pub;
00016 ros::ServiceServer snapshot_server;
00017 pcl::VoxelGrid<sensor_msgs::PointCloud2> filter;
00018 tf::TransformListener tf_listener_;
00019
00020 PclSnapshot():
00021 nh_("/"), tf_listener_(ros::Duration(20))
00022 {
00023 snapshot_pub=nh_.advertise<sensor_msgs::PointCloud2>("/pointcloud_snapshot_service/pointcloud", 1);
00024 snapshot_server=nh_.advertiseService("pointcloud_snapshot_server/pointcloud_snapshot", &PclSnapshot::take_snapshot, this);
00025 ros::Duration(1.0).sleep();
00026
00027 sensor_msgs::PointCloud2ConstPtr cloud2_ptrtemp = ros::topic::waitForMessage<sensor_msgs::PointCloud2>("/camera/rgb/points");
00028 bool found_transform = tf_listener_.waitForTransform(cloud2_ptrtemp->header.frame_id, "/base_link",
00029 cloud2_ptrtemp->header.stamp, ros::Duration(10.0));
00030
00031 }
00032 bool take_snapshot(pointcloud_snapshot_service::PointCloudSnapShot::Request &req,
00033 pointcloud_snapshot_service::PointCloudSnapShot::Response &res)
00034 {
00035
00036
00037 sensor_msgs::PointCloud2 transformed_cloud;
00038 sensor_msgs::PointCloud2ConstPtr cloud2_ptr= ros::topic::waitForMessage<sensor_msgs::PointCloud2>(req.message_name.c_str(), ros::Duration(10.0));
00039
00040
00041
00042
00043 bool found_transform = tf_listener_.waitForTransform(cloud2_ptr->header.frame_id, "/base_link",
00044 cloud2_ptr->header.stamp, ros::Duration(10.0));
00045
00046 ros::Time n = ros::Time::now();
00047 ROS_INFO("cloud time %d %d , now %d %d", cloud2_ptr->header.stamp.sec, cloud2_ptr->header.stamp.nsec, n.sec, n.nsec);
00048
00049
00050 tf::StampedTransform transform;
00051 if (found_transform)
00052 {
00053 tf_listener_.lookupTransform("/odom_combined",cloud2_ptr->header.frame_id, cloud2_ptr->header.stamp, transform);
00054 pcl_ros::transformPointCloud("/odom_combined", transform, *cloud2_ptr, transformed_cloud);
00055 }
00056 else
00057 {
00058 ROS_INFO("[ExtractClusters:] No transform found between %s and base_link", cloud2_ptr->header.frame_id.c_str());
00059 res.success=false;
00060 return false;
00061 }
00062
00063 sensor_msgs::PointCloud2 cloud_filtered;
00064
00065 filter.setInputCloud(boost::make_shared<sensor_msgs::PointCloud2>(transformed_cloud));
00066 filter.setLeafSize(0.01f, 0.01f, 0.01f);
00067 filter.filter(cloud_filtered);
00068
00069
00070 snapshot_pub.publish(cloud_filtered);
00071 res.success=true;
00072 return true;}
00073
00074 };
00075
00076
00077
00078 int main(int argc, char** argv)
00079 {
00080 ros::init(argc, argv, "pointcloud_snapshot_server");
00081 PclSnapshot snapshot;
00082 ros::Duration(1.0).sleep();
00083 ros::spin();
00084 }
