Anonymous avatar Anonymous committed 865cf09 Merge

Merged from gazebo_1.5

Comments (0)

Files changed (73)

 
 set (GAZEBO_VERSION_NAME "lithium")
 set (GAZEBO_MAJOR_VERSION 1)
-set (GAZEBO_MINOR_VERSION 4)
+set (GAZEBO_MINOR_VERSION 5)
 # The patch version may have been bumped for prerelease purposes; be sure to
 # check gazebo-release/ubuntu/debian/changelog@default to determine what the
 # next patch version should be for a regular release.
 set (CMAKE_CXX_FLAGS_CHECK "${CMAKE_C_FLAGS_CHECK} -fno-elide-constructors -fno-default-inline -fno-implicit-inline-templates")
 
 #####################################
+MESSAGE(STATUS "Checking gazebo build type")
 # Set the default build type
 if (NOT CMAKE_BUILD_TYPE)
-  set (CMAKE_BUILD_TYPE "RELWITHDEBINFO" CACHE STRING 
-    "Choose the type of build, options are: debug release relwithdebinfo profile check" FORCE)
+    set (CMAKE_BUILD_TYPE "RelWithDebInfo" CACHE STRING 
+        "Choose the type of build, options are: Debug Release RelWithDebInfo Profile Check" FORCE)
 endif (NOT CMAKE_BUILD_TYPE)
-string(TOUPPER ${CMAKE_BUILD_TYPE} CMAKE_BUILD_TYPE)
+# TODO: still convert to uppercase to keep backwards compatibility with 
+# uppercase old supported and deprecated modes
+string(TOUPPER ${CMAKE_BUILD_TYPE} CMAKE_BUILD_TYPE_UPPERCASE)
 
 #####################################
 # Set all the global build flags
-set (CMAKE_C_FLAGS "${CMAKE_C_FLAGS_${CMAKE_BUILD_TYPE}}")
-set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS_${CMAKE_BUILD_TYPE}}")
-set (CMAKE_EXE_LINKER_FLAGS "${CMAKE_LINK_FLAGS_${CMAKE_BUILD_TYPE}}")
-set (CMAKE_SHARED_LINKER_FLAGS "${CMAKE_LINK_FLAGS_${CMAKE_BUILD_TYPE}}")
-set (CMAKE_MODULE_LINKER_FLAGS "${CMAKE_LINK_FLAGS_${CMAKE_BUILD_TYPE}}")
+set (CMAKE_C_FLAGS "${CMAKE_C_FLAGS_${CMAKE_BUILD_TYPE_UPPERCASE}}")
+set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS_${CMAKE_BUILD_TYPE_UPPERCASE}}")
+set (CMAKE_EXE_LINKER_FLAGS "${CMAKE_LINK_FLAGS_${CMAKE_BUILD_TYPE_UPPERCASE}}")
+set (CMAKE_SHARED_LINKER_FLAGS "${CMAKE_LINK_FLAGS_${CMAKE_BUILD_TYPE_UPPERCASE}}")
+set (CMAKE_MODULE_LINKER_FLAGS "${CMAKE_LINK_FLAGS_${CMAKE_BUILD_TYPE_UPPERCASE}}")
 
-if ("${CMAKE_BUILD_TYPE}" STREQUAL "PROFILE")
+set (BUILD_TYPE_PROFILE FALSE)
+set (BUILD_TYPE_RELEASE FALSE)
+set (BUILD_TYPE_RELWITHDEBINFO FALSE)
+set (BUILD_TYPE_DEBUG FALSE)
+
+if ("${CMAKE_BUILD_TYPE_UPPERCASE}" STREQUAL "PROFILE")
   set (BUILD_TYPE_PROFILE TRUE)
+elseif ("${CMAKE_BUILD_TYPE_UPPERCASE}" STREQUAL "RELEASE")
+  set (BUILD_TYPE_RELEASE TRUE)
+elseif ("${CMAKE_BUILD_TYPE_UPPERCASE}" STREQUAL "RELWITHDEBINFO")
+  set (BUILD_TYPE_RELWITHDEBINFO TRUE)
+elseif ("${CMAKE_BUILD_TYPE_UPPERCASE}" STREQUAL "DEBUG")
+  set (BUILD_TYPE_DEBUG TRUE)
 else()
-  set (BUILD_TYPE_PROFILE FALSE)
+  build_error("CMAKE_BUILD_TYPE ${CMAKE_BUILD_TYPE} unknown. Valid options are: Debug Release RelWithDebInfo Profile Check")
 endif()
 
-if ("${CMAKE_BUILD_TYPE}" STREQUAL "RELEASE")
-  set (BUILD_TYPE_RELEASE TRUE)
-else()
-  set (BUILD_TYPE_RELEASE FALSE)
-endif()
-
-if ( "${CMAKE_BUILD_TYPE}" STREQUAL "RELWITHDEBINFO")
-  set (BUILD_TYPE_RELWITHDEBINFO TRUE)
-else()
-  set (BUILD_TYPE_RELWITHDEBINFO FALSE)
-endif()
-
-if ( "${CMAKE_BUILD_TYPE}" STREQUAL "DEBUG")
-  set (BUILD_TYPE_DEBUG TRUE)
-else()
-  set (BUILD_TYPE_DEBUG FALSE)
-endif()
+# TODO: remove this check when transtition period ends
+# Check if build type was sent in old uppercase format
+if ("${CMAKE_BUILD_TYPE}" STREQUAL "${CMAKE_BUILD_TYPE_UPPERCASE}")
+    build_warning("CMAKE_BUILD_TYPE was specified in uppercase. This will stop to be supported soon. Valid types: Debug Release RelWithDebInfo Profile Check")
+endif ()
 
 message (STATUS "\n\n====== Configuring 3rd Party Packages ======")
 add_subdirectory(deps)

cmake/SearchForStuff.cmake

 else (libdl_library AND libdl_include_dir)
   SET (HAVE_DL FALSE)
 endif ()
+
+########################################
+# Find QWT (QT graphing library)
+#find_path(QWT_INCLUDE_DIR NAMES qwt.h PATHS
+#  /usr/include
+#  /usr/local/include
+#  "$ENV{LIB_DIR}/include" 
+#  "$ENV{INCLUDE}" 
+#  PATH_SUFFIXES qwt-qt4 qwt qwt5
+#  )
+#
+#find_library(QWT_LIBRARY NAMES qwt qwt6 qwt5 PATHS 
+#  /usr/lib
+#  /usr/local/lib
+#  "$ENV{LIB_DIR}/lib" 
+#  "$ENV{LIB}/lib" 
+#  )
+#
+#if (QWT_INCLUDE_DIR AND QWT_LIBRARY)
+#  set(HAVE_QWT TRUE)
+#endif (QWT_INCLUDE_DIR AND QWT_LIBRARY)
+#
+#if (HAVE_QWT)
+#  if (NOT QWT_FIND_QUIETLY)
+#    message(STATUS "Found Qwt: ${QWT_LIBRARY}")
+#  endif (NOT QWT_FIND_QUIETLY)
+#else ()
+#  if (QWT_FIND_REQUIRED)
+#    BUILD_WARNING ("Could not find libqwt-dev. Plotting features will be disabled.")
+#  endif (QWT_FIND_REQUIRED)
+#endif ()

cmake/gazebo_config.h.in

 #cmakedefine INCLUDE_RTSHADER 1
 #cmakedefine HAVE_URDFDOM 1
 #cmakedefine HAVE_GTS 1
+#cmakedefine HAVE_QWT 1
+#cmakedefine ENABLE_DIAGNOSTICS 1
 
 #ifdef BUILD_TYPE_PROFILE
 #include <google/heap-checker.h>

File contents unchanged.

gazebo/CMakeLists.txt

 add_subdirectory(gui)
 add_subdirectory(physics)
 add_subdirectory(sensors)
+add_subdirectory(util)
 
 add_dependencies(gazebo_physics gazebo_msgs)
 add_dependencies(gazebo_physics_ode gazebo_msgs)
 
 
 target_link_libraries(gzserver gazebo_common
+                               gazebo_util
                                gazebo_sdf_interface
                                gazebo_transport
                                gazebo_physics
 )
 
 target_link_libraries(gazebo gazebo_common
+                             gazebo_util
                              gazebo_sdf_interface
                              gazebo_transport
                              gazebo_physics
 set_target_properties(libgazebo PROPERTIES OUTPUT_NAME "gazebo")
 
 target_link_libraries(libgazebo gazebo_common
+                                gazebo_util
                                 gazebo_transport
                                 gazebo_physics
                                 gazebo_sensors

gazebo/common/CMakeLists.txt

   Color.cc
   Common.cc
   Console.cc
-  Diagnostics.cc
   Event.cc
   Events.cc
   Exception.cc
   CommonTypes.hh
   Color.hh
   Console.hh
-  Diagnostics.hh
   Event.hh
   Events.hh
   Exception.hh
   Animation_TEST.cc
   Color_TEST.cc
   Console_TEST.cc
-  Diagnostics_TEST.cc
   Exception_TEST.cc
   LogRecord_TEST.cc
   Material_TEST.cc
 configure_file (${CMAKE_CURRENT_SOURCE_DIR}/common.hh.in ${CMAKE_CURRENT_BINARY_DIR}/common.hh )
 
 gz_add_library(gazebo_common ${sources})
+
 target_link_libraries(gazebo_common gazebo_math
                                     ${libdl_library}
                                     ${Boost_LIBRARIES}

gazebo/common/ColladaLoader.cc

     if (lambertXml)
     {
       this->LoadColorOrTexture(lambertXml, "ambient", mat);
+      this->LoadColorOrTexture(lambertXml, "emission", mat);
       this->LoadColorOrTexture(lambertXml, "diffuse", mat);
-      this->LoadColorOrTexture(lambertXml, "emission", mat);
       if (lambertXml->FirstChildElement("transparency"))
       {
         mat->SetTransparency(
     else if (phongXml)
     {
       this->LoadColorOrTexture(phongXml, "ambient", mat);
-      this->LoadColorOrTexture(phongXml, "diffuse", mat);
       this->LoadColorOrTexture(phongXml, "emission", mat);
       this->LoadColorOrTexture(phongXml, "specular", mat);
+      this->LoadColorOrTexture(phongXml, "diffuse", mat);
       if (phongXml->FirstChildElement("shininess"))
         mat->SetShininess(
             this->LoadFloat(phongXml->FirstChildElement("shininess")));
     else if (blinnXml)
     {
       this->LoadColorOrTexture(blinnXml, "ambient", mat);
-      this->LoadColorOrTexture(blinnXml, "diffuse", mat);
       this->LoadColorOrTexture(blinnXml, "emission", mat);
       this->LoadColorOrTexture(blinnXml, "specular", mat);
+      this->LoadColorOrTexture(blinnXml, "diffuse", mat);
       if (blinnXml->FirstChildElement("shininess"))
         mat->SetShininess(
             this->LoadFloat(blinnXml->FirstChildElement("shininess")));

gazebo/common/CommonTypes.hh

 
   namespace common
   {
-    class Param;
-    class Time;
+    class Animation;
+    class Color;
+    class DiagnosticTimer;
     class Image;
     class Mesh;
     class MouseEvent;
+    class NumericAnimation;
+    class Param;
     class PoseAnimation;
-    class NumericAnimation;
-    class Animation;
-    class Color;
     class SkeletonAnimation;
+    class Time;
 
     template <typename T>
     class ParamT;
     /// \def NumericAnimationPtr
     /// \brief boost::shared_ptr to a NumericAnimation class
     typedef boost::shared_ptr<NumericAnimation> NumericAnimationPtr;
+
+    /// \def DiagnosticTimerPtr
+    /// \brief boost::shared_ptr to a DiagnosticTimer class
+    typedef boost::shared_ptr<DiagnosticTimer> DiagnosticTimerPtr;
   }
 
   namespace event

gazebo/common/Diagnostics.cc

-/*
- * Copyright 2012 Open Source Robotics Foundation
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- * http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- *
- */
-/* Desc: A diagnostic class
- * Author: Nate Koenig
- * Date: 2 Feb 2011
- */
-
-#include "common/Events.hh"
-#include "common/Diagnostics.hh"
-
-using namespace gazebo;
-using namespace common;
-
-
-DiagnosticManager *DiagnosticTimer::diagManager = DiagnosticManager::Instance();
-
-//////////////////////////////////////////////////
-  DiagnosticManager::DiagnosticManager()
-: enabled(false)
-{
-}
-
-//////////////////////////////////////////////////
-DiagnosticManager::~DiagnosticManager()
-{
-}
-
-//////////////////////////////////////////////////
-DiagnosticTimerPtr DiagnosticManager::CreateTimer(const std::string &_name)
-{
-  if (this->GetEnabled())
-    return DiagnosticTimerPtr(new DiagnosticTimer(_name));
-  else
-    return DiagnosticTimerPtr();
-}
-
-//////////////////////////////////////////////////
-void DiagnosticManager::TimerStart(DiagnosticTimer *_timer)
-{
-  this->timers[_timer->GetName()] = Time();
-  event::Events::diagTimerStart(_timer->GetName());
-}
-
-
-//////////////////////////////////////////////////
-void DiagnosticManager::TimerStop(DiagnosticTimer *_timer)
-{
-  this->timers[_timer->GetName()] = _timer->GetElapsed();
-  event::Events::diagTimerStop(_timer->GetName());
-}
-
-//////////////////////////////////////////////////
-int DiagnosticManager::GetTimerCount() const
-{
-  return this->timers.size();
-}
-
-//////////////////////////////////////////////////
-Time DiagnosticManager::GetTime(int _index) const
-{
-  std::map<std::string, Time>::const_iterator iter;
-
-  iter = this->timers.begin();
-  std::advance(iter, _index);
-
-  if (iter != this->timers.end())
-    return iter->second;
-  else
-    gzerr << "Error getting time\n";
-
-  return Time();
-}
-
-//////////////////////////////////////////////////
-std::string DiagnosticManager::GetLabel(int _index) const
-{
-  std::map<std::string, Time>::const_iterator iter;
-
-  iter = this->timers.begin();
-  std::advance(iter, _index);
-
-  if (iter != this->timers.end())
-    return iter->first;
-  else
-    gzerr << "Erorr getting label\n";
-
-  return "null";
-}
-
-//////////////////////////////////////////////////
-Time DiagnosticManager::GetTime(const std::string &_label) const
-{
-  std::map<std::string, Time>::const_iterator iter;
-  iter = this->timers.find(_label);
-
-  if (iter != this->timers.end())
-    return iter->second;
-  else
-    gzerr << "Error getting time\n";
-
-  return Time();
-}

gazebo/common/Diagnostics.hh

-/*
- * Copyright 2012 Open Source Robotics Foundation
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- *     http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- *
-*/
-/* Desc: A diagnostic class
- * Author: Nate Koenig
- * Date: 2 Feb 2011
- */
-
-#ifndef _DIAGNOSTICMANAGER_HH_
-#define _DIAGNOSTICMANAGER_HH_
-
-#include <map>
-#include <string>
-
-#include "common/SingletonT.hh"
-#include "common/Timer.hh"
-
-namespace gazebo
-{
-  namespace common
-  {
-    /// \addtogroup gazebo_common Common
-    /// \{
-
-    /// \brief Create an instance of common::DiagnosticManager
-    #define DIAG_TIMER(name) DiagnosticManager::Instance()->CreateTimer(name);
-
-    class DiagnosticTimer;
-    typedef boost::shared_ptr< DiagnosticTimer > DiagnosticTimerPtr;
-
-    /// \class DiagnosticManager Diagnostics.hh common/common.hh
-    /// \brief A diagnostic manager class
-    class DiagnosticManager : public SingletonT<DiagnosticManager>
-    {
-      /// \brief Constructor
-      private: DiagnosticManager();
-
-      /// \brief Destructor
-      private: virtual ~DiagnosticManager();
-
-      /// \brief Create a new timer instance
-      /// \param[in] _name Name of the timer.
-      /// \return A pointer to the new diagnostic timer
-      public: DiagnosticTimerPtr CreateTimer(const std::string &_name);
-
-      /// \brief A diagnostic timer has started
-      /// \param[in] _timer The timer to start
-      public: void TimerStart(DiagnosticTimer *_timer);
-
-      /// \brief A diagnostic timer has stopped
-      /// \param[in] _time The timer to stop
-      public: void TimerStop(DiagnosticTimer *_timer);
-
-      /// \brief Get the number of timers
-      /// \return The number of timers
-      public: int GetTimerCount() const;
-
-      /// \brief Get the time of a timer instance
-      /// \param[in] _index The index of a timer instance
-      /// \return Time of the specified timer
-      public: Time GetTime(int _index) const;
-
-      /// \brief Get a time based on a label
-      /// \param[in] _label Name of the timer instance
-      /// \return Time of the specified timer
-      public: Time GetTime(const std::string &_label) const;
-
-      /// \brief Get a label for a timer
-      /// \param[in] _index Index of a timer instance
-      /// \return Label of the specified timer
-      public: std::string GetLabel(int _index) const;
-
-      /// \brief Set whether timers are enabled
-      /// \param[in] _e True = timers are enabled
-      public: void SetEnabled(bool _e) {this->enabled = _e;}
-
-      /// \brief Get whether the timers are enabled
-      /// \return TRue if the timers are enabled
-      public: inline bool GetEnabled() const {return this->enabled;}
-
-      /// \brief always false, has not effect
-      private: bool enabled;
-
-      /// \brief dictionary of timers index by name
-      private: std::map<std::string, Time> timers;
-
-      // Singleton implementation
-      private: friend class SingletonT<DiagnosticManager>;
-    };
-
-    /// \class DiagnosticTimer Diagnostics.hh common/common.hh
-    /// \brief A timer designed for diagnostics
-    class DiagnosticTimer : public Timer
-    {
-      /// \brief Constructor
-      /// \param[in] _name Name of the timer
-      public: DiagnosticTimer(const std::string &_name) : Timer()
-              {
-                this->Start();
-                this->name = _name;
-                this->diagManager->TimerStart(this);
-              }
-
-      /// \brief Destructor
-      public: virtual ~DiagnosticTimer()
-              {
-                this->diagManager->TimerStop(this);
-              }
-
-      /// \brief Get the name of the timer
-      /// \return The name of timer
-      public: inline const std::string GetName() const
-              { return this->name; }
-
-      /// \brief not used
-      private: std::string name;
-
-      /// \brief singleton instance
-      private: static DiagnosticManager *diagManager;
-    };
-    /// \}
-  }
-}
-#endif

gazebo/common/Diagnostics_TEST.cc

-/*
- * Copyright 2013 Open Source Robotics Foundation
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- *     http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- *
-*/
-
-#include <gtest/gtest.h>
-
-#include "gazebo/common/Diagnostics.hh"
-
-using namespace gazebo;
-
-TEST(DiagnosticsTest, Diagnostics)
-{
-  common::DiagnosticManager *mgr = common::DiagnosticManager::Instance();
-  EXPECT_TRUE(mgr != NULL);
-
-  mgr->SetEnabled(true);
-  EXPECT_TRUE(mgr->GetEnabled());
-
-  common::Time prev = common::Time::GetWallTime();
-  {
-    common::DiagnosticTimerPtr timer = mgr->CreateTimer("test");
-    EXPECT_STREQ("test", timer->GetName().c_str());
-    EXPECT_STREQ("test", mgr->GetLabel(0).c_str());
-    EXPECT_EQ(1, mgr->GetTimerCount());
-  }
-  common::Time after = common::Time::GetWallTime();
-
-  EXPECT_TRUE(mgr->GetTime(0) == mgr->GetTime("test"));
-  EXPECT_TRUE(mgr->GetTime(0) <= after - prev);
-}
-
-
-
-/////////////////////////////////////////////////
-int main(int argc, char **argv)
-{
-  ::testing::InitGoogleTest(&argc, argv);
-  return RUN_ALL_TESTS();
-}

gazebo/common/Event.hh

     template<typename T>
     void EventT<T>::Disconnect(ConnectionPtr _c)
     {
+      if (!_c)
+        return;
+
       this->Disconnect(_c->GetId());
       _c->event = NULL;
       _c->id = -1;

gazebo/common/ModelDatabase.cc

     }
 
     TiXmlElement *uriElem;
-    for (uriElem = modelsElem->FirstChildElement("uri"); uriElem != NULL;
-        uriElem = uriElem->NextSiblingElement("uri"))
+    for (uriElem = modelsElem->FirstChildElement("uri");
+         uriElem != NULL && !this->stop;
+         uriElem = uriElem->NextSiblingElement("uri"))
     {
       std::string uri = uriElem->GetText();
 

gazebo/common/Timer.cc

  * Date: 22 Nov 2009
  */
 
-#include "common/Timer.hh"
+#include "gazebo/common/Timer.hh"
 
 using namespace gazebo;
 using namespace common;
 
 //////////////////////////////////////////////////
 Timer::Timer()
+  : running(false)
 {
 }
 
 void Timer::Start()
 {
   this->start = Time::GetWallTime();
+  this->running = true;
+}
+
+//////////////////////////////////////////////////
+void Timer::Stop()
+{
+  this->stop = Time::GetWallTime();
+  this->running = false;
+}
+
+//////////////////////////////////////////////////
+bool Timer::GetRunning() const
+{
+  return this->running;
 }
 
 //////////////////////////////////////////////////
 Time Timer::GetElapsed() const
 {
-  Time currentTime;
+  if (this->running)
+  {
+    Time currentTime;
+    currentTime = Time::GetWallTime();
 
-  currentTime = Time::GetWallTime();
-
-  return currentTime - this->start;
+    return currentTime - this->start;
+  }
+  else
+    return this->stop - this->start;
 }
-
-

gazebo/common/Timer.hh

       public: virtual ~Timer();
 
       /// \brief Start the timer
-      public: void Start();
+      public: virtual void Start();
+
+      /// \brief Stop the timer
+      public: virtual void Stop();
+
+      /// \brief Returns true if the timer is running.
+      /// \return Tue if the timer has been started and not stopped.
+      public: bool GetRunning() const;
 
       /// \brief Get the elapsed time
       /// \return The time
       public: Time GetElapsed() const;
 
-      /// \brief stream operator friendly
+      /// \brief Stream operator friendly
       public: friend std::ostream &operator<<(std::ostream &out,
                                               const gazebo::common::Timer &t)
               {
                 return out;
               }
 
-      /// \brief the time of the last call to Start
+      /// \brief The time of the last call to Start
       private: Time start;
+
+      /// \brief The time when Stop was called.
+      private: Time stop;
+
+      /// \brief True if the timer is running.
+      private: bool running;
     };
     /// \}
   }

gazebo/gui/Actions.cc

 
 QAction *gazebo::gui::g_topicVisAct = 0;
 
+QAction *gazebo::gui::g_diagnosticsAct = 0;
+
 gazebo::gui::DeleteAction *gazebo::gui::g_deleteAct = 0;

gazebo/gui/Actions.hh

 
     extern QAction *g_topicVisAct;
 
+    extern QAction *g_diagnosticsAct;
+
     extern QAction *g_buildingEditorSaveAct;
     extern QAction *g_buildingEditorDiscardAct;
     extern QAction *g_buildingEditorDoneAct;

gazebo/gui/CMakeLists.txt

   ${Boost_LIBRARY_DIRS}
 )
 
+include_directories(
+  ${CMAKE_SOURCE_DIR}/deps
+)
+
 add_definitions(${QT_DEFINITIONS})
 
 add_subdirectory(qtpropertybrowser)
   viewers/ViewFactory.hh
 )
 
-set (qt_tests 
+#if (HAVE_QWT)
+#  set (sources ${sources}
+#    Diagnostics.cc
+#    IncrementalPlot.cc
+#  )
+#  set (qt_headers ${qt_headers}
+#    Diagnostics.hh
+#    IncrementalPlot.hh
+#  )
+#endif()
+
+set (qt_tests
   DataLogger_TEST.cc
   TimePanel_TEST.cc
   viewers/ImagesView_TEST.cc
 )
 
+# Generate executables for each of the QT unit tests
 gz_build_qt_tests(${qt_tests})
 
 set (resources resources.qrc)
 
-
 QT4_WRAP_CPP(headers_MOC ${qt_headers})
 QT4_ADD_RESOURCES(resources_RCC ${resources})
 

gazebo/gui/Diagnostics.cc

+/*
+ * Copyright 2012 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+
+#include "gazebo/transport/Transport.hh"
+#include "gazebo/transport/Node.hh"
+#include "gazebo/transport/Publisher.hh"
+
+#include "gazebo/gui/IncrementalPlot.hh"
+#include "gazebo/gui/Diagnostics.hh"
+
+using namespace gazebo;
+using namespace gui;
+
+// A special list widget that allows dragging of items from it to a
+// plot
+class DragableListWidget : public QListWidget
+{
+  public: DragableListWidget(QWidget *_parent)
+          : QListWidget(_parent)
+          {
+          }
+
+  protected: virtual void startDrag(Qt::DropActions /*_supportedActions*/)
+             {
+               QListWidgetItem *currItem = this->currentItem();
+               QMimeData *currMimeData = new QMimeData;
+               QByteArray ba;
+               ba = currItem->text().toLatin1().data();
+               currMimeData->setData("application/x-item", ba);
+               QDrag *drag = new QDrag(this);
+               drag->setMimeData(currMimeData);
+               drag->exec(Qt::LinkAction);
+             }
+
+  protected: virtual Qt::DropActions supportedDropActions()
+             {
+               return Qt::LinkAction;
+             }
+};
+
+/////////////////////////////////////////////////
+Diagnostics::Diagnostics(QWidget *_parent)
+  : QDialog(_parent)
+{
+  this->paused = false;
+  this->setWindowIcon(QIcon(":/images/gazebo.svg"));
+  this->setWindowTitle("Gazebo: Diagnostics");
+  this->setObjectName("diagnosticsPlot");
+
+  this->plotLayout = new QVBoxLayout;
+
+  QScrollArea *plotScrollArea = new QScrollArea(this);
+  plotScrollArea->setLineWidth(0);
+  plotScrollArea->setFrameShape(QFrame::NoFrame);
+  plotScrollArea->setFrameShadow(QFrame::Plain);
+  plotScrollArea->setSizePolicy(QSizePolicy::Minimum,
+                                QSizePolicy::Minimum);
+
+  plotScrollArea->setWidgetResizable(true);
+  plotScrollArea->viewport()->installEventFilter(this);
+
+  QFrame *plotFrame = new QFrame(plotScrollArea);
+  plotFrame->setSizePolicy(QSizePolicy::Minimum, QSizePolicy::Minimum);
+  plotFrame->setLayout(this->plotLayout);
+
+  plotScrollArea->setWidget(plotFrame);
+
+
+  IncrementalPlot *plot = new IncrementalPlot(this);
+  this->plots.push_back(plot);
+  this->plotLayout->addWidget(plot);
+
+  this->labelList = new DragableListWidget(this);
+  this->labelList->setDragEnabled(true);
+  this->labelList->setDragDropMode(QAbstractItemView::DragOnly);
+  QListWidgetItem *item = new QListWidgetItem("Real Time Factor");
+  item->setToolTip(tr("Drag onto graph to plot"));
+  this->labelList->addItem(item);
+
+  QPushButton *addPlotButton = new QPushButton("Add");
+  connect(addPlotButton, SIGNAL(clicked()), this, SLOT(OnAddPlot()));
+
+  QCheckBox *pauseCheck = new QCheckBox;
+  pauseCheck->setChecked(this->paused);
+  connect(pauseCheck, SIGNAL(clicked(bool)), this, SLOT(OnPause(bool)));
+
+  QHBoxLayout *pauseLayout = new QHBoxLayout;
+  pauseLayout->addWidget(pauseCheck);
+  pauseLayout->addWidget(new QLabel("Pause"));
+  pauseLayout->addWidget(addPlotButton);
+  pauseLayout->addStretch(1);
+
+  QVBoxLayout *leftLayout = new QVBoxLayout;
+  leftLayout->addWidget(this->labelList);
+  leftLayout->addLayout(pauseLayout);
+
+  QHBoxLayout *mainLayout = new QHBoxLayout;
+  mainLayout->addLayout(leftLayout);
+  mainLayout->addWidget(plotScrollArea, 2);
+
+  this->setLayout(mainLayout);
+  this->setSizeGripEnabled(true);
+
+  this->node = transport::NodePtr(new transport::Node());
+  this->node->Init();
+  this->sub = this->node->Subscribe("~/diagnostics", &Diagnostics::OnMsg, this);
+
+  QTimer *displayTimer = new QTimer(this);
+  connect(displayTimer, SIGNAL(timeout()), this, SLOT(Update()));
+  displayTimer->start(60);
+}
+
+/////////////////////////////////////////////////
+Diagnostics::~Diagnostics()
+{
+}
+
+/////////////////////////////////////////////////
+void Diagnostics::Update()
+{
+  boost::mutex::scoped_lock lock(this->mutex);
+
+  // Update all the plots
+  for (std::vector<IncrementalPlot *>::iterator iter = this->plots.begin();
+      iter != this->plots.end(); ++iter)
+  {
+    (*iter)->Update();
+  }
+}
+
+/////////////////////////////////////////////////
+void Diagnostics::OnMsg(ConstDiagnosticsPtr &_msg)
+{
+  // Make sure plotting is not paused.
+  if (this->paused)
+    return;
+
+  // Make sure there is timing information
+  if (_msg->time_size() == 0)
+    return;
+
+  boost::mutex::scoped_lock lock(this->mutex);
+
+  common::Time wallTime, elapsedTime;
+
+  wallTime = msgs::Convert(_msg->real_time());
+
+  // Add real-time factor if it has been requested.
+  for (std::vector<IncrementalPlot*>::iterator iter = this->plots.begin();
+      iter != this->plots.end(); ++iter)
+  {
+    if ((*iter)->HasCurve(QString("Real Time Factor")))
+    {
+      (*iter)->Add(QString("Real Time Factor"),
+                   QPointF(wallTime.Double(), _msg->real_time_factor()));
+    }
+  }
+
+  // Process each time point
+  for (int i = 0; i < _msg->time_size(); ++i)
+  {
+    QString qstr = QString::fromStdString(_msg->time(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);
+    }
+
+    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))
+      {
+        elapsedTime = msgs::Convert(_msg->time(i).elapsed());
+
+        double msTime = elapsedTime.Double() * 1e3;
+        QPointF pt(wallTime.Double(), msTime);
+
+        (*iter)->Add(labelStr, pt);
+      }
+    }
+  }
+}
+
+/////////////////////////////////////////////////
+void Diagnostics::OnPause(bool _value)
+{
+  this->paused = _value;
+}
+
+/////////////////////////////////////////////////
+void Diagnostics::OnAddPlot()
+{
+  IncrementalPlot *plot = new IncrementalPlot(this);
+  this->plotLayout->addWidget(plot);
+  this->plots.push_back(plot);
+}
+
+/////////////////////////////////////////////////
+bool Diagnostics::eventFilter(QObject *o, QEvent *e)
+{
+  if (e->type() == QEvent::Wheel)
+  {
+    e->ignore();
+    return true;
+  }
+
+  return QWidget::eventFilter(o, e);
+}

gazebo/gui/Diagnostics.hh

+/*
+ * Copyright 2012 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+
+#ifndef _DIAGNOSTICS_WIDGET_HH_
+#define _DIAGNOSTICS_WIDGET_HH_
+
+#include <map>
+#include <list>
+#include <vector>
+#include <boost/thread/mutex.hpp>
+
+#include "gazebo/msgs/msgs.hh"
+#include "gazebo/common/Time.hh"
+#include "gazebo/transport/TransportTypes.hh"
+#include "gazebo/gui/qt.h"
+
+namespace gazebo
+{
+  namespace gui
+  {
+    class IncrementalPlot;
+
+    /// \brief Plot diagnostic information
+    class Diagnostics : public QDialog
+    {
+      Q_OBJECT
+
+      /// \brief Constructor.
+      /// \param[in] _parent Pointer to the parent widget.
+      public: Diagnostics(QWidget *_parent);
+
+      /// \brief Destructor.
+      public: virtual ~Diagnostics();
+
+      /// \brief Used to filter scroll wheel events.
+      /// \param[in] _o Object that receives the event.
+      /// \param[in] _event Pointer to the event.
+      public: virtual bool eventFilter(QObject *_o, QEvent *_e);
+
+      /// \brief Called when a diagnostic message is received.
+      /// \param[in] _msg Diagnostic message.
+      private: void OnMsg(ConstDiagnosticsPtr &_msg);
+
+      /// \brief Update plots.
+      private slots: void Update();
+
+      /// \brief QT callback for the pause check button.
+      /// \param[in] _value True when paused.
+      private slots: void OnPause(bool _value);
+
+      /// \brief Qt Callback when a plot should be added.
+      private slots: void OnAddPlot();
+
+      /// \brief Node for communications.
+      private: transport::NodePtr node;
+
+      /// \brief Subscribes to diagnostic info.
+      private: transport::SubscriberPtr sub;
+
+      /// \brief The list of diagnostic labels.
+      private: QListWidget *labelList;
+
+      private: typedef std::map<QString, std::list<QPointF> > PointMap;
+
+      /// \brief The currently selected label.
+      private: PointMap selectedLabels;
+
+      /// \brief True when plotting is paused.
+      private: bool paused;
+
+      /// \brief Mutex to protect the point map
+      private: boost::mutex mutex;
+
+      /// \brief Plotting widget
+      private: std::vector<IncrementalPlot *> plots;
+
+      /// \brief Layout to hold all the plots.
+      private: QVBoxLayout *plotLayout;
+    };
+  }
+}
+#endif

gazebo/gui/GLWidget.cc

 void GLWidget::paintEvent(QPaintEvent *_e)
 {
   rendering::UserCameraPtr cam = gui::get_active_camera();
-  if (cam && cam->IsInitialized())
+  if (cam && cam->GetInitialized())
   {
     event::Events::preRender();
 

gazebo/gui/IncrementalPlot.cc

+/*
+ * Copyright 2012 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+
+#include <qwt/qwt_plot.h>
+#include <qwt/qwt_scale_widget.h>
+#include <qwt/qwt_plot_panner.h>
+#include <qwt/qwt_plot_layout.h>
+#include <qwt/qwt_plot_grid.h>
+#include <qwt/qwt_plot_canvas.h>
+#include <qwt/qwt_plot_curve.h>
+#include <qwt/qwt_curve_fitter.h>
+#include <qwt/qwt_symbol.h>
+#include <qwt/qwt_legend.h>
+#include <qwt/qwt_legend_item.h>
+#include <qwt/qwt_plot_directpainter.h>
+
+#include "gazebo/common/Assert.hh"
+
+#include "gazebo/math/Helpers.hh"
+#include "gazebo/gui/IncrementalPlot.hh"
+
+using namespace gazebo;
+using namespace gui;
+
+// The number of unique color
+static const int ColorCount = 5;
+
+// The unique colors
+static const QColor Colors[ColorCount] =
+{
+  QColor(255, 0, 0),
+  QColor(0, 255, 0),
+  QColor(0, 0, 255),
+  QColor(255, 255, 0),
+  QColor(255, 0, 255)
+};
+
+// A class that manages plotting data
+class CurveData: public QwtArraySeriesData<QPointF>
+{
+  public: CurveData()
+          {}
+
+  public: virtual QRectF boundingRect() const
+          {
+            if (this->d_boundingRect.width() < 0.0)
+              this->d_boundingRect = qwtBoundingRect(*this);
+
+            return this->d_boundingRect;
+          }
+
+  public: inline void Add(const QPointF &_point)
+          {
+            this->d_samples += _point;
+            if (this->d_samples.size() > 6000)
+              this->d_samples.remove(0, 1000);
+          }
+
+  public: void Clear()
+          {
+            this->d_samples.clear();
+            this->d_samples.squeeze();
+            this->d_boundingRect = QRectF(0.0, 0.0, -1.0, -1.0);
+          }
+};
+
+/////////////////////////////////////////////////
+IncrementalPlot::IncrementalPlot(QWidget *_parent)
+  : QwtPlot(_parent)
+{
+  this->directPainter = new QwtPlotDirectPainter(this);
+
+  // panning with the left mouse button
+  (void) new QwtPlotPanner(this->canvas());
+
+  // zoom in/out with the wheel
+  this->magnifier = new QwtPlotMagnifier(this->canvas());
+
+#if defined(Q_WS_X11)
+  this->canvas()->setAttribute(Qt::WA_PaintOutsidePaintEvent, true);
+  this->canvas()->setAttribute(Qt::WA_PaintOnScreen, true);
+#endif
+
+  this->setAutoReplot(false);
+
+  this->setFrameStyle(QFrame::NoFrame);
+  this->setLineWidth(0);
+  this->setCanvasLineWidth(2);
+
+  this->plotLayout()->setAlignCanvasToScales(true);
+
+  QwtLegend *qLegend = new QwtLegend;
+  qLegend->setItemMode(QwtLegend::CheckableItem);
+  this->insertLegend(qLegend, QwtPlot::RightLegend);
+
+  QwtPlotGrid *grid = new QwtPlotGrid;
+  grid->setMajPen(QPen(Qt::gray, 0, Qt::DotLine));
+  grid->attach(this);
+
+  /// \todo Figure out a way to properly lable the y-axis
+  QwtText ytitle("Duration (ms)");
+  ytitle.setFont(QFont(fontInfo().family(), 10, QFont::Bold));
+  this->setAxisTitle(QwtPlot::yLeft, ytitle);
+
+  this->setAxisAutoScale(QwtPlot::yRight, true);
+  this->setAxisAutoScale(QwtPlot::yLeft, true);
+
+  this->replot();
+
+  this->setAcceptDrops(true);
+}
+
+/////////////////////////////////////////////////
+IncrementalPlot::~IncrementalPlot()
+{
+  for (CurveMap::iterator iter = this->curves.begin();
+       iter != this->curves.end(); ++iter)
+  {
+    delete iter->second;
+  }
+
+  this->curves.clear();
+}
+
+/////////////////////////////////////////////////
+void IncrementalPlot::Add(const QString &_label,
+                          const std::list<QPointF> &_pts)
+{
+  if (_label.isEmpty())
+    return;
+
+  QwtPlotCurve *curve = NULL;
+
+  CurveMap::iterator iter = this->curves.find(_label);
+  if (iter == this->curves.end())
+    curve = this->AddCurve(_label);
+  else
+    curve = iter->second;
+
+  GZ_ASSERT(curve != NULL, "Curve is NULL");
+
+  // Get the  curve data
+  CurveData *curveData = static_cast<CurveData *>(curve->data());
+
+  GZ_ASSERT(curveData != NULL, "Curve data is NULL");
+
+  // Add all the points
+  for (std::list<QPointF>::const_iterator ptIter = _pts.begin();
+       ptIter != _pts.end(); ++ptIter)
+  {
+    curveData->Add(*ptIter);
+  }
+
+  // Adjust the curve
+  this->AdjustCurve(curve);
+}
+
+/////////////////////////////////////////////////
+void IncrementalPlot::Add(const QString &_label, const QPointF &_pt)
+{
+  if (_label.isEmpty())
+    return;
+
+  QwtPlotCurve *curve = NULL;
+
+  CurveMap::iterator iter = this->curves.find(_label);
+  if (iter == this->curves.end())
+    curve = this->AddCurve(_label);
+  else
+    curve = iter->second;
+
+  GZ_ASSERT(curve != NULL, "Curve is NULL");
+
+  // Get the curve data
+  CurveData *curveData = static_cast<CurveData *>(curve->data());
+
+  GZ_ASSERT(curveData != NULL, "Curve data is NULL");
+
+  // Add a point
+  curveData->Add(_pt);
+}
+
+/////////////////////////////////////////////////
+void IncrementalPlot::AdjustCurve(QwtPlotCurve *_curve)
+{
+  GZ_ASSERT(_curve != NULL, "Curve is NULL");
+
+  CurveData *curveData = static_cast<CurveData *>(_curve->data());
+  const QPointF &lastPoint = curveData->samples().back();
+
+  const bool doClip = !this->canvas()->testAttribute(Qt::WA_PaintOnScreen);
+
+  if (doClip)
+  {
+    // Depending on the platform setting a clip might be an important
+    // performance issue. F.e. for Qt Embedded this reduces the
+    // part of the backing store that has to be copied out - maybe
+    // to an unaccelerated frame buffer device.
+    const QwtScaleMap xMap = this->canvasMap(_curve->xAxis());
+    const QwtScaleMap yMap = this->canvasMap(_curve->yAxis());
+
+    QRegion clipRegion;
+
+    const QSize symbolSize = _curve->symbol()->size();
+    QRect r(0, 0, symbolSize.width() + 2, symbolSize.height() + 2);
+
+    const QPointF center = QwtScaleMap::transform(xMap, yMap, lastPoint);
+    r.moveCenter(center.toPoint());
+    clipRegion += r;
+
+    this->directPainter->setClipRegion(clipRegion);
+  }
+
+  this->setAxisScale(this->xBottom,
+      std::max(0.0, static_cast<double>(lastPoint.x() -
+          5.0 * this->magnifier->wheelFactor())),
+      std::max(1.0, static_cast<double>(lastPoint.x())));
+
+  // this->setAxisScale(_curve->yAxis(), 0.0, _curve->maxYValue() * 2.0);
+
+  // this->setAxisAutoScale(this->yRight, true);
+  // this->setAxisAutoScale(this->yLeft, true);
+
+  this->directPainter->drawSeries(_curve,
+      curveData->size() - 1, curveData->size() - 1);
+
+  this->replot();
+}
+
+/////////////////////////////////////////////////
+QwtPlotCurve *IncrementalPlot::AddCurve(const QString &_label)
+{
+  QwtPlotCurve *curve = new QwtPlotCurve(_label);
+
+  curve->setStyle(QwtPlotCurve::Lines);
+  curve->setData(new CurveData());
+
+  // Delete an old curve if it exists.
+  if (this->curves.find(_label) != this->curves.end())
+  {
+    CurveData *curveData = static_cast<CurveData*>(
+        this->curves[_label]->data());
+    curveData->Clear();
+    delete this->curves[_label];
+  }
+
+  this->curves[_label] = curve;
+
+  QColor penColor = Colors[(this->curves.size()-1) % ColorCount];
+
+  /// \todo The following will add the curve to the right hand axis. Need
+  /// a better way to do this based on user input.
+  // this->enableAxis(QwtPlot::yRight);
+  // this->axisAutoScale(QwtPlot::yRight);
+  // curve->setYAxis(QwtPlot::yRight);
+
+  QPen pen(penColor);
+  pen.setWidth(1.0);
+  curve->setPen(pen);
+  curve->setStyle(QwtPlotCurve::Lines);
+
+  curve->setSymbol(new QwtSymbol(QwtSymbol::Ellipse,
+        Qt::NoBrush, QPen(penColor), QSize(2, 2)));
+
+  curve->attach(this);
+
+  return curve;
+}
+
+/////////////////////////////////////////////////
+void IncrementalPlot::Clear(const QString &_label)
+{
+  CurveMap::iterator iter = this->curves.find(_label);
+
+  if (iter == this->curves.end())
+    return;
+
+  CurveData *curveData = static_cast<CurveData *>(iter->second->data());
+  curveData->Clear();
+
+  delete iter->second;
+  this->curves.erase(iter);
+
+  this->replot();
+}
+
+/////////////////////////////////////////////////
+void IncrementalPlot::Clear()
+{
+  CurveData *curveData = NULL;
+
+  for (CurveMap::iterator iter = this->curves.begin();
+       iter != this->curves.end(); ++iter)
+  {
+    curveData = static_cast<CurveData *>(iter->second->data());
+    curveData->Clear();
+    delete iter->second;
+  }
+
+  this->curves.clear();
+
+  this->replot();
+}
+
+/////////////////////////////////////////////////
+QSize IncrementalPlot::sizeHint() const
+{
+  return QSize(540, 400);
+}
+
+/////////////////////////////////////////////////
+void IncrementalPlot::dragEnterEvent(QDragEnterEvent *_evt)
+{
+  if (_evt->mimeData()->hasFormat("application/x-item") &&
+      _evt->source() != this)
+  {
+    _evt->setDropAction(Qt::LinkAction);
+    _evt->acceptProposedAction();
+  }
+  else
+    _evt->ignore();
+}
+
+/////////////////////////////////////////////////
+bool IncrementalPlot::HasCurve(const QString &_label)
+{
+  return this->curves.find(_label) != this->curves.end();
+}
+
+/////////////////////////////////////////////////
+void IncrementalPlot::dropEvent(QDropEvent *_evt)
+{
+  QString name = _evt->mimeData()->data("application/x-item");
+  this->AddCurve(name);
+}
+
+/////////////////////////////////////////////////
+void IncrementalPlot::Update()
+{
+  for (CurveMap::iterator iter = this->curves.begin();
+       iter != this->curves.end(); ++iter)
+  {
+    this->AdjustCurve(iter->second);
+  }
+}

gazebo/gui/IncrementalPlot.hh

+/*
+ * Copyright 2012 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+
+#ifndef _INCREMENTAL_PLOT_HH_
+#define _INCREMENTAL_PLOT_HH_
+
+#include <map>
+#include <list>
+
+#include <qwt/qwt_plot_magnifier.h>
+#include <qwt/qwt_plot.h>
+
+#include "gazebo/math/Vector2d.hh"
+
+#include "gazebo/gui/qt.h"
+
+class QwtPlotCurve;
+class QwtPlotDirectPainter;
+
+namespace gazebo
+{
+  namespace gui
+  {
+    /// \brief A plotting widget that handles incremental addition of data.
+    class IncrementalPlot : public QwtPlot
+    {
+      Q_OBJECT
+
+      /// \brief Constructor
+      /// \param[in] _parent Pointer to a parent widget
+      public: IncrementalPlot(QWidget *_parent = NULL);
+
+      /// \brief Destructor
+      public: virtual ~IncrementalPlot();
+
+      /// \brief Give QT a size hint.
+      /// \return Default size of the plot.
+      public: virtual QSize sizeHint() const;
+
+      /// \brief Add a new point to a curve.
+      /// \param[in] _label Name of the curve to add a point to. A curve
+      /// will be added if it doesn't exist.
+      /// \param[in] _pt Point to add.
+      public slots: void Add(const QString &_label, const QPointF &_pt);
+
+      /// \brief Add new points to a curve.
+      /// \param[in] _label Name of the curve to add a point to. A curve
+      /// will be added if it doesn't exist.
+      /// \param[in] _pt Points to add.
+      public slots: void Add(const QString &_label,
+                             const std::list<QPointF> &_pts);
+
+      /// \brief Clear a single curve from the plot.
+      /// \param[in] _label Name of the curve to remove.
+      public: void Clear(const QString &_label);
+
+      /// \brief Clear all points from the plot.
+      public: void Clear();
+
+      /// \brief Return true if the plot has the labled curve.
+      /// \param[in] _label Name of the curve to check for.
+      /// \return True if _label is currently plotted.
+      public: bool HasCurve(const QString &_label);
+
+      /// \brief Update all the curves in the plot
+      public: void Update();
+
+      /// \brief Used to accept drag enter events.
+      /// \param[in] _evt The drag event.
+      protected: void dragEnterEvent(QDragEnterEvent *_evt);
+
+      /// \brief Used to accept drop events.
+      /// \param[in] _evt The drop event.
+      protected: void dropEvent(QDropEvent *_evt);
+
+      /// \brief Adjust a curve to fit new data.
+      /// \param[in] _curve Curve to adjust
+      private: void AdjustCurve(QwtPlotCurve *_curve);
+
+      /// \brief Add a named curve.
+      /// \param[in] _label Name of the curve.
+      /// \return A pointer to the new curve.
+      private: QwtPlotCurve *AddCurve(const QString &_label);
+
+      /// \def DiagnosticTimerPtr
+      /// \brief A map of strings to qwt plot curves.
+      private: typedef std::map<QString, QwtPlotCurve *> CurveMap;
+
+      /// \brief The curve to draw.
+      private: CurveMap curves;
+
+      /// \brief Drawing utility
+      private: QwtPlotDirectPainter *directPainter;
+
+      /// \brief Pointer to the plot maginfier
+      private: QwtPlotMagnifier *magnifier;
+    };
+  }
+}
+#endif

gazebo/gui/MainWindow.cc

  * limitations under the License.
  *
  */
+#include "gazebo_config.h"
 
 #include "gazebo/gui/TopicSelector.hh"
 #include "gazebo/gui/DataLogger.hh"
 
 #include "sdf/sdf.hh"
 
-#include "gazebo_config.h"
+#ifdef HAVE_QWT
+#include "gazebo/gui/Diagnostics.hh"
+#endif
+
 
 using namespace gazebo;
 using namespace gui;
 MainWindow::MainWindow()
   : renderWidget(0)
 {
+  this->menuBar = NULL;
   this->setObjectName("mainWindow");
 
   this->requestMsg = NULL;
 }
 
 /////////////////////////////////////////////////
+void MainWindow::Diagnostics()
+{
+#ifdef HAVE_QWT
+  gui::Diagnostics *diag = new gui::Diagnostics(this);
+  diag->show();
+#endif
+}
+
+/////////////////////////////////////////////////
 void MainWindow::SelectTopic()
 {
   TopicSelector *selector = new TopicSelector(this);
 {
   gui::editor::Events::exitBuildingEditor();
 }
+
 /////////////////////////////////////////////////
 void MainWindow::CreateActions()
 {
   g_topicVisAct->setStatusTip(tr("Select a topic to visualize"));
   connect(g_topicVisAct, SIGNAL(triggered()), this, SLOT(SelectTopic()));
 
+#ifdef HAVE_QWT
+  /*g_diagnosticsAct = new QAction(tr("Diagnostic Plot"), this);
+  g_diagnosticsAct->setShortcut(tr("Ctrl+U"));
+  g_diagnosticsAct->setStatusTip(tr("Plot diagnostic information"));
+  connect(g_diagnosticsAct, SIGNAL(triggered()), this, SLOT(Diagnostics()));
+  */
+#endif
+
   g_openAct = new QAction(tr("&Open World"), this);
   g_openAct->setShortcut(tr("Ctrl+O"));
   g_openAct->setStatusTip(tr("Open an world file"));
 {
   if (this->menuBar)
   {
-    delete menuBar;
+    this->menuLayout->removeWidget(this->menuBar);
+    delete this->menuBar;
   }
 
   this->menuBar = new QMenuBar;
   buildingEditorFileMenu->addAction(g_buildingEditorDoneAct);
   buildingEditorFileMenu->addAction(g_buildingEditorExitAct);
 
-  this->menuLayout->addWidget(this->menuBar);
+  this->menuLayout->setMenuBar(this->menuBar);
 }
 
 /////////////////////////////////////////////////
 {
   if (this->menuBar)
   {
-    delete menuBar;
+    this->menuLayout->removeWidget(this->menuBar);
+    delete this->menuBar;
   }
 
   this->menuBar =  new QMenuBar;
   windowMenu->addSeparator();
   windowMenu->addAction(g_dataLoggerAct);
 
+#ifdef HAVE_QWT
+  // windowMenu->addAction(g_diagnosticsAct);
+#endif
+
   this->menuBar->addSeparator();
 
   QMenu *helpMenu = this->menuBar->addMenu(tr("&Help"));
   helpMenu->addAction(g_aboutAct);
 
-  this->menuLayout->addWidget(this->menuBar);
+  this->menuLayout->setMenuBar(this->menuBar);
 }
 
 /////////////////////////////////////////////////

gazebo/gui/MainWindow.hh

       /// \brief Callback when topic selection action.
       private slots: void SelectTopic();
 
+      /// \brief Callback for diagnostics action.
+      private slots: void Diagnostics();
+
       private: void OnFullScreen(bool _value);
       private: void OnMoveMode(bool _mode);
 

gazebo/gui/RenderWidget.cc

 
   rendering::UserCameraPtr cam = this->glWidget->GetCamera();
 
-  if (!cam || !cam->IsInitialized())
+  if (!cam || !cam->GetInitialized())
   {
     event::Events::preRender();
     return;

gazebo/gui/style.qss

   color: #d0d0d0;
 }
 
+QFrame
+{
+  background-color: transparent;
+}
+
 QDialog
 {
   background-color: #303030;
 
 QWidget#topicSelector,
 QWidget#cameraSensor,
+QWidget#diagnosticsPlot,
 QWidget#dataLogger
 {
   background-color: #303030;
   border-radius: 4px;
 }
 
-
 QTreeView
 {
   color: #d0d0d0;
   margin: 0px;
   padding: 0px;
 }
+
+QwtTextLabel
+{
+  font-size: 10px;
+}
+
+QwtScaleWidget
+{
+  color: white;
+  font-size: 11px;
+}
+

gazebo/msgs/CMakeLists.txt

   contacts.proto
   contactsensor.proto
   cylindergeom.proto
+  diagnostics.proto
   factory.proto
   fog.proto
   friction.proto

gazebo/msgs/diagnostics.proto

+package gazebo.msgs;
+
+/// \ingroup gazebo_msgs
+/// \interface Diagnostic information 
+/// \brief Diagnostic information about a running instance of Gazebo.
+/// Gazebo must have been compiled with the ENABLE_DIAGNOSTICS flag.
+
+import "time.proto";
+
+message Diagnostics
+{
+  message DiagTime
+  {
+    required string name = 1;
+    required Time elapsed = 2;
+    required Time wall = 3;
+  }
+
+  repeated DiagTime time = 1;
+  required Time real_time = 2;
+  required Time sim_time = 3;
+  required double real_time_factor = 4;
+}

gazebo/msgs/world_stats.proto

   required uint64 iterations = 6;
   optional int32 model_count = 7;
 }
-
-

gazebo/physics/CMakeLists.txt

 
 target_link_libraries(gazebo_physics
   gazebo_common 
+  gazebo_util 
   gazebo_sdf_interface
   gazebo_physics_ode
   ${TBB_LIBRARIES} 
 endif()
 
 gz_add_executable(gzphysics server.cc)
-target_link_libraries(gzphysics gazebo_common
-                                gazebo_transport
-                                gazebo_physics
-                                gazebo_sensors
-                                gazebo_sdf_interface
-                                gazebo_msgs
-                                pthread
-                                )
+target_link_libraries(gzphysics
+  gazebo_common
+  gazebo_util 
+  gazebo_transport
+  gazebo_physics
+  gazebo_sensors
+  gazebo_sdf_interface
+  gazebo_msgs
+  pthread
+  )
 
 gz_install_executable( gzphysics )
 gz_install_library(gazebo_physics)

gazebo/physics/Joint.cc

   this->effortLimit[1] = -1;
   this->velocityLimit[0] = -1;
   this->velocityLimit[1] = -1;
+  this->inertiaRatio[0] = 0;
+  this->inertiaRatio[1] = 0;
 }
 
 //////////////////////////////////////////////////
   if (!this->childLink && childName != std::string("world"))
     gzthrow("Couldn't Find Child Link[" + childName  + "]");
 
-  this->LoadImpl(_sdf->GetValuePose("pose"));
+  this->anchorPose = _sdf->GetValuePose("pose");
+  this->LoadImpl(this->anchorPose);
 }
 
 /////////////////////////////////////////////////
       }
     }
   }
+  this->ComputeInertiaRatio();
 }
 
 //////////////////////////////////////////////////
 {
   _msg.set_name(this->GetScopedName());
 
-  if (this->sdf->HasElement("pose"))
-  {
-    msgs::Set(_msg.mutable_pose(),
-              this->sdf->GetValuePose("pose"));
-  }
-  else
-    msgs::Set(_msg.mutable_pose(), math::Pose(0, 0, 0, 0, 0, 0));
+  msgs::Set(_msg.mutable_pose(), this->anchorPose);
 
   if (this->HasType(Base::HINGE_JOINT))
   {
 }
 
 //////////////////////////////////////////////////
-double Joint::GetForce(int _index)
+double Joint::GetForce(unsigned int _index)
 {
-  if (_index >= 0 && static_cast<unsigned int>(_index) < this->GetAngleCount())
+  if (_index < this->GetAngleCount())
   {
     return this->forceApplied[_index];
   }
 }
 
 //////////////////////////////////////////////////
+double Joint::GetForce(int _index)
+{
+  return this->GetForce(static_cast<unsigned int>(_index));
+}
+
+//////////////////////////////////////////////////
 void Joint::ApplyDamping()
 {
   double dampingForce = -this->dampingCoefficient * this->GetVelocity(0);
   this->SetForce(0, dampingForce);
 }
+
+//////////////////////////////////////////////////
+void Joint::ComputeInertiaRatio()
+{
+  for (unsigned int i = 0; i < this->GetAngleCount(); ++i)
+  {
+    math::Vector3 axis = this->GetGlobalAxis(i);
+    if (this->parentLink && this->childLink)
+    {
+      physics::InertialPtr pi = this->parentLink->GetInertial();
+      physics::InertialPtr ci = this->childLink->GetInertial();
+      math::Matrix3 pm(
+       pi->GetIXX(), pi->GetIXY(), pi->GetIXZ(),
+       pi->GetIXY(), pi->GetIYY(), pi->GetIYZ(),
+       pi->GetIXZ(), pi->GetIYZ(), pi->GetIZZ());
+      math::Matrix3 cm(
+       ci->GetIXX(), ci->GetIXY(), ci->GetIXZ(),
+       ci->GetIXY(), ci->GetIYY(), ci->GetIYZ(),
+       ci->GetIXZ(), ci->GetIYZ(), ci->GetIZZ());
+
+      // rotate pm and cm into inertia frame
+      math::Pose pPose = this->parentLink->GetWorldPose();
+      math::Pose cPose = this->childLink->GetWorldPose();
+      for (unsigned col = 0; col < 3; ++col)
+      {
+        // get each column, and inverse rotate by pose
+        math::Vector3 pmCol(pm[0][col], pm[1][col], pm[2][col]);
+        pmCol = pPose.rot.RotateVector(pmCol);
+        pm.SetCol(col, pmCol);
+        math::Vector3 cmCol(cm[0][col], cm[1][col], cm[2][col]);
+        cmCol = pPose.rot.RotateVector(cmCol);
+        cm.SetCol(col, cmCol);
+      }
+
+      // matrix times axis
+      // \todo: add operator in Matrix3 class so we can do Matrix3 * Vector3
+      math::Vector3 pia(
+        pm[0][0] * axis.x + pm[0][1] * axis.y + pm[0][2] * axis.z,
+        pm[1][0] * axis.x + pm[1][1] * axis.y + pm[1][2] * axis.z,
+        pm[2][0] * axis.x + pm[2][1] * axis.y + pm[2][2] * axis.z);
+      math::Vector3 cia(
+        cm[0][0] * axis.x + cm[0][1] * axis.y + cm[0][2] * axis.z,
+        cm[1][0] * axis.x + cm[1][1] * axis.y + cm[1][2] * axis.z,
+        cm[2][0] * axis.x + cm[2][1] * axis.y + cm[2][2] * axis.z);
+      double piam = pia.GetLength();
+      double ciam = cia.GetLength();
+
+      // should we flip? sure, so the measure of ratio is between [1, +inf]
+      if (piam > ciam)
+        this->inertiaRatio[i] = piam/ciam;
+      else
+        this->inertiaRatio[i] = ciam/piam;
+    }
+  }
+}
+
+//////////////////////////////////////////////////
+double Joint::GetInertiaRatio(unsigned int _index) const
+{
+  if (_index < this->GetAngleCount())
+  {
+    return this->inertiaRatio[_index];
+  }
+  else
+  {
+    gzerr << "Invalid joint index [" << _index
+          << "] when trying to get inertia ratio across joint.\n";
+    return 0;
+  }
+}

gazebo/physics/Joint.hh

       /// not (lb-mass), etc.
       /// \param[in] _index Index of the axis.
       /// \return The force applied to an axis.
-      public: virtual double GetForce(int _index);
+      public: virtual double GetForce(int _index) GAZEBO_DEPRECATED;
+
+      /// \brief @todo: not yet implemented.
+      /// Get the forces applied at this Joint.
+      /// Note that the unit of force should be consistent with the rest
+      /// of the simulation scales.  E.g.  if you are using
+      /// metric units, the unit for force is Newtons.  If using
+      /// imperial units (sorry), then unit of force is lb-force
+      /// not (lb-mass), etc.
+      /// \param[in] _index Index of the axis.
+      /// \return The force applied to an axis.
+      public: virtual double GetForce(unsigned int _index);
 
       /// \brief get internal force and torque values at a joint
       /// Note that you must set
       /// \param[in] _index Force and torque on child link if _index = 0
       /// and on parent link of _index = 1
       /// \return The force and torque at the joint
-      public: virtual JointWrench GetForceTorque(int _index) = 0;
+      public: virtual JointWrench GetForceTorque(int _index)
+        GAZEBO_DEPRECATED = 0;
+
+      /// \brief get internal force and torque values at a joint
+      /// Note that you must set
+      ///   <provide_feedback>true<provide_feedback>
+      /// in the joint sdf to use this.
+      /// \param[in] _index Force and torque on child link if _index = 0
+      /// and on parent link of _index = 1
+      /// \return The force and torque at the joint
+      public: virtual JointWrench GetForceTorque(unsigned int _index) = 0;
 
       /// \brief Set the max allowed force of an axis(index).
       /// Note that the unit of force should be consistent with the rest
       /// \param[out] _msg Message to fill with this joint's properties.
       public: void FillMsg(msgs::Joint &_msg);
 
+      /// \brief Accessor to inertia ratio across this joint.
+      /// \param[in] _index Joint axis index.
+      /// \return returns the inertia ratio across specified joint axis.
+      public: double GetInertiaRatio(unsigned int _index) const;
+
       /// \brief Get the angle of an axis helper function.
       /// \param[in] _index Index of the axis.
       /// \return Angle of the axis.
       /// \param[in] _pos Position of the anchor.
       private: void LoadImpl(const math::Vector3 &_pos) GAZEBO_DEPRECATED;
 
+      /// \brief Computes inertiaRatio for this joint during Joint::Init
+      /// The inertia ratio for each joint between [1, +inf] gives a sense
+      /// of how well this model will perform in iterative LCP methods.
+      private: void ComputeInertiaRatio();
+
       /// \brief The first link this joint connects to
       protected: LinkPtr childLink;
 
       /// \brief Pointer to the parent model.
       protected: ModelPtr model;
 
-      /// \brief Anchor position of joint, expressed in world frame.
+      /// \brief Anchor pose.  This is the xyz offset of the joint frame from
+      /// child frame specified in the parent link frame
       protected: math::Vector3 anchorPos;
 
+      /// \brief Anchor pose specified in SDF <joint><pose> tag.
+      /// AnchorPose is the transform from child link frame to joint frame
+      /// specified in the child link frame.
+      /// AnchorPos is more relevant in normal usage, but sometimes,
+      /// we do need this (e.g. GetForceTorque and joint visualization).
+      protected: math::Pose anchorPose;
+
+      /// \brief Anchor link.
+      protected: LinkPtr anchorLink;
+
       /// \brief Joint update event.
       private: event::EventT<void ()> jointUpdate;
 
 
       /// \brief Store Joint velocity limit as specified in SDF
       protected: double velocityLimit[2];
+
+      /// \brief Store Joint inertia ratio.  This is a measure of how well
+      /// this model behaves using interative LCP solvers.
+      protected: double inertiaRatio[2];
     };
     /// \}
   }