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