All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Modules Pages
World.hh
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2012-2014 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 _WORLD_HH_
18 #define _WORLD_HH_
19 
20 #include <vector>
21 #include <list>
22 #include <set>
23 #include <deque>
24 #include <string>
25 #include <boost/thread.hpp>
26 #include <boost/enable_shared_from_this.hpp>
27 #include <boost/shared_ptr.hpp>
28 
29 #include <sdf/sdf.hh>
30 
32 
33 #include "gazebo/msgs/msgs.hh"
34 
37 #include "gazebo/common/Event.hh"
38 
39 #include "gazebo/physics/Base.hh"
42 #include "gazebo/util/system.hh"
43 
44 namespace gazebo
45 {
46  namespace physics
47  {
50 
59  class GAZEBO_VISIBLE World : public boost::enable_shared_from_this<World>
60  {
64  public: explicit World(const std::string &_name = "");
65 
67  public: ~World();
68 
72  public: void Load(sdf::ElementPtr _sdf);
73 
77  public: void Save(const std::string &_filename);
78 
81  public: void Init();
82 
87  public: void Run(unsigned int _iterations = 0);
88 
91  public: bool GetRunning() const;
92 
95  public: void Stop();
96 
99  public: void Fini();
100 
104  public: void Clear();
105 
108  public: std::string GetName() const;
109 
113  public: PhysicsEnginePtr GetPhysicsEngine() const;
114 
117  public: common::SphericalCoordinatesPtr GetSphericalCoordinates() const;
118 
121  public: unsigned int GetModelCount() const;
122 
128  public: ModelPtr GetModel(unsigned int _index) const;
129 
132  public: Model_V GetModels() const;
133 
138  public: void ResetEntities(Base::EntityType _type = Base::BASE);
139 
141  public: void ResetTime();
142 
144  public: void Reset();
145 
149  public: EntityPtr GetSelectedEntity() const;
150 
153  public: void PrintEntityTree();
154 
158  public: common::Time GetSimTime() const;
159 
162  public: void SetSimTime(const common::Time &_t);
163 
166  public: common::Time GetPauseTime() const;
167 
170  public: common::Time GetStartTime() const;
171 
174  public: common::Time GetRealTime() const;
175 
178  public: bool IsPaused() const;
179 
182  public: void SetPaused(bool _p);
183 
189  public: BasePtr GetByName(const std::string &_name);
190 
196  public: ModelPtr GetModel(const std::string &_name);
197 
203  public: EntityPtr GetEntity(const std::string &_name);
204 
212  public: ModelPtr GetModelBelowPoint(const math::Vector3 &_pt);
213 
219  public: EntityPtr GetEntityBelowPoint(const math::Vector3 &_pt);
220 
223  public: void SetState(const WorldState &_state);
224 
228  public: void InsertModelFile(const std::string &_sdfFilename);
229 
233  public: void InsertModelString(const std::string &_sdfString);
234 
238  public: void InsertModelSDF(const sdf::SDF &_sdf);
239 
243  public: std::string StripWorldName(const std::string &_name) const;
244 
248  public: void EnableAllModels();
249 
253  public: void DisableAllModels();
254 
258  public: void StepWorld(int _steps) GAZEBO_DEPRECATED(3.0);
259 
262  public: void Step(unsigned int _steps);
263 
268  public: void LoadPlugin(const std::string &_filename,
269  const std::string &_name,
270  sdf::ElementPtr _sdf);
271 
274  public: void RemovePlugin(const std::string &_name);
275 
278  public: boost::mutex *GetSetWorldPoseMutex() const
279  {return this->setWorldPoseMutex;}
280 
283  public: bool GetEnablePhysicsEngine()
284  {return this->enablePhysicsEngine;}
285 
288  public: void EnablePhysicsEngine(bool _enable)
289  {this->enablePhysicsEngine = _enable;}
290 
292  public: void UpdateStateSDF();
293 
296  public: bool IsLoaded() const;
297 
300  public: void ClearModels();
301 
306  public: void PublishModelPose(physics::ModelPtr _model);
307 
310  public: uint32_t GetIterations() const;
311 
314  public: msgs::Scene GetSceneMsg() const;
315 
320  public: void RunBlocking(unsigned int _iterations = 0);
321 
326  public: void RemoveModel(ModelPtr _model);
327 
332  public: void RemoveModel(const std::string &_name);
333 
341  private: ModelPtr GetModelById(unsigned int _id);
343 
347  private: void LoadPlugins();
348 
352  private: void LoadEntities(sdf::ElementPtr _sdf, BasePtr _parent);
353 
358  private: ModelPtr LoadModel(sdf::ElementPtr _sdf, BasePtr _parent);
359 
364  private: ActorPtr LoadActor(sdf::ElementPtr _sdf, BasePtr _parent);
365 
370  private: RoadPtr LoadRoad(sdf::ElementPtr _sdf, BasePtr _parent);
371 
373  private: void RunLoop();
374 
376  private: void Step();
377 
379  private: void LogStep();
380 
382  private: void Update();
383 
386  private: void OnPause(bool _p);
387 
389  private: void OnStep();
390 
393  private: void OnControl(ConstWorldControlPtr &_data);
394 
397  private: void OnRequest(ConstRequestPtr &_msg);
398 
401  private: void SetSelectedEntityCB(const std::string &_name);
402 
407  private: void BuildSceneMsg(msgs::Scene &_scene, BasePtr _entity);
408 
411  private: void JointLog(ConstJointPtr &_msg);
412 
415  private: void OnFactoryMsg(ConstFactoryPtr &_data);
416 
419  private: void OnModelMsg(ConstModelPtr &_msg);
420 
422  private: void ModelUpdateTBB();
423 
425  private: void ModelUpdateSingleLoop();
426 
429  private: void LoadPlugin(sdf::ElementPtr _sdf);
430 
434  private: void FillModelMsg(msgs::Model &_msg, ModelPtr _model);
435 
438  private: void ProcessEntityMsgs();
439 
442  private: void ProcessRequestMsgs();
443 
446  private: void ProcessFactoryMsgs();
447 
450  private: void ProcessModelMsgs();
451 
453  private: bool OnLog(std::ostringstream &_stream);
454 
456  private: void ProcessMessages();
457 
459  private: void PublishWorldStats();
460 
462  private: void LogWorker();
463 
466  private: void OnLightMsg(ConstLightPtr &_msg);
467 
469  private: common::Time prevStepWallTime;
470 
472  private: PhysicsEnginePtr physicsEngine;
473 
475  private: common::SphericalCoordinatesPtr sphericalCoordinates;
476 
478  private: BasePtr rootElement;
479 
481  private: boost::thread *thread;
482 
484  private: bool stop;
485 
487  private: EntityPtr selectedEntity;
488 
490  private: std::string name;
491 
493  private: common::Time simTime;
494 
496  private: common::Time pauseTime;
497 
499  private: common::Time startTime;
500 
502  private: bool pause;
503 
505  private: int stepInc;
506 
508  private: event::Connection_V connections;
509 
511  private: transport::NodePtr node;
512 
514  private: transport::PublisherPtr selectionPub;
515 
517  private: transport::PublisherPtr statPub;
518 
520  private: transport::PublisherPtr diagPub;
521 
523  private: transport::PublisherPtr responsePub;
524 
526  private: transport::PublisherPtr modelPub;
527 
529  private: transport::PublisherPtr guiPub;
530 
532  private: transport::PublisherPtr lightPub;
533 
535  private: transport::PublisherPtr posePub;
536 
538  private: transport::PublisherPtr poseLocalPub;
539 
541  private: transport::SubscriberPtr controlSub;
542 
544  private: transport::SubscriberPtr factorySub;
545 
547  private: transport::SubscriberPtr jointSub;
548 
550  private: transport::SubscriberPtr lightSub;
551 
553  private: transport::SubscriberPtr modelSub;
554 
556  private: transport::SubscriberPtr requestSub;
557 
559  private: msgs::WorldStatistics worldStatsMsg;
560 
562  private: msgs::Scene sceneMsg;
563 
565  private: void (World::*modelUpdateFunc)();
566 
568  private: common::Time prevStatTime;
569 
571  private: common::Time pauseStartTime;
572 
574  private: common::Time realTimeOffset;
575 
577  private: boost::recursive_mutex *receiveMutex;
578 
580  private: boost::mutex *loadModelMutex;
581 
586  private: boost::mutex *setWorldPoseMutex;
587 
594  private: boost::recursive_mutex *worldUpdateMutex;
595 
597  private: sdf::ElementPtr sdf;
598 
600  private: std::vector<WorldPluginPtr> plugins;
601 
603  private: std::list<std::string> deleteEntity;
604 
608  public: std::list<Entity*> dirtyPoses;
609 
611  private: std::list<msgs::Request> requestMsgs;
612 
614  private: std::list<msgs::Factory> factoryMsgs;
615 
617  private: std::list<msgs::Model> modelMsgs;
618 
620  private: bool needsReset;
621 
623  private: bool resetAll;
624 
626  private: bool resetTimeOnly;
627 
629  private: bool resetModelOnly;
630 
632  private: bool initialized;
633 
635  private: bool loaded;
636 
638  private: bool enablePhysicsEngine;
639 
641  private: RayShapePtr testRay;
642 
644  private: bool pluginsLoaded;
645 
647  private: common::Time sleepOffset;
648 
650  private: common::Time prevProcessMsgsTime;
651 
653  private: common::Time processMsgsPeriod;
654 
656  private: std::deque<WorldState> states[2];
657 
659  private: int currentStateBuffer;
660 
661  private: WorldState prevStates[2];
662  private: int stateToggle;
663 
664  private: sdf::ElementPtr logPlayStateSDF;
665  private: WorldState logPlayState;
666 
669  private: sdf::SDFPtr factorySDF;
670 
672  private: std::set<ModelPtr> publishModelPoses;
673 
675  private: common::UpdateInfo updateInfo;
676 
678  private: uint64_t iterations;
679 
681  private: uint64_t stopIterations;
682 
684  private: boost::condition_variable logCondition;
685 
688  private: boost::condition_variable logContinueCondition;
689 
691  private: uint64_t logPrevIteration;
692 
694  private: common::Time logRealTime;
695 
697  private: boost::mutex logMutex;
698 
700  private: boost::mutex logBufferMutex;
701 
703  private: boost::mutex entityDeleteMutex;
704 
706  private: boost::thread *logThread;
707 
709  private: Model_V models;
710  };
712  }
713 }
714 #endif
boost::shared_ptr< Base > BasePtr
Definition: PhysicsTypes.hh:65
boost::shared_ptr< Model > ModelPtr
Definition: PhysicsTypes.hh:81
Base type.
Definition: Base.hh:79
Forward declarations for the common classes.
Definition: Animation.hh:24
The Vector3 class represents the generic vector containing 3 elements.
Definition: Vector3.hh:43
void EnablePhysicsEngine(bool _enable)
enable/disable physics engine during World::Update.
Definition: World.hh:288
boost::shared_ptr< Subscriber > SubscriberPtr
Definition: TransportTypes.hh:48
Forward declarations for transport.
#define GAZEBO_DEPRECATED(version)
Definition: CommonTypes.hh:44
Information for use in an update event.
Definition: UpdateInfo.hh:30
default namespace for gazebo
bool GetEnablePhysicsEngine()
check if physics engine is enabled/disabled.
Definition: World.hh:283
boost::shared_ptr< PhysicsEngine > PhysicsEnginePtr
Definition: PhysicsTypes.hh:105
The world provides access to all other object within a simulated environment.
Definition: World.hh:59
boost::shared_ptr< Entity > EntityPtr
Definition: PhysicsTypes.hh:73
std::vector< ConnectionPtr > Connection_V
Definition: CommonTypes.hh:148
boost::shared_ptr< Node > NodePtr
Definition: TransportTypes.hh:52
boost::shared_ptr< Actor > ActorPtr
Definition: PhysicsTypes.hh:85
GAZEBO_VISIBLE void Init(google::protobuf::Message &_message, const std::string &_id="")
Initialize a message.
std::vector< ModelPtr > Model_V
Definition: PhysicsTypes.hh:161
boost::shared_ptr< Publisher > PublisherPtr
Definition: TransportTypes.hh:44
Store state information of a physics::World object.
Definition: WorldState.hh:46
EntityType
Unique identifiers for all entity types.
Definition: Base.hh:77
boost::mutex * GetSetWorldPoseMutex() const
Get the set world pose mutex.
Definition: World.hh:278
boost::shared_ptr< SphericalCoordinates > SphericalCoordinatesPtr
Definition: CommonTypes.hh:135
#define GAZEBO_VISIBLE
Use to represent "symbol visible" if supported.
Definition: system.hh:48
std::list< Entity * > dirtyPoses
when physics engine makes an update and changes a link pose, this flag is set to trigger Entity::SetW...
Definition: World.hh:608
A Time class, can be used to hold wall- or sim-time.
Definition: Time.hh:43
boost::shared_ptr< RayShape > RayShapePtr
Definition: PhysicsTypes.hh:113
GAZEBO_VISIBLE void stop() GAZEBO_DEPRECATED(2.3)
Deprecated.
boost::shared_ptr< Road > RoadPtr
Definition: PhysicsTypes.hh:129