World.hh
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2012-2015 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_WORLD_HH_
18 #define _GAZEBO_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 <boost/thread.hpp>
32 #include <boost/enable_shared_from_this.hpp>
33 #include <boost/shared_ptr.hpp>
34 
35 #include <sdf/sdf.hh>
36 
38 
39 #include "gazebo/msgs/msgs.hh"
40 
43 #include "gazebo/common/Event.hh"
44 
45 #include "gazebo/physics/Base.hh"
48 #include "gazebo/util/system.hh"
49 
50 namespace gazebo
51 {
52  namespace physics
53  {
55  class WorldPrivate;
56 
59 
69  public boost::enable_shared_from_this<World>
70  {
74  public: explicit World(const std::string &_name = "");
75 
77  public: ~World();
78 
82  public: void Load(sdf::ElementPtr _sdf);
83 
87  public: void Save(const std::string &_filename);
88 
91  public: void Init();
92 
97  public: void Run(unsigned int _iterations = 0);
98 
101  public: bool GetRunning() const;
102 
105  public: void Stop();
106 
109  public: void Fini();
110 
114  public: void Clear();
115 
118  public: std::string GetName() const;
119 
123  public: PhysicsEnginePtr GetPhysicsEngine() const;
124 
127  public: PresetManagerPtr GetPresetManager() const;
128 
131  public: common::SphericalCoordinatesPtr GetSphericalCoordinates() const;
132 
135  public: unsigned int GetModelCount() const;
136 
142  public: ModelPtr GetModel(unsigned int _index) const;
143 
146  public: Model_V GetModels() const;
147 
152  public: void ResetEntities(Base::EntityType _type = Base::BASE);
153 
155  public: void ResetTime();
156 
158  public: void Reset();
159 
162  public: void PrintEntityTree();
163 
167  public: common::Time GetSimTime() const;
168 
171  public: void SetSimTime(const common::Time &_t);
172 
175  public: common::Time GetPauseTime() const;
176 
179  public: common::Time GetStartTime() const;
180 
183  public: common::Time GetRealTime() const;
184 
187  public: bool IsPaused() const;
188 
191  public: void SetPaused(bool _p);
192 
198  public: BasePtr GetByName(const std::string &_name);
199 
205  public: ModelPtr GetModel(const std::string &_name);
206 
212  public: EntityPtr GetEntity(const std::string &_name);
213 
221  public: ModelPtr GetModelBelowPoint(const math::Vector3 &_pt);
222 
228  public: EntityPtr GetEntityBelowPoint(const math::Vector3 &_pt);
229 
232  public: void SetState(const WorldState &_state);
233 
237  public: void InsertModelFile(const std::string &_sdfFilename);
238 
242  public: void InsertModelString(const std::string &_sdfString);
243 
247  public: void InsertModelSDF(const sdf::SDF &_sdf);
248 
252  public: std::string StripWorldName(const std::string &_name) const;
253 
257  public: void EnableAllModels();
258 
262  public: void DisableAllModels();
263 
266  public: void Step(unsigned int _steps);
267 
272  public: void LoadPlugin(const std::string &_filename,
273  const std::string &_name,
274  sdf::ElementPtr _sdf);
275 
278  public: void RemovePlugin(const std::string &_name);
279 
282  public: boost::mutex *GetSetWorldPoseMutex() const;
283 
286  public: bool GetEnablePhysicsEngine();
287 
290  public: void EnablePhysicsEngine(bool _enable);
291 
293  public: void UpdateStateSDF();
294 
297  public: bool IsLoaded() const;
298 
301  public: void ClearModels();
302 
307  public: void PublishModelPose(physics::ModelPtr _model);
308 
311  public: uint32_t GetIterations() const;
312 
315  public: msgs::Scene GetSceneMsg() const;
316 
321  public: void RunBlocking(unsigned int _iterations = 0);
322 
327  public: void RemoveModel(ModelPtr _model);
328 
333  public: void RemoveModel(const std::string &_name);
334 
341  public: void _AddDirty(Entity *_entity);
342 
350  private: ModelPtr GetModelById(unsigned int _id);
352 
356  private: void LoadPlugins();
357 
361  private: void LoadEntities(sdf::ElementPtr _sdf, BasePtr _parent);
362 
367  private: ModelPtr LoadModel(sdf::ElementPtr _sdf, BasePtr _parent);
368 
373  private: ActorPtr LoadActor(sdf::ElementPtr _sdf, BasePtr _parent);
374 
379  private: RoadPtr LoadRoad(sdf::ElementPtr _sdf, BasePtr _parent);
380 
382  private: void RunLoop();
383 
385  private: void Step();
386 
388  private: void LogStep();
389 
391  private: void Update();
392 
395  private: void OnPause(bool _p);
396 
398  private: void OnStep();
399 
402  private: void OnControl(ConstWorldControlPtr &_data);
403 
406  private: void OnPlaybackControl(ConstLogPlaybackControlPtr &_data);
407 
410  private: void OnRequest(ConstRequestPtr &_msg);
411 
416  private: void BuildSceneMsg(msgs::Scene &_scene, BasePtr _entity);
417 
420  private: void JointLog(ConstJointPtr &_msg);
421 
424  private: void OnFactoryMsg(ConstFactoryPtr &_data);
425 
428  private: void OnModelMsg(ConstModelPtr &_msg);
429 
431  private: void ModelUpdateTBB();
432 
434  private: void ModelUpdateSingleLoop();
435 
438  private: void LoadPlugin(sdf::ElementPtr _sdf);
439 
443  private: void FillModelMsg(msgs::Model &_msg, ModelPtr _model);
444 
447  private: void ProcessEntityMsgs();
448 
451  private: void ProcessRequestMsgs();
452 
455  private: void ProcessFactoryMsgs();
456 
459  private: void ProcessModelMsgs();
460 
462  private: bool OnLog(std::ostringstream &_stream);
463 
465  private: void ProcessMessages();
466 
468  private: void PublishWorldStats();
469 
471  private: void LogWorker();
472 
475  private: void OnLightMsg(ConstLightPtr &_msg);
476 
479  private: WorldPrivate *dataPtr;
480 
482  private: friend class DARTLink;
483 
485  private: friend class SimbodyPhysics;
486  };
488  }
489 }
490 #endif
boost::shared_ptr< Base > BasePtr
Definition: PhysicsTypes.hh:68
boost::shared_ptr< Model > ModelPtr
Definition: PhysicsTypes.hh:84
#define GZ_PHYSICS_VISIBLE
Definition: system.hh:318
The Vector3 class represents the generic vector containing 3 elements.
Definition: Vector3.hh:39
Forward declarations for transport.
default namespace for gazebo
Private data class for World.
Definition: WorldPrivate.hh:43
boost::shared_ptr< PhysicsEngine > PhysicsEnginePtr
Definition: PhysicsTypes.hh:108
The world provides access to all other object within a simulated environment.
Definition: World.hh:68
boost::shared_ptr< Entity > EntityPtr
Definition: PhysicsTypes.hh:76
Simbody physics engine.
Definition: SimbodyPhysics.hh:42
Base class for all physics objects in Gazebo.
Definition: Entity.hh:56
EntityType
Unique identifiers for all entity types.
Definition: Base.hh:84
boost::shared_ptr< Actor > ActorPtr
Definition: PhysicsTypes.hh:88
GAZEBO_VISIBLE void Init(google::protobuf::Message &_message, const std::string &_id="")
Initialize a message.
std::vector< ModelPtr > Model_V
Definition: PhysicsTypes.hh:180
Store state information of a physics::World object.
Definition: WorldState.hh:46
boost::shared_ptr< SphericalCoordinates > SphericalCoordinatesPtr
Definition: CommonTypes.hh:138
Base type.
Definition: Base.hh:86
A Time class, can be used to hold wall- or sim-time.
Definition: Time.hh:39
boost::shared_ptr< Road > RoadPtr
Definition: PhysicsTypes.hh:136
boost::shared_ptr< PresetManager > PresetManagerPtr
Definition: PhysicsTypes.hh:112