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 #include "gazebo/math/Vector3.hh"
41 
44 #include "gazebo/common/Event.hh"
45 #include "gazebo/common/URI.hh"
46 
47 #include "gazebo/physics/Base.hh"
50 #include "gazebo/physics/Wind.hh"
51 #include "gazebo/util/system.hh"
52 
53 // Forward declare reference and pointer parameters
54 namespace ignition
55 {
56  namespace msgs
57  {
58  class Plugin_V;
59  class StringMsg;
60  }
61 }
62 
63 namespace gazebo
64 {
65  namespace physics
66  {
68  class WorldPrivate;
69 
72 
81  class GZ_PHYSICS_VISIBLE World :
82  public boost::enable_shared_from_this<World>
83  {
87  public: explicit World(const std::string &_name = "");
88 
90  public: ~World();
91 
95  public: void Load(sdf::ElementPtr _sdf);
96 
100  public: void Save(const std::string &_filename);
101 
104  public: void Init();
105 
110  public: void Run(const unsigned int _iterations = 0);
111 
115  public: bool GetRunning() const GAZEBO_DEPRECATED(8.0);
116 
119  public: bool Running() const;
120 
123  public: void Stop();
124 
127  public: void Fini();
128 
132  public: void Clear();
133 
137  public: std::string GetName() const GAZEBO_DEPRECATED(8.0);
138 
141  public: std::string Name() const;
142 
147  public: PhysicsEnginePtr GetPhysicsEngine() const GAZEBO_DEPRECATED(8.0);
148 
152  public: PhysicsEnginePtr Physics() const;
153 
156  public: physics::Atmosphere &Atmosphere() const;
157 
161  public: PresetManagerPtr GetPresetManager() const GAZEBO_DEPRECATED(8.0);
162 
165  public: PresetManagerPtr PresetMgr() const;
166 
169  public: physics::Wind &Wind() const;
170 
174  public: common::SphericalCoordinatesPtr GetSphericalCoordinates() const
175  GAZEBO_DEPRECATED(8.0);
176 
179  public: common::SphericalCoordinatesPtr SphericalCoords() const;
180 
183  public: ignition::math::Vector3d Gravity() const;
184 
187  public: void SetGravity(const ignition::math::Vector3d &_gravity);
188 
191  public: void SetGravitySDF(const ignition::math::Vector3d &_gravity);
192 
195  public: virtual ignition::math::Vector3d MagneticField() const;
196 
199  public: void SetMagneticField(const ignition::math::Vector3d &_mag);
200 
204  public: unsigned int GetModelCount() const GAZEBO_DEPRECATED(8.0);
205 
208  public: unsigned int ModelCount() const;
209 
216  public: ModelPtr GetModel(unsigned int _index) const
217  GAZEBO_DEPRECATED(8.0);
218 
224  public: ModelPtr ModelByIndex(const unsigned int _index) const;
225 
229  public: Model_V GetModels() const GAZEBO_DEPRECATED(8.0);
230 
233  public: Model_V Models() const;
234 
237  public: unsigned int LightCount() const;
238 
241  public: Light_V Lights() const;
242 
247  public: void ResetEntities(Base::EntityType _type = Base::BASE);
248 
250  public: void ResetTime();
251 
253  public: void Reset();
254 
257  public: void PrintEntityTree();
258 
263  public: common::Time GetSimTime() const GAZEBO_DEPRECATED(8.0);
264 
268  public: common::Time SimTime() const;
269 
272  public: void SetSimTime(const common::Time &_t);
273 
277  public: common::Time GetPauseTime() const GAZEBO_DEPRECATED(8.0);
278 
281  public: common::Time PauseTime() const;
282 
286  public: common::Time GetStartTime() const GAZEBO_DEPRECATED(8.0);
287 
290  public: common::Time StartTime() const;
291 
295  public: common::Time GetRealTime() const GAZEBO_DEPRECATED(8.0);
296 
299  public: common::Time RealTime() const;
300 
303  public: bool IsPaused() const;
304 
307  public: void SetPaused(const bool _p);
308 
315  public: BasePtr GetByName(const std::string &_name)
316  GAZEBO_DEPRECATED(8.0);
317 
323  public: BasePtr BaseByName(const std::string &_name) const;
324 
331  public: ModelPtr GetModel(const std::string &_name)
332  GAZEBO_DEPRECATED(8.0);
333 
339  public: ModelPtr ModelByName(const std::string &_name) const;
340 
347  public: LightPtr Light(const std::string &_name)
348  GAZEBO_DEPRECATED(8.0);
349 
355  public: LightPtr LightByName(const std::string &_name) const;
356 
363  public: EntityPtr GetEntity(const std::string &_name)
364  GAZEBO_DEPRECATED(8.0);
365 
371  public: EntityPtr EntityByName(const std::string &_name) const;
372 
382  public: ModelPtr GetModelBelowPoint(const math::Vector3 &_pt)
383  GAZEBO_DEPRECATED(8.0);
384 
392  public: ModelPtr ModelBelowPoint(
393  const ignition::math::Vector3d &_pt) const;
394 
402  public: EntityPtr GetEntityBelowPoint(const math::Vector3 &_pt)
403  GAZEBO_DEPRECATED(8.0);
404 
410  public: EntityPtr EntityBelowPoint(
411  const ignition::math::Vector3d &_pt) const;
412 
415  public: void SetState(const WorldState &_state);
416 
420  public: void InsertModelFile(const std::string &_sdfFilename);
421 
425  public: void InsertModelString(const std::string &_sdfString);
426 
430  public: void InsertModelSDF(const sdf::SDF &_sdf);
431 
435  public: std::string StripWorldName(const std::string &_name) const;
436 
440  public: void EnableAllModels();
441 
445  public: void DisableAllModels();
446 
449  public: void Step(const unsigned int _steps);
450 
455  public: void LoadPlugin(const std::string &_filename,
456  const std::string &_name,
457  sdf::ElementPtr _sdf);
458 
461  public: void RemovePlugin(const std::string &_name);
462 
466  public: std::mutex &GetSetWorldPoseMutex() const GAZEBO_DEPRECATED(8.0);
467 
470  public: std::mutex &WorldPoseMutex() const;
471 
475  public: bool GetEnablePhysicsEngine() GAZEBO_DEPRECATED(8.0);
476 
479  public: bool PhysicsEnabled() const;
480 
484  public: void EnablePhysicsEngine(const bool _enable)
485  GAZEBO_DEPRECATED(8.0);
486 
489  public: void SetPhysicsEnabled(const bool _enable);
490 
493  public: bool WindEnabled() const;
494 
497  public: void SetWindEnabled(const bool _enable);
498 
501  public: bool AtmosphereEnabled() const;
502 
505  public: void SetAtmosphereEnabled(const bool _enable);
506 
508  public: void UpdateStateSDF();
509 
512  public: bool IsLoaded() const;
513 
516  public: void ClearModels();
517 
522  public: void PublishModelPose(physics::ModelPtr _model);
523 
528  public: void PublishModelScale(physics::ModelPtr _model);
529 
534  public: void PublishLightPose(const physics::LightPtr _light);
535 
539  public: uint32_t GetIterations() const GAZEBO_DEPRECATED(8.0);
540 
543  public: uint32_t Iterations() const;
544 
548  public: msgs::Scene GetSceneMsg() const GAZEBO_DEPRECATED(8.0);
549 
552  public: msgs::Scene SceneMsg() const;
553 
558  public: void RunBlocking(const unsigned int _iterations = 0);
559 
564  public: void RemoveModel(ModelPtr _model);
565 
570  public: void RemoveModel(const std::string &_name);
571 
574  public: void ResetPhysicsStates();
575 
582  public: void _AddDirty(Entity *_entity);
583 
586  public: bool SensorsInitialized() const;
587 
592  public: void _SetSensorsInitialized(const bool _init);
593 
596  public: common::URI URI() const;
597 
603  public: std::string UniqueModelName(const std::string &_name);
604 
612  private: ModelPtr ModelById(const unsigned int _id) const;
614 
618  private: void LoadPlugins();
619 
623  private: void LoadEntities(sdf::ElementPtr _sdf, BasePtr _parent);
624 
629  private: ModelPtr LoadModel(sdf::ElementPtr _sdf, BasePtr _parent);
630 
635  private: LightPtr LoadLight(const sdf::ElementPtr &_sdf,
636  const BasePtr &_parent);
637 
642  private: ActorPtr LoadActor(sdf::ElementPtr _sdf, BasePtr _parent);
643 
648  private: RoadPtr LoadRoad(sdf::ElementPtr _sdf, BasePtr _parent);
649 
651  private: void RunLoop();
652 
654  private: void Step();
655 
657  private: void LogStep();
658 
660  private: void Update();
661 
664  private: void OnPause(bool _p);
665 
667  private: void OnStep();
668 
671  private: void OnControl(ConstWorldControlPtr &_data);
672 
675  private: void OnPlaybackControl(ConstLogPlaybackControlPtr &_data);
676 
679  private: void OnRequest(ConstRequestPtr &_msg);
680 
685  private: void BuildSceneMsg(msgs::Scene &_scene, BasePtr _entity);
686 
689  private: void JointLog(ConstJointPtr &_msg);
690 
693  private: void OnFactoryMsg(ConstFactoryPtr &_data);
694 
697  private: void OnModelMsg(ConstModelPtr &_msg);
698 
700  private: void ModelUpdateTBB();
701 
703  private: void ModelUpdateSingleLoop();
704 
707  private: void LoadPlugin(sdf::ElementPtr _sdf);
708 
712  private: void FillModelMsg(msgs::Model &_msg, ModelPtr _model);
713 
716  private: void ProcessEntityMsgs();
717 
720  private: void ProcessRequestMsgs();
721 
724  private: void ProcessFactoryMsgs();
725 
728  private: void ProcessModelMsgs();
729 
732  private: void ProcessLightFactoryMsgs();
733 
736  private: void ProcessLightModifyMsgs();
737 
739  private: bool OnLog(std::ostringstream &_stream);
740 
742  private: void ProcessMessages();
743 
745  private: void PublishWorldStats();
746 
748  private: void LogWorker();
749 
751  private: void RegisterIntrospectionItems();
752 
754  private: void UnregisterIntrospectionItems();
755 
761  private: void OnLightMsg(ConstLightPtr &_msg);
762 
766  private: void OnLightFactoryMsg(ConstLightPtr &_msg);
767 
771  private: void OnLightModifyMsg(ConstLightPtr &_msg);
772 
789  private: void PluginInfoService(const ignition::msgs::StringMsg &_request,
790  ignition::msgs::Plugin_V &_plugins, bool &_success);
791 
794  private: std::unique_ptr<WorldPrivate> dataPtr;
795 
797  private: friend class DARTLink;
798 
800  private: friend class SimbodyPhysics;
801  };
803  }
804 }
805 #endif
boost::shared_ptr< Actor > ActorPtr
Definition: PhysicsTypes.hh:97
boost::shared_ptr< PresetManager > PresetManagerPtr
Definition: PhysicsTypes.hh:129
A light entity.
Definition: physics/Light.hh:33
std::vector< ModelPtr > Model_V
Definition: PhysicsTypes.hh:205
The Vector3 class represents the generic vector containing 3 elements.
Definition: Vector3.hh:44
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:81
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:56
#define GAZEBO_DEPRECATED(version)
Definition: system.hh:302
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