player_actarray_log_to_msg.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2009 Radu Bogdan Rusu <rusu -=- cs.tum.edu>
00003  *
00004  * All rights reserved.
00005  *
00006  * Redistribution and use in source and binary forms, with or without
00007  * modification, are permitted provided that the following conditions are met:
00008  *
00009  *     * Redistributions of source code must retain the above copyright
00010  *       notice, this list of conditions and the following disclaimer.
00011  *     * Redistributions in binary form must reproduce the above copyright
00012  *       notice, this list of conditions and the following disclaimer in the
00013  *       documentation and/or other materials provided with the distribution.
00014  *
00015  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00016  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00017  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00018  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00019  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00020  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00021  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00022  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00023  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00024  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00025  * POSSIBILITY OF SUCH DAMAGE.
00026  *
00027  * $Id: player_actarray_log_to_msg.cpp 200 2009-09-21 20:21:21Z dejanpan $
00028  *
00029  */
00030 
00042 // ROS core
00043 #include <ros/ros.h>
00044 // ROS messages
00045 #include <player_log_actarray/PlayerActarray.h>
00046 
00047 #include <fstream>
00048 #include <boost/algorithm/string.hpp>
00049 
00050 using namespace std;
00051 using namespace ros;
00052 using namespace player_log_actarray;
00053 
00054 class PlayerLogToMsg
00055 {
00056 protected:
00057   string tf_frame_;
00058   NodeHandle nh_;
00059 
00060 public:
00061   // ROS messages
00062   PlayerActarray msg_act_;
00063 
00064   string file_name_, msg_topic_;
00065   Publisher act_pub_;
00066 
00067   int joints_to_publish_;
00068   ifstream logfile_stream_;
00069   bool is_file_, spin_, multifile_;
00070   int save_pcd_actarray_;
00071 
00073   PlayerLogToMsg () : tf_frame_ ("laser_tilt_mount_link"),
00074                       is_file_ (true), multifile_(false)
00075   {
00076     nh_.param ("~joints_to_publish", joints_to_publish_, -1);     // publish only the first <joints_to_publish_> joints
00077     msg_topic_ = "/player_actarray";
00078     act_pub_   = nh_.advertise<PlayerActarray> (msg_topic_.c_str (), 1);
00079   }
00080 
00082   virtual ~PlayerLogToMsg () { }
00083 
00085   //initialize args needed for multi log files
00087   void init()
00088   {
00089     save_pcd_actarray_ = 0;
00090     nh_.setParam("/save_pcd_actarray", 0);
00091     spin_ = true;
00092   }
00093   
00094 
00096   // Start
00097   int
00098   start ()
00099   {
00100     // Open file
00101     logfile_stream_.open (file_name_.c_str ());
00102     long int nr_lines = count (istreambuf_iterator<char>(logfile_stream_), istreambuf_iterator<char> (), '\n');
00103 
00104     if (nr_lines == 0)
00105       ROS_WARN ("No lines found in %s", file_name_.c_str ());
00106     logfile_stream_.seekg (0, ios::beg);
00107     ROS_INFO ("Extracting poses from %s ...", file_name_.c_str ());
00108     msg_act_.header.frame_id = tf_frame_;
00109     return (0);
00110   }
00111 
00112   
00114   // Start overridden
00116   int
00117   start2 (string file_name)
00118   {
00119     // Open file
00120     logfile_stream_.open (file_name.c_str ());
00121     long int nr_lines = count (istreambuf_iterator<char>(logfile_stream_), istreambuf_iterator<char> (), '\n');
00122       
00123     if (nr_lines == 0)
00124       ROS_WARN ("No lines found in %s", file_name.c_str ());
00125     logfile_stream_.seekg (0, ios::beg);
00126     ROS_INFO ("Extracting poses from %s ...", file_name.c_str ());
00127     msg_act_.header.frame_id = tf_frame_;
00128     return (0);
00129   }
00130 
00132   // Spin (!)
00133   bool
00134   spin ()
00135   {
00136     double ti, tj = 0, tdif = 0;
00137     int total_nr_poses = 0;
00138     string line;
00139     vector<string> st;
00140 
00141     while (nh_.ok ())
00142       {
00143         getline (logfile_stream_, line);
00144         // Do we assume that the input is a file? If so, and EOF, break
00145         if (logfile_stream_.eof () && is_file_)
00146           break;
00147         // If any bad/eof/fail flags are set, continue
00148         if (!logfile_stream_.good ())
00149           {
00150             usleep (500);
00151             logfile_stream_.clear ();
00152             continue;
00153           }
00154         // If the line is empty, continue
00155         if (line == "")
00156           continue;
00157 
00158         // Split a line into tokens
00159         boost::trim (line);
00160         boost::split (st, line, boost::is_any_of (" "), boost::token_compress_on);
00161 
00162         string line_type = st.at (0);
00163         if (line_type.substr (0, 1) == "#")
00164           continue;
00165 
00166         // Get the interface name
00167         string interface = st.at (3);
00168 
00169         // ---[ actarray
00170         if (interface.substr (0, 8) != "actarray")
00171           continue;
00172 
00173         ti = atof (line_type.c_str ());
00174 
00175         tdif = fabs (ti - tj);                    // Just in case time decides to go backwards :)
00176 
00177         msg_act_.header.stamp = Time (ti);
00178 
00179         // index = st.at (4)
00180         int type = atoi (st.at (5).c_str ());
00181         if (type != 1)                            // we only process DATA packets
00182           continue;
00183 
00184         int nr_joints = atoi (st.at (7).c_str ());
00185         if (joints_to_publish_ != -1)
00186           nr_joints = min (nr_joints, joints_to_publish_);
00187 
00188         msg_act_.joints.resize (nr_joints);
00189         if (msg_act_.joints.size () == 0)         // If no joints found, continue to the next packet
00190           continue;
00191 
00192         for (unsigned int i = 0; i < msg_act_.joints.size (); i++)
00193           msg_act_.joints[i] = atof (st.at (8 + 5 * i).c_str ());
00194         total_nr_poses++;
00195 
00196         if (is_file_)
00197           ROS_DEBUG ("Publishing data (%d joint positions) on topic %s in frame %s.",
00198                      (int)msg_act_.joints.size (), nh_.resolveName (msg_topic_).c_str (), msg_act_.header.frame_id.c_str ());
00199         act_pub_.publish (msg_act_);
00200 
00201         // Sleep for a certain number of seconds (tdif)
00202         if (tj != 0 && is_file_)
00203           {
00204             Duration tictoc (tdif);
00205             tictoc.sleep ();
00206           }
00207 
00208         spinOnce ();
00209         tj = ti;
00210 
00211         logfile_stream_.clear ();
00212       }
00213 
00214     // Close the file and finish the movie
00215     logfile_stream_.close ();
00216     ROS_INFO ("[done : %d poses extracted]", total_nr_poses);
00217 
00218     return (true);
00219   }
00220 
00222   // Spin for multiple files (!)
00224   bool
00225   spin2 ()
00226   {
00227     double ti, tj = 0, tdif = 0;
00228     int total_nr_poses = 0;
00229     string line;
00230     vector<string> st;
00231     ros::Rate loop_rate(0.2);
00232     
00233     while (nh_.ok ())
00234       {
00235         update_parameters_from_server();
00236         if(spin_)
00237           {
00238             getline (logfile_stream_, line);
00239             // Do we assume that the input is a file? If so, and EOF, signal and wait for next file
00240             if (logfile_stream_.eof () && is_file_ && file_name_ != "nothing")
00241               {
00242                 ROS_INFO("EOF actarray!!!");
00243                 logfile_stream_.close();
00244                 spin_ = false;
00245                 continue;
00246               }
00247             //break;
00248             // If any bad/eof/fail flags are set, continue
00249             if (!logfile_stream_.good ())
00250               {
00251                 usleep (500);
00252                 logfile_stream_.clear ();
00253                 continue;
00254               }
00255             // If the line is empty, continue
00256             if (line == "")
00257               continue;
00258       
00259             // Split a line into tokens
00260             boost::trim (line);
00261             boost::split (st, line, boost::is_any_of (" "), boost::token_compress_on);
00262       
00263             string line_type = st.at (0);
00264             if (line_type.substr (0, 1) == "#")
00265               continue;
00266       
00267             // Get the interface name
00268             string interface = st.at (3);
00269       
00270             // ---[ actarray
00271             if (interface.substr (0, 8) != "actarray")
00272               continue;
00273       
00274             ti = atof (line_type.c_str ());
00275       
00276             tdif = fabs (ti - tj);                    // Just in case time decides to go backwards :)
00277       
00278             msg_act_.header.stamp = Time (ti);
00279       
00280             // index = st.at (4)
00281             int type = atoi (st.at (5).c_str ());
00282             if (type != 1)                            // we only process DATA packets
00283               continue;
00284       
00285             int nr_joints = atoi (st.at (7).c_str ());
00286             if (joints_to_publish_ != -1)
00287               nr_joints = min (nr_joints, joints_to_publish_);
00288       
00289             msg_act_.joints.resize (nr_joints);
00290             if (msg_act_.joints.size () == 0)         // If no joints found, continue to the next packet
00291               continue;
00292       
00293             for (unsigned int i = 0; i < msg_act_.joints.size (); i++)
00294               msg_act_.joints[i] = atof (st.at (8 + 5 * i).c_str ());
00295             total_nr_poses++;
00296       
00297             if (is_file_)
00298               ROS_DEBUG ("Publishing data (%d joint positions) on topic %s in frame %s.",
00299                          (int)msg_act_.joints.size (), nh_.resolveName (msg_topic_).c_str (), msg_act_.header.frame_id.c_str ());
00300             act_pub_.publish (msg_act_);
00301       
00302             // Sleep for a certain number of seconds (tdif)
00303             if (tj != 0 && is_file_)
00304               {
00305                 Duration tictoc (tdif);
00306                 tictoc.sleep ();
00307               }
00308             nh_.getParam("/save_pcd_actarray", save_pcd_actarray_);
00309             if(save_pcd_actarray_ == 1 || save_pcd_actarray_ == 2)
00310               nh_.setParam("/save_pcd_actarray", 0);
00311             spinOnce ();
00312             tj = ti;
00313       
00314             logfile_stream_.clear ();
00315           }
00316   
00317         if(!spin_)
00318           {
00319             loop_rate.sleep();
00320             tj = 0, tdif = 0;
00321             total_nr_poses = 0;
00322             line = "";
00323             st.clear();
00324             nh_.getParam("/save_pcd_actarray", save_pcd_actarray_);
00325             if(save_pcd_actarray_ == 0)
00326               nh_.setParam("/save_pcd_actarray", 1);
00327             ROS_WARN("player actarray looping!!!!");
00328           }
00329   
00330       }
00331     // Close the file and finish the movie
00332     if(logfile_stream_.is_open())
00333       logfile_stream_.close ();
00334     ROS_INFO ("[done : %d poses extracted]", total_nr_poses);
00335     
00336     return (true);
00337   }
00338     
00340   // Update parameters from server
00342   void
00343   update_parameters_from_server()
00344   {
00345     string log_filename_tmp = file_name_;
00346     nh_.getParam("/log_filename", file_name_);
00347     if (log_filename_tmp != file_name_ && file_name_ != "nothing")
00348       {
00349         ROS_INFO("Actarray filename set to:  %s", file_name_.c_str());
00350         start2(file_name_);
00351         spin_ = true;
00352       }
00353   }
00354 };
00355 
00356 /* ---[ */
00357 int
00358 main (int argc, char** argv)
00359 {
00360   if (argc < 3)
00361     {
00362       ROS_WARN ("Syntax is: %s <file.log> [bool is_file (0/1)]", argv[0]);
00363       ROS_INFO ("           [note: set 'is_file' to 0 if the input .log file is a stream (i.e., data is still written to it during the execution of %s)]", argv[0]);
00364       ROS_INFO ("Usage example: %s 1190378714.log 0", argv[0]);
00365       return (-1);
00366     }
00367 
00368   init (argc, argv, "player_actarray_log_to_msg");
00369 
00370   PlayerLogToMsg p;
00371   p.file_name_ = string (argv[1]);
00372   p.is_file_   = atoi (argv[2]);
00373 
00374   //do we pass files in directory?
00375   if (atoi(argv[2]) != 2)
00376     {
00377       if (p.start () == -1)
00378         {
00379           ROS_ERROR ("Error processing %s. Exiting.", argv[1]);
00380           return (-1);
00381         }
00382       p.spin ();
00383     }
00384   else
00385     {
00386       p.init();
00387       p.file_name_ = "nothing";
00388       p.spin2();
00389      
00390     }
00391 
00392   return (0);
00393 }
00394 /* ]--- */
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends Defines


player_log_actarray
Author(s): Radu Bogdan Rusu
autogenerated on Thu May 23 2013 18:58:38