Ian Chen avatar Ian Chen committed 011d333 Merge

Merge from default, resolve conflict

Comments (0)

Files changed (53)

cmake/SearchForStuff.cmake

 
   ########################################
   # Find urdfdom and urdfdom_headers
-  pkg_check_modules(urdfdom_headers urdfdom_headers)
+  find_package(urdfdom_headers)
   if (NOT urdfdom_headers_FOUND)
     BUILD_WARNING ("urdfdom_headers not found, urdf parser will not be built.")
   endif ()
     set (HAVE_URDFDOM_HEADERS TRUE)
   endif ()
 
-  pkg_check_modules(urdfdom urdfdom)
+  find_package(urdfdom)
   if (NOT urdfdom_FOUND)
     BUILD_WARNING ("urdfdom not found, urdf parser will not be built.")
   endif ()
     set (HAVE_URDFDOM TRUE)
   endif ()
 
-  pkg_check_modules(console_bridge console_bridge)
+  find_package(console_bridge)
   if (NOT console_bridge_FOUND)
     BUILD_WARNING ("console_bridge not found, urdf parser will not be built.")
   endif ()

gazebo/CMakeLists.txt

 if(HAVE_BULLET)
   add_dependencies(gazebo_physics_bullet gazebo_msgs)
 endif()
+if (HAVE_Simbody)
+  add_dependencies(gazebo_physics_simbody gazebo_msgs)
+endif()
 
 gz_add_executable(gzmaster master_main.cc Master.cc gazebo.cc)
 gz_add_executable(gzserver server_main.cc Server.cc Master.cc gazebo.cc)
     // 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);

gazebo/common/LogPlay.cc

 
   // 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;
+}

gazebo/common/LogPlay.hh

       /// \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>;
     };

gazebo/gui/CMakeLists.txt

   ToolsWidget.cc
   TopicSelector.cc
   viewers/ImageView.cc
+  viewers/LaserView.cc
   viewers/TopicView.cc
   viewers/TextView.cc
   viewers/ViewFactory.cc
   ToolsWidget.hh
   TopicSelector.hh
   viewers/ImageView.hh
+  viewers/LaserView.hh
   viewers/TextView.hh
   viewers/TopicView.hh
 )

gazebo/gui/GuiTypes.hh

     class TopicView;
     class ImageView;
     class TextView;
+    class LaserView;
 
     /// \def TopicViewPtr
     /// \brief Boost shared pointer to a TopicView object
     /// \brief Boost shared pointer to a ImageView object
     typedef boost::shared_ptr<ImageView> ImageViewPtr;
 
+    /// \def LaserViewPtr
+    /// \brief Boost shared pointer to a LaserView object
+    typedef boost::shared_ptr<LaserView> LaserViewPtr;
+
     /// \def TextViewPtr
     /// \brief Boost shared pointer to a TextView object
     typedef boost::shared_ptr<TextView> TextViewPtr;

gazebo/gui/MainWindow.cc

 #include "gui/model_editor/BuildingEditorPalette.hh"
 #include "gui/model_editor/EditorEvents.hh"
 
+#include "sdf/sdf.hh"
+
 using namespace gazebo;
 using namespace gui;
 
   splitter->addWidget(this->toolsWidget);
 
   QList<int> sizes;
-  sizes.push_back(300);
-  sizes.push_back(300);
-  sizes.push_back(700);
-  sizes.push_back(300);
+
+  sizes.push_back(250);
+  sizes.push_back(250);
+  sizes.push_back(this->width() - 250);
+  sizes.push_back(0);
   splitter->setSizes(sizes);
-  splitter->setStretchFactor(0, 1);
-  splitter->setStretchFactor(1, 1);
+
+  splitter->setStretchFactor(0, 0);
+  splitter->setStretchFactor(1, 0);
   splitter->setStretchFactor(2, 2);
-  splitter->setStretchFactor(3, 1);
+  splitter->setStretchFactor(3, 0);
   splitter->setCollapsible(2, false);
+  splitter->setHandleWidth(10);
+
 
   centerLayout->addWidget(splitter);
   centerLayout->setContentsMargins(0, 0, 0, 0);
     transport::request(get_world(), "world_sdf");
 
   msgs::GzString msg;
+  std::string msgData;
 
   // Make sure the response is correct
   if (response->response() != "error" && response->type() == msg.GetTypeName())
   {
-    // Parse the response
+    // Parse the response message
     msg.ParseFromString(response->serialized_data());
 
+    // Parse the string into sdf, so that we can insert user camera settings.
+    sdf::SDF sdf_parsed;
+    sdf_parsed.SetFromString(msg.data());
+    // Check that sdf contains world
+    if (sdf_parsed.root->HasElement("world"))
+    {
+      sdf::ElementPtr world = sdf_parsed.root->GetElement("world");
+      sdf::ElementPtr guiElem = world->GetElement("gui");
+
+      if (guiElem->HasAttribute("fullscreen"))
+        guiElem->GetAttribute("fullscreen")->Set(g_fullscreen);
+
+      sdf::ElementPtr cameraElem = guiElem->GetElement("camera");
+      rendering::UserCameraPtr cam = gui::get_active_camera();
+
+      cameraElem->GetElement("pose")->Set(cam->GetWorldPose());
+      cameraElem->GetElement("view_controller")->Set(
+        cam->GetViewControllerTypeString());
+      // TODO: export track_visual properties as well.
+      msgData = sdf_parsed.root->ToString("");
+    }
+    else
+    {
+      msgData = msg.data();
+      gzerr << "Unable to parse world file to add user camera settings.\n";
+    }
+
     // Open the file
     std::ofstream out(this->saveFilename.c_str(), std::ios::out);
 
       msgBox.exec();
     }
     else
-      out << msg.data();
+      out << msgData;
 
     out.close();
   }
       cam->SetViewController(_msg->camera().view_controller());
     }
 
-    if (_msg->camera().has_view_controller())
-    {
-      cam->SetViewController(_msg->camera().view_controller());
-    }
-
     if (_msg->camera().has_track())
     {
       std::string name = _msg->camera().track().name();

gazebo/gui/TopicSelector.cc

 /////////////////////////////////////////////////
 void TopicSelector::OnCancel()
 {
+  this->msgType.clear();
+  this->topicName.clear();
   this->done(QDialog::Rejected);
 }
 

gazebo/gui/style.qss

   alternate-background-color: #606060;
 
   selection-color: #f58113;
-  selection-background-color: transparent;
+  selection-background-color: #808080;
+  show-decoration-selected: 0;
 
-  border: 0px solid #737373;
+  border: 1px solid #737373;
   border-radius: 4;
 }
 
+QListView::item
+{
+  color: #ffffff;
+  border-radius: 4px;
+}
+
 QTreeView
 {
   color: #d0d0d0;
   padding: 2px;
 }
 
-QComboBox
-{
-  border: 1px solid #101010;
-  background-color: #808080;
-  color: #d0d0d0;
-  border-radius: 4px;
-}
-
-QListView
-{
-  border: 1px solid #000000;
-  border-radius: 4px;
-}
-
-QListView::item
-{
-  color: #ffffff;
-  border-radius: 4px;
-}
-
-/*
-QListWidget#topicTextList::item
-{
-  border-bottom: 1px solid black;
-  border-radius: 4px;
-  background-color: #404040;
-  margin: 4px;
-}
-*/
-
 QListWidget#topicTextList::item
 {
   border-bottom: 1px solid black;
   margin: 4px;
 }
 
-QComboBox QListView::item
+QComboBox
 {
-  color: #f58113;
-  background: #ffffff;
+  border: 1px solid #101010;
+  background-color: #808080;
+  color: #d0d0d0;
+  border-radius: 4px;
+
+  selection-color: #f58113;
+  selection-background-color: #808080;
+  show-decoration-selected: 0;
 }
 
 QComboBox:on
 {
-  /* shift the text when the popup opens */
   padding-top: -10px;
   padding-left: 4px;
 }
 
-/* This modifies the drop-down arrow container */
 QComboBox::drop-down
 {
    subcontrol-origin: padding;
 
    border-left-width: 0px;
    border-left-color: #000000;
-   border-left-style: solid; /* just a single line */
-   border-top-right-radius: 3px; /* same radius as the QComboBox */
+   border-left-style: solid;
+   border-top-right-radius: 3px;
    border-bottom-right-radius: 3px;
 }
 

gazebo/gui/viewers/ImageView.cc

   TopicView::SetTopic(_topicName);
 
   // Subscribe to the new topic.
-  this->sub.reset();
   this->sub = this->node->Subscribe(_topicName, &ImageView::OnImage, this);
 }
 

gazebo/gui/viewers/LaserView.cc

  * limitations under the License.
  *
  */
+
 #include "gazebo/transport/Transport.hh"
 #include "gazebo/transport/Node.hh"
 #include "gazebo/transport/Publisher.hh"
 
+#include "gazebo/math/Vector2d.hh"
+
 #include "gazebo/gui/viewers/ViewFactory.hh"
 #include "gazebo/gui/viewers/LaserView.hh"
 
 using namespace gazebo;
 using namespace gui;
 
-GZ_REGISTER_STATIC_VIEWER("gazebo.msgs.ImageStamped", LaserView)
+GZ_REGISTER_STATIC_VIEWER("gazebo.msgs.LaserScanStamped", LaserView)
 
 /////////////////////////////////////////////////
 LaserView::LaserView(QWidget *_parent)
-: TopicView(_parent, "gazebo.msgs.ImageStamped", "laser")
+: TopicView(_parent, "gazebo.msgs.LaserScanStamped", "laser")
 {
   this->setWindowTitle(tr("Gazebo: Laser View"));
 
-  // Create the image display
+  this->firstMsg = true;
+
+  // Create the laser display
   // {
   QVBoxLayout *frameLayout = new QVBoxLayout;
 
-  this->pixmap = QPixmap(":/images/no_image.png");
-  QPixmap image = (this->pixmap.scaled(320, 240, Qt::KeepAspectRatio,
-                                 Qt::SmoothTransformation));
-  this->imageLabel = new QLabel();
-  this->imageLabel->setPixmap(image);
-  this->imageLabel->setSizePolicy(QSizePolicy::Ignored, QSizePolicy::Ignored);
-  this->imageLabel->setMinimumSize(320, 240);
-  this->imageLabel->setScaledContents(true);
+  QGraphicsScene *scene = new QGraphicsScene();
 
-  frameLayout->addWidget(this->imageLabel);
+  QBrush brush(QColor(250, 250, 250), Qt::SolidPattern);
+  scene->setBackgroundBrush(brush);
+
+  this->view = new CustomView(this);
+  this->view->setScene(scene);
+  this->view->setDragMode(QGraphicsView::ScrollHandDrag);
+  this->view->centerOn(QPointF(0, 0));
+  this->view->setMinimumHeight(240);
+
+  this->laserItem = new LaserView::LaserItem();
+  scene->addItem(this->laserItem);
+
+  QHBoxLayout *controlLayout = new QHBoxLayout;
+
+  QPushButton *fitButton = new QPushButton("Fit in View");
+  connect(fitButton, SIGNAL(clicked()), this, SLOT(OnFitInView()));
+
+  QRadioButton *degreeToggle = new QRadioButton();
+  degreeToggle->setText("Degrees");
+  connect(degreeToggle, SIGNAL(toggled(bool)), this, SLOT(OnDegree(bool)));
+
+  controlLayout->addWidget(degreeToggle);
+  controlLayout->addWidget(fitButton);
+  controlLayout->addStretch(1);
+
+  frameLayout->addWidget(this->view);
+  frameLayout->addLayout(controlLayout);
+
   this->frame->setObjectName("blackBorderFrame");
   this->frame->setLayout(frameLayout);
   // }
 /////////////////////////////////////////////////
 void LaserView::SetTopic(const std::string &_topicName)
 {
+  this->firstMsg = true;
+
   TopicView::SetTopic(_topicName);
 
   // Subscribe to the new topic.
-  this->sub.reset();
   this->sub = this->node->Subscribe(_topicName, &LaserView::OnScan, this);
 }
 
 /////////////////////////////////////////////////
-void LaserView::OnScan(ConstLaserScanPtr &_msg)
+void LaserView::resizeEvent(QResizeEvent *_event)
+{
+  // Automatically fit the laser item to the view if the user has not zoomed
+  // in/out
+  if (!this->view->viewZoomed && this->laserItem)
+  {
+    QRectF bound = this->laserItem->GetBoundingRect();
+    this->view->fitInView(bound, Qt::KeepAspectRatio);
+  }
+
+  QDialog::resizeEvent(_event);
+}
+
+/////////////////////////////////////////////////
+void LaserView::OnScan(ConstLaserScanStampedPtr &_msg)
 {
   // Update the Hz and Bandwidth info
   this->OnMsg(msgs::Convert(_msg->time()), _msg->ByteSize());
+
+  this->laserItem->Clear();
+
+  double angle = _msg->scan().angle_min();
+
+  double r;
+  for (unsigned int i = 0;
+       i < static_cast<unsigned int>(_msg->scan().ranges_size()); i++)
+  {
+    r = _msg->scan().ranges(i) + _msg->scan().range_min();
+
+    if (i+1 >= this->laserItem->GetRangeCount())
+      this->laserItem->AddRange(r);
+    else
+      this->laserItem->SetRange(i+1, r);
+
+    angle += _msg->scan().angle_step();
+  }
+
+  // Recalculate the points to draw.
+  this->laserItem->Update(
+      _msg->scan().angle_min(),
+      _msg->scan().angle_max(),
+      _msg->scan().angle_step(),
+      _msg->scan().range_max(),
+      _msg->scan().range_min());
+
+  if (this->firstMsg)
+  {
+    QRectF bound = this->laserItem->GetBoundingRect();
+    this->view->fitInView(bound, Qt::KeepAspectRatio);
+    this->firstMsg = false;
+  }
 }
+
+/////////////////////////////////////////////////
+void LaserView::OnFitInView()
+{
+  QRectF bound = this->laserItem->GetBoundingRect();
+  this->view->fitInView(bound, Qt::KeepAspectRatio);
+  this->view->viewZoomed = false;
+}
+
+/////////////////////////////////////////////////
+void LaserView::OnDegree(bool _toggled)
+{
+  this->laserItem->radians = !_toggled;
+}
+
+/////////////////////////////////////////////////
+LaserView::LaserItem::LaserItem()
+{
+  this->setFlag(QGraphicsItem::ItemIsSelectable, false);
+  this->setAcceptHoverEvents(true);
+  this->indexAngle = -999;
+  this->scale = 100.0;
+  this->rangeMax = 8.0;
+  this->rangeMin = 0.0;
+  this->radians = true;
+}
+
+/////////////////////////////////////////////////
+void LaserView::LaserItem::paint(QPainter *_painter,
+    const QStyleOptionGraphicsItem * /*_option*/, QWidget * /*_widget*/)
+{
+  boost::mutex::scoped_lock lock(this->mutex);
+
+  QColor orange(245, 129, 19, 255);
+  QColor darkGrey(100, 100, 100, 255);
+  QColor lightGrey(100, 100, 100, 50);
+
+  _painter->setPen(QPen(darkGrey));
+  _painter->setBrush(lightGrey);
+
+  // Draw the filled polygon that represents the current laser data.
+  _painter->drawPolygon(&this->points[0], this->points.size());
+
+  // Draw a box with an arrow around the (0, 0) location
+  _painter->setPen(QPen(orange));
+  _painter->setBrush(QColor(0, 0, 0, 0));
+  _painter->drawRect(-10, -10, 20, 20);
+  _painter->drawLine(0, 0, 20, 0);
+  _painter->drawLine(25, 0, 20, -5);
+  _painter->drawLine(20, -5, 20, 5);
+  _painter->drawLine(20, 5, 25, 0);
+
+
+  // Compute the index of the ray that the mouse is hovering over.
+  int index = static_cast<int>(
+      rint((this->indexAngle - this->angleMin) / this->angleStep));
+
+  // Draw the ray and associated data if the index is valid.
+  if (index >= 0 && index < static_cast<int>(this->ranges.size()))
+  {
+    double x1, y1;
+    double x2, y2;
+
+    double rangeScaled = this->ranges[index] * this->scale;
+    double rangeMaxScaled = this->rangeMax * this->scale;
+
+    // Draw the ray
+    x1 = rangeScaled * cos(this->indexAngle);
+    y1 = -rangeScaled * sin(this->indexAngle);
+    _painter->setPen(QPen(orange));
+    _painter->drawLine(0, 0, x1, y1);
+
+    // Set the text for the range measurement
+    std::ostringstream stream;
+    stream << std::fixed << std::setprecision(4)
+           << this->ranges[index] << " m";
+
+    // Compute the size of the box we want to use for the range measurement.
+    // This will help us scale the text properly.
+    float textBoxWidth = (rangeMaxScaled * 1.2) -rangeMaxScaled;
+
+    // Compute the text scaling factor.
+    float textFactor = textBoxWidth /
+      _painter->fontMetrics().width(stream.str().c_str());
+
+    // Set the font according to the scaling factor.
+    QFont f = _painter->font();
+    f.setPointSizeF(f.pointSizeF() * textFactor);
+    _painter->setFont(f);
+
+    // The final text width and height
+    float textWidth = _painter->fontMetrics().width(stream.str().c_str());
+    float textHeight = _painter->fontMetrics().height();
+
+    // Compute the additional offset to apply to the range text. This will
+    // prevent the text from overlapping the range polygon.
+    double rAngle = M_PI - this->indexAngle;
+    double xOff = cos(rAngle) < 0 ? 0 : cos(rAngle) * textWidth;
+    double yOff = sin(rAngle) * textHeight;
+
+    // The location to draw the range text
+    x1 = (rangeScaled * 1.05) * cos(this->indexAngle);
+    y1 = -(rangeScaled * 1.05) * sin(this->indexAngle);
+
+    x1 -= xOff;
+    y1 -= yOff;
+
+    // Draw the range text
+    _painter->setPen(QPen(orange));
+    _painter->drawText(x1, y1, stream.str().c_str());
+
+    // This section draws the arc and the angle of the ray
+    {
+      // Give the arc some padding.
+      textWidth *= 1.4;
+
+      // Compute the position for the angle text.
+      x1 = (rangeMaxScaled * 1.15 + textWidth) * cos(this->indexAngle * 0.5);
+      y1 = -(rangeMaxScaled * 1.15 + textWidth) * sin(this->indexAngle * 0.5);
+
+      // Draw the text for the angle of the ray
+      stream.str(std::string());
+      if (this->radians)
+        stream << std::fixed << std::setprecision(4)
+          << this->indexAngle << " radians";
+      else
+        stream << std::fixed << std::setprecision(4)
+          << GZ_RTOD(this->indexAngle) << " degrees";
+
+      _painter->setPen(QPen(orange));
+      _painter->drawText(x1, y1, stream.str().c_str());
+
+      // Draw an arc that depicts the angle of the ray
+      QRectF rect(-(rangeMaxScaled * 1.1 + textWidth),
+                  -(rangeMaxScaled * 1.1 + textWidth),
+                  rangeMaxScaled * 1.1 * 2.0 + textWidth * 2.0,
+                  rangeMaxScaled * 1.1 * 2.0 + textWidth * 2.0);
+
+      _painter->setPen(QPen(orange));
+      _painter->drawArc(rect, 0, GZ_RTOD(this->indexAngle) * 16);
+
+
+      // Draw the line that marks the start of the arc
+      x1 = (rangeMaxScaled * 1.1 + textWidth);
+      x2 = (rangeMaxScaled * 1.15 + textWidth);
+
+      _painter->setPen(QPen(orange));
+      _painter->drawLine(x1, 0, x2, 0);
+
+      // Draw the line that marks the end of the arc
+      x1 = (rangeMaxScaled * 1.1 + textWidth) * cos(this->indexAngle);
+      y1 = -(rangeMaxScaled * 1.1 + textWidth) * sin(this->indexAngle);
+
+      x2 = (rangeMaxScaled * 1.15 + textWidth) * cos(this->indexAngle);
+      y2 = -(rangeMaxScaled * 1.15 + textWidth) * sin(this->indexAngle);
+
+      _painter->setPen(QPen(orange));
+      _painter->drawLine(x1, y1, x2, y2);
+    }
+  }
+}
+
+/////////////////////////////////////////////////
+QRectF LaserView::LaserItem::GetBoundingRect() const
+{
+  if (this->ranges.size() == 0)
+    return QRectF(0, 0, 0, 0);
+
+  // Compute the maximum size of bound box by scaling up the maximum
+  // possible range. The multiplication of 1.8 increases the bounding box
+  // size to make the mouse hover behavior easier to use.
+  double max = this->rangeMax * this->scale * 1.8;
+
+  // Return the top-left position and the width and height.
+  return QRectF(-max, -max, max * 2.0, max * 2.0);
+}
+
+/////////////////////////////////////////////////
+QRectF LaserView::LaserItem::boundingRect() const
+{
+  return this->GetBoundingRect();
+}
+
+/////////////////////////////////////////////////
+void LaserView::LaserItem::Clear()
+{
+  boost::mutex::scoped_lock lock(this->mutex);
+  this->ranges.clear();
+  this->points.clear();
+}
+
+/////////////////////////////////////////////////
+void LaserView::LaserItem::AddRange(double _range)
+{
+  boost::mutex::scoped_lock lock(this->mutex);
+  this->ranges.push_back(_range);
+}
+
+/////////////////////////////////////////////////
+void LaserView::LaserItem::SetRange(unsigned int _index, double _range)
+{
+  boost::mutex::scoped_lock lock(this->mutex);
+  if (_index < this->ranges.size())
+    this->ranges[_index] = _range;
+}
+
+/////////////////////////////////////////////////
+void LaserView::LaserItem::Update(double _angleMin, double _angleMax,
+    double _angleStep, double _rangeMax, double _rangeMin)
+{
+  boost::mutex::scoped_lock lock(this->mutex);
+
+  this->angleMin = _angleMin;
+  this->angleMax = _angleMax;
+  this->angleStep = _angleStep;
+  this->rangeMax = _rangeMax;
+  this->rangeMin = _rangeMin;
+
+  // Resize the point array if the number of ranges has changed.
+  if (this->rangeMin > 0.0 &&
+      this->ranges.size() * 2 != this->points.size())
+  {
+    // A min range > 0 means we have to draw an inner circle, so we twice as
+    // many points
+    this->points.resize(this->ranges.size() * 2);
+  }
+  else if (math::equal(this->rangeMin, 0.0) &&
+      this->ranges.size() + 1 != this->points.size())
+  {
+    // A min range == 0 mean we just need a closing point at the (0, 0)
+    // location
+    this->points.resize(this->ranges.size() + 1);
+  }
+
+  double angle = this->angleMin;
+
+  // Add a point for each laser reading
+  for (unsigned int i = 0; i < this->ranges.size(); ++i)
+  {
+    QPointF pt(this->ranges[i] * this->scale * cos(angle),
+        -this->ranges[i] * this->scale * sin(angle));
+    this->points[i] = pt;
+    angle += this->angleStep;
+  }
+
+  // If a min range is set, then draw an inner circle
+  if (this->rangeMin > 0.0)
+  {
+    // Create the inner circle that denotes the min range
+    for (int i = this->ranges.size()-1; i >=0 ; --i)
+    {
+      QPointF pt(this->rangeMin * this->scale * cos(angle),
+          -this->rangeMin * this->scale * sin(angle));
+      this->points[i + this->ranges.size()] = pt;
+      angle -= this->angleStep;
+    }
+  }
+  else
+  {
+    // Connect the last point to the (0,0)
+    this->points[this->ranges.size()] = QPointF(0, 0);
+  }
+
+  // Tell QT we have changed.
+  this->prepareGeometryChange();
+}
+
+/////////////////////////////////////////////////
+unsigned int LaserView::LaserItem::GetRangeCount()
+{
+  boost::mutex::scoped_lock lock(this->mutex);
+  return this->ranges.size();
+}
+
+/////////////////////////////////////////////////
+void LaserView::LaserItem::hoverEnterEvent(QGraphicsSceneHoverEvent *_event)
+{
+  this->indexAngle = atan2(-_event->pos().y(), _event->pos().x());
+
+  QApplication::setOverrideCursor(Qt::CrossCursor);
+}
+
+//////////////////////////////////////////////////
+void LaserView::LaserItem::hoverLeaveEvent(
+    QGraphicsSceneHoverEvent * /*_event*/)
+{
+  this->indexAngle = -999;
+  QApplication::setOverrideCursor(Qt::OpenHandCursor);
+}
+
+/////////////////////////////////////////////////
+void LaserView::LaserItem::hoverMoveEvent(QGraphicsSceneHoverEvent *_event)
+{
+  this->indexAngle = atan2(-_event->pos().y(), _event->pos().x());
+}

gazebo/gui/viewers/LaserView.hh

 #define _LASERVIEW_HH_
 
 #include <string>
+#include <vector>
+#include <boost/thread/mutex.hpp>
 
 #include "gazebo/common/Time.hh"
 #include "gazebo/msgs/msgs.hh"
 
       /// \brief Constructor
       /// \param[in] _parent Pointer to the parent widget.
-      public: LaserView(QWidget *_parent);
+      public: LaserView(QWidget *_parent = NULL);
 
       /// \brief Destructor
       public: virtual ~LaserView();
       // Documentation inherited
       private: virtual void UpdateImpl();
 
+      /// \brief QT event, called when view is resized.
+      /// \param[in] _event Pointer to the resize event info.
+      protected: virtual void resizeEvent(QResizeEvent *_event);
+
       /// \brief Receives incoming laser scan messages.
       /// \param[in] _msg New laser scan message.
-      private: void OnScan(ConstLaserScantampedPtr &_msg);
+      private: void OnScan(ConstLaserScanStampedPtr &_msg);
+
+      /// \brief QT callback. Used when the "fit in view" button is pressed.
+      private slots: void OnFitInView();
+
+      /// \brief QT callback. Used when the "degrees" button is pressed.
+      private slots: void OnDegree(bool _toggled);
+
+      /// \brief Class that draws the laser ranges.
+      private: class LaserItem : public QGraphicsItem
+               {
+                 /// \brief Constructor
+                 public: LaserItem();
+
+                 /// \brief Clear all the stored ranges.
+                 public: void Clear();
+
+                 /// \brief Get the number of range values.
+                 /// \return The size of the ranges vector.
+                 public: unsigned int GetRangeCount();
+
+                 /// \brief Add a new range value.
+                 /// \param[in] _range The new range value.
+                 public: void AddRange(double _range);
+
+                 /// \brief Set a specific range value.
+                 /// \param[in] _index Index of the range to set.
+                 /// \param[in] _range The value of the range.
+                 public: void SetRange(unsigned int _index, double _range);
+
+                 /// \brief Return the bounding rectangle for this item.
+                 public: QRectF GetBoundingRect() const;
+
+                 /// \brief Update the list of points to draw.
+                 /// \param[in] _angleMin Minimum angle, in radians.
+                 /// \param[in] _angleMax Maximum angle, in radians.
+                 /// \param[in] _angleStep Angle step size, in radians.
+                 /// \param[in] _rangeMax Maximum range in meters.
+                 /// \param[in] _rangeMin Minimum range in meters.
+                 public: void Update(double _angleMin, double _angleMax,
+                             double _angleStep, double _rangeMax,
+                             double _rangeMin);
+
+                 /// \brief A QT pure virtual function that must be defined.
+                 /// This calls GetBoundingRect.
+                 private: virtual QRectF boundingRect() const;
+
+                 /// \brief A QT function that is called when the item
+                 /// should be redrawn.
+                 /// \param[in] _painter Pointer to the QPainter, which
+                 /// draws shapes in the scene.
+                 /// \param[in] _option An unsued graphics options.
+                 /// \param[in] _widget Option param, the widget to paint
+                 /// on.
+                 private: virtual void paint(QPainter *_painter,
+                              const QStyleOptionGraphicsItem *_option,
+                              QWidget *_widget);
+
+                 /// \brief QT event that is called when the mouse starts to
+                 /// hover over this item.
+                 /// \param[in] _event The hover event info.
+                 private: virtual void hoverEnterEvent(
+                              QGraphicsSceneHoverEvent *_event);
+
+                 /// \brief QT event that is called when the mouse leaves
+                 /// this item.
+                 /// \param[in] _event The hover event info.
+                 private: virtual void hoverLeaveEvent(
+                              QGraphicsSceneHoverEvent *_event);
+
+                 /// \brief QT event that is called when the mouse moves
+                 /// over this item.
+                 /// \param[in] _event The hover event info.
+                 private: void hoverMoveEvent(QGraphicsSceneHoverEvent *_event);
+
+                 /// \brief The vector of points to draw.
+                 private: std::vector<QPointF> points;
+
+                 /// \brief The vector of range values, as returned by the
+                 /// laser sensor.
+                 private: std::vector<double> ranges;
+
+                 /// \brief The minimum angle of the laser.
+                 private: double angleMin;
+
+                 /// \brief The maximum angle of the laser.
+                 private: double angleMax;
+
+                 /// \brief The size of the step between rays, in radians.
+                 private: double angleStep;
+
+                 /// \brief The maximum range value.
+                 private: double rangeMax;
+
+                 /// \brief The minimum range value.
+                 private: double rangeMin;
+
+                 /// \brief Angle that is highlighted by the mouse position.
+                 private: double indexAngle;
+
+                 /// \brief Scaling factor that is applied to range data.
+                 private: double scale;
+
+                 /// \brief True to draw the hover angles in radians
+                 public: bool radians;
+
+                 /// \brief Mutex to protect the laser data.
+                 private: mutable boost::mutex mutex;
+               };
+
+      /// \brief This class exists so that we can properly capture the
+      /// QT wheelEvent.
+      private: class CustomView : public QGraphicsView
+               {
+                 /// \brief Constructor
+                 /// \param[in] _parent Pointer to the parent widget.
+                 public: CustomView(QWidget *_parent)
+                         : QGraphicsView(_parent), viewZoomed(false) {}
+
+                 /// \brief QT callback. Used when a wheel event occurs.
+                 /// \param[in] _event QT wheel event info.
+                 private: void wheelEvent(QWheelEvent *_event)
+                 {
+                   this->viewZoomed = true;
+                   _event->delta() > 0 ? this->scale(1.15, 1.15) :
+                                         this->scale(1.0 / 1.15, 1.0 / 1.15);
+                   _event->accept();
+                 }
+
+                 /// \brief True if the view has been zoomed in/out by the user.
+                 public: bool viewZoomed;
+               };
+
+      /// \brief The item that draws the laser data.
+      private: LaserItem *laserItem;
+
+      /// \brief The view that displays the laser item.
+      private: CustomView *view;
+
+      /// \brief Flag used to determine if a recieved message is the first.
+      private: bool firstMsg;
     };
   }
 }

gazebo/gui/viewers/TextView.cc

 {
   TopicView::SetTopic(_topicName);
 
+  boost::mutex::scoped_lock lock(this->mutex);
   this->msg = msgs::MsgFactory::NewMsg(this->msgTypeName);
 
   this->msgList->clear();
   // Subscribe to the new topic if we have generated an appropriate message
   if (this->msg)
   {
-    this->sub.reset();
     this->sub = this->node->Subscribe(_topicName, &TextView::OnText, this);
   }
   else
 {
   if (this->paused)
     return;
+  boost::mutex::scoped_lock lock(this->mutex);
 
   // Update the Hz and Bandwidth info.
   this->OnMsg(common::Time::GetWallTime(), _msg.size());
   // Convert the raw data to a message.
   this->msg->ParseFromString(_msg);
 
-  {
-    boost::mutex::scoped_lock lock(this->mutex);
+  // Create a new list item.
+  QListWidgetItem *item = new QListWidgetItem(
+      QString::fromStdString(msg->DebugString()));
 
-    // Create a new list item.
-    QListWidgetItem *item = new QListWidgetItem(
-        QString::fromStdString(msg->DebugString()));
+  // Add the new text to the output view.
+  this->msgList->addItem(item);
 
-    // Add the new text to the output view.
-    this->msgList->addItem(item);
-
-    // Remove items if the list is too long.
-    while (this->msgList->count() > this->bufferSize)
-      delete this->msgList->takeItem(0);
-  }
+  // Remove items if the list is too long.
+  while (this->msgList->count() > this->bufferSize)
+    delete this->msgList->takeItem(0);
 }
 
 /////////////////////////////////////////////////

gazebo/gui/viewers/TextView.hh

       /// \brief The protobuf message used to decode incoming message data.
       private: boost::shared_ptr<google::protobuf::Message> msg;
 
-      /// \brief Mutex to protec message buffer.
+      /// \brief Mutex to protect message buffer.
       private: boost::mutex mutex;
 
       /// \brief Flag used to pause message parsing.

gazebo/gui/viewers/TopicView.cc

   QLabel *topicLabel = new QLabel(tr("Topic: "));
   this->topicCombo = new TopicCombo(this, this->msgTypeName,
       _viewType, this->node);
-  this->topicCombo->setObjectName("comboList");
   this->topicCombo->setMinimumSize(300, 25);
 
   topicLayout->addSpacing(10);
   mainLayout->addWidget(frame);
   this->setLayout(mainLayout);
   this->layout()->setContentsMargins(8, 8, 8, 10);
+  this->setSizeGripEnabled(true);
 
-  this->setSizeGripEnabled(true);
   QTimer::singleShot(500, this, SLOT(Update()));
+
+  this->setWindowFlags(Qt::Window);
 }
 
 /////////////////////////////////////////////////
 /////////////////////////////////////////////////
 void TopicView::Update()
 {
+  boost::mutex::scoped_lock lock(this->updateMutex);
+
   // Update the child class.
   this->UpdateImpl();
 
 /////////////////////////////////////////////////
 void TopicView::OnTopicChanged(int _index)
 {
+  boost::mutex::scoped_lock lock(this->updateMutex);
+
   // Set the current topic based on the index of the item selected in the
   // combobox
   this->SetTopic(this->topicCombo->itemText(_index).toStdString());
   if (_topicName.empty())
     return;
 
+  this->sub.reset();
   this->msgTypeName = transport::getTopicMsgType(
       this->node->DecodeTopicName(_topicName));
 
 /////////////////////////////////////////////////
 void TopicCombo::UpdateList()
 {
+  boost::mutex::scoped_lock lock(this->mutex);
+
   QString myText = this->currentText();
 
   this->blockSignals(true);
 
   this->blockSignals(false);
 }
+
+//////////////////////////////////////////////////
+void TopicView::closeEvent(QCloseEvent * /*_event*/)
+{
+  this->sub.reset();
+  this->node.reset();
+}

gazebo/gui/viewers/TopicView.hh

 
 #include <string>
 #include <list>
+#include <boost/thread/mutex.hpp>
 
 #include "gazebo/common/Time.hh"
 #include "gazebo/msgs/msgs.hh"
 
       /// \brief Tranport node pointer.
       private: transport::NodePtr node;
+
+      private: boost::mutex mutex;
     };
 
     class TopicView : public QDialog
       /// \param[in] _size Size of the message in bytes.
       protected: void OnMsg(const common::Time &_dataTime, int _size);
 
+      /// \brief Qt close event callback.
+      /// \param[in] _event The close event info.
+      protected: virtual void closeEvent(QCloseEvent *_event);
+
       /// \brief Update the camera sensor widget.
       private slots: void Update();
 
 
       /// \brief A list of clock times that messages have been received.
       private: std::list<common::Time> times;
+
+      /// \brief A mutex to protect the update cycle.
+      private: boost::mutex updateMutex;
     };
   }
 }

gazebo/gui/viewers/ViewFactory.cc

 #include "gazebo/gui/viewers/ViewFactory.hh"
 
 #include "gazebo/gui/viewers/ImageView.hh"
+#include "gazebo/gui/viewers/LaserView.hh"
 #include "gazebo/gui/viewers/TextView.hh"
 
 void RegisterImageView();
+void RegisterLaserView();
 
 using namespace gazebo;
 using namespace gui;
 /////////////////////////////////////////////////
 void ViewFactory::RegisterAll()
 {
+  RegisterLaserView();
   RegisterImageView();
 }
 

gazebo/physics/Model.cc

 {
   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);

gazebo/physics/ModelState.cc

 {
   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 =

gazebo/physics/ModelState.hh

       /// \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 =

gazebo/physics/World.cc

   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;
 }
 

gazebo/rendering/Scene.cc

       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");

gazebo/rendering/UserCamera.cc

 }
 
 //////////////////////////////////////////////////
+std::string UserCamera::GetViewControllerTypeString()
+{
+  if (this->viewController)
+    return this->viewController->GetTypeString();
+  return "";
+}
+
+//////////////////////////////////////////////////
 void UserCamera::Resize(unsigned int /*_w*/, unsigned int /*_h*/)
 {
   if (this->viewport)

gazebo/rendering/UserCamera.hh

       public: void SetViewController(const std::string &_type,
                                      const math::Vector3 &_pos);
 
+      /// \brief Get current view controller type.
+      /// \return Type of the current view controller: "orbit", "fps"
+      public: std::string GetViewControllerTypeString();
+
       /// \brief Resize the camera.
       /// \param[in] _w Width of the camera image.
       /// \param[in] _h Height of the camera image.

gazebo/sdf/interface/parser.cc

   {
 #ifdef HAVE_URDFDOM
     urdf2gazebo::URDF2Gazebo u2g;
-    TiXmlDocument doc = u2g.initModelFile(filename);
+    TiXmlDocument doc = u2g.InitModelFile(filename);
     if (sdf::readDoc(&doc, _sdf, "urdf file"))
     {
       gzwarn << "parse from urdf file [" << filename << "].\n";
   {
 #ifdef HAVE_URDFDOM
     urdf2gazebo::URDF2Gazebo u2g;
-    TiXmlDocument doc = u2g.initModelString(_xmlString);
+    TiXmlDocument doc = u2g.InitModelString(_xmlString);
     if (sdf::readDoc(&doc, _sdf, "urdf string"))
     {
       gzwarn << "parse from urdf.\n";

gazebo/sdf/interface/parser_urdf.cc

 #include <algorithm>
 #include <string>
 
-#include "common/SystemPaths.hh"
+#include "gazebo/common/SystemPaths.hh"
 
 namespace urdf2gazebo
 {
+////////////////////////////////////////////////////////////////////////////////
 std::string lowerStr(std::string str)
 {
   std::string out = str;
   return out;
 }
 
+////////////////////////////////////////////////////////////////////////////////
 URDF2Gazebo::URDF2Gazebo()
 {
     // default options
-    this->enforce_limits = true;
-    this->reduce_fixed_joints = true;
+    this->enforceLimits = true;
+    this->reduceFixedJoints = true;
 }
 
+////////////////////////////////////////////////////////////////////////////////
 URDF2Gazebo::~URDF2Gazebo()
 {
 }
 
-
-urdf::Vector3 URDF2Gazebo::parseVector3(TiXmlNode* key, double scale)
+////////////////////////////////////////////////////////////////////////////////
+urdf::Vector3 URDF2Gazebo::ParseVector3(TiXmlNode* _key, double _scale)
 {
-  if (key != NULL)
+  if (_key != NULL)
   {
-    std::string str = key->Value();
+    std::string str = _key->Value();
     std::vector<std::string> pieces;
     std::vector<double> vals;
 
       {
         try
         {
-          vals.push_back(scale
+          vals.push_back(_scale
                          * boost::lexical_cast<double>(pieces[i].c_str()));
         }
         catch(boost::bad_lexical_cast &e)
     return urdf::Vector3(0, 0, 0);
 }
 
-void URDF2Gazebo::reduceVisualToParent(boost::shared_ptr<urdf::Link> link,
-       std::string group_name, boost::shared_ptr<urdf::Visual> visual)
+////////////////////////////////////////////////////////////////////////////////
+void URDF2Gazebo::ReduceVisualToParent(UrdfLinkPtr _link,
+       const std::string &_groupName, UrdfVisualPtr _visual)
 {
-  boost::shared_ptr<std::vector<boost::shared_ptr<urdf::Visual > > > viss
-    = link->getVisuals(group_name);
+  boost::shared_ptr<std::vector<UrdfVisualPtr> > viss
+    = _link->getVisuals(_groupName);
   if (!viss)
   {
     // group does not exist, create one and add to map
-    viss.reset(new std::vector<boost::shared_ptr<urdf::Visual > >);
+    viss.reset(new std::vector<UrdfVisualPtr>);
     // new group name, create vector, add vector to map and
     //   add Visual to the vector
-    link->visual_groups.insert(make_pair(group_name, viss));
+    _link->visual_groups.insert(make_pair(_groupName, viss));
     // gzdbg << "successfully added a new visual group name ["
-    //       << group_name << "]\n";
+    //       << _groupName << "]\n";
   }
 
   // group exists, add Visual to the vector in the map if it's not there
-  std::vector<boost::shared_ptr<urdf::Visual > >::iterator vis_it
-    = find(viss->begin(), viss->end(), visual);
-  if (vis_it != viss->end())
+  std::vector<UrdfVisualPtr>::iterator visIt
+    = find(viss->begin(), viss->end(), _visual);
+  if (visIt != viss->end())
     gzwarn << "attempted to add visual to link ["
-           << link->name
+           << _link->name
            << "], but it already exists under group ["
-           << group_name << "]\n";
+           << _groupName << "]\n";
   else
-    viss->push_back(visual);
+    viss->push_back(_visual);
 }
 
-void URDF2Gazebo::reduceCollisionToParent(boost::shared_ptr<urdf::Link> link,
-      std::string group_name, boost::shared_ptr<urdf::Collision> collision)
+////////////////////////////////////////////////////////////////////////////////
+void URDF2Gazebo::ReduceCollisionToParent(UrdfLinkPtr _link,
+      const std::string &_groupName, UrdfCollisionPtr _collision)
 {
-  boost::shared_ptr<std::vector<boost::shared_ptr<urdf::Collision > > >
-    cols = link->getCollisions(group_name);
+  boost::shared_ptr<std::vector<UrdfCollisionPtr> >
+    cols = _link->getCollisions(_groupName);
   if (!cols)
   {
     // group does not exist, create one and add to map
-    cols.reset(new std::vector<boost::shared_ptr<urdf::Collision > >);
+    cols.reset(new std::vector<UrdfCollisionPtr>);
     // new group name, create add vector to map and add Collision to the vector
-    link->collision_groups.insert(make_pair(group_name, cols));
+    _link->collision_groups.insert(make_pair(_groupName, cols));
   }
 
   // group exists, add Collision to the vector in the map
-  std::vector<boost::shared_ptr<urdf::Collision > >::iterator col_it =
-    find(cols->begin(), cols->end(), collision);
-  if (col_it != cols->end())
+  std::vector<UrdfCollisionPtr>::iterator colIt =
+    find(cols->begin(), cols->end(), _collision);
+  if (colIt != cols->end())
     gzwarn << "attempted to add collision to link ["
-           << link->name
+           << _link->name
            << "], but it already exists under group ["
-           << group_name << "]\n";
+           << _groupName << "]\n";
   else
-    cols->push_back(collision);
+    cols->push_back(_collision);
 }
 
-std::string URDF2Gazebo::vector32str(const urdf::Vector3 vector)
+////////////////////////////////////////////////////////////////////////////////
+std::string URDF2Gazebo::Vector32Str(const urdf::Vector3 _vector)
 {
   std::stringstream ss;
-  ss << vector.x;
+  ss << _vector.x;
   ss << " ";
-  ss << vector.y;
+  ss << _vector.y;
   ss << " ";
-  ss << vector.z;
+  ss << _vector.z;
   return ss.str();
 }
 
-std::string URDF2Gazebo::values2str(unsigned int count, const double *values)
+////////////////////////////////////////////////////////////////////////////////
+std::string URDF2Gazebo::Values2str(unsigned int _count, const double *_values)
 {
   std::stringstream ss;
-  for (unsigned int i = 0 ; i < count ; ++i)
+  for (unsigned int i = 0 ; i < _count ; ++i)
   {
       if (i > 0)
           ss << " ";
-      ss << values[i];
+      ss << _values[i];
   }
   return ss.str();
 }
 
-void URDF2Gazebo::addKeyValue(TiXmlElement *elem, const std::string& key,
-  const std::string &value)
+////////////////////////////////////////////////////////////////////////////////
+void URDF2Gazebo::AddKeyValue(TiXmlElement *_elem, const std::string &_key,
+  const std::string &_value)
 {
-  TiXmlElement* child_elem = elem->FirstChildElement(key);
-  if (child_elem)
+  TiXmlElement* childElem = _elem->FirstChildElement(_key);
+  if (childElem)
   {
-    std::string old_value = getKeyValueAsString(child_elem);
-    if (old_value != value)
-      gzwarn << "multiple inconsistent <" << key
+    std::string oldValue = this->GetKeyValueAsString(childElem);
+    if (oldValue != _value)
+      gzwarn << "multiple inconsistent <" << _key
              << "> exists due to fixed joint reduction"
-             << " overwriting previous value [" << old_value
-             << "] with [" << value << "].\n";
+             << " overwriting previous value [" << oldValue
+             << "] with [" << _value << "].\n";
     // else
-    //   gzdbg << "multiple consistent <" << key
-    //          << "> exists with [" << value
+    //   gzdbg << "multiple consistent <" << _key
+    //          << "> exists with [" << _value
     //          << "] due to fixed joint reduction.\n";
-    elem->RemoveChild(child_elem);  // remove old elem
+    _elem->RemoveChild(childElem);  // remove old _elem
   }
 
-  TiXmlElement *ekey      = new TiXmlElement(key);
-  TiXmlText    *text_ekey = new TiXmlText(value);
-  ekey->LinkEndChild(text_ekey);
-  elem->LinkEndChild(ekey);
+  TiXmlElement *ekey      = new TiXmlElement(_key);
+  TiXmlText    *textEkey = new TiXmlText(_value);
+  ekey->LinkEndChild(textEkey);
+  _elem->LinkEndChild(ekey);
 }
 
-void URDF2Gazebo::addTransform(TiXmlElement *elem,
-  const::gazebo::math::Pose& transform)
+////////////////////////////////////////////////////////////////////////////////
+void URDF2Gazebo::AddTransform(TiXmlElement *_elem,
+  const::gazebo::math::Pose& _transform)
 {
-  gazebo::math::Vector3 e = transform.rot.GetAsEuler();
-  double cpose[6] = { transform.pos.x, transform.pos.y,
-                      transform.pos.z, e.x, e.y, e.z };
+  gazebo::math::Vector3 e = _transform.rot.GetAsEuler();
+  double cpose[6] = { _transform.pos.x, _transform.pos.y,
+                      _transform.pos.z, e.x, e.y, e.z };
 
   /* set geometry transform */
-  addKeyValue(elem, "pose", values2str(6, cpose));
+  this->AddKeyValue(_elem, "pose", this->Values2str(6, cpose));
 }
 
-std::string URDF2Gazebo::getKeyValueAsString(TiXmlElement* elem)
+////////////////////////////////////////////////////////////////////////////////
+std::string URDF2Gazebo::GetKeyValueAsString(TiXmlElement* _elem)
 {
-  std::string value_str;
-  if (elem->Attribute("value"))
+  std::string valueStr;
+  if (_elem->Attribute("value"))
   {
-    value_str = elem->Attribute("value");
+    valueStr = _elem->Attribute("value");
   }
-  else if (elem->FirstChild())
+  else if (_elem->FirstChild())
   /// @todo: FIXME: comment out check for now, different tinyxml
   /// versions fails to compile:
-  //  && elem->FirstChild()->Type() == TiXmlNode::TINYXML_TEXT)
+  //  && _elem->FirstChild()->Type() == TiXmlNode::TINYXML_TEXT)
   {
-    value_str = elem->FirstChild()->ValueStr();
+    valueStr = _elem->FirstChild()->ValueStr();
   }
-  return value_str;
+  return valueStr;
 }
 
-void URDF2Gazebo::parseGazeboExtension(TiXmlDocument &urdf_xml)
+////////////////////////////////////////////////////////////////////////////////
+void URDF2Gazebo::ParseGazeboExtension(TiXmlDocument &_urdfXml)
 {
-  TiXmlElement* robot_xml = urdf_xml.FirstChildElement("robot");
+  TiXmlElement* robotXml = _urdfXml.FirstChildElement("robot");
 
   // Get all Gazebo extension elements, put everything in
-  //   this->gazebo_extensions_ map, containing a key string
+  //   this->extensions map, containing a key string
   //   (link/joint name) and values
-  for (TiXmlElement* gazebo_xml = robot_xml->FirstChildElement("gazebo");
-       gazebo_xml; gazebo_xml = gazebo_xml->NextSiblingElement("gazebo"))
+  for (TiXmlElement* gazeboXml = robotXml->FirstChildElement("gazebo");
+       gazeboXml; gazeboXml = gazeboXml->NextSiblingElement("gazebo"))
   {
-    const char* ref = gazebo_xml->Attribute("reference");
-    std::string ref_str;
+    const char* ref = gazeboXml->Attribute("reference");
+    std::string refStr;
     if (!ref)
     {
       // copy extensions for robot (outside of link/joint)
-      ref_str.clear();
+      refStr.clear();
     }
     else
     {
       // copy extensions for link/joint
-      ref_str = std::string(ref);
+      refStr = std::string(ref);
     }
 
-    if (this->gazebo_extensions_.find(ref_str) ==
-        this->gazebo_extensions_.end())
+    if (this->extensions.find(refStr) ==
+        this->extensions.end())
     {
         // create extension map for reference
-        std::vector<GazeboExtension*> extensions;
-        this->gazebo_extensions_.insert(std::make_pair(ref_str, extensions));
+        std::vector<GazeboExtension*> ge;
+        this->extensions.insert(std::make_pair(refStr, ge));
     }
 
     // create and insert a new GazeboExtension into the map
     GazeboExtension* gazebo = new GazeboExtension();
 
     // begin parsing xml node
-    for (TiXmlElement *child_elem = gazebo_xml->FirstChildElement();
-         child_elem; child_elem = child_elem->NextSiblingElement())
+    for (TiXmlElement *childElem = gazeboXml->FirstChildElement();
+         childElem; childElem = childElem->NextSiblingElement())
     {
-      gazebo->old_link_name = ref_str;
+      gazebo->oldLinkName = refStr;
 
       // go through all elements of the extension,
       //   extract what we know, and save the rest in blobs
       //         objects
 
       // material
-      if (child_elem->ValueStr() == "material")
+      if (childElem->ValueStr() == "material")
       {
-          gazebo->material = getKeyValueAsString(child_elem);
+          gazebo->material = this->GetKeyValueAsString(childElem);
       }
-      else if (child_elem->ValueStr() == "static")
+      else if (childElem->ValueStr() == "static")
       {
-        std::string value_str = getKeyValueAsString(child_elem);
+        std::string valueStr = this->GetKeyValueAsString(childElem);
 
         // default of setting static flag is false
-        if (lowerStr(value_str) == "true" || lowerStr(value_str) == "yes" ||
-            value_str == "1")
+        if (lowerStr(valueStr) == "true" || lowerStr(valueStr) == "yes" ||
+            valueStr == "1")
           gazebo->setStaticFlag = true;
         else
           gazebo->setStaticFlag = false;
       }
-      else if (child_elem->ValueStr() == "gravity")
+      else if (childElem->ValueStr() == "gravity")
       {
-        std::string value_str = getKeyValueAsString(child_elem);
+        std::string valueStr = this->GetKeyValueAsString(childElem);
 
         // default of gravity is true
-        if (lowerStr(value_str) == "false" || lowerStr(value_str) == "no" ||
-            value_str == "0")
+        if (lowerStr(valueStr) == "false" || lowerStr(valueStr) == "no" ||
+            valueStr == "0")
           gazebo->gravity = false;
         else
           gazebo->gravity = true;
       }
-      else if (child_elem->ValueStr() == "damping_factor")
+      else if (childElem->ValueStr() == "dampingFactor")
       {
-          gazebo->is_damping_factor = true;
-          gazebo->damping_factor = boost::lexical_cast<double>(
-              getKeyValueAsString(child_elem).c_str());
+          gazebo->isDampingFactor = true;
+          gazebo->dampingFactor = boost::lexical_cast<double>(
+              this->GetKeyValueAsString(childElem).c_str());
       }
-      else if (child_elem->ValueStr() == "maxVel")
+      else if (childElem->ValueStr() == "maxVel")
       {
-          gazebo->is_maxVel = true;
+          gazebo->isMaxVel = true;
           gazebo->maxVel = boost::lexical_cast<double>(
-              getKeyValueAsString(child_elem).c_str());
+              this->GetKeyValueAsString(childElem).c_str());
       }
-      else if (child_elem->ValueStr() == "minDepth")
+      else if (childElem->ValueStr() == "minDepth")
       {
-          gazebo->is_minDepth = true;
+          gazebo->isMinDepth = true;
           gazebo->minDepth = boost::lexical_cast<double>(
-            getKeyValueAsString(child_elem).c_str());
+            this->GetKeyValueAsString(childElem).c_str());
       }
-      else if (child_elem->ValueStr() == "mu1")
+      else if (childElem->ValueStr() == "mu1")
       {
-          gazebo->is_mu1 = true;
+          gazebo->isMu1 = true;
           gazebo->mu1 = boost::lexical_cast<double>(
-            getKeyValueAsString(child_elem).c_str());
+            this->GetKeyValueAsString(childElem).c_str());
       }
-      else if (child_elem->ValueStr() == "mu2")
+      else if (childElem->ValueStr() == "mu2")
       {
-          gazebo->is_mu2 = true;
+          gazebo->isMu2 = true;
           gazebo->mu2 = boost::lexical_cast<double>(
-            getKeyValueAsString(child_elem).c_str());
+            this->GetKeyValueAsString(childElem).c_str());
       }
-      else if (child_elem->ValueStr() == "fdir1")
+      else if (childElem->ValueStr() == "fdir1")
       {
-          gazebo->fdir1 = getKeyValueAsString(child_elem);
+          gazebo->fdir1 = this->GetKeyValueAsString(childElem);
       }
-      else if (child_elem->ValueStr() == "kp")
+      else if (childElem->ValueStr() == "kp")
       {
-          gazebo->is_kp = true;
+          gazebo->isKp = true;
           gazebo->kp = boost::lexical_cast<double>(
-            getKeyValueAsString(child_elem).c_str());
+            this->GetKeyValueAsString(childElem).c_str());
       }
-      else if (child_elem->ValueStr() == "kd")
+      else if (childElem->ValueStr() == "kd")
       {
-          gazebo->is_kd = true;