legListener.cpp

Go to the documentation of this file.

#include "ros/ros.h"
#include "sensor_msgs/PointCloud.h"
#include <fstream>
#include <string>
#include <iostream>
using namespace std;

void legCallback(const sensor_msgs::PointCloud::ConstPtr& people_cloud)
{


        cout << "Opening  file: /home/alex/srs/legdata.txt for writing \n";

        std::fstream testleg("/home/alex/srs/legdata.txt",std::ios::out | std::ios::app);
        if (testleg.is_open()){
        cout << "Writing data from /particle_filt_cloud \n";
        testleg<<"seq: ";
        testleg<<people_cloud->header.seq;
        testleg<<"\n";
        testleg<<"frame_id: ";
        testleg<<people_cloud->header.frame_id;
        testleg<<"\n";
        testleg<<"points: "<<"\n";

        if ((int)people_cloud->points.size()!=0){
                for (int  i=0; i<(int)people_cloud->points.size(); ++i){
                        testleg<<" x: ";testleg<<people_cloud->points[i].x;testleg<<"\n";
                        testleg<<" y: ";testleg<<people_cloud->points[i].y;testleg<<"\n";
                        testleg<<" z: ";testleg<<people_cloud->points[i].z;testleg<<"\n";
                }
        }
        cout << "Closing file: \n\n";
        testleg.close();
        }
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "legListener");

  ros::NodeHandle n;

  ros::Subscriber subleg = n.subscribe("/particle_filt_cloud", 10, legCallback);

  ros::spin();

  return 0;
}