Commits

"Nat...@gmail.com>"  committed 5c20283

Added markers and variable plotting

  • Participants
  • Parent commits aab78c9
  • Branches diagnostics2_nate

Comments (0)

Files changed (8)

File gazebo/gui/Diagnostics.cc

       this->labelList->addItem(item);
     }
 
-    QString labelStr(_msg->time(i).name().c_str());
-
     // Check to see if the data belongs in a plot, and add it.
     for (std::vector<IncrementalPlot*>::iterator iter = this->plots.begin();
         iter != this->plots.end(); ++iter)
     {
-      if ((*iter)->HasCurve(labelStr))
+      if ((*iter)->HasCurve(qstr))
       {
         elapsedTime = msgs::Convert(_msg->time(i).elapsed());
 
         double msTime = elapsedTime.Double() * 1e3;
         QPointF pt(wallTime.Double(), msTime);
 
-        (*iter)->Add(labelStr, pt);
+        (*iter)->Add(qstr, pt);
       }
     }
   }
 
   // Process each variable
-  for (int i = 0; i < _msg->var_size(); ++i)
+  for (int i = 0; i < _msg->variable_size(); ++i)
   {
-    QString qstr = QString::fromStdString(_msg->time(i).name());
+    QString qstr = QString::fromStdString(_msg->variable(i).name());
 
     // Add the time label to the list if it's not already there.
     QList<QListWidgetItem*> items = this->labelList->findItems(qstr,
       item->setToolTip(tr("Drag onto graph to plot"));
       this->labelList->addItem(item);
     }
+
+    // Check to see if the data belongs in a plot, and add it.
+    for (std::vector<IncrementalPlot*>::iterator iter = this->plots.begin();
+        iter != this->plots.end(); ++iter)
+    {
+      if ((*iter)->HasCurve(qstr))
+      {
+        QPointF pt(wallTime.Double(), _msg->variable(i).value());
+        (*iter)->Add(qstr, pt);
+      }
+    }
+  }
+
+  // Process each marker
+  for (int i = 0; i < _msg->marker_size(); ++i)
+  {
+    QString qstr = QString::fromStdString(_msg->marker(i).name());
+
+    // Add the time label to the list if it's not already there.
+    QList<QListWidgetItem*> items = this->labelList->findItems(qstr,
+        Qt::MatchExactly);
+
+    if (items.size() == 0)
+    {
+      QListWidgetItem *item = new QListWidgetItem(qstr);
+      item->setToolTip(tr("Drag onto graph to plot"));
+      this->labelList->addItem(item);
+    }
+
+    // Add all marker to all plots
+    for (std::vector<IncrementalPlot*>::iterator iter = this->plots.begin();
+        iter != this->plots.end(); ++iter)
+    {
+      (*iter)->AddVLine(qstr, wallTime.Double());
+    }
   }
 }
 

File gazebo/gui/IncrementalPlot.cc

 #include <qwt/qwt_plot_grid.h>
 #include <qwt/qwt_plot_canvas.h>
 #include <qwt/qwt_plot_curve.h>
+#include <qwt/qwt_plot_marker.h>
 #include <qwt/qwt_curve_fitter.h>
 #include <qwt/qwt_symbol.h>
 #include <qwt/qwt_legend.h>
 }
 
 /////////////////////////////////////////////////
+void IncrementalPlot::AddVLine(const QString &_label, double _x)
+{
+  QwtPlotMarker *marker = new QwtPlotMarker();
+  marker->setValue(_x, 0.0);
+  marker->setLineStyle(QwtPlotMarker::VLine);
+  marker->setLabelAlignment(Qt::AlignRight | Qt::AlignBottom);
+  marker->setLinePen(QPen(Qt::green, 0, Qt::DashDotLine));
+  marker->attach(this);
+  marker->setLabel(_label);
+}
+
+/////////////////////////////////////////////////
 void IncrementalPlot::AdjustCurve(QwtPlotCurve *_curve)
 {
   GZ_ASSERT(_curve != NULL, "Curve is NULL");

File gazebo/gui/IncrementalPlot.hh

       public slots: void Add(const QString &_label,
                              const std::list<QPointF> &_pts);
 
+      /// \brief Add a vertical line to the plot.
+      /// \param[in] _label Label for the line.
+      /// \param[in] _x X position for the vertical line.
+      public: void AddVLine(const QString &_label, double _x);
+
       /// \brief Clear a single curve from the plot.
       /// \param[in] _label Name of the curve to remove.
       public: void Clear(const QString &_label);

File gazebo/msgs/diagnostics.proto

     required double value = 3;
   }
 
+  message DiagMarker
+  {
+    required string name = 1;
+    required Time wall = 2;
+  }
+
   repeated DiagTime time = 1;
-  repeated DiagVariable time = 2;
-  required Time real_time = 3;
-  required Time sim_time = 4;
-  required double real_time_factor = 5;
+  repeated DiagVariable variable = 2;
+  repeated DiagMarker marker = 3;
+  required Time real_time = 4;
+  required Time sim_time = 5;
+  required double real_time_factor = 6;
 }

File gazebo/physics/World.cc

     }
     DIAG_TIMER_LAP("World::Update", "dirtyPoses");
 
-    DIAG_VARIABLE("dirty_poses", this->dirtyPoses.size());
     this->dirtyPoses.clear();
   }
 
   if (this->pause == _p)
     return;
 
+  DIAG_MARKER("paused");
+
   {
     boost::recursive_mutex::scoped_lock(*this->worldUpdateMutex);
     this->pause = _p;

File gazebo/physics/ode/ODEPhysics.cc

   dSpaceCollide(this->spaceId, this, CollisionCallback);
   DIAG_TIMER_LAP("ODEPhysics::UpdateCollision", "dSpaceCollide");
 
+  DIAG_VARIABLE("colliders", this->collidersCount);
+  DIAG_VARIABLE("trimesh colliders", this->trimeshCollidersCount);
+
   // Generate non-trimesh collisions.
   for (i = 0; i < this->collidersCount; ++i)
   {

File gazebo/util/Diagnostics.cc

   this->enabled = false;
 
   this->node = transport::NodePtr(new transport::Node());
+
   // Get the base of the time logging path
   if (!getenv("HOME"))
   {
 {
   event::Events::DisconnectWorldUpdateBegin(this->updateConnection);
 
+  this->varLog.close();
+
   // Stop all the timers
   for (TimerMap::iterator iter = this->timers.begin();
        iter != this->timers.end(); ++iter)
     this->pub->Publish(this->msg);
 
   this->msg.clear_time();
+  this->msg.clear_variable();
+  this->msg.clear_marker();
 }
 
 //////////////////////////////////////////////////
     if (!boost::filesystem::exists(this->logPath))
       boost::filesystem::create_directories(this->logPath);
 
+    boost::filesystem::path varLogPath;
+
+    varLogPath = this->logPath / "variables.log";
+    this->varLog.open(varLogPath.string().c_str(),
+        std::ios::out | std::ios::app);
+
     this->updateConnection = event::Events::ConnectWorldUpdateBegin(
         boost::bind(&DiagnosticManager::Update, this, _1));
 
     _diagStopPtr = &_DiagnosticManager_Stop;
     _diagLapPtr = &_DiagnosticManager_Lap;
     _diagVariablePtr = &_DiagnosticManager_Variable;
+    _diagMarkerPtr = &_DiagnosticManager_Marker;
   }
   else
   {
     _diagStopPtr = &_DiagnosticManager_Noop1;
     _diagLapPtr = &_DiagnosticManager_Noop2;
     _diagVariablePtr = &_DiagnosticManager_Noop3;
+    _diagMarkerPtr = &_DiagnosticManager_Noop1;
   }
 }
 
 }
 
 //////////////////////////////////////////////////
+void DiagnosticManager::Variable(const std::string &_name, double _value)
+{
+  // Get the current elapsed time.
+  common::Time currTime = common::Time::GetWallTime();
+
+  // Write out the delta time
+  this->varLog << _name << " " << currTime << " " << _value << std::endl;
+
+  msgs::Diagnostics::DiagVariable *var = this->msg.add_variable();
+  var->set_name(_name);
+  var->set_value(_value);
+  msgs::Set(var->mutable_wall(), currTime);
+}
+
+//////////////////////////////////////////////////
+void DiagnosticManager::Marker(const std::string &_name)
+{
+  // Get the current elapsed time.
+  common::Time currTime = common::Time::GetWallTime();
+
+  // Write out the delta time
+  this->varLog << _name << " " << currTime << std::endl;
+
+  msgs::Diagnostics::DiagMarker *marker = this->msg.add_marker();
+  marker->set_name(_name);
+  msgs::Set(marker->mutable_wall(), currTime);
+}
+
+//////////////////////////////////////////////////
 DiagnosticTimer::DiagnosticTimer(const std::string &_name) : Timer()
 {
   boost::filesystem::path logPath;
   this->prevLap = elapsed;
 }
 
-//////////////////////////////////////////////////
-void DiagnosticTimer::Variable(const std::string &_prefix, double &_value)
-{
-  // Get the current elapsed time.
-  common::Time currTime = common::Time::GetWallTime();
 
-  // Write out the delta time.
-  this->log << _prefix << " " << currTime << " " << _value << std::endl;
-
-  msgs::Diagnostics::DiagVariable *var = this->msg.add_variable();
-  var->set_name(_prefix);
-  var->set_value(_value);
-  msgs::Set(var->mutable_wall(), _wallTime);
-}

File gazebo/util/Diagnostics.hh

 
     /// \brief Add an a arbitrary variable to diagnostics.
     /// \param[in] _name Name associated with the variable.
-    /// \param[in] _value Value of the variable.
+    /// \param[in] _value Value of the variable. Value must be a double.
     #define DIAG_VARIABLE(_name, _value) \
     (*gazebo::util::_diagVariablePtr)(_name, _value);
 
+    /// \brief Add a marker at the current time.
+    /// \param[in] _name Name of the marker
+    #define DIAG_MARKER(_name) \
+    (*gazebo::util::_diagMarkerPtr)(_name);
+
+
     /// \class DiagnosticManager Diagnostics.hh util/util.hh
     /// \brief A diagnostic manager class
     class DiagnosticManager : public SingletonT<DiagnosticManager>
       /// elapsed time.
       public: void Lap(const std::string &_name, const std::string &_prefix);
 
+      /// \brief Add an an arbitrary variable to diagnostics.
+      /// \param[in] _name Name associated with the variable.
+      /// \param[in] _value Value of the variable.
+      public: void Variable(const std::string &_name, double _value);
+
+      /// \brief Add an a marker to diagnostics.
+      /// \param[in] _name Name of the marker.
+      public: void Marker(const std::string &_name);
+
       /// \brief Get the number of timers
       /// \return The number of timers
       public: int GetTimerCount() const;
 
       /// \brief Give DiagnosticTimer special rights.
       private: friend class DiagnosticTimer;
+
+      /// \brief Log file for variables.
+      private: std::ofstream varLog;
     };
 
     /// \class DiagnosticTimer Diagnostics.hh util/util.hh
 
     /// \brief A no-op function used by Diagnostics when it is disabled.
     static inline void _DiagnosticManager_Noop3(const std::string &/*_name*/,
-        const boost::any &/*_value*/) {}
+        double /*_value*/) {}
 
 
     /// \brief Function used to start a timer.
     /// \param[in] _name Name associated with the variable.
     /// \param[in] _value A value to add
     static inline void _DiagnosticManager_Variable(const std::string &_name,
-                                                   const boost::any &_value)
+                                                   double _value)
     {
-      try
-      {
-      gazebo::util::DiagnosticManager::Instance()->Variable(_name,
-          boost::any_cast<double>(_value));
-      }
-      catch (...)
-        gzerr << "Unable to convert boost::any value to double for diagnostic "
-        << "variable[" << _name << "]\n";
-      }
+      gazebo::util::DiagnosticManager::Instance()->Variable(_name, _value);
+    }
+
+    /// \brief Function used to add a marker to diagnostics.
+    /// \param[in] _name Name of the marker.
+    static inline void _DiagnosticManager_Marker(const std::string &_name)
+    {
+      gazebo::util::DiagnosticManager::Instance()->Marker(_name);
     }
 
     /// \brief Function pointer to start a timer.
 
     /// \brief Function pointer to stop a timer.
     void (*_diagStopPtr)(const std::string &_name) = &_DiagnosticManager_Noop1;
-    /// \endcond
 
     /// \brief Function pointer a variable diagnostic information.
-    void (*_diagVariablePtr)(const std::string &_name, const boost::any &_var) =
+    void (*_diagVariablePtr)(const std::string &_name, double _var) =
       &_DiagnosticManager_Noop3;
+
+    /// \brief Function pointer for a marker.
+    void (*_diagMarkerPtr)(const std::string &_name) =
+      &_DiagnosticManager_Noop1;
+
     /// \endcond
 
   }