Go to the documentation of this file.00001
00002 #include <ros/ros.h>
00003 #include <ros/package.h>
00004
00005 #include <XnOpenNI.h>
00006 #include <XnCppWrapper.h>
00007
00008 #include <sensor_msgs/Image.h>
00009 #include <sensor_msgs/image_encodings.h>
00010 #include <sensor_msgs/PointCloud2.h>
00011 #include <pcl_ros/point_cloud.h>
00012 #include <pcl/point_types.h>
00013 #include <pcl/PointIndices.h>
00014
00015 using std::string;
00016 xn::Context g_Context;
00017 xn::SceneAnalyzer g_SceneAnalyzer;
00018
00019
00020 std::vector<ros::Publisher> pub_index;
00021
00022
00023 #define MAX(a,b) ((a)>(b)?(a):(b))
00024 #define AVG(a,b) (((int)(a) + (int)(b)) >> 1)
00025 #define AVG3(a,b,c) (((int)(a) + (int)(b) + (int)(c)) / 3)
00026 #define AVG4(a,b,c,d) (((int)(a) + (int)(b) + (int)(c) + (int)(d)) >> 2)
00027
00028 #define CHECK_RC(nRetVal, what) \
00029 if (nRetVal != XN_STATUS_OK){ \
00030 printf ("%s failed: %s\n", what, xnGetStatusString (nRetVal)); \
00031 return nRetVal; \
00032 }
00033
00034 unsigned int nLabel = 0;
00035 std::vector< std::vector<int > > nIndices;
00036
00037
00038 void createScenePointCloud (const xn::SceneMetaData& smd)
00039 {
00040 const XnLabel* pLabels = smd.Data();
00041 nLabel = 0;
00042 nIndices.clear();
00043 for (unsigned int i = 0; i < smd.XRes()*smd.YRes();i++)
00044 {
00045 nLabel = MAX(nLabel, pLabels[i]);
00046 }
00047 nIndices.resize(nLabel);
00048 ROS_INFO("found %d people", nLabel);
00049
00050 for (unsigned int i = 0; i < smd.XRes()*smd.YRes();i++)
00051 {
00052
00053 if ( pLabels[i] <= 0) continue;
00054 nIndices[pLabels[i]-1].push_back(i);
00055 }
00056 }
00057
00058 int main (int argc, char **argv)
00059 {
00060 ros::init (argc, argv, "openni_scene");
00061 ros::NodeHandle nh("~");
00062
00063
00064
00065
00066 string configFilename = ros::package::getPath ("openni_scene") + "/openni_scene.xml";
00067 XnStatus nRetVal = g_Context.InitFromXmlFile (configFilename.c_str ());
00068 CHECK_RC (nRetVal, "InitFromXml");
00069
00070 nRetVal = g_Context.FindExistingNode (XN_NODE_TYPE_SCENE, g_SceneAnalyzer);
00071 CHECK_RC (nRetVal, "Find scene analyzer");
00072
00073
00074 nRetVal = g_Context.StartGeneratingAll ();
00075 CHECK_RC (nRetVal, "StartGenerating");
00076
00077 while (ros::ok ())
00078 {
00079 g_Context.WaitAndUpdateAll ();
00080 ros::Time time = ros::Time::now();
00081
00082 xn::SceneMetaData sceneMD;
00083 g_SceneAnalyzer.GetMetaData( sceneMD );
00084
00085 createScenePointCloud ( sceneMD );
00086
00087
00088 #define max(a,b) (((a)<(b))?(b):(a))
00089 for (unsigned int i = 0; i < max(nLabel, pub_index.size()); i++) {
00090
00091 if ( i >= nLabel ) {
00092 if ( pub_index[i] != NULL ) {
00093 pub_index[i].shutdown ();
00094 }
00095 continue;
00096 }
00097
00098 pcl::PointIndices msg;
00099 msg.header.frame_id = "openni_depth_frame";
00100 msg.header.stamp = time;
00101 msg.indices = nIndices[i];
00102 std::string topic = "indices" + string(1, '0' + i);
00103
00104 ROS_INFO_STREAM(" index " << i << "/" << nLabel << " " << ((i<nLabel)?nIndices[i].size():-1) << " " << pub_index.size() );
00105 if ( i < pub_index.size() ) {
00106 fprintf(stderr, "%p %d\n", &(pub_index[i]), pub_index[i] == NULL );
00107 }
00108
00109 if ( i < pub_index.size() ) {
00110 if ( pub_index[i] == NULL ) {
00111 if ( nIndices[i].size() > 0 ) {
00112 ROS_INFO_STREAM( " 1 " << i << " " << pub_index.size());
00113 pub_index[i] = nh.advertise<pcl::PointIndices>(topic, 10);
00114 pub_index[i].publish(msg);
00115 } else {
00116 }
00117 } else {
00118 if ( nIndices[i].size() > 0 ) {
00119 ROS_INFO_STREAM( " 3 " << i << " " << pub_index.size());
00120 pub_index[i].publish(msg);
00121 } else {
00122 pub_index[i].shutdown ();
00123 }
00124 }
00125 } else {
00126 pub_index.push_back( nh.advertise<pcl::PointIndices>(topic, 10) );
00127 if ( nIndices[i].size() > 0 ) {
00128 ROS_INFO_STREAM( " 2 " << i << " " << pub_index.size());
00129 pub_index[i].publish(msg);
00130 } else {
00131 pub_index[i].shutdown ();
00132 }
00133 }
00134
00135 }
00136 }
00137 g_Context.Shutdown ();
00138 return 0;
00139 }