John Hsu avatar John Hsu committed d9f94ec Merge

merging with default

Comments (0)

Files changed (87)

cmake/FindGooglePerfTools.cmake

+# -*- cmake -*-
+
+# - Find Google perftools
+# Find the Google perftools includes and libraries
+# This module defines
+#  GOOGLE_PERFTOOLS_INCLUDE_DIR, where to find heap-profiler.h, etc.
+#  GOOGLE_PERFTOOLS_FOUND, If false, do not try to use Google perftools.
+# also defined for general use are
+#  TCMALLOC_LIBRARIES, where to find the tcmalloc library.
+#  STACKTRACE_LIBRARIES, where to find the stacktrace library.
+#  PROFILER_LIBRARIES, where to find the profiler library.
+
+FIND_PATH(GOOGLE_PERFTOOLS_INCLUDE_DIR google/heap-profiler.h
+/usr/local/include
+/usr/include
+)
+
+SET(TCMALLOC_NAMES ${TCMALLOC_NAMES} tcmalloc)
+FIND_LIBRARY(TCMALLOC_LIBRARY
+  NAMES ${TCMALLOC_NAMES}
+  PATHS /usr/lib /usr/local/lib
+  )
+
+IF (TCMALLOC_LIBRARY AND GOOGLE_PERFTOOLS_INCLUDE_DIR)
+    SET(TCMALLOC_LIBRARIES ${TCMALLOC_LIBRARY})
+    SET(GOOGLE_PERFTOOLS_FOUND "YES")
+ELSE (TCMALLOC_LIBRARY AND GOOGLE_PERFTOOLS_INCLUDE_DIR)
+  SET(GOOGLE_PERFTOOLS_FOUND "NO")
+ENDIF (TCMALLOC_LIBRARY AND GOOGLE_PERFTOOLS_INCLUDE_DIR)
+
+SET(STACKTRACE_NAMES ${STACKTRACE_NAMES} stacktrace)
+FIND_LIBRARY(STACKTRACE_LIBRARY
+  NAMES ${STACKTRACE_LIBRARY}
+  PATHS /usr/lib /usr/local/lib
+  )
+
+IF (STACKTRACE_LIBRARY AND GOOGLE_PERFTOOLS_INCLUDE_DIR)
+    SET(STACKTRACE_LIBRARIES ${STACKTRACE_LIBRARY})
+ENDIF (STACKTRACE_LIBRARY AND GOOGLE_PERFTOOLS_INCLUDE_DIR)
+
+SET(PROFILER_NAMES ${PROFILER_NAMES} profiler)
+FIND_LIBRARY(PROFILER_LIBRARY
+  NAMES ${PROFILER_LIBRARY}
+  PATHS /usr/lib /usr/local/lib
+  )
+
+IF (PROFILER_LIBRARY AND GOOGLE_PERFTOOLS_INCLUDE_DIR)
+    SET(PROFILER_LIBRARIES ${PROFILER_LIBRARY})
+ENDIF (PROFILER_LIBRARY AND GOOGLE_PERFTOOLS_INCLUDE_DIR)
+
+IF (GOOGLE_PERFTOOLS_FOUND)
+   IF (NOT GOOGLE_PERFTOOLS_FIND_QUIETLY)
+      MESSAGE(STATUS "Found Google perftools: ${GOOGLE_PERFTOOLS_LIBRARIES}")
+   ENDIF (NOT GOOGLE_PERFTOOLS_FIND_QUIETLY)
+ELSE (GOOGLE_PERFTOOLS_FOUND)
+   IF (GOOGLE_PERFTOOLS_FIND_REQUIRED)
+      MESSAGE(FATAL_ERROR "Could not find Google perftools library")
+   ENDIF (GOOGLE_PERFTOOLS_FIND_REQUIRED)
+ENDIF (GOOGLE_PERFTOOLS_FOUND)
+
+MARK_AS_ADVANCED(
+  TCMALLOC_LIBRARY
+  STACKTRACE_LIBRARY
+  PROFILER_LIBRARY
+  GOOGLE_PERFTOOLS_INCLUDE_DIR
+  )

cmake/SearchForStuff.cmake

   RESULT_VARIABLE protobuf_modversion_failed)
 
 ########################################
+# 1. can not use BUILD_TYPE_PROFILE is defined after include this module
+# 2. TODO: TOUPPER is a hack until we fix the build system to support standard build names
+if (CMAKE_BUILD_TYPE)
+  string(TOUPPER ${CMAKE_BUILD_TYPE} TMP_CMAKE_BUILD_TYPE)
+  if ("${TMP_CMAKE_BUILD_TYPE}" STREQUAL "PROFILE")
+    include (${gazebo_cmake_dir}/FindGooglePerfTools.cmake)
+    if (GOOGLE_PERFTOOLS_FOUND)
+      message(STATUS "Include google-perftools")
+    else()
+      BUILD_ERROR("Need google/heap-profiler.h (libgoogle-perftools-dev) tools to compile in Profile mode")
+    endif()
+  endif()
+endif()
+
+########################################
 if (PROTOBUF_VERSION LESS 2.3.0)
   BUILD_ERROR("Incorrect version: Gazebo requires protobuf version 2.3.0 or greater")
 endif()

gazebo/common/Animation_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/math/Vector3.hh"
+#include "gazebo/math/Quaternion.hh"
+#include "gazebo/common/KeyFrame.hh"
+#include "gazebo/common/Animation.hh"
+
+using namespace gazebo;
+
+TEST(AnimationTest, PoseAnimation)
+{
+  {
+    common::PoseAnimation anim("test", 1.0, true);
+    anim.SetTime(-0.5);
+    EXPECT_DOUBLE_EQ(0.5, anim.GetTime());
+  }
+
+  {
+    common::PoseAnimation anim("test", 1.0, false);
+    anim.SetTime(-0.5);
+    EXPECT_DOUBLE_EQ(0.0, anim.GetTime());
+
+    anim.SetTime(1.5);
+    EXPECT_DOUBLE_EQ(1.0, anim.GetTime());
+  }
+
+
+  common::PoseAnimation anim("pose_test", 5.0, false);
+  common::PoseKeyFrame *key = anim.CreateKeyFrame(0.0);
+
+  EXPECT_DOUBLE_EQ(5.0, anim.GetLength());
+  anim.SetLength(10.0);
+  EXPECT_DOUBLE_EQ(10.0, anim.GetLength());
+
+  key->SetTranslation(math::Vector3(0, 0, 0));
+  EXPECT_TRUE(key->GetTranslation() == math::Vector3(0, 0, 0));
+
+  key->SetRotation(math::Quaternion(0, 0, 0));
+  EXPECT_TRUE(key->GetRotation() == math::Quaternion(0, 0, 0));
+
+  key = anim.CreateKeyFrame(10.0);
+  key->SetTranslation(math::Vector3(10, 20, 30));
+  EXPECT_TRUE(key->GetTranslation() == math::Vector3(10, 20, 30));
+
+  key->SetRotation(math::Quaternion(0.1, 0.2, 0.3));
+  EXPECT_TRUE(key->GetRotation() == math::Quaternion(0.1, 0.2, 0.3));
+
+  anim.AddTime(5.0);
+  EXPECT_DOUBLE_EQ(5.0, anim.GetTime());
+
+  anim.SetTime(4.0);
+  EXPECT_DOUBLE_EQ(4.0, anim.GetTime());
+
+  common::PoseKeyFrame interpolatedKey(-1.0);
+  anim.GetInterpolatedKeyFrame(interpolatedKey);
+  EXPECT_TRUE(interpolatedKey.GetTranslation() ==
+      math::Vector3(3.76, 7.52, 11.28));
+  EXPECT_TRUE(interpolatedKey.GetRotation() ==
+      math::Quaternion(0.0302776, 0.0785971, 0.109824));
+}
+
+TEST(AnimationTest, NumericAnimation)
+{
+  common::NumericAnimation anim("numeric_test", 10, false);
+  common::NumericKeyFrame *key = anim.CreateKeyFrame(0.0);
+
+  key->SetValue(0.0);
+  EXPECT_DOUBLE_EQ(0.0, key->GetValue());
+
+  key = anim.CreateKeyFrame(10.0);
+  key->SetValue(30);
+  EXPECT_DOUBLE_EQ(30, key->GetValue());
+
+  anim.AddTime(5.0);
+  EXPECT_DOUBLE_EQ(5.0, anim.GetTime());
+
+  anim.SetTime(4.0);
+  EXPECT_DOUBLE_EQ(4.0, anim.GetTime());
+
+  common::NumericKeyFrame interpolatedKey(0);
+  anim.GetInterpolatedKeyFrame(interpolatedKey);
+  EXPECT_DOUBLE_EQ(12, interpolatedKey.GetValue());
+}
+
+
+/////////////////////////////////////////////////
+int main(int argc, char **argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}

gazebo/common/CMakeLists.txt

   SystemPaths.hh
   Time.hh
   Timer.hh
+  UpdateInfo.hh
   Video.hh
  )
 
 endif()
 
 set (gtest_sources
+  Animation_TEST.cc
+  Color_TEST.cc
   Console_TEST.cc
+  Diagnostics_TEST.cc
+  Exception_TEST.cc
+  LogRecord_TEST.cc
+  Material_TEST.cc
+  Mesh_TEST.cc
+  Image_TEST.cc
+  SystemPaths_TEST.cc
+  Time_TEST.cc
 )
 gz_build_tests(${gtest_sources})
 

gazebo/common/Color_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/Color.hh"
+
+using namespace gazebo;
+
+TEST(Color, Color)
+{
+  common::Color clr(.1, .2, .3, 1.0);
+  EXPECT_FLOAT_EQ(0.1f, clr.r);
+  EXPECT_FLOAT_EQ(0.2f, clr.g);
+  EXPECT_FLOAT_EQ(0.3f, clr.b);
+  EXPECT_FLOAT_EQ(1.0f, clr.a);
+
+  clr.Reset();
+  EXPECT_FLOAT_EQ(0.0f, clr.r);
+  EXPECT_FLOAT_EQ(0.0f, clr.g);
+  EXPECT_FLOAT_EQ(0.0f, clr.b);
+  EXPECT_FLOAT_EQ(0.0f, clr.a);
+
+  clr.SetFromHSV(0, 0.5, 1.0);
+  EXPECT_FLOAT_EQ(1.0f, clr.r);
+  EXPECT_FLOAT_EQ(0.5f, clr.g);
+  EXPECT_FLOAT_EQ(0.5f, clr.b);
+  EXPECT_FLOAT_EQ(0.0f, clr.a);
+
+  EXPECT_TRUE(clr.GetAsHSV() == math::Vector3(6, 0.5, 1));
+
+  clr.SetFromHSV(60, 0.0, 1.0);
+  EXPECT_FLOAT_EQ(1.0f, clr.r);
+  EXPECT_FLOAT_EQ(1.0f, clr.g);
+  EXPECT_FLOAT_EQ(1.0f, clr.b);
+  EXPECT_FLOAT_EQ(0.0f, clr.a);
+
+  clr.SetFromHSV(120, 0.5, 1.0);
+  EXPECT_FLOAT_EQ(0.5f, clr.r);
+  EXPECT_FLOAT_EQ(1.0f, clr.g);
+  EXPECT_FLOAT_EQ(0.5f, clr.b);
+  EXPECT_FLOAT_EQ(0.0f, clr.a);
+
+  clr.SetFromHSV(180, 0.5, 1.0);
+  EXPECT_FLOAT_EQ(0.5f, clr.r);
+  EXPECT_FLOAT_EQ(1.0f, clr.g);
+  EXPECT_FLOAT_EQ(1.0f, clr.b);
+  EXPECT_FLOAT_EQ(0.0f, clr.a);
+
+  clr.SetFromHSV(240, 0.5, 1.0);
+  EXPECT_FLOAT_EQ(0.5f, clr.r);
+  EXPECT_FLOAT_EQ(0.5f, clr.g);
+  EXPECT_FLOAT_EQ(1.0f, clr.b);
+  EXPECT_FLOAT_EQ(0.0f, clr.a);
+
+  clr.SetFromHSV(300, 0.5, 1.0);
+  EXPECT_FLOAT_EQ(1.0f, clr[0]);
+  EXPECT_FLOAT_EQ(0.5f, clr[1]);
+  EXPECT_FLOAT_EQ(1.0f, clr[2]);
+  EXPECT_FLOAT_EQ(0.0f, clr[3]);
+  EXPECT_FLOAT_EQ(0.0f, clr[4]);
+
+  clr.r = 0.1;
+  clr.g = 0.2;
+  clr.b = 0.3;
+  clr.a = 0.4;
+  EXPECT_FLOAT_EQ(0.1f, clr[0]);
+  EXPECT_FLOAT_EQ(0.2f, clr[1]);
+  EXPECT_FLOAT_EQ(0.3f, clr[2]);
+  EXPECT_FLOAT_EQ(0.4f, clr[3]);
+
+  clr.Set(0.1, 0.2, 0.3, 0.4);
+  clr = clr + 0.2;
+  EXPECT_TRUE(clr == common::Color(0.3, 0.4, 0.5, 0.6));
+
+  clr.Set(0.1, 0.2, 0.3, 0.4);
+  clr += common::Color(0.2, 0.2, 0.2, 0.2);
+  EXPECT_TRUE(clr == common::Color(0.3, 0.4, 0.5, 0.6));
+
+
+  clr.Set(0.1, 0.2, 0.3, 0.4);
+  clr = clr - 0.1;
+  EXPECT_TRUE(clr == common::Color(0.0, 0.1, 0.2, 0.3));
+
+  clr.Set(0.1, 0.2, 0.3, 0.4);
+  clr -= common::Color(0.1, 0.1, 0.1, 0.1);
+  EXPECT_TRUE(clr == common::Color(0.0, 0.1, 0.2, 0.3));
+
+
+  clr.Set(1, 1, 1, 1.);
+  clr = clr / 1.6;
+  EXPECT_TRUE(clr == common::Color(0.625, 0.625, 0.625, 0.625));
+
+  clr.Set(1, 1, 1, 1);
+  clr /= common::Color(1, 1, 1, 1);
+  EXPECT_TRUE(clr == common::Color(1, 1, 1, 1));
+
+
+  clr.Set(.1, .2, .3, .4);
+  clr = clr * .1;
+  EXPECT_TRUE(clr == common::Color(0.01, 0.02, 0.03, 0.04));
+
+  clr.Set(.1, .2, .3, .4);
+  clr *= common::Color(0.1, 0.1, 0.1, 0.1);
+  EXPECT_TRUE(clr == common::Color(0.01, 0.02, 0.03, 0.04));
+
+
+  clr.SetFromYUV(0.5, 0.2, 0.8);
+  EXPECT_TRUE(math::equal(0.00553f, clr.r, 1e-3f));
+  EXPECT_TRUE(math::equal(0.0f, clr.g));
+  EXPECT_TRUE(math::equal(0.9064f, clr.b, 1e-3f));
+  EXPECT_TRUE(math::equal(0.04f, clr.a));
+
+  EXPECT_TRUE(clr.GetAsYUV() == math::Vector3(0.104985, 0.95227, 0.429305));
+
+  clr = common::Color(1.0, 0.0, 0.5, 1.0) + common::Color(0.1, 0.3, 0.4, 1.0);
+  EXPECT_TRUE(math::equal(0.00431373f, clr.r));
+  EXPECT_TRUE(math::equal(0.3f, clr.g));
+  EXPECT_TRUE(math::equal(0.9f, clr.b));
+  EXPECT_TRUE(math::equal(2.0f, clr.a));
+
+  clr = common::Color(1.0, 0.0, 0.5, 1.0) - common::Color(0.1, 0.3, 0.4, 1.0);
+  EXPECT_TRUE(math::equal(0.9f, clr.r));
+  EXPECT_TRUE(math::equal(0.0f, clr.g));
+  EXPECT_TRUE(math::equal(0.1f, clr.b));
+  EXPECT_TRUE(math::equal(0.0f, clr.a));
+
+  clr = common::Color(0.5, 0.2, 0.4, 0.6) / 2.0;
+  EXPECT_TRUE(math::equal(0.25f, clr.r));
+  EXPECT_TRUE(math::equal(0.1f, clr.g));
+  EXPECT_TRUE(math::equal(0.2f, clr.b));
+  EXPECT_TRUE(math::equal(0.3f, clr.a));
+}
+
+
+/////////////////////////////////////////////////
+int main(int argc, char **argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}

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/Events.cc

 EventT<void (std::string)> Events::deleteEntity;
 
 EventT<void ()> Events::worldUpdateStart;
+EventT<void (const common::UpdateInfo &)> Events::worldUpdateBegin;
+
 EventT<void ()> Events::worldUpdateEnd;
 
 EventT<void ()> Events::preRender;
 
 EventT<void (std::string)> Events::diagTimerStart;
 EventT<void (std::string)> Events::diagTimerStop;
+
+/////////////////////////////////////////////////
+void Events::DisconnectWorldUpdateStart(ConnectionPtr _subscriber)
+{
+  worldUpdateStart.Disconnect(_subscriber);
+}
+
+/////////////////////////////////////////////////
+void Events::DisconnectWorldUpdateBegin(ConnectionPtr _subscriber)
+{
+  worldUpdateBegin.Disconnect(_subscriber);
+}

gazebo/common/Events.hh

 #define _EVENTS_HH_
 
 #include <string>
-#include "common/Event.hh"
+
+#include "gazebo/common/Console.hh"
+#include "gazebo/common/UpdateInfo.hh"
+#include "gazebo/common/Event.hh"
 
 namespace gazebo
 {
       public: template<typename T>
               static ConnectionPtr ConnectPause(T _subscriber)
               { return pause.Connect(_subscriber); }
+
       /// \brief Disconnect a boost::slot the the pause signal
       /// \param[in] _subscriber the subscriber to this event
       public: static void DisconnectPause(ConnectionPtr _subscriber)
               { pause.Disconnect(_subscriber); }
+
       //////////////////////////////////////////////////////////////////////////
       /// \brief Connect a boost::slot the the step signal
       /// \param[in] _subscriber the subscriber to this event
       /// \return a connection
       public: template<typename T>
               static ConnectionPtr ConnectWorldUpdateStart(T _subscriber)
-              { return worldUpdateStart.Connect(_subscriber); }
+              {
+                // Putting in this comment so the deprecation message
+                // will be found easier: GAZEBO_DEPRECATED.
+                gzerr << "Events::ConnectWorldUpdateStart is deprecated "
+                      << "in v 1.5.0. Please use "
+                      << "Events::ConnectWorldUpdateBegin\n";
+                 return worldUpdateStart.Connect(_subscriber);
+              }
+
       /// \brief Disconnect a boost::slot the the world update start signal
       /// \param[in] _subscriber the subscriber to this event
       public: static void DisconnectWorldUpdateStart(ConnectionPtr _subscriber)
-              { worldUpdateStart.Disconnect(_subscriber); }
+              GAZEBO_DEPRECATED;
+
+      //////////////////////////////////////////////////////////////////////////
+      /// \brief Connect a boost::slot the the world update start signal
+      /// \param[in] _subscriber the subscriber to this event
+      /// \return a connection
+      public: template<typename T>
+              static ConnectionPtr ConnectWorldUpdateBegin(T _subscriber)
+              { return worldUpdateBegin.Connect(_subscriber); }
+
+      /// \brief Disconnect a boost::slot the the world update start signal
+      /// \param[in] _subscriber the subscriber to this event
+      public: static void DisconnectWorldUpdateBegin(
+                  ConnectionPtr _subscriber);
+
       //////////////////////////////////////////////////////////////////////////
       /// \brief Connect a boost::slot the the world update end signal
       /// \param[in] _subscriber the subscriber to this event
       /// \brief World update has started
       public: static EventT<void ()> worldUpdateStart;
 
+      /// \brief World update has started
+      public: static EventT<void (const common::UpdateInfo &)> worldUpdateBegin;
+
       /// \brief World update has ended
       public: static EventT<void ()> worldUpdateEnd;
 
Add a comment to this file

gazebo/common/Exception.cc

File contents unchanged.

gazebo/common/Exception_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/Assert.hh"
+#include "gazebo/common/Exception.hh"
+
+using namespace gazebo;
+
+TEST(Exception, Exception)
+{
+  try
+  {
+    gzthrow("test");
+  }
+  catch(common::Exception &_e)
+  {
+    std::cout << "Exception[" << _e.GetErrorFile() << "]\n";
+    std::cout << "Exception[" << _e.GetErrorStr() << "]\n";
+  }
+}
+
+TEST(Exception, InternalError_DefaultConstructor_Throw)
+{
+  ASSERT_THROW(
+    throw common::InternalError(),
+    common::InternalError);
+}
+
+TEST(Exception, InternalError_FileLineMsgConstructor_Throw)
+{
+  ASSERT_THROW(
+      throw common::InternalError(__FILE__, __LINE__, "test"),
+      common::InternalError);
+}
+
+TEST(Exception, InternalAssertionError_AssertionConstructor_Throw)
+{
+  ASSERT_THROW(
+      throw common::AssertionInternalError(__FILE__, __LINE__, "expression",
+                                          "function", "msg"),
+      common::AssertionInternalError);
+}
+
+#if defined(BOOST_DISABLE_ASSERTS) || defined(NDEBUG)
+TEST(Exception, GZASSERT_Disabled_NoThrow)
+{
+  ASSERT_NO_THROW(
+     GZ_ASSERT(true == false, "Assert thrown"));
+}
+#elif defined(BOOST_ENABLE_ASSERT_HANDLER)
+TEST(Exception, GZASSERT_WithHandler_ThrowException)
+{
+  ASSERT_THROW(
+    GZ_ASSERT(true == false, "Assert thrown"),
+               common::AssertionInternalError);
+}
+#else
+TEST(Exception, GZASSERT_Enabled_ThrowAssertion)
+{
+    ASSERT_DEATH(
+      GZ_ASSERT(true == false, "Assert thrown"),
+      ".*Internal Program Error - assertion.*");
+}
+#endif
+
+
+/////////////////////////////////////////////////
+int main(int argc, char **argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}

gazebo/common/Image_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/Image.hh"
+
+using namespace gazebo;
+
+TEST(ImageTest, Image)
+{
+  common::Image img;
+  EXPECT_EQ(-1, img.Load("/file/shouldn/never/exist.png"));
+  EXPECT_EQ(0, img.Load("file://media/materials/textures/wood.jpg"));
+  EXPECT_EQ(static_cast<unsigned int>(496), img.GetWidth());
+  EXPECT_EQ(static_cast<unsigned int>(329), img.GetHeight());
+  EXPECT_EQ(static_cast<unsigned int>(24), img.GetBPP());
+  EXPECT_TRUE(img.GetPixel(10, 10) ==
+      common::Color(0.133333, 0.376471, 0.654902, 1));
+  EXPECT_TRUE(img.GetAvgColor() ==
+      common::Color(0.260456, 0.506047, 0.758062, 1));
+  EXPECT_TRUE(img.GetMaxColor() ==
+      common::Color(0.807843, 0.909804, 0.964706, 1));
+  EXPECT_TRUE(img.Valid());
+  EXPECT_TRUE(img.GetFilename().find("materials/textures/wood.jpg") !=
+      std::string::npos);
+
+  unsigned char *data = NULL;
+  unsigned int size = 0;
+  img.GetData(&data, size);
+  EXPECT_EQ(static_cast<unsigned int>(489552), size);
+
+  img.SetFromData(data, img.GetWidth(), img.GetHeight(),
+                  common::Image::RGB_INT8);
+}
+
+
+/////////////////////////////////////////////////
+int main(int argc, char **argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}

gazebo/common/LogPlay.cc

   // Read in the header.
   this->ReadHeader();
 
+  this->logCurrXml = this->logStartXml;
   this->encoding.clear();
 }
 
 /////////////////////////////////////////////////
 bool LogPlay::Step(std::string &_data)
 {
-  if (!this->logCurrXml)
+  if (this->logCurrXml == this->logStartXml)
     this->logCurrXml = this->logStartXml->FirstChildElement("chunk");
+  else if (this->logCurrXml)
+    this->logCurrXml = this->logCurrXml->NextSiblingElement("chunk");
   else
-    this->logCurrXml = this->logCurrXml->NextSiblingElement("chunk");
+    return false;
 
   return this->GetChunkData(this->logCurrXml, _data);
 }

gazebo/common/LogRecord.cc

 
 #include "gazebo/math/Rand.hh"
 
+#include "gazebo/common/Assert.hh"
 #include "gazebo/common/Events.hh"
 #include "gazebo/common/Time.hh"
 #include "gazebo/common/Console.hh"
 //////////////////////////////////////////////////
 LogRecord::LogRecord()
 {
-  this->stop = true;
+  this->running = false;
+  this->paused = false;
   this->initialized = false;
+  this->stopThread = false;
+  this->firstUpdate = true;
 
   // Get the user's home directory
   // \todo getenv is not portable, and there is no generic cross-platform
   // method. Must check OS and choose a method
   char *homePath = getenv("HOME");
+  GZ_ASSERT(homePath, "HOME environment variable is missing");
 
   if (!homePath)
-    this->logPath = boost::filesystem::path("/tmp/gazebo");
+    this->logBasePath = boost::filesystem::path("/tmp/gazebo");
   else
-    this->logPath = boost::filesystem::path(homePath);
+    this->logBasePath = boost::filesystem::path(homePath);
 
-  this->logPath /= "/.gazebo/log/";
-
-  // Add the current time
-  boost::posix_time::ptime now = boost::posix_time::second_clock::local_time();
-  this->logPath /= boost::posix_time::to_iso_extended_string(now);
+  this->logBasePath /= "/.gazebo/log/";
 
   this->logsEnd = this->logs.end();
 }
 LogRecord::~LogRecord()
 {
   // Stop the write thread.
-  this->Stop();
-
-  // Delete all the log objects
-  for (Log_M::iterator iter = this->logs.begin();
-       iter != this->logs.end(); ++iter)
-  {
-    delete iter->second;
-  }
-
-  this->logs.clear();
+  this->Fini();
 }
 
 //////////////////////////////////////////////////
 bool LogRecord::Init(const std::string &_subdir)
 {
-  if (this->initialized)
+  if (_subdir.empty())
+  {
+    gzerr << "LogRecord initialization directory is empty." << std::endl;
     return false;
+  }
 
-  if (!_subdir.empty())
-    this->logPath /=  _subdir;
+  this->logSubDir = _subdir;
 
-  this->logsEnd = this->logs.end();
+  this->ClearLogs();
+
   this->initialized = true;
+  this->running = false;
+  this->paused = false;
+  this->stopThread = false;
+  this->firstUpdate = true;
 
   return true;
 }
 
-
 //////////////////////////////////////////////////
-void LogRecord::Start(const std::string &_encoding)
+bool LogRecord::Start(const std::string &_encoding)
 {
   boost::mutex::scoped_lock lock(this->controlMutex);
 
+  // Make sure ::Init has been called.
+  if (!this->initialized)
+  {
+    gzerr << "LogRecord has not been initialized." << std::endl;
+    return false;
+  }
+
   // Check to see if the logger is already started.
-  if (!this->stop)
-    return;
+  if (this->running)
+  {
+    /// \TODO replace this with gzlog
+    gzerr << "LogRecord has already been started" << std::endl;
+    return false;
+  }
+
+  // Get the current time as an ISO string.
+  std::string logTimeDir = common::Time::GetWallTimeAsISOString();
+
+  this->logCompletePath = this->logBasePath / logTimeDir / this->logSubDir;
+
+  // Create the log directory if necessary
+  if (!boost::filesystem::exists(this->logCompletePath))
+    boost::filesystem::create_directories(logCompletePath);
 
   if (_encoding != "bz2" && _encoding != "txt")
     gzthrow("Invalid log encoding[" + _encoding +
 
   this->encoding = _encoding;
 
-  // Create the log directory if necessary
-  if (!boost::filesystem::exists(this->logPath))
-    boost::filesystem::create_directories(this->logPath);
+  {
+    boost::mutex::scoped_lock logLock(this->writeMutex);
+    this->logsEnd = this->logs.end();
 
-  this->logsEnd = this->logs.end();
-
-  this->stop = false;
+    // Start all the logs
+    for (Log_M::iterator iter = this->logs.begin();
+         iter != this->logsEnd; ++iter)
+      iter->second->Start(logCompletePath);
+  }
 
   // Listen to the world update event
-  if (!this->updateConnection)
-  {
-    this->updateConnection =
-      event::Events::ConnectWorldUpdateStart(
-          boost::bind(&LogRecord::Update, this));
-  }
-  else
-  {
-    gzerr << "LogRecord has already been initialized\n";
-    return;
-  }
+  this->updateConnection = event::Events::ConnectWorldUpdateBegin(
+        boost::bind(&LogRecord::Update, this, _1));
 
-  // Start the logging thread
+  // Start the writing thread if it has not already been started
   if (!this->writeThread)
     this->writeThread = new boost::thread(boost::bind(&LogRecord::Run, this));
-  else
-  {
-    gzerr << "LogRecord has already been initialized\n";
-    return;
-  }
-}
 
-//////////////////////////////////////////////////
-bool LogRecord::GetRunning() const
-{
-  return !this->stop;
+  this->running = true;
+  this->paused = false;
+  this->firstUpdate = true;
+
+  this->startTime = this->currTime = common::Time();
+
+  return true;
 }
 
 //////////////////////////////////////////////////
 }
 
 //////////////////////////////////////////////////
-void LogRecord::Stop()
+void LogRecord::Fini()
 {
-  boost::mutex::scoped_lock lock(this->controlMutex);
+  this->stopThread = true;
 
-  this->stop = true;
-
-  // Kick the write thread
-  this->dataAvailableCondition.notify_one();
+  this->Stop();
 
   // Wait for the write thread, if it exists
   if (this->writeThread)
     this->writeThread->join();
   delete this->writeThread;
   this->writeThread = NULL;
+}
+
+//////////////////////////////////////////////////
+void LogRecord::Stop()
+{
+  boost::mutex::scoped_lock lock(this->controlMutex);
 
   // Disconnect from the world update signale
   if (this->updateConnection)
-    event::Events::DisconnectWorldUpdateStart(this->updateConnection);
+    event::Events::DisconnectWorldUpdateBegin(this->updateConnection);
   this->updateConnection.reset();
+
+  // Kick the write thread
+  this->dataAvailableCondition.notify_one();
+
+  // Remove all the logs.
+  this->ClearLogs();
+
+  // Reset the times
+  this->startTime = this->currTime = common::Time();
+
+  // Reset the flags
+  this->running = false;
+  this->paused = false;
+}
+
+//////////////////////////////////////////////////
+void LogRecord::ClearLogs()
+{
+  boost::mutex::scoped_lock logLock(this->writeMutex);
+
+  // Delete all the log objects
+  for (Log_M::iterator iter = this->logs.begin();
+      iter != this->logs.end(); ++iter)
+  {
+    delete iter->second;
+  }
+
+  this->logs.clear();
+  this->logsEnd = this->logs.end();
+}
+
+//////////////////////////////////////////////////
+void LogRecord::SetPaused(bool _paused)
+{
+  this->paused = _paused;
+}
+
+//////////////////////////////////////////////////
+bool LogRecord::GetPaused() const
+{
+  return this->paused;
+}
+
+//////////////////////////////////////////////////
+bool LogRecord::GetRunning() const
+{
+  return this->running;
 }
 
 //////////////////////////////////////////////////
 void LogRecord::Add(const std::string &_name, const std::string &_filename,
-                 boost::function<bool (std::ostringstream &)> _logCallback)
+                    boost::function<bool (std::ostringstream &)> _logCallback)
 {
-  boost::mutex::scoped_lock lock(this->controlMutex);
+  boost::mutex::scoped_lock logLock(this->writeMutex);
 
-  // Check to see if the logger is already started.
-  if (this->stop)
-    return;
+  // Check to see if the log has already been added.
+  if (this->logs.find(_name) != this->logs.end())
+  {
+    /// \todo Good place to use GZ_ASSERT
+    /// GZ_ASSERT(this->logs.find(_name)->second != NULL);
 
-  if (this->logs.find(_name) != this->logs.end())
-    gzthrow("Log file with name[" + _name + "] already exists.\n");
-
-  // Make the full path
-  boost::filesystem::path path = this->logPath / _filename;
-
-  // Make sure the file does not exist
-  if (boost::filesystem::exists(path))
-    gzthrow("Filename[" + path.string() + "], already exists\n");
+    if (this->logs.find(_name)->second->GetRelativeFilename() != _filename)
+    {
+      gzthrow(std::string("Attempting to add a duplicate log object named[")
+          + _name + "] with a filename of [" + _filename + "]\n");
+    }
+    else
+    {
+      return;
+    }
+  }
 
   LogRecord::Log *newLog;
 
   // Create a new log object
   try
   {
-    newLog = new LogRecord::Log(this, path.string(), _logCallback);
+    newLog = new LogRecord::Log(this, _filename, _logCallback);
   }
   catch(...)
   {
     gzthrow("Unable to create log. File permissions are probably bad.");
   }
 
+  if (this->running)
+    newLog->Start(this->logCompletePath);
+
   // Add the log to our map
   this->logs[_name] = newLog;
 
 //////////////////////////////////////////////////
 bool LogRecord::Remove(const std::string &_name)
 {
+  boost::mutex::scoped_lock logLock(this->writeMutex);
+
   bool result = false;
 
   Log_M::iterator iter = this->logs.find(_name);
 }
 
 //////////////////////////////////////////////////
-void LogRecord::Update()
+std::string LogRecord::GetFilename(const std::string &_name) const
 {
+  boost::mutex::scoped_lock logLock(this->writeMutex);
+
+  std::string result;
+
+  Log_M::const_iterator iter = this->logs.find(_name);
+  if (iter != this->logs.end())
+  {
+    /// \TODO GZ_ASSERT(iter->second);
+    result = iter->second->GetCompleteFilename();
+  }
+
+  return result;
+}
+
+//////////////////////////////////////////////////
+unsigned int LogRecord::GetFileSize(const std::string &_name) const
+{
+  unsigned int result = 0;
+
+  // Get the filename of the specified log object;
+  std::string filename = this->GetFilename(_name);
+
+  // Get the size of the log file on disk.
+  if (!filename.empty())
+  {
+    // Get the size of the file
+    if (!filename.empty() && boost::filesystem::exists(filename))
+      result = boost::filesystem::file_size(filename);
+  }
+
+  // Add in the contents of the write buffer. This is the data that will be
+  // written to disk soon.
   {
     boost::mutex::scoped_lock lock(this->writeMutex);
+    Log_M::const_iterator iter = this->logs.find(_name);
 
-    // Collect all the new log data. This will not write data to disk.
-    for (this->updateIter = this->logs.begin();
-         this->updateIter != this->logsEnd; ++this->updateIter)
+    if (iter != this->logs.end())
     {
-      this->updateIter->second->Update();
+      GZ_ASSERT(iter->second, "Log object is NULL");
+      result += iter->second->GetBufferSize();
     }
   }
 
-  // Signal that new data is available.
-  this->dataAvailableCondition.notify_one();
+  return result;
+}
+
+//////////////////////////////////////////////////
+void LogRecord::SetBasePath(const std::string &_path)
+{
+  // Make sure the  directory exists
+  if (!boost::filesystem::exists(_path))
+    boost::filesystem::create_directories(_path);
+
+  // Make sure we have a directory
+  if (!boost::filesystem::is_directory(_path))
+  {
+    gzerr << "Path " << _path << " is not a directory. Please only specify a "
+           << "directory for data logging.\n";
+    return;
+  }
+
+  // Make sure the path is writable.
+  // Note: This is not cross-platform compatible.
+  if (access(_path.c_str(), W_OK) != 0)
+  {
+    gzerr << "You do no have permission to write into " << _path << "\n";
+    return;
+  }
+
+  this->logBasePath = _path;
+}
+
+//////////////////////////////////////////////////
+std::string LogRecord::GetBasePath() const
+{
+  return this->logBasePath.string();
+}
+
+//////////////////////////////////////////////////
+bool LogRecord::GetFirstUpdate() const
+{
+  return this->firstUpdate;
+}
+
+//////////////////////////////////////////////////
+void LogRecord::Update(const common::UpdateInfo &_info)
+{
+  if (!this->paused)
+  {
+    unsigned int size = 0;
+
+    {
+      boost::mutex::scoped_lock lock(this->writeMutex);
+
+      // Collect all the new log data. This will not write data to disk.
+      for (this->updateIter = this->logs.begin();
+          this->updateIter != this->logsEnd; ++this->updateIter)
+      {
+        size += this->updateIter->second->Update();
+      }
+    }
+
+    if (this->firstUpdate)
+    {
+      this->firstUpdate = false;
+      this->startTime = _info.simTime;
+    }
+
+
+    // Signal that new data is available.
+    if (size > 0)
+      this->dataAvailableCondition.notify_one();
+
+    this->currTime = _info.simTime;
+  }
 }
 
 //////////////////////////////////////////////////
 void LogRecord::Run()
 {
+  if (!this->running)
+    gzerr << "Running LogRecord before it has been started." << std::endl;
+
+  this->stopThread = false;
+
   // This loop will write data to disk.
-  while (!this->stop)
+  while (!this->stopThread)
   {
     {
       // Wait for new data.
     }
 
     // Throttle the write loop.
-    common::Time::MSleep(2000);
+    common::Time::MSleep(1000);
   }
 }
 
 //////////////////////////////////////////////////
-LogRecord::Log::Log(LogRecord *_parent, const std::string &_filename,
+common::Time LogRecord::GetRunTime() const
+{
+  return this->currTime - this->startTime;
+}
+
+//////////////////////////////////////////////////
+LogRecord::Log::Log(LogRecord *_parent, const std::string &_relativeFilename,
                  boost::function<bool (std::ostringstream &)> _logCB)
 {
   this->parent = _parent;
   this->logCB = _logCB;
 
-  this->filename = _filename;
+  this->relativeFilename = _relativeFilename;
   std::ostringstream stream;
   stream << "<?xml version='1.0'?>\n"
          << "<gazebo_log>\n"
 }
 
 //////////////////////////////////////////////////
-void LogRecord::Log::Update()
+unsigned int LogRecord::Log::Update()
 {
   std::ostringstream stream;
 
+  // Get log data via the callback.
   if (this->logCB(stream) && !stream.str().empty())
   {
     const std::string encoding = this->parent->GetEncoding();
 
     this->buffer.append("</chunk>\n");
   }
+
+  return this->buffer.size();
 }
 
 //////////////////////////////////////////////////
 }
 
 //////////////////////////////////////////////////
+unsigned int LogRecord::Log::GetBufferSize()
+{
+  return this->buffer.size();
+}
+
+//////////////////////////////////////////////////
+std::string LogRecord::Log::GetRelativeFilename() const
+{
+  return this->relativeFilename;
+}
+
+//////////////////////////////////////////////////
+std::string LogRecord::Log::GetCompleteFilename() const
+{
+  return this->completePath.string();
+}
+
+//////////////////////////////////////////////////
+void LogRecord::Log::Start(const boost::filesystem::path &_path)
+{
+  // Make the full path for the log file
+  this->completePath = _path / this->relativeFilename;
+
+  // Make sure the file does not exist
+  if (boost::filesystem::exists(this->completePath))
+    gzthrow("Filename[" + this->completePath.string() + "], already exists\n");
+}
+
+//////////////////////////////////////////////////
 void LogRecord::Log::Write()
 {
   // Make sure the file is open for writing
   if (!this->logFile.is_open())
   {
     // Try to open it...
-    this->logFile.open(this->filename.c_str(),
+    this->logFile.open(this->completePath.string().c_str(),
                        std::fstream::out | std::ios::binary);
 
     // Throw an error if we couldn't open the file for writing.
     if (!this->logFile.is_open())
-      gzthrow("Unable to open file for logging:" + this->filename + "]");
+      gzthrow("Unable to open file for logging:" +
+              this->completePath.string() + "]");
   }
 
+  // Check to see if the log file still exists on disk. This will catch the
+  // case when someone deletes a log file while recording.
+  if (!boost::filesystem::exists(this->completePath.string().c_str()))
+  {
+    gzerr << "Log file[" << this->completePath << "] no longer exists. "
+          << "Unable to write log data.\n";
+
+    // We have to clear the buffer, or else it may grow indefinitely.
+    this->buffer.clear();
+    return;
+  }
+
+
   // Write out the contents of the buffer.
   this->logFile.write(this->buffer.c_str(), this->buffer.size());
   this->logFile.flush();

gazebo/common/LogRecord.hh

 #include <boost/archive/iterators/ostream_iterator.hpp>
 #include <boost/filesystem.hpp>
 
-#include "common/Event.hh"
-#include "common/SingletonT.hh"
+#include "gazebo/common/UpdateInfo.hh"
+#include "gazebo/common/Event.hh"
+#include "gazebo/common/SingletonT.hh"
 
 #define GZ_LOG_VERSION "1.0"
 
       /// \brief Stop the logger.
       public: void Stop();
 
+      /// \brief Set whether logging should pause. A paused state means the
+      /// log file is still open, but data is not written to it.
+      /// \param[in] _paused True to pause data logging.
+      /// \sa LogRecord::GetPaused
+      public: void SetPaused(bool _paused);
+
+      /// \brief Get whether logging is paused.
+      /// \return True if logging is paused.
+      /// \sa LogRecord::SetPaused
+      public: bool GetPaused() const;
+
+      /// \brief Get whether logging is running.
+      /// \return True if logging has been started.
+      public: bool GetRunning() const;
+
       /// \brief Start the logger.
       /// \param[in] _encoding The type of encoding (txt, or bz2).
-      public: void Start(const std::string &_encoding="bz2");
+      public: bool Start(const std::string &_encoding="bz2");
 
       /// \brief Get the encoding used.
       /// \return Either [txt, or bz2], where txt is plain txt and bz2 is
       /// bzip2 compressed data with Base64 encoding.
       public: const std::string &GetEncoding() const;
 
-      /// \brief Return true if running.
-      /// \return True if LogRecord has been started.
-      public: bool GetRunning() const;
+      /// \brief Get the filename for a log object.
+      /// \param[in] _name Name of the log object.
+      /// \return Filename, empty string if not found.
+      public: std::string GetFilename(const std::string &_name) const;
+
+      /// \brief Get the file size for a log object.
+      /// \param[in] _name Name of the log object.
+      /// \return Size in bytes.
+      public: unsigned int GetFileSize(const std::string &_name) const;
+
+      /// \brief Set the base path.
+      /// \param[in] _path Path to the new logging location.
+      public: void SetBasePath(const std::string &_path);
+
+      /// \brief Get the base path for a log recording.
+      /// \return Path for log recording.
+      public: std::string GetBasePath() const;
+
+      /// \brief Get the run time in sim time.
+      /// \return Run sim time.
+      public: common::Time GetRunTime() const;
+
+      /// \brief Finialize, and shutdown.
+      public: void Fini();
+
+      /// \brief Return true if an Update has not yet been completed.
+      /// \return True if an Update has not yet been completed.
+      public: bool GetFirstUpdate() const;
 
       /// \brief Update the log files
       ///
       /// Captures the current state of all registered entities, and outputs
       /// the data to their respective log files.
-      private: void Update();
+      private: void Update(const common::UpdateInfo &_info);
 
       /// \brief Run the Write loop.
       private: void Run();
 
+      /// \brief Clear and delete the log buffers.
+      private: void ClearLogs();
+
       /// \brief Write the header to file.
       // private: void WriteHeader();
 
       /// \cond
       private: class Log
       {
-        public: Log(LogRecord *_parent, const std::string &_filename,
+        /// \brief Constructor
+        /// \param[in] _parent Pointer to the LogRecord parent.
+        /// \param[in] _relativeFilename The name of the log file to
+        /// generate, sans the complete path.
+        /// \param[in] _logCB Callback function, which is used to get log
+        /// data.
+        public: Log(LogRecord *_parent, const std::string &_relativeFilename,
                     boost::function<bool (std::ostringstream &)> _logCB);
 
+        /// \brief Destructor
         public: virtual ~Log();
 
+        /// \brief Start the log.
+        /// \param[in] _path The complete path in which to put the log file.
+        public: void Start(const boost::filesystem::path &_path);
+
+        /// \brief Write data to disk.
         public: void Write();
 
-        public: void Update();
+        /// \brief Update the data buffer.
+        /// \return The size of the data buffer.
+        public: unsigned int Update();
 
+        /// \brief Clear the data buffer.
         public: void ClearBuffer();
 
+        /// \brief Get the byte size of the buffer.
+        /// \return Buffer byte size.
+        public: unsigned int GetBufferSize();
+
+        /// \brief Get the relative filename. This is the filename passed
+        /// to the constructor.
+        /// \return The relative filename.
+        public: std::string GetRelativeFilename() const;
+
+        /// \brief Get the complete filename.
+        /// \return The complete filename.
+        public: std::string GetCompleteFilename() const;
+
+        /// \brief Pointer to the log record parent.
         public: LogRecord *parent;
+
+        /// \brief Callback from which to get data.
         public: boost::function<bool (std::ostringstream &)> logCB;
+
+        /// \brief Data buffer.
         public: std::string buffer;
+
+        /// \brief The log file.
         public: std::ofstream logFile;
-        public: std::string filename;
+
+        /// \brief Relative log filename.
+        public: std::string relativeFilename;
+
+        private: boost::filesystem::path completePath;
       };
       /// \endcond
 
       /// \brief Event connected to the World update.
       private: event::ConnectionPtr updateConnection;
 
-      /// \brief True if logging is stopped.
-      private: bool stop;
+      /// \brief True if logging is running.
+      private: bool running;
 
       /// \brief Thread used to write data to disk.
       private: boost::thread *writeThread;
 
       /// \brief Mutext to protect writing.
-      private: boost::mutex writeMutex;
+      private: mutable boost::mutex writeMutex;
 
       /// \brief Mutex to protect logging control.
       private: boost::mutex controlMutex;
       private: boost::condition_variable dataAvailableCondition;
 
       /// \brief The base pathname for all the logs.
-      private: boost::filesystem::path logPath;
+      private: boost::filesystem::path logBasePath;
+
+      /// \brief The complete pathname for all the logs.
+      private: boost::filesystem::path logCompletePath;
+
+      /// \brief Subdirectory for log files. This is appended to
+      /// logBasePath.
+      private: std::string logSubDir;
 
       /// \brief Encoding format for each chunk.
       private: std::string encoding;
       /// \brief True if initialized.
       private: bool initialized;
 
+      /// \brief True to pause recording.
+      private: bool paused;
+
+      /// \brief Used to indicate the first update callback.
+      private: bool firstUpdate;
+
+      /// \brief Flag used to stop the write thread.
+      private: bool stopThread;
+
+      /// \brief Start simulation time.
+      private: common::Time startTime;
+
+      /// \brief Current simulation time.
+      private: common::Time currTime;
+
       /// \brief This is a singleton
       private: friend class SingletonT<LogRecord>;
     };

gazebo/common/LogRecord_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 <boost/filesystem.hpp>
+
+#include "gazebo/common/Exception.hh"
+#include "gazebo/common/LogRecord.hh"
+
+/////////////////////////////////////////////////
+/// \brief Test LogRecord constructor and a few accessors
+TEST(LogRecord_TEST, Constructor)
+{
+  gazebo::common::LogRecord *recorder = gazebo::common::LogRecord::Instance();
+
+  char *homePath = getenv("HOME");
+  EXPECT_TRUE(homePath != NULL);
+
+  boost::filesystem::path logPath = "/tmp/gazebo";
+  if (homePath)
+    logPath = boost::filesystem::path(homePath);
+  logPath /= "/.gazebo/log/";
+
+  // Make sure the log path is correct
+  EXPECT_EQ(recorder->GetBasePath(), logPath.string());
+
+  EXPECT_FALSE(recorder->GetPaused());
+  EXPECT_FALSE(recorder->GetRunning());
+  EXPECT_TRUE(recorder->GetFirstUpdate());
+
+  // Init without a subdirectory
+  EXPECT_FALSE(recorder->Init(""));
+}
+
+/////////////////////////////////////////////////
+/// \brief Test LogRecord Start errors
+TEST(LogRecord_TEST, StartErrors)
+{
+  gazebo::common::LogRecord *recorder = gazebo::common::LogRecord::Instance();
+
+  // Start without an init
+  {
+    EXPECT_FALSE(recorder->Start("bz2"));
+  }
+
+  // Invalid encoding
+  {
+    EXPECT_TRUE(recorder->Init("test"));
+    EXPECT_THROW(recorder->Start("garbage"), gazebo::common::Exception);
+  }
+
+  // Double start
+  {
+    EXPECT_TRUE(recorder->Start("bz2"));
+    EXPECT_TRUE(recorder->GetRunning());
+    EXPECT_FALSE(recorder->Start("bz2"));
+  }
+}
+
+/////////////////////////////////////////////////
+/// \brief Test LogRecord Init and Start
+TEST(LogRecord_TEST, Start)
+{
+  gazebo::common::LogRecord *recorder = gazebo::common::LogRecord::Instance();
+  recorder->Init("test");
+  recorder->Start("bz2");
+
+  // Make sure the right flags have been set
+  EXPECT_FALSE(recorder->GetPaused());
+  EXPECT_TRUE(recorder->GetRunning());
+  EXPECT_TRUE(recorder->GetFirstUpdate());
+
+  // Make sure the right encoding is set
+  EXPECT_EQ(recorder->GetEncoding(), std::string("bz2"));
+
+  // Make sure the log directories exist
+  EXPECT_TRUE(boost::filesystem::exists(recorder->GetBasePath()));
+  EXPECT_TRUE(boost::filesystem::is_directory(recorder->GetBasePath()));
+
+  // Run time should be zero since no update has been triggered.
+  EXPECT_EQ(recorder->GetRunTime(), gazebo::common::Time());
+
+  // Stop recording.
+  recorder->Stop();
+
+  // Make sure everything has reset.
+  EXPECT_FALSE(recorder->GetRunning());
+  EXPECT_FALSE(recorder->GetPaused());
+  EXPECT_EQ(recorder->GetRunTime(), gazebo::common::Time());
+}
+
+/////////////////////////////////////////////////
+int main(int argc, char **argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}

gazebo/common/Material_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/Material.hh"
+
+using namespace gazebo;
+
+TEST(MaterialTest, Material)
+{
+  common::Material mat(common::Color(1.0, 0.5, 0.2, 1.0));
+  EXPECT_TRUE(mat.GetAmbient() == common::Color(1.0, 0.5, 0.2, 1.0));
+  EXPECT_TRUE(mat.GetDiffuse() == common::Color(1.0, 0.5, 0.2, 1.0));
+  EXPECT_STREQ("gazebo_material_0", mat.GetName().c_str());
+
+  mat.SetTextureImage("texture_image");
+  EXPECT_STREQ("texture_image", mat.GetTextureImage().c_str());
+
+  mat.SetTextureImage("texture_image", "/path");
+  EXPECT_STREQ("/path/../materials/textures/texture_image",
+               mat.GetTextureImage().c_str());
+
+  mat.SetAmbient(common::Color(0.1, 0.2, 0.3, 0.4));
+  EXPECT_TRUE(mat.GetAmbient() == common::Color(0.1, 0.2, 0.3, 0.4));
+
+  mat.SetDiffuse(common::Color(0.1, 0.2, 0.3, 0.4));
+  EXPECT_TRUE(mat.GetDiffuse() == common::Color(0.1, 0.2, 0.3, 0.4));
+
+  mat.SetSpecular(common::Color(0.1, 0.2, 0.3, 0.4));
+  EXPECT_TRUE(mat.GetSpecular() == common::Color(0.1, 0.2, 0.3, 0.4));
+
+  mat.SetEmissive(common::Color(0.1, 0.2, 0.3, 0.4));
+  EXPECT_TRUE(mat.GetEmissive() == common::Color(0.1, 0.2, 0.3, 0.4));
+
+  mat.SetTransparency(0.2);
+  EXPECT_DOUBLE_EQ(0.2, mat.GetTransparency());
+
+  mat.SetShininess(0.2);
+  EXPECT_DOUBLE_EQ(0.2, mat.GetShininess());
+
+  mat.SetBlendFactors(.1, .5);
+  double a, b;
+  mat.GetBlendFactors(a, b);
+  EXPECT_DOUBLE_EQ(.1, a);
+  EXPECT_DOUBLE_EQ(0.5, b);
+
+  mat.SetBlendMode(common::Material::MODULATE);
+  EXPECT_EQ(common::Material::MODULATE, mat.GetBlendMode());
+
+  mat.SetShadeMode(common::Material::BLINN);
+  EXPECT_EQ(common::Material::BLINN, mat.GetShadeMode());
+
+  mat.SetPointSize(0.2);
+  EXPECT_DOUBLE_EQ(0.2, mat.GetPointSize());
+
+  mat.SetDepthWrite(false);
+  EXPECT_FALSE(mat.GetDepthWrite());
+
+  mat.SetLighting(true);
+  EXPECT_TRUE(mat.GetLighting());
+}
+
+/////////////////////////////////////////////////
+int main(int argc, char **argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}

gazebo/common/Mesh_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/math/Vector3.hh"
+#include "gazebo/common/Exception.hh"
+#include "gazebo/common/MeshManager.hh"
+#include "gazebo/common/Mesh.hh"
+
+using namespace gazebo;
+
+std::string asciiSTLBox =
+"solid MYSOLID\n\
+  facet normal  0.0   0.0  -1.0\n\
+    outer loop\n\
+      vertex    0.0   0.0   0.0\n\
+      vertex    1.0   1.0   0.0\n\
+      vertex    1.0   0.0   0.0\n\
+    endloop\n\
+  endfacet\n\
+  facet normal  0.0   0.0  -1.0\n\
+    outer loop\n\
+      vertex    0.0   0.0   0.0\n\
+      vertex    0.0   1.0   0.0\n\
+      vertex    1.0   1.0   0.0\n\
+    endloop\n\
+  endfacet\n\
+  facet normal -1.0   0.0   0.0\n\
+    outer loop\n\
+      vertex    0.0   0.0   0.0\n\
+      vertex    0.0   1.0   1.0\n\
+      vertex    0.0   1.0   0.0\n\
+    endloop\n\
+  endfacet\n\
+  facet normal -1.0   0.0   0.0\n\
+    outer loop\n\
+      vertex    0.0   0.0   0.0\n\
+      vertex    0.0   0.0   1.0\n\
+      vertex    0.0   1.0   1.0\n\
+    endloop\n\
+  endfacet\n\
+  facet normal  0.0   1.0   0.0\n\
+    outer loop\n\
+      vertex    0.0   1.0   0.0\n\
+      vertex    1.0   1.0   1.0\n\
+      vertex    1.0   1.0   0.0\n\
+    endloop\n\
+  endfacet\n\
+  facet normal  0.0   1.0   0.0\n\
+    outer loop\n\
+      vertex    0.0   1.0   0.0\n\
+      vertex    0.0   1.0   1.0\n\
+      vertex    1.0   1.0   1.0\n\
+    endloop\n\
+  endfacet\n\
+  facet normal  1.0   0.0   0.0\n\
+    outer loop\n\
+      vertex    1.0   0.0   0.0\n\
+      vertex    1.0   1.0   0.0\n\
+      vertex    1.0   1.0   1.0\n\
+    endloop\n\
+  endfacet\n\
+  facet normal  1.0   0.0   0.0\n\
+    outer loop\n\
+      vertex    1.0   0.0   0.0\n\
+      vertex    1.0   1.0   1.0\n\
+      vertex    1.0   0.0   1.0\n\
+    endloop\n\
+  endfacet\n\
+  facet normal  0.0  -1.0   0.0\n\
+    outer loop\n\
+      vertex    0.0   0.0   0.0\n\
+      vertex    1.0   0.0   0.0\n\
+      vertex    1.0   0.0   1.0\n\
+    endloop\n\
+  endfacet\n\
+  facet normal  0.0  -1.0   0.0\n\
+    outer loop\n\
+      vertex    0.0   0.0   0.0\n\
+      vertex    1.0   0.0   1.0\n\
+      vertex    0.0   0.0   1.0\n\
+    endloop\n\
+  endfacet\n\
+  facet normal  0.0   0.0   1.0\n\
+    outer loop\n\
+      vertex    0.0   0.0   1.0\n\
+      vertex    1.0   0.0   1.0\n\
+      vertex    1.0   1.0   1.0\n\
+    endloop\n\
+  endfacet\n\
+  facet normal  0.0   0.0   1.0\n\
+    outer loop\n\
+      vertex    0.0   0.0   1.0\n\
+      vertex    1.0   1.0   1.0\n\
+      vertex    0.0   1.0   1.0\n\
+    endloop\n\
+  endfacet\n\
+endsolid MYSOLID";
+
+
+TEST(MeshTest, Mesh)
+{
+  EXPECT_EQ(NULL, common::MeshManager::Instance()->Load("break.mesh"));
+  EXPECT_EQ(NULL, common::MeshManager::Instance()->Load("break.3ds"));
+  EXPECT_EQ(NULL, common::MeshManager::Instance()->Load("break.xml"));
+
+  const common::Mesh *mesh =
+    common::MeshManager::Instance()->GetMesh("unit_box");
+  EXPECT_EQ(static_cast<unsigned int>(24), mesh->GetVertexCount());
+  EXPECT_EQ(static_cast<unsigned int>(24), mesh->GetNormalCount());
+  EXPECT_EQ(static_cast<unsigned int>(36), mesh->GetIndexCount());
+  EXPECT_EQ(static_cast<unsigned int>(24), mesh->GetTexCoordCount());
+  EXPECT_EQ(static_cast<unsigned int>(0), mesh->GetMaterialCount());
+
+  math::Vector3 center, min, max;
+  mesh->GetAABB(center, min, max);
+  EXPECT_TRUE(center == math::Vector3(0, 0, 0));
+  EXPECT_TRUE(min == math::Vector3(-.5, -.5, -.5));
+  EXPECT_TRUE(max == math::Vector3(.5, .5, .5));
+
+
+  float *vertArray = NULL;
+  int *indArray = NULL;
+  mesh->FillArrays(&vertArray, &indArray);
+
+  int i = 0;
+  EXPECT_FLOAT_EQ(.5, vertArray[i++]);
+  EXPECT_FLOAT_EQ(-.5, vertArray[i++]);
+  EXPECT_FLOAT_EQ(.5, vertArray[i++]);
+
+  EXPECT_FLOAT_EQ(-.5, vertArray[i++]);
+  EXPECT_FLOAT_EQ(-.5, vertArray[i++]);
+  EXPECT_FLOAT_EQ(.5, vertArray[i++]);
+
+  EXPECT_FLOAT_EQ(-.5, vertArray[i++]);
+  EXPECT_FLOAT_EQ(-.5, vertArray[i++]);
+  EXPECT_FLOAT_EQ(-.5, vertArray[i++]);
+
+  EXPECT_FLOAT_EQ(.5, vertArray[i++]);
+  EXPECT_FLOAT_EQ(-.5, vertArray[i++]);
+  EXPECT_FLOAT_EQ(-.5, vertArray[i++]);
+
+  EXPECT_FLOAT_EQ(-.5, vertArray[i++]);
+  EXPECT_FLOAT_EQ(.5, vertArray[i++]);
+  EXPECT_FLOAT_EQ(.5, vertArray[i++]);
+
+  EXPECT_FLOAT_EQ(.5, vertArray[i++]);
+  EXPECT_FLOAT_EQ(.5, vertArray[i++]);
+  EXPECT_FLOAT_EQ(.5, vertArray[i++]);
+
+  EXPECT_FLOAT_EQ(.5, vertArray[i++]);
+  EXPECT_FLOAT_EQ(.5, vertArray[i++]);
+  EXPECT_FLOAT_EQ(-.5, vertArray[i++]);
+
+  EXPECT_FLOAT_EQ(-.5, vertArray[i++]);
+  EXPECT_FLOAT_EQ(.5, vertArray[i++]);
+  EXPECT_FLOAT_EQ(-.5, vertArray[i++]);
+
+  common::Mesh *newMesh = new common::Mesh();
+  newMesh->SetName("testBox");
+  common::SubMesh *subMesh = new common::SubMesh();
+  newMesh->AddSubMesh(subMesh);
+
+  std::vector<math::Vector3> verts;
+  std::vector<math::Vector3> norms;
+
+  EXPECT_THROW(mesh->GetSubMesh(1), common::Exception);
+
+  for (i = 0; i < 24; ++i)
+  {
+    verts.push_back(mesh->GetSubMesh(0)->GetVertex(i));
+    norms.push_back(mesh->GetSubMesh(0)->GetNormal(i));
+  }
+
+  subMesh->CopyVertices(verts);
+  subMesh->CopyNormals(norms);
+  EXPECT_TRUE(subMesh->HasVertex(math::Vector3(-.5, -.5, -.5)));
+  EXPECT_FALSE(subMesh->HasVertex(math::Vector3(0, 0, 0)));
+
+  newMesh->GetAABB(center, min, max);
+  EXPECT_TRUE(center == math::Vector3(0, 0, 0));
+  EXPECT_TRUE(min == math::Vector3(-.5, -.5, -.5));
+  EXPECT_TRUE(max == math::Vector3(.5, .5, .5));
+
+  subMesh->SetVertexCount(1);
+  subMesh->SetIndexCount(1);
+  subMesh->SetNormalCount(1);
+  subMesh->SetTexCoordCount(1);
+
+  EXPECT_EQ(static_cast<unsigned int>(1), subMesh->GetVertexCount());
+  EXPECT_EQ(static_cast<unsigned int>(1), subMesh->GetIndexCount());
+  EXPECT_EQ(static_cast<unsigned int>(1), subMesh->GetNormalCount());
+  EXPECT_EQ(static_cast<unsigned int>(1), subMesh->GetTexCoordCount());
+
+  subMesh->SetVertex(0, math::Vector3(1, 2, 3));
+  EXPECT_TRUE(subMesh->GetVertex(0) == math::Vector3(1, 2, 3));
+
+  subMesh->SetTexCoord(0, math::Vector2d(.1, .2));
+  EXPECT_TRUE(subMesh->GetTexCoord(0) == math::Vector2d(.1, .2));
+
+  newMesh->GenSphericalTexCoord(math::Vector3(0, 0, 0));
+  delete newMesh;
+
+  std::ofstream stlFile("/tmp/gazebo_stl_test.stl", std::ios::out);
+  stlFile << asciiSTLBox;
+  stlFile.close();
+
+  mesh = common::MeshManager::Instance()->Load("/tmp/gazebo_stl_test-bad.stl");
+  EXPECT_EQ(NULL, mesh);
+
+  mesh = common::MeshManager::Instance()->Load("/tmp/gazebo_stl_test.stl");
+  mesh->GetAABB(center, min, max);
+  EXPECT_TRUE(center == math::Vector3(0.5, 0.5, 0.5));
+  EXPECT_TRUE(min == math::Vector3(0, 0, 0));
+  EXPECT_TRUE(max == math::Vector3(1, 1, 1));
+}
+
+
+/////////////////////////////////////////////////
+int main(int argc, char **argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}

gazebo/common/ModelDatabase.cc

     if (success != CURLE_OK)
     {
       gzwarn << "Unable to connect to model database using [" << _uri
-        << "]. Only locally installed models will be available.";
+        << "]. Only locally installed models will be available.\n";
     }
 
     curl_easy_cleanup(curl);

gazebo/common/SystemPaths_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/SystemPaths.hh"
+
+using namespace gazebo;
+
+TEST(SystemPathsTest, SystemPaths)
+{
+  std::string gazeboResourcePathBackup = "GAZEBO_RESOURCE_PATH=";
+  std::string ogreResourcePathBackup = "OGRE_RESOURCE_PATH=";
+  std::string pluginPathBackup = "GAZEBO_PLUGIN_PATH=";
+
+  if (getenv("GAZEBO_RESOURCE_PATH"))
+    gazeboResourcePathBackup += getenv("GAZEBO_RESOURCE_PATH");
+
+  if (getenv("GAZEBO_RESOURCE_PATH"))
+    ogreResourcePathBackup += getenv("GAZEBO_RESOURCE_PATH");
+
+  if (getenv("GAZEBO_PLUGIN_PATH"))
+    pluginPathBackup += getenv("GAZEBO_PLUGIN_PATH");
+
+  putenv(const_cast<char*>("GAZEBO_LOG_PATH="));
+  common::SystemPaths *paths = common::SystemPaths::Instance();
+
+  paths->ClearGazeboPaths();
+  paths->ClearOgrePaths();
+  paths->ClearPluginPaths();
+
+  putenv(const_cast<char*>("GAZEBO_RESOURCE_PATH=/tmp/resource:/test/me/now"));
+  const std::list<std::string> pathList1 = paths->GetGazeboPaths();
+  EXPECT_EQ(static_cast<unsigned int>(2), pathList1.size());
+  EXPECT_STREQ("/tmp/resource", pathList1.front().c_str());
+  EXPECT_STREQ("/test/me/now", pathList1.back().c_str());
+
+  putenv(const_cast<char*>("OGRE_RESOURCE_PATH=/tmp/ogre:/test/ogre/now"));
+  const std::list<std::string> pathList2 = paths->GetOgrePaths();
+  EXPECT_EQ(static_cast<unsigned int>(2), pathList2.size());
+  EXPECT_STREQ("/tmp/ogre", pathList2.front().c_str());
+  EXPECT_STREQ("/test/ogre/now", pathList2.back().c_str());
+
+  putenv(const_cast<char*>("GAZEBO_PLUGIN_PATH=/tmp/plugin:/test/plugin/now"));
+  const std::list<std::string> pathList3 = paths->GetPluginPaths();
+  EXPECT_EQ(static_cast<unsigned int>(2), pathList3.size());
+  EXPECT_STREQ("/tmp/plugin", pathList3.front().c_str());
+  EXPECT_STREQ("/test/plugin/now", pathList3.back().c_str());
+
+  EXPECT_STREQ("/worlds", paths->GetWorldPathExtension().c_str());
+
+  paths->AddGazeboPaths("/gazebo/path:/other/gazebo");
+  EXPECT_EQ(static_cast<unsigned int>(4), paths->GetGazeboPaths().size());
+  EXPECT_STREQ("/other/gazebo", paths->GetGazeboPaths().back().c_str());
+
+  paths->AddPluginPaths("/plugin/path:/other/plugin");
+  EXPECT_EQ(static_cast<unsigned int>(4), paths->GetGazeboPaths().size());
+  EXPECT_STREQ("/other/plugin", paths->GetPluginPaths().back().c_str());
+
+  paths->AddOgrePaths("/ogre/path:/other/ogre");
+  EXPECT_EQ(static_cast<unsigned int>(4), paths->GetOgrePaths().size());
+  EXPECT_STREQ("/other/ogre", paths->GetOgrePaths().back().c_str());
+
+  paths->ClearGazeboPaths();
+  paths->ClearOgrePaths();
+  paths->ClearPluginPaths();
+
+  EXPECT_EQ(static_cast<unsigned int>(2), paths->GetGazeboPaths().size());
+  EXPECT_EQ(static_cast<unsigned int>(2), paths->GetOgrePaths().size());
+  EXPECT_EQ(static_cast<unsigned int>(2), paths->GetPluginPaths().size());
+
+  putenv(const_cast<char*>("GAZEBO_RESOURCE_PATH="));
+  paths->ClearGazeboPaths();
+  EXPECT_EQ(static_cast<unsigned int>(0), paths->GetGazeboPaths().size());
+
+  putenv(const_cast<char*>("OGRE_RESOURCE_PATH="));
+  paths->ClearOgrePaths();
+  EXPECT_EQ(static_cast<unsigned int>(0), paths->GetOgrePaths().size());
+
+  putenv(const_cast<char*>("GAZEBO_PLUGIN_PATH="));
+  paths->ClearPluginPaths();
+  EXPECT_EQ(static_cast<unsigned int>(0), paths->GetPluginPaths().size());
+
+  std::cout << "GAZEBO_RESOURCE_BACKUP[" << gazeboResourcePathBackup << "]\n";
+  std::cout << "OGRE_RESOURCE_BACKUP[" << ogreResourcePathBackup << "]\n";
+  std::cout << "GAZEBO_PLUGIN_BACKUP[" << ogreResourcePathBackup << "]\n";
+
+  putenv(const_cast<char*>(gazeboResourcePathBackup.c_str()));
+  putenv(const_cast<char*>(ogreResourcePathBackup.c_str()));
+  putenv(const_cast<char*>(pluginPathBackup.c_str()));
+}
+
+/////////////////////////////////////////////////
+int main(int argc, char **argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}