17 #ifndef _GAZEBO_SERVER_FIXTURE_HH_ 18 #define _GAZEBO_SERVER_FIXTURE_HH_ 20 #pragma GCC diagnostic ignored "-Wswitch-default" 21 #pragma GCC diagnostic ignored "-Wfloat-equal" 22 #pragma GCC diagnostic ignored "-Wshadow" 26 # include <mach/mach.h> 31 #include <gtest/gtest.h> 32 #include <boost/thread.hpp> 33 #include <boost/filesystem.hpp> 39 #include <ignition/math/Pose3.hh> 41 #include "gazebo/transport/transport.hh" 49 #include "gazebo/sensors/sensors.hh" 50 #include "gazebo/rendering/rendering.hh" 56 #include "gazebo/gazebo_config.h" 60 #include "test_config.h" 75 protected:
virtual void TearDown();
78 protected:
virtual void Unload();
82 protected:
virtual void Load(
const std::string &_worldFilename);
88 protected:
virtual void Load(
const std::string &_worldFilename,
98 protected:
virtual void Load(
const std::string &_worldFilename,
99 bool _paused,
const std::string &_physics,
100 const std::vector<std::string> &_systemPlugins = {});
106 protected:
virtual void LoadArgs(
const std::string &_args);
112 protected:
void RunServer(
const std::vector<std::string> &_args);
117 const std::string &_sceneName =
"default");
121 protected:
void OnStats(ConstWorldStatisticsPtr &_msg);
124 protected:
void SetPause(
bool _pause);
128 protected:
double GetPercentRealTime()
const;
133 protected:
void OnPose(ConstPosesStampedPtr &_msg);
138 protected:
math::Pose GetEntityPose(
const std::string &_name);
143 protected:
bool HasEntity(
const std::string &_name);
151 protected:
void PrintImage(
const std::string &_name,
unsigned char **_image,
152 unsigned int _width,
unsigned int _height,
153 unsigned int _depth);
159 protected:
void PrintScan(
const std::string &_name,
double *_scan,
160 unsigned int _sampleCount);
170 protected:
void FloatCompare(
float *_scanA,
float *_scanB,
171 unsigned int _sampleCount,
float &_diffMax,
172 float &_diffSum,
float &_diffAvg);
182 protected:
void DoubleCompare(
double *_scanA,
double *_scanB,
183 unsigned int _sampleCount,
double &_diffMax,
184 double &_diffSum,
double &_diffAvg);
195 protected:
void ImageCompare(
unsigned char *_imageA,
196 unsigned char *_imageB,
197 unsigned int _width,
unsigned int _height,
199 unsigned int &_diffMax,
unsigned int &_diffSum,
208 private:
void OnNewFrame(
const unsigned char *_image,
209 unsigned int _width,
unsigned int _height,
211 const std::string &);
218 protected:
void GetFrame(
const std::string &_cameraName,
219 unsigned char **_imgData,
unsigned int &_width,
220 unsigned int &_height);
231 protected:
template<
typename T>
234 ASSERT_TRUE(_ptr !=
NULL);
255 protected:
void SpawnCamera(
const std::string &_modelName,
256 const std::string &_cameraName,
258 unsigned int _width = 320,
unsigned int _height = 240,
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);
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,
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");
319 protected:
void SpawnRaySensor(
const std::string &_modelName,
320 const std::string &_raySensorName,
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);
339 const std::string &_sonarName,
340 const ignition::math::Pose3d &_pose,
341 const double _minRange,
342 const double _maxRange,
343 const double _radius);
360 protected:
void SpawnGpuRaySensor(
const std::string &_modelName,
361 const std::string &_raySensorName,
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);
382 protected:
void SpawnImuSensor(
const std::string &_modelName,
383 const std::string &_imuSensorName,
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);
398 protected:
void SpawnUnitContactSensor(
const std::string &_name,
399 const std::string &_sensorName,
400 const std::string &_collisionType,
const math::Vector3 &_pos,
411 protected:
void SpawnUnitImuSensor(
const std::string &_name,
412 const std::string &_sensorName,
413 const std::string &_collisionType,
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);
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);
453 private:
void launchTimeoutFailure(
const char *_logMsg,
454 const int _timeoutCS);
466 protected:
void SpawnWirelessTransmitterSensor(
const std::string &_name,
467 const std::string &_sensorName,
470 const std::string &_essid,
474 bool _visualize =
true);
487 protected:
void SpawnWirelessReceiverSensor(
const std::string &_name,
488 const std::string &_sensorName,
496 bool _visualize =
true);
502 protected:
void WaitUntilEntitySpawn(
const std::string &_name,
503 unsigned int _sleepEach,
510 protected:
void WaitUntilSensorSpawn(
const std::string &_name,
511 unsigned int _sleepEach,
519 protected:
void WaitUntilIteration(
const uint32_t _goalIteration,
520 const int _sleepEach,
521 const int _retries)
const;
528 protected:
void WaitUntilSimTime(
const common::Time &_goalTime,
530 const int _maxRetries)
const;
548 protected:
void SpawnLight(
const std::string &_name,
549 const std::string &_type,
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);
568 protected:
void SpawnCylinder(
const std::string &_name,
570 bool _static =
false);
579 protected:
void SpawnSphere(
const std::string &_name,
581 bool _wait =
true,
bool _static =
false);
592 protected:
void SpawnSphere(
const std::string &_name,
595 bool _wait =
true,
bool _static =
false);
603 protected:
void SpawnBox(
const std::string &_name,
614 protected:
void SpawnTrimesh(
const std::string &_name,
617 bool _static =
false);
624 protected:
void SpawnEmptyLink(
const std::string &_name,
626 bool _static =
false);
630 protected:
void SpawnModel(
const std::string &_filename);
634 protected:
void SpawnSDF(
const std::string &_sdf);
639 protected:
void LoadPlugin(
const std::string &_filename,
640 const std::string &_name);
649 protected:
void RemoveModel(
const std::string &_name);
653 protected:
void RemovePlugin(
const std::string &_name);
658 protected:
void GetMemInfo(
double &_resident,
double &_share);
663 protected: std::string GetUniqueString(
const std::string &_prefix);
668 protected:
void Record(
const std::string &_name,
const double _data);
673 protected:
void Record(
const std::string &_prefix,
679 protected:
void Record(
const std::string &_prefix,
704 protected: std::map<std::string, math::Pose>
poses;
710 private:
unsigned char **imgData;
713 private:
int gotImage;
719 private:
double percentRealTime;
722 private:
bool paused;
725 private:
bool serverRunning;
728 private:
int uniqueCounter;
734 public:
virtual void SetUp();
737 protected:
virtual void Unload();
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:51
boost::shared_ptr< Subscriber > SubscriberPtr
Definition: TransportTypes.hh:55
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:59
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:82
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:33
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
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