////////////////////////////////////////////////////////////////////////////////
// Load the controller
void GazeboRosF3D::Load( physics::ModelPtr _parent, sdf::ElementPtr _sdf )
{
  // Get the world name.
  this->world_ = _parent->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("f3d plugin missing <bodyName>, cannot proceed");
    return;
  }
  else
    this->link_name_ = _sdf->GetElement("bodyName")->Get<std::string>();

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

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

  if (!_sdf->HasElement("frameName"))
  {
    ROS_INFO("f3d plugin missing <frameName>, defaults to world");
    this->frame_name_ = "world";
  }
  else
  {
    this->frame_name_ = _sdf->GetElement("frameName")->Get<std::string>();
    // todo: frameName not used
    ROS_INFO("f3d plugin specifies <frameName> [%s], not used, default to world",this->frame_name_.c_str());
  }
  

  // 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_);
  
  // resolve tf prefix
  std::string prefix;
  this->rosnode_->getParam(std::string("tf_prefix"), prefix);
  this->frame_name_ = tf::resolve(prefix, this->frame_name_);

  // Custom Callback Queue
  ros::AdvertiseOptions ao = ros::AdvertiseOptions::create<geometry_msgs::WrenchStamped>(
    this->topic_name_,1,
    boost::bind( &GazeboRosF3D::F3DConnect,this),
    boost::bind( &GazeboRosF3D::F3DDisconnect,this), ros::VoidPtr(), &this->queue_);
  this->pub_ = this->rosnode_->advertise(ao);
  
  // Custom Callback Queue
  this->callback_queue_thread_ = boost::thread( boost::bind( &GazeboRosF3D::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(&GazeboRosF3D::UpdateChild, this));
}
Exemplo n.º 2
0
/**
 * Saves the gazebo pointer, creates the device driver
 */
void GazeboYarpTripod::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
{
    yarp::os::Network::init();
    if (!yarp::os::Network::checkNetwork(GazeboYarpPlugins::yarpNetworkInitializationTimeout)) {
        std::cerr << "GazeboYarpTripod::Load error: yarp network does not seem to be available, is the yarpserver running?"<<std::endl;
        return;
    }
    std::cout<<"*** GazeboYarpTripod plugin started ***"<<std::endl;

    if (!_parent) {
        gzerr << "GazeboYarpTripod plugin requires a parent.\n";
        return;
    }

    m_robotName = _parent->GetScopedName();
    GazeboYarpPlugins::Handler::getHandler()->setRobot(get_pointer(_parent));

    // Add the gazebo_controlboard device driver to the factory.
    yarp::dev::Drivers::factory().add(new yarp::dev::DriverCreatorOf<cer::dev::GazeboTripodMotionControl>("gazebo_tripod", "controlboardwrapper2", "GazeboTripodMotionControl"));

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

    yarp::os::Bottle wrapper_group;
    yarp::os::Bottle driver_group;
    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);

        GazeboYarpPlugins::addGazeboEnviromentalVariablesModel(_parent,_sdf,m_parameters);

        bool wipe = false;
        if (ini_file_path != "" && m_parameters.fromConfigFile(ini_file_path.c_str(),wipe))
        {
            std::cout << "GazeboYarpTripod: Found yarpConfigurationFile: loading from " << ini_file_path << std::endl;
            m_parameters.put("gazebo_ini_file_path",ini_file_path.c_str());

            wrapper_group = m_parameters.findGroup("WRAPPER");
            if(wrapper_group.isNull()) {
                printf("GazeboYarpTripod::Load  Error: [WRAPPER] group not found in config file\n");
                return;
            }
            if(m_parameters.check("ROS"))
            {
                yarp::os::ConstString ROS;
                ROS = yarp::os::ConstString ("(") + m_parameters.findGroup("ROS").toString() + yarp::os::ConstString (")");
                wrapper_group.append(yarp::os::Bottle(ROS));
            }
            configuration_loaded = true;
        }

    }

    if (!configuration_loaded) {
        std::cout << "GazeboYarpTripod: File .ini not found, quitting\n" << std::endl;
        return;
    }

    m_wrapper.open(wrapper_group);

    if (!m_wrapper.isValid())
        fprintf(stderr, "GazeboYarpTripod: wrapper did not open\n");
    else
        fprintf(stderr, "GazeboYarpTripod: wrapper opened correctly\n");

    if (!m_wrapper.view(m_iWrap)) {
        printf("Wrapper interface not found\n");
    }

    yarp::os::Bottle *netList = wrapper_group.find("networks").asList();
    if (netList->isNull()) {
        printf("GazeboYarpTripod ERROR, net list to attach to was not found, exiting\n");
        m_wrapper.close();
        // m_controlBoard.close();
        return;
    }

    for (int n = 0; n < netList->size(); n++)
    {
        yarp::dev::PolyDriverDescriptor newPoly;

        newPoly.key = netList->get(n).asString();
        std::string scopedDeviceName = m_robotName + "::" + newPoly.key.c_str();
        newPoly.poly = GazeboYarpPlugins::Handler::getHandler()->getDevice(scopedDeviceName);

        if( newPoly.poly != NULL)
        {
            // device already exists, use it, setting it againg to increment the usage counter.
            printf("controlBoard %s already opened\n", newPoly.key.c_str());

        }
        else
        {
            driver_group = m_parameters.findGroup(newPoly.key.c_str());
            if (driver_group.isNull()) {
                fprintf(stderr, "GazeboYarpTripod::Load  Error: [%s] group not found in config file. Closing wrapper \n", newPoly.key.c_str());
                m_wrapper.close();
                return;
            }

            m_parameters.put("name", newPoly.key.c_str());
            m_parameters.fromString(driver_group.toString(), false);
            m_parameters.put("robotScopedName", m_robotName);
            std::cout << "GazeboYarpTripod: setting robotScopedName " << m_robotName << std::endl;
            //std::cout << "before open: params are " << m_parameters.toString() << std::endl;

            if (_sdf->HasElement("initialConfiguration")) {
                std::string configuration_s = _sdf->Get<std::string>("initialConfiguration");
                m_parameters.put("initialConfiguration", configuration_s.c_str());
            }

            newPoly.poly = new yarp::dev::PolyDriver;
            if(! newPoly.poly->open(m_parameters) || ! newPoly.poly->isValid())
            {
                std::cerr << "controlBoard <" << newPoly.key << "> did not open!!";
                for(int idx=0; idx<m_controlBoards.size(); idx++)
                {
                    m_controlBoards[idx]->poly->close();
                }
                m_wrapper.close();
                return;
            }
            else
            {
                printf("controlBoard %s opened correctly\n", newPoly.key.c_str());
            }
        }
        GazeboYarpPlugins::Handler::getHandler()->setDevice(scopedDeviceName, newPoly.poly);
        m_controlBoards.push(newPoly);
    }

    if (!m_iWrap || !m_iWrap->attachAll(m_controlBoards))
    {
        printf("GazeboYarpTripod: Error while attaching wrapper to device\n");
        m_wrapper.close();
        for (int n = 0; n < netList->size(); n++) {
            std::string scopedDeviceName = m_robotName + "::" + m_controlBoards[n]->key.c_str();
            GazeboYarpPlugins::Handler::getHandler()->removeDevice(scopedDeviceName);
        }
        return;
    }

    printf("Device initialized correctly, now sitting and waiting cause I am just the main of the yarp device, and the robot is linked to the onUpdate event of gazebo\n");
    std::cout<<"Loaded GazeboYarpTripod Plugin"<<std::endl;
}
Exemplo n.º 3
0
////////////////////////////////////////////////////////////////////////////////
// Load the controller
void GazeboRosGps::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
{
  world = _model->GetWorld();

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

  if (!_sdf->HasElement("bodyName"))
  {
    link = _model->GetLink();
    link_name_ = link->GetName();
  }
  else {
    link_name_ = _sdf->GetElement("bodyName")->GetValueString();
    link = boost::shared_dynamic_cast<physics::Link>(world->GetEntity(link_name_));
  }

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

  double update_rate = 4.0;
  if (_sdf->HasElement("updateRate")) update_rate = _sdf->GetElement("updateRate")->GetValueDouble();
  update_period = update_rate > 0.0 ? 1.0/update_rate : 0.0;

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

  if (!_sdf->HasElement("topicName"))
    fix_topic_ = "fix";
  else
    fix_topic_ = _sdf->GetElement("topicName")->GetValueString();

  if (!_sdf->HasElement("velocityTopicName"))
    velocity_topic_ = "fix_velocity";
  else
    velocity_topic_ = _sdf->GetElement("velocityTopicName")->GetValueString();

  if (!_sdf->HasElement("referenceLatitude"))
    reference_latitude_ = DEFAULT_REFERENCE_LATITUDE;
  else
    reference_latitude_ = _sdf->GetElement("referenceLatitude")->GetValueDouble();

  if (!_sdf->HasElement("referenceLongitude"))
    reference_longitude_ = DEFAULT_REFERENCE_LONGITUDE;
  else
    reference_longitude_ = _sdf->GetElement("referenceLongitude")->GetValueDouble();

  if (!_sdf->HasElement("referenceHeading"))
    reference_heading_ = DEFAULT_REFERENCE_HEADING * M_PI/180.0;
  else
    reference_heading_ = _sdf->GetElement("referenceLatitude")->GetValueDouble() * M_PI/180.0;

  if (!_sdf->HasElement("referenceAltitude"))
    reference_altitude_ = DEFAULT_REFERENCE_ALTITUDE;
  else
    reference_altitude_ = _sdf->GetElement("referenceLatitude")->GetValueDouble();

  if (!_sdf->HasElement("status"))
    status_ = sensor_msgs::NavSatStatus::STATUS_FIX;
  else
    status_ = _sdf->GetElement("status")->GetValueUInt();

  if (!_sdf->HasElement("service"))
    service_ = sensor_msgs::NavSatStatus::SERVICE_GPS;
  else
    service_ = _sdf->GetElement("service")->GetValueUInt();

  fix_.header.frame_id = frame_id_;
  fix_.status.status  = status_;
  fix_.status.service = service_;
  velocity_.header.frame_id = frame_id_;

  position_error_model_.Load(_sdf);
  velocity_error_model_.Load(_sdf, "velocity");

  // 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_);
  fix_publisher_ = node_handle_->advertise<sensor_msgs::NavSatFix>(fix_topic_, 10);
  velocity_publisher_ = node_handle_->advertise<geometry_msgs::Vector3Stamped>(velocity_topic_, 10);

  Reset();

  // New Mechanism for Updating every World Cycle
  // Listen to the update event. This event is broadcast every
  // simulation iteration.
  updateConnection = event::Events::ConnectWorldUpdateStart(
      boost::bind(&GazeboRosGps::Update, this));
}
////////////////////////////////////////////////////////////////////////////////
// Load the controller
void GazeboQuadrotorAerodynamics::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
{
  model = _model;
  world = _model->GetWorld();

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

  if (!_sdf->HasElement("paramNamespace"))
    param_namespace_ = "~/quadrotor_aerodynamics/";
  else
    param_namespace_ = _sdf->GetElement("paramNamespace")->GetValueString() + "/";

  if (!_sdf->HasElement("voltageTopicName"))
    voltage_topic_ = "motor_pwm";
  else
    voltage_topic_ = _sdf->GetElement("voltageTopicName")->GetValueString();

  if (!_sdf->HasElement("windTopicName"))
    wind_topic_ = "wind";
  else
    wind_topic_ = _sdf->GetElement("windTopicName")->GetValueString();

  if (!_sdf->HasElement("wrenchTopic"))
    wrench_topic_ = "wrench_out";
  else
    wrench_topic_ = _sdf->GetElement("wrenchTopic")->GetValueString();

  if (!_sdf->HasElement("statusTopic"))
    status_topic_ = "motor_status";
  else
    status_topic_ = _sdf->GetElement("statusTopic")->GetValueString();

  // set control timing parameters
  control_rate_ = 100.0;
  if (_sdf->HasElement("controlRate")) control_rate_ = _sdf->GetElement("controlRate")->GetValueDouble();
  control_period_ = (control_rate_ > 0) ? (1.0 / control_rate_) : 0.0;
  control_tolerance_ = control_period_ / 2.0;
  if (_sdf->HasElement("controlTolerance")) control_tolerance_ = _sdf->GetElement("controlTolerance")->GetValueDouble();
  control_delay_ = Time();
  if (_sdf->HasElement("controlDelay")) control_delay_ = _sdf->GetElement("controlDelay")->GetValueDouble();

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

  // subscribe command
  if (!voltage_topic_.empty())
  {
    ros::SubscribeOptions ops = ros::SubscribeOptions::create<hector_uav_msgs::MotorPWM>(
      voltage_topic_, 1,
      boost::bind(&GazeboQuadrotorAerodynamics::CommandCallback, this, _1),
      ros::VoidPtr(), &callback_queue_);
    voltage_subscriber_ = node_handle_->subscribe(ops);
  }

  // subscribe command
  if (!wind_topic_.empty())
  {
    ros::SubscribeOptions ops = ros::SubscribeOptions::create<geometry_msgs::Vector3>(
      wind_topic_, 1,
      boost::bind(&GazeboQuadrotorAerodynamics::WindCallback, this, _1),
      ros::VoidPtr(), &callback_queue_);
    wind_subscriber_ = node_handle_->subscribe(ops);
  }

  // publish wrench
  if (!wrench_topic_.empty())
  {
    ros::AdvertiseOptions ops = ros::AdvertiseOptions::create<geometry_msgs::Wrench>(
      wrench_topic_, 10,
      ros::SubscriberStatusCallback(), ros::SubscriberStatusCallback(),
      ros::VoidConstPtr(), &callback_queue_);
    wrench_publisher_ = node_handle_->advertise(ops);
  }

  // publish motor_status
  if (!status_topic_.empty())
  {
    ros::AdvertiseOptions ops = ros::AdvertiseOptions::create<hector_uav_msgs::MotorStatus>(
      status_topic_, 10,
      ros::SubscriberStatusCallback(), ros::SubscriberStatusCallback(),
      ros::VoidConstPtr(), &callback_queue_);
    motor_status_publisher_ = node_handle_->advertise(ops);
  }

  callback_queue_thread_ = boost::thread( boost::bind( &GazeboQuadrotorAerodynamics::QueueThread,this ) );

  Reset();

  // get model parameters
  ros::NodeHandle param(param_namespace_);
  param.getParam("k_t",  propulsion_model_->parameters_.k_t);
  param.getParam("CT0s", propulsion_model_->parameters_.CT0s);
  param.getParam("CT1s", propulsion_model_->parameters_.CT1s);
  param.getParam("CT2s", propulsion_model_->parameters_.CT2s);
  param.getParam("J_M",  propulsion_model_->parameters_.J_M);
  param.getParam("l_m",  propulsion_model_->parameters_.l_m);
  param.getParam("Psi",  propulsion_model_->parameters_.Psi);
  param.getParam("R_A",  propulsion_model_->parameters_.R_A);

  param.getParam("C_wxy", drag_model_->parameters_.C_wxy);
  param.getParam("C_wz",  drag_model_->parameters_.C_wz);
  param.getParam("C_mxy", drag_model_->parameters_.C_mxy);
  param.getParam("C_mz",  drag_model_->parameters_.C_mz);

  std::cout << "Loaded the following quadrotor propulsion model parameters from namespace " << param.getNamespace() << ":" << std::endl;
  std::cout << "k_t = " << propulsion_model_->parameters_.k_t << std::endl;
  std::cout << "CT2s = " << propulsion_model_->parameters_.CT2s << std::endl;
  std::cout << "CT1s = " << propulsion_model_->parameters_.CT1s << std::endl;
  std::cout << "CT0s = " << propulsion_model_->parameters_.CT0s << std::endl;
  std::cout << "Psi = "  << propulsion_model_->parameters_.Psi << std::endl;
  std::cout << "J_M = "  << propulsion_model_->parameters_.J_M << std::endl;
  std::cout << "R_A = "  << propulsion_model_->parameters_.R_A << std::endl;
  std::cout << "l_m = "  << propulsion_model_->parameters_.l_m << std::endl;

  std::cout << "Loaded the following quadrotor drag model parameters from namespace " << param.getNamespace() << ":" << std::endl;
  std::cout << "C_wxy = " << drag_model_->parameters_.C_wxy << std::endl;
  std::cout << "C_wz = "  << drag_model_->parameters_.C_wz << std::endl;
  std::cout << "C_mxy = " << drag_model_->parameters_.C_mxy << std::endl;
  std::cout << "C_mz = "  << drag_model_->parameters_.C_mz << std::endl;

  // New Mechanism for Updating every World Cycle
  // Listen to the update event. This event is broadcast every
  // simulation iteration.
  updateConnection = event::Events::ConnectWorldUpdateStart(
      boost::bind(&GazeboQuadrotorAerodynamics::Update, this));
}
////////////////////////////////////////////////////////////////////////////////
// Load the controller
void GazeboRosBaro::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
{
  world = _model->GetWorld();

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

  if (!_sdf->HasElement("bodyName"))
  {
    link = _model->GetLink();
    link_name_ = link->GetName();
  }
  else {
    link_name_ = _sdf->GetElement("bodyName")->GetValueString();
    link = _model->GetLink( link_name_ );
  }

  ROS_INFO_NAMED( "gazebo_ros_baro", "got link %s for model %s for baro", link->GetName().c_str(), link->GetModel()->GetName().c_str() );

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

  double update_rate = 0.0;
  if (_sdf->HasElement("updateRate")) update_rate = _sdf->GetElement("updateRate")->GetValueDouble();
  update_period = update_rate > 0.0 ? 1.0/update_rate : 0.0;

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

  if (!_sdf->HasElement("topicName"))
    height_topic_ = "pressure_height";
  else
    height_topic_ = _sdf->GetElement("topicName")->GetValueString();

  if (!_sdf->HasElement("altimeterTopicName"))
    altimeter_topic_ = "altimeter";
  else
    altimeter_topic_ = _sdf->GetElement("altimeterTopicName")->GetValueString();

  if (!_sdf->HasElement("elevation"))
    elevation_ = DEFAULT_ELEVATION;
  else
    elevation_ = _sdf->GetElement("elevation")->GetValueDouble();

  if (!_sdf->HasElement("qnh"))
    qnh_ = DEFAULT_QNH;
  else
    qnh_ = _sdf->GetElement("qnh")->GetValueDouble();

  sensor_model_.Load(_sdf);
  height_.header.frame_id = frame_id_;

  // 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_);
  if (!height_topic_.empty()) {
#ifdef USE_MAV_MSGS
    height_publisher_ = node_handle_->advertise<mav_msgs::Height>(height_topic_, 10);
#else
    height_publisher_ = node_handle_->advertise<geometry_msgs::PointStamped>(height_topic_, 10);
#endif
  }

  if (!altimeter_topic_.empty()) {
    altimeter_publisher_ = node_handle_->advertise<hector_uav_msgs::Altimeter>(altimeter_topic_, 10);
  }

  Reset();

  // New Mechanism for Updating every World Cycle
  // Listen to the update event. This event is broadcast every
  // simulation iteration.
  updateConnection = event::Events::ConnectWorldUpdateStart(
      boost::bind(&GazeboRosBaro::Update, this));
}
Exemplo n.º 6
0
////////////////////////////////////////////////////////////////////////////////
// Load the controller
void GazeboRosMagnetic::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
{
  world = _model->GetWorld();

  // load parameters
  if (!_sdf->HasElement("robotNamespace"))
    namespace_.clear();
  else
    namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>() + "/";

  if (!_sdf->HasElement("topicName"))
    topic_ = "magnetic";
  else
    topic_ = _sdf->GetElement("topicName")->Get<std::string>();

  link =  _model->GetChildLink("base_link");

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

  double update_rate = 0.0;
  if (_sdf->HasElement("updateRate")) update_rate = _sdf->GetElement("updateRate")->Get<double>();
  update_period = update_rate > 0.0 ? 1.0/update_rate : 0.0;

  if (!_sdf->HasElement("frameId"))
    frame_id_ = link_name_;
  else
    frame_id_ = _sdf->GetElement("frameId")->Get<std::string>();

  if (!_sdf->HasElement("magnitude"))
    magnitude_ = DEFAULT_MAGNITUDE;
  else
    magnitude_ = _sdf->GetElement("magnitude")->Get<double>();

  if (!_sdf->HasElement("referenceHeading"))
    reference_heading_ = DEFAULT_REFERENCE_HEADING * M_PI/180.0;
  else
    reference_heading_ = _sdf->GetElement("referenceHeading")->Get<double>() * M_PI/180.0;

  if (!_sdf->HasElement("declination"))
    declination_ = DEFAULT_DECLINATION * M_PI/180.0;
  else
    declination_ = _sdf->GetElement("declination")->Get<double>() * M_PI/180.0;

  if (!_sdf->HasElement("inclination"))
    inclination_ = DEFAULT_INCLINATION * M_PI/180.0;
  else
    inclination_ = _sdf->GetElement("inclination")->Get<double>() * M_PI/180.0;

  // Note: Gazebo uses NorthWestUp coordinate system, heading and declination are compass headings
  magnetic_field_.header.frame_id = frame_id_;
  magnetic_field_world_.x = magnitude_ *  cos(inclination_) * cos(reference_heading_ - declination_);
  magnetic_field_world_.y = magnitude_ *  sin(reference_heading_ - declination_);
  magnetic_field_world_.z = magnitude_ * -sin(inclination_) * cos(reference_heading_ - declination_);

  sensor_model_.Load(_sdf);

  // 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<geometry_msgs::Vector3Stamped>(topic_, 1);

  Reset();

  // New Mechanism for Updating every World Cycle
  // Listen to the update event. This event is broadcast every
  // simulation iteration.
  updateConnection = event::Events::ConnectWorldUpdateBegin(
      boost::bind(&GazeboRosMagnetic::Update, this));
}
    private: void updateContacts(sensors::SensorPtr sensor, const string& sensorName){
      // Get all the contacts.
      msgs::Contacts contacts = boost::dynamic_pointer_cast<sensors::ContactSensor>(sensor)->GetContacts();
      physics::LinkPtr trunk = model->GetLink("trunk");

      for (unsigned int i = 0; i < contacts.contact_size(); ++i){
        #if(PRINT_SENSORS)
        cout << "Collision between[" << contacts.contact(i).collision1()
             << "] and [" << contacts.contact(i).collision2() << "]\n";
        cout << " t[" << this->world->GetSimTime()
             << "] i[" << i
             << "] s[" << contacts.contact(i).time().sec()
             << "] n[" << contacts.contact(i).time().nsec()
             << "] size[" << contacts.contact(i).position_size()
             << "]\n";
         #endif

        math::Vector3 fTotal;
        math::Vector3 tTotal;

        for(unsigned int j = 0; j < contacts.contact(i).position_size(); ++j){
          #if(PRINT_SENSORS)
          cout << j << "  Position:"
               << contacts.contact(i).position(j).x() << " "
               << contacts.contact(i).position(j).y() << " "
               << contacts.contact(i).position(j).z() << "\n";
          cout << "   Normal:"
               << contacts.contact(i).normal(j).x() << " "
               << contacts.contact(i).normal(j).y() << " "
               << contacts.contact(i).normal(j).z() << "\n";
           cout << "   Wrench 1:"
           << contacts.contact(i).wrench(j).body_1_wrench().force().x() << " "
           << contacts.contact(i).wrench(j).body_1_wrench().force().y() << " "
           << contacts.contact(i).wrench(j).body_1_wrench().force().z() << "\n";
           cout << "   Wrench 2:"
           << contacts.contact(i).wrench(j).body_2_wrench().force().x() << " "
           << contacts.contact(i).wrench(j).body_2_wrench().force().y() << " "
           << contacts.contact(i).wrench(j).body_2_wrench().force().z() << "\n";
          cout << "   Depth:" << contacts.contact(i).depth(j) << "\n";
          #endif
          
          fTotal += math::Vector3(
                            contacts.contact(i).wrench(j).body_1_wrench().force().x(),
                            contacts.contact(i).wrench(j).body_1_wrench().force().y(),
                            contacts.contact(i).wrench(j).body_1_wrench().force().z());
          tTotal += math::Vector3(
                            contacts.contact(i).wrench(j).body_1_wrench().torque().x(),
                            contacts.contact(i).wrench(j).body_1_wrench().torque().y(),
                            contacts.contact(i).wrench(j).body_1_wrench().torque().z());
        }
        #if(PRINT_SENSORS)
          cout << "Total Force: " << fTotal.GetLength() << endl;
          cout << "Total Torque: " << tTotal.GetLength() << endl;
        #endif

        if(fTotal.GetLength() > maxForces[sensorName]){
            maxForces[sensorName] = fTotal.GetLength();
            maxForceTimes[sensorName] = this->world->GetSimTime();
            // We want the link it collides with
           vector<string> strs;
           boost::split(strs, sensorName, boost::is_any_of("::"));
           collidingLink[sensorName] = !boost::contains(contacts.contact(i).collision1(), strs[0]) ? contacts.contact(i).collision1() : contacts.contact(i).collision2();
#if PRINT_DEBUG
           cout << "Colliding link is: " << collidingLink[sensorName] << ". Options were: " << contacts.contact(i).collision1() << " and " << contacts.contact(i).collision2() << endl;
#endif
            maxForceVelocities[sensorName] = trunk->GetWorldLinearVel();
            maxForceAccs[sensorName] = trunk->GetWorldLinearAccel();
        }
        if(tTotal.GetLength() > maxTorques[sensorName]){
            maxTorques[sensorName] = tTotal.GetLength();
        }
      }
    }
void GazeboRosKobuki::Load(physics::ModelPtr parent, sdf::ElementPtr sdf)
{
  model_ = parent;
  if (!model_)
  {
    ROS_ERROR_STREAM("Invalid model pointer! [" << node_name_ << "]");
    return;
  }
  // Get then name of the parent model and use it as node name
  std::string model_name = sdf->GetParent()->GetValueString("name");
  gzdbg << "Plugin model name: " << model_name << "\n";
  nh_ = ros::NodeHandle("");
  // creating a private name pace until Gazebo implements topic remappings
  nh_priv_ = ros::NodeHandle("/" + model_name);
  node_name_ = model_name;

  world_ = parent->GetWorld();

// TODO: use when implementing subs
//  ros_spinner_thread_ = new boost::thread(boost::bind(&GazeboRosKobuki::spin, this));
//
//  this->node_namespace_ = "";
//  if (_sdf->HasElement("node_namespace"))
//    this->node_namespace_ = _sdf->GetElement("node_namespace")->GetValueString() + "/";

  /*
   * Prepare receiving motor power commands
   */
  motor_power_sub_ = nh_priv_.subscribe("commands/motor_power", 10, &GazeboRosKobuki::motorPowerCB, this);
  motors_enabled_ = true;

  /*
   * Prepare joint state publishing
   */
  if (sdf->HasElement("left_wheel_joint_name"))
  {
    left_wheel_joint_name_ = sdf->GetElement("left_wheel_joint_name")->GetValueString();
  }
  else
  {
    ROS_ERROR_STREAM("Couldn't find left wheel joint in the model description!"
                     << " Did you specify the correct joint name?" << " [" << node_name_ <<"]");
    return;
  }
  if (sdf->HasElement("right_wheel_joint_name"))
  {
    right_wheel_joint_name_ = sdf->GetElement("right_wheel_joint_name")->GetValueString();
  }
  else
  {
    ROS_ERROR_STREAM("Couldn't find right wheel joint in the model description!"
                     << " Did you specify the correct joint name?" << " [" << node_name_ <<"]");
    return;
  }
  joints_[LEFT] = parent->GetJoint(left_wheel_joint_name_);
  joints_[RIGHT] = parent->GetJoint(right_wheel_joint_name_);
  if (!joints_[LEFT] || !joints_[RIGHT])
  {
    ROS_ERROR_STREAM("Couldn't find specified wheel joints in the model! [" << node_name_ <<"]");
    return;
  }
  joint_state_.header.frame_id = "Joint States";
  joint_state_.name.push_back(left_wheel_joint_name_);
  joint_state_.position.push_back(0);
  joint_state_.velocity.push_back(0);
  joint_state_.effort.push_back(0);
  joint_state_.name.push_back(right_wheel_joint_name_);
  joint_state_.position.push_back(0);
  joint_state_.velocity.push_back(0);
  joint_state_.effort.push_back(0);
  joint_state_pub_ = nh_.advertise<sensor_msgs::JointState>("joint_states", 1);

  /*
   * Prepare publishing odometry data
   */
  if (sdf->HasElement("wheel_separation"))
  {
    wheel_sep_ = sdf->GetElement("wheel_separation")->GetValueDouble();
  }
  else
  {
    ROS_ERROR_STREAM("Couldn't find the wheel separation parameter in the model description!"
                     << " Did you specify it?" << " [" << node_name_ <<"]");
    return;
  }
  if (sdf->HasElement("wheel_diameter"))
  {
    wheel_diam_ = sdf->GetElement("wheel_diameter")->GetValueDouble();
  }
  else
  {
    ROS_ERROR_STREAM("Couldn't find the wheel diameter parameter in the model description!"
                     << " Did you specify it?" << " [" << node_name_ <<"]");
    return;
  }
  if (sdf->HasElement("torque"))
  {
    torque_ = sdf->GetElement("torque")->GetValueDouble();
  }
  else
  {
    ROS_ERROR_STREAM("Couldn't find the torque parameter in the model description!"
                     << " Did you specify it?" << " [" << node_name_ <<"]");
    return;
  }
  odom_pub_ = nh_.advertise<nav_msgs::Odometry>("odom", 1);

  /*
   * Prepare receiving velocity commands
   */
  if (sdf->HasElement("velocity_command_timeout"))
  {
    cmd_vel_timeout_ = sdf->GetElement("velocity_command_timeout")->GetValueDouble();
  }
  else
  {
    ROS_ERROR_STREAM("Couldn't find the wheel separation parameter in the model description!"
                     << " Did you specify it?" << " [" << node_name_ <<"]");
    return;
  }
  last_cmd_vel_time_ = world_->GetSimTime();
  cmd_vel_sub_ = nh_priv_.subscribe("commands/velocity", 100, &GazeboRosKobuki::cmdVelCB, this);

  /*
   * Prepare cliff sensors
   */
  std::string cliff_sensor_left_name, cliff_sensor_front_name, cliff_sensor_right_name;
  if (sdf->HasElement("cliff_sensor_left_name"))
  {
    cliff_sensor_left_name = sdf->GetElement("cliff_sensor_left_name")->GetValueString();
  }
  else
  {
    ROS_ERROR_STREAM("Couldn't find the name of left cliff sensor in the model description!"
                     << " Did you specify it?" << " [" << node_name_ <<"]");
    return;
  }
  if (sdf->HasElement("cliff_sensor_front_name"))
  {
    cliff_sensor_front_name = sdf->GetElement("cliff_sensor_front_name")->GetValueString();
  }
  else
  {
    ROS_ERROR_STREAM("Couldn't find the name of frontal cliff sensor in the model description!"
                     << " Did you specify it?" << " [" << node_name_ <<"]");
    return;
  }
  if (sdf->HasElement("cliff_sensor_right_name"))
  {
    cliff_sensor_right_name = sdf->GetElement("cliff_sensor_right_name")->GetValueString();
  }
  else
  {
    ROS_ERROR_STREAM("Couldn't find the name of right cliff sensor in the model description!"
                     << " Did you specify it?" << " [" << node_name_ <<"]");
    return;
  }
  cliff_sensor_left_ = boost::shared_dynamic_cast<sensors::RaySensor>(
                       sensors::SensorManager::Instance()->GetSensor(cliff_sensor_left_name));
  cliff_sensor_front_ = boost::shared_dynamic_cast<sensors::RaySensor>(
                        sensors::SensorManager::Instance()->GetSensor(cliff_sensor_front_name));
  cliff_sensor_right_ = boost::shared_dynamic_cast<sensors::RaySensor>(
                        sensors::SensorManager::Instance()->GetSensor(cliff_sensor_right_name));
  if (!cliff_sensor_left_)
  {
    ROS_ERROR_STREAM("Couldn't find the left cliff sensor in the model! [" << node_name_ <<"]");
    return;
  }
  if (!cliff_sensor_front_)
  {
    ROS_ERROR_STREAM("Couldn't find the frontal cliff sensor in the model! [" << node_name_ <<"]");
    return;
  }
  if (!cliff_sensor_right_)
  {
    ROS_ERROR_STREAM("Couldn't find the right cliff sensor in the model! [" << node_name_ <<"]");
    return;
  }
  if (sdf->HasElement("cliff_detection_threshold"))
  {
    cliff_detection_threshold_ = sdf->GetElement("cliff_detection_threshold")->GetValueDouble();
  }
  else
  {
    ROS_ERROR_STREAM("Couldn't find the cliff detection threshold parameter in the model description!"
                     << " Did you specify it?" << " [" << node_name_ <<"]");
    return;
  }
  cliff_sensor_left_->SetActive(true);
  cliff_sensor_front_->SetActive(true);
  cliff_sensor_right_->SetActive(true);
  cliff_event_pub_ = nh_priv_.advertise<kobuki_msgs::CliffEvent>("events/cliff", 1);

  /*
   * Prepare bumper
   */
  std::string bumper_name;
  if (sdf->HasElement("bumper_name"))
  {
    bumper_name = sdf->GetElement("bumper_name")->GetValueString();
  }
  else
  {
    ROS_ERROR_STREAM("Couldn't find the name of bumper sensor in the model description!"
                     << " Did you specify it?" << " [" << node_name_ <<"]");
    return;
  }
  bumper_ = boost::shared_dynamic_cast<sensors::ContactSensor>(
            sensors::SensorManager::Instance()->GetSensor(bumper_name));
  bumper_->SetActive(true);
  bumper_event_pub_ = nh_priv_.advertise<kobuki_msgs::BumperEvent>("events/bumper", 1);

  prev_update_time_ = world_->GetSimTime();
  ROS_INFO_STREAM("GazeboRosKobuki plugin ready to go! [" << node_name_ << "]");
  update_connection_ = event::Events::ConnectWorldUpdateStart(boost::bind(&GazeboRosKobuki::OnUpdate, this));
}
////////////////////////////////////////////////////////////////////////////////
// Load the controller
void GazeboQuadrotorStateController::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
{
  world = _model->GetWorld();

  // load parameters
  if (!_sdf->HasElement("robotNamespace"))
    namespace_.clear();
  else
    namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>() + "/";

  if (!_sdf->HasElement("topicName"))
    velocity_topic_ = "cmd_vel";
  else
    velocity_topic_ = _sdf->GetElement("topicName")->Get<std::string>();

  if (!_sdf->HasElement("takeoffTopic"))
    takeoff_topic_ = "/ardrone/takeoff";
  else
    takeoff_topic_ = _sdf->GetElement("takeoffTopic")->Get<std::string>();

  if (!_sdf->HasElement("/ardrone/land"))
    land_topic_ = "/ardrone/land";
  else
    land_topic_ = _sdf->GetElement("landTopic")->Get<std::string>();

  if (!_sdf->HasElement("resetTopic"))
    reset_topic_ = "/ardrone/reset";
  else
    reset_topic_ = _sdf->GetElement("resetTopic")->Get<std::string>();

  if (!_sdf->HasElement("navdataTopic"))
    navdata_topic_ = "/ardrone/navdata";
  else
    navdata_topic_ = _sdf->GetElement("navdataTopic")->Get<std::string>();

  if (!_sdf->HasElement("imuTopic"))
    imu_topic_.clear();
  else
    imu_topic_ = _sdf->GetElement("imuTopic")->Get<std::string>();

  if (!_sdf->HasElement("sonarTopic"))
    sonar_topic_.clear();
  else
    sonar_topic_ = _sdf->GetElement("sonarTopic")->Get<std::string>();

  if (!_sdf->HasElement("contactTopic"))
	contact_topic_.clear();
  else
	contact_topic_ = _sdf->GetElement("contactTopic")->Get<std::string>();

  if (!_sdf->HasElement("stateTopic"))
    state_topic_.clear();
  else
    state_topic_ = _sdf->GetElement("stateTopic")->Get<std::string>();

  if (!_sdf->HasElement("bodyName"))
  {
    link = _model->GetLink();
    link_name_ = link->GetName();
  }
  else {
    link_name_ = _sdf->GetElement("bodyName")->Get<std::string>();
    link = boost::dynamic_pointer_cast<physics::Link>(world->GetEntity(link_name_));
  }

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

  node_handle_ = new ros::NodeHandle(namespace_);

  // subscribe command: velocity control command
  if (!velocity_topic_.empty())
  {
    ros::SubscribeOptions ops = ros::SubscribeOptions::create<geometry_msgs::Twist>(
      velocity_topic_, 1,
      boost::bind(&GazeboQuadrotorStateController::VelocityCallback, this, _1),
      ros::VoidPtr(), &callback_queue_);
    velocity_subscriber_ = node_handle_->subscribe(ops);
  }

  // subscribe command: take off command
  if (!takeoff_topic_.empty())
  {
    ros::SubscribeOptions ops = ros::SubscribeOptions::create<std_msgs::Empty>(
      takeoff_topic_, 1,
      boost::bind(&GazeboQuadrotorStateController::TakeoffCallback, this, _1),
      ros::VoidPtr(), &callback_queue_);
    takeoff_subscriber_ = node_handle_->subscribe(ops);
  }

  // subscribe command: take off command
  if (!land_topic_.empty())
  {
    ros::SubscribeOptions ops = ros::SubscribeOptions::create<std_msgs::Empty>(
      land_topic_, 1,
      boost::bind(&GazeboQuadrotorStateController::LandCallback, this, _1),
      ros::VoidPtr(), &callback_queue_);
    land_subscriber_ = node_handle_->subscribe(ops);
  }

  // subscribe command: take off command
  if (!reset_topic_.empty())
  {
    ros::SubscribeOptions ops = ros::SubscribeOptions::create<std_msgs::Empty>(
      reset_topic_, 1,
      boost::bind(&GazeboQuadrotorStateController::ResetCallback, this, _1),
      ros::VoidPtr(), &callback_queue_);
    reset_subscriber_ = node_handle_->subscribe(ops);
  }

    m_navdataPub = node_handle_->advertise< ardrone_autonomy::Navdata >( navdata_topic_ , 25 );


  // subscribe imu
  if (!imu_topic_.empty())
  {
    ros::SubscribeOptions ops = ros::SubscribeOptions::create<sensor_msgs::Imu>(
      imu_topic_, 1,
      boost::bind(&GazeboQuadrotorStateController::ImuCallback, this, _1),
      ros::VoidPtr(), &callback_queue_);
    imu_subscriber_ = node_handle_->subscribe(ops);

    ROS_INFO_NAMED("quadrotor_state_controller", "Using imu information on topic %s as source of orientation and angular velocity.", imu_topic_.c_str());
  }

  // subscribe sonar senor info
  if (!sonar_topic_.empty())
  {
    ros::SubscribeOptions ops = ros::SubscribeOptions::create<sensor_msgs::Range>(
      sonar_topic_, 1,
      boost::bind(&GazeboQuadrotorStateController::SonarCallback, this, _1),
      ros::VoidPtr(), &callback_queue_);
    sonar_subscriber_ = node_handle_->subscribe(ops);

    ROS_INFO_NAMED("quadrotor_state_controller", "Using sonar information on topic %s as source of altitude.", sonar_topic_.c_str());
  }

  // subscribe contact senor info
  if (!contact_topic_.empty())
  {
    ros::SubscribeOptions ops = ros::SubscribeOptions::create<std_msgs::Int32>(
      contact_topic_, 1,
      boost::bind(&GazeboQuadrotorStateController::ContactCallback, this, _1),
      ros::VoidPtr(), &callback_queue_);
    contact_subscriber_ = node_handle_->subscribe(ops);

    ROS_INFO_NAMED("quadrotor_state_controller", "Using contact information on topic %s as source of collisions.", contact_topic_.c_str());
  }

  // subscribe state
  if (!state_topic_.empty())
  {
    ros::SubscribeOptions ops = ros::SubscribeOptions::create<nav_msgs::Odometry>(
      state_topic_, 1,
      boost::bind(&GazeboQuadrotorStateController::StateCallback, this, _1),
      ros::VoidPtr(), &callback_queue_);
    state_subscriber_ = node_handle_->subscribe(ops);

    ROS_INFO_NAMED("quadrotor_state_controller", "Using state information on topic %s as source of state information.", state_topic_.c_str());
  }


  // for camera control
  // switch camera server
  std::string toggleCam_topic  = "ardrone/togglecam";
  ros::AdvertiseServiceOptions toggleCam_ops = ros::AdvertiseServiceOptions::create<std_srvs::Empty>(
    toggleCam_topic,
    boost::bind(&GazeboQuadrotorStateController::toggleCamCallback, this, _1,_2),
    ros::VoidPtr(),
    &callback_queue_);

  toggleCam_service = node_handle_->advertiseService(toggleCam_ops);

  // camera image data
  std::string cam_out_topic  = "/ardrone/image_raw";
  std::string cam_front_in_topic = "/ardrone/front/image_raw";
  std::string cam_bottom_in_topic = "/ardrone/bottom/image_raw";
  std::string in_transport = "raw";

  camera_it_ = new image_transport::ImageTransport(*node_handle_);
  camera_publisher_ = camera_it_->advertise(cam_out_topic, 1);

  camera_front_subscriber_ = camera_it_->subscribe(
    cam_front_in_topic, 1,
    boost::bind(&GazeboQuadrotorStateController::CameraFrontCallback, this, _1),
    ros::VoidPtr(), in_transport);

  camera_bottom_subscriber_ = camera_it_->subscribe(
    cam_bottom_in_topic, 1,
    boost::bind(&GazeboQuadrotorStateController::CameraBottomCallback, this, _1),
    ros::VoidPtr(), in_transport);

  // camera image data
  std::string cam_info_out_topic  = "/ardrone/camera_info";
  std::string cam_info_front_in_topic = "/ardrone/front/camera_info";
  std::string cam_info_bottom_in_topic = "/ardrone/bottom/camera_info";

  camera_info_publisher_ = node_handle_->advertise<sensor_msgs::CameraInfo>(cam_info_out_topic,1);

  ros::SubscribeOptions cam_info_front_ops = ros::SubscribeOptions::create<sensor_msgs::CameraInfo>(
    cam_info_front_in_topic, 1,
    boost::bind(&GazeboQuadrotorStateController::CameraInfoFrontCallback, this, _1),
    ros::VoidPtr(), &callback_queue_);
  camera_info_front_subscriber_ = node_handle_->subscribe(cam_info_front_ops);

  ros::SubscribeOptions cam_info_bottom_ops = ros::SubscribeOptions::create<sensor_msgs::CameraInfo>(
    cam_info_bottom_in_topic, 1,
    boost::bind(&GazeboQuadrotorStateController::CameraInfoBottomCallback, this, _1),
    ros::VoidPtr(), &callback_queue_);
  camera_info_bottom_subscriber_ = node_handle_->subscribe(cam_info_bottom_ops);

  // callback_queue_thread_ = boost::thread( boost::bind( &GazeboQuadrotorStateController::CallbackQueueThread,this ) );


  Reset();

  // New Mechanism for Updating every World Cycle
  // Listen to the update event. This event is broadcast every
  // simulation iteration.
  updateConnection = event::Events::ConnectWorldUpdateBegin(
      boost::bind(&GazeboQuadrotorStateController::Update, this));

  robot_current_state = LANDED_MODEL;
}
void GazeboJointStateClient::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
{
    // Make sure the ROS node for Gazebo has already been initalized
    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;
    }

    // get joint names from parameters
    std::string armNamespace = _parent->GetName();
    if (_sdf->HasElement("robot_components_namespace"))
    {
        sdf::ElementPtr armParamElem = _sdf->GetElement("robot_components_namespace");
        armNamespace = armParamElem->Get<std::string>();
    }
    else
    {
        ROS_WARN("SDF Element 'robot_components_namespace' not defined, so using robot name as namespace for components.");
    }

    // ROS_INFO_STREAM("GazeboJointStateClient loading joints from components namespace '"<<armNamespace<<"'");

    ROS_INFO_STREAM("GazeboJointStateClient: Loading arm component parameters from "<< armNamespace);
    joints = ArmComponentsNameManagerPtr(new ArmComponentsNameManager(armNamespace,false));
    if (!joints->waitToLoadParameters(1, 3, 0.5))
    {
        ROS_FATAL_STREAM("Cannot load arm components for robot "<<_parent->GetName()<<" from namespace "<<armNamespace);
        return;
    }

    physics::BasePtr jcChild = _parent->GetChild(physics::JointControllerThreadsafe::UniqueName());
    if (!jcChild.get())
    {
        ROS_ERROR("Cannot load GazeboJointStateClient if no default JointControllerThreadsafe is set for the model");
        throw std::string("Cannot load GazeboJointStateClient if no default JointController is set for the model");
    }
    physics::JointControllerThreadsafePtr ptr =
        boost::dynamic_pointer_cast<physics::JointControllerThreadsafe>(jcChild);
    if (!ptr.get())
    {
        ROS_ERROR_STREAM("Cannot load GazeboJointStateClient if child '"
                         << physics::JointControllerThreadsafe::UniqueName()
                         << "' is not of class JointControllerThreadsafe");
        throw std::string("Cannot load GazeboJointStateClient if child '"
                          + physics::JointControllerThreadsafe::UniqueName()
                          + "' is not of class JointControllerThreadsafe");
    }
    jointController = ptr;

    // get joint names from parameters
    std::vector<std::string> joint_names;
    joints->getJointNames(joint_names, true);
    const std::vector<float>& arm_init = joints->getArmJointsInitPose();
    const std::vector<float>& gripper_init = joints->getGripperJointsInitPose();

    // Print the joint names to help debugging
    std::map<std::string, physics::JointPtr > jntMap = jointController->GetJoints();
    for (std::map<std::string, physics::JointPtr>::iterator it = jntMap.begin(); it != jntMap.end(); ++it)
    {
        physics::JointPtr j = it->second;
        ROS_INFO_STREAM("Gazebo joint: '"<<j->GetName()<<"' is registered as '"<<it->first<<"'");
    }


    // check if the joint names maintained in 'joints' match the names in gazebo,
    // that the joints can be used by this class, and if yes, load PID controllers.
    int i = 0;
    for (std::vector<std::string>::iterator it = joint_names.begin();
            it != joint_names.end(); ++it)
    {
        // ROS_INFO_STREAM("Local joint name: '"<<*it<<"'");

        physics::JointPtr joint = _parent->GetJoint(*it);
        if (!joint.get())
        {
            ROS_FATAL_STREAM("Joint name " << *it << " not found as robot joint");
            throw std::string("Joint not found");
        }

        std::string scopedName = joint->GetScopedName();
        std::map<std::string, physics::JointPtr >::iterator jit = jntMap.find(scopedName);
        if (jit == jntMap.end())
        {
            ROS_ERROR_STREAM("Joint name " << *it << " not found in joint controller joints");
            throw std::string("Joint not found");
        }

        ++i;
    }

    model = _parent;

    jsSub = nh.subscribe(jointStateTopic, 1, &GazeboJointStateClient::JointStateCallback, this);
}
    private: void worldUpdate(){
#if(PRINT_DEBUG)
       cout << "Updating the world for time: " << world->GetSimTime().Float() << endl;
#endif
        outputCSV << world->GetSimTime().Float() << ", ";
        for(unsigned int i = 0; i < boost::size(contacts); ++i){
           outputCSV << contactForces[contacts[i]].GetLength() << ", ";
        }
        outputCSV << endl;

        if(world->GetSimTime().Float() >= MAX_TIME){
          #if(PRINT_DEBUG)
          cout << "Scenario completed. Updating results" << endl;
          #endif
          event::Events::DisconnectWorldUpdateBegin(this->connection);

          // Disconnect the sensors
          for(unsigned int i = 0; i < boost::size(contacts); ++i){
            sensors::SensorPtr sensor = sensors::SensorManager::Instance()->GetSensor(world->GetName() + "::" + model->GetScopedName()
                                                       + "::" + contacts[i]);
            if(sensor == nullptr){
              cout << "Could not find sensor " << contacts[i] << endl;
              continue;
            }
            sensor->SetActive(false);
            sensor->DisconnectUpdated(sensorConnections[i]);
          }

          sensorConnections.clear();
          outputCSV << endl;
          outputCSV.close();
        }
    }
////////////////////////////////////////////////////////////////////////////////
// Load the controller
void GazeboRosP3D::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
{
  // Get the world name.
  this->world_ = _parent->GetWorld();
  this->model_ = _parent;

  // load parameters
  this->robot_namespace_ = "";
  if (_sdf->HasElement("robotNamespace"))
    this->robot_namespace_ =
      _sdf->GetElement("robotNamespace")->Get<std::string>() + "/";

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

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

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

  if (!_sdf->HasElement("frameName"))
  {
    ROS_DEBUG("p3d plugin missing <frameName>, defaults to world");
    this->frame_name_ = "world";
  }
  else
    this->frame_name_ = _sdf->GetElement("frameName")->Get<std::string>();

  if (!_sdf->HasElement("xyzOffset"))
  {
    ROS_DEBUG("p3d plugin missing <xyzOffset>, defaults to 0s");
    this->offset_.pos = math::Vector3(0, 0, 0);
  }
  else
    this->offset_.pos = _sdf->GetElement("xyzOffset")->Get<math::Vector3>();

  if (!_sdf->HasElement("rpyOffset"))
  {
    ROS_DEBUG("p3d plugin missing <rpyOffset>, defaults to 0s");
    this->offset_.rot = math::Vector3(0, 0, 0);
  }
  else
    this->offset_.rot = _sdf->GetElement("rpyOffset")->Get<math::Vector3>();

  if (!_sdf->HasElement("gaussianNoise"))
  {
    ROS_DEBUG("p3d plugin missing <gaussianNoise>, defaults to 0.0");
    this->gaussian_noise_ = 0;
  }
  else
    this->gaussian_noise_ = _sdf->GetElement("gaussianNoise")->Get<double>();

  if (!_sdf->HasElement("updateRate"))
  {
    ROS_DEBUG("p3d plugin missing <updateRate>, defaults to 0.0"
             " (as fast as possible)");
    this->update_rate_ = 0;
  }
  else
    this->update_rate_ = _sdf->GetElement("updateRate")->Get<double>();

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

  // publish multi queue
  this->pmq.startServiceThread();

  // resolve tf prefix
  std::string prefix;
  this->rosnode_->getParam(std::string("tf_prefix"), prefix);
  this->tf_frame_name_ = tf::resolve(prefix, this->frame_name_);

  if (this->topic_name_ != "")
  {
    this->pub_Queue = this->pmq.addPub<nav_msgs::Odometry>();
    this->pub_ =
      this->rosnode_->advertise<nav_msgs::Odometry>(this->topic_name_, 1);
  }

  this->last_time_ = this->world_->GetSimTime();
  // initialize body
  this->last_vpos_ = this->link_->GetWorldLinearVel();
  this->last_veul_ = this->link_->GetWorldAngularVel();
  this->apos_ = 0;
  this->aeul_ = 0;

  // if frameName specified is "/world", "world", "/map" or "map" report
  // back inertial values in the gazebo world
  if (this->frame_name_ != "/world" &&
      this->frame_name_ != "world" &&
      this->frame_name_ != "/map" &&
      this->frame_name_ != "map")
  {
    this->reference_link_ = this->model_->GetLink(this->frame_name_);
    if (!this->reference_link_)
    {
      ROS_ERROR("gazebo_ros_p3d plugin: frameName: %s does not exist, will"
                " not publish pose\n", this->frame_name_.c_str());
      return;
    }
  }

  // init reference frame state
  if (this->reference_link_)
  {
    ROS_DEBUG("got body %s", this->reference_link_->GetName().c_str());
    this->frame_apos_ = 0;
    this->frame_aeul_ = 0;
    this->last_frame_vpos_ = this->reference_link_->GetWorldLinearVel();
    this->last_frame_veul_ = this->reference_link_->GetWorldAngularVel();
  }


  // start custom queue for p3d
  this->callback_queue_thread_ = boost::thread(
    boost::bind(&GazeboRosP3D::P3DQueueThread, 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(&GazeboRosP3D::UpdateChild, this));
}
void GazeboPressurePlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) {
  if (kPrintOnPluginLoad) {
    gzdbg << __FUNCTION__ << "() called." << std::endl;
  }

  gzdbg << "_model = " << _model->GetName() << std::endl;

  // Store the pointer to the model and the world.
  model_ = _model;
  world_ = model_->GetWorld();

  //==============================================//
  //========== READ IN PARAMS FROM SDF ===========//
  //==============================================//

  // Use the robot namespace to create the node handle.
  if (_sdf->HasElement("robotNamespace"))
    namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>();
  else
    gzerr << "[gazebo_pressure_plugin] Please specify a robotNamespace.\n";

  // Get node handle.
  node_handle_ = transport::NodePtr(new transport::Node());

  // Initialise with default namespace (typically /gazebo/default/).
  node_handle_->Init();

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

  frame_id_ = link_name;

  // Retrieve the rest of the SDF parameters.
  getSdfParam<std::string>(_sdf, "pressureTopic", pressure_topic_, kDefaultPressurePubTopic);
  getSdfParam<double>(_sdf, "referenceAltitude", ref_alt_, kDefaultRefAlt);
  getSdfParam<double>(_sdf, "pressureVariance", pressure_var_, kDefaultPressureVar);
  CHECK(pressure_var_ >= 0.0);

  // Initialize the normal distribution for pressure.
  double mean = 0.0;
  pressure_n_[0] = NormalDistribution(mean, sqrt(pressure_var_));

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

  //==============================================//
  //=== POPULATE STATIC PARTS OF PRESSURE MSG ====//
  //==============================================//

  pressure_message_.mutable_header()->set_frame_id(frame_id_);
  pressure_message_.set_variance(pressure_var_);
}
////////////////////////////////////////////////////////////////////////////////
// Load the controller
void DRCVehicleROSPlugin::Load(physics::ModelPtr _parent,
                                 sdf::ElementPtr _sdf)
{
  DRCVehiclePlugin::Load(_parent, _sdf);

  // initialize ros
  if (!ros::isInitialized())
  {
    gzerr << "Not loading plugin since ROS hasn't been "
          << "properly initialized.  Try starting gazebo with ros plugin:\n"
          << "  gazebo -s libgazebo_ros_api_plugin.so\n";
    return;
  }

  // ros stuff
  this->rosNode = new ros::NodeHandle("");

  // Get the world name.
  this->world = _parent->GetWorld();
  this->model = _parent;

  ros::SubscribeOptions hand_wheel_cmd_so =
    ros::SubscribeOptions::create<std_msgs::Float64>(
    this->model->GetName() + "/hand_wheel/cmd", 100,
    boost::bind(static_cast<void (DRCVehicleROSPlugin::*)
      (const std_msgs::Float64::ConstPtr&)>(
        &DRCVehicleROSPlugin::SetHandWheelState), this, _1),
    ros::VoidPtr(), &this->queue);
  this->subHandWheelCmd = this->rosNode->subscribe(hand_wheel_cmd_so);

  ros::SubscribeOptions hand_brake_cmd_so =
    ros::SubscribeOptions::create<std_msgs::Float64>(
    this->model->GetName() + "/hand_brake/cmd", 100,
    boost::bind(static_cast<void (DRCVehicleROSPlugin::*)
      (const std_msgs::Float64::ConstPtr&)>(
        &DRCVehicleROSPlugin::SetHandBrakeState), this, _1),
    ros::VoidPtr(), &this->queue);
  this->subHandBrakeCmd = this->rosNode->subscribe(hand_brake_cmd_so);

  ros::SubscribeOptions gas_pedal_cmd_so =
    ros::SubscribeOptions::create<std_msgs::Float64>(
    this->model->GetName() + "/gas_pedal/cmd", 100,
    boost::bind(static_cast<void (DRCVehicleROSPlugin::*)
      (const std_msgs::Float64::ConstPtr&)>(
        &DRCVehicleROSPlugin::SetGasPedalState), this, _1),
    ros::VoidPtr(), &this->queue);
  this->subGasPedalCmd = this->rosNode->subscribe(gas_pedal_cmd_so);

  ros::SubscribeOptions brake_pedal_cmd_so =
    ros::SubscribeOptions::create<std_msgs::Float64>(
    this->model->GetName() + "/brake_pedal/cmd", 100,
    boost::bind(static_cast<void (DRCVehicleROSPlugin::*)
      (const std_msgs::Float64::ConstPtr&)>(
        &DRCVehicleROSPlugin::SetBrakePedalState), this, _1),
    ros::VoidPtr(), &this->queue);
  this->subBrakePedalCmd = this->rosNode->subscribe(brake_pedal_cmd_so);

  ros::SubscribeOptions key_cmd_so =
    ros::SubscribeOptions::create<std_msgs::Int8>(
    this->model->GetName() + "/key/cmd", 100,
    boost::bind(static_cast<void (DRCVehicleROSPlugin::*)
      (const std_msgs::Int8::ConstPtr&)>(
        &DRCVehicleROSPlugin::SetKeyState), this, _1),
    ros::VoidPtr(), &this->queue);
  this->subKeyCmd = this->rosNode->subscribe(key_cmd_so);

  ros::SubscribeOptions direction_cmd_so =
    ros::SubscribeOptions::create<std_msgs::Int8>(
    this->model->GetName() + "/direction/cmd", 100,
    boost::bind(static_cast<void (DRCVehicleROSPlugin::*)
      (const std_msgs::Int8::ConstPtr&)>(
        &DRCVehicleROSPlugin::SetDirectionState), this, _1),
    ros::VoidPtr(), &this->queue);
  this->subDirectionCmd = this->rosNode->subscribe(direction_cmd_so);

  this->pubHandWheelState = this->rosNode->advertise<std_msgs::Float64>(
    this->model->GetName() + "/hand_wheel/state", 10);
  this->pubHandBrakeState = this->rosNode->advertise<std_msgs::Float64>(
    this->model->GetName() + "/hand_brake/state", 10);
  this->pubGasPedalState = this->rosNode->advertise<std_msgs::Float64>(
    this->model->GetName() + "/gas_pedal/state", 10);
  this->pubBrakePedalState = this->rosNode->advertise<std_msgs::Float64>(
    this->model->GetName() + "/brake_pedal/state", 10);
  this->pubKeyState = this->rosNode->advertise<std_msgs::Int8>(
    this->model->GetName() + "/key/state", 10);
  this->pubDirectionState = this->rosNode->advertise<std_msgs::Int8>(
    this->model->GetName() + "/direction/state", 10);

  // ros callback queue for processing subscription
  this->callbackQueueThread = boost::thread(
    boost::bind(&DRCVehicleROSPlugin::QueueThread, this));

  this->ros_publish_connection_ = event::Events::ConnectWorldUpdateBegin(
      boost::bind(&DRCVehicleROSPlugin::RosPublishStates, this));
}
Exemplo n.º 15
0
////////////////////////////////////////////////////////////////////////////////
// Load the controller
void GazeboRosGps::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
{
  world = _model->GetWorld();

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

  if (!_sdf->HasElement("bodyName"))
  {
    link = _model->GetLink();
    link_name_ = link->GetName();
  }
  else {
    link_name_ = _sdf->GetElement("bodyName")->GetValue()->GetAsString();
    link = _model->GetLink(link_name_);
  }

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

  // default parameters
  frame_id_ = "/world";
  fix_topic_ = "fix";
  velocity_topic_ = "fix_velocity";

  reference_latitude_  = DEFAULT_REFERENCE_LATITUDE;
  reference_longitude_ = DEFAULT_REFERENCE_LONGITUDE;
  reference_heading_   = DEFAULT_REFERENCE_HEADING * M_PI/180.0;
  reference_altitude_  = DEFAULT_REFERENCE_ALTITUDE;

  status_ = sensor_msgs::NavSatStatus::STATUS_FIX;
  service_ = sensor_msgs::NavSatStatus::SERVICE_GPS;

  fix_.header.frame_id = frame_id_;
  fix_.status.status  = status_;
  fix_.status.service = service_;
  velocity_.header.frame_id = frame_id_;

  if (_sdf->HasElement("frameId"))
    frame_id_ = _sdf->GetElement("frameId")->GetValue()->GetAsString();

  if (_sdf->HasElement("topicName"))
    fix_topic_ = _sdf->GetElement("topicName")->GetValue()->GetAsString();

  if (_sdf->HasElement("velocityTopicName"))
    velocity_topic_ = _sdf->GetElement("velocityTopicName")->GetValue()->GetAsString();

  if (_sdf->HasElement("referenceLatitude"))
    _sdf->GetElement("referenceLatitude")->GetValue()->Get(reference_latitude_);

  if (_sdf->HasElement("referenceLongitude"))
    _sdf->GetElement("referenceLongitude")->GetValue()->Get(reference_longitude_);

  if (_sdf->HasElement("referenceHeading"))
    if (_sdf->GetElement("referenceHeading")->GetValue()->Get(reference_heading_))
      reference_heading_ *= M_PI/180.0;

  if (_sdf->HasElement("referenceAltitude"))
    _sdf->GetElement("referenceAltitude")->GetValue()->Get(reference_altitude_);

  if (_sdf->HasElement("status"))
    _sdf->GetElement("status")->GetValue()->Get(status_);

  if (_sdf->HasElement("service"))
    _sdf->GetElement("service")->GetValue()->Get(service_);

  fix_.header.frame_id = frame_id_;
  fix_.status.status  = status_;
  fix_.status.service = service_;
  velocity_.header.frame_id = frame_id_;

  position_error_model_.Load(_sdf);
  velocity_error_model_.Load(_sdf, "velocity");

  // calculate earth radii
  double temp = 1.0 / (1.0 - excentrity2 * sin(reference_latitude_ * M_PI/180.0) * sin(reference_latitude_ * M_PI/180.0));
  double prime_vertical_radius = equatorial_radius * sqrt(temp);
  radius_north_ = prime_vertical_radius * (1 - excentrity2) * temp;
  radius_east_  = prime_vertical_radius * cos(reference_latitude_ * M_PI/180.0);

  // 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_);
  fix_publisher_ = node_handle_->advertise<sensor_msgs::NavSatFix>(fix_topic_, 10);
  velocity_publisher_ = node_handle_->advertise<geometry_msgs::Vector3Stamped>(velocity_topic_, 10);

  Reset();

  // connect Update function
  updateTimer.setUpdateRate(4.0);
  updateTimer.Load(world, _sdf);
  updateConnection = updateTimer.Connect(boost::bind(&GazeboRosGps::Update, this));
}
Exemplo n.º 16
0
////////////////////////////////////////////////////////////////////////////////
// Load the controller
void BonebrakerController::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
{

   std::cerr << std::endl << std::endl << "Cargando plugin" << std::endl << std::endl;
  world = _model->GetWorld();

  // load parameters
  if (!_sdf->HasElement("robotNamespace") || !_sdf->GetElement("robotNamespace")->GetValue())
   namespace_.clear();
  else
   namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>() + "/";

  if (!_sdf->HasElement("controlReferencesRW") || !_sdf->GetElement("controlReferencesRW")->GetValue())
   control_ref_rw_topic_ = "/bonebraker/control_ref_gazebo";
  else
   control_ref_rw_topic_ = _sdf->GetElement("control_ref_gazebo")->Get<std::string>();


  if (!_sdf->HasElement("joystickTopic") || !_sdf->GetElement("joystickTopic")->GetValue())
   joystick_topic_ = "/bonebraker/joystick_control";
  else
   joystick_topic_ = _sdf->GetElement("joystickTopic")->Get<std::string>();

  if (!_sdf->HasElement("jointsState") || !_sdf->GetElement("jointsState")->GetValue())
   joint_state_subscriber_topic_ = "/bonebraker/joints_state";
  else
   joint_state_subscriber_topic_ = _sdf->GetElement("jointsState")->Get<std::string>();

  if (!_sdf->HasElement("armControlReference") || !_sdf->GetElement("armControlReference")->GetValue())
   arm_control_ref_topic_ = "/bonebraker/arm_control_ref_gazebo";
  else
   arm_control_ref_topic_ = _sdf->GetElement("armControlReference")->Get<std::string>();


  if (!_sdf->HasElement("jointPosition_gazebo") || !_sdf->GetElement("jointPosition_gazebo")->GetValue())
   joint_position_publisher_topic_ = "/bonebraker/set_joint_position";
  else
   joint_position_publisher_topic_ = _sdf->GetElement("jointPosition_gazebo")->Get<std::string>();

  if (!_sdf->HasElement("imuTopic") || !_sdf->GetElement("imuTopic")->GetValue())
   imu_topic_.clear();
  else
   imu_topic_ = _sdf->GetElement("imuTopic")->Get<std::string>();

  if (!_sdf->HasElement("stateTopic") || !_sdf->GetElement("stateTopic")->GetValue())
   state_topic_.clear();
  else
   state_topic_ = _sdf->GetElement("stateTopic")->Get<std::string>();

  if (!_sdf->HasElement("bodyName") || !_sdf->GetElement("bodyName")->GetValue())
  {
   link = _model->GetLink();
   link_name_ = link->GetName();
  }
  else {
   link_name_ = _sdf->GetElement("bodyName")->Get<std::string>();
   link = _model->GetLink(link_name_);
  }

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

  if (!_sdf->HasElement("maxForce") || !_sdf->GetElement("maxForce")->GetValue())
   max_force_ = -1;
  else
   max_force_ = _sdf->GetElement("maxForce")->Get<double>();


  // Get inertia and mass of quadrotor body
  inertia = link->GetInertial()->GetPrincipalMoments();
  mass = link->GetInertial()->GetMass();

  node_handle_ = new ros::NodeHandle(namespace_);

  // joystick command
  if (!joystick_topic_.empty())
  {
   ros::SubscribeOptions ops = ros::SubscribeOptions::create<arcas_msgs::Joystick>(
     joystick_topic_, 1,
     boost::bind(&BonebrakerController::Joystick_Callback, this, _1),
     ros::VoidPtr(), &callback_queue_);
   joystick_subscriber_ = node_handle_->subscribe(ops);
  }

  // joints State
  if (!joint_state_subscriber_topic_.empty())
  {
   ros::SubscribeOptions ops = ros::SubscribeOptions::create<sensor_msgs::JointState>(
     joint_state_subscriber_topic_, 15,
     boost::bind(&BonebrakerController::JointsState_Callback, this, _1),
     ros::VoidPtr(), &callback_queue_);
   joint_state_subscriber_ = node_handle_->subscribe(ops);
  }

  // subscribe command
  if (!control_ref_rw_topic_.empty())
  {
   ros::SubscribeOptions ops = ros::SubscribeOptions::create<arcas_msgs::QuadControlReferencesStamped>(
     control_ref_rw_topic_, 1,
     boost::bind(&BonebrakerController::QuadControlRefRW_Callback, this, _1),
     ros::VoidPtr(), &callback_queue_);
   control_ref_rw_subscriber_ = node_handle_->subscribe(ops);
  }
  // subscribe command
  if (!arm_control_ref_topic_.empty())
  {
   ros::SubscribeOptions ops = ros::SubscribeOptions::create<arcas_msgs::ArmControlReferencesStamped>(
     arm_control_ref_topic_, 1,
     boost::bind(&BonebrakerController::ArmControlRef_Callback, this, _1),
     ros::VoidPtr(), &callback_queue_);
   arm_control_ref_subscriber_ = node_handle_->subscribe(ops);
  }


  // subscribe imu
  if (!imu_topic_.empty())
  {
   ros::SubscribeOptions ops = ros::SubscribeOptions::create<sensor_msgs::Imu>(
     imu_topic_, 1,
     boost::bind(&BonebrakerController::ImuCallback, this, _1),
     ros::VoidPtr(), &callback_queue_);
   imu_subscriber_ = node_handle_->subscribe(ops);

   ROS_INFO_NAMED("quadrotor_simple_controller", "Using imu information on topic %s as source of orientation and angular velocity.", imu_topic_.c_str());
  }

  // subscribe state
  if (!state_topic_.empty())
  {
   ros::SubscribeOptions ops = ros::SubscribeOptions::create<nav_msgs::Odometry>(
     state_topic_, 1,
     boost::bind(&BonebrakerController::StateCallback, this, _1),
     ros::VoidPtr(), &callback_queue_);
   state_subscriber_ = node_handle_->subscribe(ops);

   ROS_INFO_NAMED("quadrotor_simple_controller", "Using state information on topic %s as source of state information.", state_topic_.c_str());
  }



  if(!joint_position_publisher_topic_.empty())
  {
     joint_position_publisher_ = node_handle_->advertise<arcas_msgs::JointControl>(joint_position_publisher_topic_,0);
  }

  // callback_queue_thread_ = boost::thread( boost::bind( &GazeboQuadrotorSimpleController::CallbackQueueThread,this ) );


  Reset();

  // New Mechanism for Updating every World Cycle
  // Listen to the update event. This event is broadcast every
  // simulation iteration.
  controlTimer.Load(world, _sdf);
  updateConnection = event::Events::ConnectWorldUpdateBegin(
     boost::bind(&BonebrakerController::Update, this));


  //Initialize matlab model
   /* External mode */
   char *argv[3];
   char arg0[32],arg1[32],arg2[32];

   argv[0] = arg0;
   argv[1] = arg1;
   argv[2] = arg2;

   sprintf(argv[0],"exec");

   std::cerr << std::endl << std::endl << "Initialize Ext Mode" << std::endl << std::endl;
   rtERTExtModeParseArgs(1, (const char **)argv);
   /* Initialize model */
   std::cerr << std::endl << std::endl << "Initialize Matlab Model" << std::endl << std::endl;
   quadrotor_controller_initialize();

      std::cerr << std::endl << std::endl << "Matlab Initicializado" << std::endl << std::endl;
}
void GazeboFwDynamicsPlugin::Load(physics::ModelPtr _model,
                                  sdf::ElementPtr _sdf) {
  if (kPrintOnPluginLoad) {
    gzdbg << __FUNCTION__ << "() called." << std::endl;
  }

  gzdbg << "_model = " << _model->GetName() << std::endl;

  // Store the pointer to the model.
  model_ = _model;
  world_ = model_->GetWorld();

  namespace_.clear();

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

  // Create the node handle.
  node_handle_ = transport::NodePtr(new transport::Node());

  // Initialise with default namespace (typically /gazebo/default/).
  node_handle_->Init();

  // Get the link name.
  std::string link_name;
  if (_sdf->HasElement("linkName"))
    link_name = _sdf->GetElement("linkName")->Get<std::string>();
  else
    gzerr << "[gazebo_fw_dynamics_plugin] Please specify a linkName.\n";
  // Get the pointer to the link.
  link_ = model_->GetLink(link_name);
  if (link_ == NULL) {
    gzthrow("[gazebo_fw_dynamics_plugin] Couldn't find specified link \""
            << link_name << "\".");
  }

  // Get the path to fixed-wing aerodynamics parameters YAML file. If not
  // provided, default Techpod parameters are used.
  if (_sdf->HasElement("aeroParamsYAML")) {
    std::string aero_params_yaml =
        _sdf->GetElement("aeroParamsYAML")->Get<std::string>();

    aero_params_.LoadAeroParamsYAML(aero_params_yaml);
  } else {
    gzwarn << "[gazebo_fw_dynamics_plugin] No aerodynamic paramaters YAML file"
        << " specified, using default Techpod parameters.\n";
  }

  // Get the path to fixed-wing vehicle parameters YAML file. If not provided,
  // default Techpod parameters are used.
  if (_sdf->HasElement("vehicleParamsYAML")) {
    std::string vehicle_params_yaml =
        _sdf->GetElement("vehicleParamsYAML")->Get<std::string>();

    vehicle_params_.LoadVehicleParamsYAML(vehicle_params_yaml);
  } else {
    gzwarn << "[gazebo_fw_dynamics_plugin] No vehicle paramaters YAML file"
        << " specified, using default Techpod parameters.\n";
  }

  // Get the rest of the sdf parameters.
  getSdfParam<bool>(_sdf, "isInputJoystick", is_input_joystick_,
                    kDefaultIsInputJoystick);
  getSdfParam<std::string>(_sdf, "actuatorsSubTopic",
                           actuators_sub_topic_,
                           mav_msgs::default_topics::COMMAND_ACTUATORS);
  getSdfParam<std::string>(_sdf, "rollPitchYawrateThrustSubTopic",
                           roll_pitch_yawrate_thrust_sub_topic_,
                           mav_msgs::default_topics::
                               COMMAND_ROLL_PITCH_YAWRATE_THRUST);
  getSdfParam<std::string>(_sdf, "windSpeedSubTopic",
                           wind_speed_sub_topic_,
                           mav_msgs::default_topics::WIND_SPEED);

  // Listen to the update event. This event is broadcast every
  // simulation iteration.
  this->updateConnection_ = event::Events::ConnectWorldUpdateBegin(
          boost::bind(&GazeboFwDynamicsPlugin::OnUpdate, this, _1));
}
void GazeboRosControllerManager::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
{
  // Get then name of the parent model
  std::string modelName = _sdf->GetParent()->GetValueString("name");

  // Get the world name.
  this->world = _parent->GetWorld();

  // Get a pointer to the model
  this->parent_model_ = _parent;

  // Error message if the model couldn't be found
  if (!this->parent_model_)
    gzerr << "Unable to get parent model\n";

  // Listen to the update event. This event is broadcast every
  // simulation iteration.
  this->updateConnection = event::Events::ConnectWorldUpdateStart(
      boost::bind(&GazeboRosControllerManager::UpdateChild, this));
  gzdbg << "plugin model name: " << modelName << "\n";

  physics_ = false;
  this->world->EnablePhysicsEngine(physics_); //TODO should be false


  if (getenv("CHECK_SPEEDUP"))
  {
    wall_start_ = this->world->GetRealTime().Double();
    sim_start_  = this->world->GetSimTime().Double();
  }

  // check update rate against world physics update rate
  // should be equal or higher to guarantee the wrench applied is not "diluted"
  //if (this->updatePeriod > 0 &&
  //    (this->world->GetPhysicsEngine()->GetUpdateRate() > 1.0/this->updatePeriod))
  //  ROS_ERROR("gazebo_ros_force controller update rate is less than physics update rate, wrench applied will be diluted (applied intermittently)");





  // get parameter name
  this->robotNamespace = "";
  if (_sdf->HasElement("robotNamespace"))
    this->robotNamespace = _sdf->GetElement("robotNamespace")->GetValueString();

  this->robotParam = "robot_description";
  if (_sdf->HasElement("robotParam"))
    this->robotParam = _sdf->GetElement("robotParam")->GetValueString();

  this->robotParam = this->robotNamespace+"/" + this->robotParam;

  if (!ros::isInitialized())
  {
    int argc = 0;
    char** argv = NULL;
    ros::init(argc,argv,"gazebo",ros::init_options::NoSigintHandler|ros::init_options::AnonymousName);
  }
  this->rosnode_ = new ros::NodeHandle(this->robotNamespace);
  ROS_INFO("starting gazebo_ros_controller_manager plugin in ns: %s",this->robotNamespace.c_str());

  // pr2_etherCAT calls ros::spin(), we'll thread out one spinner here to mimic that
  this->ros_spinner_thread_ = boost::thread( boost::bind( &GazeboRosControllerManager::ControllerManagerROSThread,this ) );


  // load a controller manager
  this->cm_ = new pr2_controller_manager::ControllerManager(&hw_,*this->rosnode_);
  this->hw_.current_time_ = ros::Time(this->world->GetSimTime().Double());
  if (this->hw_.current_time_ < ros::Time(0.001)) this->hw_.current_time_ == ros::Time(0.001); // hardcoded to minimum of 1ms on start up

  this->rosnode_->param("gazebo/start_robot_calibrated",this->fake_calibration_,true);

  // read pr2 urdf
  // setup actuators, then setup mechanism control node
  ReadPr2Xml();

  // Initializes the fake state (for running the transmissions backwards).
  this->fake_state_ = new pr2_mechanism_model::RobotState(&this->cm_->model_);

  // The gazebo joints and mechanism joints should match up.
  if (this->cm_->state_ != NULL) // could be NULL if ReadPr2Xml is unsuccessful
  {
    for (unsigned int i = 0; i < this->cm_->state_->joint_states_.size(); ++i)
    {
      std::string joint_name = this->cm_->state_->joint_states_[i].joint_->name;

      // fill in gazebo joints pointer
      gazebo::physics::JointPtr joint = this->parent_model_->GetJoint(joint_name);
      if (joint)
      {
        this->joints_.push_back(joint);
      }
      else
      {
        //ROS_WARN("A joint named \"%s\" is not part of Mechanism Controlled joints.\n", joint_name.c_str());
        //this->joints_.push_back(NULL);  // FIXME: cannot be null, must be an empty boost shared pointer
        this->joints_.push_back(gazebo::physics::JointPtr());  // FIXME: cannot be null, must be an empty boost shared pointer
        ROS_ERROR("A joint named \"%s\" is not part of Mechanism Controlled joints.\n", joint_name.c_str());
      }

    }
  }

#ifdef USE_CBQ
  // start custom queue for controller manager
  this->controller_manager_callback_queue_thread_ = boost::thread( boost::bind( &GazeboRosControllerManager::ControllerManagerQueueThread,this ) );
#endif
}
Exemplo n.º 19
0
////////////////////////////////////////////////////////////////////////////////
// Load the controller
void GazeboRosMagnetic::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
{
    world = _model->GetWorld();

    // load parameters
    if (_sdf->HasElement("robotNamespace"))
        namespace_ = _sdf->GetElement("robotNamespace")->GetValue()->GetAsString();
    else
        namespace_.clear();

    if (!_sdf->HasElement("topicName"))
        topic_ = "magnetic";
    else
        topic_ = _sdf->GetElement("topicName")->Get<std::string>();


    if (_sdf->HasElement("bodyName"))
    {
        link_name_ = _sdf->GetElement("bodyName")->GetValue()->GetAsString();
        link = _model->GetLink(link_name_);
    }
    else
    {
        link = _model->GetLink();
        link_name_ = link->GetName();
    }

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

    // default parameters
    frame_id_ = link_name_;
    magnitude_ = DEFAULT_MAGNITUDE;
    reference_heading_ = DEFAULT_REFERENCE_HEADING * M_PI/180.0;
    declination_ = DEFAULT_DECLINATION * M_PI/180.0;
    inclination_ = DEFAULT_INCLINATION * M_PI/180.0;

    if (_sdf->HasElement("frameId"))
        frame_id_ = _sdf->GetElement("frameId")->GetValue()->GetAsString();

    if (_sdf->HasElement("magnitude"))
        _sdf->GetElement("magnitude")->GetValue()->Get(magnitude_);

    if (_sdf->HasElement("referenceHeading"))
        if (_sdf->GetElement("referenceHeading")->GetValue()->Get(reference_heading_))
            reference_heading_ *= M_PI/180.0;

    if (_sdf->HasElement("declination"))
        if (_sdf->GetElement("declination")->GetValue()->Get(declination_))
            declination_ *= M_PI/180.0;

    if (_sdf->HasElement("inclination"))
        if (_sdf->GetElement("inclination")->GetValue()->Get(inclination_))
            inclination_ *= M_PI/180.0;

    // Note: Gazebo uses NorthWestUp coordinate system, heading and declination are compass headings
    magnetic_field_.header.frame_id = frame_id_;
    magnetic_field_world_.x = magnitude_ *  cos(inclination_) * cos(reference_heading_ - declination_);
    magnetic_field_world_.y = magnitude_ *  sin(reference_heading_ - declination_);
    magnetic_field_world_.z = magnitude_ * -sin(inclination_) * cos(reference_heading_ - declination_);

    sensor_model_.Load(_sdf);

    // 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<geometry_msgs::Vector3Stamped>(topic_, 1);

    Reset();

    // connect Update function
    updateTimer.Load(world, _sdf);
    updateConnection = updateTimer.Connect(boost::bind(&GazeboRosMagnetic::Update, this));
}
void GazeboRosEklavya::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf )
{
  this->my_world_ = _parent->GetWorld();

  this->my_parent_ = _parent;
  if (!this->my_parent_)
  {
    ROS_FATAL("Gazebo_ROS_Eklavya controller requires a Model as its parent");
    return;
  }


  this->node_namespace_ = "";
  if (_sdf->HasElement("node_namespace"))
    this->node_namespace_ = _sdf->GetElement("node_namespace")->GetValueString() + "/";


  left_wheel_joint_name_ = "left_wheel_joint";
  if (_sdf->HasElement("left_wheel_joint"))
    left_wheel_joint_name_ = _sdf->GetElement("left_wheel_joint")->GetValueString();

  right_wheel_joint_name_ = "right_wheel_joint";
  if (_sdf->HasElement("right_wheel_joint"))
    right_wheel_joint_name_ = _sdf->GetElement("right_wheel_joint")->GetValueString();

  front_castor_wheel_joint_name_ = "front_castor_wheel_joint";
  if (_sdf->HasElement("front_castor_wheel_joint"))
    front_castor_wheel_joint_name_ = _sdf->GetElement("front_castor_wheel_joint")->GetValueString();

  front_castor_swivel_joint_name_ = "front_castor_swivel_joint";
  if (_sdf->HasElement("front_castor_swivel_joint"))
    front_castor_swivel_joint_name_ = _sdf->GetElement("front_castor_swivel_joint")->GetValueString();

  wheel_sep_ = 0.34;
  if (_sdf->HasElement("wheel_separation"))
    wheel_sep_ = _sdf->GetElement("wheel_separation")->GetValueDouble();

  wheel_sep_ = 0.34;
  if (_sdf->HasElement("wheel_separation"))
    wheel_sep_ = _sdf->GetElement("wheel_separation")->GetValueDouble();

  wheel_diam_ = 0.15;
  if (_sdf->HasElement("wheel_diameter"))
    wheel_diam_ = _sdf->GetElement("wheel_diameter")->GetValueDouble();

  torque_ = 10.0;
  if (_sdf->HasElement("torque"))
    torque_ = _sdf->GetElement("torque")->GetValueDouble();


  
  // Get then name of the parent model
  std::string modelName = _sdf->GetParent()->GetValueString("name");

  // Listen to the update event. This event is broadcast every
  // simulation iteration.
  this->updateConnection = event::Events::ConnectWorldUpdateStart(
      boost::bind(&GazeboRosEklavya::UpdateChild, this));
  gzdbg << "plugin model name: " << modelName << "\n";





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

  rosnode_ = new ros::NodeHandle( node_namespace_ );

  cmd_vel_sub_ = rosnode_->subscribe("/cmd_vel", 1, &GazeboRosEklavya::OnCmdVel, this );
  odom_pub_ = rosnode_->advertise<nav_msgs::Odometry>("/odom", 1);

  joint_state_pub_ = rosnode_->advertise<sensor_msgs::JointState>("/joint_states", 1);

  js_.name.push_back( left_wheel_joint_name_ );
  js_.position.push_back(0);
  js_.velocity.push_back(0);
  js_.effort.push_back(0);

  js_.name.push_back( right_wheel_joint_name_ );
  js_.position.push_back(0);
  js_.velocity.push_back(0);
  js_.effort.push_back(0);

  js_.name.push_back( front_castor_wheel_joint_name_ );
  js_.position.push_back(0);
  js_.velocity.push_back(0);
  js_.effort.push_back(0);

  js_.name.push_back( front_castor_swivel_joint_name_ );
  js_.position.push_back(0);
  js_.velocity.push_back(0);
  js_.effort.push_back(0);

  prev_update_time_ = 0;
  last_cmd_vel_time_ = 0;



  //TODO: fix this

  joints_[LEFT] = my_parent_->GetJoint(left_wheel_joint_name_);
  joints_[RIGHT] = my_parent_->GetJoint(right_wheel_joint_name_);
  joints_[FRONT_CASTOR_WHEEL] = my_parent_->GetJoint(front_castor_wheel_joint_name_);
  joints_[FRONT_CASTOR_SWIVEL] = my_parent_->GetJoint(front_castor_swivel_joint_name_);

  if (joints_[LEFT]) set_joints_[LEFT] = true;
  if (joints_[RIGHT]) set_joints_[RIGHT] = true;
  if (joints_[FRONT_CASTOR_WHEEL]) set_joints_[FRONT_CASTOR_WHEEL] = true;
  if (joints_[FRONT_CASTOR_SWIVEL]) set_joints_[FRONT_CASTOR_SWIVEL] = true;

  //initialize time and odometry position
  prev_update_time_ = last_cmd_vel_time_ = this->my_world_->GetSimTime();
  odom_pose_[0] = 0.0;
  odom_pose_[1] = 0.0;
  odom_pose_[2] = 0.0;
}
////////////////////////////////////////////////////////////////////////////////
// Load the controller
void GazeboRosVacuumGripper::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
{
  ROS_INFO("Loading gazebo_ros_vacuum_gripper");

  // Set attached model;
  parent_ = _model;

  // Get the world name.
  world_ = _model->GetWorld();

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

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

  link_ = _model->GetLink(link_name_);
  if (!link_)
  {
    std::string found;
    physics::Link_V links = _model->GetLinks();
    for (size_t i = 0; i < links.size(); i++) {
      found += std::string(" ") + links[i]->GetName();
    }
    ROS_FATAL("gazebo_ros_vacuum_gripper plugin error: link named: %s does not exist", link_name_.c_str());
    ROS_FATAL("gazebo_ros_vacuum_gripper plugin error: You should check it exists and is not connected with fixed joint");
    ROS_FATAL("gazebo_ros_vacuum_gripper plugin error: Found links are: %s", found.c_str());
    return;
  }

  if (!_sdf->HasElement("topicName"))
  {
    ROS_FATAL("vacuum_gripper plugin missing <serviceName>, cannot proceed");
    return;
  }
  else
    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;
  }

  rosnode_ = new ros::NodeHandle(robot_namespace_);

  // Custom Callback Queue
  ros::AdvertiseOptions ao = ros::AdvertiseOptions::create<std_msgs::Bool>(
    topic_name_, 1,
    boost::bind(&GazeboRosVacuumGripper::Connect, this),
    boost::bind(&GazeboRosVacuumGripper::Disconnect, this),
    ros::VoidPtr(), &queue_);
  pub_ = rosnode_->advertise(ao);

  // Custom Callback Queue
  ros::AdvertiseServiceOptions aso1 =
    ros::AdvertiseServiceOptions::create<std_srvs::Empty>(
    "on", boost::bind(&GazeboRosVacuumGripper::OnServiceCallback,
    this, _1, _2), ros::VoidPtr(), &queue_);
  srv1_ = rosnode_->advertiseService(aso1);
  ros::AdvertiseServiceOptions aso2 =
    ros::AdvertiseServiceOptions::create<std_srvs::Empty>(
    "off", boost::bind(&GazeboRosVacuumGripper::OffServiceCallback,
    this, _1, _2), ros::VoidPtr(), &queue_);
  srv2_ = rosnode_->advertiseService(aso2);

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

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

  ROS_INFO("Loaded gazebo_ros_vacuum_gripper");
}
////////////////////////////////////////////////////////////////////////////////
// Load the controller
void GazeboQuadrotorSimpleController::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
{
  world = _model->GetWorld();

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

  if (!_sdf->HasElement("topicName") || !_sdf->GetElement("topicName")->GetValue())
    velocity_topic_ = "cmd_vel";
  else
    velocity_topic_ = _sdf->GetElement("topicName")->GetValueString();

  if (!_sdf->HasElement("imuTopic") || !_sdf->GetElement("imuTopic")->GetValue())
    imu_topic_.clear();
  else
    imu_topic_ = _sdf->GetElement("imuTopic")->GetValueString();

  if (!_sdf->HasElement("stateTopic") || !_sdf->GetElement("stateTopic")->GetValue())
    state_topic_.clear();
  else
    state_topic_ = _sdf->GetElement("stateTopic")->GetValueString();

  if (!_sdf->HasElement("bodyName") || !_sdf->GetElement("bodyName")->GetValue())
  {
    link = _model->GetLink();
    link_name_ = link->GetName();
  }
  else {
    link_name_ = _sdf->GetElement("bodyName")->GetValueString();
    link = _model->GetLink(link_name_);
  }

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

  if (!_sdf->HasElement("maxForce") || !_sdf->GetElement("maxForce")->GetValue())
    max_force_ = -1;
  else
    max_force_ = _sdf->GetElement("maxForce")->GetValueDouble();

  controllers_.roll.Load(_sdf, "rollpitch");
  controllers_.pitch.Load(_sdf, "rollpitch");
  controllers_.yaw.Load(_sdf, "yaw");
  controllers_.velocity_x.Load(_sdf, "velocityXY");
  controllers_.velocity_y.Load(_sdf, "velocityXY");
  controllers_.velocity_z.Load(_sdf, "velocityZ");

  // Get inertia and mass of quadrotor body
  inertia = link->GetInertial()->GetPrincipalMoments();
  mass = link->GetInertial()->GetMass();

  node_handle_ = new ros::NodeHandle(namespace_);

  // subscribe command
  if (!velocity_topic_.empty())
  {
    ros::SubscribeOptions ops = ros::SubscribeOptions::create<geometry_msgs::Twist>(
      velocity_topic_, 1,
      boost::bind(&GazeboQuadrotorSimpleController::VelocityCallback, this, _1),
      ros::VoidPtr(), &callback_queue_);
    velocity_subscriber_ = node_handle_->subscribe(ops);
  }

  // subscribe imu
  if (!imu_topic_.empty())
  {
    ros::SubscribeOptions ops = ros::SubscribeOptions::create<sensor_msgs::Imu>(
      imu_topic_, 1,
      boost::bind(&GazeboQuadrotorSimpleController::ImuCallback, this, _1),
      ros::VoidPtr(), &callback_queue_);
    imu_subscriber_ = node_handle_->subscribe(ops);

    ROS_INFO_NAMED("quadrotor_simple_controller", "Using imu information on topic %s as source of orientation and angular velocity.", imu_topic_.c_str());
  }

  // subscribe state
  if (!state_topic_.empty())
  {
    ros::SubscribeOptions ops = ros::SubscribeOptions::create<nav_msgs::Odometry>(
      state_topic_, 1,
      boost::bind(&GazeboQuadrotorSimpleController::StateCallback, this, _1),
      ros::VoidPtr(), &callback_queue_);
    state_subscriber_ = node_handle_->subscribe(ops);

    ROS_INFO_NAMED("quadrotor_simple_controller", "Using state information on topic %s as source of state information.", state_topic_.c_str());
  }

  // callback_queue_thread_ = boost::thread( boost::bind( &GazeboQuadrotorSimpleController::CallbackQueueThread,this ) );


  Reset();

  // New Mechanism for Updating every World Cycle
  // Listen to the update event. This event is broadcast every
  // simulation iteration.
  controlTimer.Load(world, _sdf);
  updateConnection = event::Events::ConnectWorldUpdateBegin(
      boost::bind(&GazeboQuadrotorSimpleController::Update, this));
}
Exemplo n.º 23
0
////////////////////////////////////////////////////////////////////////////////
// Load the controller
void DRCVehicleROSPlugin::Load(physics::ModelPtr _parent,
                                 sdf::ElementPtr _sdf)
{
  // By default, cheats are off.  Allow override via environment variable.
  char* cheatsEnabledString = getenv("VRC_CHEATS_ENABLED");
  if (cheatsEnabledString && (std::string(cheatsEnabledString) == "1"))
    this->cheatsEnabled = true;
  else
    this->cheatsEnabled = false;

  try
  {
  DRCVehiclePlugin::Load(_parent, _sdf);
  }
  catch(gazebo::common::Exception &_e)
  {
    gzerr << "Error loading plugin."
          << "Please ensure that your vehicle model is correct and up-to-date."
          << '\n';
    return;
  }

  // initialize ros
  if (!ros::isInitialized())
  {
    gzerr << "Not loading plugin since ROS hasn't been "
          << "properly initialized.  Try starting gazebo with ros plugin:\n"
          << "  gazebo -s libgazebo_ros_api_plugin.so\n";
    return;
  }

  // ros stuff
  this->rosNode = new ros::NodeHandle("");

  // Get the world name.
  this->world = _parent->GetWorld();
  this->model = _parent;

  if (this->cheatsEnabled)
  {
    ros::SubscribeOptions hand_wheel_cmd_so =
      ros::SubscribeOptions::create<std_msgs::Float64>(
      this->model->GetName() + "/hand_wheel/cmd", 100,
      boost::bind(static_cast< void (DRCVehicleROSPlugin::*)
        (const std_msgs::Float64::ConstPtr&) >(
          &DRCVehicleROSPlugin::SetHandWheelState), this, _1),
      ros::VoidPtr(), &this->queue);
    this->subHandWheelCmd = this->rosNode->subscribe(hand_wheel_cmd_so);
  
    ros::SubscribeOptions hand_brake_cmd_so =
      ros::SubscribeOptions::create< std_msgs::Float64 >(
      this->model->GetName() + "/hand_brake/cmd", 100,
      boost::bind(static_cast< void (DRCVehicleROSPlugin::*)
        (const std_msgs::Float64::ConstPtr&) >(
          &DRCVehicleROSPlugin::SetHandBrakePercent), this, _1),
      ros::VoidPtr(), &this->queue);
    this->subHandBrakeCmd = this->rosNode->subscribe(hand_brake_cmd_so);
  
    ros::SubscribeOptions gas_pedal_cmd_so =
      ros::SubscribeOptions::create< std_msgs::Float64 >(
      this->model->GetName() + "/gas_pedal/cmd", 100,
      boost::bind(static_cast< void (DRCVehicleROSPlugin::*)
        (const std_msgs::Float64::ConstPtr&) >(
          &DRCVehicleROSPlugin::SetGasPedalPercent), this, _1),
      ros::VoidPtr(), &this->queue);
    this->subGasPedalCmd = this->rosNode->subscribe(gas_pedal_cmd_so);
  
    ros::SubscribeOptions brake_pedal_cmd_so =
      ros::SubscribeOptions::create< std_msgs::Float64 >(
      this->model->GetName() + "/brake_pedal/cmd", 100,
      boost::bind(static_cast< void (DRCVehicleROSPlugin::*)
        (const std_msgs::Float64::ConstPtr&) >(
          &DRCVehicleROSPlugin::SetBrakePedalPercent), this, _1),
      ros::VoidPtr(), &this->queue);
    this->subBrakePedalCmd = this->rosNode->subscribe(brake_pedal_cmd_so);
  
    ros::SubscribeOptions key_cmd_so =
      ros::SubscribeOptions::create< std_msgs::Int8 >(
      this->model->GetName() + "/key/cmd", 100,
      boost::bind(static_cast< void (DRCVehicleROSPlugin::*)
        (const std_msgs::Int8::ConstPtr&) >(
          &DRCVehicleROSPlugin::SetKeyState), this, _1),
      ros::VoidPtr(), &this->queue);
    this->subKeyCmd = this->rosNode->subscribe(key_cmd_so);
  
    ros::SubscribeOptions direction_cmd_so =
      ros::SubscribeOptions::create< std_msgs::Int8 >(
      this->model->GetName() + "/direction/cmd", 100,
      boost::bind(static_cast< void (DRCVehicleROSPlugin::*)
        (const std_msgs::Int8::ConstPtr&) >(
          &DRCVehicleROSPlugin::SetDirectionState), this, _1),
      ros::VoidPtr(), &this->queue);
    this->subDirectionCmd = this->rosNode->subscribe(direction_cmd_so);

    this->pubHandWheelState = this->rosNode->advertise<std_msgs::Float64>(
      this->model->GetName() + "/hand_wheel/state", 10);
    this->pubHandBrakeState = this->rosNode->advertise<std_msgs::Float64>(
      this->model->GetName() + "/hand_brake/state", 10);
    this->pubGasPedalState = this->rosNode->advertise<std_msgs::Float64>(
      this->model->GetName() + "/gas_pedal/state", 10);
    this->pubBrakePedalState = this->rosNode->advertise<std_msgs::Float64>(
      this->model->GetName() + "/brake_pedal/state", 10);
    this->pubKeyState = this->rosNode->advertise<std_msgs::Int8>(
      this->model->GetName() + "/key/state", 10);
    this->pubDirectionState = this->rosNode->advertise<std_msgs::Int8>(
      this->model->GetName() + "/direction/state", 10);

    // ros callback queue for processing subscription
    this->callbackQueueThread = boost::thread(
      boost::bind(&DRCVehicleROSPlugin::QueueThread, this));

    this->ros_publish_connection_ = event::Events::ConnectWorldUpdateBegin(
        boost::bind(&DRCVehicleROSPlugin::RosPublishStates, this));
  }
}
Exemplo n.º 24
0
// Load the controller
void ServoPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
{
  // Get the world name.
  world = _model->GetWorld();

  // default parameters
  topicName = "drive";
  jointStateName = "joint_states";
  robotNamespace.clear();
  controlPeriod = 0;
  proportionalControllerGain = 8.0;
  derivativeControllerGain = 0.0;
  maximumVelocity = 0.0;
  maximumTorque = 0.0;

  // load parameters
  if (_sdf->HasElement("robotNamespace")) robotNamespace = _sdf->Get<std::string>("robotNamespace");
  if (_sdf->HasElement("topicName")) topicName = _sdf->Get<std::string>("topicName");
  if (_sdf->HasElement("jointStateName")) jointStateName = _sdf->Get<std::string>("jointStateName");
  if (_sdf->HasElement("firstServoName")) servo[FIRST].name = _sdf->Get<std::string>("firstServoName");
  if (_sdf->HasElement("firstServoAxis")) servo[FIRST].axis = _sdf->Get<math::Vector3>("firstServoAxis");
  if (_sdf->HasElement("secondServoName")) servo[SECOND].name = _sdf->Get<std::string>("secondServoName");
  if (_sdf->HasElement("secondServoAxis")) servo[SECOND].axis = _sdf->Get<math::Vector3>("secondServoAxis");
  if (_sdf->HasElement("thirdServoName")) servo[THIRD].name = _sdf->Get<std::string>("thirdServoName");
  if (_sdf->HasElement("thirdServoAxis")) servo[THIRD].axis = _sdf->Get<math::Vector3>("thirdServoAxis");
  if (_sdf->HasElement("proportionalControllerGain")) proportionalControllerGain = _sdf->Get<double>("proportionalControllerGain");
  if (_sdf->HasElement("derivativeControllerGain")) derivativeControllerGain = _sdf->Get<double>("derivativeControllerGain");
  if (_sdf->HasElement("maxVelocity")) maximumVelocity = _sdf->Get<double>("maxVelocity");
  if (_sdf->HasElement("torque")) maximumTorque = _sdf->Get<double>("torque");

  double controlRate = 0.0;
  if (_sdf->HasElement("controlRate")) controlRate = _sdf->Get<double>("controlRate");
  controlPeriod = controlRate > 0.0 ? 1.0/controlRate : 0.0;

  servo[FIRST].joint  = _model->GetJoint(servo[FIRST].name);
  servo[SECOND].joint = _model->GetJoint(servo[SECOND].name);
  servo[THIRD].joint  = _model->GetJoint(servo[THIRD].name);

  if (!servo[FIRST].joint)
    gzthrow("The controller couldn't get first joint");

  countOfServos = 1;
  if (servo[SECOND].joint) {
    countOfServos = 2;
    if (servo[THIRD].joint) {
      countOfServos = 3;
    }
  }
  else {
    if (servo[THIRD].joint) {
      gzthrow("The controller couldn't get second joint, but third joint was loaded");
    }
  }

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

  rosnode_ = new ros::NodeHandle(robotNamespace);

  transform_listener_ = new tf::TransformListener();
  transform_listener_->setExtrapolationLimit(ros::Duration(1.0));

  if (!topicName.empty()) {
    ros::SubscribeOptions so = ros::SubscribeOptions::create<geometry_msgs::QuaternionStamped>(topicName, 1,
                                                                                               boost::bind(&ServoPlugin::cmdCallback, this, _1),
                                                                                               ros::VoidPtr(), &queue_);
    sub_ = rosnode_->subscribe(so);
  }

  if (!jointStateName.empty()) {
    jointStatePub_ = rosnode_->advertise<sensor_msgs::JointState>(jointStateName, 10);
  }

  joint_state.header.frame_id = transform_listener_->resolve(_model->GetLink()->GetName());

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