HeartBeatMonitor.cpp
Go to the documentation of this file.
00001 // this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
00002 // -- BEGIN LICENSE BLOCK ----------------------------------------------
00003 // This file is part of the SCHUNK Canopen Driver suite.
00004 //
00005 // This program is free software licensed under the LGPL
00006 // (GNU LESSER GENERAL PUBLIC LICENSE Version 3).
00007 // You can find a copy of this license in LICENSE folder in the top
00008 // directory of the source code.
00009 //
00010 // © Copyright 2016 SCHUNK GmbH, Lauffen/Neckar Germany
00011 // © Copyright 2016 FZI Forschungszentrum Informatik, Karlsruhe, Germany
00012 // -- END LICENSE BLOCK ------------------------------------------------
00013 //----------------------------------------------------------------------
00020 //----------------------------------------------------------------------
00021 
00022 #include "HeartBeatMonitor.h"
00023 #include "Logging.h"
00024 
00025 namespace icl_hardware {
00026 namespace canopen_schunk {
00027 
00028 HeartBeatMonitor::HeartBeatMonitor()
00029   : m_period_time_ms(100),
00030   m_running(false)
00031 {
00032   start();
00033 }
00034 
00035 HeartBeatMonitor::~HeartBeatMonitor()
00036 {
00037   stop();
00038   m_thread.join();
00039 }
00040 
00041 void HeartBeatMonitor::registerErrorCallback (const boost::function< void() >& f)
00042 {
00043   m_error_function = f;
00044 }
00045 
00046 
00047 void HeartBeatMonitor::workerFunction()
00048 {
00049   while (true)
00050   {
00051     icl_core::TimeStamp now_time = icl_core::TimeStamp::now();
00052     for (std::map<uint8_t, icl_core::TimeStamp>::iterator it = m_timestamp_record.begin();
00053          it != m_timestamp_record.end();
00054     ++it)
00055     {
00056       icl_core::TimeSpan time_passed = now_time - it->second;
00057       LOGGING_TRACE (CanOpen, "Time passed since last heartbeat from node " <<
00058                              it->first << ": " << time_passed.toMSec() << "ms" << endl);
00059       if (time_passed.toMSec() > m_period_time_ms)
00060       {
00061         LOGGING_ERROR_C (CanOpen, HeartBeatMonitor, "Missing heartbeat from node " <<
00062                          it->first << endl);
00063         m_error_function();
00064         stop();
00065       }
00066     }
00067 
00068     // Wait for the thread period so that the timing is in sync.
00069     try
00070     {
00071         boost::this_thread::sleep(boost::posix_time::milliseconds(m_period_time_ms));
00072     }
00073     catch(boost::thread_interrupted&)
00074     {
00075         return;
00076     }
00077   }
00078 }
00079 
00080 
00081 void HeartBeatMonitor::addHeartbeat (const uint8_t node_id)
00082 {
00083   if (m_running)
00084   {
00085     LOGGING_TRACE_C(CanOpen, HeartBeatMonitor, "New heartbeat received from node " << node_id << endl);
00086 
00087     m_timestamp_record[node_id] = icl_core::TimeStamp::now();
00088   }
00089 }
00090 
00091 void HeartBeatMonitor::stop()
00092 {
00093   m_running = false;
00094   m_thread.interrupt();
00095 }
00096 
00097 void HeartBeatMonitor::start()
00098 {
00099   if (!m_running)
00100   {
00101     m_thread = boost::thread(&HeartBeatMonitor::workerFunction, this);
00102     m_running = true;
00103   }
00104   else
00105   {
00106     LOGGING_WARNING(CanOpen, "start() called although HeartbeatMonitor thread is already running. Start request will be ignored." << endl);
00107   }
00108 }
00109 
00110 void HeartBeatMonitor::reset()
00111 {
00112   // stop monitoring to prevent thread issues
00113   if (m_running)
00114   {
00115     stop();
00116   }
00117 
00118   // set all heartbeat times to zero
00119   for (std::map<uint8_t, icl_core::TimeStamp>::iterator it = m_timestamp_record.begin();
00120        it != m_timestamp_record.end();
00121       ++it)
00122   {
00123       it->second = icl_core::TimeStamp::now();
00124   }
00125 
00126   // restart monitoring
00127   start();
00128 }
00129 
00130 
00131 
00132 
00133 
00134 }} // End of NS


schunk_canopen_driver
Author(s): Felix Mauch , Georg Heppner
autogenerated on Thu Jun 6 2019 20:17:24