World.hh
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2012 Open Source Robotics Foundation
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  *
16 */
17 #ifndef GAZEBO_PHYSICS_WORLD_HH_
18 #define GAZEBO_PHYSICS_WORLD_HH_
19 
20 #include <vector>
21 #include <list>
22 #include <set>
23 #include <deque>
24 #include <string>
25 #include <memory>
26 
27 #include <boost/enable_shared_from_this.hpp>
28 
29 #include <sdf/sdf.hh>
30 
32 
33 #include "gazebo/msgs/msgs.hh"
34 
37 #include "gazebo/common/Event.hh"
38 #include "gazebo/common/URI.hh"
39 
40 #include "gazebo/physics/Base.hh"
43 #include "gazebo/physics/Wind.hh"
44 #include "gazebo/util/system.hh"
45 
46 // Forward declare reference and pointer parameters
47 namespace ignition
48 {
49  namespace msgs
50  {
51  class Plugin_V;
52  class StringMsg;
53  }
54 }
55 
56 namespace gazebo
57 {
58  namespace physics
59  {
61  class WorldPrivate;
62 
65 
74  class GZ_PHYSICS_VISIBLE World :
75  public boost::enable_shared_from_this<World>
76  {
80  public: explicit World(const std::string &_name = "");
81 
83  public: ~World();
84 
88  public: void Load(sdf::ElementPtr _sdf);
89 
92  public: const sdf::ElementPtr SDF();
93 
97  public: void Save(const std::string &_filename);
98 
101  public: void Init();
102 
107  public: void Run(const unsigned int _iterations = 0);
108 
111  public: bool Running() const;
112 
115  public: void Stop();
116 
119  public: void Fini();
120 
124  public: void Clear();
125 
128  public: std::string Name() const;
129 
133  public: PhysicsEnginePtr Physics() const;
134 
137  public: physics::Atmosphere &Atmosphere() const;
138 
141  public: PresetManagerPtr PresetMgr() const;
142 
145  public: physics::Wind &Wind() const;
146 
149  public: common::SphericalCoordinatesPtr SphericalCoords() const;
150 
153  public: ignition::math::Vector3d Gravity() const;
154 
157  public: void SetGravity(const ignition::math::Vector3d &_gravity);
158 
161  public: void SetGravitySDF(const ignition::math::Vector3d &_gravity);
162 
165  public: virtual ignition::math::Vector3d MagneticField() const;
166 
169  public: void SetMagneticField(const ignition::math::Vector3d &_mag);
170 
173  public: unsigned int ModelCount() const;
174 
180  public: ModelPtr ModelByIndex(const unsigned int _index) const;
181 
184  public: Model_V Models() const;
185 
188  public: unsigned int LightCount() const;
189 
192  public: Light_V Lights() const;
193 
198  public: void ResetEntities(Base::EntityType _type = Base::BASE);
199 
201  public: void ResetTime();
202 
204  public: void Reset();
205 
208  public: void PrintEntityTree();
209 
213  public: common::Time SimTime() const;
214 
217  public: void SetSimTime(const common::Time &_t);
218 
221  public: common::Time PauseTime() const;
222 
225  public: common::Time StartTime() const;
226 
229  public: common::Time RealTime() const;
230 
233  public: bool IsPaused() const;
234 
237  public: void SetPaused(const bool _p);
238 
244  public: BasePtr BaseByName(const std::string &_name) const;
245 
251  public: ModelPtr ModelByName(const std::string &_name) const;
252 
258  public: LightPtr LightByName(const std::string &_name) const;
259 
265  public: EntityPtr EntityByName(const std::string &_name) const;
266 
274  public: ModelPtr ModelBelowPoint(
275  const ignition::math::Vector3d &_pt) const;
276 
282  public: EntityPtr EntityBelowPoint(
283  const ignition::math::Vector3d &_pt) const;
284 
287  public: void SetState(const WorldState &_state);
288 
292  public: void InsertModelFile(const std::string &_sdfFilename);
293 
297  public: void InsertModelString(const std::string &_sdfString);
298 
302  public: void InsertModelSDF(const sdf::SDF &_sdf);
303 
307  public: std::string StripWorldName(const std::string &_name) const;
308 
312  public: void EnableAllModels();
313 
317  public: void DisableAllModels();
318 
321  public: void Step(const unsigned int _steps);
322 
327  public: void LoadPlugin(const std::string &_filename,
328  const std::string &_name,
329  sdf::ElementPtr _sdf);
330 
333  public: void RemovePlugin(const std::string &_name);
334 
337  public: std::mutex &WorldPoseMutex() const;
338 
341  public: bool PhysicsEnabled() const;
342 
345  public: void SetPhysicsEnabled(const bool _enable);
346 
349  public: bool WindEnabled() const;
350 
353  public: void SetWindEnabled(const bool _enable);
354 
357  public: bool AtmosphereEnabled() const;
358 
361  public: void SetAtmosphereEnabled(const bool _enable);
362 
364  public: void UpdateStateSDF();
365 
368  public: bool IsLoaded() const;
369 
372  public: void ClearModels();
373 
378  public: void PublishModelPose(physics::ModelPtr _model);
379 
384  public: void PublishModelScale(physics::ModelPtr _model);
385 
390  public: void PublishLightPose(const physics::LightPtr _light);
391 
394  public: uint32_t Iterations() const;
395 
398  public: msgs::Scene SceneMsg() const;
399 
404  public: void RunBlocking(const unsigned int _iterations = 0);
405 
410  public: void RemoveModel(ModelPtr _model);
411 
416  public: void RemoveModel(const std::string &_name);
417 
420  public: void ResetPhysicsStates();
421 
428  public: void _AddDirty(Entity *_entity);
429 
432  public: bool SensorsInitialized() const;
433 
438  public: void _SetSensorsInitialized(const bool _init);
439 
442  public: common::URI URI() const;
443 
449  public: std::string UniqueModelName(const std::string &_name);
450 
458  private: ModelPtr ModelById(const unsigned int _id) const;
460 
464  private: void LoadPlugins();
465 
469  private: void LoadEntities(sdf::ElementPtr _sdf, BasePtr _parent);
470 
475  private: ModelPtr LoadModel(sdf::ElementPtr _sdf, BasePtr _parent);
476 
481  public: LightPtr LoadLight(const sdf::ElementPtr &_sdf,
482  const BasePtr &_parent);
483 
488  private: ActorPtr LoadActor(sdf::ElementPtr _sdf, BasePtr _parent);
489 
494  private: RoadPtr LoadRoad(sdf::ElementPtr _sdf, BasePtr _parent);
495 
497  private: void RunLoop();
498 
500  private: void Step();
501 
503  private: void LogStep();
504 
506  private: void Update();
507 
510  private: void OnPause(bool _p);
511 
513  private: void OnStep();
514 
517  private: void OnControl(ConstWorldControlPtr &_data);
518 
521  private: void OnPlaybackControl(ConstLogPlaybackControlPtr &_data);
522 
525  private: void OnRequest(ConstRequestPtr &_msg);
526 
531  private: void BuildSceneMsg(msgs::Scene &_scene, BasePtr _entity);
532 
535  private: void JointLog(ConstJointPtr &_msg);
536 
539  private: void OnFactoryMsg(ConstFactoryPtr &_data);
540 
543  private: void OnModelMsg(ConstModelPtr &_msg);
544 
546  private: void ModelUpdateTBB();
547 
549  private: void ModelUpdateSingleLoop();
550 
553  private: void LoadPlugin(sdf::ElementPtr _sdf);
554 
558  private: void FillModelMsg(msgs::Model &_msg, ModelPtr _model);
559 
562  private: void ProcessEntityMsgs();
563 
566  private: void ProcessRequestMsgs();
567 
570  private: void ProcessFactoryMsgs();
571 
574  private: void ProcessModelMsgs();
575 
578  private: void ProcessLightFactoryMsgs();
579 
582  private: void ProcessLightModifyMsgs();
583 
586  private: void ProcessPlaybackControlMsgs();
587 
589  private: bool OnLog(std::ostringstream &_stream);
590 
593  private: void LogModelResources();
594 
596  private: void ProcessMessages();
597 
599  private: void PublishWorldStats();
600 
602  private: void LogWorker();
603 
605  private: void RegisterIntrospectionItems();
606 
608  private: void UnregisterIntrospectionItems();
609 
613  private: void OnLightFactoryMsg(ConstLightPtr &_msg);
614 
618  private: void OnLightModifyMsg(ConstLightPtr &_msg);
619 
636  private: bool PluginInfoService(const ignition::msgs::StringMsg &_request,
637  ignition::msgs::Plugin_V &_plugins);
638 
641  private: std::unique_ptr<WorldPrivate> dataPtr;
642 
644  private: friend class DARTLink;
645 
647  private: friend class SimbodyPhysics;
648  };
650  }
651 }
652 #endif
void _AddDirty(Entity *_entity)
boost::shared_ptr< Actor > ActorPtr
Definition: PhysicsTypes.hh:97
A complete URI.
Definition: URI.hh:176
Definition: Model.hh:40
PhysicsEnginePtr Physics() const
Return the physics engine.
Forward declarations for the common classes.
Definition: Animation.hh:26
void PublishModelPose(physics::ModelPtr _model)
Publish pose updates for a model.
A Time class, can be used to hold wall- or sim-time. stored as sec and nano-sec.
Definition: Time.hh:47
void SetPhysicsEnabled(const bool _enable)
enable/disable physics engine during World::Update.
Base type.
Definition: Base.hh:77
void InsertModelSDF(const sdf::SDF &_sdf)
Insert a model using SDF.
boost::shared_ptr< Model > ModelPtr
Definition: PhysicsTypes.hh:93
std::vector< ModelPtr > Model_V
Definition: PhysicsTypes.hh:205
msgs::Scene SceneMsg() const
Get the current scene in message form.
common::SphericalCoordinatesPtr SphericalCoords() const
Return the spherical coordinates converter.
Forward declarations for transport.
void Init()
Initialize the world.
bool Running() const
Return the running state of the world.
void ResetEntities(Base::EntityType _type=Base::BASE)
Reset with options.
std::mutex & WorldPoseMutex() const
Get the set world pose mutex.
void Run(const unsigned int _iterations=0)
Run the world in a thread.
default namespace for gazebo
void SetWindEnabled(const bool _enable)
enable/disable wind.
bool IsLoaded() const
Return true if the world has been loaded.
void PublishModelScale(physics::ModelPtr _model)
Publish scale updates for a model.
void UpdateStateSDF()
Update the state SDF value from the current state.
bool WindEnabled() const
check if wind is enabled/disabled.
void Step(const unsigned int _steps)
Step the world forward in time.
void DisableAllModels()
Disable all links in all the models.
LightPtr LoadLight(const sdf::ElementPtr &_sdf, const BasePtr &_parent)
Load a light.
void PrintEntityTree()
Print Entity tree.
void InsertModelString(const std::string &_sdfString)
Insert a model from an SDF string.
void SetAtmosphereEnabled(const bool _enable)
enable/disable atmosphere model.
void EnableAllModels()
Enable all links in all the models.
void SetGravity(const ignition::math::Vector3d &_gravity)
Set the gravity vector.
boost::shared_ptr< Road > RoadPtr
Definition: PhysicsTypes.hh:161
This models a base atmosphere class to serve as a common interface to any derived atmosphere models.
Definition: Atmosphere.hh:42
void RemoveModel(ModelPtr _model)
Remove a model.
unsigned int LightCount() const
Get the number of lights.
boost::shared_ptr< Light > LightPtr
Definition: PhysicsTypes.hh:105
The world provides access to all other object within a simulated environment.
Definition: World.hh:74
PresetManagerPtr PresetMgr() const
Return the preset manager.
void ResetPhysicsStates()
Reset the velocity, acceleration, force and torque of all child models.
void Save(const std::string &_filename)
Save a world to a file.
void Load(sdf::ElementPtr _sdf)
Load the world using SDF parameters.
Light_V Lights() const
Get a list of all the lights.
~World()
Destructor.
void SetSimTime(const common::Time &_t)
Set the sim time.
boost::shared_ptr< SphericalCoordinates > SphericalCoordinatesPtr
Definition: CommonTypes.hh:121
Base class for wind.
Definition: Wind.hh:41
boost::shared_ptr< Base > BasePtr
Definition: PhysicsTypes.hh:77
void Fini()
Finalize the world.
Simbody physics engine.
Definition: SimbodyPhysics.hh:42
common::Time PauseTime() const
Get the amount of time simulation has been paused.
GAZEBO_VISIBLE void Init(google::protobuf::Message &_message, const std::string &_id="")
Initialize a message.
void Clear()
Remove all entities from the world.
void ResetTime()
Reset simulation time back to zero.
void Stop()
Stop the world.
Model_V Models() const
Get a list of all the models.
void _SetSensorsInitialized(const bool _init)
void RemovePlugin(const std::string &_name)
Remove a running plugin.
EntityPtr EntityBelowPoint(const ignition::math::Vector3d &_pt) const
Get the nearest entity below a point.
void SetMagneticField(const ignition::math::Vector3d &_mag)
Set the magnetic field vector.
EntityPtr EntityByName(const std::string &_name) const
Get a pointer to an Entity based on a name.
boost::shared_ptr< Entity > EntityPtr
Definition: PhysicsTypes.hh:85
void LoadPlugin(const std::string &_filename, const std::string &_name, sdf::ElementPtr _sdf)
Load a plugin.
bool IsPaused() const
Returns the state of the simulation true if paused.
Base class for all physics objects in Gazebo.
Definition: Entity.hh:52
common::Time StartTime() const
Get the wall time simulation was started.
void Reset()
Reset time and model poses, configurations in simulation.
void SetState(const WorldState &_state)
Set the current world state.
boost::shared_ptr< PhysicsEngine > PhysicsEnginePtr
Definition: PhysicsTypes.hh:125
void SetGravitySDF(const ignition::math::Vector3d &_gravity)
Set the gravity sdf value.
physics::Wind & Wind() const
Get a reference to the wind used by the world.
ignition::math::Vector3d Gravity() const
Return the gravity vector.
void PublishLightPose(const physics::LightPtr _light)
Publish pose updates for a light.
BasePtr BaseByName(const std::string &_name) const
Get an element by name.
uint32_t Iterations() const
Get the total number of iterations.
void InsertModelFile(const std::string &_sdfFilename)
Insert a model from an SDF file.
void ClearModels()
Remove all entities from the world.
std::string StripWorldName(const std::string &_name) const
Return a version of the name with "<world_name>::" removed.
Store state information of a physics::World object.
Definition: WorldState.hh:47
EntityType
Unique identifiers for all entity types.
Definition: Base.hh:75
common::Time RealTime() const
Get the real time (elapsed time).
ModelPtr ModelByIndex(const unsigned int _index) const
Get a model based on an index.
LightPtr LightByName(const std::string &_name) const
Get a light by name.
unsigned int ModelCount() const
Get the number of models.
World(const std::string &_name="")
Constructor.
void SetPaused(const bool _p)
Set whether the simulation is paused.
ModelPtr ModelByName(const std::string &_name) const
Get a model by name.
void RunBlocking(const unsigned int _iterations=0)
Run the world.
boost::shared_ptr< PresetManager > PresetManagerPtr
Definition: PhysicsTypes.hh:129
bool AtmosphereEnabled() const
check if atmosphere model is enabled/disabled.
const sdf::ElementPtr SDF()
Get the SDF of the world in the current state.
bool SensorsInitialized() const
Get whether sensors have been initialized.
std::string UniqueModelName(const std::string &_name)
Get a model name which doesn't equal any existing model's name, by appending numbers to the given nam...
common::Time SimTime() const
Get the world simulation time, note if you want the PC wall clock call common::Time::GetWallTime.
bool PhysicsEnabled() const
check if physics engine is enabled/disabled.
std::string Name() const
Get the name of the world.
common::URI URI() const
Return the URI of the world.
std::vector< LightPtr > Light_V
Definition: PhysicsTypes.hh:221
ModelPtr ModelBelowPoint(const ignition::math::Vector3d &_pt) const
Get the nearest model below and not encapsulating a point.
physics::Atmosphere & Atmosphere() const
Get a reference to the atmosphere model used by the world.
virtual ignition::math::Vector3d MagneticField() const
Return the magnetic field vector.