twist_mux_diagnostics.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (CC BY-NC-SA 4.0 License)
00003  *
00004  *  Copyright (c) 2014, PAL Robotics, S.L.
00005  *  All rights reserved.
00006  *
00007  *  This work is licensed under the Creative Commons
00008  *  Attribution-NonCommercial-ShareAlike 4.0 International License.
00009  *
00010  *  To view a copy of this license, visit
00011  *  http://creativecommons.org/licenses/by-nc-sa/4.0/
00012  *  or send a letter to
00013  *  Creative Commons, 444 Castro Street, Suite 900,
00014  *  Mountain View, California, 94041, USA.
00015  *********************************************************************/
00016 
00017 /*
00018  * @author Enrique Fernandez
00019  */
00020 
00021 #include <twist_mux/twist_mux_diagnostics.h>
00022 #include <twist_mux/twist_mux_diagnostics_status.h>
00023 
00024 #include <diagnostic_updater/diagnostic_updater.h>
00025 
00026 namespace twist_mux
00027 {
00028 
00029 TwistMuxDiagnostics::TwistMuxDiagnostics()
00030 {
00031   diagnostic_.add("Twist mux status", this, &TwistMuxDiagnostics::diagnostics);
00032   diagnostic_.setHardwareID("none");
00033 }
00034 
00035 TwistMuxDiagnostics::~TwistMuxDiagnostics()
00036 {}
00037 
00038 void TwistMuxDiagnostics::update()
00039 {
00040   diagnostic_.update();
00041 }
00042 
00043 void TwistMuxDiagnostics::updateStatus(const status_type::ConstPtr& status)
00044 {
00045   ROS_DEBUG_THROTTLE(1.0, "Updating status.");
00046 
00047   status_.velocity_hs = status->velocity_hs;
00048   status_.lock_hs     = status->lock_hs;
00049   status_.priority    = status->priority;
00050 
00051   status_.main_loop_time = status->main_loop_time;
00052   status_.reading_age    = status->reading_age;
00053 
00054   update();
00055 }
00056 
00057 void TwistMuxDiagnostics::diagnostics(diagnostic_updater::DiagnosticStatusWrapper& stat)
00058 {
00060   if (status_.main_loop_time > MAIN_LOOP_TIME_MIN)
00061     stat.summary(ERROR, "loop time too long");
00062   else if (status_.reading_age > READING_AGE_MIN)
00063     stat.summary(ERROR, "data received is too old");
00064   else
00065     stat.summary(OK, "ok");
00066 
00067   for (const auto& velocity_h : *status_.velocity_hs)
00068   {
00069     stat.addf("velocity " + velocity_h.getName(),
00070               " %s (listening to %s @ %fs with priority #%d)",
00071               (velocity_h.isMasked(status_.priority) ? "masked" : "unmasked"),
00072               velocity_h.getTopic().c_str(),
00073               velocity_h.getTimeout(),
00074               static_cast<int>(velocity_h.getPriority()));
00075   }
00076 
00077   for (const auto& lock_h : *status_.lock_hs)
00078   {
00079     stat.addf("lock " + lock_h.getName(),
00080               " %s (listening to %s @ %fs with priority #%d)",
00081               (lock_h.isLocked() ? "locked" : "free"),
00082               lock_h.getTopic().c_str(),
00083               lock_h.getTimeout(),
00084               static_cast<int>(lock_h.getPriority()));
00085   }
00086 
00087   stat.add("current priority", static_cast<int>(status_.priority));
00088 
00089   stat.add("loop time in [sec]", status_.main_loop_time);
00090   stat.add("data age in [sec]", status_.reading_age);
00091 
00092   ROS_DEBUG_THROTTLE(1.0, "Publishing diagnostics.");
00093 }
00094 
00095 } // namespace twist_mux


twist_mux
Author(s): Enrique Fernandez , Siegfried-A. Gevatter Pujals
autogenerated on Fri Aug 28 2015 13:25:02