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 
43 #include "gazebo/common/Event.hh"
44 #include "gazebo/common/URI.hh"
45 
46 #include "gazebo/physics/Base.hh"
49 #include "gazebo/physics/Wind.hh"
50 #include "gazebo/util/system.hh"
51 
52 // Forward declare reference and pointer parameters
53 namespace ignition
54 {
55  namespace msgs
56  {
57  class Plugin_V;
58  class StringMsg;
59  }
60 }
61 
62 namespace 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 
121  public: void Stop();
122 
125  public: void Fini();
126 
130  public: void Clear();
131 
134  public: std::string Name() const;
135 
139  public: PhysicsEnginePtr Physics() const;
140 
143  public: physics::Atmosphere &Atmosphere() const;
144 
147  public: PresetManagerPtr PresetMgr() const;
148 
151  public: physics::Wind &Wind() const;
152 
155  public: common::SphericalCoordinatesPtr SphericalCoords() const;
156 
159  public: ignition::math::Vector3d Gravity() const;
160 
163  public: void SetGravity(const ignition::math::Vector3d &_gravity);
164 
167  public: void SetGravitySDF(const ignition::math::Vector3d &_gravity);
168 
171  public: virtual ignition::math::Vector3d MagneticField() const;
172 
175  public: void SetMagneticField(const ignition::math::Vector3d &_mag);
176 
179  public: unsigned int ModelCount() const;
180 
186  public: ModelPtr ModelByIndex(const unsigned int _index) const;
187 
190  public: Model_V Models() const;
191 
194  public: unsigned int LightCount() const;
195 
198  public: Light_V Lights() const;
199 
204  public: void ResetEntities(Base::EntityType _type = Base::BASE);
205 
207  public: void ResetTime();
208 
210  public: void Reset();
211 
214  public: void PrintEntityTree();
215 
219  public: common::Time SimTime() const;
220 
223  public: void SetSimTime(const common::Time &_t);
224 
227  public: common::Time PauseTime() const;
228 
231  public: common::Time StartTime() const;
232 
235  public: common::Time RealTime() const;
236 
239  public: bool IsPaused() const;
240 
243  public: void SetPaused(const bool _p);
244 
250  public: BasePtr BaseByName(const std::string &_name) const;
251 
257  public: ModelPtr ModelByName(const std::string &_name) const;
258 
264  public: LightPtr LightByName(const std::string &_name) const;
265 
271  public: EntityPtr EntityByName(const std::string &_name) const;
272 
280  public: ModelPtr ModelBelowPoint(
281  const ignition::math::Vector3d &_pt) const;
282 
288  public: EntityPtr EntityBelowPoint(
289  const ignition::math::Vector3d &_pt) const;
290 
293  public: void SetState(const WorldState &_state);
294 
298  public: void InsertModelFile(const std::string &_sdfFilename);
299 
303  public: void InsertModelString(const std::string &_sdfString);
304 
308  public: void InsertModelSDF(const sdf::SDF &_sdf);
309 
313  public: std::string StripWorldName(const std::string &_name) const;
314 
318  public: void EnableAllModels();
319 
323  public: void DisableAllModels();
324 
327  public: void Step(const unsigned int _steps);
328 
333  public: void LoadPlugin(const std::string &_filename,
334  const std::string &_name,
335  sdf::ElementPtr _sdf);
336 
339  public: void RemovePlugin(const std::string &_name);
340 
343  public: std::mutex &WorldPoseMutex() const;
344 
347  public: bool PhysicsEnabled() const;
348 
351  public: void SetPhysicsEnabled(const bool _enable);
352 
355  public: bool WindEnabled() const;
356 
359  public: void SetWindEnabled(const bool _enable);
360 
363  public: bool AtmosphereEnabled() const;
364 
367  public: void SetAtmosphereEnabled(const bool _enable);
368 
370  public: void UpdateStateSDF();
371 
374  public: bool IsLoaded() const;
375 
378  public: void ClearModels();
379 
384  public: void PublishModelPose(physics::ModelPtr _model);
385 
390  public: void PublishModelScale(physics::ModelPtr _model);
391 
396  public: void PublishLightPose(const physics::LightPtr _light);
397 
400  public: uint32_t Iterations() const;
401 
404  public: msgs::Scene SceneMsg() const;
405 
410  public: void RunBlocking(const unsigned int _iterations = 0);
411 
416  public: void RemoveModel(ModelPtr _model);
417 
422  public: void RemoveModel(const std::string &_name);
423 
426  public: void ResetPhysicsStates();
427 
434  public: void _AddDirty(Entity *_entity);
435 
438  public: bool SensorsInitialized() const;
439 
444  public: void _SetSensorsInitialized(const bool _init);
445 
448  public: common::URI URI() const;
449 
455  public: std::string UniqueModelName(const std::string &_name);
456 
464  private: ModelPtr ModelById(const unsigned int _id) const;
466 
470  private: void LoadPlugins();
471 
475  private: void LoadEntities(sdf::ElementPtr _sdf, BasePtr _parent);
476 
481  private: ModelPtr LoadModel(sdf::ElementPtr _sdf, BasePtr _parent);
482 
487  public: LightPtr LoadLight(const sdf::ElementPtr &_sdf,
488  const BasePtr &_parent);
489 
494  private: ActorPtr LoadActor(sdf::ElementPtr _sdf, BasePtr _parent);
495 
500  private: RoadPtr LoadRoad(sdf::ElementPtr _sdf, BasePtr _parent);
501 
503  private: void RunLoop();
504 
506  private: void Step();
507 
509  private: void LogStep();
510 
512  private: void Update();
513 
516  private: void OnPause(bool _p);
517 
519  private: void OnStep();
520 
523  private: void OnControl(ConstWorldControlPtr &_data);
524 
527  private: void OnPlaybackControl(ConstLogPlaybackControlPtr &_data);
528 
531  private: void OnRequest(ConstRequestPtr &_msg);
532 
537  private: void BuildSceneMsg(msgs::Scene &_scene, BasePtr _entity);
538 
541  private: void JointLog(ConstJointPtr &_msg);
542 
545  private: void OnFactoryMsg(ConstFactoryPtr &_data);
546 
549  private: void OnModelMsg(ConstModelPtr &_msg);
550 
552  private: void ModelUpdateTBB();
553 
555  private: void ModelUpdateSingleLoop();
556 
559  private: void LoadPlugin(sdf::ElementPtr _sdf);
560 
564  private: void FillModelMsg(msgs::Model &_msg, ModelPtr _model);
565 
568  private: void ProcessEntityMsgs();
569 
572  private: void ProcessRequestMsgs();
573 
576  private: void ProcessFactoryMsgs();
577 
580  private: void ProcessModelMsgs();
581 
584  private: void ProcessLightFactoryMsgs();
585 
588  private: void ProcessLightModifyMsgs();
589 
591  private: bool OnLog(std::ostringstream &_stream);
592 
594  private: void ProcessMessages();
595 
597  private: void PublishWorldStats();
598 
600  private: void LogWorker();
601 
603  private: void RegisterIntrospectionItems();
604 
606  private: void UnregisterIntrospectionItems();
607 
611  private: void OnLightFactoryMsg(ConstLightPtr &_msg);
612 
616  private: void OnLightModifyMsg(ConstLightPtr &_msg);
617 
634  private: bool PluginInfoService(const ignition::msgs::StringMsg &_request,
635  ignition::msgs::Plugin_V &_plugins);
636 
639  private: std::unique_ptr<WorldPrivate> dataPtr;
640 
642  private: friend class DARTLink;
643 
645  private: friend class SimbodyPhysics;
646  };
648  }
649 }
650 #endif
boost::shared_ptr< Actor > ActorPtr
Definition: PhysicsTypes.hh:97
boost::shared_ptr< PresetManager > PresetManagerPtr
Definition: PhysicsTypes.hh:129
std::vector< ModelPtr > Model_V
Definition: PhysicsTypes.hh:205
std::vector< LightPtr > Light_V
Definition: PhysicsTypes.hh:221
Forward declarations for transport.
default namespace for gazebo
A complete URI.
Definition: URI.hh:176
Base type.
Definition: Base.hh:83
boost::shared_ptr< PhysicsEngine > PhysicsEnginePtr
Definition: PhysicsTypes.hh:125
The world provides access to all other object within a simulated environment.
Definition: World.hh:80
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
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
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:81
boost::shared_ptr< Light > LightPtr
Definition: PhysicsTypes.hh:105
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:77
A Time class, can be used to hold wall- or sim-time.
Definition: Time.hh:44