ias_drawer_executive: ias_drawer_executive: Kinect.cpp Source File

00001 
00002 #include <ias_drawer_executive/Kinect.h>
00003 #include <ias_drawer_executive/Geometry.h>
00004 #include <sensor_msgs/PointCloud2.h>
00005 #include <tf/tf.h>
00006 
00007 #include <pcl/ros/conversions.h>
00008 #include <pcl_ros/point_cloud.h>
00009 #include <pcl_ros/transforms.h>
00010 
00011 
00012 Kinect *Kinect::instance_ = 0;
00013 
00014 
00015 void Kinect::getCloud(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud, std::string frame_id, ros::Time after)
00016 {
00017 
00018     sensor_msgs::PointCloud2 pc;// = *(ros::topic::waitForMessage<sensor_msgs::PointCloud2>("/kinect/cloud_throttled"));
00019     bool found = false;
00020     while (!found)
00021     {
00022         //pc  = *(ros::topic::waitForMessage<sensor_msgs::PointCloud2>("/kinect/cloud_throttled"));
00023         pc  = *(ros::topic::waitForMessage<sensor_msgs::PointCloud2>("/camera/rgb/points"));
00024         if ((after == ros::Time(0,0)) || (pc.header.stamp > after))
00025             found = true;
00026         else
00027         {
00028             ROS_ERROR("getKinectCloudXYZ cloud too old : stamp %f , target time %f",pc.header.stamp.toSec(), after.toSec());
00029         }
00030     }
00031     tf::Stamped<tf::Pose> net_stamped = Geometry::getPose(frame_id.c_str(),"/openni_rgb_optical_frame");
00032     tf::Transform net_transform;
00033     net_transform.setOrigin(net_stamped.getOrigin());
00034     net_transform.setRotation(net_stamped.getRotation());
00035 
00036     sensor_msgs::PointCloud2 pct; //in map frame
00037 
00038     pcl_ros::transformPointCloud(frame_id.c_str(),net_transform,pc,pct);
00039     pct.header.frame_id = frame_id.c_str();
00040 
00041     pcl::fromROSMsg(pct, *cloud);
00042 }
00043 
00044 void Kinect::getCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, std::string frame_id, ros::Time after)
00045 {
00046 
00047     sensor_msgs::PointCloud2 pc;
00048     bool found = false;
00049     while (!found)
00050     {
00051         //pc  = *(ros::topic::waitForMessage<sensor_msgs::PointCloud2>("/kinect/cloud_throttled"));
00052         pc  = *(ros::topic::waitForMessage<sensor_msgs::PointCloud2>("/camera/rgb/points"));
00053         if ((after == ros::Time(0,0)) || (pc.header.stamp > after))
00054             found = true;
00055         else
00056         {
00057             ROS_ERROR("getKinectCloudXYZ cloud too old : stamp %f , target time %f",pc.header.stamp.toSec(), after.toSec());
00058         }
00059     }
00060     tf::Stamped<tf::Pose> net_stamped = Geometry::getPose(frame_id.c_str(),"/openni_rgb_optical_frame");
00061     tf::Transform net_transform;
00062     net_transform.setOrigin(net_stamped.getOrigin());
00063     net_transform.setRotation(net_stamped.getRotation());
00064 
00065     sensor_msgs::PointCloud2 pct; //in map frame
00066 
00067     pcl_ros::transformPointCloud(frame_id.c_str(),net_transform,pc,pct);
00068     pct.header.frame_id = frame_id.c_str();
00069 
00070     pcl::fromROSMsg(pct, *cloud);
00071 }