Commits

John Hsu  committed 9fad69c Merge

merging from default

  • Participants
  • Parent commits aa9dc72, c6ec36e
  • Branches ode_gearbox

Comments (0)

Files changed (20)

File gazebo/gui/CMakeLists.txt

 
 set (headers
   qt.h
+  qt_test.h
   BoxMaker.hh
   CylinderMaker.hh
   EntityMaker.hh

File gazebo/gui/LightMaker.cc

 
   this->light->SetLightType(this->lightTypename);
   this->light->SetPosition(math::Vector3(0, 0, 1));
+  if (this->lightTypename == "directional")
+    this->light->SetDirection(math::Vector3(.1, .1, -0.9));
 
   std::ostringstream stream;
   stream << "user_" << this->lightTypename << "_light_" << counter++;

File gazebo/gui/QTestFixture.hh

 #include "gazebo/rendering/rendering.hh"
 #include "gazebo/Server.hh"
 #include "gazebo/gui/qt.h"
+#include "gazebo/gui/qt_test.h"
 
 #include "gazebo_config.h"
 #include "test_config.h"

File gazebo/gui/qt.h

 #include <QApplication>
 #include <qmainwindow.h>
 #include <QAction>
-#include <QtTest/QtTest>
 
 #endif

File gazebo/gui/qt_test.h

+/*
+ * Copyright 2011 Nate Koenig
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+
+#ifndef GAZEBO_QT_TEST_HEADERS_H_
+#define GAZEBO_QT_TEST_HEADERS_H_
+
+#pragma GCC system_header
+
+#include <QtTest/QtTest>
+#endif

File gazebo/physics/Entity.cc

 
 #include "msgs/msgs.hh"
 
-#include "common/Events.hh"
-#include "common/Console.hh"
-#include "common/Animation.hh"
-#include "common/KeyFrame.hh"
+#include "gazebo/common/Assert.hh"
+#include "gazebo/common/Events.hh"
+#include "gazebo/common/Console.hh"
+#include "gazebo/common/Animation.hh"
+#include "gazebo/common/KeyFrame.hh"
 
-#include "transport/Publisher.hh"
-#include "transport/Transport.hh"
-#include "transport/Node.hh"
+#include "gazebo/transport/Publisher.hh"
+#include "gazebo/transport/Transport.hh"
+#include "gazebo/transport/Node.hh"
 
-#include "physics/RayShape.hh"
-#include "physics/Collision.hh"
-#include "physics/Model.hh"
-#include "physics/Link.hh"
-#include "physics/World.hh"
-#include "physics/PhysicsEngine.hh"
-#include "physics/Entity.hh"
+#include "gazebo/physics/RayShape.hh"
+#include "gazebo/physics/Collision.hh"
+#include "gazebo/physics/Model.hh"
+#include "gazebo/physics/Link.hh"
+#include "gazebo/physics/World.hh"
+#include "gazebo/physics/PhysicsEngine.hh"
+#include "gazebo/physics/Entity.hh"
 
 using namespace gazebo;
 using namespace physics;
 //////////////////////////////////////////////////
 void Entity::PublishPose()
 {
-  math::Pose relativePose = this->GetRelativePose();
-  if (relativePose != msgs::Convert(*this->poseMsg))
-  {
-    msgs::Set(this->poseMsg, relativePose);
-    this->world->EnqueueMsg(this->poseMsg);
-  }
+  GZ_ASSERT(this->GetParentModel() != NULL,
+      "An entity without a parent model should not happen");
+
+  this->world->PublishModelPose(this->GetParentModel()->GetName());
 }
 
 //////////////////////////////////////////////////
     return boost::shared_dynamic_cast<Model>(shared_from_this());
 
   p = this->parent;
+  GZ_ASSERT(p, "Parent of an entity is NULL");
 
   while (p->GetParent() && p->GetParent()->HasType(MODEL))
     p = p->GetParent();

File gazebo/physics/Joint.cc

   else
     msgs::Set(_msg.mutable_pose(), math::Pose(0, 0, 0, 0, 0, 0));
 
-  msgs::Set(_msg.mutable_pose()->mutable_position(), this->anchorPos);
-
-
   if (this->HasType(Base::HINGE_JOINT))
   {
     _msg.set_type(msgs::Joint::REVOLUTE);
     _msg.add_angle(this->GetAngle(1).Radian());
   }
 
-  // msgs::Set(_msg.mutable_axis1()->mutable_xyz(), this->GetGlobalAxis(0));
-  msgs::Set(_msg.mutable_axis1()->mutable_xyz(), math::Vector3());
+  msgs::Set(_msg.mutable_axis1()->mutable_xyz(), this->GetLocalAxis(0));
   _msg.mutable_axis1()->set_limit_lower(0);
   _msg.mutable_axis1()->set_limit_upper(0);
   _msg.mutable_axis1()->set_limit_effort(0);
   double dampingForce = -this->dampingCoefficient * this->GetVelocity(0);
   this->SetForce(0, dampingForce);
 }
-

File gazebo/physics/World.cc

   this->node = transport::NodePtr(new transport::Node());
   this->node->Init(this->GetName());
 
-  this->posePub = this->node->Advertise<msgs::Pose_V>("~/pose/info", 10);
+  this->posePub = this->node->Advertise<msgs::Pose_V>("~/pose/info", 10, 60.0);
 
   this->guiPub = this->node->Advertise<msgs::GUI>("~/gui");
   if (this->sdf->HasElement("gui"))
     _sdf->PrintValues("  ");
   }
 
+  this->PublishModelPose(model->GetName());
   return model;
 }
 
 void World::ProcessMessages()
 {
   boost::recursive_mutex::scoped_lock lock(*this->receiveMutex);
+  msgs::Pose *poseMsg;
 
   if (this->posePub && this->posePub->HasConnections() &&
-      this->poseMsgs.pose_size() > 0)
+      this->publishModelPoses.size() > 0)
   {
-    this->posePub->Publish(this->poseMsgs);
+    msgs::Pose_V msg;
+
+    for (std::set<std::string>::iterator iter =
+         this->publishModelPoses.begin();
+         iter != this->publishModelPoses.end(); ++iter)
+    {
+      ModelPtr model = this->GetModel(*iter);
+
+      // It's possible that the model was deleted somewhere along the line.
+      // So check to make sure we get a valid model pointer.
+      if (!model)
+        continue;
+
+      poseMsg = msg.add_pose();
+
+      // Publish the model's relative pose
+      poseMsg->set_name(model->GetScopedName());
+      msgs::Set(poseMsg, model->GetRelativePose());
+
+      // Publish each of the model's children relative poses
+      Link_V links = model->GetLinks();
+      for (Link_V::iterator linkIter = links.begin();
+           linkIter != links.end(); ++linkIter)
+      {
+        poseMsg = msg.add_pose();
+        poseMsg->set_name((*linkIter)->GetScopedName());
+        msgs::Set(poseMsg, (*linkIter)->GetRelativePose());
+      }
+    }
+    this->posePub->Publish(msg);
   }
-  this->poseMsgs.clear_pose();
+  this->publishModelPoses.clear();
 
   if (common::Time::GetWallTime() - this->prevProcessMsgsTime >
       this->processMsgsPeriod)
 }
 
 //////////////////////////////////////////////////
-void World::EnqueueMsg(msgs::Pose *_msg)
+void World::PublishModelPose(const std::string &_modelName)
 {
-  if (!_msg)
-    return;
+  boost::recursive_mutex::scoped_lock lock(*this->receiveMutex);
 
-  boost::recursive_mutex::scoped_lock lock(*this->receiveMutex);
-  int i = 0;
-
-  // Replace old pose messages with the new one.
-  for (; i < this->poseMsgs.pose_size(); ++i)
-  {
-    if (this->poseMsgs.pose(i).name() == _msg->name())
-    {
-      this->poseMsgs.mutable_pose(i)->CopyFrom(*_msg);
-      break;
-    }
-  }
-
-  // Add the pose message if no old pose messages were found.
-  if (i >= this->poseMsgs.pose_size())
-      this->poseMsgs.add_pose()->CopyFrom(*_msg);
+  // Only add if the model name is not in the list
+  this->publishModelPoses.insert(_modelName);
 }
 
 //////////////////////////////////////////////////

File gazebo/physics/World.hh

 
 #include <vector>
 #include <list>
+#include <set>
 #include <deque>
 #include <string>
 #include <boost/enable_shared_from_this.hpp>
       /// \return True if World::Load has completed.
       public: bool IsLoaded() const;
 
-      /// \brief Enqueue a pose message for publication.
-      /// These messages will be transmitted at the end of every iteration.
-      /// \param[in] _msg The message to enqueue.
-      public: void EnqueueMsg(msgs::Pose *_msg);
+      /// \brief Publish pose updates for a model.
+      /// This list of models to publish is processed and cleared once every
+      /// iteration.
+      /// \param[in] _modelName Name of the model to publish.
+      public: void PublishModelPose(const std::string &_modelName);
 
       /// \cond
       /// This is an internal function.
       private: sdf::SDFPtr factorySDF;
 
       /// \brief The list of pose messages to output.
-      private: msgs::Pose_V poseMsgs;
+      private: std::set<std::string> publishModelPoses;
 
       /// \brief Info passed through the WorldUpdateBegin event.
       private: common::UpdateInfo updateInfo;

File gazebo/rendering/JointVisual.cc

       new AxisVisual(this->GetName() + "_AXIS", shared_from_this()));
   this->axisVisual->Load();
 
-  this->SetWorldPosition(msgs::Convert(_msg->pose().position()));
-  this->SetWorldRotation(msgs::Convert(_msg->pose().orientation()));
+  this->SetPosition(msgs::Convert(_msg->pose().position()));
+  this->SetRotation(msgs::Convert(_msg->pose().orientation()));
 
   if (math::equal(_msg->axis1().xyz().x(), 1.0))
     this->axisVisual->ShowRotation(0);

File gazebo/rendering/Scene.cc

 /////////////////////////////////////////////////
 bool Scene::ProcessJointMsg(ConstJointPtr &_msg)
 {
-  VisualPtr childVis;
+  Visual_M::iterator iter;
+  iter = this->visuals.find(_msg->name() + "_JOINT_VISUAL__");
 
-  if (_msg->child() == "world")
-    childVis = this->worldVisual;
-  else
-    childVis = this->GetVisual(_msg->child());
+  if (iter == this->visuals.end())
+  {
+    VisualPtr childVis;
 
-  if (!childVis)
-    return false;
+    if (_msg->child() == "world")
+      childVis = this->worldVisual;
+    else
+      childVis = this->GetVisual(_msg->child());
 
-  JointVisualPtr jointVis(new JointVisual(
-          _msg->name() + "_JOINT_VISUAL__", childVis));
-  jointVis->Load(_msg);
-  jointVis->SetVisible(this->showJoints);
+    if (!childVis)
+      return false;
 
-  this->visuals[jointVis->GetName()] = jointVis;
+    JointVisualPtr jointVis(new JointVisual(
+            _msg->name() + "_JOINT_VISUAL__", childVis));
+    jointVis->Load(_msg);
+    jointVis->SetVisible(this->showJoints);
+
+    this->visuals[jointVis->GetName()] = jointVis;
+  }
   return true;
 }
 

File gazebo/sdf/interface/SDF.cc

     const std::string &_type, const std::string &_defaultValue, bool _required,
     const std::string &_description)
 {
+  if (_type == "char")
+  {
+    boost::shared_ptr<ParamT<char> > param(
+        new ParamT<char>(_key, _defaultValue, _required, _type,
+                           _description));
+    return param;
+  }
   if (_type == "double")
   {
     boost::shared_ptr<ParamT<double> > param(
                                        _type, _description));
     return param;
   }
+  else if (_type == "quaternion")
+  {
+    boost::shared_ptr<ParamT<gazebo::math::Quaternion> > param(
+        new ParamT<gazebo::math::Quaternion>(_key, _defaultValue, _required,
+                                       _type, _description));
+    return param;
+  }
   else if (_type == "time")
   {
     boost::shared_ptr<ParamT<gazebo::common::Time> > param(
   bool result = false;
 
   if (_key.empty())
-    this->value->Get(result);
+  {
+    if (this->value)
+    {
+      this->value->Get(result);
+    }
+    else
+    {
+      gzwarn << "Parameter [" << this->GetName()
+             << "] has no value when attempting to get as a bool.\n";
+    }
+  }
   else
   {
     ParamPtr param = this->GetAttribute(_key);
     else
       gzerr << "Unable to find value for key[" << _key << "]\n";
   }
+
   return result;
 }
 
 int Element::GetValueInt(const std::string &_key)
 {
   int result = 0;
+
   if (_key.empty())
-    this->value->Get(result);
+  {
+    if (this->value)
+    {
+      this->value->Get(result);
+    }
+    else
+    {
+      gzwarn << "Parameter [" << this->GetName()
+             << "] has no value when attempting to get as an int.\n";
+    }
+  }
   else
   {
     ParamPtr param = this->GetAttribute(_key);
 float Element::GetValueFloat(const std::string &_key)
 {
   float result = 0.0;
+
   if (_key.empty())
-    this->value->Get(result);
+  {
+    if (this->value)
+    {
+      this->value->Get(result);
+    }
+    else
+    {
+      gzwarn << "Parameter [" << this->GetName()
+             << "] has no value when attempting to get as a float.\n";
+    }
+  }
   else
   {
     ParamPtr param = this->GetAttribute(_key);
 double Element::GetValueDouble(const std::string &_key)
 {
   double result = 0.0;
+
   if (_key.empty())
   {
-    if (this->value->IsStr())
-      result = boost::lexical_cast<double>(this->value->GetAsString());
+    if (this->value)
+    {
+      if (this->value->IsStr())
+        result = boost::lexical_cast<double>(this->value->GetAsString());
+      else
+        this->value->Get(result);
+    }
     else
-      this->value->Get(result);
+    {
+      gzwarn << "Parameter [" << this->GetName()
+             << "] has no value when attempting to get as a double.\n";
+    }
   }
   else
   {
   unsigned int result = 0;
   if (_key.empty())
   {
-    if (this->value->IsStr())
-      result = boost::lexical_cast<unsigned int>(this->value->GetAsString());
+    if (this->value)
+    {
+      if (this->value->IsStr())
+        result = boost::lexical_cast<unsigned int>(this->value->GetAsString());
+      else
+        this->value->Get(result);
+    }
     else
-      this->value->Get(result);
+    {
+      gzwarn << "Parameter [" << this->GetName()
+             << "] has no value when attempting to get as an unsigned int.\n";
+    }
   }
   else
   {
 char Element::GetValueChar(const std::string &_key)
 {
   char result = '\0';
+
   if (_key.empty())
   {
-    if (this->value->IsStr())
-      result = boost::lexical_cast<char>(this->value->GetAsString());
+    if (this->value)
+    {
+      if (this->value->IsStr())
+        result = boost::lexical_cast<char>(this->value->GetAsString());
+      else
+        this->value->Get(result);
+    }
     else
-      this->value->Get(result);
+    {
+      gzwarn << "Parameter [" << this->GetName()
+             << "] has no value when attempting to get as a char.\n";
+    }
   }
   else
   {
 std::string Element::GetValueString(const std::string &_key)
 {
   std::string result = "";
+
   if (_key.empty())
-    this->value->Get(result);
+  {
+    if (this->value)
+    {
+      this->value->Get(result);
+    }
+    else
+    {
+      gzwarn << "Parameter [" << this->GetName()
+             << "] has no value, returning empty string.\n";
+    }
+  }
   else
   {
     ParamPtr param = this->GetAttribute(_key);
 gazebo::math::Vector3 Element::GetValueVector3(const std::string &_key)
 {
   gazebo::math::Vector3 result;
+
   if (_key.empty())
-    this->value->Get(result);
+  {
+    if (this->value)
+    {
+      this->value->Get(result);
+    }
+    else
+    {
+      gzwarn << "Parameter [" << this->GetName()
+             << "] has no value when attempting to get as a vector3.\n";
+    }
+  }
   else
   {
     ParamPtr param = this->GetAttribute(_key);
 gazebo::math::Vector2d Element::GetValueVector2d(const std::string &_key)
 {
   gazebo::math::Vector2d result;
+
   if (_key.empty())
-    this->value->Get(result);
+  {
+    if (this->value)
+    {
+      this->value->Get(result);
+    }
+    else
+    {
+      gzwarn << "Parameter [" << this->GetName()
+             << "] has no value when attempting to get as a vector2d.\n";
+    }
+  }
   else
   {
     ParamPtr param = this->GetAttribute(_key);
 gazebo::math::Quaternion Element::GetValueQuaternion(const std::string &_key)
 {
   gazebo::math::Quaternion result;
+
   if (_key.empty())
-    this->value->Get(result);
+  {
+    if (this->value)
+    {
+      this->value->Get(result);
+    }
+    else
+    {
+      gzwarn << "Parameter [" << this->GetName()
+             << "] has no value when attempting to get as a quaternion.\n";
+    }
+  }
   else
   {
     ParamPtr param = this->GetAttribute(_key);
 gazebo::math::Pose Element::GetValuePose(const std::string &_key)
 {
   gazebo::math::Pose result;
+
   if (_key.empty())
-    this->value->Get(result);
+  {
+    if (this->value)
+    {
+      this->value->Get(result);
+    }
+    else
+    {
+      gzwarn << "Parameter [" << this->GetName()
+             << "] has no value when attempting to get as a pose.\n";
+    }
+  }
   else
   {
     ParamPtr param = this->GetAttribute(_key);
 gazebo::common::Color Element::GetValueColor(const std::string &_key)
 {
   gazebo::common::Color result;
+
   if (_key.empty())
-    this->value->Get(result);
+  {
+    if (this->value)
+    {
+      this->value->Get(result);
+    }
+    else
+    {
+      gzwarn << "Parameter [" << this->GetName()
+             << "] has no value when attempting to get as a color.\n";
+    }
+  }
   else
   {
     ParamPtr param = this->GetAttribute(_key);
 gazebo::common::Time Element::GetValueTime(const std::string &_key)
 {
   gazebo::common::Time result;
+
   if (_key.empty())
-    this->value->Get(result);
+  {
+    if (this->value)
+    {
+      this->value->Get(result);
+    }
+    else
+    {
+      gzwarn << "Parameter [" << this->GetName()
+             << "] has no value when attempting to get as a time.\n";
+    }
+  }
   else
   {
     ParamPtr param = this->GetAttribute(_key);

File gazebo/sdf/interface/SDF_TEST.cc

 #include <gtest/gtest.h>
 
 #include "gazebo/sdf/sdf.hh"
-#include "gazebo/math/Pose.hh"
+#include "gazebo/common/common.hh"
+#include "gazebo/math/gzmath.hh"
 
 class SdfUpdateFixture
 {
   EXPECT_FALSE(elem);
 }
 
+////////////////////////////////////////////////////
+/// Ensure that getting empty values with empty keys returns correct values.
+TEST(SdfUpdate, EmptyValues)
+{
+  std::string emptyString;
+  sdf::ElementPtr elem;
+
+  elem.reset(new sdf::Element());
+  EXPECT_FALSE(elem->GetValueBool(emptyString));
+  elem->AddValue("bool", "true", "0", "description");
+  EXPECT_TRUE(elem->GetValueBool(emptyString));
+
+  elem.reset(new sdf::Element());
+  EXPECT_EQ(elem->GetValueInt(emptyString), 0);
+  elem->AddValue("int", "12", "0", "description");
+  EXPECT_EQ(elem->GetValueInt(emptyString), 12);
+
+  elem.reset(new sdf::Element());
+  EXPECT_EQ(elem->GetValueUInt(emptyString), static_cast<unsigned int>(0));
+  elem->AddValue("unsigned int", "123", "0", "description");
+  EXPECT_EQ(elem->GetValueUInt(emptyString), static_cast<unsigned int>(123));
+
+  elem.reset(new sdf::Element());
+  EXPECT_EQ(elem->GetValueChar(emptyString), '\0');
+  elem->AddValue("char", "a", "0", "description");
+  EXPECT_EQ(elem->GetValueChar(emptyString), 'a');
+
+  elem.reset(new sdf::Element());
+  EXPECT_EQ(elem->GetValueString(emptyString), "");
+  elem->AddValue("string", "hello", "0", "description");
+  EXPECT_EQ(elem->GetValueString(emptyString), "hello");
+
+  elem.reset(new sdf::Element());
+  EXPECT_EQ(elem->GetValueVector2d(emptyString), gazebo::math::Vector2d());
+  elem->AddValue("vector2d", "1 2", "0", "description");
+  EXPECT_EQ(elem->GetValueVector2d(emptyString), gazebo::math::Vector2d(1, 2));
+
+  elem.reset(new sdf::Element());
+  EXPECT_EQ(elem->GetValueVector3(emptyString), gazebo::math::Vector3());
+  elem->AddValue("vector3", "1 2 3", "0", "description");
+  EXPECT_EQ(elem->GetValueVector3(emptyString), gazebo::math::Vector3(1, 2, 3));
+
+  elem.reset(new sdf::Element());
+  EXPECT_EQ(elem->GetValueQuaternion(emptyString), gazebo::math::Quaternion());
+  elem->AddValue("quaternion", "1 2 3", "0", "description");
+  EXPECT_EQ(elem->GetValueQuaternion(emptyString),
+            gazebo::math::Quaternion(-2.14159, 1.14159, -0.141593));
+
+  elem.reset(new sdf::Element());
+  EXPECT_EQ(elem->GetValuePose(emptyString), gazebo::math::Pose());
+  elem->AddValue("pose", "1 2 3 4 5 6", "0", "description");
+  EXPECT_EQ(elem->GetValuePose(emptyString),
+            gazebo::math::Pose(1, 2, 3, 4, 5, 6));
+
+  elem.reset(new sdf::Element());
+  EXPECT_EQ(elem->GetValueColor(emptyString), gazebo::common::Color());
+  elem->AddValue("color", ".1 .2 .3 1.0", "0", "description");
+  EXPECT_EQ(elem->GetValueColor(emptyString),
+            gazebo::common::Color(.1, .2, .3, 1.0));
+
+  elem.reset(new sdf::Element());
+  EXPECT_EQ(elem->GetValueTime(emptyString), gazebo::common::Time());
+  elem->AddValue("time", "1 2", "0", "description");
+  EXPECT_EQ(elem->GetValueTime(emptyString), gazebo::common::Time(1, 2));
+
+  elem.reset(new sdf::Element());
+  EXPECT_NEAR(elem->GetValueFloat(emptyString), 0.0, 1e-6);
+  elem->AddValue("float", "12.34", "0", "description");
+  EXPECT_NEAR(elem->GetValueFloat(emptyString), 12.34, 1e-6);
+
+  elem.reset(new sdf::Element());
+  EXPECT_NEAR(elem->GetValueDouble(emptyString), 0.0, 1e-6);
+  elem->AddValue("double", "12.34", "0", "description");
+  EXPECT_NEAR(elem->GetValueDouble(emptyString), 12.34, 1e-6);
+}
 
 /////////////////////////////////////////////////
 /// Main

File gazebo/transport/Node.hh

       /// \param[in] _topic The topic to advertise
       /// \param[in] _queueLimit The maximum number of outgoing messages to
       /// queue for delivery
+      /// \param[in] _hz Update rate for the publisher. Units are
+      /// 1.0/seconds.
       /// \return Pointer to new publisher object
       public: template<typename M>
       transport::PublisherPtr Advertise(const std::string &_topic,
-                                        unsigned int _queueLimit = 1000)
+                                        unsigned int _queueLimit = 1000,
+                                        double _hzRate = 0)
       {
         std::string decodedTopic = this->DecodeTopicName(_topic);
         PublisherPtr publisher =
           transport::TopicManager::Instance()->Advertise<M>(
-              decodedTopic, _queueLimit);
+              decodedTopic, _queueLimit, _hzRate);
 
         boost::recursive_mutex::scoped_lock lock(this->publisherMutex);
         this->publishers.push_back(publisher);

File gazebo/transport/Publisher.cc

 {
   this->prevMsg = NULL;
   this->queueLimitWarned = false;
+  this->updatePeriod = 0;
 }
 
 //////////////////////////////////////////////////
 Publisher::Publisher(const std::string &_topic, const std::string &_msgType,
-                     unsigned int _limit)
-  : topic(_topic), msgType(_msgType), queueLimit(_limit)
+                     unsigned int _limit, double _hzRate)
+  : topic(_topic), msgType(_msgType), queueLimit(_limit),
+    updatePeriod(0)
 {
+  if (!math::equal(_hzRate, 0.0))
+    this->updatePeriod = 1.0 / _hzRate;
+
   this->prevMsg = NULL;
   this->queueLimitWarned = false;
 }
   // if (!this->HasConnections())
   // return;
 
+  // Check if a throttling rate has been set
+  if (this->updatePeriod > 0)
+  {
+    // Get the current time
+    this->currentTime = common::Time::GetWallTime();
+
+    // Skip publication if the time difference is less than the update period.
+    if (this->prevPublishTime != common::Time(0, 0) &&
+        (this->currentTime - this->prevPublishTime).Double() <
+         this->updatePeriod)
+    {
+      return;
+    }
+
+    // Set the previous time a message was published
+    this->prevPublishTime = this->currentTime;
+  }
+
   // Save the latest message
   google::protobuf::Message *msg = _message.New();
   msg->CopyFrom(_message);

File gazebo/transport/Publisher.hh

       /// \param[in] _topic Name of topic to be published
       /// \param[in] _msgType Type of the message to be published
       /// \param[in] _limit Maximum number of outgoing messages to queue
+      /// \param[in] _hz Update rate for the publisher. Units are
+      /// 1.0/seconds.
       public: Publisher(const std::string &_topic, const std::string &_msgType,
-                        unsigned int _limit);
+                        unsigned int _limit, double _hzRate);
 
       /// \brief Destructor
       public: virtual ~Publisher();
       /// \brief Callback when a publish is completed
       private: void OnPublishComplete();
 
+      /// \brief Topic on which messages are published.
       private: std::string topic;
+
+      /// \brief Type of message published.
       private: std::string msgType;
+
+      /// \brief Maximum number of messages that can be queued prior to
+      /// publication.
       private: unsigned int queueLimit;
+
+      /// \brief Period at which messages are published. Zero indicates no
+      /// limit.
+      private: double updatePeriod;
+
+      /// \brief True if queueLimit has been reached, and a warning message
+      /// was produced.
       private: bool queueLimitWarned;
+
+      /// \brief List of messages to publish.
       private: std::list<google::protobuf::Message *> messages;
+
+      /// \brief For mutual exclusion.
       private: mutable boost::recursive_mutex mutex;
+
+      /// \brief The publication pointers. One for normal publication, and
+      /// one for debug.
       private: PublicationPtr publications[2];
 
+      /// \brief The previous message published. Used for latching topics.
       private: google::protobuf::Message *prevMsg;
+
+      private: common::Time currentTime;
+      private: common::Time prevPublishTime;
     };
     /// \}
   }
 }
-
 #endif
-
-

File gazebo/transport/TopicManager.hh

       /// \param[in] _topic The name of the topic
       /// \param[in] _queueLimit The maximum number of outgoing messages
       /// to queue
+      /// \param[in] _hz Update rate for the publisher. Units are
+      /// 1.0/seconds.
       /// \return Pointer to the newly created Publisher
       public: template<typename M>
               PublisherPtr Advertise(const std::string &_topic,
-                                     unsigned int _queueLimit)
+                                     unsigned int _queueLimit,
+                                     double _hzRate)
               {
                 google::protobuf::Message *msg = NULL;
                 M msgtype;
                 this->UpdatePublications(_topic, msg->GetTypeName());
 
                 PublisherPtr pub = PublisherPtr(new Publisher(_topic,
-                      msg->GetTypeName(), _queueLimit));
+                      msg->GetTypeName(), _queueLimit, _hzRate));
 
                 std::string msgTypename;
                 PublicationPtr publication;

File sdf/worlds/shapes.world

         </visual>
       </link>
     </model>
-    <!--
+
     <model name="sphere">
       <pose>0 1.5 0.5 0 0 0</pose>
       <link name="link">
         </visual>
       </link>
     </model>
+
     <model name="cylinder">
       <pose>0 -1.5 0.5 0 0 0</pose>
       <link name="link">
         </visual>
       </link>
     </model>
--->
   </world>
 </sdf>

File test/ServerFixture.hh

                this->node = transport::NodePtr(new transport::Node());
                ASSERT_NO_THROW(this->node->Init());
                this->poseSub = this->node->Subscribe("~/pose/info",
-                   &ServerFixture::OnPose, this);
+                   &ServerFixture::OnPose, this, true);
                this->statsSub = this->node->Subscribe("~/world_stats",
                    &ServerFixture::OnStats, this);
 
                this->factoryPub =
                  this->node->Advertise<msgs::Factory>("~/factory");
+               this->factoryPub->WaitForConnection();
 
                // Wait for the world to reach the correct pause state.
                // This might not work properly with multiple worlds.
                       ++waitCount < maxWaitCount)
                  common::Time::MSleep(10);
                ASSERT_LT(waitCount, maxWaitCount);
+
              }
 
   protected: void RunServer(const std::string &_worldFilename)
                msg.set_sdf(newModelStr.str());
                this->factoryPub->Publish(msg);
 
+               int i = 0;
                // Wait for the entity to spawn
-               while (!this->HasEntity(_modelName))
-                 common::Time::MSleep(10);
+               while (!this->HasEntity(_modelName) && i < 50)
+               {
+                 common::Time::MSleep(20);
+                 ++i;
+               }
+               EXPECT_LT(i, 50);
              }
 
   protected: void SpawnCylinder(const std::string &_name,
 
   protected: void SpawnSDF(const std::string &_sdf)
              {
+               // Wait for the first pose message
+               while (this->poses.size() == 0)
+                 common::Time::MSleep(10);
+
                msgs::Factory msg;
                msg.set_sdf(_sdf);
                this->factoryPub->Publish(msg);

File test/regression/laser.cc

   while (laser->GetRange(0) == 0)
     common::Time::MSleep(100);
 
-
   EXPECT_EQ(640, laser->GetRayCount());
   EXPECT_EQ(640, laser->GetRangeCount());
 
     laser->GetRanges(scan);
 
     DoubleCompare(box_scan, &scan[0], 640, diffMax, diffSum, diffAvg);
-    EXPECT_LT(diffMax, 1e-6);
-    EXPECT_LT(diffSum, 1e-5);
-    EXPECT_LT(diffAvg, 1e-6);
+    EXPECT_LT(diffMax, 2e-6);
+    EXPECT_LT(diffSum, 1e-4);
+    EXPECT_LT(diffAvg, 2e-6);
 
     // This line will print the current scan. Use this to generate
     // a new test scan sample