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

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

    void AuRo666Plugin::Load(physics::ModelPtr model_ptr, sdf::ElementPtr sdf_element_ptr) {
        this->model_ptr = model_ptr;

        initializePID(sdf_element_ptr);

        /* ********************* Assigning the joint pointers *********************** */
        // TODO: Replace such strings with const vars
        joints[front_left_yaw_id] = model_ptr->GetJoint("front_left_joint");
        joints[front_right_yaw_id] = model_ptr->GetJoint("front_right_joint");
        joints[back_left_velocity_id] = model_ptr->GetJoint("back_left_joint");
        joints[back_right_velocity_id] = model_ptr->GetJoint("back_right_joint");

        last_update_time = model_ptr->GetWorld()->GetSimTime();
        connection_ptr = gazebo::event::Events::ConnectWorldUpdateBegin(boost::bind(&AuRo666Plugin::updateState, this));
    }

void PneumaticPiston::Load(physics::ModelPtr model, sdf::ElementPtr sdf) {
  this->model = model;
  signal = 0;

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

  forward_force = sdf->Get<double>("forward-force");
  reverse_force = sdf->Get<double>("reverse-force");

  if (sdf->HasElement("direction") && sdf->Get<std::string>("direction") == "reversed") {
    forward_force = -forward_force;
    reverse_force = -reverse_force;
  }

  gzmsg << "Initializing piston: " << topic << " joint=" << joint->GetName()
        << " forward_force=" << forward_force
        << " reverse_force=" << reverse_force<< 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);
  sub = node->Subscribe(topic, &PneumaticPiston::Callback, this);

  // Connect to the world update event.
  // This will trigger the Update function every Gazebo iteration
  updateConn = event::Events::ConnectWorldUpdateBegin(boost::bind(&PneumaticPiston::Update, this, _1));
}

void Servo::Load(physics::ModelPtr model, sdf::ElementPtr sdf) {
  this->model = model;
  signal = 0;

  // parse SDF Properries
  joint = model->GetJoint(sdf->Get<std::string>("joint"));
  if (sdf->HasElement("topic")) {
    topic = sdf->Get<std::string>("topic");
  } else {
    topic = "~/" + sdf->GetAttribute("name")->GetAsString();
  }

  if (sdf->HasElement("torque")) {
    torque = sdf->Get<double>("torque");
  } else {
    torque = 5;
  }

  gzmsg << "initializing servo: " << topic << " joint=" << joint->GetName()
        << " torque=" << torque << 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);
  sub = node->Subscribe(topic, &Servo::Callback, this);

  // connect to the world update event
  // this will call update every iteration
  updateConn = event::Events::ConnectWorldUpdateBegin(
      boost::bind(&Servo::Update, this, _1));
}

void DCMotor::Load(physics::ModelPtr model, sdf::ElementPtr sdf) {
  this->model = model;
  signal = 0;

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

  if (sdf->HasElement("multiplier")) {
    multiplier = sdf->Get<double>("multiplier");
  } else {
    multiplier = 1;
  }

  gzmsg << "Initializing motor: " << topic << " joint=" << joint->GetName()
        << " multiplier=" << multiplier << 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);
  sub = node->Subscribe(topic, &DCMotor::Callback, this);

  // Connect to the world update event.
  // This will trigger the Update function every Gazebo iteration
  updateConn = event::Events::ConnectWorldUpdateBegin(boost::bind(&DCMotor::Update, this, _1));
}

void Potentiometer::Load(physics::ModelPtr model, sdf::ElementPtr sdf) {
  this->model = model;

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

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

  gzmsg << "Initializing potentiometer: " << topic << " joint=" << joint->GetName()
        << " 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);
  pub = node->Advertise<msgs::Float64>(topic);

  // Connect to the world update event.
  // This will trigger the Update function every Gazebo iteration
  updateConn = event::Events::ConnectWorldUpdateBegin(boost::bind(&Potentiometer::Update, this, _1));
}

bool findJointByName(physics::ModelPtr model, physics::JointPtr &joint, const std::string name) {
	joint = model->GetJoint(name);
	if (!joint) {
		gzerr << "joint by name [" << name << "] not found in model\n";
		return false;
	}
	return true;
}

 void writeHeader(ofstream& file) {
    file << "Time, ";
    for (unsigned int i = 0; i < boost::size(joints); ++i) {
       physics::JointPtr currJoint = model->GetJoint(joints[i]);
       for (unsigned int j = 0; j < currJoint->GetAngleCount(); ++j) {
          file << joints[i] << "(" << j << "),";
       }
    }
    file << endl;
 }

physics::JointPtr GazeboRos::getJoint(physics::ModelPtr &_parent, const char *_tag_name, const std::string &_joint_default_name) {
    std::string joint_name;
    getParameter<std::string>(joint_name, _tag_name, _joint_default_name);
    physics::JointPtr joint = _parent->GetJoint(joint_name);
    if (!joint)
    {
        char error[200];
        snprintf(error, 200,
                 "%s: couldn't get wheel hinge joint named %s",
                 info() , joint_name.c_str());
        gzthrow(error);
    }
    return joint;
}

InternalLimitSwitch::InternalLimitSwitch(physics::ModelPtr model, sdf::ElementPtr sdf) {
  joint = model->GetJoint(sdf->Get<std::string>("joint"));
  min = sdf->Get<double>("min");
  max = sdf->Get<double>("max");

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

  gzmsg << "\tinternal limit switch: " << " type=" << joint->GetName()
        << " range=[" << min << "," << max << "] radians=" << radians << std::endl;
}

 private: void worldUpdate(){
    if(this->model->GetWorld()->GetSimTime().nsec % 10000000 != 0){
       return;
    }
    
   csvFile << world->GetSimTime().Float() << ", ";
   // Iterate over model joints and print them
   for (unsigned int i = 0; i < boost::size(joints); ++i) {
     physics::JointPtr currJoint = model->GetJoint(joints[i]);
     for(unsigned int j = 0; j < currJoint->GetAngleCount(); ++j){
       csvFile << "   " << currJoint->GetForce(j) << ", ";
     }
   }
   csvFile << endl;
 }

void Encoder::Load(physics::ModelPtr model, sdf::ElementPtr sdf) {
  this->model = model;

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

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

  gzmsg << "Initializing encoder: " << topic << " joint=" << joint->GetName()
        << " 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", &Encoder::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(&Encoder::Update, this, _1));
}

void GazeboRosKobuki::Load(physics::ModelPtr parent, sdf::ElementPtr sdf)
{
  model_ = parent;
  if (!model_)
  {
    ROS_ERROR_STREAM("Invalid model pointer! [" << node_name_ << "]");
    return;
  }
  // Get then name of the parent model and use it as node name
  std::string model_name = sdf->GetParent()->GetValueString("name");
  gzdbg << "Plugin model name: " << model_name << "\n";
  nh_ = ros::NodeHandle("");
  // creating a private name pace until Gazebo implements topic remappings
  nh_priv_ = ros::NodeHandle("/" + model_name);
  node_name_ = model_name;

  world_ = parent->GetWorld();

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

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

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

  /*
   * Prepare publishing odometry data
   */
  if (sdf->HasElement("wheel_separation"))
  {
    wheel_sep_ = sdf->GetElement("wheel_separation")->GetValueDouble();
  }
  else
  {
    ROS_ERROR_STREAM("Couldn't find the wheel separation parameter in the model description!"
                     << " Did you specify it?" << " [" << node_name_ <<"]");
    return;
  }
  if (sdf->HasElement("wheel_diameter"))
  {
    wheel_diam_ = sdf->GetElement("wheel_diameter")->GetValueDouble();
  }
  else
  {
    ROS_ERROR_STREAM("Couldn't find the wheel diameter parameter in the model description!"
                     << " Did you specify it?" << " [" << node_name_ <<"]");
    return;
  }
  if (sdf->HasElement("torque"))
  {
    torque_ = sdf->GetElement("torque")->GetValueDouble();
  }
  else
//.........这里部分代码省略.........

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

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