////////////////////////////////////////////////////////////////////////////////
// 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));
}
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 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 BatteryConsumer::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
{
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;
}
double default_consume = DEFAULT_CONSUMER_CONSUME;
std::string name = DEFAULT_CONSUMER_NAME;
if (_sdf->HasElement("robotNamespace")) namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>();
if (_sdf->HasElement("default_consume")) default_consume = _sdf->GetElement("default_consume")->Get<double>();
if (_sdf->HasElement("default_consumer_name")) name = _sdf->GetElement("default_consumer_name")->Get<std::string>();
default_consumer.first = name;
default_consumer.second = default_consume;
consumers.insert(default_consumer);
this->model = _parent;
n = ros::NodeHandle(namespace_);
battery_pub = n.advertise<std_msgs::Float64>("battery_state", 1);
add_consumer_service = n.advertiseService("add_consumer",&BatteryConsumer::addConsumer,this);
remove_consumer_service = n.advertiseService("remove_consumer",&BatteryConsumer::removeConsumer,this);
last_time = model->GetWorld()->GetSimTime().Double();
this->updateConnection = event::Events::ConnectWorldUpdateBegin(
boost::bind(&BatteryConsumer::OnUpdate, 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 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));
}
////////////////////////////////////////////////////////////////////////////////
// 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));
}
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));
}
////////////////////////////////////////////////////////////////////////////////
// Load the controller
void GazeboRosSonar::Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sdf)
{
// Get then name of the parent sensor
sensor_ = boost::shared_dynamic_cast<sensors::RaySensor>(_sensor);
if (!sensor_)
{
gzthrow("GazeboRosSonar requires a Ray Sensor as its parent");
return;
}
// Get the world name.
std::string worldName = sensor_->GetWorldName();
world = physics::get_world(worldName);
// load parameters
if (!_sdf->HasElement("robotNamespace"))
namespace_.clear();
else
namespace_ = _sdf->GetElement("robotNamespace")->GetValueString() + "/";
if (!_sdf->HasElement("frameId"))
frame_id_ = "";
else
frame_id_ = _sdf->GetElement("frameId")->GetValueString();
if (!_sdf->HasElement("topicName"))
topic_ = "sonar";
else
topic_ = _sdf->GetElement("topicName")->GetValueString();
sensor_model_.Load(_sdf);
range_.header.frame_id = frame_id_;
range_.radiation_type = sensor_msgs::Range::ULTRASOUND;
range_.field_of_view = std::min(fabs((sensor_->GetAngleMax() - sensor_->GetAngleMin()).GetAsRadian()), fabs((sensor_->GetVerticalAngleMax() - sensor_->GetVerticalAngleMin()).GetAsRadian()));
range_.max_range = sensor_->GetRangeMax();
range_.min_range = sensor_->GetRangeMin();
// start ros node
if (!ros::isInitialized())
{
int argc = 0;
char** argv = NULL;
ros::init(argc,argv,"gazebo",ros::init_options::NoSigintHandler|ros::init_options::AnonymousName);
}
node_handle_ = new ros::NodeHandle(namespace_);
publisher_ = node_handle_->advertise<sensor_msgs::Range>(topic_, 1);
Reset();
updateConnection = sensor_->GetLaserShape()->ConnectNewLaserScans(
boost::bind(&GazeboRosSonar::Update, this));
// activate RaySensor
sensor_->SetActive(true);
}
void BasicSonar::Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sdf)
{
sensor_ = boost::dynamic_pointer_cast<sensors::RaySensor>(_sensor);
if (!sensor_)
{
gzthrow("BasicSonar requires a Ray Sensor as its parent");
return;
}
pixels_per_line = std::max(sensor_->GetRangeCount(), 1);
gaussian_kernel = 0;
if (_sdf->HasElement("gaussian_kernel"))
gaussian_kernel = _sdf->Get<double>("gaussian_kernel");
base_water_noise = 0;
if (_sdf->HasElement("base_water_noise"))
base_water_noise = _sdf->Get<double>("base_water_noise");
default_scan_retro = 0;
if (_sdf->HasElement("default_scan_retro"))
default_scan_retro = _sdf->Get<double>("default_scan_retro");
interpolation_limit = 0.5;
if (_sdf->HasElement("interpolation_limit"))
interpolation_limit = _sdf->Get<double>("interpolation_limit");
if (_sdf->HasElement("service_name"))
service_name = _sdf->Get<std::string>("service_name");
if (service_name.empty())
{
gzthrow("BasicSonar requires a unique service name");
return;
}
std::string worldName = sensor_->GetWorldName();
world = physics::get_world(worldName);
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("gazebo/basic_sonar/");
service = node_handle_->advertiseService(service_name.c_str(), &BasicSonar::getSonarScan, this);
sensor_->SetActive(true);
ROS_INFO("loaded sonar plugin");
}
gz::common::PID Motor::createPid(sdf::ElementPtr pidElem) {
double pv = 0, iv = 0, dv = 0, iMax = 0, iMin = 0,
cmdMax = 0, cmdMin = 0;
if (pidElem->HasElement("rv:p")) pv = pidElem->GetElement("rv:p")->Get<double>();
if (pidElem->HasElement("rv:i")) iv = pidElem->GetElement("rv:i")->Get<double>();
if (pidElem->HasElement("rv:d")) dv = pidElem->GetElement("rv:d")->Get<double>();
if (pidElem->HasElement("rv:i_max")) iMax = pidElem->GetElement("rv:i_max")->Get<double>();
if (pidElem->HasElement("rv:i_min")) iMin = pidElem->GetElement("rv:i_min")->Get<double>();
if (pidElem->HasElement("rv:cmd_max")) cmdMax = pidElem->GetElement("rv:cmd_max")->Get<double>();
if (pidElem->HasElement("rv:cmd_min")) cmdMin = pidElem->GetElement("rv:cmd_min")->Get<double>();
return gz::common::PID(pv, iv, dv, iMax, iMin, cmdMax, cmdMin);
}
void GazeboAltimeterPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
{
// Configure Gazebo Integration
model_ = _model;
world_ = model_->GetWorld();
namespace_.clear();
if (_sdf->HasElement("robotNamespace"))
namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>();
else
gzerr << "[gazebo_altimeter_plugin] Please specify a robotNamespace.\n";
if (_sdf->HasElement("linkName"))
link_name_ = _sdf->GetElement("linkName")->Get<std::string>();
else
gzerr << "[gazebo_altimeter_plugin] Please specify a linkName.\n";
link_ = model_->GetLink(link_name_);
if (link_ == NULL)
gzthrow("[gazebo_altimeter_plugin] Couldn't find specified link \"" << link_name_ << "\".");
// Connect to the Gazebo Update
this->updateConnection_ = event::Events::ConnectWorldUpdateBegin(boost::bind(&GazeboAltimeterPlugin::OnUpdate, this, _1));
frame_id_ = link_name_;
// load params from xacro
getSdfParam<std::string>(_sdf, "altimeterTopic", alt_topic_, "altimeter");
getSdfParam<double>(_sdf, "altimeterMinRange", min_range_, 0.381);
getSdfParam<double>(_sdf, "altimeterMaxRange", max_range_, 6.45);
getSdfParam<double>(_sdf, "altimeterErrorStdev", error_stdev_, 0.10);
getSdfParam<double>(_sdf, "altimeterFOV", field_of_view_, 1.107);
getSdfParam<double>(_sdf, "altimeterPublishRate", pub_rate_, 20.0);
getSdfParam<bool>(_sdf, "altimeterNoiseOn", alt_noise_on_, true);
getSdfParam<bool>(_sdf, "publishFloat32", publish_float_, false);
last_time_ = world_->GetSimTime();
// Configure ROS Integration
node_handle_ = new ros::NodeHandle(namespace_);
if(publish_float_)
alt_pub_ = node_handle_->advertise<std_msgs::Float32>(alt_topic_, 10);
else
alt_pub_ = node_handle_->advertise<sensor_msgs::Range>(alt_topic_, 10);
// Fill static members of alt message.
alt_message_.header.frame_id = frame_id_;
alt_message_.max_range = max_range_;
alt_message_.min_range = min_range_;
alt_message_.field_of_view = field_of_view_;
alt_message_.radiation_type = sensor_msgs::Range::ULTRASOUND;
// Configure Noise
random_generator_= std::default_random_engine(std::chrono::system_clock::now().time_since_epoch().count());
standard_normal_distribution_ = std::normal_distribution<double>(0.0, error_stdev_);
}
void GazeboWindPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) {
// Store the pointer to the model.
model_ = _model;
world_ = model_->GetWorld();
double wind_gust_start = kDefaultWindGustStart;
double wind_gust_duration = kDefaultWindGustDuration;
if (_sdf->HasElement("robotNamespace"))
namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>();
else
gzerr << "[gazebo_wind_plugin] Please specify a robotNamespace.\n";
node_handle_ = new ros::NodeHandle(namespace_);
if (_sdf->HasElement("xyzOffset"))
xyz_offset_ = _sdf->GetElement("xyzOffset")->Get<math::Vector3>();
else
gzerr << "[gazebo_wind_plugin] Please specify a xyzOffset.\n";
getSdfParam<std::string>(_sdf, "windPubTopic", wind_pub_topic_, "/" + namespace_ + wind_pub_topic_);
getSdfParam<std::string>(_sdf, "frameId", frame_id_, frame_id_);
getSdfParam<std::string>(_sdf, "linkName", link_name_, link_name_);
// Get the wind params from SDF.
getSdfParam<double>(_sdf, "windForceMean", wind_force_mean_, wind_force_mean_);
getSdfParam<double>(_sdf, "windForceVariance", wind_force_variance_, wind_force_variance_);
getSdfParam<math::Vector3>(_sdf, "windDirection", wind_direction_, wind_direction_);
// Get the wind gust params from SDF.
getSdfParam<double>(_sdf, "windGustStart", wind_gust_start, wind_gust_start);
getSdfParam<double>(_sdf, "windGustDuration", wind_gust_duration, wind_gust_duration);
getSdfParam<double>(_sdf, "windGustForceMean", wind_gust_force_mean_, wind_gust_force_mean_);
getSdfParam<double>(_sdf, "windGustForceVariance", wind_gust_force_variance_, wind_gust_force_variance_);
getSdfParam<math::Vector3>(_sdf, "windGustDirection", wind_gust_direction_, wind_gust_direction_);
wind_direction_.Normalize();
wind_gust_direction_.Normalize();
wind_gust_start_ = common::Time(wind_gust_start);
wind_gust_end_ = common::Time(wind_gust_start + wind_gust_duration);
link_ = model_->GetLink(link_name_);
if (link_ == NULL) gzthrow("[gazebo_wind_plugin] Couldn't find specified link \"" << link_name_ << "\".");
// Listen to the update event. This event is broadcast every
// simulation iteration.
update_connection_ = event::Events::ConnectWorldUpdateBegin(boost::bind(&GazeboWindPlugin::OnUpdate, this, _1));
wind_pub_ = node_handle_->advertise<geometry_msgs::WrenchStamped>(wind_pub_topic_, 10);
}
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));
}
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 GazeboLidarPlugin::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf)
{
if(kPrintOnPluginLoad) {
gzdbg << __FUNCTION__ << "() called." << std::endl;
}
// Get then name of the parent sensor
this->parentSensor =
std::dynamic_pointer_cast<sensors::RaySensor>(_parent);
if (!this->parentSensor)
gzthrow("RayPlugin requires a Ray Sensor as its parent");
this->world = physics::get_world(this->parentSensor->WorldName());
this->newLaserScansConnection =
this->parentSensor->LaserShape()->ConnectNewLaserScans(
boost::bind(&GazeboLidarPlugin::OnNewLaserScans, this));
if (_sdf->HasElement("robotNamespace"))
namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>();
else
gzwarn << "[gazebo_lidar_plugin] Please specify a robotNamespace.\n";
node_handle_ = transport::NodePtr(new transport::Node());
node_handle_->Init(namespace_);
const string scopedName = _parent->ParentName();
string topicName = "~/" + scopedName + "/lidar";
boost::replace_all(topicName, "::", "/");
lidar_pub_ = node_handle_->Advertise<lidar_msgs::msgs::lidar>(topicName, 10);
}
void GazeboQuadrotorSimpleController::PIDController::Load(sdf::ElementPtr _sdf, const std::string& prefix)
{
gain_p = 0.0;
gain_d = 0.0;
gain_i = 0.0;
time_constant = 0.0;
limit = -1.0;
if (!_sdf) return;
// _sdf->PrintDescription(_sdf->GetName());
if (_sdf->HasElement(prefix + "ProportionalGain")) gain_p = _sdf->GetElement(prefix + "ProportionalGain")->GetValueDouble();
if (_sdf->HasElement(prefix + "DifferentialGain")) gain_d = _sdf->GetElement(prefix + "DifferentialGain")->GetValueDouble();
if (_sdf->HasElement(prefix + "IntegralGain")) gain_i = _sdf->GetElement(prefix + "IntegralGain")->GetValueDouble();
if (_sdf->HasElement(prefix + "TimeConstant")) time_constant = _sdf->GetElement(prefix + "TimeConstant")->GetValueDouble();
if (_sdf->HasElement(prefix + "Limit")) limit = _sdf->GetElement(prefix + "Limit")->GetValueDouble();
}
void RayPlugin::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf)
{
// Get then name of the parent sensor
this->parentSensor =
boost::dynamic_pointer_cast<sensors::RaySensor>(_parent);
if (!this->parentSensor)
gzthrow("RayPlugin requires a Ray Sensor as its parent");
this->world = physics::get_world(this->parentSensor->GetWorldName());
this->newLaserScansConnection =
this->parentSensor->GetLaserShape()->ConnectNewLaserScans(
boost::bind(&RayPlugin::OnNewLaserScans, this));
if (_sdf->HasElement("robotNamespace"))
namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>();
else
gzwarn << "[gazebo_lidar_plugin] Please specify a robotNamespace.\n";
node_handle_ = transport::NodePtr(new transport::Node());
node_handle_->Init(namespace_);
lidar_pub_ = node_handle_->Advertise<lidar_msgs::msgs::lidar>("lidar", 10);
}
void GazeboAirspeedPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) {
// Store the pointer to the model
model_ = _model;
world_ = model_->GetWorld();
// default params
namespace_.clear();
if (_sdf->HasElement("namespace"))
namespace_ = _sdf->GetElement("namespace")->Get<std::string>();
else
gzerr << "[gazebo_imu_plugin] Please specify a namespace.\n";
nh_ = new ros::NodeHandle(namespace_);
if (_sdf->HasElement("linkName"))
link_name_ = _sdf->GetElement("linkName")->Get<std::string>();
else
gzerr << "[gazebo_imu_plugin] Please specify a linkName.\n";
link_ = model_->GetLink(link_name_);
if (link_ == NULL)
gzthrow("[gazebo_imu_plugin] Couldn't find specified link \"" << link_name_ << "\".");
frame_id_ = link_name_;
getSdfParam<std::string>(_sdf, "airspeedTopic", airspeed_topic_, "airspeed/data");
getSdfParam<double>(_sdf, "pressureBias", pressure_bias_, 0);
getSdfParam<double>(_sdf, "pressureNoiseSigma", pressure_noise_sigma_,10);
getSdfParam<double>(_sdf, "airDensity", rho_, 1.225);
getSdfParam<double>(_sdf, "maxPressure", max_pressure_, 4000.0);
getSdfParam<double>(_sdf, "minPressure", min_pressure_, 0.0);
last_time_ = world_->GetSimTime();
// Listen to the update event. This event is broadcast every simulation iteration.
this->updateConnection_ = event::Events::ConnectWorldUpdateBegin(boost::bind(&GazeboAirspeedPlugin::OnUpdate, this, _1));
airspeed_pub_ = nh_->advertise<sensor_msgs::FluidPressure>(airspeed_topic_, 10);
// Fill static members of airspeed message.
airspeed_message_.header.frame_id = frame_id_;
airspeed_message_.variance = pressure_noise_sigma_*pressure_noise_sigma_;
standard_normal_distribution_ = std::normal_distribution<double>(0.0, 1.0);
}
////////////////////////////////////////////////////////////////////////////////
// Load the controller
void GazeboRosOpenniKinect::Load(sensors::SensorPtr _parent,
sdf::ElementPtr _sdf) {
std::cout << "LOAD OPENNI" << std::endl;
DepthCameraPlugin::Load(_parent, _sdf);
// copying from DepthCameraPlugin into GazeboRosCameraUtils
this->parentSensor_ = this->parentSensor;
this->width_ = this->width;
this->height_ = this->height;
this->depth_ = this->depth;
this->format_ = this->format;
this->camera_ = this->depthCamera;
// using a different default
if (!_sdf->GetElement("imageTopicName"))
this->image_topic_name_ = "ir/image_raw";
if (!_sdf->HasElement("cameraInfoTopicName"))
this->camera_info_topic_name_ = "ir/camera_info";
// point cloud stuff
if (!_sdf->GetElement("pointCloudTopicName"))
this->point_cloud_topic_name_ = "points";
else
this->point_cloud_topic_name_ =
_sdf->GetElement("pointCloudTopicName")->Get<std::string>();
// depth image stuff
this->depth_image_topic_name_ = "mukodik";
// if (!_sdf->GetElement("depthImageTopicName"))
// this->depth_image_topic_name_ = "depth/image_raw";
// else
// this->depth_image_topic_name_ =
// _sdf->GetElement("depthImageTopicName")->Get<std::string>();
if (!_sdf->GetElement("depthImageCameraInfoTopicName"))
this->depth_image_camera_info_topic_name_ = "depth/camera_info";
else
this->depth_image_camera_info_topic_name_ = _sdf->GetElement(
"depthImageCameraInfoTopicName")->Get<std::string>();
if (!_sdf->GetElement("pointCloudCutoff"))
this->point_cloud_cutoff_ = 0.4;
else
this->point_cloud_cutoff_ = _sdf->GetElement("pointCloudCutoff")->Get<
double>();
load_connection_ = GazeboRosCameraUtils::OnLoad(
boost::bind(&GazeboRosOpenniKinect::Advertise, this));
std::cout << "Establishing Update Connection!" << std::endl;
update_connection_ = event::Events::ConnectPreRender(
boost::bind(&GazeboRosOpenniKinect::OnUpdate, this));
GazeboRosCameraUtils::Load(_parent, _sdf);
}
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));
}
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));
}
/** on loading of the plugin
* @param _parent Parent Model
*/
void Puck::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
{
printf("Loading Puck Plugin of model %s\n", _parent->GetName().c_str());
// Store the pointer to the model
this->model_ = _parent;
// Listen to the update event. This event is broadcast every
// simulation iteration.
this->update_connection_ = event::Events::ConnectWorldUpdateBegin(boost::bind(&Puck::OnUpdate, this, _1));
// Create the communication Node for communication with fawkes
this->node_ = transport::NodePtr(new transport::Node());
// the namespace is set to the world name!
this->node_->Init(model_->GetWorld()->GetName());
// register visual publisher
this->visual_pub_ = this->node_->Advertise<msgs::Visual>("~/visual");
// initialize without rings or cap
this->ring_count_ = 0;
this->have_cap = false;
this->announced_ = false;
this->new_puck_publisher = this->node_->Advertise<gazsim_msgs::NewPuck>("~/new_puck");
// subscribe for puck commands
this->command_subscriber = this->node_->Subscribe(std::string("~/pucks/cmd"), &Puck::on_command_msg, this);
// publisher for workpiece command results
this->workpiece_result_pub_ = node_->Advertise<gazsim_msgs::WorkpieceResult>("~/pucks/cmd/result");
if (!_sdf->HasElement("baseColor")) {
printf("SDF for base has no baseColor configured, defaulting to RED!\n");
base_color_ = gazsim_msgs::Color::RED;
} else {
std::string config_color = _sdf->GetElement("baseColor")->Get<std::string>();
if (config_color == "RED") {
base_color_ = gazsim_msgs::Color::RED;
} else if (config_color == "BLACK") {
base_color_ = gazsim_msgs::Color::BLACK;
} else if (config_color == "SILVER") {
base_color_ = gazsim_msgs::Color::SILVER;
} else {
printf("SDF for base has no baseColor configured, defaulting to RED!\n");
base_color_ = gazsim_msgs::Color::RED;
}
printf("Base spawns in color %s\n", config_color.c_str());
}
delivery_pub_ = node_->Advertise<llsf_msgs::SetOrderDeliveredByColor>(TOPIC_SET_ORDER_DELIVERY_BY_COLOR);
}
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 GazeboRosElevator::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
{
// load parameters
this->robotNamespace_ = "";
if (_sdf->HasElement("robotNamespace"))
{
this->robotNamespace_ = _sdf->GetElement(
"robotNamespace")->Get<std::string>() + "/";
}
// Make sure the ROS node for Gazebo has already been initialized
if (!ros::isInitialized())
{
ROS_FATAL_STREAM_NAMED("elevator", "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;
}
std::string topic = "elevator";
if (_sdf->HasElement("topic"))
topic = _sdf->Get<std::string>("topic");
ElevatorPlugin::Load(_parent, _sdf);
this->rosnode_ = new ros::NodeHandle(this->robotNamespace_);
ros::SubscribeOptions so =
ros::SubscribeOptions::create<std_msgs::String>(topic, 1,
boost::bind(&GazeboRosElevator::OnElevator, this, _1),
ros::VoidPtr(), &this->queue_);
this->elevatorSub_ = this->rosnode_->subscribe(so);
// start custom queue for elevator
this->callbackQueueThread_ =
boost::thread(boost::bind(&GazeboRosElevator::QueueThread, this));
}
InternalLimitSwitch::InternalLimitSwitch(physics::ModelPtr model, sdf::ElementPtr sdf) {
joint = model->GetJoint(sdf->Get<std::string>("joint"));
min = sdf->Get<double>("min");
max = sdf->Get<double>("max");
if (sdf->HasElement("units")) {
radians = sdf->Get<std::string>("units") != "degrees";
} else {
radians = true;
}
gzmsg << "\tinternal limit switch: " << " type=" << joint->GetName()
<< " range=[" << min << "," << max << "] radians=" << radians << std::endl;
}
bool AllWheelSteeringPlugin::FindJointByParam(sdf::ElementPtr _sdf, physics::JointPtr &_joint, std::string _param) {
if (!_sdf->HasElement(_param)) {
gzerr << "param [" << _param << "] not found\n";
return false;
}
else {
_joint = this->model->GetJoint(_sdf->GetElement(_param)->GetValueString());
if (!_joint) {
gzerr << "joint by name [" << _sdf->GetElement(_param)->GetValueString() << "] not found in model\n";
return false;
}
}
return true;
}
void GazeboRosMultiCamera::Load(sensors::SensorPtr _parent,
sdf::ElementPtr _sdf)
{
MultiCameraPlugin::Load(_parent, _sdf);
// 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;
}
// initialize shared_ptr members
this->image_connect_count_ = boost::shared_ptr<int>(new int(0));
this->image_connect_count_lock_ = boost::shared_ptr<boost::mutex>(new boost::mutex);
this->was_active_ = boost::shared_ptr<bool>(new bool(false));
// copying from CameraPlugin into GazeboRosCameraUtils
for (unsigned i = 0; i < this->camera.size(); ++i)
{
GazeboRosCameraUtils* util = new GazeboRosCameraUtils();
util->parentSensor_ = this->parentSensor;
util->width_ = this->width[i];
util->height_ = this->height[i];
util->depth_ = this->depth[i];
util->format_ = this->format[i];
util->camera_ = this->camera[i];
// Set up a shared connection counter
util->image_connect_count_ = this->image_connect_count_;
util->image_connect_count_lock_ = this->image_connect_count_lock_;
util->was_active_ = this->was_active_;
if (this->camera[i]->GetName().find("left") != std::string::npos)
{
// FIXME: hardcoded, left hack_baseline_ 0
util->Load(_parent, _sdf, "/left", 0.0);
}
else if (this->camera[i]->GetName().find("right") != std::string::npos)
{
double hackBaseline = 0.0;
if (_sdf->HasElement("hackBaseline"))
hackBaseline = _sdf->Get<double>("hackBaseline");
util->Load(_parent, _sdf, "/right", hackBaseline);
}
this->utils.push_back(util);
}
}
