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 
96  public: const sdf::World *GetSDFDom() const;
97 
101  public: void Save(const std::string &_filename);
102 
106  public: void Init() GAZEBO_DEPRECATED(11.0);
107 
111  public: void Init(UpdateScenePosesFunc _func);
112 
117  public: void Run(const unsigned int _iterations = 0);
118 
121  public: bool Running() const;
122 
126  public: void Stop();
127 
130  public: void Fini();
131 
135  public: void Clear();
136 
139  public: std::string Name() const;
140 
144  public: PhysicsEnginePtr Physics() const;
145 
148  public: physics::Atmosphere &Atmosphere() const;
149 
152  public: PresetManagerPtr PresetMgr() const;
153 
156  public: physics::Wind &Wind() const;
157 
160  public: common::SphericalCoordinatesPtr SphericalCoords() const;
161 
164  public: ignition::math::Vector3d Gravity() const;
165 
168  public: void SetGravity(const ignition::math::Vector3d &_gravity);
169 
172  public: void SetGravitySDF(const ignition::math::Vector3d &_gravity);
173 
176  public: ignition::math::Vector3d MagneticField() const;
177 
180  public: void SetMagneticField(const ignition::math::Vector3d &_mag);
181 
184  public: unsigned int ModelCount() const;
185 
191  public: ModelPtr ModelByIndex(const unsigned int _index) const;
192 
195  public: Model_V Models() const;
196 
199  public: unsigned int LightCount() const;
200 
203  public: Light_V Lights() const;
204 
209  public: void ResetEntities(Base::EntityType _type = Base::BASE);
210 
212  public: void ResetTime();
213 
215  public: void Reset();
216 
219  public: void PrintEntityTree();
220 
224  public: common::Time SimTime() const;
225 
228  public: void SetSimTime(const common::Time &_t);
229 
232  public: common::Time PauseTime() const;
233 
236  public: common::Time StartTime() const;
237 
240  public: common::Time RealTime() const;
241 
244  public: bool IsPaused() const;
245 
248  public: void SetPaused(const bool _p);
249 
255  public: BasePtr BaseByName(const std::string &_name) const;
256 
262  public: ModelPtr ModelByName(const std::string &_name) const;
263 
269  public: LightPtr LightByName(const std::string &_name) const;
270 
276  public: EntityPtr EntityByName(const std::string &_name) const;
277 
285  public: ModelPtr ModelBelowPoint(
286  const ignition::math::Vector3d &_pt) const;
287 
293  public: EntityPtr EntityBelowPoint(
294  const ignition::math::Vector3d &_pt) const;
295 
298  public: void SetState(const WorldState &_state);
299 
303  public: void InsertModelFile(const std::string &_sdfFilename);
304 
308  public: void InsertModelString(const std::string &_sdfString);
309 
313  public: void InsertModelSDF(const sdf::SDF &_sdf);
314 
318  public: std::string StripWorldName(const std::string &_name) const;
319 
323  public: void EnableAllModels();
324 
328  public: void DisableAllModels();
329 
332  public: void Step(const unsigned int _steps);
333 
338  public: void LoadPlugin(const std::string &_filename,
339  const std::string &_name,
340  sdf::ElementPtr _sdf);
341 
344  public: void RemovePlugin(const std::string &_name);
345 
348  public: std::mutex &WorldPoseMutex() const;
349 
352  public: bool PhysicsEnabled() const;
353 
356  public: void SetPhysicsEnabled(const bool _enable);
357 
360  public: bool WindEnabled() const;
361 
364  public: void SetWindEnabled(const bool _enable);
365 
368  public: bool AtmosphereEnabled() const;
369 
372  public: void SetAtmosphereEnabled(const bool _enable);
373 
375  public: void UpdateStateSDF();
376 
379  public: bool IsLoaded() const;
380 
383  public: void ClearModels();
384 
389  public: void PublishModelPose(physics::ModelPtr _model);
390 
395  public: void PublishModelScale(physics::ModelPtr _model);
396 
401  public: void PublishLightPose(const physics::LightPtr _light);
402 
405  public: uint32_t Iterations() const;
406 
409  public: msgs::Scene SceneMsg() const;
410 
415  public: void RunBlocking(const unsigned int _iterations = 0);
416 
421  public: void RemoveModel(ModelPtr _model);
422 
427  public: void RemoveModel(const std::string &_name);
428 
431  public: void ResetPhysicsStates();
432 
439  public: void _AddDirty(Entity *_entity);
440 
443  public: bool SensorsInitialized() const;
444 
449  public: void _SetSensorsInitialized(const bool _init);
450 
453  public: common::URI URI() const;
454 
460  public: std::string UniqueModelName(const std::string &_name);
461 
469  private: ModelPtr ModelById(const unsigned int _id) const;
471 
475  private: void LoadPlugins();
476 
480  private: void LoadEntities(sdf::ElementPtr _sdf, BasePtr _parent);
481 
486  private: ModelPtr LoadModel(sdf::ElementPtr _sdf, BasePtr _parent);
487 
492  public: LightPtr LoadLight(const sdf::ElementPtr &_sdf,
493  const BasePtr &_parent);
494 
499  private: ActorPtr LoadActor(sdf::ElementPtr _sdf, BasePtr _parent);
500 
505  private: RoadPtr LoadRoad(sdf::ElementPtr _sdf, BasePtr _parent);
506 
508  private: void RunLoop();
509 
511  private: void Step();
512 
514  private: void LogStep();
515 
517  private: void Update();
518 
521  private: void OnPause(bool _p);
522 
524  private: void OnStep();
525 
528  private: void OnControl(ConstWorldControlPtr &_data);
529 
532  private: void OnPlaybackControl(ConstLogPlaybackControlPtr &_data);
533 
536  private: void OnRequest(ConstRequestPtr &_msg);
537 
542  private: void BuildSceneMsg(msgs::Scene &_scene, BasePtr _entity);
543 
546  private: void JointLog(ConstJointPtr &_msg);
547 
550  private: void OnFactoryMsg(ConstFactoryPtr &_data);
551 
554  private: void OnModelMsg(ConstModelPtr &_msg);
555 
557  private: void ModelUpdateTBB();
558 
560  private: void ModelUpdateSingleLoop();
561 
564  private: void LoadPlugin(sdf::ElementPtr _sdf);
565 
569  private: void FillModelMsg(msgs::Model &_msg, ModelPtr _model);
570 
573  private: void ProcessEntityMsgs();
574 
577  private: void ProcessRequestMsgs();
578 
581  private: void ProcessFactoryMsgs();
582 
585  private: void ProcessModelMsgs();
586 
589  private: void ProcessLightFactoryMsgs();
590 
593  private: void ProcessLightModifyMsgs();
594 
597  private: void ProcessPlaybackControlMsgs();
598 
600  private: bool OnLog(std::ostringstream &_stream);
601 
604  private: void LogModelResources();
605 
607  private: void ProcessMessages();
608 
610  private: void PublishWorldStats();
611 
613  private: void LogWorker();
614 
616  private: void RegisterIntrospectionItems();
617 
619  private: void UnregisterIntrospectionItems();
620 
624  private: void OnLightFactoryMsg(ConstLightPtr &_msg);
625 
629  private: void OnLightModifyMsg(ConstLightPtr &_msg);
630 
647  private: bool PluginInfoService(const ignition::msgs::StringMsg &_request,
648  ignition::msgs::Plugin_V &_plugins);
649 
652  private: std::unique_ptr<WorldPrivate> dataPtr;
653 
655  private: friend class DARTLink;
656 
658  private: friend class SimbodyPhysics;
659  };
661  }
662 }
663 #endif
boost::shared_ptr< Actor > ActorPtr
Definition: PhysicsTypes.hh:98
boost::shared_ptr< PresetManager > PresetManagerPtr
Definition: PhysicsTypes.hh:130
Forward declarations for the common classes.
Definition: Animation.hh:26
std::vector< ModelPtr > Model_V
Definition: PhysicsTypes.hh:206
std::function< void(const std::string &, const msgs::PosesStamped &)> UpdateScenePosesFunc
Function signature for API that updates scene poses.
Definition: PhysicsTypes.hh:252
std::vector< LightPtr > Light_V
Definition: PhysicsTypes.hh:222
Forward declarations for transport.
default namespace for gazebo
A complete URI.
Definition: URI.hh:176
boost::shared_ptr< PhysicsEngine > PhysicsEnginePtr
Definition: PhysicsTypes.hh:126
The world provides access to all other object within a simulated environment.
Definition: World.hh:74
Simbody physics engine.
Definition: SimbodyPhysics.hh:42
boost::shared_ptr< SphericalCoordinates > SphericalCoordinatesPtr
Definition: CommonTypes.hh:121
Base class for wind.
Definition: Wind.hh:41
Base class for all physics objects in Gazebo.
Definition: Entity.hh:52
#define GAZEBO_DEPRECATED(version)
Definition: system.hh:328
boost::shared_ptr< Entity > EntityPtr
Definition: PhysicsTypes.hh:86
boost::shared_ptr< Road > RoadPtr
Definition: PhysicsTypes.hh:162
boost::shared_ptr< Model > ModelPtr
Definition: PhysicsTypes.hh:94
GAZEBO_VISIBLE void Init(google::protobuf::Message &_message, const std::string &_id="")
Initialize a message.
EntityType
Unique identifiers for all entity types.
Definition: Base.hh:76
Definition: Model.hh:40
boost::shared_ptr< Light > LightPtr
Definition: PhysicsTypes.hh:106
Store state information of a physics::World object.
Definition: WorldState.hh:47
This models a base atmosphere class to serve as a common interface to any derived atmosphere models...
Definition: Atmosphere.hh:42
boost::shared_ptr< Base > BasePtr
Definition: PhysicsTypes.hh:78
A Time class, can be used to hold wall- or sim-time.
Definition: Time.hh:47