Class
List
Modules
Common
Events
Messages
Physics
Rendering
Sensors
Transport
Links
Gazebo Website
Tutorials
Download
Report Documentation Issues
ServerFixture.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_SERVER_FIXTURE_HH_
18 #define _GAZEBO_SERVER_FIXTURE_HH_
19 
20 #pragma GCC diagnostic ignored "-Wswitch-default"
21 #pragma GCC diagnostic ignored "-Wfloat-equal"
22 #pragma GCC diagnostic ignored "-Wshadow"
23 
24 // The following is needed to enable the GetMemInfo function for OSX
25 #ifdef __MACH__
26 # include <mach/mach.h>
27 #endif // __MACH__
28 
29 #include <sdf/sdf.hh>
30 
31 #include <gtest/gtest.h>
32 #include <boost/thread.hpp>
33 #include <mutex>
34 #include <boost/filesystem.hpp>
35 
36 #include <map>
37 #include <string>
38 #include <vector>
39 
40 #include <ignition/math/Pose3.hh>
41 #include <ignition/math/SignalStats.hh>
42 #include <ignition/math/Vector3Stats.hh>
43 
44 #include "gazebo/transport/transport.hh"
45 
46 #include "gazebo/common/CommonIface.hh"
47 #include "gazebo/common/SystemPaths.hh"
48 #include "gazebo/common/Console.hh"
49 #include "gazebo/physics/World.hh"
50 #include "gazebo/physics/PhysicsTypes.hh"
51 #include "gazebo/physics/PhysicsIface.hh"
52 #include "gazebo/sensors/sensors.hh"
53 #include "gazebo/rendering/rendering.hh"
54 #include "gazebo/msgs/msgs.hh"
55 
56 #include "gazebo/gazebo_config.h"
57 #include "gazebo/Server.hh"
58 #include "gazebo/util/system.hh"
59 
60 #include "test_config.h"
61 
62 namespace gazebo
63 {
64  std::string custom_exec(std::string _cmd);
65 
66  class GAZEBO_VISIBLE ServerFixture : public testing::Test
67  {
69  protected: ServerFixture();
70 
72  protected: virtual ~ServerFixture();
73 
75  protected: virtual void TearDown();
76 
78  protected: virtual void Unload();
79 
82  protected: virtual void Load(const std::string &_worldFilename);
83 
88  protected: virtual void Load(const std::string &_worldFilename,
89  bool _paused);
90 
98  protected: virtual void Load(const std::string &_worldFilename,
99  bool _paused, const std::string &_physics,
100  const std::vector<std::string> &_systemPlugins = {});
101 
106  protected: virtual void LoadArgs(const std::string &_args);
107 
112  protected: void RunServer(const std::vector<std::string> &_args);
113 
116  protected: rendering::ScenePtr GetScene(
117  const std::string &_sceneName = "default");
118 
121  protected: void OnStats(ConstWorldStatisticsPtr &_msg);
122 
124  protected: void SetPause(bool _pause);
125 
128  protected: double GetPercentRealTime() const;
129 
133  protected: void OnPose(ConstPosesStampedPtr &_msg);
134 
138  protected: ignition::math::Pose3d EntityPose(
139  const std::string &_name);
140 
144  protected: bool HasEntity(const std::string &_name);
145 
152  protected: void PrintImage(const std::string &_name, unsigned char **_image,
153  unsigned int _width, unsigned int _height,
154  unsigned int _depth);
155 
160  protected: void PrintScan(const std::string &_name, double *_scan,
161  unsigned int _sampleCount);
162 
171  protected: void FloatCompare(float *_scanA, float *_scanB,
172  unsigned int _sampleCount, float &_diffMax,
173  float &_diffSum, float &_diffAvg);
174 
183  protected: void DoubleCompare(double *_scanA, double *_scanB,
184  unsigned int _sampleCount, double &_diffMax,
185  double &_diffSum, double &_diffAvg);
186 
196  protected: void ImageCompare(unsigned char *_imageA,
197  unsigned char *_imageB,
198  unsigned int _width, unsigned int _height,
199  unsigned int _depth,
200  unsigned int &_diffMax, unsigned int &_diffSum,
201  double &_diffAvg);
202 
209  private: void OnNewFrame(const unsigned char *_image,
210  unsigned int _width, unsigned int _height,
211  unsigned int _depth,
212  const std::string &/*_format*/);
213 
219  protected: void GetFrame(const std::string &_cameraName,
220  unsigned char **_imgData, unsigned int &_width,
221  unsigned int &_height);
222 
226  protected: physics::ModelPtr SpawnModel(const msgs::Model &_msg);
227 
232  protected: template<typename T>
233  static void CheckPointer(boost::shared_ptr<T> _ptr)
234  {
235  ASSERT_TRUE(_ptr != NULL);
236  }
237 
257  protected: void SpawnCamera(const std::string &_modelName,
258  const std::string &_cameraName,
259  const ignition::math::Vector3d &_pos =
260  ignition::math::Vector3d::Zero,
261  const ignition::math::Vector3d &_rpy =
262  ignition::math::Vector3d::Zero,
263  unsigned int _width = 320, unsigned int _height = 240,
264  double _rate = 25,
265  const std::string &_noiseType = "",
266  double _noiseMean = 0.0, double _noiseStdDev = 0.0,
267  bool _distortion = false, double _distortionK1 = 0.0,
268  double _distortionK2 = 0.0, double _distortionK3 = 0.0,
269  double _distortionP1 = 0.0, double _distortionP2 = 0.0,
270  double _cx = 0.5, double _cy = 0.5);
271 
295  protected: void SpawnWideAngleCamera(const std::string &_modelName,
296  const std::string &_cameraName,
297  const ignition::math::Vector3d &_pos =
298  ignition::math::Vector3d::Zero,
299  const ignition::math::Vector3d &_rpy =
300  ignition::math::Vector3d::Zero,
301  unsigned int _width = 320,
302  unsigned int _height = 240,
303  double _rate = 25,
304  const double _hfov = 60,
305  const std::string &_lensType = "stereographic",
306  const bool _scaleToHfov = true,
307  const double _cutoffAngle = 3.1415,
308  const double _envTextureSize = 512,
309  const double _c1 = 1.05, const double _c2 = 4,
310  const double _f = 1.0,
311  const std::string &_fun = "tan");
312 
328  protected: void SpawnRaySensor(const std::string &_modelName,
329  const std::string &_raySensorName,
330  const ignition::math::Vector3d &_pos =
331  ignition::math::Vector3d::Zero,
332  const ignition::math::Vector3d &_rpy =
333  ignition::math::Vector3d::Zero,
334  double _hMinAngle = -2.0, double _hMaxAngle = 2.0,
335  double _vMinAngle = -1.0, double _vMaxAngle = 1.0,
336  double _minRange = 0.08, double _maxRange = 10,
337  double _rangeResolution = 0.01, unsigned int _samples = 640,
338  unsigned int _vSamples = 1, double _hResolution = 1.0,
339  double _vResolution = 1.0,
340  const std::string &_noiseType = "", double _noiseMean = 0.0,
341  double _noiseStdDev = 0.0);
342 
350  protected: sensors::SonarSensorPtr SpawnSonar(const std::string &_modelName,
351  const std::string &_sonarName,
352  const ignition::math::Pose3d &_pose,
353  const double _minRange,
354  const double _maxRange,
355  const double _radius);
356 
371  protected: void SpawnGpuRaySensor(const std::string &_modelName,
372  const std::string &_raySensorName,
373  const ignition::math::Vector3d &_pos =
374  ignition::math::Vector3d::Zero,
375  const ignition::math::Vector3d &_rpy =
376  ignition::math::Vector3d::Zero,
377  double _hMinAngle = -2.0, double _hMaxAngle = 2.0,
378  double _minRange = 0.08, double _maxRange = 10,
379  double _rangeResolution = 0.01, unsigned int _samples = 640,
380  const std::string &_noiseType = "", double _noiseMean = 0.0,
381  double _noiseStdDev = 0.0);
382 
402  protected: void SpawnGpuRaySensorVertical(const std::string &_modelName,
403  const std::string &_raySensorName,
404  const ignition::math::Vector3d &_pos,
405  const ignition::math::Vector3d &_rpy,
406  double _hMinAngle = -2.0, double _hMaxAngle = 2.0,
407  double _vMinAngle = -1.0, double _vMaxAngle = 1.0,
408  double _minRange = 0.08, double _maxRange = 10,
409  double _rangeResolution = 0.01, unsigned int _samples = 640,
410  unsigned int _vSamples = 1, double _hResolution = 1.0,
411  double _vResolution = 1.0,
412  const std::string &_noiseType = "", double _noiseMean = 0.0,
413  double _noiseStdDev = 0.0);
414 
425  protected: void SpawnDepthCameraSensor(const std::string &_modelName,
426  const std::string &_cameraName,
427  const ignition::math::Vector3d &_pos =
428  ignition::math::Vector3d::Zero,
429  const ignition::math::Vector3d &_rpy =
430  ignition::math::Vector3d::Zero,
431  const unsigned int _width = 320,
432  const unsigned int _height = 240,
433  const double _rate = 25, const double _near = 0.1,
434  const double _far = 10);
435 
449  protected: void SpawnImuSensor(const std::string &_modelName,
450  const std::string &_imuSensorName,
451  const ignition::math::Vector3d &_pos =
452  ignition::math::Vector3d::Zero,
453  const ignition::math::Vector3d &_rpy =
454  ignition::math::Vector3d::Zero,
455  const std::string &_noiseType = "",
456  double _rateNoiseMean = 0.0, double _rateNoiseStdDev = 0.0,
457  double _rateBiasMean = 0.0, double _rateBiasStdDev = 0.0,
458  double _accelNoiseMean = 0.0, double _accelNoiseStdDev = 0.0,
459  double _accelBiasMean = 0.0, double _accelBiasStdDev = 0.0);
460 
468  protected: void SpawnUnitContactSensor(const std::string &_name,
469  const std::string &_sensorName,
470  const std::string &_collisionType,
471  const ignition::math::Vector3d &_pos =
472  ignition::math::Vector3d::Zero,
473  const ignition::math::Vector3d &_rpy =
474  ignition::math::Vector3d::Zero,
475  bool _static = false);
476 
485  protected: void SpawnUnitImuSensor(const std::string &_name,
486  const std::string &_sensorName,
487  const std::string &_collisionType,
488  const std::string &_topic,
489  const ignition::math::Vector3d &_pos =
490  ignition::math::Vector3d::Zero,
491  const ignition::math::Vector3d &_rpy =
492  ignition::math::Vector3d::Zero,
493  bool _static = false);
494 
503  protected: void SpawnUnitAltimeterSensor(const std::string &_name,
504  const std::string &_sensorName,
505  const std::string &_collisionType,
506  const std::string &_topic,
507  const ignition::math::Vector3d &_pos =
508  ignition::math::Vector3d::Zero,
509  const ignition::math::Vector3d &_rpy =
510  ignition::math::Vector3d::Zero,
511  bool _static = false);
512 
521  protected: void SpawnUnitMagnetometerSensor(const std::string &_name,
522  const std::string &_sensorName,
523  const std::string &_collisionType,
524  const std::string &_topic,
525  const ignition::math::Vector3d &_pos =
526  ignition::math::Vector3d::Zero,
527  const ignition::math::Vector3d &_rpy =
528  ignition::math::Vector3d::Zero,
529  bool _static = false);
530 
535  private: void launchTimeoutFailure(const char *_logMsg,
536  const int _timeoutCS);
537 
548  protected: void SpawnWirelessTransmitterSensor(const std::string &_name,
549  const std::string &_sensorName,
550  const ignition::math::Vector3d &_pos,
551  const ignition::math::Vector3d &_rpy,
552  const std::string &_essid,
553  double _freq,
554  double _power,
555  double _gain,
556  bool _visualize = true);
557 
569  protected: void SpawnWirelessReceiverSensor(const std::string &_name,
570  const std::string &_sensorName,
571  const ignition::math::Vector3d &_pos,
572  const ignition::math::Vector3d &_rpy,
573  double _minFreq,
574  double _maxFreq,
575  double _power,
576  double _gain,
577  double _sensitivity,
578  bool _visualize = true);
579 
584  protected: void WaitUntilEntitySpawn(const std::string &_name,
585  unsigned int _sleepEach,
586  int _retries);
587 
592  protected: void WaitUntilSensorSpawn(const std::string &_name,
593  unsigned int _sleepEach,
594  int _retries);
595 
601  protected: void WaitUntilIteration(const uint32_t _goalIteration,
602  const int _sleepEach,
603  const int _retries) const;
604 
610  protected: void WaitUntilSimTime(const common::Time &_goalTime,
611  const int _ms,
612  const int _maxRetries) const;
613 
630  protected: void SpawnLight(const std::string &_name,
631  const std::string &_type,
632  const ignition::math::Vector3d &_pos =
633  ignition::math::Vector3d::Zero,
634  const ignition::math::Vector3d &_rpy =
635  ignition::math::Vector3d::Zero,
636  const ignition::math::Color &_diffuse =
637  ignition::math::Color::White,
638  const ignition::math::Color &_specular =
639  ignition::math::Color::White,
640  const ignition::math::Vector3d &_direction =
641  -ignition::math::Vector3d::UnitZ,
642  const double _attenuationRange = 20,
643  const double _attenuationConstant = 0.5,
644  const double _attenuationLinear = 0.01,
645  const double _attenuationQuadratic = 0.001,
646  const double _spotInnerAngle = 0,
647  const double _spotOuterAngle = 0,
648  const double _spotFallOff = 0,
649  const bool _castShadows = true);
650 
656  protected: void SpawnCylinder(const std::string &_name,
657  const ignition::math::Vector3d &_pos =
658  ignition::math::Vector3d::Zero,
659  const ignition::math::Vector3d &_rpy =
660  ignition::math::Vector3d::Zero,
661  bool _static = false);
662 
670  protected: void SpawnSphere(const std::string &_name,
671  const ignition::math::Vector3d &_pos,
672  const ignition::math::Vector3d &_rpy,
673  bool _wait = true, bool _static = false);
674 
684  protected: void SpawnSphere(const std::string &_name,
685  const ignition::math::Vector3d &_pos,
686  const ignition::math::Vector3d &_rpy,
687  const ignition::math::Vector3d &_cog,
688  double _radius = 1.0,
689  bool _wait = true, bool _static = false);
690 
697  protected: void SpawnBox(const std::string &_name,
698  const ignition::math::Vector3d &_size =
699  ignition::math::Vector3d::One,
700  const ignition::math::Vector3d &_pos =
701  ignition::math::Vector3d::Zero,
702  const ignition::math::Vector3d &_rpy =
703  ignition::math::Vector3d::Zero,
704  bool _static = false);
705 
713  protected: void SpawnTrimesh(const std::string &_name,
714  const std::string &_modelPath,
715  const ignition::math::Vector3d &_scale =
716  ignition::math::Vector3d::One,
717  const ignition::math::Vector3d &_pos =
718  ignition::math::Vector3d::Zero,
719  const ignition::math::Vector3d &_rpy =
720  ignition::math::Vector3d::Zero,
721  bool _static = false);
722 
728  protected: void SpawnEmptyLink(const std::string &_name,
729  const ignition::math::Vector3d &_pos =
730  ignition::math::Vector3d::Zero,
731  const ignition::math::Vector3d &_rpy =
732  ignition::math::Vector3d::Zero,
733  bool _static = false);
734 
737  protected: void SpawnModel(const std::string &_filename);
738 
741  protected: void SpawnSDF(const std::string &_sdf);
742 
746  protected: void LoadPlugin(const std::string &_filename,
747  const std::string &_name);
748 
752  protected: physics::ModelPtr GetModel(const std::string &_name);
753 
756  protected: void RemoveModel(const std::string &_name);
757 
760  protected: void RemovePlugin(const std::string &_name);
761 
765  protected: void GetMemInfo(double &_resident, double &_share);
766 
770  protected: std::string GetUniqueString(const std::string &_prefix);
771 
775  protected: void Record(const std::string &_name, const double _data);
776 
780  protected: void Record(const std::string &_prefix,
781  const ignition::math::SignalStats &_stats);
782 
786  protected: void Record(const std::string &_prefix,
787  const ignition::math::Vector3Stats &_stats);
788 
790  protected: Server *server;
791 
793  protected: boost::thread *serverThread;
794 
796  protected: transport::NodePtr node;
797 
799  protected: transport::SubscriberPtr poseSub;
800 
802  protected: transport::SubscriberPtr statsSub;
803 
805  protected: transport::PublisherPtr factoryPub;
806 
808  protected: transport::PublisherPtr requestPub;
809 
811  protected: std::map<std::string, ignition::math::Pose3d> poses;
812 
814  protected: std::mutex receiveMutex;
815 
817  private: unsigned char **imgData;
818 
820  private: int gotImage;
821 
823  protected: common::Time simTime, realTime, pauseTime;
824 
826  private: double percentRealTime;
827 
829  private: bool paused;
830 
832  private: bool serverRunning;
833 
835  private: int uniqueCounter;
836  };
837 
838  class GAZEBO_VISIBLE RenderingFixture : public ServerFixture
839  {
840  // Documentation inherited.
841  public: virtual void SetUp();
842 
843  // Documentation inherited.
844  protected: virtual void Unload();
845  };
846 } // namespace gazebo
847 #endif // define _GAZEBO_SERVER_FIXTURE_HH_
gazebo::ServerFixture::statsSub
transport::SubscriberPtr statsSub
World statistics subscription.
Definition: ServerFixture.hh:802
gazebo::ServerFixture::poseSub
transport::SubscriberPtr poseSub
Pose subscription.
Definition: ServerFixture.hh:799
gazebo
Forward declarations for the common classes.
Definition: Animation.hh:26
gazebo::ServerFixture
Definition: ServerFixture.hh:66
gazebo::custom_exec
std::string custom_exec(std::string _cmd)
gazebo::ServerFixture::factoryPub
transport::PublisherPtr factoryPub
Factory publisher.
Definition: ServerFixture.hh:805
gazebo::transport::PublisherPtr
boost::shared_ptr< Publisher > PublisherPtr
Definition: TransportTypes.hh:49
gazebo::transport::SubscriberPtr
boost::shared_ptr< Subscriber > SubscriberPtr
Definition: TransportTypes.hh:53
gazebo::ServerFixture::serverThread
boost::thread * serverThread
Pointer the thread the runs the server.
Definition: ServerFixture.hh:793
gazebo::transport::NodePtr
boost::shared_ptr< Node > NodePtr
Definition: TransportTypes.hh:57
PhysicsTypes.hh
default namespace for gazebo
gazebo::ServerFixture::node
transport::NodePtr node
Pointer to a node for communication.
Definition: ServerFixture.hh:796
gazebo::RenderingFixture
Definition: ServerFixture.hh:838
gazebo::rendering::ScenePtr
boost::shared_ptr< Scene > ScenePtr
Definition: RenderTypes.hh:82
gazebo::ServerFixture::simTime
common::Time simTime
Current simulation time, real time, and pause time.
Definition: ServerFixture.hh:823
gazebo::ServerFixture::receiveMutex
std::mutex receiveMutex
Mutex to protect data structures that store messages.
Definition: ServerFixture.hh:814
gazebo::ServerFixture::CheckPointer
static void CheckPointer(boost::shared_ptr< T > _ptr)
Check that a pointer is not NULL.
Definition: ServerFixture.hh:233
gazebo::ServerFixture::poses
std::map< std::string, ignition::math::Pose3d > poses
Map of received poses.
Definition: ServerFixture.hh:811
NULL
#define NULL
Definition: CommonTypes.hh:31
gazebo::ServerFixture::requestPub
transport::PublisherPtr requestPub
Request publisher.
Definition: ServerFixture.hh:808
gazebo::ServerFixture::server
Server * server
Pointer the Gazebo server.
Definition: ServerFixture.hh:790
gazebo::physics::ModelPtr
boost::shared_ptr< Model > ModelPtr
Definition: PhysicsTypes.hh:93
gazebo::Server
Definition: Server.hh:41
gazebo::sensors::SonarSensorPtr
std::shared_ptr< SonarSensor > SonarSensorPtr
Definition: SensorTypes.hh:112
GAZEBO_VISIBLE
#define GAZEBO_VISIBLE
Use to represent "symbol visible" if supported.
Definition: system.hh:59
gazebo::common::Time
A Time class, can be used to hold wall- or sim-time.
Definition: Time.hh:44