void LimitSwitch::Load(physics::ModelPtr model, sdf::ElementPtr sdf) {
  this->model = model;

  // Parse SDF properties
  if (sdf->HasElement("topic")) {
    topic = sdf->Get<std::string>("topic");
  } else {
    topic = "~/" + sdf->GetAttribute("name")->GetAsString();
  }
  std::string type = sdf->Get<std::string>("type");

  gzmsg << "Initializing limit switch: " << topic << " type=" << type
        << std::endl;
  if (type == "internal") {
    ls = new InternalLimitSwitch(model, sdf);
  } else if (type == "external") {
    ls = new ExternalLimitSwitch(sdf);
  } else {
    gzerr << "WARNING: unsupported limit switch type " << type;
  }

  // Connect to Gazebo transport for messaging
  std::string scoped_name =
      model->GetWorld()->GetName() + "::" + model->GetScopedName();
  boost::replace_all(scoped_name, "::", "/");
  node = transport::NodePtr(new transport::Node());
  node->Init(scoped_name);
  pub = node->Advertise<msgs::Bool>(topic);

  // Connect to the world update event.
  // This will trigger the Update function every Gazebo iteration
  updateConn = event::Events::ConnectWorldUpdateBegin(
      boost::bind(&LimitSwitch::Update, this, _1));
}

void GazeboLidarPlugin::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf)
{
  if(kPrintOnPluginLoad) {
    gzdbg << __FUNCTION__ << "() called." << std::endl;
  }

  // Get then name of the parent sensor
  this->parentSensor =
    std::dynamic_pointer_cast<sensors::RaySensor>(_parent);

  if (!this->parentSensor)
    gzthrow("RayPlugin requires a Ray Sensor as its parent");

  this->world = physics::get_world(this->parentSensor->WorldName());

  this->newLaserScansConnection =
    this->parentSensor->LaserShape()->ConnectNewLaserScans(
      boost::bind(&GazeboLidarPlugin::OnNewLaserScans, this));

  if (_sdf->HasElement("robotNamespace"))
    namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>();
  else
    gzwarn << "[gazebo_lidar_plugin] Please specify a robotNamespace.\n";

  node_handle_ = transport::NodePtr(new transport::Node());
  node_handle_->Init(namespace_);

  const string scopedName = _parent->ParentName();

  string topicName = "~/" + scopedName + "/lidar";
  boost::replace_all(topicName, "::", "/");

  lidar_pub_ = node_handle_->Advertise<lidar_msgs::msgs::lidar>(topicName, 10);
}

PositionMotor::PositionMotor(gz::physics::ModelPtr model, std::string partId,
							 std::string motorId, sdf::ElementPtr motor):
	JointMotor(model, partId, motorId, motor, 1),
	positionTarget_(0),
	noise_(0) {
	// Retrieve upper / lower limit from joint set in parent constructor
	// Truncate ranges to [-pi, pi]
	upperLimit_ = fmin(M_PI, joint_->GetUpperLimit(0).Radian());
	lowerLimit_ = fmax(-M_PI, joint_->GetLowerLimit(0).Radian());
	fullRange_ = (upperLimit_ - lowerLimit_ + 1e-12) >= (2 * M_PI);

	if (motor->HasElement("rv:pid")) {
		auto pidElem = motor->GetElement("rv:pid");
		pid_ = Motor::createPid(pidElem);
	}

	auto noiseParam = motor->GetAttribute("noise");
	if (noiseParam) {
		noiseParam->Get(noise_);
	}

	// I've asked this question at the Gazebo forums:
	// http://answers.gazebosim.org/question/9071/joint-target-velocity-with-maximum-force/
	// Until it is answered I'm resorting to calling ODE functions directly to get this
	// to work. This will result in some deprecation warnings.
	// It has the added benefit of not requiring the world update connection though.
//	updateConnection_ = gz::event::Events::ConnectWorldUpdateBegin(boost::bind(
//			&PositionMotor::OnUpdate, this, _1));

	double maxEffort = joint_->GetEffortLimit(0);
	joint_->SetParam("fmax", 0, maxEffort);
}

void RayPlugin::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf)
{
  // Get then name of the parent sensor
  this->parentSensor =
    boost::dynamic_pointer_cast<sensors::RaySensor>(_parent);

  if (!this->parentSensor)
    gzthrow("RayPlugin requires a Ray Sensor as its parent");

  this->world = physics::get_world(this->parentSensor->GetWorldName());

  this->newLaserScansConnection =
    this->parentSensor->GetLaserShape()->ConnectNewLaserScans(
      boost::bind(&RayPlugin::OnNewLaserScans, this));

  if (_sdf->HasElement("robotNamespace"))
    namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>();
  else
    gzwarn << "[gazebo_lidar_plugin] Please specify a robotNamespace.\n";

  node_handle_ = transport::NodePtr(new transport::Node());
  node_handle_->Init(namespace_);

  lidar_pub_ = node_handle_->Advertise<lidar_msgs::msgs::lidar>("lidar", 10);
}

void Rangefinder::Load(physics::ModelPtr model, sdf::ElementPtr sdf) {
  this->model = model;

  // Parse SDF properties
  sensor = std::dynamic_pointer_cast<sensors::SonarSensor>(
      sensors::get_sensor(sdf->Get<std::string>("sensor")));
  if (sdf->HasElement("topic")) {
    topic = sdf->Get<std::string>("topic");
  } else {
    topic = "~/" + sdf->GetAttribute("name")->GetAsString();
  }

  gzmsg << "Initializing rangefinder: " << topic << " sensor=" << sensor->Name()
        << std::endl;

  // Connect to Gazebo transport for messaging
  std::string scoped_name =
      model->GetWorld()->GetName() + "::" + model->GetScopedName();
  boost::replace_all(scoped_name, "::", "/");
  node = transport::NodePtr(new transport::Node());
  node->Init(scoped_name);
  pub = node->Advertise<msgs::Float64>(topic);

  // Connect to the world update event.
  // This will trigger the Update function every Gazebo iteration
  updateConn = event::Events::ConnectWorldUpdateBegin(
      boost::bind(&Rangefinder::Update, this, _1));
}

void PneumaticPiston::Load(physics::ModelPtr model, sdf::ElementPtr sdf) {
  this->model = model;
  signal = 0;

  // Parse SDF properties
  joint = model->GetJoint(sdf->Get<std::string>("joint"));
  if (sdf->HasElement("topic")) {
    topic = sdf->Get<std::string>("topic");
  } else {
    topic = "~/"+sdf->GetAttribute("name")->GetAsString();
  }

  forward_force = sdf->Get<double>("forward-force");
  reverse_force = sdf->Get<double>("reverse-force");

  if (sdf->HasElement("direction") && sdf->Get<std::string>("direction") == "reversed") {
    forward_force = -forward_force;
    reverse_force = -reverse_force;
  }

  gzmsg << "Initializing piston: " << topic << " joint=" << joint->GetName()
        << " forward_force=" << forward_force
        << " reverse_force=" << reverse_force<< std::endl;

  // Connect to Gazebo transport for messaging
  std::string scoped_name = model->GetWorld()->GetName()+"::"+model->GetScopedName();
  boost::replace_all(scoped_name, "::", "/");
  node = transport::NodePtr(new transport::Node());
  node->Init(scoped_name);
  sub = node->Subscribe(topic, &PneumaticPiston::Callback, this);

  // Connect to the world update event.
  // This will trigger the Update function every Gazebo iteration
  updateConn = event::Events::ConnectWorldUpdateBegin(boost::bind(&PneumaticPiston::Update, this, _1));
}

void DCMotor::Load(physics::ModelPtr model, sdf::ElementPtr sdf) {
  this->model = model;
  signal = 0;

  // Parse SDF properties
  joint = model->GetJoint(sdf->Get<std::string>("joint"));
  if (sdf->HasElement("topic")) {
    topic = sdf->Get<std::string>("topic");
  } else {
    topic = "~/"+sdf->GetAttribute("name")->GetAsString();
  }

  if (sdf->HasElement("multiplier")) {
    multiplier = sdf->Get<double>("multiplier");
  } else {
    multiplier = 1;
  }

  gzmsg << "Initializing motor: " << topic << " joint=" << joint->GetName()
        << " multiplier=" << multiplier << std::endl;

  // Connect to Gazebo transport for messaging
  std::string scoped_name = model->GetWorld()->GetName()+"::"+model->GetScopedName();
  boost::replace_all(scoped_name, "::", "/");
  node = transport::NodePtr(new transport::Node());
  node->Init(scoped_name);
  sub = node->Subscribe(topic, &DCMotor::Callback, this);

  // Connect to the world update event.
  // This will trigger the Update function every Gazebo iteration
  updateConn = event::Events::ConnectWorldUpdateBegin(boost::bind(&DCMotor::Update, this, _1));
}

void Servo::Load(physics::ModelPtr model, sdf::ElementPtr sdf) {
  this->model = model;
  signal = 0;

  // parse SDF Properries
  joint = model->GetJoint(sdf->Get<std::string>("joint"));
  if (sdf->HasElement("topic")) {
    topic = sdf->Get<std::string>("topic");
  } else {
    topic = "~/" + sdf->GetAttribute("name")->GetAsString();
  }

  if (sdf->HasElement("torque")) {
    torque = sdf->Get<double>("torque");
  } else {
    torque = 5;
  }

  gzmsg << "initializing servo: " << topic << " joint=" << joint->GetName()
        << " torque=" << torque << std::endl;

  // Connect to Gazebo transport for messaging
  std::string scoped_name =
      model->GetWorld()->GetName() + "::" + model->GetScopedName();
  boost::replace_all(scoped_name, "::", "/");
  node = transport::NodePtr(new transport::Node());
  node->Init(scoped_name);
  sub = node->Subscribe(topic, &Servo::Callback, this);

  // connect to the world update event
  // this will call update every iteration
  updateConn = event::Events::ConnectWorldUpdateBegin(
      boost::bind(&Servo::Update, this, _1));
}

void Potentiometer::Load(physics::ModelPtr model, sdf::ElementPtr sdf) {
  this->model = model;

  // Parse SDF properties
  joint = model->GetJoint(sdf->Get<std::string>("joint"));
  if (sdf->HasElement("topic")) {
    topic = sdf->Get<std::string>("topic");
  } else {
    topic = "~/"+sdf->GetAttribute("name")->GetAsString();
  }

  if (sdf->HasElement("units")) {
    radians = sdf->Get<std::string>("units") != "degrees";
  } else {
    radians = true;
  }

  gzmsg << "Initializing potentiometer: " << topic << " joint=" << joint->GetName()
        << " radians=" << radians << std::endl;

  // Connect to Gazebo transport for messaging
  std::string scoped_name = model->GetWorld()->GetName()+"::"+model->GetScopedName();
  boost::replace_all(scoped_name, "::", "/");
  node = transport::NodePtr(new transport::Node());
  node->Init(scoped_name);
  pub = node->Advertise<msgs::Float64>(topic);

  // Connect to the world update event.
  // This will trigger the Update function every Gazebo iteration
  updateConn = event::Events::ConnectWorldUpdateBegin(boost::bind(&Potentiometer::Update, this, _1));
}

void BasicSonar::Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sdf)
{
    sensor_ = boost::dynamic_pointer_cast<sensors::RaySensor>(_sensor);
    if (!sensor_)
    {
        gzthrow("BasicSonar requires a Ray Sensor as its parent");
        return;
    }

    pixels_per_line = std::max(sensor_->GetRangeCount(), 1);

    gaussian_kernel = 0;
    if (_sdf->HasElement("gaussian_kernel"))
        gaussian_kernel = _sdf->Get<double>("gaussian_kernel");

    base_water_noise = 0;
    if (_sdf->HasElement("base_water_noise"))
        base_water_noise = _sdf->Get<double>("base_water_noise");

    default_scan_retro = 0;
    if (_sdf->HasElement("default_scan_retro"))
        default_scan_retro = _sdf->Get<double>("default_scan_retro");

    interpolation_limit = 0.5;
    if (_sdf->HasElement("interpolation_limit"))
        interpolation_limit = _sdf->Get<double>("interpolation_limit");

    if (_sdf->HasElement("service_name"))
        service_name = _sdf->Get<std::string>("service_name");

    if (service_name.empty())
    {
        gzthrow("BasicSonar requires a unique service name");
        return;
    }



    std::string worldName = sensor_->GetWorldName();
    world = physics::get_world(worldName);


    if (!ros::isInitialized())
    {
        ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. "
                         << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
        return;
    }

    node_handle_ = new ros::NodeHandle("gazebo/basic_sonar/");
    service = node_handle_->advertiseService(service_name.c_str(), &BasicSonar::getSonarScan, this);

    sensor_->SetActive(true);

    ROS_INFO("loaded sonar plugin");
}

/** on loading of the plugin
 * @param _parent Parent Model
 */
void Puck::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
{
  printf("Loading Puck Plugin of model %s\n", _parent->GetName().c_str());
  // Store the pointer to the model
  this->model_ = _parent;  

  // Listen to the update event. This event is broadcast every
  // simulation iteration.
  this->update_connection_ = event::Events::ConnectWorldUpdateBegin(boost::bind(&Puck::OnUpdate, this, _1));

  // Create the communication Node for communication with fawkes
  this->node_ = transport::NodePtr(new transport::Node());
  // the namespace is set to the world name!
  this->node_->Init(model_->GetWorld()->GetName());

  // register visual publisher
  this->visual_pub_ = this->node_->Advertise<msgs::Visual>("~/visual");
  
  // initialize without rings or cap
  this->ring_count_ = 0;
  this->have_cap = false;
  this->announced_ = false;
  
  this->new_puck_publisher = this->node_->Advertise<gazsim_msgs::NewPuck>("~/new_puck");
  
  // subscribe for puck commands
  this->command_subscriber = this->node_->Subscribe(std::string("~/pucks/cmd"), &Puck::on_command_msg, this);
  
  // publisher for workpiece command results
  this->workpiece_result_pub_ = node_->Advertise<gazsim_msgs::WorkpieceResult>("~/pucks/cmd/result");
  
  if (!_sdf->HasElement("baseColor")) {
    printf("SDF for base has no baseColor configured, defaulting to RED!\n");
    base_color_ = gazsim_msgs::Color::RED;
  } else {
    std::string config_color = _sdf->GetElement("baseColor")->Get<std::string>();
    if (config_color == "RED") {
      base_color_ = gazsim_msgs::Color::RED;
    } else if (config_color == "BLACK") {
      base_color_ = gazsim_msgs::Color::BLACK;
    } else if (config_color == "SILVER") {
      base_color_ = gazsim_msgs::Color::SILVER;
    } else {
      printf("SDF for base has no baseColor configured, defaulting to RED!\n");
      base_color_ = gazsim_msgs::Color::RED;
    }
    printf("Base spawns in color %s\n", config_color.c_str());
  }
  
  delivery_pub_ = node_->Advertise<llsf_msgs::SetOrderDeliveredByColor>(TOPIC_SET_ORDER_DELIVERY_BY_COLOR);
}

void GazeboAltimeterPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
{
  // Configure Gazebo Integration
  model_ = _model;
  world_ = model_->GetWorld();
  namespace_.clear();
  if (_sdf->HasElement("robotNamespace"))
    namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>();
  else
    gzerr << "[gazebo_altimeter_plugin] Please specify a robotNamespace.\n";
  if (_sdf->HasElement("linkName"))
    link_name_ = _sdf->GetElement("linkName")->Get<std::string>();
  else
    gzerr << "[gazebo_altimeter_plugin] Please specify a linkName.\n";
  link_ = model_->GetLink(link_name_);
  if (link_ == NULL)
    gzthrow("[gazebo_altimeter_plugin] Couldn't find specified link \"" << link_name_ << "\".");
  // Connect to the Gazebo Update
  this->updateConnection_ = event::Events::ConnectWorldUpdateBegin(boost::bind(&GazeboAltimeterPlugin::OnUpdate, this, _1));
  frame_id_ = link_name_;

  // load params from xacro
  getSdfParam<std::string>(_sdf, "altimeterTopic", alt_topic_, "altimeter");
  getSdfParam<double>(_sdf, "altimeterMinRange", min_range_, 0.381);
  getSdfParam<double>(_sdf, "altimeterMaxRange", max_range_, 6.45);
  getSdfParam<double>(_sdf, "altimeterErrorStdev", error_stdev_, 0.10);
  getSdfParam<double>(_sdf, "altimeterFOV", field_of_view_, 1.107);
  getSdfParam<double>(_sdf, "altimeterPublishRate", pub_rate_, 20.0);
  getSdfParam<bool>(_sdf, "altimeterNoiseOn", alt_noise_on_, true);
  getSdfParam<bool>(_sdf, "publishFloat32", publish_float_, false);
  last_time_ = world_->GetSimTime();

  // Configure ROS Integration
  node_handle_ = new ros::NodeHandle(namespace_);
  if(publish_float_)
      alt_pub_ = node_handle_->advertise<std_msgs::Float32>(alt_topic_, 10);
  else
      alt_pub_ = node_handle_->advertise<sensor_msgs::Range>(alt_topic_, 10);

  // Fill static members of alt message.
  alt_message_.header.frame_id = frame_id_;
  alt_message_.max_range = max_range_;
  alt_message_.min_range = min_range_;
  alt_message_.field_of_view = field_of_view_;
  alt_message_.radiation_type = sensor_msgs::Range::ULTRASOUND;

  // Configure Noise
  random_generator_= std::default_random_engine(std::chrono::system_clock::now().time_since_epoch().count());
  standard_normal_distribution_ = std::normal_distribution<double>(0.0, error_stdev_);
}

////////////////////////////////////////////////////////////////////////////////
// Load the controller
void GazeboTreasure::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
{
  model_ = _model;
  world_ = _model->GetWorld();
  link_ = _model->GetLink();
  link_name_ = link_->GetName();
  namespace_.clear();

  vel_x = vel_y = vel_yaw = 0;
  static_object_ = false;
  last_time_ = world_->GetSimTime();
  terminated_ = false;

  // load parameters from sdf
  if (_sdf->HasElement("robotNamespace")) namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>();

  if (_sdf->HasElement("bodyName") && _sdf->GetElement("bodyName")->GetValue())
    {
      link_name_ = _sdf->GetElement("bodyName")->Get<std::string>();
      link_ = _model->GetLink(link_name_);
    }

  if (_sdf->HasElement("staticObject") && _sdf->GetElement("staticObject")->GetValue())
    {
      static_object_ = _sdf->GetElement("staticObject")->Get<std::string>() == "true"?true:false;
    }

  // boost::random::mt19937 rng;
  boost::random::random_device rng;
  boost::random::uniform_int_distribution<> x_rand(-40, 40);
  boost::random::uniform_int_distribution<> y_rand(-20, 20);
  double x = x_rand(rng);
  double y = y_rand(rng);
  model_->SetLinkWorldPose(math::Pose(x, y, 0.2, 0, 0, 0), link_);

  if (!link)
  {
    ROS_FATAL("gazebo_ros_baro plugin error: bodyName: %s does not exist\n", link_name_.c_str());
    return;
  }


  // Make sure the ROS node for Gazebo has already been initialized
  if (!ros::isInitialized())
  {
    ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. "
                     << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
    return;
  }

  node_handle_ = new ros::NodeHandle(namespace_);
  pub_score_ = node_handle_->advertise<std_msgs::String>("score", 1, true);  // set latch true
  ros::NodeHandle param_handle(*node_handle_, "controller");



  update_connection_ = event::Events::ConnectWorldUpdateBegin(
                                                              boost::bind(&GazeboTreasure::Update, this));
}

	bool AllWheelSteeringPlugin::FindJointByParam(sdf::ElementPtr _sdf, physics::JointPtr &_joint, std::string _param) {
		if (!_sdf->HasElement(_param)) {
		  gzerr << "param [" << _param << "] not found\n";
		  return false;
		}
		else {
			_joint = this->model->GetJoint(_sdf->GetElement(_param)->GetValueString());
	
			if (!_joint) {
				gzerr << "joint by name [" << _sdf->GetElement(_param)->GetValueString() << "] not found in model\n";
				return false;
			}
		}
		return true;
	}

    void GazeboWindPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) {
        // Store the pointer to the model.
        model_ = _model;
        world_ = model_->GetWorld();

        double wind_gust_start = kDefaultWindGustStart;
        double wind_gust_duration = kDefaultWindGustDuration;

        if (_sdf->HasElement("robotNamespace"))
            namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>();
        else
            gzerr << "[gazebo_wind_plugin] Please specify a robotNamespace.\n";
        node_handle_ = new ros::NodeHandle(namespace_);

        if (_sdf->HasElement("xyzOffset"))
            xyz_offset_ = _sdf->GetElement("xyzOffset")->Get<math::Vector3>();
        else
            gzerr << "[gazebo_wind_plugin] Please specify a xyzOffset.\n";

        getSdfParam<std::string>(_sdf, "windPubTopic", wind_pub_topic_, "/" + namespace_ + wind_pub_topic_);
        getSdfParam<std::string>(_sdf, "frameId", frame_id_, frame_id_);
        getSdfParam<std::string>(_sdf, "linkName", link_name_, link_name_);
        // Get the wind params from SDF.
        getSdfParam<double>(_sdf, "windForceMean", wind_force_mean_, wind_force_mean_);
        getSdfParam<double>(_sdf, "windForceVariance", wind_force_variance_, wind_force_variance_);
        getSdfParam<math::Vector3>(_sdf, "windDirection", wind_direction_, wind_direction_);
        // Get the wind gust params from SDF.
        getSdfParam<double>(_sdf, "windGustStart", wind_gust_start, wind_gust_start);
        getSdfParam<double>(_sdf, "windGustDuration", wind_gust_duration, wind_gust_duration);
        getSdfParam<double>(_sdf, "windGustForceMean", wind_gust_force_mean_, wind_gust_force_mean_);
        getSdfParam<double>(_sdf, "windGustForceVariance", wind_gust_force_variance_, wind_gust_force_variance_);
        getSdfParam<math::Vector3>(_sdf, "windGustDirection", wind_gust_direction_, wind_gust_direction_);

        wind_direction_.Normalize();
        wind_gust_direction_.Normalize();
        wind_gust_start_ = common::Time(wind_gust_start);
        wind_gust_end_ = common::Time(wind_gust_start + wind_gust_duration);

        link_ = model_->GetLink(link_name_);
        if (link_ == NULL) gzthrow("[gazebo_wind_plugin] Couldn't find specified link \"" << link_name_ << "\".");


        // Listen to the update event. This event is broadcast every
        // simulation iteration.
        update_connection_ = event::Events::ConnectWorldUpdateBegin(boost::bind(&GazeboWindPlugin::OnUpdate, this, _1));

        wind_pub_ = node_handle_->advertise<geometry_msgs::WrenchStamped>(wind_pub_topic_, 10);
    }

////////////////////////////////////////////////////////////////////////////////
// Load the controller
void GazeboRosOpenniKinect::Load(sensors::SensorPtr _parent,
		sdf::ElementPtr _sdf) {

	std::cout << "LOAD OPENNI" << std::endl;
	DepthCameraPlugin::Load(_parent, _sdf);

	// copying from DepthCameraPlugin into GazeboRosCameraUtils
	this->parentSensor_ = this->parentSensor;
	this->width_ = this->width;
	this->height_ = this->height;
	this->depth_ = this->depth;
	this->format_ = this->format;
	this->camera_ = this->depthCamera;

	// using a different default
	if (!_sdf->GetElement("imageTopicName"))
		this->image_topic_name_ = "ir/image_raw";
	if (!_sdf->HasElement("cameraInfoTopicName"))
		this->camera_info_topic_name_ = "ir/camera_info";

	// point cloud stuff
	if (!_sdf->GetElement("pointCloudTopicName"))
		this->point_cloud_topic_name_ = "points";
	else
		this->point_cloud_topic_name_ =
				_sdf->GetElement("pointCloudTopicName")->Get<std::string>();

	// depth image stuff
	this->depth_image_topic_name_ = "mukodik";
//	if (!_sdf->GetElement("depthImageTopicName"))
//		this->depth_image_topic_name_ = "depth/image_raw";
//	else
//		this->depth_image_topic_name_ =
//				_sdf->GetElement("depthImageTopicName")->Get<std::string>();

	if (!_sdf->GetElement("depthImageCameraInfoTopicName"))
		this->depth_image_camera_info_topic_name_ = "depth/camera_info";
	else
		this->depth_image_camera_info_topic_name_ = _sdf->GetElement(
				"depthImageCameraInfoTopicName")->Get<std::string>();

	if (!_sdf->GetElement("pointCloudCutoff"))
		this->point_cloud_cutoff_ = 0.4;
	else
		this->point_cloud_cutoff_ = _sdf->GetElement("pointCloudCutoff")->Get<
				double>();

	load_connection_ = GazeboRosCameraUtils::OnLoad(
			boost::bind(&GazeboRosOpenniKinect::Advertise, this));

	std::cout << "Establishing Update Connection!" << std::endl;

	update_connection_ = event::Events::ConnectPreRender(
			boost::bind(&GazeboRosOpenniKinect::OnUpdate, this));

	GazeboRosCameraUtils::Load(_parent, _sdf);
}

void GazeboMavlinkInterface::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) {
  // Store the pointer to the model.
  model_ = _model;

  world_ = model_->GetWorld();

  namespace_.clear();

  if (_sdf->HasElement("robotNamespace"))
    namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>();
  else
    gzerr << "[gazebo_mavlink_interface] Please specify a robotNamespace.\n";

  node_handle_ = new ros::NodeHandle(namespace_);

  getSdfParam<std::string>(_sdf, "motorSpeedCommandPubTopic", motor_velocity_reference_pub_topic_,
                           motor_velocity_reference_pub_topic_);

  // Listen to the update event. This event is broadcast every
  // simulation iteration.
  updateConnection_ = event::Events::ConnectWorldUpdateBegin(
      boost::bind(&GazeboMavlinkInterface::OnUpdate, this, _1));

  mav_control_sub_ = node_handle_->subscribe(mavlink_control_sub_topic_, 10,
                                           &GazeboMavlinkInterface::MavlinkControlCallback,
                                           this);
  // Subscriber to IMU sensor_msgs::Imu Message.
  imu_sub_ = node_handle_->subscribe(imu_sub_topic_, 10, &GazeboMavlinkInterface::ImuCallback, this);
  
  motor_velocity_reference_pub_ = node_handle_->advertise<mav_msgs::Actuators>(motor_velocity_reference_pub_topic_, 10);
  hil_sensor_pub_ = node_handle_->advertise<mavros_msgs::Mavlink>(hil_sensor_mavlink_pub_topic_, 10);

  _rotor_count = 4;
  last_time_ = world_->GetSimTime();
  last_gps_time_ = world_->GetSimTime();
  double gps_update_interval_ = 200*1000000;  // nanoseconds for 5Hz

  gravity_W_ = world_->GetPhysicsEngine()->GetGravity();

  // Magnetic field data for Zurich from WMM2015 (10^5xnanoTesla (N, E, D))
  
  mag_W_.x = 0.21523;
  mag_W_.y = 0.00771;
  mag_W_.z = 0.42741;

}

////////////////////////////////////////////////////////////////////////////////
// Load the controller
void GazeboPanel::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
{
  gzwarn << "The GazeboPanel plugin is DEPRECATED in ROS hydro." << std::endl;

  model_ = _model;
  world_ = _model->GetWorld();
  link_ = _model->GetLink();
  link_name_ = link_->GetName();
  namespace_.clear();

  terminated_ = false;

  // load parameters from sdf
  if (_sdf->HasElement("robotNamespace")) namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>();

  if (_sdf->HasElement("bodyName") && _sdf->GetElement("bodyName")->GetValue())
    {
      link_name_ = _sdf->GetElement("bodyName")->Get<std::string>();
      link_ = _model->GetLink(link_name_);
    }

  if (_sdf->HasElement("jointName") && _sdf->GetElement("jointName")->GetValue())
    {
      std::string joint_name_ = _sdf->GetElement("jointName")->Get<std::string>();
      joint_ = _model->GetJoint(joint_name_);
    }
  else
    {
      joint_ = _model->GetJoint("stem_joint");
    }

  if (!link)
  {
    ROS_FATAL("gazebo_ros_baro plugin error: bodyName: %s does not exist\n", link_name_.c_str());
    return;
  }


  // Make sure the ROS node for Gazebo has already been initialized
  if (!ros::isInitialized())
  {
    ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. "
                     << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
    return;
  }

  node_handle_ = new ros::NodeHandle(namespace_);
  pub_score_ = node_handle_->advertise<std_msgs::String>("score", 1, true);  // set latch true
  pub_time_ = node_handle_->advertise<std_msgs::String>("remaining_time", 1);
  ros::NodeHandle param_handle(*node_handle_, "controller");



  update_connection_ = event::Events::ConnectWorldUpdateBegin(
                                                              boost::bind(&GazeboPanel::Update, this));
}

void GazeboAirspeedPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) {
  // Store the pointer to the model
  model_ = _model;
  world_ = model_->GetWorld();

  // default params
  namespace_.clear();

  if (_sdf->HasElement("namespace"))
    namespace_ = _sdf->GetElement("namespace")->Get<std::string>();
  else
    gzerr << "[gazebo_imu_plugin] Please specify a namespace.\n";
  nh_ = new ros::NodeHandle(namespace_);

  if (_sdf->HasElement("linkName"))
    link_name_ = _sdf->GetElement("linkName")->Get<std::string>();
  else
    gzerr << "[gazebo_imu_plugin] Please specify a linkName.\n";
  link_ = model_->GetLink(link_name_);
  if (link_ == NULL)
    gzthrow("[gazebo_imu_plugin] Couldn't find specified link \"" << link_name_ << "\".");

  frame_id_ = link_name_;

  getSdfParam<std::string>(_sdf, "airspeedTopic", airspeed_topic_, "airspeed/data");
  getSdfParam<double>(_sdf, "pressureBias", pressure_bias_, 0);
  getSdfParam<double>(_sdf, "pressureNoiseSigma", pressure_noise_sigma_,10);
  getSdfParam<double>(_sdf, "airDensity", rho_, 1.225);
  getSdfParam<double>(_sdf, "maxPressure", max_pressure_, 4000.0);
  getSdfParam<double>(_sdf, "minPressure", min_pressure_, 0.0);


  last_time_ = world_->GetSimTime();

  // Listen to the update event. This event is broadcast every simulation iteration.
  this->updateConnection_ = event::Events::ConnectWorldUpdateBegin(boost::bind(&GazeboAirspeedPlugin::OnUpdate, this, _1));

  airspeed_pub_ = nh_->advertise<sensor_msgs::FluidPressure>(airspeed_topic_, 10);

  // Fill static members of airspeed message.
  airspeed_message_.header.frame_id = frame_id_;
  airspeed_message_.variance = pressure_noise_sigma_*pressure_noise_sigma_;

  standard_normal_distribution_ = std::normal_distribution<double>(0.0, 1.0);
}

void WorldInterface::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
{
  if (m_network!=0)
    return;

  m_network=new yarp::os::Network();

  if (!yarp::os::Network::checkNetwork(GazeboYarpPlugins::yarpNetworkInitializationTimeout)) {
        yError() << "WorldInterface::Load error: yarp network does not seem to be available, is the yarpserver running?";
        return;
  }

  //setting up proxy
  m_proxy.attachWorldPointer(_model->GetWorld());
  m_proxy.attachModelPointer(_model);

  //pass reference to server
  m_wi_server.attachWorldProxy(&m_proxy);

  //Getting .ini configuration file from sdf
  bool configuration_loaded = false;

  if (_sdf->HasElement("yarpConfigurationFile")) {
        std::string ini_file_name = _sdf->Get<std::string>("yarpConfigurationFile");
        std::string ini_file_path = gazebo::common::SystemPaths::Instance()->FindFileURI(ini_file_name);

        if (ini_file_path != "" && m_parameters.fromConfigFile(ini_file_path.c_str())) {
            yInfo() << "Found yarpConfigurationFile: loading from " << ini_file_path ;
            configuration_loaded = true;
        }
  }

  if (!configuration_loaded) {
    yError() << "WorldInterface::Load error could not load configuration file";
    return;
  }

  std::string portname=m_parameters.find("name").asString();
  int synchronous=m_parameters.find("synchro").asInt32();

  if (synchronous)
     m_proxy.setSynchronousMode(true);
  else
     m_proxy.setSynchronousMode(false);

  m_rpcport=new yarp::os::RpcServer();
  m_rpcport->open(portname);

  m_wi_server.yarp().attachAsServer(*m_rpcport);

  // Listen to the update event. This event is broadcast every
  // simulation iteration.
  this->updateConnection = event::Events::ConnectWorldUpdateBegin(
      boost::bind(&WorldInterface::OnUpdate, this, _1));

}