ncd_parser.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2011, Ivan Dryanovski, William Morris
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the CCNY Robotics Lab nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 #include "ncd_parser/ncd_parser.h"
00031 
00032 int main(int argc, char** argv)
00033 {
00034   if(argc != 4)
00035   {
00036     printf("usage is `rosrun ncd_parser ncd_parser filename.alog`\n");
00037     return 1;
00038   }
00039 
00040   ros::init (argc, argv, "ncd_parser");
00041   NCDParser parser(argv[1]);
00042   sleep(2);
00043   parser.launch();
00044   return 0;
00045 }
00046 
00047 NCDParser::NCDParser(char* filename):filename_(filename)
00048 {
00049   ROS_INFO ("Starting NCDParser");
00050 
00051   ros::NodeHandle nh;
00052   ros::NodeHandle nh_private ("~");
00053 
00054   createOdomToLeftLaserTf();
00055   createOdomToRightLaserTf();
00056   lastTime_ = -1;
00057 
00058   // **** parameters
00059 
00060   if (!nh_private.getParam ("start", start_))
00061     start_ = 0.0;
00062   if (!nh_private.getParam ("end", end_))
00063     end_ = -1;
00064   if (!nh_private.getParam ("rate", rate_))
00065     rate_ = 1.0;
00066 
00067   if (rate_ == 0) ROS_FATAL("rate parameter cannot be 0");
00068 
00069   // **** topics
00070 
00071   leftLaserPublisher_  = nh.advertise<sensor_msgs::LaserScan>("scan",  100);
00072   rightLaserPublisher_ = nh.advertise<sensor_msgs::LaserScan>("scan", 100);
00073 }
00074 
00075 NCDParser::~NCDParser()
00076 {
00077   ROS_INFO ("Shutting down NCDParser");
00078 }
00079 
00080 void NCDParser::launch()
00081 {
00082   std::ifstream aLogFile;
00083   aLogFile.open(filename_);
00084 
00085   if(!aLogFile.is_open())
00086     ROS_FATAL("Could not open %s\n", filename_);
00087 
00088   int lineCounter = 0;
00089   std::string line;
00090 
00091   // **** skip first lines
00092 
00093   for (int i = 0; i < 210; i++)
00094   {
00095     getline(aLogFile, line);
00096     lineCounter++;
00097   }
00098 
00099   // **** iterate over rest of file
00100 
00101   while (getline(aLogFile, line))
00102   {
00103     lineCounter++;
00104     std::vector<std::string> tokens;
00105     tokenize(line, tokens, " ");
00106 
00107     // skip incomplete line
00108     if (tokens.size() < 4) continue;
00109 
00110     // skip log entries before start time
00111     if (strtod(tokens[0].c_str(), NULL) <= start_) continue;
00112 
00113     // stop if time is bigger than end point time
00114     if (strtod(tokens[0].c_str(), NULL) > end_ && end_ != -1)
00115     {
00116       std::cout << strtod(tokens[0].c_str(), NULL) << ", " << end_ << std::endl;
00117       ROS_INFO("Reached specified end time.");
00118       break;
00119     }
00120 
00121     // publish messages
00122     if      (tokens[1].compare("LMS_LASER_2D_LEFT") == 0)
00123       publishLaserMessage(tokens, leftLaserFrame_, leftLaserPublisher_);  
00124     else if (tokens[1].compare("LMS_LASER_2D_RIGHT") == 0)
00125       publishLaserMessage(tokens, rightLaserFrame_, rightLaserPublisher_);
00126     else if (tokens[1].compare("ODOMETRY_POSE") == 0)
00127       publishTfMessages(tokens);
00128     else
00129       continue;
00130 
00131     // wait before publishing next message
00132 
00133     double time = extractValue(tokens[3], "time=");
00134 
00135     if(lastTime_ == -1) lastTime_ = time; 
00136     else
00137     {
00138       ros::Duration d = (ros::Time(time) - ros::Time(lastTime_)) * ( 1.0 / rate_);
00139       lastTime_ = time;
00140       if (d.toNSec() > 0) d.sleep();
00141     }
00142   }
00143 }
00144 
00145 void NCDParser::publishLaserMessage(const std::vector<std::string>& tokens,
00146                                     const std::string& laserFrame,
00147                                     const ros::Publisher& publisher)
00148 {
00149   ROS_DEBUG("Laser message");
00150 
00151   sensor_msgs::LaserScan scan;
00152 
00153   double time = extractValue(tokens[3], "time=");
00154 
00155   scan.header.stamp    = ros::Time(time);
00156   scan.header.frame_id = laserFrame;
00157 
00158   scan.angle_min       = extractValue(tokens[3], "minAngle=") * DEG_TO_RAD; 
00159   scan.angle_max       = extractValue(tokens[3], "maxAngle=") * DEG_TO_RAD; 
00160   scan.angle_increment = extractValue(tokens[3], "angRes=")   * DEG_TO_RAD; 
00161   scan.range_min       = RANGE_MIN;
00162   scan.range_max       = RANGE_MAX;
00163   scan.ranges          = extractArray(tokens[3], "Range=[181]");
00164   scan.intensities     = extractArray(tokens[3], "Reflectance=[181]");
00165 
00166   publisher.publish(scan);
00167 }
00168 
00169 void NCDParser::publishTfMessages(const std::vector<std::string>& tokens)
00170 {
00171   ROS_DEBUG("Tf message");
00172 
00173   // extract time
00174   double time = extractValue(tokens[3], "time=");
00175 
00176   // extract x, y, theta
00177   std::vector<float> xytheta = extractArray(tokens[3], "Pose=[3x1]");
00178   double x     = xytheta[0];
00179   double y     = xytheta[1];
00180   double z     = 0.0;
00181   double theta = xytheta[2];       
00182 
00183   // extract Pitch
00184   double pitch = extractValue(tokens[3], "Pitch=");
00185 
00186   // extract Roll
00187   double roll = extractValue(tokens[3], "Roll=");
00188 
00189   tf::Quaternion rotation;
00190   rotation.setRPY (roll, pitch, theta);
00191   worldToOdom_.setRotation (rotation);
00192 
00193   tf::Vector3 origin;
00194   origin.setValue (x, y, z);
00195   worldToOdom_.setOrigin (origin);
00196 
00197   tf::StampedTransform worldToOdomStamped(worldToOdom_, ros::Time(time), worldFrame_, odomFrame_);
00198   tfBroadcaster_.sendTransform(worldToOdomStamped);
00199 
00200   tf::StampedTransform odomToLeftLaserStamped(odomToLeftLaser_, ros::Time(time), odomFrame_, leftLaserFrame_);
00201   tfBroadcaster_.sendTransform(odomToLeftLaserStamped);
00202 
00203   tf::StampedTransform odomToRightLaserStamped(odomToRightLaser_, ros::Time(time), odomFrame_, rightLaserFrame_);
00204   tfBroadcaster_.sendTransform(odomToRightLaserStamped);
00205 }
00206 
00207 void NCDParser::tokenize (const std::string& str, 
00208                                 std::vector <std::string> &tokens, 
00209                                 std::string sentinel)
00210 {
00211   std::string::size_type lastPos = str.find_first_not_of (sentinel, 0);
00212   std::string::size_type pos = str.find_first_of (sentinel, lastPos);
00213 
00214   while (std::string::npos != pos || std::string::npos != lastPos)
00215   {
00216     std::string stringToken = str.substr (lastPos, pos - lastPos);
00217     tokens.push_back (stringToken);
00218     lastPos = str.find_first_not_of (sentinel, pos);
00219     pos = str.find_first_of (sentinel, lastPos);
00220   }
00221 }
00222 
00223 std::vector<float> NCDParser::extractArray(std::string s, std::string pattern)
00224 {
00225   int n0 = s.find(pattern);
00226   int n1 = s.find("{", n0);
00227   int n2 = s.find("}", n1);
00228   std::string valueList = s.substr(n1+1, n2-n1-1);
00229   
00230   std::vector<float> values;
00231   std::vector<std::string> s_values;
00232   tokenize(valueList, s_values, ",");
00233 
00234   for (unsigned int i = 0; i < s_values.size(); i++)
00235     values.push_back(strtod(s_values[i].c_str(), NULL));
00236 
00237   return values;
00238 }
00239 
00240 double NCDParser::extractValue(std::string s, std::string pattern)
00241 {
00242   int n1 = s.find(pattern);
00243   int n2 = s.find(",", n1);
00244   std::string s_value = s.substr(n1+pattern.length(), n2-n1-pattern.length());
00245   return strtod(s_value.c_str(), NULL);
00246 }
00247 
00248 
00249 void NCDParser::createOdomToLeftLaserTf()
00250 {
00251   double x     = -0.270;
00252   double y     = -0.030;
00253   double z     =  0.495;
00254   double roll  =  180.0 * DEG_TO_RAD;
00255   double pitch =  90.0 * DEG_TO_RAD;
00256   double yaw   =  -90.0 * DEG_TO_RAD;
00257 
00258   tf::Quaternion rotation;
00259   rotation.setRPY (roll, pitch, yaw);
00260   odomToLeftLaser_.setRotation (rotation);
00261 
00262   tf::Vector3 origin;
00263   origin.setValue (x, y, z);
00264   odomToLeftLaser_.setOrigin (origin);
00265 }
00266 
00267 void NCDParser::createOdomToRightLaserTf()
00268 {
00269   double x     =  0.270;
00270   double y     = -0.030;
00271   double z     =  0.495;
00272   double roll  =  90.0 * DEG_TO_RAD;
00273   double pitch =  -90.0 * DEG_TO_RAD;
00274   double yaw   =  180.0 * DEG_TO_RAD;
00275 
00276   tf::Quaternion rotation;
00277   rotation.setRPY (roll, pitch, yaw);
00278   odomToRightLaser_.setRotation (rotation);
00279 
00280   tf::Vector3 origin;
00281   origin.setValue (x, y, z);
00282   odomToRightLaser_.setOrigin (origin);
00283 }


ncd_parser
Author(s): Ivan Dryanovski
autogenerated on Thu Mar 23 2017 03:34:36