C++ ElementPtr::HasElement方法代码示例

////////////////////////////////////////////////////////////////////////////////
// Load the controller
void GazeboRosForce::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
{
  // Get the world name.
  this->world_ = _model->GetWorld();
  
  // load parameters
  this->robot_namespace_ = "";
  if (_sdf->HasElement("robotNamespace"))
    this->robot_namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>() + "/";

  if (!_sdf->HasElement("bodyName"))
  {
    ROS_FATAL("force plugin missing <bodyName>, cannot proceed");
    return;
  }
  else
    this->link_name_ = _sdf->GetElement("bodyName")->Get<std::string>();

  this->link_ = _model->GetLink(this->link_name_);
  if (!this->link_)
  {
    ROS_FATAL("gazebo_ros_force plugin error: link named: %s does not exist\n",this->link_name_.c_str());
    return;
  }

  if (!_sdf->HasElement("topicName"))
  {
    ROS_FATAL("force plugin missing <topicName>, cannot proceed");
    return;
  }
  else
    this->topic_name_ = _sdf->GetElement("topicName")->Get<std::string>();


  // 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;
  }

  this->rosnode_ = new ros::NodeHandle(this->robot_namespace_);

  // Custom Callback Queue
  ros::SubscribeOptions so = ros::SubscribeOptions::create<geometry_msgs::Wrench>(
    this->topic_name_,1,
    boost::bind( &GazeboRosForce::UpdateObjectForce,this,_1),
    ros::VoidPtr(), &this->queue_);
  this->sub_ = this->rosnode_->subscribe(so);

  // Custom Callback Queue
  this->callback_queue_thread_ = boost::thread( boost::bind( &GazeboRosForce::QueueThread,this ) );

  // New Mechanism for Updating every World Cycle
  // Listen to the update event. This event is broadcast every
  // simulation iteration.
  this->update_connection_ = event::Events::ConnectWorldUpdateBegin(
      boost::bind(&GazeboRosForce::UpdateChild, this));
}

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 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 BatteryConsumer::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
{
    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;
    }
    double default_consume = DEFAULT_CONSUMER_CONSUME;
    std::string name = DEFAULT_CONSUMER_NAME;
    if (_sdf->HasElement("robotNamespace")) namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>();
    if (_sdf->HasElement("default_consume")) default_consume = _sdf->GetElement("default_consume")->Get<double>();
    if (_sdf->HasElement("default_consumer_name")) name = _sdf->GetElement("default_consumer_name")->Get<std::string>();


    default_consumer.first = name;
    default_consumer.second = default_consume;
    consumers.insert(default_consumer);


    this->model = _parent;
    n = ros::NodeHandle(namespace_);


    battery_pub = n.advertise<std_msgs::Float64>("battery_state", 1);

    add_consumer_service = n.advertiseService("add_consumer",&BatteryConsumer::addConsumer,this);


    remove_consumer_service = n.advertiseService("remove_consumer",&BatteryConsumer::removeConsumer,this);
    last_time = model->GetWorld()->GetSimTime().Double();
    this->updateConnection = event::Events::ConnectWorldUpdateBegin(
                                 boost::bind(&BatteryConsumer::OnUpdate, 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 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));
}

////////////////////////////////////////////////////////////////////////////////
// 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));
}

void GazeboRosJointStatePublisher::Load ( physics::ModelPtr _parent, sdf::ElementPtr _sdf ) {
    // Store the pointer to the model
    this->parent_ = _parent;
    this->world_ = _parent->GetWorld();

    this->robot_namespace_ = parent_->GetName ();
    if ( !_sdf->HasElement ( "robotNamespace" ) ) {
        ROS_INFO_NAMED("joint_state_publisher", "GazeboRosJointStatePublisher Plugin missing <robotNamespace>, defaults to \"%s\"",
                   this->robot_namespace_.c_str() );
    } else {
        this->robot_namespace_ = _sdf->GetElement ( "robotNamespace" )->Get<std::string>();
        if ( this->robot_namespace_.empty() ) this->robot_namespace_ = parent_->GetName ();
    }
    if ( !robot_namespace_.empty() ) this->robot_namespace_ += "/";
    rosnode_ = boost::shared_ptr<ros::NodeHandle> ( new ros::NodeHandle ( this->robot_namespace_ ) );

    if ( !_sdf->HasElement ( "jointName" ) ) {
        ROS_ASSERT ( "GazeboRosJointStatePublisher Plugin missing jointNames" );
    } else {
        sdf::ElementPtr element = _sdf->GetElement ( "jointName" ) ;
        std::string joint_names = element->Get<std::string>();
        boost::erase_all ( joint_names, " " );
        boost::split ( joint_names_, joint_names, boost::is_any_of ( "," ) );
    }

    this->update_rate_ = 100.0;
    if ( !_sdf->HasElement ( "updateRate" ) ) {
        ROS_WARN_NAMED("joint_state_publisher", "GazeboRosJointStatePublisher Plugin (ns = %s) missing <updateRate>, defaults to %f",
                   this->robot_namespace_.c_str(), this->update_rate_ );
    } else {
        this->update_rate_ = _sdf->GetElement ( "updateRate" )->Get<double>();
    }

    // Initialize update rate stuff
    if ( this->update_rate_ > 0.0 ) {
        this->update_period_ = 1.0 / this->update_rate_;
    } else {
        this->update_period_ = 0.0;
    }
    last_update_time_ = this->world_->GetSimTime();

    for ( unsigned int i = 0; i< joint_names_.size(); i++ ) {
        joints_.push_back ( this->parent_->GetJoint ( joint_names_[i] ) );
        ROS_INFO_NAMED("joint_state_publisher", "GazeboRosJointStatePublisher is going to publish joint: %s", joint_names_[i].c_str() );
    }

    ROS_INFO_NAMED("joint_state_publisher", "Starting GazeboRosJointStatePublisher Plugin (ns = %s)!, parent name: %s", this->robot_namespace_.c_str(), parent_->GetName ().c_str() );

    tf_prefix_ = tf::getPrefixParam ( *rosnode_ );
    joint_state_publisher_ = rosnode_->advertise<sensor_msgs::JointState> ( "joint_states",1000 );

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

////////////////////////////////////////////////////////////////////////////////
// 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));
}

////////////////////////////////////////////////////////////////////////////////
// Load the controller
void GazeboRosSonar::Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sdf)
{
  // Get then name of the parent sensor
  sensor_ = boost::shared_dynamic_cast<sensors::RaySensor>(_sensor);
  if (!sensor_)
  {
    gzthrow("GazeboRosSonar requires a Ray Sensor as its parent");
    return;
  }

  // Get the world name.
  std::string worldName = sensor_->GetWorldName();
  world = physics::get_world(worldName);

  // load parameters
  if (!_sdf->HasElement("robotNamespace"))
    namespace_.clear();
  else
    namespace_ = _sdf->GetElement("robotNamespace")->GetValueString() + "/";

  if (!_sdf->HasElement("frameId"))
    frame_id_ = "";
  else
    frame_id_ = _sdf->GetElement("frameId")->GetValueString();

  if (!_sdf->HasElement("topicName"))
    topic_ = "sonar";
  else
    topic_ = _sdf->GetElement("topicName")->GetValueString();

  sensor_model_.Load(_sdf);

  range_.header.frame_id = frame_id_;
  range_.radiation_type = sensor_msgs::Range::ULTRASOUND;
  range_.field_of_view = std::min(fabs((sensor_->GetAngleMax() - sensor_->GetAngleMin()).GetAsRadian()), fabs((sensor_->GetVerticalAngleMax() - sensor_->GetVerticalAngleMin()).GetAsRadian()));
  range_.max_range = sensor_->GetRangeMax();
  range_.min_range = sensor_->GetRangeMin();

  // start ros node
  if (!ros::isInitialized())
  {
    int argc = 0;
    char** argv = NULL;
    ros::init(argc,argv,"gazebo",ros::init_options::NoSigintHandler|ros::init_options::AnonymousName);
  }

  node_handle_ = new ros::NodeHandle(namespace_);
  publisher_ = node_handle_->advertise<sensor_msgs::Range>(topic_, 1);

  Reset();
  updateConnection = sensor_->GetLaserShape()->ConnectNewLaserScans(
        boost::bind(&GazeboRosSonar::Update, this));

  // activate RaySensor
  sensor_->SetActive(true);
}

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");
}

gz::common::PID Motor::createPid(sdf::ElementPtr pidElem) {
	double pv = 0, iv = 0, dv = 0, iMax = 0, iMin = 0,
			cmdMax = 0, cmdMin = 0;

	if (pidElem->HasElement("rv:p")) pv = pidElem->GetElement("rv:p")->Get<double>();
	if (pidElem->HasElement("rv:i")) iv = pidElem->GetElement("rv:i")->Get<double>();
	if (pidElem->HasElement("rv:d")) dv = pidElem->GetElement("rv:d")->Get<double>();
	if (pidElem->HasElement("rv:i_max")) iMax = pidElem->GetElement("rv:i_max")->Get<double>();
	if (pidElem->HasElement("rv:i_min")) iMin = pidElem->GetElement("rv:i_min")->Get<double>();
	if (pidElem->HasElement("rv:cmd_max")) cmdMax = pidElem->GetElement("rv:cmd_max")->Get<double>();
	if (pidElem->HasElement("rv:cmd_min")) cmdMin = pidElem->GetElement("rv:cmd_min")->Get<double>();

	return gz::common::PID(pv, iv, dv, iMax, iMin, cmdMax, cmdMin);
}

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_);
}

    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);
    }

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));
}