HardwareCanSinkCanfile.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 //----------------------------------------------------------------------
00024 //----------------------------------------------------------------------
00025 #include "HardwareCanSinkCanfile.h"
00026 
00027 #include <iomanip>
00028 #include <boost/date_time.hpp>
00029 
00030 #include <icl_sourcesink/SimpleURI.h>
00031 #include <icl_hardware_can/Logging.h>
00032 #include <icl_hardware_can/tException.h>
00033 
00034 namespace icl_hardware {
00035 namespace can {
00036 
00037 HardwareCanSinkCanfile::HardwareCanSinkCanfile(const std::string& uri, const std::string& name)
00038   : HardwareCanSink(uri, name),
00039     m_data_file(),
00040     m_msg_container(),
00041     m_msg_container_size(500),
00042     m_commit_timediff(boost::posix_time::seconds(1)),
00043     m_last_msg_time(),
00044     m_last_commit_time()
00045 {
00046   if (!prepareFile())
00047   {
00048     LOGGING_WARNING(CAN, "Failed to open URI " << uri << " for recording!" << endl);
00049     return;
00050   }
00051 
00052   m_msg_container.reserve(m_msg_container_size);
00053 }
00054 
00055 HardwareCanSinkCanfile::~HardwareCanSinkCanfile()
00056 { }
00057 
00058 bool HardwareCanSinkCanfile::prepareFile()
00059 {
00060   icl_sourcesink::SimpleURI parsed_uri(uri());
00061 
00062   // Extract message container size.
00063   boost::optional<std::size_t> msg_container_size = parsed_uri.getQuery<std::size_t>("container_size");
00064   if (msg_container_size)
00065   {
00066     if (msg_container_size > 0)
00067     {
00068       m_msg_container_size = *msg_container_size;
00069     }
00070     else
00071     {
00072       LOGGING_ERROR(CAN, "container_size must be > 0. Using default value " << m_msg_container_size << "." << endl);
00073     }
00074   }
00075 
00076   // Ensure the filename ends in ".can".
00077   std::string can_filename = parsed_uri.path();
00078   if (can_filename.size() < 4 || can_filename.substr(can_filename.size()-4) != ".can")
00079   {
00080     can_filename += ".can";
00081   }
00082 
00083   m_data_file.open(can_filename.c_str());
00084   if (!m_data_file)
00085   {
00086     LOGGING_ERROR(CAN, "Failed to open data file '" << m_data_file << "'." << endl);
00087     return false;
00088   }
00089 
00090   // Write file header.
00091   m_data_file << "# FZI-CAN Stream version " << cMAJOR_FILEVERSION << "." << cMINOR_FILEVERSION << std::endl;
00092 
00093   return true;
00094 }
00095 
00096 void HardwareCanSinkCanfile::set(const CanMessageStamped::Ptr& msg)
00097 {
00098   if (!msg || ((*msg)->id == 0 && (*msg)->dlc == 0 && (*msg)->rtr == 0))
00099   {
00100     throw(tException(eMESSAGE_ERROR, "Received empty message"));
00101   }
00102 
00103   if (!m_last_msg_time.is_special() && msg->header().timestamp < m_last_msg_time)
00104   {
00105     LOGGING_WARNING(CAN, "CAN message sequence out of order! (" << msg->header().timestamp << " < " << m_last_msg_time << ")" << endl);
00106   }
00107   m_msg_container.push_back(msg);
00108 
00109   if (m_msg_container.size() >= m_msg_container_size
00110       || m_last_commit_time + m_commit_timediff > msg->header().timestamp)
00111   {
00112     if (m_data_file.bad())
00113     {
00114       throw tException(eFILE_ERROR, "Data file stream went bad.");
00115     }
00116 
00117     // Write content to file.
00118     m_data_file << std::setfill('0');
00119     for (std::vector<CanMessageStamped::ConstPtr>::const_iterator iter = m_msg_container.begin();
00120          iter != m_msg_container.end(); ++iter)
00121     {
00122       // Build unix timestamp.
00123       boost::posix_time::time_duration time_since_epoch
00124         = (*iter)->header().timestamp - boost::posix_time::from_time_t(0);
00125       uint64_t seconds = time_since_epoch.ticks() / boost::posix_time::time_duration::ticks_per_second();
00126       long microseconds = time_since_epoch.fractional_seconds() / (boost::posix_time::time_duration::ticks_per_second() / 1000000);
00127       m_data_file << std::dec << seconds << "." << std::setw(6) << microseconds
00128                   << " " << (**iter)->id << " " << unsigned((**iter)->dlc)
00129                   << std::hex
00130                   << " " << std::setw(2) << unsigned((**iter)->data[0])
00131                   << " " << std::setw(2) << unsigned((**iter)->data[1])
00132                   << " " << std::setw(2) << unsigned((**iter)->data[2])
00133                   << " " << std::setw(2) << unsigned((**iter)->data[3])
00134                   << " " << std::setw(2) << unsigned((**iter)->data[4])
00135                   << " " << std::setw(2) << unsigned((**iter)->data[5])
00136                   << " " << std::setw(2) << unsigned((**iter)->data[6])
00137                   << " " << std::setw(2) << unsigned((**iter)->data[7])
00138                   << "\n";
00139 
00140       if (m_data_file.bad())
00141       {
00142         throw tException(eFILE_ERROR, "Stream went bad while writing to file.");
00143       }
00144     }
00145 
00146     m_last_commit_time = msg->header().timestamp;
00147     m_msg_container.clear();
00148   }
00149   m_last_msg_time = msg->header().timestamp;
00150 }
00151 
00152 }
00153 }


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