logger.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2010 Oscar Martinez Mozos <omozos@googlemail.com> 
00003  * Dejan Pangercic <pangercic -=- cs.tum.edu> (ROS maintaniner)
00004  *
00005  * All rights reserved.
00006  *
00007  * Redistribution and use in source and binary forms, with or without
00008  * modification, are permitted provided that the following conditions are met:
00009  *
00010  *     * Redistributions of source code must retain the above copyright
00011  *       notice, this list of conditions and the following disclaimer.
00012  *     * Redistributions in binary form must reproduce the above copyright
00013  *       notice, this list of conditions and the following disclaimer in the
00014  *       documentation and/or other materials provided with the distribution.
00015  *
00016  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00017  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00018  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00019  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00020  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00021  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00022  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00023  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00024  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00025  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00026  * POSSIBILITY OF SUCH DAMAGE.
00027  *
00028  *
00029  */
00030 
00042 //standards
00043 #include <iostream>
00044 #include <float.h>
00045 
00046 //ROS
00047 #include <ros/ros.h>
00048 #include <ros/node_handle.h>
00049 #include <sensor_msgs/LaserScan.h>
00050 
00051 using namespace std;
00052 
00053 
00054 class LoggerNode 
00055 {
00056 public:
00057   ros::NodeHandle nh_;
00058   //Subscribers/Publishers
00059   ros::Subscriber scan_;
00060   //Parameters
00061   string filename_;
00062   string input_scan_topic_;
00063   FILE *f_out_;
00064   int scan_id_; // identifier for the scans
00065   
00066 
00070   LoggerNode() : nh_("~")
00071   {
00072     nh_.param("input_scan_topic", input_scan_topic_, std::string("/base_scan_front"));
00073     scan_ = nh_.subscribe<sensor_msgs::LaserScan>(input_scan_topic_, 10, &LoggerNode::scan_cb, this);
00074   }
00075   
00079   ~LoggerNode()
00080   {
00081     fclose( f_out_ );
00082   }
00083 
00087   void init ()
00088   {
00089           scan_id_ = 0;
00090           
00091       f_out_ = fopen(filename_.c_str(), "w");
00092       if ( f_out_ == NULL ) 
00093       {
00094         ROS_ERROR("ERROR opening output file %s", filename_.c_str());
00095       }
00096   }
00097   
00102   void scan_cb(const sensor_msgs::LaserScan::ConstPtr &msg)
00103   {
00104         double value;
00105         double angle; // radians
00106   int num_readings = msg->ranges.size();
00107     
00108         scan_id_ ++;
00109         
00110         fprintf(f_out_, "%d %.8f 1 %d", scan_id_, msg->header.stamp.toSec(), num_readings);
00111         for (int i = 0; i < num_readings; i++)
00112     {
00113       value = msg->ranges[i];
00114       angle = msg->angle_min + i * msg->angle_increment;
00115           
00116       fprintf(f_out_, " %.8f %.8f", angle, value);        
00117     }
00118     fprintf(f_out_, "\n");
00119     ROS_INFO("Scan nr %d saved to disk.", scan_id_);
00120   }
00121   //end LoggerNode
00122 };
00123   
00124 
00125 /* ---[ */
00126 int main (int argc, char** argv)
00127 {
00128                 
00129   if (argc < 2) {
00130         cerr << "USE: " << argv[0] << " <output_file>" << endl;
00131         return 1;
00132   }
00133   ros::init (argc, argv, "logger_node");
00134   LoggerNode lg_node;
00135   lg_node.filename_.assign(argv[1]);
00136   ROS_INFO("filename %s", lg_node.filename_.c_str());
00137   lg_node.init();
00138   ros::spin ();
00139   return (0);
00140 }
00141 /* ]--- */
00142 


people_detector_node
Author(s): Oscar Martinez Mozos, Dejan Pangercic(ROS)
autogenerated on Mon Oct 6 2014 08:55:11