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

////////////////////////////////////////////////////////////////////////////////
// Load the controller
void GazeboQuadrotorStateController::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
{
  world = _model->GetWorld();

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

  namespace_ = robot_namespace_;

  if (!_sdf->HasElement("node_namespace"))
  {
    node_namespace_.clear();
  }
  else
  {
    this->node_namespace_ = this->robot_namespace_ + _sdf->GetElement("node_namespace")->GetValueString() + "/";
    namespace_ = namespace_ + node_namespace_;
  }

  //////////////////////////

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

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

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

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

  if (!_sdf->HasElement("navdataTopic"))
  {
  	//navdata_topic_ = "/ardrone/navdata"; ORIGINAL
  	navdata_topic_ = "navdata";
  	//required from tum_ardrone
  	navdata_topic_tum = "/ardrone/navdata";
  }
  else
  {
  	navdata_topic_ = _sdf->GetElement("navdataTopic")->Get<std::string>();
  	//required frim tum_ardrone
  	navdata_topic_tum = "/ardrone/navdata";
  }

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

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

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

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

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

////////////////////////////////////////////////////////////////////////////////
// Load the controller
void GazeboRosMagnetic::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
{
  world = _model->GetWorld();

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

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

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

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

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

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

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

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

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

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

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

  sensor_model_.Load(_sdf);

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

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

  Reset();

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