00001 #include "ros/ros.h"
00002 #include "sensor_msgs/PointCloud.h"
00003 #include <fstream>
00004 #include <string>
00005 #include <iostream>
00006 using namespace std;
00007
00008 void legCallback(const sensor_msgs::PointCloud::ConstPtr& people_cloud)
00009 {
00010
00011
00012 cout << "Opening file: /home/alex/srs/legdata.txt for writing \n";
00013
00014 std::fstream testleg("/home/alex/srs/legdata.txt",std::ios::out | std::ios::app);
00015 if (testleg.is_open()){
00016 cout << "Writing data from /particle_filt_cloud \n";
00017 testleg<<"seq: ";
00018 testleg<<people_cloud->header.seq;
00019 testleg<<"\n";
00020 testleg<<"frame_id: ";
00021 testleg<<people_cloud->header.frame_id;
00022 testleg<<"\n";
00023 testleg<<"points: "<<"\n";
00024
00025 if ((int)people_cloud->points.size()!=0){
00026 for (int i=0; i<(int)people_cloud->points.size(); ++i){
00027 testleg<<" x: ";testleg<<people_cloud->points[i].x;testleg<<"\n";
00028 testleg<<" y: ";testleg<<people_cloud->points[i].y;testleg<<"\n";
00029 testleg<<" z: ";testleg<<people_cloud->points[i].z;testleg<<"\n";
00030 }
00031 }
00032 cout << "Closing file: \n\n";
00033 testleg.close();
00034 }
00035 }
00036
00037 int main(int argc, char **argv)
00038 {
00049 ros::init(argc, argv, "legListener");
00050
00056 ros::NodeHandle n;
00057
00073 ros::Subscriber subleg = n.subscribe("/particle_filt_cloud", 10, legCallback);
00074
00080 ros::spin();
00081
00082 return 0;
00083 }
00084
