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 
48 #include "gazebo/common/Console.hh"
49 #include "gazebo/physics/World.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 
631  protected: void SpawnLight(const std::string &_name,
632  const std::string &_type,
633  const ignition::math::Vector3d &_pos,
634  const ignition::math::Vector3d &_rpy,
635  const common::Color &_diffuse,
636  const common::Color &_specular = common::Color::White,
637  const ignition::math::Vector3d &_direction =
638  -ignition::math::Vector3d::UnitZ,
639  double _attenuationRange = 20,
640  double _attenuationConstant = 0.5,
641  double _attenuationLinear = 0.01,
642  double _attenuationQuadratic = 0.001,
643  double _spotInnerAngle = 0,
644  double _spotOuterAngle = 0,
645  double _spotFallOff = 0,
646  bool _castShadows = true) GAZEBO_DEPRECATED(9.0);
647 
664  protected: void SpawnLight(const std::string &_name,
665  const std::string &_type,
666  const ignition::math::Vector3d &_pos =
667  ignition::math::Vector3d::Zero,
668  const ignition::math::Vector3d &_rpy =
669  ignition::math::Vector3d::Zero,
670  const ignition::math::Color &_diffuse =
671  ignition::math::Color::White,
672  const ignition::math::Color &_specular =
673  ignition::math::Color::White,
674  const ignition::math::Vector3d &_direction =
675  -ignition::math::Vector3d::UnitZ,
676  const double _attenuationRange = 20,
677  const double _attenuationConstant = 0.5,
678  const double _attenuationLinear = 0.01,
679  const double _attenuationQuadratic = 0.001,
680  const double _spotInnerAngle = 0,
681  const double _spotOuterAngle = 0,
682  const double _spotFallOff = 0,
683  const bool _castShadows = true);
684 
690  protected: void SpawnCylinder(const std::string &_name,
691  const ignition::math::Vector3d &_pos =
692  ignition::math::Vector3d::Zero,
693  const ignition::math::Vector3d &_rpy =
694  ignition::math::Vector3d::Zero,
695  bool _static = false);
696 
704  protected: void SpawnSphere(const std::string &_name,
705  const ignition::math::Vector3d &_pos,
706  const ignition::math::Vector3d &_rpy,
707  bool _wait = true, bool _static = false);
708 
718  protected: void SpawnSphere(const std::string &_name,
719  const ignition::math::Vector3d &_pos,
720  const ignition::math::Vector3d &_rpy,
721  const ignition::math::Vector3d &_cog,
722  double _radius = 1.0,
723  bool _wait = true, bool _static = false);
724 
731  protected: void SpawnBox(const std::string &_name,
732  const ignition::math::Vector3d &_size =
733  ignition::math::Vector3d::One,
734  const ignition::math::Vector3d &_pos =
735  ignition::math::Vector3d::Zero,
736  const ignition::math::Vector3d &_rpy =
737  ignition::math::Vector3d::Zero,
738  bool _static = false);
739 
747  protected: void SpawnTrimesh(const std::string &_name,
748  const std::string &_modelPath,
749  const ignition::math::Vector3d &_scale =
750  ignition::math::Vector3d::One,
751  const ignition::math::Vector3d &_pos =
752  ignition::math::Vector3d::Zero,
753  const ignition::math::Vector3d &_rpy =
754  ignition::math::Vector3d::Zero,
755  bool _static = false);
756 
762  protected: void SpawnEmptyLink(const std::string &_name,
763  const ignition::math::Vector3d &_pos =
764  ignition::math::Vector3d::Zero,
765  const ignition::math::Vector3d &_rpy =
766  ignition::math::Vector3d::Zero,
767  bool _static = false);
768 
771  protected: void SpawnModel(const std::string &_filename);
772 
775  protected: void SpawnSDF(const std::string &_sdf);
776 
780  protected: void LoadPlugin(const std::string &_filename,
781  const std::string &_name);
782 
786  protected: physics::ModelPtr GetModel(const std::string &_name);
787 
790  protected: void RemoveModel(const std::string &_name);
791 
794  protected: void RemovePlugin(const std::string &_name);
795 
799  protected: void GetMemInfo(double &_resident, double &_share);
800 
804  protected: std::string GetUniqueString(const std::string &_prefix);
805 
809  protected: void Record(const std::string &_name, const double _data);
810 
814  protected: void Record(const std::string &_prefix,
815  const ignition::math::SignalStats &_stats);
816 
820  protected: void Record(const std::string &_prefix,
821  const ignition::math::Vector3Stats &_stats);
822 
824  protected: Server *server;
825 
827  protected: boost::thread *serverThread;
828 
830  protected: transport::NodePtr node;
831 
833  protected: transport::SubscriberPtr poseSub;
834 
836  protected: transport::SubscriberPtr statsSub;
837 
839  protected: transport::PublisherPtr factoryPub;
840 
842  protected: transport::PublisherPtr requestPub;
843 
845  protected: std::map<std::string, ignition::math::Pose3d> poses;
846 
848  protected: std::mutex receiveMutex;
849 
851  private: unsigned char **imgData;
852 
854  private: int gotImage;
855 
857  protected: common::Time simTime, realTime, pauseTime;
858 
860  private: double percentRealTime;
861 
863  private: bool paused;
864 
866  private: bool serverRunning;
867 
869  private: int uniqueCounter;
870  };
871 
873  {
874  // Documentation inherited.
875  public: virtual void SetUp();
876 
877  // Documentation inherited.
878  protected: virtual void Unload();
879  };
880 } // namespace gazebo
881 #endif // define _GAZEBO_SERVER_FIXTURE_HH_
Definition: ServerFixture.hh:872
std::string custom_exec(std::string _cmd)
Definition: Model.hh:40
Forward declarations for the common classes.
Definition: Animation.hh:26
A Time class, can be used to hold wall- or sim-time. stored as sec and nano-sec.
Definition: Time.hh:44
boost::shared_ptr< Model > ModelPtr
Definition: PhysicsTypes.hh:93
#define NULL
Definition: CommonTypes.hh:31
boost::shared_ptr< Publisher > PublisherPtr
Definition: TransportTypes.hh:49
Definition: JointMaker.hh:44
boost::shared_ptr< Subscriber > SubscriberPtr
Definition: TransportTypes.hh:53
static const Color White
(1, 1, 1)
Definition: Color.hh:39
default namespace for gazebo
boost::shared_ptr< Node > NodePtr
Definition: TransportTypes.hh:57
boost::shared_ptr< Scene > ScenePtr
Definition: RenderTypes.hh:82
Definition: ServerFixture.hh:66
std::shared_ptr< SonarSensor > SonarSensorPtr
Definition: SensorTypes.hh:112
Definition: Server.hh:41
static void CheckPointer(boost::shared_ptr< T > _ptr)
Check that a pointer is not NULL.
Definition: ServerFixture.hh:233
Defines a color.
Definition: Color.hh:36
#define GAZEBO_DEPRECATED(version)
Definition: system.hh:328
#define GAZEBO_VISIBLE
Definition: system.hh:59