CanFileResolutionHelper.cpp
Go to the documentation of this file.
00001 // this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
00002 
00003 // -- BEGIN LICENSE BLOCK ----------------------------------------------
00004 // This file is part of FZIs ic_workspace.
00005 //
00006 // This program is free software licensed under the LGPL
00007 // (GNU LESSER GENERAL PUBLIC LICENSE Version 3).
00008 // You can find a copy of this license in LICENSE folder in the top
00009 // directory of the source code.
00010 //
00011 // © Copyright 2016 FZI Forschungszentrum Informatik, Karlsruhe, Germany
00012 //
00013 // -- END LICENSE BLOCK ------------------------------------------------
00014 
00015 //----------------------------------------------------------------------
00022 //----------------------------------------------------------------------
00023 #include "CanFileResolutionHelper.h"
00024 
00025 #include <icl_core/BaseTypes.h>
00026 #include <icl_math/MathFunctions.h>
00027 
00028 namespace icl_hardware {
00029 namespace can {
00030 
00031 CanFileResolutionHelper::CanFileResolutionHelper(const std::string& filename)
00032   : m_filename(filename),
00033     m_file(m_filename.c_str())
00034 {
00035   m_file.imbue(std::locale("C"));
00036 
00037   // Find the first valid line.
00038   std::string line;
00039   std::ifstream::pos_type current_pos = m_file.tellg();
00040   while (m_file && (line.empty() || line[0] == '#'))
00041   {
00042     current_pos = m_file.tellg();
00043     std::getline(m_file, line);
00044   }
00045   if (tryParse(line, current_pos))
00046   {
00047     m_file.seekg(current_pos);
00048     m_current_position.dsin = icl_sourcesink::DSIN(current_pos);
00049   }
00050   else
00051   {
00052     // File is broken somehow, or end of file was reached.  Make sure
00053     // we go to the end in any case, so that subsequent good() calls
00054     // return false as well.
00055     m_file.seekg(0, std::ios_base::end);
00056   }
00057 }
00058 
00059 boost::posix_time::ptime CanFileResolutionHelper::timestampToPtime(double stamp)
00060 {
00061   unsigned long stamp_usec(icl_math::round((stamp - icl_math::floor(stamp)) * 1e6));
00062   return boost::posix_time::ptime(boost::gregorian::date(1970,1,1),
00063                                   boost::posix_time::seconds(static_cast<unsigned long>(stamp))
00064                                   + boost::posix_time::microsec(stamp_usec));
00065 }
00066 
00067 bool CanFileResolutionHelper::advance()
00068 {
00069   // We're on a valid line to begin with.  Skip beyond that, then find
00070   // the next valid line.
00071   std::string line;
00072   std::getline(m_file, line);
00073   line = "";
00074   std::ifstream::pos_type current_pos = m_file.tellg();
00075   while (m_file && (line.empty() || line[0] == '#'))
00076   {
00077     current_pos = m_file.tellg();
00078     std::getline(m_file, line);
00079   }
00080   if (tryParse(line, current_pos))
00081   {
00082     m_file.seekg(current_pos);
00083     return true;
00084   }
00085   else
00086   {
00087     // File is broken somehow, or end of file was reached.  Make sure
00088     // we go to the end in any case, so that subsequent good() calls
00089     // return false as well.
00090     m_file.seekg(0, std::ios_base::end);
00091     return false;
00092   }
00093 }
00094 
00095 bool CanFileResolutionHelper::tryParse(const std::string& line, uint64_t current_pos)
00096 {
00097   // FZI-CAN FILE V1.0
00098   m_current_position.index = icl_sourcesink::Index(icl_sourcesink::Position::INVALID_INDEX);
00099   m_current_position.timestamp = boost::posix_time::not_a_date_time;
00100   m_current_position.dsin = icl_sourcesink::DSIN(icl_sourcesink::Position::INVALID_DSIN);
00101   std::istringstream ss(line);
00102   ss.imbue(std::locale("C"));
00103   double gmtime;
00104   unsigned id, dlc, msg0, msg1, msg2, msg3, msg4, msg5, msg6, msg7;
00105   try
00106   {
00107     if (ss >> std::dec >> gmtime >> id >> dlc
00108         >> std::hex >> msg0 >> msg1 >> msg2 >> msg3 >> msg4 >> msg5 >> msg6 >> msg7)
00109     {
00110       m_current_position.index = icl_sourcesink::Index(icl_sourcesink::Position::INVALID_INDEX);
00111       m_current_position.timestamp = timestampToPtime(gmtime);
00112       m_current_position.dsin = icl_sourcesink::DSIN(current_pos);
00113       m_current_message.reset(
00114         new CanMessageStamped(
00115           tCanMessage(id, dlc),
00116           icl_core::DataHeader("", m_current_position.timestamp)));
00117       m_current_message->header().dsin = m_current_position.dsin;
00118       (*m_current_message)->data[0] = msg0;
00119       (*m_current_message)->data[1] = msg1;
00120       (*m_current_message)->data[2] = msg2;
00121       (*m_current_message)->data[3] = msg3;
00122       (*m_current_message)->data[4] = msg4;
00123       (*m_current_message)->data[5] = msg5;
00124       (*m_current_message)->data[6] = msg6;
00125       (*m_current_message)->data[7] = msg7;
00126       return true;
00127     }
00128   }
00129   catch (...)
00130   {
00131     return false;
00132   }
00133   return bool(ss);
00134 }
00135 
00136 bool CanFileResolutionHelper::resolveHere()
00137 {
00138   std::ifstream::pos_type current_pos = m_file.tellg();
00139   std::string line;
00140   std::getline(m_file, line);
00141   if (tryParse(line, current_pos))
00142   {
00143     m_file.seekg(current_pos);
00144     m_current_position.dsin = icl_sourcesink::DSIN(current_pos);
00145     return true;
00146   }
00147   else
00148   {
00149     // File is broken somehow, or end of file was reached.  Make sure
00150     // we go to the end in any case, so that subsequent good() calls
00151     // return false as well.
00152     m_file.seekg(0, std::ios_base::end);
00153     return false;
00154   }
00155 }
00156 
00157 }
00158 }


fzi_icl_can
Author(s):
autogenerated on Tue Aug 8 2017 03:07:51