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#ifdef _WIN32
21 // Ensure that Winsock2.h is included before Windows.h, which can get
22 // pulled in by anybody (e.g., Boost).
23 #include <Winsock2.h>
24#endif
25
26#include <vector>
27#include <list>
28#include <set>
29#include <deque>
30#include <string>
31#include <memory>
32
33#include <boost/enable_shared_from_this.hpp>
34
35#include <sdf/sdf.hh>
36
38
39#include "gazebo/msgs/msgs.hh"
40
44#include "gazebo/common/URI.hh"
45
50#include "gazebo/util/system.hh"
51
52// Forward declare reference and pointer parameters
53namespace ignition
54{
55 namespace msgs
56 {
57 class Plugin_V;
58 class StringMsg;
59 }
60}
61
62namespace gazebo
63{
64 namespace physics
65 {
67 class WorldPrivate;
68
71
80 class GZ_PHYSICS_VISIBLE World :
81 public boost::enable_shared_from_this<World>
82 {
86 public: explicit World(const std::string &_name = "");
87
89 public: ~World();
90
94 public: void Load(sdf::ElementPtr _sdf);
95
98 public: const sdf::ElementPtr SDF();
99
103 public: void Save(const std::string &_filename);
104
107 public: void Init();
108
113 public: void Run(const unsigned int _iterations = 0);
114
117 public: bool Running() const;
118
122 public: void Stop();
123
126 public: void Fini();
127
131 public: void Clear();
132
135 public: std::string Name() const;
136
140 public: PhysicsEnginePtr Physics() const;
141
145
149
152 public: physics::Wind &Wind() const;
153
157
160 public: ignition::math::Vector3d Gravity() const;
161
164 public: void SetGravity(const ignition::math::Vector3d &_gravity);
165
168 public: void SetGravitySDF(const ignition::math::Vector3d &_gravity);
169
172 public: virtual ignition::math::Vector3d MagneticField() const;
173
176 public: void SetMagneticField(const ignition::math::Vector3d &_mag);
177
180 public: unsigned int ModelCount() const;
181
187 public: ModelPtr ModelByIndex(const unsigned int _index) const;
188
191 public: Model_V Models() const;
192
195 public: unsigned int LightCount() const;
196
199 public: Light_V Lights() const;
200
205 public: void ResetEntities(Base::EntityType _type = Base::BASE);
206
208 public: void ResetTime();
209
211 public: void Reset();
212
215 public: void PrintEntityTree();
216
220 public: common::Time SimTime() const;
221
224 public: void SetSimTime(const common::Time &_t);
225
228 public: common::Time PauseTime() const;
229
232 public: common::Time StartTime() const;
233
236 public: common::Time RealTime() const;
237
240 public: bool IsPaused() const;
241
244 public: void SetPaused(const bool _p);
245
251 public: BasePtr BaseByName(const std::string &_name) const;
252
258 public: ModelPtr ModelByName(const std::string &_name) const;
259
265 public: LightPtr LightByName(const std::string &_name) const;
266
272 public: EntityPtr EntityByName(const std::string &_name) const;
273
282 const ignition::math::Vector3d &_pt) const;
283
290 const ignition::math::Vector3d &_pt) const;
291
294 public: void SetState(const WorldState &_state);
295
299 public: void InsertModelFile(const std::string &_sdfFilename);
300
304 public: void InsertModelString(const std::string &_sdfString);
305
309 public: void InsertModelSDF(const sdf::SDF &_sdf);
310
314 public: std::string StripWorldName(const std::string &_name) const;
315
319 public: void EnableAllModels();
320
324 public: void DisableAllModels();
325
328 public: void Step(const unsigned int _steps);
329
334 public: void LoadPlugin(const std::string &_filename,
335 const std::string &_name,
336 sdf::ElementPtr _sdf);
337
340 public: void RemovePlugin(const std::string &_name);
341
344 public: std::mutex &WorldPoseMutex() const;
345
348 public: bool PhysicsEnabled() const;
349
352 public: void SetPhysicsEnabled(const bool _enable);
353
356 public: bool WindEnabled() const;
357
360 public: void SetWindEnabled(const bool _enable);
361
364 public: bool AtmosphereEnabled() const;
365
368 public: void SetAtmosphereEnabled(const bool _enable);
369
371 public: void UpdateStateSDF();
372
375 public: bool IsLoaded() const;
376
379 public: void ClearModels();
380
386
392
397 public: void PublishLightPose(const physics::LightPtr _light);
398
401 public: uint32_t Iterations() const;
402
405 public: msgs::Scene SceneMsg() const;
406
411 public: void RunBlocking(const unsigned int _iterations = 0);
412
417 public: void RemoveModel(ModelPtr _model);
418
423 public: void RemoveModel(const std::string &_name);
424
427 public: void ResetPhysicsStates();
428
435 public: void _AddDirty(Entity *_entity);
436
439 public: bool SensorsInitialized() const;
440
445 public: void _SetSensorsInitialized(const bool _init);
446
449 public: common::URI URI() const;
450
456 public: std::string UniqueModelName(const std::string &_name);
457
465 private: ModelPtr ModelById(const unsigned int _id) const;
467
471 private: void LoadPlugins();
472
476 private: void LoadEntities(sdf::ElementPtr _sdf, BasePtr _parent);
477
482 private: ModelPtr LoadModel(sdf::ElementPtr _sdf, BasePtr _parent);
483
488 public: LightPtr LoadLight(const sdf::ElementPtr &_sdf,
489 const BasePtr &_parent);
490
495 private: ActorPtr LoadActor(sdf::ElementPtr _sdf, BasePtr _parent);
496
501 private: RoadPtr LoadRoad(sdf::ElementPtr _sdf, BasePtr _parent);
502
504 private: void RunLoop();
505
507 private: void Step();
508
510 private: void LogStep();
511
513 private: void Update();
514
517 private: void OnPause(bool _p);
518
520 private: void OnStep();
521
524 private: void OnControl(ConstWorldControlPtr &_data);
525
528 private: void OnPlaybackControl(ConstLogPlaybackControlPtr &_data);
529
532 private: void OnRequest(ConstRequestPtr &_msg);
533
538 private: void BuildSceneMsg(msgs::Scene &_scene, BasePtr _entity);
539
542 private: void JointLog(ConstJointPtr &_msg);
543
546 private: void OnFactoryMsg(ConstFactoryPtr &_data);
547
550 private: void OnModelMsg(ConstModelPtr &_msg);
551
553 private: void ModelUpdateTBB();
554
556 private: void ModelUpdateSingleLoop();
557
560 private: void LoadPlugin(sdf::ElementPtr _sdf);
561
565 private: void FillModelMsg(msgs::Model &_msg, ModelPtr _model);
566
569 private: void ProcessEntityMsgs();
570
573 private: void ProcessRequestMsgs();
574
577 private: void ProcessFactoryMsgs();
578
581 private: void ProcessModelMsgs();
582
585 private: void ProcessLightFactoryMsgs();
586
589 private: void ProcessLightModifyMsgs();
590
593 private: void ProcessPlaybackControlMsgs();
594
596 private: bool OnLog(std::ostringstream &_stream);
597
600 private: void LogModelResources();
601
603 private: void ProcessMessages();
604
606 private: void PublishWorldStats();
607
609 private: void LogWorker();
610
612 private: void RegisterIntrospectionItems();
613
615 private: void UnregisterIntrospectionItems();
616
620 private: void OnLightFactoryMsg(ConstLightPtr &_msg);
621
625 private: void OnLightModifyMsg(ConstLightPtr &_msg);
626
643 private: bool PluginInfoService(const ignition::msgs::StringMsg &_request,
644 ignition::msgs::Plugin_V &_plugins);
645
648 private: std::unique_ptr<WorldPrivate> dataPtr;
649
651 private: friend class DARTLink;
652
654 private: friend class SimbodyPhysics;
655 };
657 }
658}
659#endif
default namespace for gazebo
Forward declarations for transport.
A Time class, can be used to hold wall- or sim-time.
Definition Time.hh:48
A complete URI.
Definition URI.hh:177
This models a base atmosphere class to serve as a common interface to any derived atmosphere models.
Definition Atmosphere.hh:43
EntityType
Unique identifiers for all entity types.
Definition Base.hh:81
@ BASE
Base type.
Definition Base.hh:83
Base class for all physics objects in Gazebo.
Definition Entity.hh:53
Simbody physics engine.
Definition SimbodyPhysics.hh:43
Base class for wind.
Definition Wind.hh:42
Store state information of a physics::World object.
Definition WorldState.hh:48
The world provides access to all other object within a simulated environment.
Definition World.hh:82
void Fini()
Finalize the world.
void SetState(const WorldState &_state)
Set the current world state.
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...
void RemoveModel(ModelPtr _model)
Remove a model.
void SetPhysicsEnabled(const bool _enable)
enable/disable physics engine during World::Update.
void InsertModelFile(const std::string &_sdfFilename)
Insert a model from an SDF file.
unsigned int LightCount() const
Get the number of lights.
void ClearModels()
Remove all entities from the world.
void Stop()
Stop the world.
bool SensorsInitialized() const
Get whether sensors have been initialized.
void SetGravitySDF(const ignition::math::Vector3d &_gravity)
Set the gravity sdf value.
void Init()
Initialize the world.
ModelPtr ModelByIndex(const unsigned int _index) const
Get a model based on an index.
void SetMagneticField(const ignition::math::Vector3d &_mag)
Set the magnetic field vector.
PhysicsEnginePtr Physics() const
Return the physics engine.
PresetManagerPtr PresetMgr() const
Return the preset manager.
void _SetSensorsInitialized(const bool _init)
void SetSimTime(const common::Time &_t)
Set the sim time.
virtual ignition::math::Vector3d MagneticField() const
Return the magnetic field vector.
void Reset()
Reset time and model poses, configurations in simulation.
void PublishLightPose(const physics::LightPtr _light)
Publish pose updates for a light.
void RemovePlugin(const std::string &_name)
Remove a running plugin.
void _AddDirty(Entity *_entity)
std::string Name() const
Get the name of the world.
physics::Atmosphere & Atmosphere() const
Get a reference to the atmosphere model used by the world.
void SetPaused(const bool _p)
Set whether the simulation is paused.
void ResetPhysicsStates()
Reset the velocity, acceleration, force and torque of all child models.
void SetWindEnabled(const bool _enable)
enable/disable wind.
bool WindEnabled() const
check if wind is enabled/disabled.
void SetAtmosphereEnabled(const bool _enable)
enable/disable atmosphere model.
std::string StripWorldName(const std::string &_name) const
Return a version of the name with "<world_name>::" removed.
void ResetEntities(Base::EntityType _type=Base::BASE)
Reset with options.
uint32_t Iterations() const
Get the total number of iterations.
void EnableAllModels()
Enable all links in all the models.
Light_V Lights() const
Get a list of all the lights.
LightPtr LightByName(const std::string &_name) const
Get a light by name.
bool IsLoaded() const
Return true if the world has been loaded.
void InsertModelString(const std::string &_sdfString)
Insert a model from an SDF string.
common::URI URI() const
Return the URI of the world.
physics::Wind & Wind() const
Get a reference to the wind used by the world.
ignition::math::Vector3d Gravity() const
Return the gravity vector.
void Run(const unsigned int _iterations=0)
Run the world in a thread.
ModelPtr ModelBelowPoint(const ignition::math::Vector3d &_pt) const
Get the nearest model below and not encapsulating a point.
void PublishModelScale(physics::ModelPtr _model)
Publish scale updates for a model.
std::mutex & WorldPoseMutex() const
Get the set world pose mutex.
LightPtr LoadLight(const sdf::ElementPtr &_sdf, const BasePtr &_parent)
Load a light.
void ResetTime()
Reset simulation time back to zero.
Model_V Models() const
Get a list of all the models.
bool PhysicsEnabled() const
check if physics engine is enabled/disabled.
bool IsPaused() const
Returns the state of the simulation true if paused.
common::Time StartTime() const
Get the wall time simulation was started.
common::Time RealTime() const
Get the real time (elapsed time).
void Clear()
Remove all entities from the world.
void InsertModelSDF(const sdf::SDF &_sdf)
Insert a model using SDF.
void Save(const std::string &_filename)
Save a world to a file.
bool Running() const
Return the running state of the world.
BasePtr BaseByName(const std::string &_name) const
Get an element by name.
void DisableAllModels()
Disable all links in all the models.
ModelPtr ModelByName(const std::string &_name) const
Get a model by name.
const sdf::ElementPtr SDF()
Get the SDF of the world in the current state.
void LoadPlugin(const std::string &_filename, const std::string &_name, sdf::ElementPtr _sdf)
Load a plugin.
void PublishModelPose(physics::ModelPtr _model)
Publish pose updates for a model.
EntityPtr EntityByName(const std::string &_name) const
Get a pointer to an Entity based on a name.
common::SphericalCoordinatesPtr SphericalCoords() const
Return the spherical coordinates converter.
void Load(sdf::ElementPtr _sdf)
Load the world using SDF parameters.
void PrintEntityTree()
Print Entity tree.
void UpdateStateSDF()
Update the state SDF value from the current state.
bool AtmosphereEnabled() const
check if atmosphere model is enabled/disabled.
World(const std::string &_name="")
Constructor.
msgs::Scene SceneMsg() const
Get the current scene in message form.
common::Time SimTime() const
Get the world simulation time, note if you want the PC wall clock call common::Time::GetWallTime.
void RunBlocking(const unsigned int _iterations=0)
Run the world.
void Step(const unsigned int _steps)
Step the world forward in time.
EntityPtr EntityBelowPoint(const ignition::math::Vector3d &_pt) const
Get the nearest entity below a point.
unsigned int ModelCount() const
Get the number of models.
void RemoveModel(const std::string &_name)
Remove a model by name.
common::Time PauseTime() const
Get the amount of time simulation has been paused.
void SetGravity(const ignition::math::Vector3d &_gravity)
Set the gravity vector.
boost::shared_ptr< SphericalCoordinates > SphericalCoordinatesPtr
Definition CommonTypes.hh:121
std::vector< ModelPtr > Model_V
Definition PhysicsTypes.hh:205
boost::shared_ptr< Light > LightPtr
Definition PhysicsTypes.hh:105
boost::shared_ptr< PhysicsEngine > PhysicsEnginePtr
Definition PhysicsTypes.hh:125
boost::shared_ptr< Actor > ActorPtr
Definition PhysicsTypes.hh:97
boost::shared_ptr< Base > BasePtr
Definition PhysicsTypes.hh:77
std::vector< LightPtr > Light_V
Definition PhysicsTypes.hh:221
boost::shared_ptr< PresetManager > PresetManagerPtr
Definition PhysicsTypes.hh:129
boost::shared_ptr< Entity > EntityPtr
Definition PhysicsTypes.hh:85
boost::shared_ptr< Road > RoadPtr
Definition PhysicsTypes.hh:161
boost::shared_ptr< Model > ModelPtr
Definition PhysicsTypes.hh:93
Forward declarations for the common classes.
Definition Animation.hh:27
Definition Model.hh:41