ServerFixture.hh
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2012-2016 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 <boost/filesystem.hpp>
34 
35 #include <map>
36 #include <string>
37 #include <vector>
38 
39 #include <ignition/math/Pose3.hh>
40 
41 #include "gazebo/transport/transport.hh"
42 
45 #include "gazebo/common/Console.hh"
46 #include "gazebo/physics/World.hh"
49 #include "gazebo/sensors/sensors.hh"
50 #include "gazebo/rendering/rendering.hh"
51 #include "gazebo/msgs/msgs.hh"
52 
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: math::Pose GetEntityPose(const std::string &_name);
139 
143  protected: bool HasEntity(const std::string &_name);
144 
151  protected: void PrintImage(const std::string &_name, unsigned char **_image,
152  unsigned int _width, unsigned int _height,
153  unsigned int _depth);
154 
159  protected: void PrintScan(const std::string &_name, double *_scan,
160  unsigned int _sampleCount);
161 
170  protected: void FloatCompare(float *_scanA, float *_scanB,
171  unsigned int _sampleCount, float &_diffMax,
172  float &_diffSum, float &_diffAvg);
173 
182  protected: void DoubleCompare(double *_scanA, double *_scanB,
183  unsigned int _sampleCount, double &_diffMax,
184  double &_diffSum, double &_diffAvg);
185 
195  protected: void ImageCompare(unsigned char *_imageA,
196  unsigned char *_imageB,
197  unsigned int _width, unsigned int _height,
198  unsigned int _depth,
199  unsigned int &_diffMax, unsigned int &_diffSum,
200  double &_diffAvg);
201 
208  private: void OnNewFrame(const unsigned char *_image,
209  unsigned int _width, unsigned int _height,
210  unsigned int _depth,
211  const std::string &/*_format*/);
212 
218  protected: void GetFrame(const std::string &_cameraName,
219  unsigned char **_imgData, unsigned int &_width,
220  unsigned int &_height);
221 
225  protected: physics::ModelPtr SpawnModel(const msgs::Model &_msg);
226 
231  protected: template<typename T>
232  static void CheckPointer(boost::shared_ptr<T> _ptr)
233  {
234  ASSERT_TRUE(_ptr != NULL);
235  }
236 
255  protected: void SpawnCamera(const std::string &_modelName,
256  const std::string &_cameraName,
257  const math::Vector3 &_pos, const math::Vector3 &_rpy,
258  unsigned int _width = 320, unsigned int _height = 240,
259  double _rate = 25,
260  const std::string &_noiseType = "",
261  double _noiseMean = 0.0, double _noiseStdDev = 0.0,
262  bool _distortion = false, double _distortionK1 = 0.0,
263  double _distortionK2 = 0.0, double _distortionK3 = 0.0,
264  double _distortionP1 = 0.0, double _distortionP2 = 0.0,
265  double _cx = 0.5, double _cy = 0.5);
266 
290  protected: void SpawnWideAngleCamera(const std::string &_modelName,
291  const std::string &_cameraName,
292  const ignition::math::Vector3d &_pos,
293  const ignition::math::Vector3d &_rpy,
294  unsigned int _width, unsigned int _height, double _rate,
295  const double _hfov,
296  const std::string &_lensType = "stereographic",
297  const bool _scaleToHfov = true,
298  const double _cutoffAngle = 3.1415,
299  const double _envTextureSize = 512,
300  const double _c1 = 1.05, const double _c2 = 4,
301  const double _f = 1.0,
302  const std::string &_fun = "tan");
303 
319  protected: void SpawnRaySensor(const std::string &_modelName,
320  const std::string &_raySensorName,
321  const math::Vector3 &_pos, const math::Vector3 &_rpy,
322  double _hMinAngle = -2.0, double _hMaxAngle = 2.0,
323  double _vMinAngle = -1.0, double _vMaxAngle = 1.0,
324  double _minRange = 0.08, double _maxRange = 10,
325  double _rangeResolution = 0.01, unsigned int _samples = 640,
326  unsigned int _vSamples = 1, double _hResolution = 1.0,
327  double _vResolution = 1.0,
328  const std::string &_noiseType = "", double _noiseMean = 0.0,
329  double _noiseStdDev = 0.0);
330 
338  protected: sensors::SonarSensorPtr SpawnSonar(const std::string &_modelName,
339  const std::string &_sonarName,
340  const ignition::math::Pose3d &_pose,
341  const double _minRange,
342  const double _maxRange,
343  const double _radius);
344 
360  protected: void SpawnGpuRaySensor(const std::string &_modelName,
361  const std::string &_raySensorName,
362  const math::Vector3 &_pos, const math::Vector3 &_rpy,
363  double _hMinAngle = -2.0, double _hMaxAngle = 2.0,
364  double _minRange = 0.08, double _maxRange = 10,
365  double _rangeResolution = 0.01, unsigned int _samples = 640,
366  const std::string &_noiseType = "", double _noiseMean = 0.0,
367  double _noiseStdDev = 0.0);
368 
382  protected: void SpawnImuSensor(const std::string &_modelName,
383  const std::string &_imuSensorName,
384  const math::Vector3 &_pos, const math::Vector3 &_rpy,
385  const std::string &_noiseType = "",
386  double _rateNoiseMean = 0.0, double _rateNoiseStdDev = 0.0,
387  double _rateBiasMean = 0.0, double _rateBiasStdDev = 0.0,
388  double _accelNoiseMean = 0.0, double _accelNoiseStdDev = 0.0,
389  double _accelBiasMean = 0.0, double _accelBiasStdDev = 0.0);
390 
398  protected: void SpawnUnitContactSensor(const std::string &_name,
399  const std::string &_sensorName,
400  const std::string &_collisionType, const math::Vector3 &_pos,
401  const math::Vector3 &_rpy, bool _static = false);
402 
411  protected: void SpawnUnitImuSensor(const std::string &_name,
412  const std::string &_sensorName,
413  const std::string &_collisionType,
414  const std::string &_topic, const math::Vector3 &_pos,
415  const math::Vector3 &_rpy, bool _static = false);
416 
425  protected: void SpawnUnitAltimeterSensor(const std::string &_name,
426  const std::string &_sensorName,
427  const std::string &_collisionType,
428  const std::string &_topic,
429  const ignition::math::Vector3d &_pos,
430  const ignition::math::Vector3d &_rpy,
431  bool _static = false);
432 
441  protected: void SpawnUnitMagnetometerSensor(const std::string &_name,
442  const std::string &_sensorName,
443  const std::string &_collisionType,
444  const std::string &_topic,
445  const ignition::math::Vector3d &_pos,
446  const ignition::math::Vector3d &_rpy,
447  bool _static = false);
448 
453  private: void launchTimeoutFailure(const char *_logMsg,
454  const int _timeoutCS);
455 
466  protected: void SpawnWirelessTransmitterSensor(const std::string &_name,
467  const std::string &_sensorName,
468  const math::Vector3 &_pos,
469  const math::Vector3 &_rpy,
470  const std::string &_essid,
471  double _freq,
472  double _power,
473  double _gain,
474  bool _visualize = true);
475 
487  protected: void SpawnWirelessReceiverSensor(const std::string &_name,
488  const std::string &_sensorName,
489  const math::Vector3 &_pos,
490  const math::Vector3 &_rpy,
491  double _minFreq,
492  double _maxFreq,
493  double _power,
494  double _gain,
495  double _sensitivity,
496  bool _visualize = true);
497 
502  protected: void WaitUntilEntitySpawn(const std::string &_name,
503  unsigned int _sleepEach,
504  int _retries);
505 
510  protected: void WaitUntilSensorSpawn(const std::string &_name,
511  unsigned int _sleepEach,
512  int _retries);
513 
519  protected: void WaitUntilIteration(const uint32_t _goalIteration,
520  const int _sleepEach,
521  const int _retries) const;
522 
528  protected: void WaitUntilSimTime(const common::Time &_goalTime,
529  const int _ms,
530  const int _maxRetries) const;
531 
548  protected: void SpawnLight(const std::string &_name,
549  const std::string &_type,
550  const math::Vector3 &_pos, const math::Vector3 &_rpy,
551  const common::Color &_diffuse = common::Color::White,
552  const common::Color &_specular = common::Color::White,
553  const math::Vector3 &_direction = -math::Vector3::UnitZ,
554  double _attenuationRange = 20,
555  double _attenuationConstant = 0.5,
556  double _attenuationLinear = 0.01,
557  double _attenuationQuadratic = 0.001,
558  double _spotInnerAngle = 0,
559  double _spotOuterAngle = 0,
560  double _spotFallOff = 0,
561  bool _castShadows = true);
562 
568  protected: void SpawnCylinder(const std::string &_name,
569  const math::Vector3 &_pos, const math::Vector3 &_rpy,
570  bool _static = false);
571 
579  protected: void SpawnSphere(const std::string &_name,
580  const math::Vector3 &_pos, const math::Vector3 &_rpy,
581  bool _wait = true, bool _static = false);
582 
592  protected: void SpawnSphere(const std::string &_name,
593  const math::Vector3 &_pos, const math::Vector3 &_rpy,
594  const math::Vector3 &_cog, double _radius,
595  bool _wait = true, bool _static = false);
596 
603  protected: void SpawnBox(const std::string &_name,
604  const math::Vector3 &_size, const math::Vector3 &_pos,
605  const math::Vector3 &_rpy, bool _static = false);
606 
614  protected: void SpawnTrimesh(const std::string &_name,
615  const std::string &_modelPath, const math::Vector3 &_scale,
616  const math::Vector3 &_pos, const math::Vector3 &_rpy,
617  bool _static = false);
618 
624  protected: void SpawnEmptyLink(const std::string &_name,
625  const math::Vector3 &_pos, const math::Vector3 &_rpy,
626  bool _static = false);
627 
630  protected: void SpawnModel(const std::string &_filename);
631 
634  protected: void SpawnSDF(const std::string &_sdf);
635 
639  protected: void LoadPlugin(const std::string &_filename,
640  const std::string &_name);
641 
645  protected: physics::ModelPtr GetModel(const std::string &_name);
646 
649  protected: void RemoveModel(const std::string &_name);
650 
653  protected: void RemovePlugin(const std::string &_name);
654 
658  protected: void GetMemInfo(double &_resident, double &_share);
659 
663  protected: std::string GetUniqueString(const std::string &_prefix);
664 
668  protected: void Record(const std::string &_name, const double _data);
669 
673  protected: void Record(const std::string &_prefix,
674  const math::SignalStats &_stats);
675 
679  protected: void Record(const std::string &_prefix,
680  const math::Vector3Stats &_stats);
681 
683  protected: Server *server;
684 
686  protected: boost::thread *serverThread;
687 
690 
693 
696 
699 
702 
704  protected: std::map<std::string, math::Pose> poses;
705 
707  protected: boost::mutex receiveMutex;
708 
710  private: unsigned char **imgData;
711 
713  private: int gotImage;
714 
716  protected: common::Time simTime, realTime, pauseTime;
717 
719  private: double percentRealTime;
720 
722  private: bool paused;
723 
725  private: bool serverRunning;
726 
728  private: int uniqueCounter;
729  };
730 
732  {
733  // Documentation inherited.
734  public: virtual void SetUp();
735 
736  // Documentation inherited.
737  protected: virtual void Unload();
738  };
739 } // namespace gazebo
740 #endif // define _GAZEBO_SERVER_FIXTURE_HH_
boost::mutex receiveMutex
Mutex to protect data structures that store messages.
Definition: ServerFixture.hh:707
transport::SubscriberPtr statsSub
World statistics subscription.
Definition: ServerFixture.hh:695
static const Color White
(1, 1, 1)
Definition: Color.hh:39
transport::SubscriberPtr poseSub
Pose subscription.
Definition: ServerFixture.hh:692
Forward declarations for the common classes.
Definition: Animation.hh:33
Encapsulates a position and rotation in three space.
Definition: Pose.hh:37
The Vector3 class represents the generic vector containing 3 elements.
Definition: Vector3.hh:39
Definition: ServerFixture.hh:66
std::string custom_exec(std::string _cmd)
transport::PublisherPtr factoryPub
Factory publisher.
Definition: ServerFixture.hh:698
boost::shared_ptr< Publisher > PublisherPtr
Definition: TransportTypes.hh:49
boost::shared_ptr< Subscriber > SubscriberPtr
Definition: TransportTypes.hh:53
Collection of statistics for a scalar signal.
Definition: SignalStats.hh:124
boost::thread * serverThread
Pointer the thread the runs the server.
Definition: ServerFixture.hh:686
Collection of statistics for a Vector3 signal.
Definition: Vector3Stats.hh:37
boost::shared_ptr< Node > NodePtr
Definition: TransportTypes.hh:57
default namespace for gazebo
transport::NodePtr node
Pointer to a node for communication.
Definition: ServerFixture.hh:689
Definition: ServerFixture.hh:731
boost::shared_ptr< Scene > ScenePtr
Definition: RenderTypes.hh:80
common::Time simTime
Current simulation time, real time, and pause time.
Definition: ServerFixture.hh:716
static const Vector3 UnitZ
math::Vector3(0, 0, 1)
Definition: Vector3.hh:54
static void CheckPointer(boost::shared_ptr< T > _ptr)
Check that a pointer is not NULL.
Definition: ServerFixture.hh:232
#define NULL
Definition: CommonTypes.hh:31
Defines a color.
Definition: Color.hh:36
transport::PublisherPtr requestPub
Request publisher.
Definition: ServerFixture.hh:701
Server * server
Pointer the Gazebo server.
Definition: ServerFixture.hh:683
boost::shared_ptr< Model > ModelPtr
Definition: PhysicsTypes.hh:88
Definition: Server.hh:41
std::map< std::string, math::Pose > poses
Map of received poses.
Definition: ServerFixture.hh:704
std::shared_ptr< SonarSensor > SonarSensorPtr
Definition: SensorTypes.hh:112
#define GAZEBO_VISIBLE
Use to represent "symbol visible" if supported.
Definition: system.hh:59
A Time class, can be used to hold wall- or sim-time.
Definition: Time.hh:44