ServerFixture.hh

Go to the documentation of this file.

/*
 * Copyright (C) 2012 Open Source Robotics Foundation
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
*/
#ifndef _GAZEBO_SERVER_FIXTURE_HH_
#define _GAZEBO_SERVER_FIXTURE_HH_

#pragma GCC diagnostic ignored "-Wswitch-default"
#pragma GCC diagnostic ignored "-Wfloat-equal"
#pragma GCC diagnostic ignored "-Wshadow"

// The following is needed to enable the GetMemInfo function for OSX
#ifdef __MACH__
# include <mach/mach.h>
#endif  // __MACH__

#include <sdf/sdf.hh>

#include <gtest/gtest.h>
#include <boost/thread.hpp>
#include <mutex>
#include <boost/filesystem.hpp>

#include <map>
#include <string>
#include <vector>

#include <ignition/math/Pose3.hh>
#include <ignition/math/SignalStats.hh>
#include <ignition/math/Vector3Stats.hh>

#include "gazebo/transport/transport.hh"

#include "gazebo/common/CommonIface.hh"
#include "gazebo/common/SystemPaths.hh"
#include "gazebo/common/Console.hh"
#include "gazebo/physics/World.hh"
#include "gazebo/physics/PhysicsTypes.hh"
#include "gazebo/physics/PhysicsIface.hh"
#include "gazebo/sensors/sensors.hh"
#include "gazebo/rendering/rendering.hh"
#include "gazebo/msgs/msgs.hh"

#include "gazebo/gazebo_config.h"
#include "gazebo/Server.hh"
#include "gazebo/util/system.hh"

#include "test_config.h"

namespace gazebo
{
  std::string custom_exec(std::string _cmd);

  class ServerFixture : public testing::Test
  {
    protected: ServerFixture();

    protected: virtual ~ServerFixture();

    protected: virtual void TearDown();

    protected: virtual void Unload();

    protected: virtual void Load(const std::string &_worldFilename);

    protected: virtual void Load(const std::string &_worldFilename,
                                 bool _paused);

    protected: virtual void Load(const std::string &_worldFilename,
                                 bool _paused, const std::string &_physics,
                          const std::vector<std::string> &_systemPlugins = {});

    protected: virtual void LoadArgs(const std::string &_args);

    protected: void RunServer(const std::vector<std::string> &_args);

    protected: rendering::ScenePtr GetScene(
                   const std::string &_sceneName = "default");

    protected: void OnStats(ConstWorldStatisticsPtr &_msg);

    protected: void SetPause(bool _pause);

    protected: double GetPercentRealTime() const;

    protected: void OnPose(ConstPosesStampedPtr &_msg);

    protected: ignition::math::Pose3d EntityPose(
                   const std::string &_name);

    protected: bool HasEntity(const std::string &_name);

    protected: void PrintImage(const std::string &_name, unsigned char **_image,
                  unsigned int _width, unsigned int _height,
                  unsigned int _depth);

    protected: void PrintScan(const std::string &_name, double *_scan,
                              unsigned int _sampleCount);

    protected: void FloatCompare(float *_scanA, float *_scanB,
                   unsigned int _sampleCount, float &_diffMax,
                   float &_diffSum, float &_diffAvg);

    protected: void DoubleCompare(double *_scanA, double *_scanB,
                   unsigned int _sampleCount, double &_diffMax,
                   double &_diffSum, double &_diffAvg);

    protected: void ImageCompare(unsigned char *_imageA,
                   unsigned char *_imageB,
                   unsigned int _width, unsigned int _height,
                   unsigned int _depth,
                   unsigned int &_diffMax, unsigned int &_diffSum,
                   double &_diffAvg);

    private: void OnNewFrame(const unsigned char *_image,
                             unsigned int _width, unsigned int _height,
                             unsigned int _depth,
                             const std::string &/*_format*/);

    protected: void GetFrame(const std::string &_cameraName,
                   unsigned char **_imgData, unsigned int &_width,
                   unsigned int &_height);

    protected: physics::ModelPtr SpawnModel(const msgs::Model &_msg);

    protected: template<typename T>
      static void CheckPointer(boost::shared_ptr<T> _ptr)
      {
        ASSERT_TRUE(_ptr != NULL);
      }

    protected: void SpawnCamera(const std::string &_modelName,
                   const std::string &_cameraName,
                   const ignition::math::Vector3d &_pos =
                   ignition::math::Vector3d::Zero,
                   const ignition::math::Vector3d &_rpy =
                   ignition::math::Vector3d::Zero,
                   unsigned int _width = 320, unsigned int _height = 240,
                   double _rate = 25,
                   const std::string &_noiseType = "",
                   double _noiseMean = 0.0, double _noiseStdDev = 0.0,
                   bool _distortion = false, double _distortionK1 = 0.0,
                   double _distortionK2 = 0.0, double _distortionK3 = 0.0,
                   double _distortionP1 = 0.0, double _distortionP2 = 0.0,
                   double _cx = 0.5, double _cy = 0.5);

    protected: void SpawnWideAngleCamera(const std::string &_modelName,
                  const std::string &_cameraName,
                  const ignition::math::Vector3d &_pos =
                  ignition::math::Vector3d::Zero,
                  const ignition::math::Vector3d &_rpy =
                  ignition::math::Vector3d::Zero,
                  unsigned int _width = 320,
                  unsigned int _height = 240,
                  double _rate = 25,
                  const double _hfov = 60,
                  const std::string &_lensType = "stereographic",
                  const bool _scaleToHfov = true,
                  const double _cutoffAngle = 3.1415,
                  const double _envTextureSize = 512,
                  const double _c1 = 1.05, const double _c2 = 4,
                  const double _f = 1.0,
                  const std::string &_fun = "tan");

    protected: void SpawnRaySensor(const std::string &_modelName,
                   const std::string &_raySensorName,
                   const ignition::math::Vector3d &_pos =
                   ignition::math::Vector3d::Zero,
                   const ignition::math::Vector3d &_rpy =
                   ignition::math::Vector3d::Zero,
                   double _hMinAngle = -2.0, double _hMaxAngle = 2.0,
                   double _vMinAngle = -1.0, double _vMaxAngle = 1.0,
                   double _minRange = 0.08, double _maxRange = 10,
                   double _rangeResolution = 0.01, unsigned int _samples = 640,
                   unsigned int _vSamples = 1, double _hResolution = 1.0,
                   double _vResolution = 1.0,
                   const std::string &_noiseType = "", double _noiseMean = 0.0,
                   double _noiseStdDev = 0.0);

    protected: sensors::SonarSensorPtr SpawnSonar(const std::string &_modelName,
                   const std::string &_sonarName,
                   const ignition::math::Pose3d &_pose,
                   const double _minRange,
                   const double _maxRange,
                   const double _radius);

    protected: void SpawnGpuRaySensor(const std::string &_modelName,
                   const std::string &_raySensorName,
                   const ignition::math::Vector3d &_pos =
                   ignition::math::Vector3d::Zero,
                   const ignition::math::Vector3d &_rpy =
                   ignition::math::Vector3d::Zero,
                   double _hMinAngle = -2.0, double _hMaxAngle = 2.0,
                   double _minRange = 0.08, double _maxRange = 10,
                   double _rangeResolution = 0.01, unsigned int _samples = 640,
                   const std::string &_noiseType = "", double _noiseMean = 0.0,
                   double _noiseStdDev = 0.0);

    protected: void SpawnGpuRaySensorVertical(const std::string &_modelName,
                   const std::string &_raySensorName,
                   const ignition::math::Vector3d &_pos,
                   const ignition::math::Vector3d &_rpy,
                   double _hMinAngle = -2.0, double _hMaxAngle = 2.0,
                   double _vMinAngle = -1.0, double _vMaxAngle = 1.0,
                   double _minRange = 0.08, double _maxRange = 10,
                   double _rangeResolution = 0.01, unsigned int _samples = 640,
                   unsigned int _vSamples = 1, double _hResolution = 1.0,
                   double _vResolution = 1.0,
                   const std::string &_noiseType = "", double _noiseMean = 0.0,
                   double _noiseStdDev = 0.0);

    protected: void SpawnDepthCameraSensor(const std::string &_modelName,
                   const std::string &_cameraName,
                   const ignition::math::Vector3d &_pos =
                   ignition::math::Vector3d::Zero,
                   const ignition::math::Vector3d &_rpy =
                   ignition::math::Vector3d::Zero,
                   const unsigned int _width = 320,
                   const unsigned int _height = 240,
                   const double _rate = 25, const double _near = 0.1,
                   const double _far = 10);

    protected: void SpawnImuSensor(const std::string &_modelName,
                   const std::string &_imuSensorName,
                   const ignition::math::Vector3d &_pos =
                   ignition::math::Vector3d::Zero,
                   const ignition::math::Vector3d &_rpy =
                   ignition::math::Vector3d::Zero,
                   const std::string &_noiseType = "",
                   double _rateNoiseMean = 0.0, double _rateNoiseStdDev = 0.0,
                   double _rateBiasMean = 0.0, double _rateBiasStdDev = 0.0,
                   double _accelNoiseMean = 0.0, double _accelNoiseStdDev = 0.0,
                   double _accelBiasMean = 0.0, double _accelBiasStdDev = 0.0);

    protected: void SpawnUnitContactSensor(const std::string &_name,
                   const std::string &_sensorName,
                   const std::string &_collisionType,
                   const ignition::math::Vector3d &_pos =
                   ignition::math::Vector3d::Zero,
                   const ignition::math::Vector3d &_rpy =
                   ignition::math::Vector3d::Zero,
                   bool _static = false);

    protected: void SpawnUnitImuSensor(const std::string &_name,
                   const std::string &_sensorName,
                   const std::string &_collisionType,
                   const std::string &_topic,
                   const ignition::math::Vector3d &_pos =
                   ignition::math::Vector3d::Zero,
                   const ignition::math::Vector3d &_rpy =
                   ignition::math::Vector3d::Zero,
                   bool _static = false);

    protected: void SpawnUnitAltimeterSensor(const std::string &_name,
                   const std::string &_sensorName,
                   const std::string &_collisionType,
                   const std::string &_topic,
                   const ignition::math::Vector3d &_pos =
                   ignition::math::Vector3d::Zero,
                   const ignition::math::Vector3d &_rpy =
                   ignition::math::Vector3d::Zero,
                   bool _static = false);

    protected: void SpawnUnitMagnetometerSensor(const std::string &_name,
                   const std::string &_sensorName,
                   const std::string &_collisionType,
                   const std::string &_topic,
                   const ignition::math::Vector3d &_pos =
                   ignition::math::Vector3d::Zero,
                   const ignition::math::Vector3d &_rpy =
                   ignition::math::Vector3d::Zero,
                   bool _static = false);

    private: void launchTimeoutFailure(const char *_logMsg,
                                       const int _timeoutCS);

    protected: void SpawnWirelessTransmitterSensor(const std::string &_name,
                   const std::string &_sensorName,
                   const ignition::math::Vector3d &_pos,
                   const ignition::math::Vector3d &_rpy,
                   const std::string &_essid,
                   double _freq,
                   double _power,
                   double _gain,
                   bool _visualize = true);

    protected: void SpawnWirelessReceiverSensor(const std::string &_name,
                   const std::string &_sensorName,
                   const ignition::math::Vector3d &_pos,
                   const ignition::math::Vector3d &_rpy,
                   double _minFreq,
                   double _maxFreq,
                   double _power,
                   double _gain,
                   double _sensitivity,
                   bool _visualize = true);

    protected: void WaitUntilEntitySpawn(const std::string &_name,
                                       unsigned int _sleepEach,
                                       int _retries);

    protected: void WaitUntilSensorSpawn(const std::string &_name,
                                         unsigned int _sleepEach,
                                         int _retries);

    protected: void WaitUntilIteration(const uint32_t _goalIteration,
                                       const int _sleepEach,
                                       const int _retries) const;

    protected: void WaitUntilSimTime(const common::Time &_goalTime,
                                     const int _ms,
                                     const int _maxRetries) const;

    protected: void SpawnLight(const std::string &_name,
                   const std::string &_type,
                   const ignition::math::Vector3d &_pos =
                   ignition::math::Vector3d::Zero,
                   const ignition::math::Vector3d &_rpy =
                   ignition::math::Vector3d::Zero,
                   const ignition::math::Color &_diffuse =
                                                ignition::math::Color::White,
                   const ignition::math::Color &_specular =
                                                ignition::math::Color::White,
                   const ignition::math::Vector3d &_direction =
                                               -ignition::math::Vector3d::UnitZ,
                   const double _attenuationRange = 20,
                   const double _attenuationConstant = 0.5,
                   const double _attenuationLinear = 0.01,
                   const double _attenuationQuadratic = 0.001,
                   const double _spotInnerAngle = 0,
                   const double _spotOuterAngle = 0,
                   const double _spotFallOff = 0,
                   const bool _castShadows = true);

    protected: void SpawnCylinder(const std::string &_name,
                   const ignition::math::Vector3d &_pos =
                   ignition::math::Vector3d::Zero,
                   const ignition::math::Vector3d &_rpy =
                   ignition::math::Vector3d::Zero,
                   bool _static = false);

    protected: void SpawnSphere(const std::string &_name,
                   const ignition::math::Vector3d &_pos,
                   const ignition::math::Vector3d &_rpy,
                   bool _wait = true, bool _static = false);

    protected: void SpawnSphere(const std::string &_name,
                   const ignition::math::Vector3d &_pos,
                   const ignition::math::Vector3d &_rpy,
                   const ignition::math::Vector3d &_cog,
                   double _radius = 1.0,
                   bool _wait = true, bool _static = false);

    protected: void SpawnBox(const std::string &_name,
                   const ignition::math::Vector3d &_size =
                   ignition::math::Vector3d::One,
                   const ignition::math::Vector3d &_pos =
                   ignition::math::Vector3d::Zero,
                   const ignition::math::Vector3d &_rpy =
                   ignition::math::Vector3d::Zero,
                   bool _static = false);

    protected: void SpawnTrimesh(const std::string &_name,
                   const std::string &_modelPath,
                   const ignition::math::Vector3d &_scale =
                   ignition::math::Vector3d::One,
                   const ignition::math::Vector3d &_pos =
                   ignition::math::Vector3d::Zero,
                   const ignition::math::Vector3d &_rpy =
                   ignition::math::Vector3d::Zero,
                   bool _static = false);

    protected: void SpawnEmptyLink(const std::string &_name,
                   const ignition::math::Vector3d &_pos =
                   ignition::math::Vector3d::Zero,
                   const ignition::math::Vector3d &_rpy =
                   ignition::math::Vector3d::Zero,
                   bool _static = false);

    protected: void SpawnModel(const std::string &_filename);

    protected: void SpawnSDF(const std::string &_sdf);

    protected: void LoadPlugin(const std::string &_filename,
                               const std::string &_name);

    protected: physics::ModelPtr GetModel(const std::string &_name);

    protected: void RemoveModel(const std::string &_name);

    protected: void RemovePlugin(const std::string &_name);

    protected: void GetMemInfo(double &_resident, double &_share);

    protected: std::string GetUniqueString(const std::string &_prefix);

    protected: void Record(const std::string &_name, const double _data);

    protected: void Record(const std::string &_prefix,
                           const ignition::math::SignalStats &_stats);

    protected: void Record(const std::string &_prefix,
                           const ignition::math::Vector3Stats &_stats);

    protected: Server *server;

    protected: boost::thread *serverThread;

    protected: transport::NodePtr node;

    protected: transport::SubscriberPtr poseSub;

    protected: transport::SubscriberPtr statsSub;

    protected: transport::PublisherPtr factoryPub;

    protected: transport::PublisherPtr requestPub;

    protected: std::map<std::string, ignition::math::Pose3d> poses;

    protected: std::mutex receiveMutex;

    private: unsigned char **imgData;

    private: int gotImage;

    protected: common::Time simTime, realTime, pauseTime;

    private: double percentRealTime;

    private: bool paused;

    private: bool serverRunning;

    private: int uniqueCounter;
  };

  class GAZEBO_VISIBLE RenderingFixture : public ServerFixture
  {
    // Documentation inherited.
    public: virtual void SetUp();

    // Documentation inherited.
    protected: virtual void Unload();
  };
}       // namespace gazebo
#endif  // define _GAZEBO_SERVER_FIXTURE_HH_