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

/** on loading of the plugin
 * @param _parent Parent Model
 */
void Puck::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
{
  printf("Loading Puck Plugin of model %s\n", _parent->GetName().c_str());
  // Store the pointer to the model
  this->model_ = _parent;  

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

  // Create the communication Node for communication with fawkes
  this->node_ = transport::NodePtr(new transport::Node());
  // the namespace is set to the world name!
  this->node_->Init(model_->GetWorld()->GetName());

  // register visual publisher
  this->visual_pub_ = this->node_->Advertise<msgs::Visual>("~/visual");
  
  // initialize without rings or cap
  this->ring_count_ = 0;
  this->have_cap = false;
  this->announced_ = false;
  
  this->new_puck_publisher = this->node_->Advertise<gazsim_msgs::NewPuck>("~/new_puck");
  
  // subscribe for puck commands
  this->command_subscriber = this->node_->Subscribe(std::string("~/pucks/cmd"), &Puck::on_command_msg, this);
  
  // publisher for workpiece command results
  this->workpiece_result_pub_ = node_->Advertise<gazsim_msgs::WorkpieceResult>("~/pucks/cmd/result");
  
  if (!_sdf->HasElement("baseColor")) {
    printf("SDF for base has no baseColor configured, defaulting to RED!\n");
    base_color_ = gazsim_msgs::Color::RED;
  } else {
    std::string config_color = _sdf->GetElement("baseColor")->Get<std::string>();
    if (config_color == "RED") {
      base_color_ = gazsim_msgs::Color::RED;
    } else if (config_color == "BLACK") {
      base_color_ = gazsim_msgs::Color::BLACK;
    } else if (config_color == "SILVER") {
      base_color_ = gazsim_msgs::Color::SILVER;
    } else {
      printf("SDF for base has no baseColor configured, defaulting to RED!\n");
      base_color_ = gazsim_msgs::Color::RED;
    }
    printf("Base spawns in color %s\n", config_color.c_str());
  }
  
  delivery_pub_ = node_->Advertise<llsf_msgs::SetOrderDeliveredByColor>(TOPIC_SET_ORDER_DELIVERY_BY_COLOR);
}

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

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

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

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

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

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

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

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

  frame_id_ = link_name;

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

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

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

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

  pressure_message_.mutable_header()->set_frame_id(frame_id_);
  pressure_message_.set_variance(pressure_var_);
}

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

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

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

  namespace_.clear();

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

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

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

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

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

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

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

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

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

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

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

}

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

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

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

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

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

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

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


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

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

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

        ++i;
    }

    model = _parent;

    jsSub = nh.subscribe(jointStateTopic, 1, &GazeboJointStateClient::JointStateCallback, this);
}