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  class Boolean;
54  }
55 }
56 
57 namespace gazebo
58 {
59  namespace physics
60  {
62  class WorldPrivate;
63 
66 
75  class GZ_PHYSICS_VISIBLE World :
76  public boost::enable_shared_from_this<World>
77  {
81  public: explicit World(const std::string &_name = "");
82 
84  public: ~World();
85 
89  public: void Load(sdf::ElementPtr _sdf);
90 
93  public: const sdf::ElementPtr SDF();
94 
97  public: const sdf::World *GetSDFDom() const;
98 
102  public: void Save(const std::string &_filename);
103 
107  public: void Init() GAZEBO_DEPRECATED(11.0);
108 
112  public: void Init(UpdateScenePosesFunc _func);
113 
118  public: void Run(const unsigned int _iterations = 0);
119 
122  public: bool Running() const;
123 
127  public: void Stop();
128 
131  public: void Fini();
132 
136  public: void Clear();
137 
140  public: std::string Name() const;
141 
145  public: PhysicsEnginePtr Physics() const;
146 
149  public: physics::Atmosphere &Atmosphere() const;
150 
153  public: PresetManagerPtr PresetMgr() const;
154 
157  public: physics::Wind &Wind() const;
158 
161  public: common::SphericalCoordinatesPtr SphericalCoords() const;
162 
165  public: ignition::math::Vector3d Gravity() const;
166 
169  public: void SetGravity(const ignition::math::Vector3d &_gravity);
170 
173  public: void SetGravitySDF(const ignition::math::Vector3d &_gravity);
174 
177  public: ignition::math::Vector3d MagneticField() const;
178 
181  public: void SetMagneticField(const ignition::math::Vector3d &_mag);
182 
185  public: unsigned int ModelCount() const;
186 
192  public: ModelPtr ModelByIndex(const unsigned int _index) const;
193 
196  public: Model_V Models() const;
197 
200  public: unsigned int LightCount() const;
201 
204  public: Light_V Lights() const;
205 
210  public: void ResetEntities(Base::EntityType _type = Base::BASE);
211 
213  public: void ResetTime();
214 
216  public: void Reset();
217 
220  public: void PrintEntityTree();
221 
225  public: common::Time SimTime() const;
226 
229  public: void SetSimTime(const common::Time &_t);
230 
233  public: common::Time PauseTime() const;
234 
237  public: common::Time StartTime() const;
238 
241  public: common::Time RealTime() const;
242 
245  public: bool IsPaused() const;
246 
249  public: void SetPaused(const bool _p);
250 
256  public: BasePtr BaseByName(const std::string &_name) const;
257 
263  public: ModelPtr ModelByName(const std::string &_name) const;
264 
270  public: LightPtr LightByName(const std::string &_name) const;
271 
277  public: EntityPtr EntityByName(const std::string &_name) const;
278 
286  public: ModelPtr ModelBelowPoint(
287  const ignition::math::Vector3d &_pt) const;
288 
294  public: EntityPtr EntityBelowPoint(
295  const ignition::math::Vector3d &_pt) const;
296 
299  public: void SetState(const WorldState &_state);
300 
304  public: void InsertModelFile(const std::string &_sdfFilename);
305 
309  public: void InsertModelString(const std::string &_sdfString);
310 
314  public: void InsertModelSDF(const sdf::SDF &_sdf);
315 
319  public: std::string StripWorldName(const std::string &_name) const;
320 
324  public: void EnableAllModels();
325 
329  public: void DisableAllModels();
330 
333  public: void Step(const unsigned int _steps);
334 
339  public: void LoadPlugin(const std::string &_filename,
340  const std::string &_name,
341  sdf::ElementPtr _sdf);
342 
345  public: void RemovePlugin(const std::string &_name);
346 
349  public: std::mutex &WorldPoseMutex() const;
350 
353  public: bool PhysicsEnabled() const;
354 
357  public: void SetPhysicsEnabled(const bool _enable);
358 
361  public: bool WindEnabled() const;
362 
365  public: void SetWindEnabled(const bool _enable);
366 
369  public: bool AtmosphereEnabled() const;
370 
373  public: void SetAtmosphereEnabled(const bool _enable);
374 
376  public: void UpdateStateSDF();
377 
380  public: bool IsLoaded() const;
381 
384  public: void ClearModels();
385 
390  public: void PublishModelPose(physics::ModelPtr _model);
391 
396  public: void PublishModelScale(physics::ModelPtr _model);
397 
402  public: void PublishLightPose(const physics::LightPtr _light);
403 
406  public: uint32_t Iterations() const;
407 
410  public: msgs::Scene SceneMsg() const;
411 
416  public: void RunBlocking(const unsigned int _iterations = 0);
417 
422  public: void RemoveModel(ModelPtr _model);
423 
428  public: void RemoveModel(const std::string &_name);
429 
432  public: void ResetPhysicsStates();
433 
440  public: void _AddDirty(Entity *_entity);
441 
444  public: bool SensorsInitialized() const;
445 
450  public: void _SetSensorsInitialized(const bool _init);
451 
454  public: common::URI URI() const;
455 
461  public: std::string UniqueModelName(const std::string &_name);
462 
465  public: void SetSensorWaitFunc(std::function<void(double, double)> _func);
466 
474  private: ModelPtr ModelById(const unsigned int _id) const;
476 
480  private: void LoadPlugins();
481 
485  private: void LoadEntities(sdf::ElementPtr _sdf, BasePtr _parent);
486 
491  private: ModelPtr LoadModel(sdf::ElementPtr _sdf, BasePtr _parent);
492 
497  public: LightPtr LoadLight(const sdf::ElementPtr &_sdf,
498  const BasePtr &_parent);
499 
504  private: ActorPtr LoadActor(sdf::ElementPtr _sdf, BasePtr _parent);
505 
510  private: RoadPtr LoadRoad(sdf::ElementPtr _sdf, BasePtr _parent);
511 
513  private: void RunLoop();
514 
516  private: void Step();
517 
519  private: void LogStep();
520 
522  private: void Update();
523 
526  private: void OnPause(bool _p);
527 
529  private: void OnStep();
530 
533  private: void OnControl(ConstWorldControlPtr &_data);
534 
537  private: void OnPlaybackControl(ConstLogPlaybackControlPtr &_data);
538 
541  private: void OnRequest(ConstRequestPtr &_msg);
542 
547  private: void BuildSceneMsg(msgs::Scene &_scene, BasePtr _entity);
548 
551  private: void JointLog(ConstJointPtr &_msg);
552 
555  private: void OnFactoryMsg(ConstFactoryPtr &_data);
556 
559  private: void OnModelMsg(ConstModelPtr &_msg);
560 
562  private: void ModelUpdateTBB();
563 
565  private: void ModelUpdateSingleLoop();
566 
569  private: void LoadPlugin(sdf::ElementPtr _sdf);
570 
574  private: void FillModelMsg(msgs::Model &_msg, ModelPtr _model);
575 
578  private: void ProcessEntityMsgs();
579 
582  private: void ProcessRequestMsgs();
583 
586  private: void ProcessFactoryMsgs();
587 
590  private: void ProcessModelMsgs();
591 
594  private: void ProcessLightFactoryMsgs();
595 
598  private: void ProcessLightModifyMsgs();
599 
602  private: void ProcessPlaybackControlMsgs();
603 
605  private: bool OnLog(std::ostringstream &_stream);
606 
609  private: void LogModelResources();
610 
612  private: void ProcessMessages();
613 
615  private: void PublishWorldStats();
616 
618  private: void LogWorker();
619 
621  private: void RegisterIntrospectionItems();
622 
624  private: void UnregisterIntrospectionItems();
625 
629  private: void OnLightFactoryMsg(ConstLightPtr &_msg);
630 
634  private: void OnLightModifyMsg(ConstLightPtr &_msg);
635 
652  private: bool PluginInfoService(const ignition::msgs::StringMsg &_request,
653  ignition::msgs::Plugin_V &_plugins);
654 
658  private: bool SceneInfoService(msgs::Scene &_response);
659 
663  private: bool ShadowCasterMaterialNameService(
664  ignition::msgs::StringMsg &_response);
665 
671  private: bool ShadowCasterRenderBackFacesService(
672  ignition::msgs::Boolean &_response);
673 
676  private: std::unique_ptr<WorldPrivate> dataPtr;
677 
679  private: friend class DARTLink;
680 
682  private: friend class SimbodyPhysics;
683  };
685  }
686 }
687 #endif
common
Definition: FuelModelDatabase.hh:42
default namespace for gazebo
Forward declarations for transport.
This models a base atmosphere class to serve as a common interface to any derived atmosphere models.
Definition: Atmosphere.hh:43
Base class for most physics classes.
Definition: Base.hh:73
Base class for all physics objects in Gazebo.
Definition: Entity.hh:53
A model is a collection of links, joints, and plugins.
Definition: Model.hh:60
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:77
void Save(const std::string &_filename)
Save a world to a file.
const sdf::ElementPtr SDF()
Get the SDF of the world in the current state.
const sdf::World * GetSDFDom() const
Get the SDF DOM for the world.
void Load(sdf::ElementPtr _sdf)
Load the world using SDF parameters.
void Init() GAZEBO_DEPRECATED(11.0)
Initialize the world.
World(const std::string &_name="")
Constructor.
GAZEBO_VISIBLE void Init(google::protobuf::Message &_message, const std::string &_id="")
Initialize a message.
boost::shared_ptr< SphericalCoordinates > SphericalCoordinatesPtr
Definition: CommonTypes.hh:121
std::vector< ModelPtr > Model_V
Definition: PhysicsTypes.hh:206
boost::shared_ptr< Light > LightPtr
Definition: PhysicsTypes.hh:106
boost::shared_ptr< PhysicsEngine > PhysicsEnginePtr
Definition: PhysicsTypes.hh:126
boost::shared_ptr< Actor > ActorPtr
Definition: PhysicsTypes.hh:98
boost::shared_ptr< Base > BasePtr
Definition: PhysicsTypes.hh:78
std::vector< LightPtr > Light_V
Definition: PhysicsTypes.hh:222
boost::shared_ptr< PresetManager > PresetManagerPtr
Definition: PhysicsTypes.hh:130
std::function< void(const std::string &, const msgs::PosesStamped &)> UpdateScenePosesFunc
Function signature for API that updates scene poses.
Definition: PhysicsTypes.hh:252
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
Forward declarations for the common classes.
Definition: Animation.hh:27
Definition: Model.hh:41
#define GAZEBO_DEPRECATED(version)
Definition: system.hh:328