stackDepthPoint.cpp
Go to the documentation of this file.
00001 #include <math.h>
00002 #include <stdio.h>
00003 #include <stdlib.h>
00004 #include <ros/ros.h>
00005 
00006 #include "pointDefinition.h"
00007 
00008 const double PI = 3.1415926;
00009 
00010 const int keyframeNum = 5;
00011 pcl::PointCloud<DepthPoint>::Ptr depthPoints[keyframeNum];
00012 double depthPointsTime[keyframeNum];
00013 int keyframeCount = 0;
00014 int frameCount = 0;
00015 
00016 pcl::PointCloud<DepthPoint>::Ptr depthPointsStacked(new pcl::PointCloud<DepthPoint>());
00017 ros::Publisher *depthPointsPubPointer = NULL;
00018 
00019 double lastPubTime = 0;
00020 
00021 void depthPointsHandler(const sensor_msgs::PointCloud2ConstPtr& depthPoints2)
00022 {
00023   frameCount = (frameCount + 1) % 5;
00024   if (frameCount != 0) {
00025     return;
00026   }
00027 
00028   pcl::PointCloud<DepthPoint>::Ptr depthPointsCur = depthPoints[0];
00029   depthPointsCur->clear();
00030   pcl::fromROSMsg(*depthPoints2, *depthPointsCur);
00031 
00032   for (int i = 0; i < keyframeNum - 1; i++) {
00033     depthPoints[i] = depthPoints[i + 1];
00034     depthPointsTime[i] = depthPointsTime[i + 1];
00035   }
00036   depthPoints[keyframeNum - 1] = depthPointsCur;
00037   depthPointsTime[keyframeNum - 1] = depthPoints2->header.stamp.toSec();
00038 
00039   keyframeCount++;
00040   if (keyframeCount >= keyframeNum && depthPointsTime[0] >= lastPubTime) {
00041     depthPointsStacked->clear();
00042     for (int i = 0; i < keyframeNum; i++) {
00043       *depthPointsStacked += *depthPoints[i];
00044     }
00045 
00046     sensor_msgs::PointCloud2 depthPoints3;
00047     pcl::toROSMsg(*depthPointsStacked, depthPoints3);
00048     depthPoints3.header.frame_id = "camera";
00049     depthPoints3.header.stamp = depthPoints2->header.stamp;
00050     depthPointsPubPointer->publish(depthPoints3);
00051 
00052     lastPubTime = depthPointsTime[keyframeNum - 1];
00053   }
00054 }
00055 
00056 int main(int argc, char** argv)
00057 {
00058   ros::init(argc, argv, "stackDepthPoint");
00059   ros::NodeHandle nh;
00060 
00061   for (int i = 0; i < keyframeNum; i++) {
00062     pcl::PointCloud<DepthPoint>::Ptr depthPointsTemp(new pcl::PointCloud<DepthPoint>());
00063     depthPoints[i] = depthPointsTemp;
00064   }
00065 
00066   ros::Subscriber depthPointsSub = nh.subscribe<sensor_msgs::PointCloud2>
00067                                    ("/depth_points_last", 5, depthPointsHandler);
00068 
00069   ros::Publisher depthPointsPub = nh.advertise<sensor_msgs::PointCloud2> ("/depth_points_stacked", 1);
00070   depthPointsPubPointer = &depthPointsPub;
00071 
00072   ros::spin();
00073 
00074   return 0;
00075 }


demo_lidar
Author(s): Ji Zhang
autogenerated on Sun Oct 5 2014 23:22:43