1. OpenSourceRoboticsFoundation
  2. Simulation
  3. gazebo

Commits

John Hsu  committed 13153e8 Merge

merging from default

  • Participants
  • Parent commits 514551e, 488f60a
  • Branches simbody

Comments (0)

Files changed (18)

File gazebo/Server.cc

View file
     // Load the log file
     common::LogPlay::Instance()->Open(this->vm["play"].as<std::string>());
 
+    gzmsg << "\nLog playback:\n"
+      << "  Log Version: "
+      << common::LogPlay::Instance()->GetLogVersion() << "\n"
+      << "  Gazebo Version: "
+      << common::LogPlay::Instance()->GetGazeboVersion() << "\n"
+      << "  Random Seed: "
+      << common::LogPlay::Instance()->GetRandSeed() << "\n";
+
     // Get the SDF world description from the log file
     std::string sdfString;
     common::LogPlay::Instance()->Step(sdfString);

File gazebo/common/LogPlay.cc

View file
 
   // Read in the header.
   this->ReadHeader();
+
+  this->encoding.clear();
 }
 
 /////////////////////////////////////////////////
 void LogPlay::ReadHeader()
 {
-  std::string logVersion, gazeboVersion;
-  uint32_t randSeed = math::Rand::GetSeed();
+  this->randSeed = math::Rand::GetSeed();
   TiXmlElement *headerXml, *childXml;
 
+  this->logVersion.clear();
+  this->gazeboVersion.clear();
+
   // Get the header element
   headerXml = this->logStartXml->FirstChildElement("header");
   if (!headerXml)
   if (!childXml)
     gzerr << "Log file header is missing the log version.\n";
   else
-    logVersion = childXml->GetText();
+    this->logVersion = childXml->GetText();
 
   // Get the gazebo version
   childXml = headerXml->FirstChildElement("gazebo_version");
   if (!childXml)
     gzerr << "Log file header is missing the gazebo version.\n";
   else
-    gazeboVersion = childXml->GetText();
+    this->gazeboVersion = childXml->GetText();
 
   // Get the random number seed.
   childXml = headerXml->FirstChildElement("rand_seed");
   if (!childXml)
     gzerr << "Log file header is missing the random number seed.\n";
   else
-    randSeed = boost::lexical_cast<uint32_t>(childXml->GetText());
+    this->randSeed = boost::lexical_cast<uint32_t>(childXml->GetText());
 
-  gzmsg << "\nLog playback:\n"
-        << "  Log Version[" << logVersion << "]\n"
-        << "  Gazebo Version[" << gazeboVersion << "]\n"
-        << "  Random Seed[" << randSeed << "]\n\n";
-
-  if (logVersion != GZ_LOG_VERSION)
-    gzwarn << "Log version[" << logVersion << "] in file[" << this->filename
+  if (this->logVersion != GZ_LOG_VERSION)
+    gzwarn << "Log version[" << this->logVersion << "] in file["
+           << this->filename
            << "] does not match Gazebo's log version["
            << GZ_LOG_VERSION << "]\n";
 
   /// Set the random number seed for simulation
-  math::Rand::SetSeed(randSeed);
+  math::Rand::SetSeed(this->randSeed);
 }
 
 /////////////////////////////////////////////////
 }
 
 /////////////////////////////////////////////////
+std::string LogPlay::GetLogVersion() const
+{
+  return this->logVersion;
+}
+
+/////////////////////////////////////////////////
+std::string LogPlay::GetGazeboVersion() const
+{
+  return this->gazeboVersion;
+}
+
+/////////////////////////////////////////////////
+uint32_t LogPlay::GetRandSeed() const
+{
+  return this->randSeed;
+}
+
+/////////////////////////////////////////////////
 bool LogPlay::Step(std::string &_data)
 {
   if (!this->logCurrXml)
   else
     this->logCurrXml = this->logCurrXml->NextSiblingElement("chunk");
 
+  return this->GetChunkData(this->logCurrXml, _data);
+}
+
+/////////////////////////////////////////////////
+bool LogPlay::GetChunk(unsigned int _index, std::string &_data)
+{
+  unsigned int count = 0;
+  TiXmlElement *xml = this->logStartXml->FirstChildElement("chunk");
+
+  while (xml && count < _index)
+  {
+    count++;
+    xml = xml->NextSiblingElement("chunk");
+  }
+
+  if (xml && count == _index)
+    return this->GetChunkData(xml, _data);
+  else
+    return false;
+}
+
+/////////////////////////////////////////////////
+bool LogPlay::GetChunkData(TiXmlElement *_xml, std::string &_data)
+{
   // Make sure we have valid xml pointer
-  if (!this->logCurrXml)
+  if (!_xml)
     return false;
 
   /// Get the chunk's encoding
-  std::string encoding = this->logCurrXml->Attribute("encoding");
+  this->encoding = _xml->Attribute("encoding");
 
   // Make sure there is an encoding value.
-  if (encoding.empty())
+  if (this->encoding.empty())
     gzthrow("Enconding missing for a chunk in log file[" +
-            this->filename + "]");
+        this->filename + "]");
 
-  if (encoding == "txt")
-    _data = this->logCurrXml->GetText();
-  else if (encoding == "bz2")
+  if (this->encoding == "txt")
+    _data = _xml->GetText();
+  else if (this->encoding == "bz2")
   {
-    std::string data = this->logCurrXml->GetText();
+    std::string data = _xml->GetText();
     std::string buffer;
 
     // Decode the base64 string
   }
   else
   {
-    gzerr << "Inavlid encoding[" << encoding << "] in log file["
-          << this->filename << "]\n";
-      return false;
+    gzerr << "Inavlid encoding[" << this->encoding << "] in log file["
+      << this->filename << "]\n";
+    return false;
   }
 
   return true;
 }
+
+/////////////////////////////////////////////////
+std::string LogPlay::GetEncoding() const
+{
+  return this->encoding;
+}
+
+/////////////////////////////////////////////////
+unsigned int LogPlay::GetChunkCount() const
+{
+  unsigned int count = 0;
+  TiXmlElement *xml = this->logStartXml->FirstChildElement("chunk");
+
+  while (xml)
+  {
+    count++;
+    xml = xml->NextSiblingElement("chunk");
+  }
+
+  return count;
+}

File gazebo/common/LogPlay.hh

View file
       /// \return True if a log file is open.
       public: bool IsOpen() const;
 
+      /// \brief Get the log version number of the open log file.
+      /// \return The log version of the open log file. Empty string if
+      /// a log file is not open.
+      public: std::string GetLogVersion() const;
+
+      /// \brief Get the Gazebo version number of the open log file.
+      /// \return The Gazebo version of the open log file. Empty string if
+      /// a log file is not open.
+      public: std::string GetGazeboVersion() const;
+
+      /// \brief Get the random number seed of the open log file.
+      /// \return The random number seed the open log file. The current
+      /// random number seed, as defined in math::Rand::GetSeed.
+      public: uint32_t GetRandSeed() const;
+
       /// \brief Step through the open log file.
       /// \param[out] _data Data from next entry in the log file.
       public: bool Step(std::string &_data);
 
+      /// \brief Get the number of chunks (steps) in the open log file.
+      /// \return The number of recorded states in the log file.
+      public: unsigned int GetChunkCount() const;
+
+      /// \brief Get data for a particular chunk index.
+      /// \param[in] _index Index of the chunk.
+      /// \param[out] _data Storage for the chunk's data.
+      /// \return True if the _index was valid.
+      public: bool GetChunk(unsigned int _index, std::string &_data);
+
+      /// \brief Get the type of encoding used for current chunck in the
+      /// open log file.
+      /// \return The type of encoding. An empty string will be returned if
+      /// LogPlay::Step has not been called at least once.
+      public: std::string GetEncoding() const;
+
+      /// \brief Helper function to get chunk data from XML.
+      /// \param[in] _xml Pointer to an xml block that has state data.
+      /// \param[out] _data Storage for the chunk's data.
+      /// \return True if the chunk was successfully parsed.
+      private: bool GetChunkData(TiXmlElement *_xml, std::string &_data);
+
       /// \brief Read the header from the log file.
       private: void ReadHeader();
 
       /// \brief Name of the log file.
       private: std::string filename;
 
+      /// \brief The version of the Gazebo logger used to create the open
+      /// log file.
+      private: std::string logVersion;
+
+      /// \brief The version of Gazebo used to create the open log file.
+      private: std::string gazeboVersion;
+
+      /// \brief The random number seed recorded in the open log file.
+      private: uint32_t randSeed;
+
+      /// \brief The encoding for the current chunk in the log file.
+      private: std::string encoding;
+
       /// \brief This is a singleton
       private: friend class SingletonT<LogPlay>;
     };

File gazebo/gui/MainWindow.cc

View file
   splitter->addWidget(this->toolsWidget);
 
   QList<int> sizes;
-  sizes.push_back(300);
-  sizes.push_back(700);
-  sizes.push_back(300);
+  sizes.push_back(250);
+  sizes.push_back(this->width() - 250);
+  sizes.push_back(0);
   splitter->setSizes(sizes);
-  splitter->setStretchFactor(0, 1);
+  splitter->setStretchFactor(0, 0);
   splitter->setStretchFactor(1, 2);
-  splitter->setStretchFactor(2, 1);
+  splitter->setStretchFactor(2, 0);
   splitter->setCollapsible(1, false);
+  splitter->setHandleWidth(10);
 
   centerLayout->addWidget(splitter);
   centerLayout->setContentsMargins(0, 0, 0, 0);

File gazebo/physics/Model.cc

View file
 {
   this->SetWorldPose(_state.GetPose(), true);
 
+  // For now we don't use the link state values to set the state of
+  // simulation.
   /*for (unsigned int i = 0; i < _state.GetLinkStateCount(); ++i)
   {
     LinkState linkState = _state.GetLinkState(i);

File gazebo/physics/ModelState.cc

View file
 {
   bool result = true;
 
-  /// \TODO: put back in.
-  // for (std::vector<LinkState>::const_iterator iter =
-  //      this->linkStates.begin();
-  //      iter != this->linkStates.end() && result; ++iter)
-  // {
-  //   result = result && (*iter).IsZero();
-  // }
+  for (std::vector<LinkState>::const_iterator iter = this->linkStates.begin();
+       iter != this->linkStates.end() && result; ++iter)
+  {
+    result = result && (*iter).IsZero();
+  }
 
   for (std::vector<JointState>::const_iterator iter = this->jointStates.begin();
        iter != this->jointStates.end() && result; ++iter)
 }
 
 /////////////////////////////////////////////////
+bool ModelState::HasLinkState(const std::string &_linkName) const
+{
+  // Search for the link name
+  for (std::vector<LinkState>::const_iterator iter = this->linkStates.begin();
+       iter != this->linkStates.end(); ++iter)
+  {
+    if ((*iter).GetName() == _linkName)
+      return true;
+  }
+
+  return false;
+}
+
+/////////////////////////////////////////////////
 const std::vector<LinkState> &ModelState::GetLinkStates() const
 {
   return this->linkStates;
 }
 
 /////////////////////////////////////////////////
+bool ModelState::HasJointState(const std::string &_jointName) const
+{
+  for (std::vector<JointState>::const_iterator iter = this->jointStates.begin();
+       iter != this->jointStates.end(); ++iter)
+  {
+    if ((*iter).GetName() == _jointName)
+      return true;
+  }
+
+  return false;
+}
+
+/////////////////////////////////////////////////
 const std::vector<JointState> &ModelState::GetJointStates() const
 {
   return this->jointStates;
   result.pose.pos = this->pose.pos - _state.pose.pos;
   result.pose.rot = _state.pose.rot.GetInverse() * this->pose.rot;
 
-  /// \TODO: put back in.
   // Insert the link state diffs.
-  // for (std::vector<LinkState>::const_iterator iter =
-  //      this->linkStates.begin(); iter != this->linkStates.end(); ++iter)
-  // {
-  //   LinkState state = (*iter) - _state.GetLinkState((*iter).GetName());
-  //   if (!state.IsZero())
-  //     result.linkStates.push_back(state);
-  // }
+  for (std::vector<LinkState>::const_iterator iter =
+       this->linkStates.begin(); iter != this->linkStates.end(); ++iter)
+  {
+    try
+    {
+      LinkState state = (*iter) - _state.GetLinkState((*iter).GetName());
+      if (!state.IsZero())
+        result.linkStates.push_back(state);
+    }
+    catch(common::Exception &)
+    {
+      // Ignore exception, which is just the fact that a link state may not
+      // have been recorded.
+    }
+  }
 
   // Insert the joint state diffs.
   for (std::vector<JointState>::const_iterator iter =
   result.pose.rot = _state.pose.rot * this->pose.rot;
 
   // Insert the link state diffs.
-  // for (std::vector<LinkState>::const_iterator iter =
-  //      this->linkStates.begin(); iter != this->linkStates.end(); ++iter)
-  // {
-  //   LinkState state = (*iter) + _state.GetLinkState((*iter).GetName());
-  //   result.linkStates.push_back(state);
-  // }
+  for (std::vector<LinkState>::const_iterator iter =
+       this->linkStates.begin(); iter != this->linkStates.end(); ++iter)
+  {
+    try
+    {
+      LinkState state = (*iter) + _state.GetLinkState((*iter).GetName());
+      result.linkStates.push_back(state);
+    }
+    catch(common::Exception &)
+    {
+      // Ignore exception, which is just the fact that a link state may not
+      // have been recorded.
+    }
+  }
 
   // Insert the joint state diffs.
   for (std::vector<JointState>::const_iterator iter =

File gazebo/physics/ModelState.hh

View file
       /// \throws common::Exception When _linkName is invalid.
       public: LinkState GetLinkState(const std::string &_linkName) const;
 
+      /// \brief Return true if there is a link with the specified name.
+      /// \param[in] _linkName Name of the LinkState.
+      /// \return True if the link exists in the model.
+      public: bool HasLinkState(const std::string &_linkName) const;
+
       /// \brief Get the link states.
       /// \return A vector of link states.
       public: const std::vector<LinkState> &GetLinkStates() const;
       /// \return A vector of joint states.
       public: const std::vector<JointState> &GetJointStates() const;
 
+      /// \brief Return true if there is a joint with the specified name.
+      /// \param[in] _jointName Name of the Jointtate.
+      /// \return True if the joint exists in the model.
+      public: bool HasJointState(const std::string &_jointName) const;
+
       /// \brief Populate a state SDF element with data from the object.
       /// \param[out] _sdf SDF element to populate.
       public: void FillSDF(sdf::ElementPtr _sdf);
         _out << "<model name='" << _state.GetName() << "'>\n";
         _out << "<pose>" << _state.pose << "</pose>\n";
 
-        // for (std::vector<LinkState>::const_iterator iter =
-        //     _state.linkStates.begin(); iter != _state.linkStates.end();
-        //     ++iter)
-        // {
-        //   _out << *iter;
-        // }
+        for (std::vector<LinkState>::const_iterator iter =
+            _state.linkStates.begin(); iter != _state.linkStates.end();
+            ++iter)
+        {
+          _out << *iter;
+        }
 
         // Output the joint information
         for (std::vector<JointState>::const_iterator iter =

File gazebo/physics/World.cc

View file
   common::LogRecord::Instance()->Add(this->GetName(), "state.log",
       boost::bind(&World::OnLog, this, _1));
 
+  this->prevStates[0].SetName(this->GetName());
+  this->prevStates[1].SetName(this->GetName());
+
   this->initialized = true;
 }
 

File gazebo/rendering/Scene.cc

View file
       event::Events::ConnectPreRender(boost::bind(&Scene::PreRender, this)));
 
   this->sensorSub = this->node->Subscribe("~/sensor",
-                                          &Scene::OnSensorMsg, this);
+                                          &Scene::OnSensorMsg, this, true);
   this->visSub = this->node->Subscribe("~/visual", &Scene::OnVisualMsg, this);
 
   this->lightPub = this->node->Advertise<msgs::Light>("~/light");

File gazebo/server_main.cc

View file
 //////////////////////////////////////////////////
 int main(int argc, char **argv)
 {
+  gazebo::Server *server = NULL;
+
   try
   {
     gazebo::common::LogRecord::Instance()->Init("server");
 
-    gazebo::Server *server = new gazebo::Server();
+    server = new gazebo::Server();
     if (!server->ParseArgs(argc, argv))
       return -1;
 
   catch(gazebo::common::Exception &_e)
   {
     _e.Print();
+
+    server->Fini();
+    delete server;
   }
 
   return 0;

File gazebo/transport/ConnectionManager.cc

View file
   this->tmpIndex = 0;
   this->initialized = false;
   this->stop = false;
-  this->stopped = false;
+  this->stopped = true;
 
   this->serverConn = NULL;
   this->listMutex = new boost::recursive_mutex();

File gazebo/transport/Publisher.cc

File contents unchanged.

File tools/CMakeLists.txt

View file
 include ( ${QT_USE_FILE} )
 add_definitions(${QT_DEFINITIONS})
 
+
 include_directories( 
   ${PROJECT_BINARY_DIR}/gazebo 
   ${PROJECT_SOURCE_DIR}/gazebo 
   ${PROTOBUF_INCLUDE_DIR}
 )
 
+add_subdirectory(test)
+
 add_executable(gzsdf gzsdf.cc)
 target_link_libraries(gzsdf ${tinyxml_libraries} gazebo_sdf_interface)
 gz_install_executable(gzsdf)
 target_link_libraries(gztopic gazebo_msgs gazebo_common gazebo_transport pthread gazebo_gui ${QT_LIBRARIES})
 gz_install_executable(gztopic)
 
+add_executable(gzlog gzlog.cc)
+target_link_libraries(gzlog gazebo_common gazebo_physics gazebo_transport gazebo_sensors gazebo_msgs gazebo_sdf_interface pthread)
+gz_install_executable(gzlog)
+
 add_executable(gzstats gzstats.cc)
 target_link_libraries(gzstats gazebo_msgs gazebo_common gazebo_transport pthread)
 gz_install_executable(gzstats)

File tools/gzlog.cc

View file
+/*
+ * 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 <stdio.h>
+#include <termios.h>
+#include <unistd.h>
+
+#include <boost/program_options.hpp>
+#include <boost/algorithm/string.hpp>
+#include <boost/algorithm/string/regex.hpp>
+#include <boost/date_time/posix_time/posix_time.hpp>
+#include <boost/date_time/posix_time/posix_time_io.hpp>
+
+#include <gazebo/gazebo.hh>
+
+#include <gazebo/sdf/sdf.hh>
+
+#include <gazebo/physics/WorldState.hh>
+#include <gazebo/common/Time.hh>
+#include <gazebo/gazebo_config.h>
+
+namespace po = boost::program_options;
+
+sdf::ElementPtr g_stateSdf;
+
+/////////////////////////////////////////////////
+/// \brief Print general help
+void help()
+{
+  std::cerr << "Help:\n";
+  std::cerr << "This tool introspects Gazebo log files.\n\n";
+  std::cerr << "Usage: gzlog [command] <options> [log file]\n\n";
+
+  std::cerr << "Commands:\n"
+            << "  help\t Output this help message.\n"
+            << "  info\t Display statistical information about a log file.\n"
+            << "  echo\t Output the contents of a log file to screen.\n"
+            << "  step\t Step through the contents of a log file.\n";
+
+  std::cerr << "\n";
+}
+
+/////////////////////////////////////////////////
+/// \brief Get a character from the terminal. This bypasses the need to wait
+/// for the 'enter' key.
+int get_ch()
+{
+  struct termios oldt, newt;
+  int ch;
+  tcgetattr(STDIN_FILENO, &oldt);
+  newt = oldt;
+  newt.c_lflag &= ~(ICANON | ECHO);
+  tcsetattr(STDIN_FILENO, TCSANOW, &newt);
+  ch = getchar();
+  tcsetattr(STDIN_FILENO, TCSANOW, &oldt);
+
+  return ch;
+}
+
+/////////////////////////////////////////////////
+/// \brief Load a log file from a filename.
+/// \param[in] _filename Filename to open
+bool load_log_from_file(const std::string &_filename)
+{
+  if (_filename.empty())
+  {
+    gzerr << "Log filename is empty.\n";
+    return false;
+  }
+
+  try
+  {
+    gazebo::common::LogPlay::Instance()->Open(_filename);
+  }
+  catch(gazebo::common::Exception &_e)
+  {
+    gzerr << "Unable to open log file[" << _filename << "]\n";
+    return false;
+  }
+
+  return true;
+}
+
+/////////////////////////////////////////////////
+/// \brief Get the size of file.
+/// \param[in] _filename Name of the file to get the size of.
+/// \return The size of the file in human readable format.
+std::string get_file_size_str(const std::string &_filename)
+{
+  std::ostringstream size;
+
+  // Open the file
+  std::ifstream ifs(_filename.c_str());
+  if (!ifs)
+  {
+    gzerr << "Unable to open file[" << _filename << "]\n";
+    return std::string();
+  }
+
+  // Move to the end of the file
+  ifs.seekg(0, std::ios::end);
+
+  // Get the position of the file pointer, which is the number of bytes in
+  // the file.
+  int byteSize = ifs.tellg();
+
+  // Generate a human friendly string
+  if (byteSize < 1000)
+    size << byteSize << " B";
+  else if (byteSize < 1000000)
+    size << byteSize / 1000.0f << " KB";
+  else
+    size << byteSize / 1.0e6 << " MB";
+
+  return size.str();
+}
+
+/////////////////////////////////////////////////
+/// \bried Output information about a log file.
+void info(const std::string &_filename)
+{
+  gazebo::common::LogPlay *play = gazebo::common::LogPlay::Instance();
+
+  // Get the SDF world description from the log file
+  std::string sdfString;
+  gazebo::common::LogPlay::Instance()->Step(sdfString);
+
+  // Parse the first SDF world description
+  sdf::ElementPtr sdf(new sdf::Element);
+  sdf::initFile("root.sdf", sdf);
+  sdf::readString(sdfString, sdf);
+
+  gazebo::physics::WorldState state;
+
+  unsigned int modelCount = 0;
+
+  gazebo::common::Time endTime(0, 0);
+  gazebo::common::Time startTime(0, 0);
+
+  if (sdf)
+  {
+    // Check for a world element
+    if (sdf->HasElement("world"))
+    {
+      // Get a pointer to the world element
+      sdf::ElementPtr worldElem = sdf->GetElement("world");
+
+      // Check for a model
+      if (worldElem->HasElement("model"))
+      {
+        // Get a pointer to the first model element.
+        sdf::ElementPtr modelElem = worldElem->GetElement("model");
+
+        // Count all the model elements.
+        while (modelElem)
+        {
+          modelCount++;
+          modelElem = modelElem->GetNextElement("model");
+        }
+      }
+
+      // Get the state for the world at the start.
+      if (worldElem->HasElement("state"))
+      {
+        state.Load(worldElem->GetElement("state"));
+
+        // Store the start time. The end time is calculated by adding all
+        // the time diffs to the start time.
+        startTime = state.GetWallTime();
+        endTime = startTime;
+      }
+    }
+
+    std::string stateString;
+    // Iterate over all the chunks, and add up the wall time. This will
+    // compute the final end time.
+    for (unsigned int i = 0; i < play->GetChunkCount(); ++i)
+    {
+      stateString.clear();
+      g_stateSdf->ClearElements();
+
+      play->Step(stateString);
+
+      if (stateString.empty())
+        continue;
+
+      sdf::readString(stateString, g_stateSdf);
+
+      state.Load(g_stateSdf);
+      endTime += state.GetWallTime();
+    }
+  }
+
+  // Tell cout how to output boost dates
+  boost::posix_time::time_facet *facet =
+    new boost::posix_time::time_facet("%b %d %y %H:%M:%S");
+  std::cout.imbue(std::locale(std::locale::classic(), facet));
+
+  // Compute the duration
+  gazebo::common::Time deltaTime = endTime - startTime;
+  int hours = deltaTime.sec / 3600;
+  int minutes = (deltaTime.sec - hours * 3600) / 60;
+  int seconds = (deltaTime.sec - hours * 3600 - minutes * 60);
+
+  // Output info
+  std::cout
+    << "Log Version:    " << play->GetLogVersion() << "\n"
+    << "Gazebo Version: " << play->GetGazeboVersion() << "\n"
+    << "Random Seed:    " << play->GetRandSeed() << "\n"
+    << "Start:          " << boost::posix_time::from_time_t(startTime.sec)
+    << "." << startTime.nsec << "\n"
+    << "End:            " << boost::posix_time::from_time_t(endTime.sec)
+    << "." << endTime.nsec << "\n"
+    << "Duration:       " << std::setfill('0') << std::setw(2) << hours << ":"
+                          << std::setfill('0') << std::setw(2) << minutes << ":"
+                          << std::setfill('0') << std::setw(2) << seconds << "."
+                          << deltaTime.nsec << "\n"
+    << "Steps:          " << play->GetChunkCount() << "\n"
+    << "Size:           " << get_file_size_str(_filename) << "\n"
+    << "Encoding:       " << play->GetEncoding() << "\n"
+    << "Model Count:    " << modelCount << "\n"
+    << "\n";
+}
+
+/////////////////////////////////////////////////
+/// \brief Filter a state string.
+/// \param[in] _stateString State string data.
+/// \param[in] _filter Filter argument.
+std::string filter_state(const std::string &_stateString,
+                         std::string _filter)
+{
+  std::ostringstream result;
+  gazebo::physics::WorldState state;
+  std::vector<std::string> names;
+
+  // Read and parse the state information
+  g_stateSdf->ClearElements();
+  sdf::readString(_stateString, g_stateSdf);
+  state.Load(g_stateSdf);
+
+  // Split the filter on "::"
+  boost::split_regex(names, _filter, boost::regex("::"));
+  std::vector<std::string>::iterator iter = names.begin();
+
+  // Continue if the filter is valid. Otherwise output the raw state data.
+  if (iter != names.end() && !(*iter).empty())
+  {
+    // The first element in the filter must be a model name.
+    gazebo::physics::ModelState modelState = state.GetModelState(*(iter++));
+
+    // Continue if there is more to the filter and the current filter
+    // item valid. Otheriwise, use all of the model state data.
+    if (iter != names.end() && !(*iter).empty())
+    {
+      // Check to see if the next element in the filter is a link.
+      if (modelState.HasLinkState(*iter))
+      {
+        gazebo::physics::LinkState linkState;
+
+        // Get the link.
+        linkState = modelState.GetLinkState(*(iter++));
+
+        // Continue if the next element in the filter is valid. Otherwise
+        // use all of the link's data.
+        if (iter != names.end() && !(*iter).empty())
+        {
+          // Each data value in a link starts with a unique character, so we
+          // allow the user to use just the first character in the filter.
+          switch ((*iter)[0])
+          {
+            default:
+            case 'p':
+              result << linkState.GetPose();
+              break;
+            case 'v':
+              result << linkState.GetVelocity();
+              break;
+            case 'a':
+              result << linkState.GetAcceleration();
+              break;
+            case 'w':
+              result << linkState.GetAcceleration();
+              break;
+          }
+        }
+        else
+          result << linkState;
+      }
+      // Otherwise check to see if the next element in the filter is a joint.
+      else if (modelState.HasJointState(*iter))
+      {
+        gazebo::physics::JointState jointState;
+
+        // Get the joint.
+        jointState = modelState.GetJointState(*(iter++));
+
+        // Continue if there is more to the filter, and the filter element
+        // is valid. Otherwise use all of the joint's data.
+        if (iter != names.end() && !(*iter).empty())
+        {
+          // Try to get the index of the axis for output
+          try
+          {
+            int index = boost::lexical_cast<int>(*iter);
+            result << jointState.GetAngle(index);
+          }
+          catch(boost::bad_lexical_cast &_e)
+          {
+            gzerr << "Invalid joint angle index[" << *iter << "]\n";
+          }
+        }
+        else
+          result << jointState;
+      }
+      // Otherwise don't use any data.
+      else
+      {
+        // Don't output an error here. A link or joint will not get logged
+        // if there was no change in it's state values.
+      }
+    }
+    else
+      result << modelState;
+  }
+  else
+    result << g_stateSdf;
+
+  return result.str();
+}
+
+/////////////////////////////////////////////////
+/// \brief Dump the contents of a log file to screen
+/// \param[in] _filter Filter string
+void echo(const std::string _filter)
+{
+  std::string stateString;
+
+  for (unsigned int i = 0;
+       i < gazebo::common::LogPlay::Instance()->GetChunkCount(); ++i)
+  {
+    // Get and output the state string
+    gazebo::common::LogPlay::Instance()->Step(stateString);
+
+    if (!_filter.empty() && i > 0)
+      stateString = filter_state(stateString, _filter);
+    else if (!_filter.empty())
+      stateString.clear();
+
+    if (!stateString.empty())
+      std::cout << stateString << "\n";
+  }
+}
+
+/////////////////////////////////////////////////
+/// \brief Step through a log file.
+/// \param[in] _filter Filter string
+void step(const std::string &_filter)
+{
+  std::string stateString;
+  gazebo::common::LogPlay *play = gazebo::common::LogPlay::Instance();
+
+  char c = '\0';
+
+  for (unsigned int i = 0; i < play->GetChunkCount() && c != 'q'; ++i)
+  {
+    // Get and output the state string
+    play->Step(stateString);
+
+    if (!_filter.empty() && i > 0)
+      stateString = filter_state(stateString, _filter);
+    else if (!_filter.empty())
+      stateString.clear();
+
+    std::cout << stateString << "\n";
+
+    std::cout << "\n--- Press space to continue, 'q' to quit ---\n";
+
+    c = '\0';
+
+    // Wait for a space or 'q' key press
+    while (c != ' ' && c != 'q')
+      c = get_ch();
+  }
+}
+
+/////////////////////////////////////////////////
+int main(int argc, char **argv)
+{
+  // Hidden options
+  po::options_description hiddenOptions("hidden options");
+  hiddenOptions.add_options()
+    ("command", po::value<std::string>(), "Command");
+
+  // Options that are visible to the user through help.
+  po::options_description visibleOptions("Options");
+  visibleOptions.add_options()
+    ("help,h", "Output this help message.")
+    ("file,f", po::value<std::string>(), "Path to a log file.")
+    ("filter", po::value<std::string>(),
+     "Filter output. Valid only for the echo and step commands");
+
+  // Both the hidden and visible options
+  po::options_description allOptions("all options");
+  allOptions.add(hiddenOptions).add(visibleOptions);
+
+  // The command and file options are positional
+  po::positional_options_description positional;
+  positional.add("command", 1).add("file", -1);
+
+  po::variables_map vm;
+
+  try
+  {
+    po::store(
+        po::command_line_parser(argc, argv).options(allOptions).positional(
+          positional).run(), vm);
+
+    po::notify(vm);
+  }
+  catch(boost::exception &_e)
+  {
+    std::cerr << "Invalid arguments\n\n";
+    return -1;
+  }
+
+  std::string command, filename, filter;
+
+  // Create a state sdf element.
+  g_stateSdf.reset(new sdf::Element);
+  sdf::initFile("state.sdf", g_stateSdf);
+
+  // Get the command name
+  command = vm.count("command") ? vm["command"].as<std::string>() : "";
+
+  // Get the filter
+  filter = vm.count("filter") ? vm["filter"].as<std::string>() : "";
+
+  // Output help when appropriate
+  if (command.empty() || command == "help" || vm.count("help"))
+  {
+    help();
+    std::cerr << visibleOptions << "\n";
+
+    return 0;
+  }
+
+  // Load the log file
+  if (vm.count("file"))
+    filename = vm["file"].as<std::string>();
+  else
+  {
+    gzerr << "No log file specified\n";
+    return -1;
+  }
+
+  // Load log file from string
+  if (!load_log_from_file(filename))
+    return -1;
+
+  // Process the command
+  if (command == "info")
+    info(filename);
+  else if (command == "echo")
+    echo(filter);
+  else if (command == "step")
+    step(filter);
+
+  return 0;
+}

File tools/test/CMakeLists.txt

View file
+include_directories (
+  ${PROJECT_SOURCE_DIR}/gazebo 
+  ${PROJECT_BINARY_DIR}/gazebo
+  ${PROTOBUF_INCLUDE_DIR}
+)
+
+link_directories(${PROJECT_BINARY_DIR}/test)
+
+set (gtest_sources 
+  gz_log.cc 
+)
+
+# Build tests
+foreach(GTEST_SOURCE_file ${gtest_sources})
+  string(REGEX REPLACE ".cc" "" BINARY_NAME ${GTEST_SOURCE_file})
+  add_executable(${BINARY_NAME} ${GTEST_SOURCE_file})
+  add_dependencies(${BINARY_NAME} gtest)
+  add_dependencies(${BINARY_NAME} gtest_main)
+
+  target_link_libraries(${BINARY_NAME}
+    libgtest.a
+    libgtest_main.a 
+    gazebo_common
+    gazebo_msgs
+    pthread
+    )
+
+  add_test(${BINARY_NAME} ${CMAKE_CURRENT_BINARY_DIR}/${BINARY_NAME} --gtest_output=xml:${CMAKE_BINARY_DIR}/test_results/${BINARY_NAME}.xml)
+  set_tests_properties(${BINARY_NAME} PROPERTIES TIMEOUT 240)
+
+  # Check that the test produced a result and create a failure if it didn't.
+  # Guards against crashed and timed out tests.
+  add_test(check_${BINARY_NAME} ${PROJECT_SOURCE_DIR}/tools/check_test_ran.py ${CMAKE_BINARY_DIR}/test_results/${BINARY_NAME}.xml)
+endforeach()

File tools/test/data/empty_state.log

View file
+<?xml version='1.0'?>
+<gazebo_log>
+<header>
+<log_version>1.0</log_version>
+<gazebo_version>1.4.0</gazebo_version>
+<rand_seed>19644</rand_seed>
+</header>
+<chunk encoding='bz2'>
+<![CDATA[QlpoOTFBWSZTWWYxJvsAAgdfgHVQQIP/9wCAAAC/7/+wUANernSduuzEoGSmaU8iNPSABoAxA9QA1PQClPTFPU00MRoGgBpoBIinpNGpR+qbamp6mgaek9TQANBgAGjTQBk0BoMgNAKoUExDSeUxJ5JkABoaGbN0wJBMwiYhDoRBKUgRihArgQhKiaYIEQ1KCLhvttlNGVlj+DuTRBiw13iBi3tA6zMTEpJlEyPfPl8d6vkRpKH091BLAMxJGDDw4GmiSDpEsJULviBsciSKTY0khqmP4nUbnaPAAJdm8x3hho7aytiCGex0rJArfv+xSQGhhgKjFrFigF+Cc/aeD4bR06wPqVlWuebtMnkzxnQnNuZ8Z1dKpvPRnHPFM0iKJgckSXmJiaNURYqw5mssih07FWCO+FgPbBOPhsXxMbk9HQgic1WDT1dKOHDzcZt3QBKIorZ9MOSXCbmbzq9r5pmxPrFyiqoLiMiIMWdWG+hY9G0tp3RunwxFAbgEA4CUSiUPHgKJA1Ld2G7hoeRO30zQcEcjLLBkXGx9oTZvYwFWVU1uY48F5g8/O+/FuVmi1Sxn5PY/SnJf3YIbstCwzRG+rbf8MFMeDDk0L4w3bAG+O0zxytkQSH0SqGM6gH+DuwZUlBIaBLuCNIkC5esjFfYYPRsZO1k2kl2Bli3KaMHfrrzVmlYuUp4ONzRHY1bmcmiLlODvXKZ3LxSGBqVI2jNWWS4pblg4KDbbjyQNq7eyO4OC3sUZ3TW8StDmixGhc0D631eZmgsNg5uFQNuC7Qd3eKqoLXYO4oBqii5VTNKc25vv2T0dkNhlEKPbGpq26xyR2PFyb8+MqTO+KrtFVawdS22+PDI0O9rHOIxi3qWzz5KWscBtu+rcT5yGR2Exx2UkjfYHIiR5LnZZ453u7gm2xbzKSjJGiUM0SwhqY2N6OjoejKNrHmnBbyNNLFV3KrbPbZsJsTbsRw0vcSoji0LXqLuSKcKEgzGJN9g]]>
+</chunk>
+</gazebo_log>

File tools/test/data/pr2_state.log

View file
+<?xml version='1.0'?>
+<gazebo_log>
+<header>
+<log_version>1.0</log_version>
+<gazebo_version>1.4.0</gazebo_version>
+<rand_seed>22123</rand_seed>
+</header>
+<chunk encoding='bz2'>
+<![CDATA[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]]>
+</chunk>
+<chunk encoding='bz2'>
+<![CDATA[QlpoOTFBWSZTWeSBSGAAAWPbgBAQQIP/5wAAv/ffwEACfWppa6a4JEk0aPUU9EZPUPUaAACRCKZopkBtQAAaAiqTIYQyaYExBoDQFSRJhU3qeqfok9NNQaeoMh6mXkkhU2o6ie8jtRPrfpCXkjesROOEBZ3oE8kk+Nmm25YRgRA+XKDYSFNut5B0gKi/VgiG0cayQW2iqhJM3vfKw21iFBO0tydUZRnX5sYtvEu4aGs5U1irrV0ZFsc9NC92w7TbPO9nTnTccLaXmrq2qqyRExeMXxZAZ6xBdZ6uid05EZVncovbWTXGbqW2DGNoabewmfS21aUhNqqvcT98wnSuvmlqguRZjhttJMGg7nsWYWP0mW01+MoHMUf1KbEqrHIGWPiEuRIzKVMQMljgi8QNvRdZrCeonYTV+MlZ/B8ewl3ErjusuoCv7zz7d9+l3pvWDpa0RgUZhd3IFKSJedE2tg6zB5Ob2wWkyu75GRi6Zw/qfSQlpNGKa2IBvbUQIYWVcgGAMVSNUxVNxgMw2t7Xqr3KZd3IJwbGibJoY75lLwlJrrkZV6icJiEqIM9zm7xB4JwrlPF2gNjSy/h/pitS9Bw46lUudeZGCYQiBolUiVOTTTg7nnJuXxNJNLngJcz0wY4L4coxCJ7RJGzSRxM+xSyqIZgwIunwUEuTaDmDGlWCguAv8XckU4UJDkgUhgA]]>
+</chunk>
+<chunk encoding='bz2'>
+<![CDATA[QlpoOTFBWSZTWQKDVnMAAb5bgBAQQIP/5wAAv/ffwEAC2zJo1d06gkkJtCelRpoYQG1AAJNREmEiY1M1GhpoBoDaUaoGh6gD1NAAaAFSk0aFM1NMg2kBoAGDUCSKSKMtALgOrA4zqkkpoDQ74BpgAMgB2cKoGfCHRNiPO6uKJSO1dQgGmpqHfJI2Z6Uwa7r36bskmmjjPlnWTaqGZmw6rw1yxrxrpmMx8tkZMMqm7Jidyahr0GbMnlGmM0u+ZlbNed7qH11aw48p4IfFTESsWiTm1I5IOLzhwOOM21vnLW+2dQheNDONdTtNebq66PLOac77aG+uWr315citNOG7JyRWKqRjVjqARWUNtJJKjbuAPOxAFXbtJMFG4SNKpBBhD4vWZYoq5/o3YIz/YvCqVUPYzgqzOLuy5PZGOT8ZuT2MI0N1howilGrSqKvEss5gB6SSeskkGzjW+EQbuiSXZoMVRvU/em8mUGSSt4bdOe0qel2n10HTNevrrda4a0rN+YAcEk4edxZ+/MGHkuKKnccj6TLCqSRIxQ/sKwOXukcD8F6JAqK+kbGSbVyieMd6JFam0c5JKHNAWIDqgKL5lhMyECZOfSbcKcDJLD+WtEks3GKbkzLcGRRUFE00xmPrLqEmEWSSURIy3mr+RI2Jh1tHZcasXJTtYnaZId3f2mQPHPia9i42jJgJSToXUbE8Djiz3ncN5WxkmdTSSSsyzvWvVv4RYurhGoSTqkk8j3SrobKTAw2Cbj7pRhs60dk34zDKGA0kk/4u5IpwoSAFBqzm]]>
+</chunk>
+</gazebo_log>

File tools/test/gz_log.cc

View file
+/*
+ * 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 <gtest/gtest.h>
+#include <stdio.h>
+#include <boost/lexical_cast.hpp>
+#include <boost/algorithm/string/trim.hpp>
+
+#include "test_config.h"
+
+std::string custom_exec(const std::string &_cmd)
+{
+  FILE* pipe = popen(_cmd.c_str(), "r");
+
+  if (!pipe)
+    return "ERROR";
+
+  char buffer[128];
+  std::string result = "";
+
+  while (!feof(pipe))
+  {
+    if (fgets(buffer, 128, pipe) != NULL)
+      result += buffer;
+  }
+
+  pclose(pipe);
+  return result;
+}
+
+/////////////////////////////////////////////////
+/// Check to make sure that 'gzlog info' returns correct information
+TEST(gz_log, Info)
+{
+  std::string info = custom_exec(std::string("gzlog info ") +
+      PROJECT_SOURCE_PATH + "/tools/test/data/pr2_state.log");
+  boost::trim_right(info);
+
+  std::string validInfo =
+    "Log Version:    1.0\n"
+    "Gazebo Version: 1.4.0\n"
+    "Random Seed:    22123\n"
+    "Start:          Jan 09 13 16:17:47.384234901\n"
+    "End:            Jan 09 13 16:17:47.411576612\n"
+    "Duration:       00:00:00.27341711\n"
+    "Steps:          3\n"
+    "Size:           9.159 KB\n"
+    "Encoding:       bz2\n"
+    "Model Count:    2";
+
+  EXPECT_EQ(validInfo, info);
+}
+
+/////////////////////////////////////////////////
+/// Check to make sure that 'gzlog echo' returns correct information
+TEST(gz_log, Echo)
+{
+  std::string echo = custom_exec(std::string("gzlog echo ") +
+      PROJECT_SOURCE_PATH + "/tools/test/data/empty_state.log");
+  boost::trim_right(echo);
+
+  std::string validEcho =
+    "<sdf version ='1.3'>\n"
+    "<world name='default'>\n"
+    "  <light name='sun' type='directional'>\n"
+    "    <cast_shadows>1</cast_shadows>\n"
+    "    <pose>0.000000 0.000000 10.000000 0.000000 0.000000 0.000000</pose>\n"
+    "    <diffuse>0.800000 0.800000 0.800000 1.000000</diffuse>\n"
+    "    <specular>0.100000 0.100000 0.100000 1.000000</specular>\n"
+    "    <attenuation>\n"
+    "      <range>1000.000000</range>\n"
+    "      <constant>0.900000</constant>\n"
+    "      <linear>0.010000</linear>\n"
+    "      <quadratic>0.001000</quadratic>\n"
+    "    </attenuation>\n"
+    "    <direction>-0.500000 0.500000 -1.000000</direction>\n"
+    "  </light>\n"
+    "  <model name='ground_plane'>\n"
+    "    <static>1</static>\n"
+    "    <link name='link'>\n"
+    "      <collision name='collision'>\n"
+    "        <geometry>\n"
+    "          <plane>\n"
+    "            <normal>0.000000 0.000000 1.000000</normal>\n"
+    "            <size>100.000000 100.000000</size>\n"
+    "          </plane>\n"
+    "        </geometry>\n"
+    "        <surface>\n"
+    "          <friction>\n"
+    "            <ode>\n"
+    "              <mu>100.000000</mu>\n"
+    "              <mu2>50.000000</mu2>\n"
+    "            </ode>\n"
+    "          </friction>\n"
+    "          <bounce/>\n"
+    "          <contact>\n"
+    "            <ode/>\n"
+    "          </contact>\n"
+    "        </surface>\n"
+    "      </collision>\n"
+    "      <visual name='visual'>\n"
+    "        <cast_shadows>0</cast_shadows>\n"
+    "        <geometry>\n"
+    "          <plane>\n"
+    "            <normal>0.000000 0.000000 1.000000</normal>\n"
+    "            <size>100.000000 100.000000</size>\n"
+    "          </plane>\n"
+    "        </geometry>\n"
+    "        <material>\n"
+    "          <script>\n"
+    "            <uri>file://media/materials/scripts/gazebo.material</uri>\n"
+    "            <name>Gazebo/Grey</name>\n"
+    "          </script>\n"
+    "        </material>\n"
+    "      </visual>\n"
+    "      <velocity_decay>\n"
+    "        <linear>0.000000</linear>\n"
+    "        <angular>0.000000</angular>\n"
+    "      </velocity_decay>\n"
+    "      <self_collide>0</self_collide>\n"
+    "      <kinematic>0</kinematic>\n"
+    "      <gravity>1</gravity>\n"
+    "    </link>\n"
+    "  </model>\n"
+    "  <physics type='ode'>\n"
+    "    <update_rate>1000.000000</update_rate>\n"
+    "    <gravity>0.000000 0.000000 -9.800000</gravity>\n"
+    "  </physics>\n"
+    "  <scene>\n"
+    "    <ambient>0.000000 0.000000 0.000000 1.000000</ambient>\n"
+    "    <background>0.700000 0.700000 0.700000 1.000000</background>\n"
+    "    <shadows>1</shadows>\n"
+    "  </scene>\n"
+    "  <state world_name='__default__'>\n"
+    "    <sim_time>0 0</sim_time>\n"
+    "    <real_time>0 0</real_time>\n"
+    "    <wall_time>1357757564 822623113</wall_time>\n"
+    "  </state>\n"
+    "</world>\n"
+    "</sdf>";
+
+  EXPECT_EQ(validEcho, echo);
+
+  // Test model filter
+  echo = custom_exec(
+      std::string("gzlog echo --filter pr2 ") +
+      PROJECT_SOURCE_PATH + "/tools/test/data/pr2_state.log");
+  boost::trim_right(echo);
+
+  validEcho =
+    "<model name='pr2'>\n"
+    "<pose>0 0 -8e-06 0 0 0</pose>\n"
+    "<joint name='torso_lift_joint'>\n"
+    "<angle axis='0'>1.41007e-06</angle>\n"
+    "</joint>\n"
+    "<joint name='l_shoulder_lift_joint'>\n"
+    "<angle axis='0'>-1.24472e-05</angle>\n"
+    "</joint>\n"
+    "<joint name='l_upper_arm_roll_joint'>\n"
+    "<angle axis='0'>1.04205e-05</angle>\n"
+    "</joint>\n"
+    "<joint name='l_elbow_flex_joint'>\n"
+    "<angle axis='0'>2.63091e-06</angle>\n"
+    "</joint>\n"
+    "<joint name='l_forearm_roll_joint'>\n"
+    "<angle axis='0'>-1.03937e-05</angle>\n"
+    "</joint>\n"
+    "<joint name='l_wrist_flex_joint'>\n"
+    "<angle axis='0'>-5.93599e-06</angle>\n"
+    "</joint>\n"
+    "<joint name='l_wrist_roll_joint'>\n"
+    "<angle axis='0'>-2.00905e-05</angle>\n"
+    "</joint>\n"
+    "<joint name='l_gripper_motor_screw_joint'>\n"
+    "<angle axis='0'>-2.80641e-06</angle>\n"
+    "</joint>\n"
+    "<joint name='l_gripper_r_finger_tip_joint'>\n"
+    "<angle axis='0'>1.53825e-06</angle>\n"
+    "</joint>\n"
+    "<joint name='laser_tilt_mount_joint'>\n"
+    "<angle axis='0'>-2.47273e-06</angle>\n"
+    "</joint>\n"
+    "<joint name='r_shoulder_lift_joint'>\n"
+    "<angle axis='0'>-1.28046e-05</angle>\n"
+    "</joint>\n"
+    "<joint name='r_upper_arm_roll_joint'>\n"
+    "<angle axis='0'>-1.0234e-05</angle>\n"
+    "</joint>\n"
+    "<joint name='r_elbow_flex_joint'>\n"
+    "<angle axis='0'>3.52763e-06</angle>\n"
+    "</joint>\n"
+    "<joint name='r_forearm_roll_joint'>\n"
+    "<angle axis='0'>1.0253e-05</angle>\n"
+    "</joint>\n"
+    "<joint name='r_wrist_flex_joint'>\n"
+    "<angle axis='0'>2.10212e-05</angle>\n"
+    "</joint>\n"
+    "<joint name='r_wrist_roll_joint'>\n"
+    "<angle axis='0'>-6.13793e-06</angle>\n"
+    "</joint>\n"
+    "<joint name='r_gripper_motor_screw_joint'>\n"
+    "<angle axis='0'>-3.52607e-06</angle>\n"
+    "</joint>\n"
+    "<joint name='r_gripper_r_finger_joint'>\n"
+    "<angle axis='0'>-2.04123e-06</angle>\n"
+    "</joint>\n"
+    "<joint name='r_gripper_r_finger_tip_joint'>\n"
+    "<angle axis='0'>-1.00512e-06</angle>\n"
+    "</joint>\n"
+    "<joint name='torso_lift_motor_screw_joint'>\n"
+    "<angle axis='0'>7.9351e-05</angle>\n"
+    "</joint>\n"
+    "<joint name='torso_lift_screw_torso_lift_joint'>\n"
+    "<angle axis='0'>-1.98549e-06</angle>\n"
+    "</joint>\n"
+    "<joint name='l_gripper_r_parallel_root_joint'>\n"
+    "<angle axis='0'>-1.12785e-06</angle>\n"
+    "</joint>\n"
+    "<joint name='l_gripper_r_parallel_tip_joint'>\n"
+    "<angle axis='0'>1.85524e-06</angle>\n"
+    "</joint>\n"
+    "</model>\n"
+    "\n"
+    "<model name='pr2'>\n"
+    "<pose>0 0 -1.5e-05 0 -1e-06 0</pose>\n"
+    "<joint name='bl_caster_l_wheel_joint'>\n"
+    "<angle axis='0'>1.01079e-06</angle>\n"
+    "</joint>\n"
+    "<joint name='br_caster_l_wheel_joint'>\n"
+    "<angle axis='0'>1.00809e-06</angle>\n"
+    "</joint>\n"
+    "<joint name='br_caster_r_wheel_joint'>\n"
+    "<angle axis='0'>1.00718e-06</angle>\n"
+    "</joint>\n"
+    "<joint name='fr_caster_l_wheel_joint'>\n"
+    "<angle axis='0'>1.01174e-06</angle>\n"
+    "</joint>\n"
+    "<joint name='fr_caster_r_wheel_joint'>\n"
+    "<angle axis='0'>1.0102e-06</angle>\n"
+    "</joint>\n"
+    "<joint name='torso_lift_joint'>\n"
+    "<angle axis='0'>1.61697e-06</angle>\n"
+    "</joint>\n"
+    "<joint name='head_tilt_joint'>\n"
+    "<angle axis='0'>1.59176e-06</angle>\n"
+    "</joint>\n"
+    "<joint name='l_shoulder_lift_joint'>\n"
+    "<angle axis='0'>-2.5384e-05</angle>\n"
+    "</joint>\n"
+    "<joint name='l_upper_arm_roll_joint'>\n"
+    "<angle axis='0'>2.25789e-05</angle>\n"
+    "</joint>\n"
+    "<joint name='l_elbow_flex_joint'>\n"
+    "<angle axis='0'>2.4368e-06</angle>\n"
+    "</joint>\n"
+    "<joint name='l_forearm_roll_joint'>\n"
+    "<angle axis='0'>-2.26431e-05</angle>\n"
+    "</joint>\n"
+    "<joint name='l_wrist_flex_joint'>\n"
+    "<angle axis='0'>4.61642e-05</angle>\n"
+    "</joint>\n"
+    "<joint name='l_wrist_roll_joint'>\n"
+    "<angle axis='0'>4.04443e-06</angle>\n"
+    "</joint>\n"
+    "<joint name='l_gripper_motor_screw_joint'>\n"
+    "<angle axis='0'>-1.66172e-05</angle>\n"
+    "</joint>\n"
+    "<joint name='l_gripper_r_finger_joint'>\n"
+    "<angle axis='0'>1.55625e-06</angle>\n"
+    "</joint>\n"
+    "<joint name='laser_tilt_mount_joint'>\n"
+    "<angle axis='0'>-4.67204e-06</angle>\n"
+    "</joint>\n"
+    "<joint name='r_shoulder_lift_joint'>\n"
+    "<angle axis='0'>-2.57981e-05</angle>\n"
+    "</joint>\n"
+    "<joint name='r_upper_arm_roll_joint'>\n"
+    "<angle axis='0'>-2.22655e-05</angle>\n"
+    "</joint>\n"
+    "<joint name='r_elbow_flex_joint'>\n"
+    "<angle axis='0'>4.39678e-06</angle>\n"
+    "</joint>\n"
+    "<joint name='r_forearm_roll_joint'>\n"
+    "<angle axis='0'>2.22922e-05</angle>\n"
+    "</joint>\n"
+    "<joint name='r_wrist_flex_joint'>\n"
+    "<angle axis='0'>-1.43521e-05</angle>\n"
+    "</joint>\n"
+    "<joint name='r_wrist_roll_joint'>\n"
+    "<angle axis='0'>-2.56647e-05</angle>\n"
+    "</joint>\n"
+    "<joint name='r_gripper_l_finger_joint'>\n"
+    "<angle axis='0'>2.47396e-06</angle>\n"
+    "</joint>\n"
+    "<joint name='r_gripper_l_finger_tip_joint'>\n"
+    "<angle axis='0'>1.96603e-06</angle>\n"
+    "</joint>\n"
+    "<joint name='r_gripper_r_finger_joint'>\n"
+    "<angle axis='0'>5.37142e-06</angle>\n"
+    "</joint>\n"
+    "<joint name='r_gripper_r_finger_tip_joint'>\n"
+    "<angle axis='0'>3.84165e-06</angle>\n"
+    "</joint>\n"
+    "<joint name='torso_lift_motor_screw_joint'>\n"
+    "<angle axis='0'>0.000183324</angle>\n"
+    "</joint>\n"
+    "<joint name='torso_lift_screw_torso_lift_joint'>\n"
+    "<angle axis='0'>-2.25856e-06</angle>\n"
+    "</joint>\n"
+    "<joint name='r_gripper_r_parallel_tip_joint'>\n"
+    "<angle axis='0'>1.13052e-06</angle>\n"
+    "</joint>\n"
+    "<joint name='r_gripper_l_parallel_tip_joint'>\n"
+    "<angle axis='0'>1.09768e-06</angle>\n"
+    "</joint>\n"
+    "</model>";
+
+  EXPECT_EQ(validEcho, echo);
+
+  // Test joint filter
+  echo = custom_exec(
+      std::string("gzlog echo --filter pr2::r_upper_arm_roll_joint ") +
+      PROJECT_SOURCE_PATH + "/tools/test/data/pr2_state.log");
+  boost::trim_right(echo);
+
+  validEcho =
+    "<joint name='r_upper_arm_roll_joint'>\n"
+    "<angle axis='0'>-1.0234e-05</angle>\n"
+    "</joint>\n\n"
+    "<joint name='r_upper_arm_roll_joint'>\n"
+    "<angle axis='0'>-2.22655e-05</angle>\n"
+    "</joint>";
+
+  EXPECT_EQ(validEcho, echo);
+
+  // Test joint filter with angle
+  echo = custom_exec(
+      std::string("gzlog echo --filter pr2::r_upper_arm_roll_joint::0 ") +
+      PROJECT_SOURCE_PATH + "/tools/test/data/pr2_state.log");
+  boost::trim_right(echo);
+
+  validEcho =
+    "-1.0234e-05\n"
+    "-2.22655e-05";
+
+  EXPECT_EQ(validEcho, echo);
+}
+
+/////////////////////////////////////////////////
+/// Main
+int main(int argc, char **argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}