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 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 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 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));
}
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 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));
}
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));
}
public: void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf){
world = _model->GetWorld();
model = _model;
#if(PRINT_DEBUG)
cout << "Loading the contact forces plugin" << endl;
#endif
#if(PRINT_SENSORS)
cout << "Listing all sensors: " << endl;
vector<sensors::SensorPtr> sensors = sensors::SensorManager::Instance()->GetSensors();
for(unsigned int i = 0; i < sensors.size(); ++i){
cout << sensors[i]->GetScopedName() << endl;
}
cout << endl;
#endif
for(unsigned int i = 0; i < boost::size(contacts); ++i){
sensors::SensorPtr sensor = sensors::SensorManager::Instance()->GetSensor(world->GetName() + "::" + _model->GetScopedName()
+ "::" + contacts[i]);
if(sensor == nullptr){
cout << "Could not find sensor " << contacts[i] << endl;
continue;
}
sensor->SetActive(true);
sensorConnections.push_back(sensor->ConnectUpdated(
boost::bind(&ContactForcesPlugin::updateContacts, this, sensor, contacts[i])));
}
connection = event::Events::ConnectWorldUpdateBegin(boost::bind(&ContactForcesPlugin::worldUpdate, this));
}
private: void worldUpdate(){
physics::LinkPtr head = model->GetLink("head_neck");
#if(PRINT_DEBUG)
cout << "Updating HIC for time " << world->GetSimTime() << endl;
#endif
updateMaximumHic(false);
if(world->GetSimTime().Float() >= 2.0){
#if(PRINT_DEBUG)
cout << "Scenario completed. Updating results" << endl;
#endif
event::Events::DisconnectWorldUpdateBegin(this->connection);
// Drain the HIC calculation
for (unsigned int i = 0; i < ACC_HISTORY_MAX; ++i) {
updateMaximumHic(true);
}
// Disconnect the sensors
for(unsigned int i = 0; i < boost::size(contacts); ++i){
sensors::SensorPtr sensor = sensors::SensorManager::Instance()->GetSensor(world->GetName() + "::" + model->GetScopedName()
+ "::" + contacts[i]);
if(sensor == nullptr){
cout << "Could not find sensor " << contacts[i] << endl;
continue;
}
sensor->SetActive(false);
sensor->DisconnectUpdated(sensorConnections[i]);
}
sensorConnections.clear();
printResults();
exit(0);
}
}
/**
* Saves the gazebo pointer, creates the device driver
*/
void GazeboYarpTripod::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
{
yarp::os::Network::init();
if (!yarp::os::Network::checkNetwork(GazeboYarpPlugins::yarpNetworkInitializationTimeout)) {
std::cerr << "GazeboYarpTripod::Load error: yarp network does not seem to be available, is the yarpserver running?"<<std::endl;
return;
}
std::cout<<"*** GazeboYarpTripod plugin started ***"<<std::endl;
if (!_parent) {
gzerr << "GazeboYarpTripod plugin requires a parent.\n";
return;
}
m_robotName = _parent->GetScopedName();
GazeboYarpPlugins::Handler::getHandler()->setRobot(get_pointer(_parent));
// Add the gazebo_controlboard device driver to the factory.
yarp::dev::Drivers::factory().add(new yarp::dev::DriverCreatorOf<cer::dev::GazeboTripodMotionControl>("gazebo_tripod", "controlboardwrapper2", "GazeboTripodMotionControl"));
//Getting .ini configuration file from sdf
bool configuration_loaded = false;
yarp::os::Bottle wrapper_group;
yarp::os::Bottle driver_group;
if (_sdf->HasElement("yarpConfigurationFile")) {
std::string ini_file_name = _sdf->Get<std::string>("yarpConfigurationFile");
std::string ini_file_path = gazebo::common::SystemPaths::Instance()->FindFileURI(ini_file_name);
GazeboYarpPlugins::addGazeboEnviromentalVariablesModel(_parent,_sdf,m_parameters);
bool wipe = false;
if (ini_file_path != "" && m_parameters.fromConfigFile(ini_file_path.c_str(),wipe))
{
std::cout << "GazeboYarpTripod: Found yarpConfigurationFile: loading from " << ini_file_path << std::endl;
m_parameters.put("gazebo_ini_file_path",ini_file_path.c_str());
wrapper_group = m_parameters.findGroup("WRAPPER");
if(wrapper_group.isNull()) {
printf("GazeboYarpTripod::Load Error: [WRAPPER] group not found in config file\n");
return;
}
if(m_parameters.check("ROS"))
{
yarp::os::ConstString ROS;
ROS = yarp::os::ConstString ("(") + m_parameters.findGroup("ROS").toString() + yarp::os::ConstString (")");
wrapper_group.append(yarp::os::Bottle(ROS));
}
configuration_loaded = true;
}
}
if (!configuration_loaded) {
std::cout << "GazeboYarpTripod: File .ini not found, quitting\n" << std::endl;
return;
}
m_wrapper.open(wrapper_group);
if (!m_wrapper.isValid())
fprintf(stderr, "GazeboYarpTripod: wrapper did not open\n");
else
fprintf(stderr, "GazeboYarpTripod: wrapper opened correctly\n");
if (!m_wrapper.view(m_iWrap)) {
printf("Wrapper interface not found\n");
}
yarp::os::Bottle *netList = wrapper_group.find("networks").asList();
if (netList->isNull()) {
printf("GazeboYarpTripod ERROR, net list to attach to was not found, exiting\n");
m_wrapper.close();
// m_controlBoard.close();
return;
}
for (int n = 0; n < netList->size(); n++)
{
yarp::dev::PolyDriverDescriptor newPoly;
newPoly.key = netList->get(n).asString();
std::string scopedDeviceName = m_robotName + "::" + newPoly.key.c_str();
newPoly.poly = GazeboYarpPlugins::Handler::getHandler()->getDevice(scopedDeviceName);
if( newPoly.poly != NULL)
{
// device already exists, use it, setting it againg to increment the usage counter.
printf("controlBoard %s already opened\n", newPoly.key.c_str());
}
else
{
driver_group = m_parameters.findGroup(newPoly.key.c_str());
if (driver_group.isNull()) {
fprintf(stderr, "GazeboYarpTripod::Load Error: [%s] group not found in config file. Closing wrapper \n", newPoly.key.c_str());
m_wrapper.close();
return;
}
m_parameters.put("name", newPoly.key.c_str());
m_parameters.fromString(driver_group.toString(), false);
m_parameters.put("robotScopedName", m_robotName);
std::cout << "GazeboYarpTripod: setting robotScopedName " << m_robotName << std::endl;
//std::cout << "before open: params are " << m_parameters.toString() << std::endl;
if (_sdf->HasElement("initialConfiguration")) {
std::string configuration_s = _sdf->Get<std::string>("initialConfiguration");
m_parameters.put("initialConfiguration", configuration_s.c_str());
}
newPoly.poly = new yarp::dev::PolyDriver;
if(! newPoly.poly->open(m_parameters) || ! newPoly.poly->isValid())
{
std::cerr << "controlBoard <" << newPoly.key << "> did not open!!";
for(int idx=0; idx<m_controlBoards.size(); idx++)
{
m_controlBoards[idx]->poly->close();
}
m_wrapper.close();
return;
}
else
{
printf("controlBoard %s opened correctly\n", newPoly.key.c_str());
}
}
GazeboYarpPlugins::Handler::getHandler()->setDevice(scopedDeviceName, newPoly.poly);
m_controlBoards.push(newPoly);
}
if (!m_iWrap || !m_iWrap->attachAll(m_controlBoards))
{
printf("GazeboYarpTripod: Error while attaching wrapper to device\n");
m_wrapper.close();
for (int n = 0; n < netList->size(); n++) {
std::string scopedDeviceName = m_robotName + "::" + m_controlBoards[n]->key.c_str();
GazeboYarpPlugins::Handler::getHandler()->removeDevice(scopedDeviceName);
}
return;
}
printf("Device initialized correctly, now sitting and waiting cause I am just the main of the yarp device, and the robot is linked to the onUpdate event of gazebo\n");
std::cout<<"Loaded GazeboYarpTripod Plugin"<<std::endl;
}
public: void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf){
world = _model->GetWorld();
model = _model;
#if(PRINT_DEBUG)
cout << "Loading the contact forces per link over time plugin" << endl;
#endif
#if(PRINT_SENSORS)
cout << "Listing all sensors: " << endl;
vector<sensors::SensorPtr> sensors = sensors::SensorManager::Instance()->GetSensors();
for(unsigned int i = 0; i < sensors.size(); ++i){
cout << sensors[i]->GetScopedName() << endl;
}
cout << endl;
#endif
for(unsigned int i = 0; i < boost::size(contacts); ++i){
sensors::SensorPtr sensor = sensors::SensorManager::Instance()->GetSensor(world->GetName() + "::" + _model->GetScopedName()
+ "::" + contacts[i]);
if(sensor == nullptr){
cout << "Could not find sensor " << contacts[i] << endl;
continue;
}
sensor->SetActive(true);
sensorConnections.push_back(sensor->ConnectUpdated(
boost::bind(&ContactForcesPerLinkOverTimePlugin
::contactForceForSensor, this, sensor, contacts[i])));
}
connection = event::Events::ConnectWorldUpdateBegin(boost::bind(&ContactForcesPerLinkOverTimePlugin
::worldUpdate, this));
// Get the name of the folder to store the result in
const char* resultsFolder = std::getenv("RESULTS_FOLDER");
if(resultsFolder == nullptr){
cout << "Results folder not set. Using current directory." << endl;
resultsFolder = "./";
}
const string resultsFileName = string(resultsFolder) + "/" + "contact_forces_per_link.csv";
outputCSV.open(resultsFileName, ios::out);
assert(outputCSV.is_open());
writeHeader(outputCSV);
}
private: void worldUpdate(){
#if(PRINT_DEBUG)
cout << "Updating the world for time: " << world->GetSimTime().Float() << endl;
#endif
outputCSV << world->GetSimTime().Float() << ", ";
for(unsigned int i = 0; i < boost::size(contacts); ++i){
outputCSV << contactForces[contacts[i]].GetLength() << ", ";
}
outputCSV << endl;
if(world->GetSimTime().Float() >= MAX_TIME){
#if(PRINT_DEBUG)
cout << "Scenario completed. Updating results" << endl;
#endif
event::Events::DisconnectWorldUpdateBegin(this->connection);
// Disconnect the sensors
for(unsigned int i = 0; i < boost::size(contacts); ++i){
sensors::SensorPtr sensor = sensors::SensorManager::Instance()->GetSensor(world->GetName() + "::" + model->GetScopedName()
+ "::" + contacts[i]);
if(sensor == nullptr){
cout << "Could not find sensor " << contacts[i] << endl;
continue;
}
sensor->SetActive(false);
sensor->DisconnectUpdated(sensorConnections[i]);
}
sensorConnections.clear();
outputCSV << endl;
outputCSV.close();
}
}
