Skip to content

Commit

Permalink
Merge pull request #2791 from osrf/respect_fps_gz11new
Browse files Browse the repository at this point in the history
Test ABI compatibility of physics-sensor lockstep
  • Loading branch information
mabelzhang authored Jul 23, 2020
2 parents e7fdfcd + b351913 commit 6ec9700
Show file tree
Hide file tree
Showing 45 changed files with 2,010 additions and 81 deletions.
25 changes: 23 additions & 2 deletions gazebo/Server.cc
Original file line number Diff line number Diff line change
Expand Up @@ -100,6 +100,9 @@ namespace gazebo

/// \brief Save argv for access by system plugins.
char **systemPluginsArgv;

/// \brief Set whether to lockstep physics and rendering
bool lockstep = false;
};
}

Expand Down Expand Up @@ -149,6 +152,7 @@ bool Server::ParseArgs(int _argc, char **_argv)
("verbose", "Increase the messages written to the terminal.")
("help,h", "Produce this help message.")
("pause,u", "Start the server in a paused state.")
("lockstep", "Lockstep simulation so sensor update rates are respected.")
("physics,e", po::value<std::string>(),
"Specify a physics engine (ode|bullet|dart|simbody).")
("play,p", po::value<std::string>(), "Play a log file.")
Expand Down Expand Up @@ -282,6 +286,12 @@ bool Server::ParseArgs(int _argc, char **_argv)
}
}

if (this->dataPtr->vm.count("lockstep"))
{
this->dataPtr->lockstep = true;
}
rendering::set_lockstep_enabled(this->dataPtr->lockstep);

if (!this->PreLoad())
{
gzerr << "Unable to load gazebo\n";
Expand Down Expand Up @@ -497,7 +507,11 @@ bool Server::LoadImpl(sdf::ElementPtr _elem,
<< " seconds for namespaces. Giving up.\n";
}

physics::init_worlds(rendering::update_scene_poses);
if (this->dataPtr->lockstep)
physics::init_worlds(rendering::update_scene_poses);
else
physics::init_worlds(nullptr);

this->dataPtr->stop = false;

return true;
Expand Down Expand Up @@ -586,14 +600,21 @@ void Server::Run()
// The server and sensor manager outlive worlds
while (!this->dataPtr->stop)
{
if (this->dataPtr->lockstep)
rendering::wait_for_render_request("", 0.100);
// bool ret = rendering::wait_for_render_request("", 0.100);
// if (ret == false)
// gzerr << "time out reached!" << std::endl;

this->ProcessControlMsgs();

if (physics::worlds_running())
sensors::run_once();
else if (sensors::running())
sensors::stop();

common::Time::MSleep(1);
if (!this->dataPtr->lockstep)
common::Time::MSleep(1);
}

// Shutdown gazebo
Expand Down
1 change: 1 addition & 0 deletions gazebo/common/Events.cc
Original file line number Diff line number Diff line change
Expand Up @@ -38,6 +38,7 @@ EventT<void ()> Events::worldReset;
EventT<void ()> Events::timeReset;

EventT<void ()> Events::preRender;
EventT<void ()> Events::preRenderEnded;
EventT<void ()> Events::render;
EventT<void ()> Events::postRender;

Expand Down
11 changes: 11 additions & 0 deletions gazebo/common/Events.hh
Original file line number Diff line number Diff line change
Expand Up @@ -160,6 +160,14 @@ namespace gazebo
static ConnectionPtr ConnectCreateSensor(T _subscriber)
{ return createSensor.Connect(_subscriber); }

//////////////////////////////////////////////////////////////////////////
/// \brief End of prerender phase signal
/// \param[in] _subscriber the subscriber to this event
/// \return a connection
public: template<typename T>
static ConnectionPtr ConnectPreRenderEnded(T _subscriber)
{ return preRenderEnded.Connect(_subscriber); }

//////////////////////////////////////////////////////////////////////////
/// \brief Render start signal
/// \param[in] _subscriber the subscriber to this event
Expand Down Expand Up @@ -254,6 +262,9 @@ namespace gazebo
/// \brief Pre-render
public: static EventT<void ()> preRender;

/// \brief End of Pre-render
public: static EventT<void ()> preRenderEnded;

/// \brief Render
public: static EventT<void ()> render;

Expand Down
2 changes: 2 additions & 0 deletions gazebo/gazebo.1.ronn
Original file line number Diff line number Diff line change
Expand Up @@ -49,6 +49,8 @@ Gazebo server runs simulation and handles commandline options, starts a Master,
Load a server plugin.
* -o, --profile arg :
Physics preset profile name from the options in the world file.
* --lockstep :
Lockstep simulation so sensor update rates are respected.


## AUTHOR
Expand Down
2 changes: 2 additions & 0 deletions gazebo/gazebo_main.cc
Original file line number Diff line number Diff line change
Expand Up @@ -72,6 +72,8 @@ void help()
<< " -o [ --profile ] arg Physics preset profile name from the "
<< "options in\n"
<< " the world file.\n"
<< " --lockstep Lockstep simulation so sensor update "
<< "rates are respected.\n"
<< "\n";
}

Expand Down
2 changes: 2 additions & 0 deletions gazebo/gzserver.1.ronn
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,8 @@ Gazebo server runs simulation and handles commandline options, starts a Master,
Produce this help message.
* -u, --pause :
Start the server in a paused state.
* --lockstep :
Lockstep simulation so sensor update rates are respected.
* -e, --physics arg :
Specify a physics engine (ode|bullet|dart|simbody).
* -p, --play arg :
Expand Down
24 changes: 18 additions & 6 deletions gazebo/physics/World.cc
Original file line number Diff line number Diff line change
Expand Up @@ -160,6 +160,8 @@ World::World(const std::string &_name)
event::Events::ConnectPause(
std::bind(&World::SetPaused, this, std::placeholders::_1)));

this->dataPtr->waitForSensors = nullptr;

// Make sure dbs are initialized
common::ModelDatabase::Instance();
common::FuelModelDatabase::Instance();
Expand Down Expand Up @@ -663,6 +665,12 @@ bool World::SensorsInitialized() const
return this->dataPtr->sensorsInitialized;
}

/////////////////////////////////////////////////
void World::SetSensorWaitFunc(std::function<void(double, double)> _func)
{
this->dataPtr->waitForSensors = _func;
}

//////////////////////////////////////////////////
void World::Step()
{
Expand All @@ -685,6 +693,10 @@ void World::Step()

DIAG_TIMER_LAP("World::Step", "publishWorldStats");

if (this->dataPtr->waitForSensors)
this->dataPtr->waitForSensors(this->dataPtr->simTime.Double(),
this->dataPtr->physicsEngine->GetMaxStepSize());

double updatePeriod = this->dataPtr->physicsEngine->GetUpdatePeriod();
// sleep here to get the correct update rate
common::Time tmpTime = common::Time::GetWallTime();
Expand Down Expand Up @@ -2652,7 +2664,7 @@ void World::ProcessMessages()
if ((this->dataPtr->posePub && this->dataPtr->posePub->HasConnections()) ||
// When ready to use the direct API for updating scene poses from server,
// uncomment the following line:
// this->dataPtr->updateScenePoses ||
this->dataPtr->updateScenePoses ||
(this->dataPtr->poseLocalPub &&
this->dataPtr->poseLocalPub->HasConnections()))
{
Expand Down Expand Up @@ -2720,11 +2732,11 @@ void World::ProcessMessages()

// When ready to use the direct API for updating scene poses from server,
// uncomment the following lines:
// // Execute callback to export Pose msg
// if (this->dataPtr->updateScenePoses)
// {
// this->dataPtr->updateScenePoses(this->Name(), msg);
// }
// Execute callback to export Pose msg
if (this->dataPtr->updateScenePoses)
{
this->dataPtr->updateScenePoses(this->Name(), msg);
}
}

this->dataPtr->publishModelPoses.clear();
Expand Down
4 changes: 4 additions & 0 deletions gazebo/physics/World.hh
Original file line number Diff line number Diff line change
Expand Up @@ -459,6 +459,10 @@ namespace gazebo
/// \return Unique model name.
public: std::string UniqueModelName(const std::string &_name);

/// \brief Set callback 'waitForSensors'
/// \param[in] function to be called
public: void SetSensorWaitFunc(std::function<void(double, double)> _func);

/// \cond
/// This is an internal function.
/// \brief Get a model by id.
Expand Down
3 changes: 3 additions & 0 deletions gazebo/physics/WorldPrivate.hh
Original file line number Diff line number Diff line change
Expand Up @@ -377,6 +377,9 @@ namespace gazebo
/// \brief Node for ignition transport communication.
public: ignition::transport::Node ignNode;

/// \brief Wait until no sensors use the current step any more
public: std::function<void(double, double)> waitForSensors;

/// \brief Callback function intended to call the scene with updated Poses
public: UpdateScenePosesFunc updateScenePoses;

Expand Down
29 changes: 29 additions & 0 deletions gazebo/rendering/RenderingIface.cc
Original file line number Diff line number Diff line change
Expand Up @@ -24,6 +24,8 @@

using namespace gazebo;

bool g_lockstep = false;

//////////////////////////////////////////////////
bool rendering::load()
{
Expand Down Expand Up @@ -109,6 +111,21 @@ void rendering::remove_scene(const std::string &_name)
rendering::RenderEngine::Instance()->RemoveScene(_name);
}

//////////////////////////////////////////////////
bool rendering::wait_for_render_request(const std::string &_name,
const double _timeoutsec)
{
ScenePtr scene = get_scene(_name);

if (!scene)
{
common::Time::NSleep(_timeoutsec * 1e9);
return false;
}

return scene->WaitForRenderRequest(_timeoutsec);
}

//////////////////////////////////////////////////
void rendering::update_scene_poses(const std::string &_name,
const msgs::PosesStamped &_msg)
Expand All @@ -119,3 +136,15 @@ void rendering::update_scene_poses(const std::string &_name,
scene->UpdatePoses(_msg);
}
}

//////////////////////////////////////////////////
void rendering::set_lockstep_enabled(bool _enable)
{
g_lockstep = _enable;
}

//////////////////////////////////////////////////
bool rendering::lockstep_enabled()
{
return g_lockstep;
}
25 changes: 25 additions & 0 deletions gazebo/rendering/RenderingIface.hh
Original file line number Diff line number Diff line change
Expand Up @@ -55,6 +55,31 @@ namespace gazebo
void update_scene_poses(const std::string &_name,
const msgs::PosesStamped &_msg);

/// \brief Set whether to enable lockstepping for rendering and physics.
/// If enabled, the poses of objects in rendering will be updated via
/// direct API call instead of transport.
/// \param[in] _enable True to enable lockstepping, false to disable
/// \sa update_scene_poses
GZ_RENDERING_VISIBLE
void set_lockstep_enabled(bool _enable);

/// \brief Get whether or not lockstepping is enabled for rendering and
/// physics. If enabled, the poses of objects in rendering is updated via
/// direct API call instead of transport.
/// \return True if lockstepping is enabled, false if disabled
/// \sa lockstep_enabled
GZ_RENDERING_VISIBLE
bool lockstep_enabled();

/// \brief wait until a render request occurs
/// \param[in] _name Name of the scene to retrieve
/// \param[in] _timeoutsec timeout expressed in seconds
/// \return true if a render request occured, false in case
/// we waited until the timeout
GZ_RENDERING_VISIBLE
bool wait_for_render_request(const std::string &_name,
const double _timeoutsec);

/// \brief create rendering::Scene by name.
/// \param[in] _name Name of the scene to create.
/// \param[in] _enableVisualizations True enables visualization
Expand Down
26 changes: 23 additions & 3 deletions gazebo/rendering/Scene.cc
Original file line number Diff line number Diff line change
Expand Up @@ -62,6 +62,7 @@
#include "gazebo/rendering/TransmitterVisual.hh"
#include "gazebo/rendering/SelectionObj.hh"
#include "gazebo/rendering/RayQuery.hh"
#include "gazebo/rendering/RenderingIface.hh"

#if OGRE_VERSION_MAJOR >= 1 && OGRE_VERSION_MINOR >= 8
#include "gazebo/rendering/deferred_shading/SSAOLogic.hh"
Expand Down Expand Up @@ -157,15 +158,16 @@ Scene::Scene(const std::string &_name, const bool _enableVisualizations,
&Scene::OnLightModifyMsg, this);

this->dataPtr->isServer = _isServer;
if (_isServer)

if (_isServer && !rendering::lockstep_enabled())
{
this->dataPtr->poseSub = this->dataPtr->node->Subscribe("~/pose/local/info",
&Scene::OnPoseMsg, this);
}
else

// When ready to use the direct API for updating scene poses from server,
// uncomment the following line and delete the if and else directly above
// if (!_isServer)
if (!_isServer)
{
this->dataPtr->poseSub = this->dataPtr->node->Subscribe("~/pose/info",
&Scene::OnPoseMsg, this);
Expand Down Expand Up @@ -2890,6 +2892,24 @@ void Scene::UpdatePoses(const msgs::PosesStamped &_msg)
this->dataPtr->newPoseCondition.notify_all();
}

/////////////////////////////////////////////////
bool Scene::WaitForRenderRequest(double _timeoutsec)
{
std::unique_lock<std::mutex> lck(this->dataPtr->newPoseMutex);

while (!this->dataPtr->newPoseAvailable)
{
auto ret = this->dataPtr->newPoseCondition.wait_for(lck,
std::chrono::duration<double>(_timeoutsec));

if (ret == std::cv_status::timeout)
return false;
}

this->dataPtr->newPoseAvailable = false;
return true;
}

/////////////////////////////////////////////////
void Scene::OnSkeletonPoseMsg(ConstPoseAnimationPtr &_msg)
{
Expand Down
5 changes: 5 additions & 0 deletions gazebo/rendering/Scene.hh
Original file line number Diff line number Diff line change
Expand Up @@ -127,6 +127,11 @@ namespace gazebo
/// \brief Process all received messages.
public: void PreRender();

/// \brief Wait until a render request occurs
/// \param[in] _timeoutsec timeout expressed in seconds
/// \return True if timeout was NOT met
public: bool WaitForRenderRequest(double _timeoutsec);

/// \brief Get the OGRE scene manager.
/// \return Pointer to the Ogre SceneManager.
public: Ogre::SceneManager *OgreSceneManager() const;
Expand Down
4 changes: 2 additions & 2 deletions gazebo/rendering/Visual.cc
Original file line number Diff line number Diff line change
Expand Up @@ -820,8 +820,8 @@ void Visual::SetScale(const ignition::math::Vector3d &_scale)
}
else
{
gzerr << Name() << ": Size of the collision contains one or several zeros." <<
" Collisions may not visualize properly." << std::endl;
gzerr << Name() << ": Size of the collision contains one or several zeros."
<< " Collisions may not visualize properly." << std::endl;
}
// Scale selection object in case we have one attached. Other children were
// scaled from UpdateGeomSize
Expand Down
2 changes: 1 addition & 1 deletion gazebo/rendering/Visual.hh
Original file line number Diff line number Diff line change
Expand Up @@ -549,7 +549,7 @@ namespace gazebo
/// \param[in] _scene Pointer to the scene.
public: void SetScene(ScenePtr _scene);

/// \brief Get current.
/// \brief Get current scene.
/// \return Pointer to the scene.
public: ScenePtr GetScene() const;

Expand Down
Loading

0 comments on commit 6ec9700

Please sign in to comment.