C++ ModelPtr::GetLink方法代码示例

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

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

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

//.........这里部分代码省略.........

////////////////////////////////////////////////////////////////////////////////
// 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>();
//.........这里部分代码省略.........