C++ ElementPtr::GetAttribute方法代码示例

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 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 Rangefinder::Load(physics::ModelPtr model, sdf::ElementPtr sdf) {
  this->model = model;

  // Parse SDF properties
  sensor = std::dynamic_pointer_cast<sensors::SonarSensor>(
      sensors::get_sensor(sdf->Get<std::string>("sensor")));
  if (sdf->HasElement("topic")) {
    topic = sdf->Get<std::string>("topic");
  } else {
    topic = "~/" + sdf->GetAttribute("name")->GetAsString();
  }

  gzmsg << "Initializing rangefinder: " << topic << " sensor=" << sensor->Name()
        << 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(&Rangefinder::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 LimitSwitch::Load(physics::ModelPtr model, sdf::ElementPtr sdf) {
  this->model = model;

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

  gzmsg << "Initializing limit switch: " << topic << " type=" << type
        << std::endl;
  if (type == "internal") {
    ls = new InternalLimitSwitch(model, sdf);
  } else if (type == "external") {
    ls = new ExternalLimitSwitch(sdf);
  } else {
    gzerr << "WARNING: unsupported limit switch type " << type;
  }

  // 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::Bool>(topic);

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

PositionMotor::PositionMotor(gz::physics::ModelPtr model, std::string partId,
							 std::string motorId, sdf::ElementPtr motor):
	JointMotor(model, partId, motorId, motor, 1),
	positionTarget_(0),
	noise_(0) {
	// Retrieve upper / lower limit from joint set in parent constructor
	// Truncate ranges to [-pi, pi]
	upperLimit_ = fmin(M_PI, joint_->GetUpperLimit(0).Radian());
	lowerLimit_ = fmax(-M_PI, joint_->GetLowerLimit(0).Radian());
	fullRange_ = (upperLimit_ - lowerLimit_ + 1e-12) >= (2 * M_PI);

	if (motor->HasElement("rv:pid")) {
		auto pidElem = motor->GetElement("rv:pid");
		pid_ = Motor::createPid(pidElem);
	}

	auto noiseParam = motor->GetAttribute("noise");
	if (noiseParam) {
		noiseParam->Get(noise_);
	}

	// I've asked this question at the Gazebo forums:
	// http://answers.gazebosim.org/question/9071/joint-target-velocity-with-maximum-force/
	// Until it is answered I'm resorting to calling ODE functions directly to get this
	// to work. This will result in some deprecation warnings.
	// It has the added benefit of not requiring the world update connection though.
//	updateConnection_ = gz::event::Events::ConnectWorldUpdateBegin(boost::bind(
//			&PositionMotor::OnUpdate, this, _1));

	double maxEffort = joint_->GetEffortLimit(0);
	joint_->SetParam("fmax", 0, maxEffort);
}

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

SensorPtr SensorFactory::create(sdf::ElementPtr sensor) {
	auto typeParam = sensor->GetAttribute("type");
	auto partIdParam = sensor->GetAttribute("part_id");
	auto idParam = sensor->GetAttribute("id");

	if (!typeParam || !partIdParam || !idParam) {
		std::cerr << "Sensor is missing required attributes (`id`, `type` or `part_id`)." << std::endl;
		throw std::runtime_error("Sensor error");
	}

	auto partId = partIdParam->GetAsString();
	auto type = typeParam->GetAsString();
	auto id = idParam->GetAsString();

	SensorPtr out = this->getSensor(sensor, type, partId, id);
	if (!out) {
		std::cerr << "Sensor type '" << type << "' is not supported." << std::endl;
		throw std::runtime_error("Sensor error");
	}

	return out;
}

Sensor::Sensor(
    ::gazebo::physics::ModelPtr _model,
    sdf::ElementPtr _sensor,
    std::string _partId,
    std::string _sensorId,
    unsigned int _inputs)
    : VirtualSensor(_model, _partId, _sensorId, _inputs)
{
  if (not _sensor->HasAttribute("sensor") or not _sensor->HasAttribute("link"))
  {
    std::cerr << "Sensor is missing required attributes (`link` or `sensor`)."
              << std::endl;
    throw std::runtime_error("Sensor error");
  }

  auto sensorName = _sensor->GetAttribute("sensor")->GetAsString();
  auto linkName = _sensor->GetAttribute("link")->GetAsString();

  auto link = _model->GetLink(linkName);
  if (not link)
  {
    std::cerr << "Link '" << linkName << "' for sensor '" << sensorName
              << "' is not present in model." << std::endl;
    throw std::runtime_error("Sensor error");
  }

  std::string scopedName = link->GetScopedName(true) + "::" + sensorName;
  this->sensor_ = gz::sensors::get_sensor(scopedName);

  if (not this->sensor_)
  {
    std::cerr << "Sensor with scoped name '" << scopedName
              << "' could not be found." << std::endl;
    throw std::runtime_error("Sensor error");
  }
}

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

JointMotor::JointMotor(
    ::gazebo::physics::ModelPtr _model,
    const std::string &_partId,
    const std::string &_motorId,
    sdf::ElementPtr _motor,
    const unsigned int _outputs)
    : Motor(_model, _partId, _motorId, _outputs)
{
  if (not _motor->HasAttribute("joint"))
  {
    std::cerr << "JointMotor requires a `joint` attribute." << std::endl;
    throw std::runtime_error("Motor error");
  }

  auto jointName = _motor->GetAttribute("joint")->GetAsString();
  this->joint_ = _model->GetJoint(jointName);
  if (not this->joint_)
  {
    std::cerr << "Cannot locate joint motor `" << jointName << "`" << std::endl;
    throw std::runtime_error("Motor error");
  }

  this->jointName_ = this->joint_->GetScopedName();
}