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 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));
}
////////////////////////////////////////////////////////////////////////////////
// Load the controller
void GazeboRosForce::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
{
// Get the world name.
this->world_ = _model->GetWorld();
// load parameters
this->robot_namespace_ = "";
if (_sdf->HasElement("robotNamespace"))
this->robot_namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>() + "/";
if (!_sdf->HasElement("bodyName"))
{
ROS_FATAL("force plugin missing <bodyName>, cannot proceed");
return;
}
else
this->link_name_ = _sdf->GetElement("bodyName")->Get<std::string>();
this->link_ = _model->GetLink(this->link_name_);
if (!this->link_)
{
ROS_FATAL("gazebo_ros_force plugin error: link named: %s does not exist\n",this->link_name_.c_str());
return;
}
if (!_sdf->HasElement("topicName"))
{
ROS_FATAL("force plugin missing <topicName>, cannot proceed");
return;
}
else
this->topic_name_ = _sdf->GetElement("topicName")->Get<std::string>();
// 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;
}
this->rosnode_ = new ros::NodeHandle(this->robot_namespace_);
// Custom Callback Queue
ros::SubscribeOptions so = ros::SubscribeOptions::create<geometry_msgs::Wrench>(
this->topic_name_,1,
boost::bind( &GazeboRosForce::UpdateObjectForce,this,_1),
ros::VoidPtr(), &this->queue_);
this->sub_ = this->rosnode_->subscribe(so);
// Custom Callback Queue
this->callback_queue_thread_ = boost::thread( boost::bind( &GazeboRosForce::QueueThread,this ) );
// New Mechanism for Updating every World Cycle
// Listen to the update event. This event is broadcast every
// simulation iteration.
this->update_connection_ = event::Events::ConnectWorldUpdateBegin(
boost::bind(&GazeboRosForce::UpdateChild, this));
}
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));
}
public: void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf){
world = _model->GetWorld();
model = _model;
#if(PRINT_DEBUG)
cout << "Loading the angular momentum over time plugin" << endl;
#endif
connection = event::Events::ConnectWorldUpdateBegin(boost::bind(&AngularMomentumOverTimePlugin::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) + "/" + "angular_momentum.csv";
outputCSV.open(resultsFileName, ios::out);
assert(outputCSV.is_open());
writeHeader(outputCSV);
math::Vector3 angularMomentum;
for (unsigned int i = 0; i < boost::size(links); ++i) {
const physics::LinkPtr link = model->GetLink(links[i]);
angularMomentum += link->GetWorldInertiaMatrix() * link->GetWorldAngularVel();
}
// Write out t0
outputCSV << world->GetSimTime().Double() << ", " << angularMomentum.x / 10.0
<< ", " << angularMomentum.y / 10.0 << ", " << angularMomentum.z / 10.0 << endl;
}
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 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 StatePlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
{
GZ_ASSERT(_model != NULL, "Received NULL model pointer");
this->model = _model;
physics::WorldPtr world = _model->GetWorld();
GZ_ASSERT(world != NULL, "Model is in a NULL world");
this->physicsEngine = world->GetPhysicsEngine();
GZ_ASSERT(this->physicsEngine != NULL, "Physics engine was NULL");
GZ_ASSERT(_sdf != NULL, "Received NULL SDF pointer");
this->sdf = _sdf;
if (this->sdf->HasElement("model_offset"))
{
this->modelOffset = this->sdf->Get<math::Vector3>("model_offset");
}
else
{
this->modelOffset = math::Vector3(0.0, 0.0, 0.0);
}
// Make sure the ROS node for Gazebo has already been initialized
GZ_ASSERT(ros::isInitialized(), "ROS not initialized");
this->refSub = nh.subscribe("lqrrt/ref", 1, &StatePlugin::PoseRefUpdate, this);
}
////////////////////////////////////////////////////////////////////////////////
// Load the controller
void GazeboTreasure::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
{
model_ = _model;
world_ = _model->GetWorld();
link_ = _model->GetLink();
link_name_ = link_->GetName();
namespace_.clear();
vel_x = vel_y = vel_yaw = 0;
static_object_ = false;
last_time_ = world_->GetSimTime();
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("staticObject") && _sdf->GetElement("staticObject")->GetValue())
{
static_object_ = _sdf->GetElement("staticObject")->Get<std::string>() == "true"?true:false;
}
// boost::random::mt19937 rng;
boost::random::random_device rng;
boost::random::uniform_int_distribution<> x_rand(-40, 40);
boost::random::uniform_int_distribution<> y_rand(-20, 20);
double x = x_rand(rng);
double y = y_rand(rng);
model_->SetLinkWorldPose(math::Pose(x, y, 0.2, 0, 0, 0), link_);
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
ros::NodeHandle param_handle(*node_handle_, "controller");
update_connection_ = event::Events::ConnectWorldUpdateBegin(
boost::bind(&GazeboTreasure::Update, this));
}
public: void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf){
world = _model->GetWorld();
model = _model;
trunk = model->GetLink("trunk");
#if(PRINT_DEBUG)
cout << "Loading the velocity over time plugin" << endl;
#endif
connection = event::Events::ConnectWorldUpdateBegin(boost::bind(&VelocityOverTimePlugin::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 resultsXFileName = string(resultsFolder) + "/" + "velocities_x.csv";
bool exists = boost::filesystem::exists(resultsXFileName);
outputCSVX.open(resultsXFileName, ios::out | ios::app);
assert(outputCSVX.is_open());
if (!exists) {
writeHeader(outputCSVX);
}
}
{
const string resultsYFileName = string(resultsFolder) + "/" + "velocities_y.csv";
bool exists = boost::filesystem::exists(resultsYFileName);
outputCSVY.open(resultsYFileName, ios::out | ios::app);
assert(outputCSVY.is_open());
if (!exists) {
writeHeader(outputCSVY);
}
}
{
const string resultsZFileName = string(resultsFolder) + "/" + "velocities_z.csv";
bool exists = boost::filesystem::exists(resultsZFileName);
outputCSVZ.open(resultsZFileName, ios::out | ios::app);
assert(outputCSVZ.is_open());
if (!exists) {
writeHeader(outputCSVZ);
}
}
// Write out t0
outputCSVX << "Velocity X (m/s), " << trunk->GetWorldCoGLinearVel().x << ", ";
outputCSVY << "Velocity Y (m/s), " << trunk->GetWorldCoGLinearVel().y << ", ";
outputCSVZ << "Velocity Z (m/s), " << trunk->GetWorldCoGLinearVel().z << ", ";
}
void GazeboRosJointStatePublisher::Load ( physics::ModelPtr _parent, sdf::ElementPtr _sdf ) {
// Store the pointer to the model
this->parent_ = _parent;
this->world_ = _parent->GetWorld();
this->robot_namespace_ = parent_->GetName ();
if ( !_sdf->HasElement ( "robotNamespace" ) ) {
ROS_INFO_NAMED("joint_state_publisher", "GazeboRosJointStatePublisher Plugin missing <robotNamespace>, defaults to \"%s\"",
this->robot_namespace_.c_str() );
} else {
this->robot_namespace_ = _sdf->GetElement ( "robotNamespace" )->Get<std::string>();
if ( this->robot_namespace_.empty() ) this->robot_namespace_ = parent_->GetName ();
}
if ( !robot_namespace_.empty() ) this->robot_namespace_ += "/";
rosnode_ = boost::shared_ptr<ros::NodeHandle> ( new ros::NodeHandle ( this->robot_namespace_ ) );
if ( !_sdf->HasElement ( "jointName" ) ) {
ROS_ASSERT ( "GazeboRosJointStatePublisher Plugin missing jointNames" );
} else {
sdf::ElementPtr element = _sdf->GetElement ( "jointName" ) ;
std::string joint_names = element->Get<std::string>();
boost::erase_all ( joint_names, " " );
boost::split ( joint_names_, joint_names, boost::is_any_of ( "," ) );
}
this->update_rate_ = 100.0;
if ( !_sdf->HasElement ( "updateRate" ) ) {
ROS_WARN_NAMED("joint_state_publisher", "GazeboRosJointStatePublisher Plugin (ns = %s) missing <updateRate>, defaults to %f",
this->robot_namespace_.c_str(), this->update_rate_ );
} else {
this->update_rate_ = _sdf->GetElement ( "updateRate" )->Get<double>();
}
// Initialize update rate stuff
if ( this->update_rate_ > 0.0 ) {
this->update_period_ = 1.0 / this->update_rate_;
} else {
this->update_period_ = 0.0;
}
last_update_time_ = this->world_->GetSimTime();
for ( unsigned int i = 0; i< joint_names_.size(); i++ ) {
joints_.push_back ( this->parent_->GetJoint ( joint_names_[i] ) );
ROS_INFO_NAMED("joint_state_publisher", "GazeboRosJointStatePublisher is going to publish joint: %s", joint_names_[i].c_str() );
}
ROS_INFO_NAMED("joint_state_publisher", "Starting GazeboRosJointStatePublisher Plugin (ns = %s)!, parent name: %s", this->robot_namespace_.c_str(), parent_->GetName ().c_str() );
tf_prefix_ = tf::getPrefixParam ( *rosnode_ );
joint_state_publisher_ = rosnode_->advertise<sensor_msgs::JointState> ( "joint_states",1000 );
last_update_time_ = this->world_->GetSimTime();
// Listen to the update event. This event is broadcast every
// simulation iteration.
this->updateConnection = event::Events::ConnectWorldUpdateBegin (
boost::bind ( &GazeboRosJointStatePublisher::OnUpdate, this, _1 ) );
}
////////////////////////////////////////////////////////////////////////////////
// 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 WorldInterface::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
{
if (m_network!=0)
return;
m_network=new yarp::os::Network();
if (!yarp::os::Network::checkNetwork(GazeboYarpPlugins::yarpNetworkInitializationTimeout)) {
yError() << "WorldInterface::Load error: yarp network does not seem to be available, is the yarpserver running?";
return;
}
//setting up proxy
m_proxy.attachWorldPointer(_model->GetWorld());
m_proxy.attachModelPointer(_model);
//pass reference to server
m_wi_server.attachWorldProxy(&m_proxy);
//Getting .ini configuration file from sdf
bool configuration_loaded = false;
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);
if (ini_file_path != "" && m_parameters.fromConfigFile(ini_file_path.c_str())) {
yInfo() << "Found yarpConfigurationFile: loading from " << ini_file_path ;
configuration_loaded = true;
}
}
if (!configuration_loaded) {
yError() << "WorldInterface::Load error could not load configuration file";
return;
}
std::string portname=m_parameters.find("name").asString();
int synchronous=m_parameters.find("synchro").asInt32();
if (synchronous)
m_proxy.setSynchronousMode(true);
else
m_proxy.setSynchronousMode(false);
m_rpcport=new yarp::os::RpcServer();
m_rpcport->open(portname);
m_wi_server.yarp().attachAsServer(*m_rpcport);
// Listen to the update event. This event is broadcast every
// simulation iteration.
this->updateConnection = event::Events::ConnectWorldUpdateBegin(
boost::bind(&WorldInterface::OnUpdate, this, _1));
}
////////////////////////////////////////////////////////////////////////////////
// Load the controller
void GazeboRosIMU::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
{
// save pointers
this->world_ = _parent->GetWorld();
this->sdf = _sdf;
// ros callback queue for processing subscription
this->deferred_load_thread_ = boost::thread(
boost::bind(&GazeboRosIMU::LoadThread, this));
}
public: void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf){
#if(PRINT_DEBUG)
cout << "Loading the joint forces plugin" << std::endl;
#endif
world = _model->GetWorld();
model = _model;
// Open a file to store the results
const string resultsFileName = "joint_forces.csv";
csvFile.open(resultsFileName, ios::out);
assert(csvFile.is_open());
writeHeader(csvFile);
connection = event::Events::ConnectWorldUpdateBegin(boost::bind(&JointForcesPlugin::worldUpdate, this));
}
//Load the controller
void ThrusterPlugin::Load( physics::ModelPtr _parent, sdf::ElementPtr _sdf)
{
// save pointers
this->_matsyaWorld = _parent->GetWorld();
this->_matsyaModel = _parent;
this->_baseLink = _matsyaModel->GetLink("base_link");
this->_sdf = _sdf;
//double mass = _baseLink->GetInertial()->GetMass();
//gzdbg << "base mass is: "<< mass <<"\n";
// ros callback queue for processing subscription
this->_spinnerThread = boost::thread(
boost::bind(&ThrusterPlugin::loadThread, 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));
}
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);
}
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));
}
void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) {
world = _model->GetWorld();
model = _model;
trunk = model->GetLink("trunk");
#if(PRINT_DEBUG)
cout << "Loading the link acceleration over time plugin" << endl;
#endif
connection = event::Events::ConnectWorldUpdateBegin(boost::bind(&LinkAccelerationOverTimePlugin
::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) + "/" + "link_accelerations.csv";
outputCSV.open(resultsFileName, ios::out);
assert(outputCSV.is_open());
writeHeader(outputCSV);
}
void StatePlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
{
GZ_ASSERT(_model != NULL, "Received NULL model pointer");
model_ = _model;
physics::WorldPtr world = _model->GetWorld();
GZ_ASSERT(world != NULL, "Model is in a NULL world");
physics_engine_ = world->GetPhysicsEngine();
GZ_ASSERT(physics_engine_ != NULL, "Physics engine was NULL");
GZ_ASSERT(_sdf != NULL, "Received NULL SDF pointer");
sdf_ = _sdf;
// If model is not center frame, store static offset
if (sdf_->HasElement("staticOffset"))
{
static_offset_ = sdf_->Get<math::Vector3>("staticOffset");
}
else
{
static_offset_ = math::Vector3(0.0, 0.0, 0.0);
}
// Get PoseStamped trajectory
std::string ref_topic = "/trajectory";
if (sdf_->HasElement("referenceTopic"))
ref_topic = sdf_->Get<std::string>("referenceTopic");
else
ROS_WARN("SDF does not have 'referenceTopic' element, using default /trajectory topic");
first_pose_ = model_->GetWorldPose(); // Init pose
last_ref_pose_.pos.z = first_pose_.pos.z;
first_pose_.pos.z = 0.0;
// Make sure the ROS node for Gazebo has already been initialized
GZ_ASSERT(ros::isInitialized(), "ROS not initialized");
reference_sub_ = nh_.subscribe(ref_topic, 1, &StatePlugin::PoseRefUpdate, this);
}
void GazeboRosPulson::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
{
world_ = _model->GetWorld();
// namespace
if (!_sdf->HasElement("robotNamespace"))
{
namespace_.clear();
}
else
{
namespace_ = _sdf->GetElement("robotNamespace")->GetValue()->GetAsString();
}
// body name
if (!_sdf->HasElement("bodyName"))
{
link_ = _model->GetLink();
link_name_ = link_->GetName();
}
else
{
link_name_ = _sdf->GetElement("bodyName")->GetValue()->GetAsString();
link_ = _model->GetLink(link_name_);
}
if (!link)
{
ROS_FATAL("GazeboRosPulson plugin error bodyName: %s does not exist\n", link_name_.c_str());
return;
}
// get topic
if (_sdf->HasElement("topicName"))
{
range_topic_ = _sdf->GetElement("topicName")->GetValue()->GetAsString();
}
else
{
range_topic_ = "ranges";
}
// get update rate
double rate;
if (_sdf->HasElement("updateRate"))
{
_sdf->GetElement("updateRate")->GetValue()->Get(rate);
}
else
{
rate = 120.0;
}
// get node id
if (_sdf->HasElement("nodeId"))
{
_sdf->GetElement("nodeId")->GetValue()->Get(node_id_);
}
else
{
node_id_ = 100;
}
// set counter
counter_ = 0;
// get beacon file
std::string beacon_map_file;
if (_sdf->HasElement("beaconMapFile"))
{
beacon_map_file = _sdf->GetElement("beaconMapFile")->GetValue()->GetAsString();
}
else
{
ROS_FATAL("GazeboRosPulson plugin requires a beaconMapFile\n");
return;
}
// get package path
std::string path = ros::package::getPath("gazebo_ros_pulson");
// parse beacon map
int r = ParseBeaconMapFile(path + "/" + "config/" + beacon_map_file);
if (r != 0)
{
ROS_ERROR("GazeboRosPulson plugin can not open beaconMapFile\n");
return;
}
// ensure ros is initialized
if (!ros::isInitialized())
{
ROS_FATAL("A ROS node for Gazebo has not been initialized, unable to load plugin.\n");
return;
}
// init node handle
nh_ = new ros::NodeHandle(namespace_);
// publishers
range_pub_ = nh_->advertise<pulson_ros::RangeMeasurement>(range_topic_, 1000);
// reset plugin
Reset();
// set rates
updateTimer_.setUpdateRate(rate);
updateTimer_.Load(world_, _sdf);
// initialize callback
updateConnection_ = updateTimer_.Connect(boost::bind(&GazeboRosPulson::Update, this));
ROS_INFO("GazeboRosPulson plugin loaded!");
}
////////////////////////////////////////////////////////////////////////////////
// Load the controller
void GazeboQuadrotorStateController::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
{
world = _model->GetWorld();
// load parameters
if (!_sdf->HasElement("robotNamespace"))
//namespace_.clear();
robot_namespace_.clear();
else
//namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>() + "/";
robot_namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>() + "/";
namespace_ = robot_namespace_;
if (!_sdf->HasElement("node_namespace"))
{
node_namespace_.clear();
}
else
{
this->node_namespace_ = this->robot_namespace_ + _sdf->GetElement("node_namespace")->GetValueString() + "/";
namespace_ = namespace_ + node_namespace_;
}
//////////////////////////
if (!_sdf->HasElement("topicName"))
velocity_topic_ = "cmd_vel";
else
velocity_topic_ = _sdf->GetElement("topicName")->Get<std::string>();
if (!_sdf->HasElement("takeoffTopic"))
//takeoff_topic_ = "/ardrone/takeoff";
takeoff_topic_ = "takeoff";
else
takeoff_topic_ = _sdf->GetElement("takeoffTopic")->Get<std::string>();
//if (!_sdf->HasElement("/ardrone/land"))
// land_topic_ = "/ardrone/land";
if (!_sdf->HasElement("landTopic"))
land_topic_ = "land";
else
land_topic_ = _sdf->GetElement("landTopic")->Get<std::string>();
if (!_sdf->HasElement("resetTopic"))
//reset_topic_ = "/ardrone/reset";
reset_topic_ = "reset";
else
reset_topic_ = _sdf->GetElement("resetTopic")->Get<std::string>();
if (!_sdf->HasElement("navdataTopic"))
{
//navdata_topic_ = "/ardrone/navdata"; ORIGINAL
navdata_topic_ = "navdata";
//required from tum_ardrone
navdata_topic_tum = "/ardrone/navdata";
}
else
{
navdata_topic_ = _sdf->GetElement("navdataTopic")->Get<std::string>();
//required frim tum_ardrone
navdata_topic_tum = "/ardrone/navdata";
}
if (!_sdf->HasElement("imuTopic"))
imu_topic_.clear();
else
imu_topic_ = _sdf->GetElement("imuTopic")->Get<std::string>();
if (!_sdf->HasElement("sonarTopic"))
sonar_topic_.clear();
else
sonar_topic_ = _sdf->GetElement("sonarTopic")->Get<std::string>();
if (!_sdf->HasElement("stateTopic"))
state_topic_.clear();
else
state_topic_ = _sdf->GetElement("stateTopic")->Get<std::string>();
/*
if (!_sdf->HasElement("bodyName"))
{
link = _model->GetLink();
link_name_ = link->GetName();
}
else {
link_name_ = _sdf->GetElement("bodyName")->Get<std::string>();
link = boost::dynamic_pointer_cast<physics::Link>(world->GetEntity(link_name_));
}*/
link = _model->GetChildLink("base_link");
if (!link)
{
ROS_FATAL("gazebo_ros_baro plugin error: bodyName: %s does not exist\n", link_name_.c_str());
return;
}
node_handle_ = new ros::NodeHandle(namespace_);
// subscribe command: velocity control command
if (!velocity_topic_.empty())
{
ros::SubscribeOptions ops = ros::SubscribeOptions::create<geometry_msgs::Twist>(
velocity_topic_, 1,
boost::bind(&GazeboQuadrotorStateController::VelocityCallback, this, _1),
ros::VoidPtr(), &callback_queue_);
velocity_subscriber_ = node_handle_->subscribe(ops);
}
// subscribe command: take off command
if (!takeoff_topic_.empty())
{
ros::SubscribeOptions ops = ros::SubscribeOptions::create<std_msgs::Empty>(
takeoff_topic_, 1,
boost::bind(&GazeboQuadrotorStateController::TakeoffCallback, this, _1),
ros::VoidPtr(), &callback_queue_);
takeoff_subscriber_ = node_handle_->subscribe(ops);
}
// subscribe command: take off command
if (!land_topic_.empty())
{
ros::SubscribeOptions ops = ros::SubscribeOptions::create<std_msgs::Empty>(
land_topic_, 1,
boost::bind(&GazeboQuadrotorStateController::LandCallback, this, _1),
ros::VoidPtr(), &callback_queue_);
land_subscriber_ = node_handle_->subscribe(ops);
}
// subscribe command: take off command
if (!reset_topic_.empty())
{
ros::SubscribeOptions ops = ros::SubscribeOptions::create<std_msgs::Empty>(
reset_topic_, 1,
boost::bind(&GazeboQuadrotorStateController::ResetCallback, this, _1),
ros::VoidPtr(), &callback_queue_);
reset_subscriber_ = node_handle_->subscribe(ops);
}
m_navdataPub = node_handle_->advertise< ardrone_autonomy::Navdata >( navdata_topic_ , 25 );
//required by tum_ardrone
m_navdataPub_tum = node_handle_->advertise< ardrone_autonomy::Navdata >( navdata_topic_tum, 25);
// subscribe imu
if (!imu_topic_.empty())
{
ros::SubscribeOptions ops = ros::SubscribeOptions::create<sensor_msgs::Imu>(
imu_topic_, 1,
boost::bind(&GazeboQuadrotorStateController::ImuCallback, this, _1),
ros::VoidPtr(), &callback_queue_);
imu_subscriber_ = node_handle_->subscribe(ops);
ROS_INFO_NAMED("quadrotor_state_controller", "Using imu information on topic %s as source of orientation and angular velocity.", imu_topic_.c_str());
}
// subscribe sonar senor info
if (!sonar_topic_.empty())
{
ros::SubscribeOptions ops = ros::SubscribeOptions::create<sensor_msgs::Range>(
sonar_topic_, 1,
boost::bind(&GazeboQuadrotorStateController::SonarCallback, this, _1),
ros::VoidPtr(), &callback_queue_);
sonar_subscriber_ = node_handle_->subscribe(ops);
ROS_INFO_NAMED("quadrotor_state_controller", "Using sonar information on topic %s as source of altitude.", sonar_topic_.c_str());
}
// subscribe state
if (!state_topic_.empty())
{
ros::SubscribeOptions ops = ros::SubscribeOptions::create<nav_msgs::Odometry>(
state_topic_, 1,
boost::bind(&GazeboQuadrotorStateController::StateCallback, this, _1),
ros::VoidPtr(), &callback_queue_);
state_subscriber_ = node_handle_->subscribe(ops);
ROS_INFO_NAMED("quadrotor_state_controller", "Using state information on topic %s as source of state information.", state_topic_.c_str());
}
// for camera control
// switch camera server
//std::string toggleCam_topic = "ardrone/togglecam";
std::string toggleCam_topic = "togglecam";
ros::AdvertiseServiceOptions toggleCam_ops = ros::AdvertiseServiceOptions::create<std_srvs::Empty>(
toggleCam_topic,
boost::bind(&GazeboQuadrotorStateController::toggleCamCallback, this, _1,_2),
ros::VoidPtr(),
&callback_queue_);
toggleCam_service = node_handle_->advertiseService(toggleCam_ops);
// camera image data
/*
// Original code
std::string cam_out_topic = "/ardrone/image_raw";
std::string cam_front_in_topic = "/ardrone/front/image_raw";
std::string cam_bottom_in_topic = "/ardrone/bottom/image_raw";
*/
// CODE FOR MAKING AR_POSE WORKING: it needs /camera/etc. as topic
// ABSOLUTE PATH REQUIRED BY AR_POSE: should be changed accordingly to match proper video topic
std::string cam_out_topic = "/camera/image_raw";
std::string cam_front_in_topic = "/ardrone/front/image_raw";
std::string cam_bottom_in_topic = "/ardrone/bottom/image_raw";
std::string in_transport = "raw";
camera_it_ = new image_transport::ImageTransport(*node_handle_);
camera_publisher_ = camera_it_->advertise(cam_out_topic, 1);
camera_front_subscriber_ = camera_it_->subscribe(
cam_front_in_topic, 1,
boost::bind(&GazeboQuadrotorStateController::CameraFrontCallback, this, _1),
ros::VoidPtr(), in_transport);
camera_bottom_subscriber_ = camera_it_->subscribe(
cam_bottom_in_topic, 1,
boost::bind(&GazeboQuadrotorStateController::CameraBottomCallback, this, _1),
ros::VoidPtr(), in_transport);
// camera image data
/*
// Original code
std::string cam_info_out_topic = "/ardrone/camera_info";
std::string cam_info_front_in_topic = "/ardrone/front/camera_info";
std::string cam_info_bottom_in_topic = "/ardrone/bottom/camera_info";
*/
// CODE FOR MAKING AR_POSE WORKING: it needs /camera/etc. as topic
// ABSOLUTE PATH REQUIRED BY AR_POSE: should be changed accordingly to match proper video topic
std::string cam_info_out_topic = "/camera/camera_info";
std::string cam_info_front_in_topic = "/ardrone/front/camera_info";
std::string cam_info_bottom_in_topic = "/ardrone/bottom/camera_info";
camera_info_publisher_ = node_handle_->advertise<sensor_msgs::CameraInfo>(cam_info_out_topic,1);
ros::SubscribeOptions cam_info_front_ops = ros::SubscribeOptions::create<sensor_msgs::CameraInfo>(
cam_info_front_in_topic, 1,
boost::bind(&GazeboQuadrotorStateController::CameraInfoFrontCallback, this, _1),
ros::VoidPtr(), &callback_queue_);
camera_info_front_subscriber_ = node_handle_->subscribe(cam_info_front_ops);
ros::SubscribeOptions cam_info_bottom_ops = ros::SubscribeOptions::create<sensor_msgs::CameraInfo>(
cam_info_bottom_in_topic, 1,
boost::bind(&GazeboQuadrotorStateController::CameraInfoBottomCallback, this, _1),
ros::VoidPtr(), &callback_queue_);
camera_info_bottom_subscriber_ = node_handle_->subscribe(cam_info_bottom_ops);
// callback_queue_thread_ = boost::thread( boost::bind( &GazeboQuadrotorStateController::CallbackQueueThread,this ) );
Reset();
// 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(&GazeboQuadrotorStateController::Update, this));
robot_current_state = LANDED_MODEL;
}
////////////////////////////////////////////////////////////////////////////////
// Load the controller
void GazeboRosVacuumGripper::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
{
ROS_INFO("Loading gazebo_ros_vacuum_gripper");
// Set attached model;
parent_ = _model;
// Get the world name.
world_ = _model->GetWorld();
// load parameters
robot_namespace_ = "";
if (_sdf->HasElement("robotNamespace"))
robot_namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>() + "/";
if (!_sdf->HasElement("bodyName"))
{
ROS_FATAL("vacuum_gripper plugin missing <bodyName>, cannot proceed");
return;
}
else
link_name_ = _sdf->GetElement("bodyName")->Get<std::string>();
link_ = _model->GetLink(link_name_);
if (!link_)
{
std::string found;
physics::Link_V links = _model->GetLinks();
for (size_t i = 0; i < links.size(); i++) {
found += std::string(" ") + links[i]->GetName();
}
ROS_FATAL("gazebo_ros_vacuum_gripper plugin error: link named: %s does not exist", link_name_.c_str());
ROS_FATAL("gazebo_ros_vacuum_gripper plugin error: You should check it exists and is not connected with fixed joint");
ROS_FATAL("gazebo_ros_vacuum_gripper plugin error: Found links are: %s", found.c_str());
return;
}
if (!_sdf->HasElement("topicName"))
{
ROS_FATAL("vacuum_gripper plugin missing <serviceName>, cannot proceed");
return;
}
else
topic_name_ = _sdf->GetElement("topicName")->Get<std::string>();
// 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;
}
rosnode_ = new ros::NodeHandle(robot_namespace_);
// Custom Callback Queue
ros::AdvertiseOptions ao = ros::AdvertiseOptions::create<std_msgs::Bool>(
topic_name_, 1,
boost::bind(&GazeboRosVacuumGripper::Connect, this),
boost::bind(&GazeboRosVacuumGripper::Disconnect, this),
ros::VoidPtr(), &queue_);
pub_ = rosnode_->advertise(ao);
// Custom Callback Queue
ros::AdvertiseServiceOptions aso1 =
ros::AdvertiseServiceOptions::create<std_srvs::Empty>(
"on", boost::bind(&GazeboRosVacuumGripper::OnServiceCallback,
this, _1, _2), ros::VoidPtr(), &queue_);
srv1_ = rosnode_->advertiseService(aso1);
ros::AdvertiseServiceOptions aso2 =
ros::AdvertiseServiceOptions::create<std_srvs::Empty>(
"off", boost::bind(&GazeboRosVacuumGripper::OffServiceCallback,
this, _1, _2), ros::VoidPtr(), &queue_);
srv2_ = rosnode_->advertiseService(aso2);
// Custom Callback Queue
callback_queue_thread_ = boost::thread( boost::bind( &GazeboRosVacuumGripper::QueueThread,this ) );
// New Mechanism for Updating every World Cycle
// Listen to the update event. This event is broadcast every
// simulation iteration.
update_connection_ = event::Events::ConnectWorldUpdateBegin(
boost::bind(&GazeboRosVacuumGripper::UpdateChild, this));
ROS_INFO("Loaded gazebo_ros_vacuum_gripper");
}
////////////////////////////////////////////////////////////////////////////////
// Load the controller
void DRCVehicleROSPlugin::Load(physics::ModelPtr _parent,
sdf::ElementPtr _sdf)
{
// By default, cheats are off. Allow override via environment variable.
char* cheatsEnabledString = getenv("VRC_CHEATS_ENABLED");
if (cheatsEnabledString && (std::string(cheatsEnabledString) == "1"))
this->cheatsEnabled = true;
else
this->cheatsEnabled = false;
try
{
DRCVehiclePlugin::Load(_parent, _sdf);
}
catch(gazebo::common::Exception &_e)
{
gzerr << "Error loading plugin."
<< "Please ensure that your vehicle model is correct and up-to-date."
<< '\n';
return;
}
// initialize ros
if (!ros::isInitialized())
{
gzerr << "Not loading plugin since ROS hasn't been "
<< "properly initialized. Try starting gazebo with ros plugin:\n"
<< " gazebo -s libgazebo_ros_api_plugin.so\n";
return;
}
// ros stuff
this->rosNode = new ros::NodeHandle("");
// Get the world name.
this->world = _parent->GetWorld();
this->model = _parent;
if (this->cheatsEnabled)
{
ros::SubscribeOptions hand_wheel_cmd_so =
ros::SubscribeOptions::create<std_msgs::Float64>(
this->model->GetName() + "/hand_wheel/cmd", 100,
boost::bind(static_cast< void (DRCVehicleROSPlugin::*)
(const std_msgs::Float64::ConstPtr&) >(
&DRCVehicleROSPlugin::SetHandWheelState), this, _1),
ros::VoidPtr(), &this->queue);
this->subHandWheelCmd = this->rosNode->subscribe(hand_wheel_cmd_so);
ros::SubscribeOptions hand_brake_cmd_so =
ros::SubscribeOptions::create< std_msgs::Float64 >(
this->model->GetName() + "/hand_brake/cmd", 100,
boost::bind(static_cast< void (DRCVehicleROSPlugin::*)
(const std_msgs::Float64::ConstPtr&) >(
&DRCVehicleROSPlugin::SetHandBrakePercent), this, _1),
ros::VoidPtr(), &this->queue);
this->subHandBrakeCmd = this->rosNode->subscribe(hand_brake_cmd_so);
ros::SubscribeOptions gas_pedal_cmd_so =
ros::SubscribeOptions::create< std_msgs::Float64 >(
this->model->GetName() + "/gas_pedal/cmd", 100,
boost::bind(static_cast< void (DRCVehicleROSPlugin::*)
(const std_msgs::Float64::ConstPtr&) >(
&DRCVehicleROSPlugin::SetGasPedalPercent), this, _1),
ros::VoidPtr(), &this->queue);
this->subGasPedalCmd = this->rosNode->subscribe(gas_pedal_cmd_so);
ros::SubscribeOptions brake_pedal_cmd_so =
ros::SubscribeOptions::create< std_msgs::Float64 >(
this->model->GetName() + "/brake_pedal/cmd", 100,
boost::bind(static_cast< void (DRCVehicleROSPlugin::*)
(const std_msgs::Float64::ConstPtr&) >(
&DRCVehicleROSPlugin::SetBrakePedalPercent), this, _1),
ros::VoidPtr(), &this->queue);
this->subBrakePedalCmd = this->rosNode->subscribe(brake_pedal_cmd_so);
ros::SubscribeOptions key_cmd_so =
ros::SubscribeOptions::create< std_msgs::Int8 >(
this->model->GetName() + "/key/cmd", 100,
boost::bind(static_cast< void (DRCVehicleROSPlugin::*)
(const std_msgs::Int8::ConstPtr&) >(
&DRCVehicleROSPlugin::SetKeyState), this, _1),
ros::VoidPtr(), &this->queue);
this->subKeyCmd = this->rosNode->subscribe(key_cmd_so);
ros::SubscribeOptions direction_cmd_so =
ros::SubscribeOptions::create< std_msgs::Int8 >(
this->model->GetName() + "/direction/cmd", 100,
boost::bind(static_cast< void (DRCVehicleROSPlugin::*)
(const std_msgs::Int8::ConstPtr&) >(
&DRCVehicleROSPlugin::SetDirectionState), this, _1),
ros::VoidPtr(), &this->queue);
this->subDirectionCmd = this->rosNode->subscribe(direction_cmd_so);
this->pubHandWheelState = this->rosNode->advertise<std_msgs::Float64>(
this->model->GetName() + "/hand_wheel/state", 10);
this->pubHandBrakeState = this->rosNode->advertise<std_msgs::Float64>(
this->model->GetName() + "/hand_brake/state", 10);
this->pubGasPedalState = this->rosNode->advertise<std_msgs::Float64>(
this->model->GetName() + "/gas_pedal/state", 10);
this->pubBrakePedalState = this->rosNode->advertise<std_msgs::Float64>(
this->model->GetName() + "/brake_pedal/state", 10);
this->pubKeyState = this->rosNode->advertise<std_msgs::Int8>(
this->model->GetName() + "/key/state", 10);
this->pubDirectionState = this->rosNode->advertise<std_msgs::Int8>(
this->model->GetName() + "/direction/state", 10);
// ros callback queue for processing subscription
this->callbackQueueThread = boost::thread(
boost::bind(&DRCVehicleROSPlugin::QueueThread, this));
this->ros_publish_connection_ = event::Events::ConnectWorldUpdateBegin(
boost::bind(&DRCVehicleROSPlugin::RosPublishStates, this));
}
}
void GazeboRosCreate::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf )
{
this->my_world_ = _parent->GetWorld();
this->my_parent_ = _parent;
if (!this->my_parent_)
{
ROS_FATAL("Gazebo_ROS_Create controller requires a Model as its parent");
return;
}
this->node_namespace_ = "";
if (_sdf->HasElement("node_namespace"))
this->node_namespace_ = _sdf->GetElement("node_namespace")->GetValueString() + "/";
left_wheel_joint_name_ = "left_wheel_joint";
if (_sdf->HasElement("left_wheel_joint"))
left_wheel_joint_name_ = _sdf->GetElement("left_wheel_joint")->GetValueString();
right_wheel_joint_name_ = "right_wheel_joint";
if (_sdf->HasElement("right_wheel_joint"))
right_wheel_joint_name_ = _sdf->GetElement("right_wheel_joint")->GetValueString();
front_castor_joint_name_ = "front_castor_joint";
if (_sdf->HasElement("front_castor_joint"))
front_castor_joint_name_ = _sdf->GetElement("front_castor_joint")->GetValueString();
rear_castor_joint_name_ = "rear_castor_joint";
if (_sdf->HasElement("rear_castor_joint"))
rear_castor_joint_name_ = _sdf->GetElement("rear_castor_joint")->GetValueString();
wheel_sep_ = 0.34;
if (_sdf->HasElement("wheel_separation"))
wheel_sep_ = _sdf->GetElement("wheel_separation")->GetValueDouble();
wheel_sep_ = 0.34;
if (_sdf->HasElement("wheel_separation"))
wheel_sep_ = _sdf->GetElement("wheel_separation")->GetValueDouble();
wheel_diam_ = 0.15;
if (_sdf->HasElement("wheel_diameter"))
wheel_diam_ = _sdf->GetElement("wheel_diameter")->GetValueDouble();
torque_ = 10.0;
if (_sdf->HasElement("torque"))
torque_ = _sdf->GetElement("torque")->GetValueDouble();
//base_geom_name_ = "base_geom";
base_geom_name_ = "base_footprint_geom_base_link";
if (_sdf->HasElement("base_geom"))
base_geom_name_ = _sdf->GetElement("base_geom")->GetValueString();
base_geom_ = my_parent_->GetChildCollision(base_geom_name_);
if (!base_geom_)
{
// This is a hack for ROS Diamond back. E-turtle and future releases
// will not need this, because it will contain the fixed-joint reduction
// in urdf2gazebo
base_geom_ = my_parent_->GetChildCollision("base_footprint_geom");
if (!base_geom_)
{
ROS_ERROR("Unable to find geom[%s]",base_geom_name_.c_str());
return;
}
}
base_geom_->SetContactsEnabled(true);
contact_event_ = base_geom_->ConnectContact(boost::bind(&GazeboRosCreate::OnContact, this, _1, _2));
// Get then name of the parent model
std::string modelName = _sdf->GetParent()->GetValueString("name");
// Listen to the update event. This event is broadcast every
// simulation iteration.
this->updateConnection = event::Events::ConnectWorldUpdateStart(
boost::bind(&GazeboRosCreate::UpdateChild, this));
gzdbg << "plugin model name: " << modelName << "\n";
/*wall_sensor_ = boost::shared_dynamic_cast<sensors::RaySensor>(
sensors::SensorManager::Instance()->GetSensor("wall_sensor"));
if (!wall_sensor_)
{
ROS_ERROR("Unable to find sensor[wall_sensor]");
return;
}
left_cliff_sensor_ = boost::shared_dynamic_cast<sensors::RaySensor>(
sensors::SensorManager::Instance()->GetSensor("left_cliff_sensor"));
right_cliff_sensor_ = boost::shared_dynamic_cast<sensors::RaySensor>(
sensors::SensorManager::Instance()->GetSensor("right_cliff_sensor"));
leftfront_cliff_sensor_ = boost::shared_dynamic_cast<sensors::RaySensor>(
sensors::SensorManager::Instance()->GetSensor("leftfront_cliff_sensor"));
rightfront_cliff_sensor_ = boost::shared_dynamic_cast<sensors::RaySensor>(
sensors::SensorManager::Instance()->GetSensor("rightfront_cliff_sensor"));*/
if (!ros::isInitialized())
{
int argc = 0;
char** argv = NULL;
ros::init(argc, argv, "gazebo_myrabot", ros::init_options::NoSigintHandler|ros::init_options::AnonymousName);
}
rosnode_ = new ros::NodeHandle( node_namespace_ );
cmd_vel_sub_ = rosnode_->subscribe("/cmd_vel", 1, &GazeboRosCreate::OnCmdVel, this );
sensor_state_pub_ = rosnode_->advertise<turtlebot_node::TurtlebotSensorState>("sensor_state", 1);
odom_pub_ = rosnode_->advertise<nav_msgs::Odometry>("/odom", 1);
joint_state_pub_ = rosnode_->advertise<sensor_msgs::JointState>("/joint_states_roomba", 1);
js_.name.push_back( left_wheel_joint_name_ );
js_.position.push_back(0);
js_.velocity.push_back(0);
js_.effort.push_back(0);
js_.name.push_back( right_wheel_joint_name_ );
js_.position.push_back(0);
js_.velocity.push_back(0);
js_.effort.push_back(0);
js_.name.push_back( front_castor_joint_name_ );
js_.position.push_back(0);
js_.velocity.push_back(0);
js_.effort.push_back(0);
js_.name.push_back( rear_castor_joint_name_ );
js_.position.push_back(0);
js_.velocity.push_back(0);
js_.effort.push_back(0);
prev_update_time_ = 0;
last_cmd_vel_time_ = 0;
sensor_state_.bumps_wheeldrops = 0x0;
//TODO: fix this
joints_[LEFT] = my_parent_->GetJoint(left_wheel_joint_name_);
joints_[RIGHT] = my_parent_->GetJoint(right_wheel_joint_name_);
joints_[FRONT] = my_parent_->GetJoint(front_castor_joint_name_);
joints_[REAR] = my_parent_->GetJoint(rear_castor_joint_name_);
if (joints_[LEFT]) set_joints_[LEFT] = true;
if (joints_[RIGHT]) set_joints_[RIGHT] = true;
if (joints_[FRONT]) set_joints_[FRONT] = true;
if (joints_[REAR]) set_joints_[REAR] = true;
//initialize time and odometry position
prev_update_time_ = last_cmd_vel_time_ = this->my_world_->GetSimTime();
odom_pose_[0] = 0.0;
odom_pose_[1] = 0.0;
odom_pose_[2] = 0.0;
}
////////////////////////////////////////////////////////////////////////////////
// Load the controller
void GazeboRosP3D::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
{
// Get the world name.
this->world_ = _parent->GetWorld();
this->model_ = _parent;
// load parameters
this->robot_namespace_ = "";
if (_sdf->HasElement("robotNamespace"))
this->robot_namespace_ =
_sdf->GetElement("robotNamespace")->Get<std::string>() + "/";
if (!_sdf->HasElement("bodyName"))
{
ROS_FATAL("p3d plugin missing <bodyName>, cannot proceed");
return;
}
else
this->link_name_ = _sdf->GetElement("bodyName")->Get<std::string>();
this->link_ = _parent->GetLink(this->link_name_);
if (!this->link_)
{
ROS_FATAL("gazebo_ros_p3d plugin error: bodyName: %s does not exist\n",
this->link_name_.c_str());
return;
}
if (!_sdf->HasElement("topicName"))
{
ROS_FATAL("p3d plugin missing <topicName>, cannot proceed");
return;
}
else
this->topic_name_ = _sdf->GetElement("topicName")->Get<std::string>();
if (!_sdf->HasElement("frameName"))
{
ROS_DEBUG("p3d plugin missing <frameName>, defaults to world");
this->frame_name_ = "world";
}
else
this->frame_name_ = _sdf->GetElement("frameName")->Get<std::string>();
if (!_sdf->HasElement("xyzOffset"))
{
ROS_DEBUG("p3d plugin missing <xyzOffset>, defaults to 0s");
this->offset_.pos = math::Vector3(0, 0, 0);
}
else
this->offset_.pos = _sdf->GetElement("xyzOffset")->Get<math::Vector3>();
if (!_sdf->HasElement("rpyOffset"))
{
ROS_DEBUG("p3d plugin missing <rpyOffset>, defaults to 0s");
this->offset_.rot = math::Vector3(0, 0, 0);
}
else
this->offset_.rot = _sdf->GetElement("rpyOffset")->Get<math::Vector3>();
if (!_sdf->HasElement("gaussianNoise"))
{
ROS_DEBUG("p3d plugin missing <gaussianNoise>, defaults to 0.0");
this->gaussian_noise_ = 0;
}
else
this->gaussian_noise_ = _sdf->GetElement("gaussianNoise")->Get<double>();
if (!_sdf->HasElement("updateRate"))
{
ROS_DEBUG("p3d plugin missing <updateRate>, defaults to 0.0"
" (as fast as possible)");
this->update_rate_ = 0;
}
else
this->update_rate_ = _sdf->GetElement("updateRate")->Get<double>();
// 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;
}
this->rosnode_ = new ros::NodeHandle(this->robot_namespace_);
// publish multi queue
this->pmq.startServiceThread();
// resolve tf prefix
std::string prefix;
this->rosnode_->getParam(std::string("tf_prefix"), prefix);
this->tf_frame_name_ = tf::resolve(prefix, this->frame_name_);
if (this->topic_name_ != "")
{
this->pub_Queue = this->pmq.addPub<nav_msgs::Odometry>();
this->pub_ =
this->rosnode_->advertise<nav_msgs::Odometry>(this->topic_name_, 1);
}
this->last_time_ = this->world_->GetSimTime();
// initialize body
this->last_vpos_ = this->link_->GetWorldLinearVel();
this->last_veul_ = this->link_->GetWorldAngularVel();
this->apos_ = 0;
this->aeul_ = 0;
// if frameName specified is "world", "/map" or "map" report
// back inertial values in the gazebo world
if (this->frame_name_ != "world" &&
this->frame_name_ != "/map" &&
this->frame_name_ != "map")
{
this->reference_link_ = this->model_->GetLink(this->frame_name_);
if (!this->reference_link_)
{
ROS_ERROR("gazebo_ros_p3d plugin: frameName: %s does not exist, will"
" not publish pose\n", this->frame_name_.c_str());
return;
}
}
// init reference frame state
if (this->reference_link_)
{
ROS_DEBUG("got body %s", this->reference_link_->GetName().c_str());
this->frame_apos_ = 0;
this->frame_aeul_ = 0;
this->last_frame_vpos_ = this->reference_link_->GetWorldLinearVel();
this->last_frame_veul_ = this->reference_link_->GetWorldAngularVel();
}
// start custom queue for p3d
this->callback_queue_thread_ = boost::thread(
boost::bind(&GazeboRosP3D::P3DQueueThread, this));
// New Mechanism for Updating every World Cycle
// Listen to the update event. This event is broadcast every
// simulation iteration.
this->update_connection_ = event::Events::ConnectWorldUpdateBegin(
boost::bind(&GazeboRosP3D::UpdateChild, this));
}
