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

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

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

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

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

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

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


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

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



  update_connection_ = event::Events::ConnectWorldUpdateBegin(
                                                              boost::bind(&GazeboTreasure::Update, this));
}
////////////////////////////////////////////////////////////////////////////////
// Load the controller
void GazeboPanel::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
{
  gzwarn << "The GazeboPanel plugin is DEPRECATED in ROS hydro." << std::endl;

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

  terminated_ = false;

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

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

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

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


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

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



  update_connection_ = event::Events::ConnectWorldUpdateBegin(
                                                              boost::bind(&GazeboPanel::Update, this));
}
    private: void updateMaximumHic(bool drain) {
       // Get the current velocity for the head link.
       physics::LinkPtr head = model->GetLink("head_neck");
       assert(head);

       if (!drain) {
          headLinearAcc.push_back(head->GetWorldLinearAccel());
       }
       if(headLinearAcc.size() > ACC_HISTORY_MAX || drain) {
          headLinearAcc.erase(headLinearAcc.begin(), headLinearAcc.begin() + 1);
       }

       assert(headLinearAcc.size() <= ACC_HISTORY_MAX);

       // Now search for maximum value across all size dimensions
       for (unsigned int i = 0; i < headLinearAcc.size(); ++i) {
          for (unsigned int j = i + MIN_HIC_ACC_TIME; j < headLinearAcc.size(); ++j) {
             double hicCurrMax = hic(static_cast<double>(i) / SEC_TO_MSEC, static_cast<double>(j) / SEC_TO_MSEC, accMean(headLinearAcc.begin() + i, headLinearAcc.begin() + j + 1));
             if (hicCurrMax >= maximumHic) {
                maximumHic = hicCurrMax;
                maximumHicTime = world->GetSimTime();
             }
          }
       }
    }
    private: void worldUpdate(){
        physics::LinkPtr head = model->GetLink("head_neck");
        #if(PRINT_DEBUG)
        cout << "Updating HIC for time " << world->GetSimTime() << endl;
        #endif
        updateMaximumHic(false);

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

           // Drain the HIC calculation
           for (unsigned int i = 0; i < ACC_HISTORY_MAX; ++i) {
              updateMaximumHic(true);
           }

          // 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();
          printResults();
          exit(0);
        }
    }
    public: void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf){
      world = _model->GetWorld();
      model = _model;

      #if(PRINT_DEBUG)
      cout << "Loading the angular momentum over time plugin" << endl;
      #endif
      connection = event::Events::ConnectWorldUpdateBegin(boost::bind(&AngularMomentumOverTimePlugin::worldUpdate, this));

      // Get the name of the folder to store the result in
      const char* resultsFolder = std::getenv("RESULTS_FOLDER");
      if(resultsFolder == nullptr){
         cout << "Results folder not set. Using current directory." << endl;
         resultsFolder = "./";
      }

      const string resultsFileName = string(resultsFolder) + "/" + "angular_momentum.csv";
      outputCSV.open(resultsFileName, ios::out);
      assert(outputCSV.is_open());
      writeHeader(outputCSV);

       math::Vector3 angularMomentum;
       for (unsigned int i = 0; i < boost::size(links); ++i) {
          const physics::LinkPtr link = model->GetLink(links[i]);
          angularMomentum += link->GetWorldInertiaMatrix() * link->GetWorldAngularVel();
       }

       // Write out t0
       outputCSV << world->GetSimTime().Double() << ", " << angularMomentum.x / 10.0
       << ", " << angularMomentum.y / 10.0 << ", " << angularMomentum.z / 10.0 << endl;
    }
////////////////////////////////////////////////////////////////////////////////
// Load the controller
void GazeboRosForce::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
{
  // Get the world name.
  this->world_ = _model->GetWorld();
  
  // load parameters
  this->robot_namespace_ = "";
  if (_sdf->HasElement("robotNamespace"))
    this->robot_namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>() + "/";

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

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

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


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

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

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

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

  // New Mechanism for Updating every World Cycle
  // Listen to the update event. This event is broadcast every
  // simulation iteration.
  this->update_connection_ = event::Events::ConnectWorldUpdateBegin(
      boost::bind(&GazeboRosForce::UpdateChild, this));
}
示例#7
0
bool findLinkByName(physics::ModelPtr model, physics::LinkPtr &link, const std::string name) {
	link = model->GetLink(name);
	if (!link) {
		gzerr << "link by name [" << name << "] not found in model\n";
		return false;
	}
	return true;
}
    public: void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf){
      world = _model->GetWorld();
      model = _model;
      trunk = model->GetLink("trunk");

      #if(PRINT_DEBUG)
      cout << "Loading the velocity over time plugin" << endl;
      #endif
      connection = event::Events::ConnectWorldUpdateBegin(boost::bind(&VelocityOverTimePlugin::worldUpdate, this));

      // Get the name of the folder to store the result in
      const char* resultsFolder = std::getenv("RESULTS_FOLDER");
      if(resultsFolder == nullptr){
         cout << "Results folder not set. Using current directory." << endl;
         resultsFolder = "./";
      }

      {
        const string resultsXFileName = string(resultsFolder) + "/" + "velocities_x.csv";
        bool exists = boost::filesystem::exists(resultsXFileName);

        outputCSVX.open(resultsXFileName, ios::out | ios::app);
        assert(outputCSVX.is_open());
        if (!exists) {
          writeHeader(outputCSVX);
        }
      }

      {
        const string resultsYFileName = string(resultsFolder) + "/" + "velocities_y.csv";
        bool exists = boost::filesystem::exists(resultsYFileName);

        outputCSVY.open(resultsYFileName, ios::out | ios::app);
        assert(outputCSVY.is_open());

        if (!exists) {
          writeHeader(outputCSVY);
        }
      }


      {
        const string resultsZFileName = string(resultsFolder) + "/" + "velocities_z.csv";
        bool exists = boost::filesystem::exists(resultsZFileName);

        outputCSVZ.open(resultsZFileName, ios::out | ios::app);
        assert(outputCSVZ.is_open());

        if (!exists) {
           writeHeader(outputCSVZ);
        }
      }

       // Write out t0
       outputCSVX << "Velocity X (m/s), " << trunk->GetWorldCoGLinearVel().x << ", ";
       outputCSVY << "Velocity Y (m/s), " << trunk->GetWorldCoGLinearVel().y << ", ";
       outputCSVZ << "Velocity Z (m/s), " << trunk->GetWorldCoGLinearVel().z << ", ";
    }
    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(links); ++i) {
            outputCSV << model->GetLink(links[i])->GetWorldLinearAccel().GetLength() << ", ";
        }
        outputCSV << endl;

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

            outputCSV << endl;
            outputCSV.close();
        }
    }
示例#10
0
void Gyro::Load(physics::ModelPtr model, sdf::ElementPtr sdf) {
  this->model = model;

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

  std::string axisString = sdf->Get<std::string>("axis");
  if (axisString == "roll") axis = Roll;
  if (axisString == "pitch") axis = Pitch;
  if (axisString == "yaw") axis = Yaw;

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

  gzmsg << "Initializing gyro: " << topic << " link=" << link->GetName()
        << " axis=" << axis << " radians=" << radians << std::endl;

  // Connect to Gazebo transport for messaging
  std::string scoped_name = model->GetWorld()->GetName()+"::"+model->GetScopedName();
  boost::replace_all(scoped_name, "::", "/");
  node = transport::NodePtr(new transport::Node());
  node->Init(scoped_name);
  command_sub = node->Subscribe(topic+"/control", &Gyro::Callback, this);
  pos_pub = node->Advertise<msgs::Float64>(topic+"/position");
  vel_pub = node->Advertise<msgs::Float64>(topic+"/velocity");

  // Connect to the world update event.
  // This will trigger the Update function every Gazebo iteration
  updateConn = event::Events::ConnectWorldUpdateBegin(boost::bind(&Gyro::Update, this, _1));
}
    void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) {
        world = _model->GetWorld();
        model = _model;
        trunk = model->GetLink("trunk");

#if(PRINT_DEBUG)
        cout << "Loading the link acceleration over time plugin" << endl;
#endif
        connection = event::Events::ConnectWorldUpdateBegin(boost::bind(&LinkAccelerationOverTimePlugin
                     ::worldUpdate, this));

        // Get the name of the folder to store the result in
        const char* resultsFolder = std::getenv("RESULTS_FOLDER");
        if(resultsFolder == nullptr) {
            cout << "Results folder not set. Using current directory." << endl;
            resultsFolder = "./";
        }

        const string resultsFileName = string(resultsFolder) + "/" + "link_accelerations.csv";
        outputCSV.open(resultsFileName, ios::out);
        assert(outputCSV.is_open());
        writeHeader(outputCSV);
    }
    private: void worldUpdate(){
#if(PRINT_DEBUG)
       cout << "Updating the world for time: " << world->GetSimTime().Float() << endl;
#endif
       for (unsigned int i = 0; i < boost::size(links); ++i) {
          const physics::LinkPtr link = model->GetLink(links[i]);
          averageAngularMomentum +=  link->GetWorldInertiaMatrix() * link->GetWorldAngularVel();
       }
       if (world->GetSimTime().nsec % (10 * 1000000) == 0) {
          outputCSV << world->GetSimTime().Double() << ", " << averageAngularMomentum.x / 10.0
          << ", " << averageAngularMomentum.y / 10.0 << ", " << averageAngularMomentum.z / 10.0 << endl;
          averageAngularMomentum = math::Vector3();
       }

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

          outputCSV << endl;
          outputCSV.close();
        }
    }
////////////////////////////////////////////////////////////////////////////////
// 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")->GetValueString() + "/";

    if (!_sdf->HasElement("topicName"))
        topic_ = "magnetic";
    else
        topic_ = _sdf->GetElement("topicName")->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("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")->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("magnitude"))
        magnitude_ = DEFAULT_MAGNITUDE;
    else
        magnitude_ = _sdf->GetElement("magnitude")->GetValueDouble();

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

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

    if (!_sdf->HasElement("inclination"))
        inclination_ = DEFAULT_INCLINATION * M_PI/180.0;
    else
        inclination_ = _sdf->GetElement("inclination")->GetValueDouble() * 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::ConnectWorldUpdateStart(
                           boost::bind(&GazeboRosMagnetic::Update, 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")->GetValueString() + "/";

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

  if (!_sdf->HasElement("takeoffTopic"))
    takeoff_topic_ = "/ardrone/takeoff";
  else
    takeoff_topic_ = _sdf->GetElement("takeoffTopic")->GetValueString();

  if (!_sdf->HasElement("/ardrone/land"))
    land_topic_ = "/ardrone/land";
  else
    land_topic_ = _sdf->GetElement("landTopic")->GetValueString();

  if (!_sdf->HasElement("resetTopic"))
    reset_topic_ = "/ardrone/reset";
  else
    reset_topic_ = _sdf->GetElement("resetTopic")->GetValueString();

  if (!_sdf->HasElement("navdataTopic"))
    navdata_topic_ = "/ardrone/navdata";
  else
    navdata_topic_ = _sdf->GetElement("navdataTopic")->GetValueString();

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

  if (!_sdf->HasElement("sonarTopic"))
    sonar_topic_.clear();
  else
    sonar_topic_ = _sdf->GetElement("sonarTopic")->GetValueString();

  if (!_sdf->HasElement("stateTopic"))
    state_topic_.clear();
  else
    state_topic_ = _sdf->GetElement("stateTopic")->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("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 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::ConnectWorldUpdateStart(
      boost::bind(&GazeboQuadrotorStateController::Update, this));

  robot_current_state = LANDED_MODEL;
}
////////////////////////////////////////////////////////////////////////////////
// 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", "/map" or "map" report
    // back inertial values in the gazebo world
    if (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));
}
// Load necessary data from the .sdf file
void GazeboUUVPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) {
  namespace_.clear();
  getSdfParam<std::string>(
    _sdf, "robotNamespace", namespace_, namespace_, true);

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

  getSdfParam<std::string>(_sdf, "linkName", link_name_, link_name_, true);

  // get the base link, thus the base hippocampus model
  link_ = _model->GetLink(link_name_);
  // get the child links, these are the links which represents the rotors of the hippocampus
  rotor_links_ = link_->GetChildJointsLinks();
  for(int i = 0; i < rotor_links_.size(); i++) {
    std::cout << "Rotor Link:" << rotor_links_[i]->GetScopedName() << "\n";
    command_[i] = 0.0;
  }

  getSdfParam<std::string>(
    _sdf, "commandSubTopic", command_sub_topic_, command_sub_topic_);

  // subscribe to the commands (actuator outputs from the mixer from PX4)
  command_sub_ = node_handle_->Subscribe<mav_msgs::msgs::CommandMotorSpeed>(
    "~/" + _model->GetName() + command_sub_topic_, &GazeboUUVPlugin::CommandCallback, this);

  update_connection_ = event::Events::ConnectWorldUpdateBegin(
    boost::bind(&GazeboUUVPlugin::OnUpdate, this, _1));

  // get force and torque parameters for force and torque calculations of the rotors from motor_speed
  getSdfParam<double>(
    _sdf, "motorForceConstant", motor_force_constant_, motor_force_constant_);
  getSdfParam<double>(
    _sdf, "motorTorqueConstant", motor_torque_constant_, motor_torque_constant_);

  // parameters for added mass and damping
  ignition::math::Vector3d added_mass_linear(0,0,0);
  getSdfParam<ignition::math::Vector3d>(
    _sdf, "addedMassLinear", added_mass_linear, added_mass_linear);
  X_udot_ = added_mass_linear[0];
  Y_vdot_ = added_mass_linear[1];
  Z_wdot_ = added_mass_linear[2];

  ignition::math::Vector3d added_mass_angular(0,0,0);
  getSdfParam<ignition::math::Vector3d>(
    _sdf, "addedMassAngular", added_mass_angular, added_mass_angular);
  K_pdot_ = added_mass_angular[0];
  M_qdot_ = added_mass_angular[1];
  N_rdot_ = added_mass_angular[2];

  ignition::math::Vector3d damping_linear(0,0,0);
  getSdfParam<ignition::math::Vector3d>(
    _sdf, "dampingLinear", damping_linear, damping_linear);
  X_u_ = damping_linear[0];
  Y_v_ = damping_linear[1];
  Z_w_ = damping_linear[2];

  ignition::math::Vector3d damping_angular(0,0,0);
  getSdfParam<ignition::math::Vector3d>(
    _sdf, "dampingAngular", damping_angular, damping_angular);
  K_p_ = damping_angular[0];
  M_q_ = damping_angular[1];
  N_r_ = damping_angular[2];

  // variables for debugging
  time_ = 0.0;
  counter_ = 0.0;

}
示例#17
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;
}
////////////////////////////////////////////////////////////////////////////////
// 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));
}
////////////////////////////////////////////////////////////////////////////////
// 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 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));
}
////////////////////////////////////////////////////////////////////////////////
// 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));
}
////////////////////////////////////////////////////////////////////////////////
// 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");
}
示例#23
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));
}
    void GazeboRosPulson::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
    {

        world_ = _model->GetWorld();

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

        // body name
        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("GazeboRosPulson plugin error bodyName: %s does not exist\n", link_name_.c_str());
            return;
        }

        // get topic
        if (_sdf->HasElement("topicName"))
        {
            range_topic_ = _sdf->GetElement("topicName")->GetValue()->GetAsString();
        }
        else
        {
            range_topic_ = "ranges";
        }

        // get update rate
        double rate;
        if (_sdf->HasElement("updateRate"))
        {
            _sdf->GetElement("updateRate")->GetValue()->Get(rate);
        }
        else
        {
            rate = 120.0;
        }

        // get node id
        if (_sdf->HasElement("nodeId"))
        {
            _sdf->GetElement("nodeId")->GetValue()->Get(node_id_);
        }
        else
        {
            node_id_ = 100;
        }

        // set counter
        counter_ = 0;

        // get beacon file
        std::string beacon_map_file;
        if (_sdf->HasElement("beaconMapFile"))
        {
            beacon_map_file = _sdf->GetElement("beaconMapFile")->GetValue()->GetAsString();
        }
        else
        {
            ROS_FATAL("GazeboRosPulson plugin requires a beaconMapFile\n");
            return;
        }

        // get package path
        std::string path = ros::package::getPath("gazebo_ros_pulson");

        // parse beacon map
        int r = ParseBeaconMapFile(path + "/" + "config/" + beacon_map_file);
        if (r != 0)
        {
            ROS_ERROR("GazeboRosPulson plugin can not open beaconMapFile\n");
            return;
        }

        // ensure ros is initialized
        if (!ros::isInitialized())
        {
            ROS_FATAL("A ROS node for Gazebo has not been initialized, unable to load plugin.\n");
            return;
        }

        // init node handle
        nh_ = new ros::NodeHandle(namespace_);

        // publishers
        range_pub_ = nh_->advertise<pulson_ros::RangeMeasurement>(range_topic_, 1000);

        // reset plugin
        Reset();

        // set rates
        updateTimer_.setUpdateRate(rate);
        updateTimer_.Load(world_, _sdf);

        // initialize callback
        updateConnection_ = updateTimer_.Connect(boost::bind(&GazeboRosPulson::Update, this));

        ROS_INFO("GazeboRosPulson plugin loaded!");

    }
示例#25
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));
}
    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();
        }
      }
    }
示例#27
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));
}