Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Test ABI compatibility of physics-sensor lockstep #2791

Merged
merged 63 commits into from
Jul 23, 2020
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
63 commits
Select commit Hold shift + click to select a range
64eb44f
Make sure camera's FPS is strictly applied, should the real time fact…
slekieffre Nov 15, 2016
c2a1f2f
Update lessOrEqual() calls with new method name
slekieffre Nov 18, 2016
08d6c8b
merging from default
slekieffre May 2, 2017
b0f8197
merge from default branch
slekieffre May 21, 2018
e74da86
merge with default
scpeters Jan 22, 2020
f63709d
whitespace
scpeters Jan 22, 2020
e6ffa11
merge with respect_fps_1_set_pose_msg
scpeters Jan 22, 2020
12cf6e5
merge with respect_fps_2_sensor_abi
scpeters Jan 22, 2020
abadcdc
Merge branch 'gazebo11' into respect_fps_gz11
mabelzhang Apr 28, 2020
c2f69da
Generalize from camera to sensors. Enable lines to directly set pose.…
mabelzhang May 14, 2020
f2e88c9
add regular camera to test world
mabelzhang May 15, 2020
c3302f4
add pendulum to have changing pose in scene
mabelzhang May 24, 2020
b691c2d
copy CameraSensor functions to GpuRaySensor. GpuRaySensor tests are f…
mabelzhang Jun 2, 2020
d80846d
add pendulum to have changing pose in scene
mabelzhang May 24, 2020
760636e
fix test failures caused by backward compatibility if-stmts
mabelzhang Jun 3, 2020
ead5bd0
SDF file for GpuRaySensor lockstep test
mabelzhang Jun 8, 2020
d5bd097
style
mabelzhang Jun 9, 2020
6afd412
backward compatibility conditionals
mabelzhang Jun 9, 2020
f43b100
style and PR comments
mabelzhang Jun 10, 2020
c134381
raise strict rate tolerance to 0.06
mabelzhang Jun 10, 2020
ed6fc04
fix time calculation in test
mabelzhang Jun 18, 2020
bd0085b
correct test timing
mabelzhang Jun 18, 2020
f5e5fe6
merge respect_fps_gz11camOnly branch into respect_fps_gz11_gpuRay
mabelzhang Jun 18, 2020
90485b1
simplify header include
mabelzhang Jun 18, 2020
9a44c66
one more backward compatibility conditional
mabelzhang Jun 18, 2020
61fb487
Merge branch 'respect_fps_gz11camOnly' into respect_fps_gz11_gpuRay
mabelzhang Jun 18, 2020
034953d
sync with CameraSensor branch: simplify header include, change test t…
mabelzhang Jun 18, 2020
c7c6795
extend lockstep to MultiCameraSensor and Sensor
mabelzhang Jun 20, 2020
536c26b
minor fixes
mabelzhang Jun 20, 2020
b4991f4
Merge branch 'respect_fps_gz11camOnly' into respect_fps_gz11_gpuRay
mabelzhang Jun 20, 2020
a01101b
minor fixes
mabelzhang Jun 20, 2020
b2c9d5e
merge respect_fps_gz11_gpuRay into respect_fps_gz11_allSensors
mabelzhang Jun 20, 2020
86c90d2
minor fixes and style
mabelzhang Jun 20, 2020
8e61753
MultiCameraSensor test
mabelzhang Jun 20, 2020
ec1916a
small fixes
mabelzhang Jun 20, 2020
a8bb0db
Merge branch 'respect_fps_gz11camOnly' into respect_fps_gz11_gpuRay
mabelzhang Jun 20, 2020
64a158c
Merge branch 'respect_fps_gz11_gpuRay' into respect_fps_gz11_allSensors
mabelzhang Jun 20, 2020
805c3e9
LogicalCameraSensor test
mabelzhang Jun 20, 2020
744cc84
add reference to custom XML namespace
mabelzhang Jun 26, 2020
59b2a94
Merge branch 'respect_fps_gz11camOnly' into respect_fps_gz11_allSensors
mabelzhang Jun 26, 2020
159624f
add reference to custom XML namespaces
mabelzhang Jun 26, 2020
8349135
--lockstep command line per Ian Chen <[email protected]>
mabelzhang Jun 29, 2020
d883070
lockstep test for contact sensor and laser
mabelzhang Jun 29, 2020
1f39fd5
style
mabelzhang Jun 29, 2020
d9affae
fix hanging tests because rendering:: flag not reset after lockstep test
mabelzhang Jul 9, 2020
e1ca9f3
remove <strict_rate> custom sdf tag
mabelzhang Jul 9, 2020
35e582a
merge from respect_fps_gz11camOnly
mabelzhang Jul 9, 2020
66d2811
remove <strict_rate> from other sensor tests
mabelzhang Jul 10, 2020
9aa901c
Merge pull request #2746 from mabelzhang/respect_fps_gz11camOnly
mabelzhang Jul 10, 2020
e116de5
imu strict rate test
mabelzhang Jul 10, 2020
484f0ae
cleanup
mabelzhang Jul 10, 2020
59a2c9a
minor cleanup
mabelzhang Jul 10, 2020
867b25c
fix laser test bug
mabelzhang Jul 11, 2020
d761e4d
transceiver strict rate test
mabelzhang Jul 11, 2020
d09afc5
tidy up test worlds
mabelzhang Jul 11, 2020
7491fa4
rename gpu ray test world
mabelzhang Jul 11, 2020
37eeec9
change file name in gpu ray test
mabelzhang Jul 11, 2020
02269c0
disable simbody imu test
mabelzhang Jul 14, 2020
e7da851
make Sensor::useStrictRate static
mabelzhang Jul 14, 2020
9c61a7c
fix ManTest
mabelzhang Jul 15, 2020
0b290dd
Merge pull request #2761 from osrf/respect_fps_gz11_allSensors
mabelzhang Jul 16, 2020
a0ed0a6
Merge branch 'gazebo11' into upstream/respect_fps_gz11new
mabelzhang Jul 16, 2020
b351913
fix for wide angle camera
iche033 Jul 17, 2020
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
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 @@ -159,6 +159,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