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