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 
282  protected: void SpawnRaySensor(const std::string &_modelName,
283  const std::string &_raySensorName,
284  const math::Vector3 &_pos, const math::Vector3 &_rpy,
285  double _hMinAngle = -2.0, double _hMaxAngle = 2.0,
286  double _vMinAngle = -1.0, double _vMaxAngle = 1.0,
287  double _minRange = 0.08, double _maxRange = 10,
288  double _rangeResolution = 0.01, unsigned int _samples = 640,
289  unsigned int _vSamples = 1, double _hResolution = 1.0,
290  double _vResolution = 1.0,
291  const std::string &_noiseType = "", double _noiseMean = 0.0,
292  double _noiseStdDev = 0.0);
293 
301  protected: sensors::SonarSensorPtr SpawnSonar(const std::string &_modelName,
302  const std::string &_sonarName,
303  const ignition::math::Pose3d &_pose,
304  const double _minRange,
305  const double _maxRange,
306  const double _radius);
307 
323  protected: void SpawnGpuRaySensor(const std::string &_modelName,
324  const std::string &_raySensorName,
325  const math::Vector3 &_pos, const math::Vector3 &_rpy,
326  double _hMinAngle = -2.0, double _hMaxAngle = 2.0,
327  double _minRange = 0.08, double _maxRange = 10,
328  double _rangeResolution = 0.01, unsigned int _samples = 640,
329  const std::string &_noiseType = "", double _noiseMean = 0.0,
330  double _noiseStdDev = 0.0);
331 
342  protected: void SpawnDepthCameraSensor(const std::string &_modelName,
343  const std::string &_cameraName,
344  const ignition::math::Vector3d &_pos,
345  const ignition::math::Vector3d &_rpy,
346  const unsigned int _width = 320,
347  const unsigned int _height = 240,
348  const double _rate = 25, const double _near = 0.1,
349  const double _far = 10);
350 
364  protected: void SpawnImuSensor(const std::string &_modelName,
365  const std::string &_imuSensorName,
366  const math::Vector3 &_pos, const math::Vector3 &_rpy,
367  const std::string &_noiseType = "",
368  double _rateNoiseMean = 0.0, double _rateNoiseStdDev = 0.0,
369  double _rateBiasMean = 0.0, double _rateBiasStdDev = 0.0,
370  double _accelNoiseMean = 0.0, double _accelNoiseStdDev = 0.0,
371  double _accelBiasMean = 0.0, double _accelBiasStdDev = 0.0);
372 
380  protected: void SpawnUnitContactSensor(const std::string &_name,
381  const std::string &_sensorName,
382  const std::string &_collisionType, const math::Vector3 &_pos,
383  const math::Vector3 &_rpy, bool _static = false);
384 
393  protected: void SpawnUnitImuSensor(const std::string &_name,
394  const std::string &_sensorName,
395  const std::string &_collisionType,
396  const std::string &_topic, const math::Vector3 &_pos,
397  const math::Vector3 &_rpy, bool _static = false);
398 
407  protected: void SpawnUnitAltimeterSensor(const std::string &_name,
408  const std::string &_sensorName,
409  const std::string &_collisionType,
410  const std::string &_topic,
411  const ignition::math::Vector3d &_pos,
412  const ignition::math::Vector3d &_rpy,
413  bool _static = false);
414 
423  protected: void SpawnUnitMagnetometerSensor(const std::string &_name,
424  const std::string &_sensorName,
425  const std::string &_collisionType,
426  const std::string &_topic,
427  const ignition::math::Vector3d &_pos,
428  const ignition::math::Vector3d &_rpy,
429  bool _static = false);
430 
435  private: void launchTimeoutFailure(const char *_logMsg,
436  const int _timeoutCS);
437 
448  protected: void SpawnWirelessTransmitterSensor(const std::string &_name,
449  const std::string &_sensorName,
450  const math::Vector3 &_pos,
451  const math::Vector3 &_rpy,
452  const std::string &_essid,
453  double _freq,
454  double _power,
455  double _gain,
456  bool _visualize = true);
457 
469  protected: void SpawnWirelessReceiverSensor(const std::string &_name,
470  const std::string &_sensorName,
471  const math::Vector3 &_pos,
472  const math::Vector3 &_rpy,
473  double _minFreq,
474  double _maxFreq,
475  double _power,
476  double _gain,
477  double _sensitivity,
478  bool _visualize = true);
479 
484  protected: void WaitUntilEntitySpawn(const std::string &_name,
485  unsigned int _sleepEach,
486  int _retries);
487 
492  protected: void WaitUntilSensorSpawn(const std::string &_name,
493  unsigned int _sleepEach,
494  int _retries);
495 
501  protected: void WaitUntilIteration(const uint32_t _goalIteration,
502  const int _sleepEach,
503  const int _retries) const;
504 
510  protected: void WaitUntilSimTime(const common::Time &_goalTime,
511  const int _ms,
512  const int _maxRetries) const;
513 
530  protected: void SpawnLight(const std::string &_name,
531  const std::string &_type,
532  const math::Vector3 &_pos, const math::Vector3 &_rpy,
533  const common::Color &_diffuse = common::Color::White,
534  const common::Color &_specular = common::Color::White,
535  const math::Vector3 &_direction = -math::Vector3::UnitZ,
536  double _attenuationRange = 20,
537  double _attenuationConstant = 0.5,
538  double _attenuationLinear = 0.01,
539  double _attenuationQuadratic = 0.001,
540  double _spotInnerAngle = 0,
541  double _spotOuterAngle = 0,
542  double _spotFallOff = 0,
543  bool _castShadows = true);
544 
550  protected: void SpawnCylinder(const std::string &_name,
551  const math::Vector3 &_pos, const math::Vector3 &_rpy,
552  bool _static = false);
553 
561  protected: void SpawnSphere(const std::string &_name,
562  const math::Vector3 &_pos, const math::Vector3 &_rpy,
563  bool _wait = true, bool _static = false);
564 
574  protected: void SpawnSphere(const std::string &_name,
575  const math::Vector3 &_pos, const math::Vector3 &_rpy,
576  const math::Vector3 &_cog, double _radius,
577  bool _wait = true, bool _static = false);
578 
585  protected: void SpawnBox(const std::string &_name,
586  const math::Vector3 &_size, const math::Vector3 &_pos,
587  const math::Vector3 &_rpy, bool _static = false);
588 
596  protected: void SpawnTrimesh(const std::string &_name,
597  const std::string &_modelPath, const math::Vector3 &_scale,
598  const math::Vector3 &_pos, const math::Vector3 &_rpy,
599  bool _static = false);
600 
606  protected: void SpawnEmptyLink(const std::string &_name,
607  const math::Vector3 &_pos, const math::Vector3 &_rpy,
608  bool _static = false);
609 
612  protected: void SpawnModel(const std::string &_filename);
613 
616  protected: void SpawnSDF(const std::string &_sdf);
617 
621  protected: void LoadPlugin(const std::string &_filename,
622  const std::string &_name);
623 
627  protected: physics::ModelPtr GetModel(const std::string &_name);
628 
631  protected: void RemoveModel(const std::string &_name);
632 
635  protected: void RemovePlugin(const std::string &_name);
636 
640  protected: void GetMemInfo(double &_resident, double &_share);
641 
645  protected: std::string GetUniqueString(const std::string &_prefix);
646 
650  protected: void Record(const std::string &_name, const double _data);
651 
655  protected: void Record(const std::string &_prefix,
656  const math::SignalStats &_stats);
657 
661  protected: void Record(const std::string &_prefix,
662  const math::Vector3Stats &_stats);
663 
665  protected: Server *server;
666 
668  protected: boost::thread *serverThread;
669 
672 
675 
678 
681 
684 
686  protected: std::map<std::string, math::Pose> poses;
687 
689  protected: boost::mutex receiveMutex;
690 
692  private: unsigned char **imgData;
693 
695  private: int gotImage;
696 
698  protected: common::Time simTime, realTime, pauseTime;
699 
701  private: double percentRealTime;
702 
704  private: bool paused;
705 
707  private: bool serverRunning;
708 
710  private: int uniqueCounter;
711  };
712 
714  {
715  // Documentation inherited.
716  public: virtual void SetUp();
717  };
718 } // namespace gazebo
719 #endif // define _GAZEBO_SERVER_FIXTURE_HH_
boost::mutex receiveMutex
Mutex to protect data structures that store messages.
Definition: ServerFixture.hh:689
transport::SubscriberPtr statsSub
World statistics subscription.
Definition: ServerFixture.hh:677
static const Color White
(1, 1, 1)
Definition: Color.hh:39
transport::SubscriberPtr poseSub
Pose subscription.
Definition: ServerFixture.hh:674
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:680
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:668
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:671
Definition: ServerFixture.hh:713
boost::shared_ptr< Scene > ScenePtr
Definition: RenderTypes.hh:80
common::Time simTime
Current simulation time, real time, and pause time.
Definition: ServerFixture.hh:698
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:683
Server * server
Pointer the Gazebo server.
Definition: ServerFixture.hh:665
boost::shared_ptr< Model > ModelPtr
Definition: PhysicsTypes.hh:93
Definition: Server.hh:41
std::map< std::string, math::Pose > poses
Map of received poses.
Definition: ServerFixture.hh:686
std::shared_ptr< SonarSensor > SonarSensorPtr
Definition: SensorTypes.hh:107
#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