All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
Scene.hh
Go to the documentation of this file.
1 /*
2  * Copyright 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 _SCENE_HH_
18 #define _SCENE_HH_
19 
20 #include <vector>
21 #include <map>
22 #include <string>
23 #include <list>
24 #include <boost/enable_shared_from_this.hpp>
25 #include <boost/shared_ptr.hpp>
26 #include <boost/unordered/unordered_map.hpp>
27 
28 #include <sdf/sdf.hh>
29 
30 #include "gazebo/msgs/msgs.hh"
31 
33 
35 #include "gazebo/common/Events.hh"
36 #include "gazebo/common/Color.hh"
37 #include "gazebo/math/Vector2i.hh"
38 
39 namespace SkyX
40 {
41  class SkyX;
42  class BasicController;
43 }
44 
45 namespace Ogre
46 {
47  class SceneManager;
48  class RaySceneQuery;
49  class Node;
50  class Entity;
51  class Mesh;
52  class Vector3;
53  class Quaternion;
54 }
55 
56 namespace boost
57 {
58  class mutex;
59 }
60 
61 namespace gazebo
62 {
63  namespace rendering
64  {
65  class Projector;
66  class Light;
67  class Visual;
68  class Grid;
69  class Heightmap;
70 
73 
78  class Scene : public boost::enable_shared_from_this<Scene>
79  {
80  public: enum SkyXMode {
81  GZ_SKYX_ALL = 0x0FFFFFFF,
82  GZ_SKYX_CLOUDS = 0x0000001,
83  GZ_SKYX_MOON = 0x0000002,
85  };
86 
88  private: Scene() {}
89 
95  public: Scene(const std::string &_name,
96  bool _enableVisualizations = false);
97 
99  public: virtual ~Scene();
100 
103  public: void Load(sdf::ElementPtr _scene);
104 
106  public: void Load();
107 
109  public: void Init();
110 
112  public: void PreRender();
113 
116  public: Ogre::SceneManager *GetManager() const;
117 
120  public: std::string GetName() const;
121 
124  public: void SetAmbientColor(const common::Color &_color);
125 
128  public: common::Color GetAmbientColor() const;
129 
132  public: void SetBackgroundColor(const common::Color &_color);
133 
136  public: common::Color GetBackgroundColor() const;
137 
143  public: void CreateGrid(uint32_t _cellCount, float _cellLength,
144  float _lineWidth, const common::Color &_color);
145 
149  public: Grid *GetGrid(uint32_t _index) const;
150 
153  public: uint32_t GetGridCount() const;
154 
160  public: CameraPtr CreateCamera(const std::string &_name,
161  bool _autoRender = true);
162 
168  public: DepthCameraPtr CreateDepthCamera(const std::string &_name,
169  bool _autoRender = true);
170 
176  public: GpuLaserPtr CreateGpuLaser(const std::string &_name,
177  bool _autoRender = true);
178 
181  public: uint32_t GetCameraCount() const;
182 
187  public: CameraPtr GetCamera(uint32_t _index) const;
188 
192  public: CameraPtr GetCamera(const std::string &_name) const;
193 
199  public: UserCameraPtr CreateUserCamera(const std::string &_name);
200 
203  public: uint32_t GetUserCameraCount() const;
204 
210  public: UserCameraPtr GetUserCamera(uint32_t _index) const;
211 
214  public: void RemoveCamera(const std::string &_name);
215 
219  public: LightPtr GetLight(const std::string &_name) const;
220 
223  public: uint32_t GetLightCount() const;
224 
229  public: LightPtr GetLight(uint32_t _index) const;
230 
232  public: VisualPtr GetVisual(const std::string &_name) const;
233 
237  public: void SelectVisual(const std::string &_name,
238  const std::string &_mode);
239 
246  public: VisualPtr GetVisualAt(CameraPtr _camera,
247  const math::Vector2i &_mousePos,
248  std::string &_mod);
249 
252  public: void SnapVisualToNearestBelow(const std::string &_visualName);
253 
259  public: VisualPtr GetVisualAt(CameraPtr _camera,
260  const math::Vector2i &_mousePos);
261 
267  public: VisualPtr GetModelVisualAt(CameraPtr _camera,
268  const math::Vector2i &_mousePos);
269 
270 
274  public: VisualPtr GetVisualBelow(const std::string &_visualName);
275 
280  public: void GetVisualsBelowPoint(const math::Vector3 &_pt,
281  std::vector<VisualPtr> &_visuals);
282 
283 
288  public: double GetHeightBelowPoint(const math::Vector3 &_pt);
289 
295  public: bool GetFirstContact(CameraPtr _camera,
296  const math::Vector2i &_mousePos,
297  math::Vector3 &_position);
298 
300  public: void PrintSceneGraph();
301 
306  public: void SetVisible(const std::string &_name, bool _visible);
307 
312  public: void DrawLine(const math::Vector3 &_start,
313  const math::Vector3 &_end,
314  const std::string &_name);
315 
323  public: void SetFog(const std::string &_type,
324  const common::Color &_color,
325  double _density, double _start, double _end);
326 
329  public: uint32_t GetId() const;
330 
333  public: std::string GetIdString() const;
334 
337  public: void SetShadowsEnabled(bool _value);
338 
341  public: bool GetShadowsEnabled() const;
342 
345  public: void AddVisual(VisualPtr _vis);
346 
349  public: void RemoveVisual(VisualPtr _vis);
350 
353  public: void SetGrid(bool _enabled);
354 
357  public: VisualPtr GetWorldVisual() const;
358 
362  public: std::string StripSceneName(const std::string &_name) const;
363 
366  public: Heightmap *GetHeightmap() const;
367 
369  public: void Clear();
370 
375  public: VisualPtr CloneVisual(const std::string &_visualName,
376  const std::string &_newName);
377 
381  public: VisualPtr GetSelectedVisual() const;
382 
385  public: void SetWireframe(bool _show);
386 
389  public: void SetTransparent(bool _show);
390 
393  public: void ShowCOMs(bool _show);
394 
397  public: void ShowJoints(bool _show);
398 
401  public: void ShowCollisions(bool _show);
402 
405  public: void ShowContacts(bool _show);
406 
409  public: void ShowClouds(bool _show);
410 
413  public: bool GetShowClouds() const;
414 
415 
420  public: void SetSkyXMode(unsigned int _mode);
421 
423  public: bool GetInitialized() const;
424 
430  public: common::Time GetSimTime() const;
431 
433  private: void SetSky();
434 
436  private: void InitDeferredShading();
437 
444  private: Ogre::Entity *GetOgreEntityAt(CameraPtr _camera,
445  const math::Vector2i &_mousePos,
446  bool _ignorSelectionObj);
447 
457  // Code found in Wiki: www.ogre3d.org/wiki/index.php/RetrieveVertexData
458  private: void GetMeshInformation(const Ogre::Mesh *_mesh,
459  size_t &_vertexCount,
460  Ogre::Vector3* &_vertices,
461  size_t &_indexCount,
462  uint64_t* &_indices,
463  const Ogre::Vector3 &_position,
464  const Ogre::Quaternion &_orient,
465  const Ogre::Vector3 &_scale);
466 
470  private: void PrintSceneGraphHelper(const std::string &_prefix,
471  Ogre::Node *_node);
472 
476  private: void OnScene(ConstScenePtr &_msg);
477 
480  private: void OnResponse(ConstResponsePtr &_msg);
481 
484  private: void OnRequest(ConstRequestPtr &_msg);
485 
488  private: void OnJointMsg(ConstJointPtr &_msg);
489 
492  private: bool ProcessSensorMsg(ConstSensorPtr &_msg);
493 
496  private: bool ProcessJointMsg(ConstJointPtr &_msg);
497 
500  private: bool ProcessLinkMsg(ConstLinkPtr &_msg);
501 
504  private: bool ProcessSceneMsg(ConstScenePtr &_msg);
505 
508  private: bool ProcessModelMsg(const msgs::Model &_msg);
509 
512  private: void OnSensorMsg(ConstSensorPtr &_msg);
513 
516  private: void OnVisualMsg(ConstVisualPtr &_msg);
517 
520  private: bool ProcessVisualMsg(ConstVisualPtr &_msg);
521 
524  private: void OnLightMsg(ConstLightPtr &_msg);
525 
528  private: bool ProcessLightMsg(ConstLightPtr &_msg);
529 
532  private: void ProcessRequestMsg(ConstRequestPtr &_msg);
533 
536  private: void OnSelectionMsg(ConstSelectionPtr &_msg);
537 
540  private: void OnSkyMsg(ConstSkyPtr &_msg);
541 
544  private: void OnModelMsg(ConstModelPtr &_msg);
545 
548  private: void OnPoseMsg(ConstPosesStampedPtr &_msg);
549 
552  private: void OnSkeletonPoseMsg(ConstPoseAnimationPtr &_msg);
553 
557  private: void CreateCOMVisual(ConstLinkPtr &_msg, VisualPtr _linkVisual);
558 
562  private: void CreateCOMVisual(sdf::ElementPtr _elem,
563  VisualPtr _linkVisual);
564 
566  private: std::string name;
567 
569  private: sdf::ElementPtr sdf;
570 
572  private: std::vector<CameraPtr> cameras;
573 
575  private: std::vector<UserCameraPtr> userCameras;
576 
578  private: Ogre::SceneManager *manager;
579 
581  private: Ogre::RaySceneQuery *raySceneQuery;
582 
584  private: std::vector<Grid *> grids;
585 
587  private: static uint32_t idCounter;
588 
590  private: uint32_t id;
591 
593  private: std::string idString;
594 
597  typedef std::list<boost::shared_ptr<msgs::Visual const> > VisualMsgs_L;
598 
600  private: VisualMsgs_L visualMsgs;
601 
604  typedef std::list<boost::shared_ptr<msgs::Light const> > LightMsgs_L;
605 
607  private: LightMsgs_L lightMsgs;
608 
611  typedef std::list<msgs::Pose> PoseMsgs_L;
612 
614  private: PoseMsgs_L poseMsgs;
615 
618  typedef std::list<boost::shared_ptr<msgs::Scene const> > SceneMsgs_L;
619 
621  private: SceneMsgs_L sceneMsgs;
622 
625  typedef std::list<boost::shared_ptr<msgs::Joint const> > JointMsgs_L;
626 
628  private: JointMsgs_L jointMsgs;
629 
632  typedef std::list<boost::shared_ptr<msgs::Link const> > LinkMsgs_L;
633 
635  private: LinkMsgs_L linkMsgs;
636 
639  typedef std::list<boost::shared_ptr<msgs::Model const> > ModelMsgs_L;
641  private: ModelMsgs_L modelMsgs;
642 
645  typedef std::list<boost::shared_ptr<msgs::Sensor const> > SensorMsgs_L;
646 
648  private: SensorMsgs_L sensorMsgs;
649 
652  typedef std::list<boost::shared_ptr<msgs::Request const> > RequestMsgs_L;
654  private: RequestMsgs_L requestMsgs;
655 
658  typedef std::map<std::string, VisualPtr> Visual_M;
659 
661  private: Visual_M visuals;
662 
665  typedef std::map<std::string, LightPtr> Light_M;
666 
668  private: Light_M lights;
669 
672  typedef std::list<boost::shared_ptr<msgs::PoseAnimation const> >
673  SkeletonPoseMsgs_L;
675  private: SkeletonPoseMsgs_L skeletonPoseMsgs;
676 
678  private: boost::shared_ptr<msgs::Selection const> selectionMsg;
679 
681  private: boost::mutex *receiveMutex;
682 
684  private: transport::NodePtr node;
685 
687  private: transport::SubscriberPtr sensorSub;
688 
690  private: transport::SubscriberPtr sceneSub;
691 
693  private: transport::SubscriberPtr requestSub;
694 
696  private: transport::SubscriberPtr visSub;
697 
699  private: transport::SubscriberPtr lightSub;
700 
702  private: transport::SubscriberPtr poseSub;
703 
705  private: transport::SubscriberPtr jointSub;
706 
708  private: transport::SubscriberPtr selectionSub;
709 
711  private: transport::SubscriberPtr responseSub;
712 
714  private: transport::SubscriberPtr skeletonPoseSub;
715 
717  private: transport::SubscriberPtr skySub;
718 
720  private: transport::SubscriberPtr modelInfoSub;
721 
723  private: transport::PublisherPtr lightPub;
724 
726  private: transport::PublisherPtr responsePub;
727 
729  private: transport::PublisherPtr requestPub;
730 
732  private: std::vector<event::ConnectionPtr> connections;
733 
735  private: VisualPtr worldVisual;
736 
738  private: VisualPtr selectedVis;
739 
745  private: std::string selectionMode;
746 
748  private: msgs::Request *requestMsg;
749 
751  private: bool enableVisualizations;
752 
754  private: Heightmap *terrain;
755 
757  private: std::map<std::string, Projector *> projectors;
758 
760  public: SkyX::SkyX *skyx;
761 
763  private: SkyX::BasicController *skyxController;
764 
766  private: bool showCOMs;
767 
769  private: bool showCollisions;
770 
772  private: bool showJoints;
773 
775  private: bool transparent;
776 
778  private: bool wireframe;
779 
781  private: bool initialized;
782 
785  private: common::Time sceneSimTimePosesReceived;
786 
789  private: common::Time sceneSimTimePosesApplied;
790 
793  typedef boost::unordered_map<std::string,
794  boost::shared_ptr<msgs::Joint const> > JointMsgs_M;
795 
797  private: JointMsgs_M joints;
798  };
800  }
801 }
802 #endif