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");
}
////////////////////////////////////////////////////////////////////////////////
// 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);
}
////////////////////////////////////////////////////////////////////////////////
// Load the controller
void GazeboRosLaser::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf)
{
// load plugin
GpuRayPlugin::Load(_parent, this->sdf);
// Get the world name.
std::string worldName = _parent->GetWorldName();
this->world_ = physics::get_world(worldName);
// save pointers
this->sdf = _sdf;
GAZEBO_SENSORS_USING_DYNAMIC_POINTER_CAST;
this->parent_ray_sensor_ =
dynamic_pointer_cast<sensors::GpuRaySensor>(_parent);
if (!this->parent_ray_sensor_)
gzthrow("GazeboRosLaser controller requires a Ray Sensor as its parent");
this->robot_namespace_ = GetRobotNamespace(_parent, _sdf, "Laser");
if (!this->sdf->HasElement("frameName"))
{
ROS_INFO("GazeboRosLaser plugin missing <frameName>, defaults to /world");
this->frame_name_ = "/world";
}
else
this->frame_name_ = this->sdf->Get<std::string>("frameName");
if (!this->sdf->HasElement("topicName"))
{
ROS_INFO("GazeboRosLaser plugin missing <topicName>, defaults to /world");
this->topic_name_ = "/world";
}
else
this->topic_name_ = this->sdf->Get<std::string>("topicName");
this->laser_connect_count_ = 0;
// 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;
}
ROS_INFO ( "Starting GazeboRosLaser Plugin (ns = %s)!", this->robot_namespace_.c_str() );
// ros callback queue for processing subscription
this->deferred_load_thread_ = boost::thread(
boost::bind(&GazeboRosLaser::LoadThread, this));
}
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);
}
GazeboRosTest::GazeboRosTest(Entity *parent)
: Controller(parent),
pgain_(2.0),
dgain_(0.0)
{
this->parent_model_ = dynamic_cast<Model*>(this->parent);
if (!this->parent_model_)
gzthrow("GazeboRosTest controller requires a Model as its parent");
Param::Begin(&this->parameters);
this->robotParamP = new ParamT<std::string>("robotParam", "robot_description", 0);
this->robotNamespaceP = new ParamT<std::string>("robotNamespace", "/", 0);
this->setModelsJointsStatesServiceNameP = new ParamT<std::string>("setModelsJointsStatesServiceName","set_models_joints_states", 0);
Param::End();
}
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);
}
GazeboBattery::GazeboBattery(Entity *parent): Controller(parent)
{
this->parent_model_ = dynamic_cast<Model*>(this->parent);
if (!this->parent_model_)
gzthrow("GazeboBattery controller requires a Model as its parent");
rosnode_ = ros::g_node; // comes from where?
int argc = 0;
char** argv = NULL;
if (rosnode_ == NULL)
{
// this only works for a single camera.
ros::init(argc,argv);
rosnode_ = new ros::node("ros_gazebo",ros::node::DONT_HANDLE_SIGINT);
printf("-------------------- starting node in P3D \n");
}
}
void GazeboMultirotorBasePlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) {
model_ = _model;
world_ = model_->GetWorld();
namespace_.clear();
getSdfParam<std::string>(_sdf, "robotNamespace", namespace_, namespace_, true);
getSdfParam<std::string>(_sdf, "linkName", link_name_, link_name_, true);
getSdfParam<std::string>(_sdf, "motorPubTopic", motor_pub_topic_, motor_pub_topic_);
getSdfParam<double>(_sdf, "rotorVelocitySlowdownSim", rotor_velocity_slowdown_sim_,
rotor_velocity_slowdown_sim_);
node_handle_ = transport::NodePtr(new transport::Node());
node_handle_->Init(namespace_);
motor_pub_ = node_handle_->Advertise<mav_msgs::msgs::MotorSpeed>("~/" + model_->GetName() + motor_pub_topic_, 1);
frame_id_ = link_name_;
link_ = model_->GetLink(link_name_);
if (link_ == NULL)
gzthrow("[gazebo_multirotor_base_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(&GazeboMultirotorBasePlugin::OnUpdate, this, _1));
child_links_ = link_->GetChildJointsLinks();
for (unsigned int i = 0; i < child_links_.size(); i++) {
std::string link_name = child_links_[i]->GetScopedName();
// Check if link contains rotor_ in its name.
int pos = link_name.find("rotor_");
if (pos != link_name.npos) {
std::string motor_number_str = link_name.substr(pos + 6);
unsigned int motor_number = std::stoi(motor_number_str);
std::string joint_name = child_links_[i]->GetName() + "_joint";
physics::JointPtr joint = this->model_->GetJoint(joint_name);
motor_joints_.insert(MotorNumberToJointPair(motor_number, joint));
}
}
}
GazeboMechanismControl::GazeboMechanismControl(Entity *parent)
: Controller(parent), hw_(0), mc_(&hw_), mcn_(&mc_), fake_state_(NULL)
{
this->parent_model_ = dynamic_cast<Model*>(this->parent);
if (!this->parent_model_)
gzthrow("GazeboMechanismControl controller requires a Model as its parent");
rosnode_ = ros::g_node; // comes from where?
int argc = 0;
char** argv = NULL;
if (rosnode_ == NULL)
{
// this only works for a single camera.
ros::init(argc,argv);
rosnode_ = new ros::node("ros_gazebo",ros::node::DONT_HANDLE_SIGINT);
printf("-------------------- starting node in Gazebo Mechanism Control \n");
}
}
////////////////////////////////////////////////////////////////////////////////
// Constructor
GazeboRosIr::GazeboRosIr(Entity *parent)
: Controller(parent)
{
this->myParent = dynamic_cast<IRSensor*>(this->parent);
if (!this->myParent)
gzthrow("GazeboRosIr controller requires a IRSensor as its parent");
Param::Begin(&this->parameters);
this->robotNamespaceP = new ParamT<std::string>("robotNamespace", "/", 0);
this->gaussianNoiseP = new ParamT<double>("gaussianNoise", 0.0, 0);
this->topicNameP = new ParamT<std::string>("topicName", "", 1);
this->deprecatedTopicNameP = new ParamT<std::string>("deprecatedTopicName", "", 0);
this->frameNameP = new ParamT<std::string>("frameName", "default_gazebo_ros_ir_frame", 0);
this->maxRangeP = new ParamT<double>("maxRange", 1.45, 0);
this->minRangeP = new ParamT<double>("minRange", 0.1, 0);
this->fovP = new ParamT<double>("fov", 0.4, 0);
Param::End();
this->irConnectCount = 0;
this->deprecatedIrConnectCount = 0;
}
////////////////////////////////////////////////////////////////////////////////
// Load the controller
void GazeboRosLaser::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf) {
parent_ray_sensor_ = std::dynamic_pointer_cast<sensors::RaySensor> ( _parent );
if ( !parent_ray_sensor_ ) { gzthrow ( "GazeboRosLaser controller requires a Ray Sensor as its parent" ); }
this->parent_ray_sensor_->SetActive ( false );// sensor data modification during a cycle set to false
gazebo_ros_ = GazeboRosPtr ( new GazeboRos ( _parent, _sdf, "TUWLaser" ) );
gazebo_ros_->isInitialized(); // Make sure the ROS node for Gazebo has already been initialized
gazebo_ros_->getParameter<std::string> ( frame_name_ , "frameName", "front_laser" );
gazebo_ros_->getParameter<std::string> ( topic_name_ , "topicName", "front_laser/laser" );
gazebo_ros_->getParameter<double> ( snsRangeMin_, "rangeMin" , 0.2 );
using rosAO = ros::AdvertiseOptions;
rosAO ao = rosAO::create<sensor_msgs::LaserScan> ( topic_name_, 1, std::bind ( &GazeboRosLaser::LaserConnect , this ),
std::bind ( &GazeboRosLaser::LaserDisconnect, this ), ros::VoidPtr(), NULL );
ros_pub_laser_ = gazebo_ros_->node()->advertise ( ao );
pub_multi_queue.startServiceThread();
pub_queue_ = pub_multi_queue.addPub<sensor_msgs::LaserScan>();
laser_connect_count_ = 0;
}
////////////////////////////////////////////////////////////////////////////////
// Load the controller
void GazeboRosBlockLaser::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf)
{
// load plugin
RayPlugin::Load(_parent, _sdf);
// Get then name of the parent sensor
this->parent_sensor_ = _parent;
// Get the world name.
std::string worldName = _parent->GetWorldName();
this->world_ = physics::get_world(worldName);
last_update_time_ = this->world_->GetSimTime();
this->node_ = transport::NodePtr(new transport::Node());
this->node_->Init(worldName);
GAZEBO_SENSORS_USING_DYNAMIC_POINTER_CAST;
this->parent_ray_sensor_ = dynamic_pointer_cast<sensors::RaySensor>(this->parent_sensor_);
if (!this->parent_ray_sensor_)
gzthrow("GazeboRosBlockLaser controller requires a Ray Sensor as its parent");
this->robot_namespace_ = "";
if (_sdf->HasElement("robotNamespace"))
this->robot_namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>() + "/";
if (!_sdf->HasElement("frameName"))
{
ROS_INFO("Block laser plugin missing <frameName>, defaults to /world");
this->frame_name_ = "/world";
}
else
this->frame_name_ = _sdf->GetElement("frameName")->Get<std::string>();
if (!_sdf->HasElement("topicName"))
{
ROS_INFO("Block laser plugin missing <topicName>, defaults to /world");
this->topic_name_ = "/world";
}
else
this->topic_name_ = _sdf->GetElement("topicName")->Get<std::string>();
if (!_sdf->HasElement("gaussianNoise"))
{
ROS_INFO("Block laser plugin missing <gaussianNoise>, defaults to 0.0");
this->gaussian_noise_ = 0;
}
else
this->gaussian_noise_ = _sdf->GetElement("gaussianNoise")->Get<double>();
if (!_sdf->HasElement("hokuyoMinIntensity"))
{
ROS_INFO("Block laser plugin missing <hokuyoMinIntensity>, defaults to 101");
this->hokuyo_min_intensity_ = 101;
}
else
this->hokuyo_min_intensity_ = _sdf->GetElement("hokuyoMinIntensity")->Get<double>();
ROS_INFO("INFO: gazebo_ros_laser plugin should set minimum intensity to %f due to cutoff in hokuyo filters." , this->hokuyo_min_intensity_);
if (!_sdf->GetElement("updateRate"))
{
ROS_INFO("Block laser plugin missing <updateRate>, defaults to 0");
this->update_rate_ = 0;
}
else
this->update_rate_ = _sdf->GetElement("updateRate")->Get<double>();
// FIXME: update the update_rate_
this->laser_connect_count_ = 0;
// 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_);
// resolve tf prefix
std::string prefix;
this->rosnode_->getParam(std::string("tf_prefix"), prefix);
this->frame_name_ = tf::resolve(prefix, this->frame_name_);
// set size of cloud message, starts at 0!! FIXME: not necessary
this->cloud_msg_.points.clear();
this->cloud_msg_.channels.clear();
this->cloud_msg_.channels.push_back(sensor_msgs::ChannelFloat32());
if (this->topic_name_ != "")
{
// Custom Callback Queue
ros::AdvertiseOptions ao = ros::AdvertiseOptions::create<sensor_msgs::PointCloud>(
this->topic_name_,1,
boost::bind( &GazeboRosBlockLaser::LaserConnect,this),
boost::bind( &GazeboRosBlockLaser::LaserDisconnect,this), ros::VoidPtr(), &this->laser_queue_);
this->pub_ = this->rosnode_->advertise(ao);
}
// Initialize the controller
// sensor generation off by default
this->parent_ray_sensor_->SetActive(false);
// start custom queue for laser
this->callback_laser_queue_thread_ = boost::thread( boost::bind( &GazeboRosBlockLaser::LaserQueueThread,this ) );
}
////////////////////////////////////////////////////////////////////////////////
// Load the controller
void GazeboRosSonar::Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sdf)
{
// Get then name of the parent sensor
sensor_ = boost::dynamic_pointer_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);
// default parameters
namespace_.clear();
topic_ = "sonar";
frame_id_ = "/sonar_link";
// load parameters
if (_sdf->HasElement("robotNamespace"))
namespace_ = _sdf->GetElement("robotNamespace")->GetValue()->GetAsString();
if (_sdf->HasElement("frameId"))
frame_id_ = _sdf->GetElement("frameId")->GetValue()->GetAsString();
if (_sdf->HasElement("topicName"))
topic_ = _sdf->GetElement("topicName")->GetValue()->GetAsString();
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()).Radian()), fabs((sensor_->GetVerticalAngleMax() - sensor_->GetVerticalAngleMin()).Radian()));
range_.max_range = sensor_->GetRangeMax();
range_.min_range = sensor_->GetRangeMin();
// 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_);
publisher_ = node_handle_->advertise<sensor_msgs::Range>(topic_, 1);
// setup dynamic_reconfigure server
dynamic_reconfigure_server_.reset(new dynamic_reconfigure::Server<SensorModelConfig>(ros::NodeHandle(*node_handle_, topic_)));
dynamic_reconfigure_server_->setCallback(boost::bind(&SensorModel::dynamicReconfigureCallback, &sensor_model_, _1, _2));
Reset();
// connect Update function
updateTimer.setUpdateRate(10.0);
updateTimer.Load(world, _sdf);
updateConnection = updateTimer.Connect(boost::bind(&GazeboRosSonar::Update, this));
// activate RaySensor
sensor_->SetActive(true);
}
void GazeboWindPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) {
if (kPrintOnPluginLoad) {
gzdbg << __FUNCTION__ << "() called." << std::endl;
}
// Store the pointer to the model.
model_ = _model;
world_ = model_->GetWorld();
double wind_gust_start = kDefaultWindGustStart;
double wind_gust_duration = kDefaultWindGustDuration;
//==============================================//
//========== READ IN PARAMS FROM SDF ===========//
//==============================================//
if (_sdf->HasElement("robotNamespace"))
namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>();
else
gzerr << "[gazebo_wind_plugin] Please specify a robotNamespace.\n";
// Create Gazebo Node.
node_handle_ = gazebo::transport::NodePtr(new transport::Node());
// Initialise with default namespace (typically /gazebo/default/).
node_handle_->Init();
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, "windForcePubTopic", wind_force_pub_topic_,
wind_force_pub_topic_);
getSdfParam<std::string>(_sdf, "windSpeedPubTopic", wind_speed_pub_topic_,
wind_speed_pub_topic_);
getSdfParam<std::string>(_sdf, "frameId", frame_id_, frame_id_);
getSdfParam<std::string>(_sdf, "linkName", link_name_, link_name_);
// Get the wind speed params from SDF.
getSdfParam<double>(_sdf, "windSpeedMean", wind_speed_mean_,
wind_speed_mean_);
getSdfParam<double>(_sdf, "windSpeedVariance", wind_speed_variance_,
wind_speed_variance_);
getSdfParam<math::Vector3>(_sdf, "windDirection", wind_direction_,
wind_direction_);
// Check if a custom static wind field should be used.
getSdfParam<bool>(_sdf, "useCustomStaticWindField", use_custom_static_wind_field_,
use_custom_static_wind_field_);
if (!use_custom_static_wind_field_) {
gzdbg << "[gazebo_wind_plugin] Using user-defined constant wind field and gusts.\n";
// 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_);
// 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);
} else {
gzdbg << "[gazebo_wind_plugin] Using custom wind field from text file.\n";
// Get the wind field text file path, read it and save data.
std::string custom_wind_field_path;
getSdfParam<std::string>(_sdf, "customWindFieldPath", custom_wind_field_path,
custom_wind_field_path);
ReadCustomWindField(custom_wind_field_path);
}
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));
}
////////////////////////////////////////////////////////////////////////////////
// Load the controller
void GazeboRosVelodyneLaser::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf)
{
// load plugin
RayPlugin::Load(_parent, _sdf);
// Get then name of the parent sensor
parent_sensor_ = _parent;
// Get the world name.
#if GAZEBO_MAJOR_VERSION >= 7
std::string worldName = _parent->WorldName();
#else
std::string worldName = _parent->GetWorldName();
#endif
world_ = physics::get_world(worldName);
last_update_time_ = world_->GetSimTime();
node_ = transport::NodePtr(new transport::Node());
node_->Init(worldName);
#if GAZEBO_MAJOR_VERSION >= 7
parent_ray_sensor_ = std::dynamic_pointer_cast<sensors::RaySensor>(parent_sensor_);
#else
parent_ray_sensor_ = boost::dynamic_pointer_cast<sensors::RaySensor>(parent_sensor_);
#endif
if (!parent_ray_sensor_) {
gzthrow("GazeboRosVelodyneLaser controller requires a Ray Sensor as its parent");
}
robot_namespace_ = "";
if (_sdf->HasElement("robotNamespace")) {
robot_namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>() + "/";
}
if (!_sdf->HasElement("frameName")) {
ROS_INFO("Velodyne laser plugin missing <frameName>, defaults to /world");
frame_name_ = "/world";
} else {
frame_name_ = _sdf->GetElement("frameName")->Get<std::string>();
}
if (!_sdf->HasElement("min_range")) {
ROS_INFO("Velodyne laser plugin missing <min_range>, defaults to 0");
min_range_ = 0;
} else {
min_range_ = _sdf->GetElement("min_range")->Get<double>();
}
if (!_sdf->HasElement("max_range")) {
ROS_INFO("Velodyne laser plugin missing <max_range>, defaults to infinity");
max_range_ = INFINITY;
} else {
max_range_ = _sdf->GetElement("max_range")->Get<double>();
}
if (!_sdf->HasElement("topicName")) {
ROS_INFO("Velodyne laser plugin missing <topicName>, defaults to /world");
topic_name_ = "/world";
} else {
topic_name_ = _sdf->GetElement("topicName")->Get<std::string>();
}
if (!_sdf->HasElement("gaussianNoise")) {
ROS_INFO("Velodyne laser plugin missing <gaussianNoise>, defaults to 0.0");
gaussian_noise_ = 0;
} else {
gaussian_noise_ = _sdf->GetElement("gaussianNoise")->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;
}
rosnode_ = new ros::NodeHandle(robot_namespace_);
// resolve tf prefix
std::string prefix;
rosnode_->getParam(std::string("tf_prefix"), prefix);
frame_name_ = tf::resolve(prefix, frame_name_);
if (topic_name_ != "") {
// Custom Callback Queue
ros::AdvertiseOptions ao = ros::AdvertiseOptions::create<sensor_msgs::PointCloud2>(
topic_name_,1,
boost::bind( &GazeboRosVelodyneLaser::laserConnect,this),
boost::bind( &GazeboRosVelodyneLaser::laserDisconnect,this), ros::VoidPtr(), &laser_queue_);
pub_ = rosnode_->advertise(ao);
}
// sensor generation off by default
parent_ray_sensor_->SetActive(false);
// start custom queue for laser
callback_laser_queue_thread_ = boost::thread( boost::bind( &GazeboRosVelodyneLaser::laserQueueThread,this ) );
#if GAZEBO_MAJOR_VERSION >= 7
ROS_INFO("Velodyne laser plugin ready, %i lasers", parent_ray_sensor_->VerticalRangeCount());
#else
ROS_INFO("Velodyne laser plugin ready, %i lasers", parent_ray_sensor_->GetVerticalRangeCount());
#endif
}
void GazeboImuPlugin::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("robotNamespace"))
namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>();
else
gzerr << "[gazebo_imu_plugin] Please specify a robotNamespace.\n";
node_handle_ = 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";
// Get the pointer to the link
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, "imuTopic", imu_topic_, kDefaultImuTopic);
getSdfParam<double>(_sdf, "gyroscopeNoiseDensity",
imu_parameters_.gyroscope_noise_density,
imu_parameters_.gyroscope_noise_density);
getSdfParam<double>(_sdf, "gyroscopeBiasRandomWalk",
imu_parameters_.gyroscope_random_walk,
imu_parameters_.gyroscope_random_walk);
getSdfParam<double>(_sdf, "gyroscopeBiasCorrelationTime",
imu_parameters_.gyroscope_bias_correlation_time,
imu_parameters_.gyroscope_bias_correlation_time);
assert(imu_parameters_.gyroscope_bias_correlation_time > 0.0);
getSdfParam<double>(_sdf, "gyroscopeTurnOnBiasSigma",
imu_parameters_.gyroscope_turn_on_bias_sigma,
imu_parameters_.gyroscope_turn_on_bias_sigma);
getSdfParam<double>(_sdf, "accelerometerNoiseDensity",
imu_parameters_.accelerometer_noise_density,
imu_parameters_.accelerometer_noise_density);
getSdfParam<double>(_sdf, "accelerometerRandomWalk",
imu_parameters_.accelerometer_random_walk,
imu_parameters_.accelerometer_random_walk);
getSdfParam<double>(_sdf, "accelerometerBiasCorrelationTime",
imu_parameters_.accelerometer_bias_correlation_time,
imu_parameters_.accelerometer_bias_correlation_time);
assert(imu_parameters_.accelerometer_bias_correlation_time > 0.0);
getSdfParam<double>(_sdf, "accelerometerTurnOnBiasSigma",
imu_parameters_.accelerometer_turn_on_bias_sigma,
imu_parameters_.accelerometer_turn_on_bias_sigma);
last_time_ = world_->GetSimTime();
// Listen to the update event. This event is broadcast every
// simulation iteration.
this->updateConnection_ =
event::Events::ConnectWorldUpdateBegin(
boost::bind(&GazeboImuPlugin::OnUpdate, this, _1));
imu_pub_ = node_handle_->advertise<sensor_msgs::Imu>(imu_topic_, 10);
// Fill imu message.
imu_message_.header.frame_id = frame_id_;
// We assume uncorrelated noise on the 3 channels -> only set diagonal
// elements. Only the broadband noise component is considered, specified as a
// continuous-time density (two-sided spectrum); not the true covariance of
// the measurements.
// Angular velocity measurement covariance.
imu_message_.angular_velocity_covariance[0] =
imu_parameters_.gyroscope_noise_density *
imu_parameters_.gyroscope_noise_density;
imu_message_.angular_velocity_covariance[4] =
imu_parameters_.gyroscope_noise_density *
imu_parameters_.gyroscope_noise_density;
imu_message_.angular_velocity_covariance[8] =
imu_parameters_.gyroscope_noise_density *
imu_parameters_.gyroscope_noise_density;
// Linear acceleration measurement covariance.
imu_message_.linear_acceleration_covariance[0] =
imu_parameters_.accelerometer_noise_density *
imu_parameters_.accelerometer_noise_density;
imu_message_.linear_acceleration_covariance[4] =
imu_parameters_.accelerometer_noise_density *
imu_parameters_.accelerometer_noise_density;
imu_message_.linear_acceleration_covariance[8] =
imu_parameters_.accelerometer_noise_density *
imu_parameters_.accelerometer_noise_density;
// Orientation estimate covariance (no estimate provided).
imu_message_.orientation_covariance[0] = -1.0;
gravity_W_ = world_->GetPhysicsEngine()->GetGravity();
imu_parameters_.gravity_magnitude = gravity_W_.GetLength();
standard_normal_distribution_ = std::normal_distribution<double>(0.0, 1.0);
double sigma_bon_g = imu_parameters_.gyroscope_turn_on_bias_sigma;
double sigma_bon_a = imu_parameters_.accelerometer_turn_on_bias_sigma;
for (int i = 0; i < 3; ++i) {
gyroscope_turn_on_bias_[i] =
sigma_bon_g * standard_normal_distribution_(random_generator_);
accelerometer_turn_on_bias_[i] =
sigma_bon_a * standard_normal_distribution_(random_generator_);
}
// TODO(nikolicj) incorporate steady-state covariance of bias process
gyroscope_bias_.setZero();
accelerometer_bias_.setZero();
}
// Load the controller
void ServoPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
{
// Get the world name.
world = _model->GetWorld();
// default parameters
topicName = "drive";
jointStateName = "joint_states";
robotNamespace.clear();
controlPeriod = 0;
proportionalControllerGain = 8.0;
derivativeControllerGain = 0.0;
maximumVelocity = 0.0;
maximumTorque = 0.0;
// load parameters
if (_sdf->HasElement("robotNamespace")) robotNamespace = _sdf->Get<std::string>("robotNamespace");
if (_sdf->HasElement("topicName")) topicName = _sdf->Get<std::string>("topicName");
if (_sdf->HasElement("jointStateName")) jointStateName = _sdf->Get<std::string>("jointStateName");
if (_sdf->HasElement("firstServoName")) servo[FIRST].name = _sdf->Get<std::string>("firstServoName");
if (_sdf->HasElement("firstServoAxis")) servo[FIRST].axis = _sdf->Get<math::Vector3>("firstServoAxis");
if (_sdf->HasElement("secondServoName")) servo[SECOND].name = _sdf->Get<std::string>("secondServoName");
if (_sdf->HasElement("secondServoAxis")) servo[SECOND].axis = _sdf->Get<math::Vector3>("secondServoAxis");
if (_sdf->HasElement("thirdServoName")) servo[THIRD].name = _sdf->Get<std::string>("thirdServoName");
if (_sdf->HasElement("thirdServoAxis")) servo[THIRD].axis = _sdf->Get<math::Vector3>("thirdServoAxis");
if (_sdf->HasElement("proportionalControllerGain")) proportionalControllerGain = _sdf->Get<double>("proportionalControllerGain");
if (_sdf->HasElement("derivativeControllerGain")) derivativeControllerGain = _sdf->Get<double>("derivativeControllerGain");
if (_sdf->HasElement("maxVelocity")) maximumVelocity = _sdf->Get<double>("maxVelocity");
if (_sdf->HasElement("torque")) maximumTorque = _sdf->Get<double>("torque");
double controlRate = 0.0;
if (_sdf->HasElement("controlRate")) controlRate = _sdf->Get<double>("controlRate");
controlPeriod = controlRate > 0.0 ? 1.0/controlRate : 0.0;
servo[FIRST].joint = _model->GetJoint(servo[FIRST].name);
servo[SECOND].joint = _model->GetJoint(servo[SECOND].name);
servo[THIRD].joint = _model->GetJoint(servo[THIRD].name);
if (!servo[FIRST].joint)
gzthrow("The controller couldn't get first joint");
countOfServos = 1;
if (servo[SECOND].joint) {
countOfServos = 2;
if (servo[THIRD].joint) {
countOfServos = 3;
}
}
else {
if (servo[THIRD].joint) {
gzthrow("The controller couldn't get second joint, but third joint was loaded");
}
}
if (!ros::isInitialized()) {
int argc = 0;
char** argv = NULL;
ros::init(argc, argv, "gazebo", ros::init_options::NoSigintHandler | ros::init_options::AnonymousName);
}
rosnode_ = new ros::NodeHandle(robotNamespace);
transform_listener_ = new tf::TransformListener();
transform_listener_->setExtrapolationLimit(ros::Duration(1.0));
if (!topicName.empty()) {
ros::SubscribeOptions so = ros::SubscribeOptions::create<geometry_msgs::QuaternionStamped>(topicName, 1,
boost::bind(&ServoPlugin::cmdCallback, this, _1),
ros::VoidPtr(), &queue_);
sub_ = rosnode_->subscribe(so);
}
if (!jointStateName.empty()) {
jointStatePub_ = rosnode_->advertise<sensor_msgs::JointState>(jointStateName, 10);
}
joint_state.header.frame_id = transform_listener_->resolve(_model->GetLink()->GetName());
// 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(&ServoPlugin::Update, this));
}
void GazeboMotorModel::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) {
model_ = _model;
namespace_.clear();
if (_sdf->HasElement("robotNamespace"))
namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>();
else
gzerr << "[gazebo_motor_model] Please specify a robotNamespace.\n";
node_handle_ = new ros::NodeHandle(namespace_);
if (_sdf->HasElement("jointName"))
joint_name_ = _sdf->GetElement("jointName")->Get<std::string>();
else
gzerr << "[gazebo_motor_model] Please specify a jointName, where the rotor is attached.\n";
// Get the pointer to the joint.
joint_ = model_->GetJoint(joint_name_);
if (joint_ == NULL)
gzthrow("[gazebo_motor_model] Couldn't find specified joint \"" << joint_name_ << "\".");
if (_sdf->HasElement("linkName"))
link_name_ = _sdf->GetElement("linkName")->Get<std::string>();
else
gzerr << "[gazebo_motor_model] Please specify a linkName of the rotor.\n";
link_ = model_->GetLink(link_name_);
if (link_ == NULL)
gzthrow("[gazebo_motor_model] Couldn't find specified link \"" << link_name_ << "\".");
if (_sdf->HasElement("motorNumber"))
motor_number_ = _sdf->GetElement("motorNumber")->Get<int>();
else
gzerr << "[gazebo_motor_model] Please specify a motorNumber.\n";
if (_sdf->HasElement("turningDirection")) {
std::string turning_direction = _sdf->GetElement("turningDirection")->Get<std::string>();
if (turning_direction == "cw")
turning_direction_ = turning_direction::CW;
else if (turning_direction == "ccw")
turning_direction_ = turning_direction::CCW;
else
gzerr << "[gazebo_motor_model] Please only use 'cw' or 'ccw' as turningDirection.\n";
}
else
gzerr << "[gazebo_motor_model] Please specify a turning direction ('cw' or 'ccw').\n";
getSdfParam<std::string>(_sdf, "commandSubTopic", command_sub_topic_, command_sub_topic_);
getSdfParam<std::string>(_sdf, "motorSpeedPubTopic", motor_speed_pub_topic_,
motor_speed_pub_topic_);
getSdfParam<double>(_sdf, "rotorDragCoefficient", rotor_drag_coefficient_, rotor_drag_coefficient_);
getSdfParam<double>(_sdf, "rollingMomentCoefficient", rolling_moment_coefficient_,
rolling_moment_coefficient_);
getSdfParam<double>(_sdf, "maxRotVelocity", max_rot_velocity_, max_rot_velocity_);
getSdfParam<double>(_sdf, "motorConstant", motor_constant_, motor_constant_);
getSdfParam<double>(_sdf, "momentConstant", moment_constant_, moment_constant_);
getSdfParam<double>(_sdf, "timeConstantUp", time_constant_up_, time_constant_up_);
getSdfParam<double>(_sdf, "timeConstantDown", time_constant_down_, time_constant_down_);
getSdfParam<double>(_sdf, "rotorVelocitySlowdownSim", rotor_velocity_slowdown_sim_, 10);
// Set the maximumForce on the joint. This is deprecated from V5 on, and the joint won't move.
#if GAZEBO_MAJOR_VERSION < 5
joint_->SetMaxForce(0, max_force_);
#endif
// Listen to the update event. This event is broadcast every
// simulation iteration.
updateConnection_ = event::Events::ConnectWorldUpdateBegin(boost::bind(&GazeboMotorModel::OnUpdate, this, _1));
command_sub_ = node_handle_->subscribe(command_sub_topic_, 1000, &GazeboMotorModel::VelocityCallback, this);
motor_velocity_pub_ = node_handle_->advertise<std_msgs::Float32>(motor_speed_pub_topic_, 10);
// Create the first order filter.
rotor_velocity_filter_.reset(new FirstOrderFilter<double>(time_constant_up_, time_constant_down_, ref_motor_rot_vel_));
}
void GazeboFwDynamicsPlugin::Load(physics::ModelPtr _model,
sdf::ElementPtr _sdf) {
if (kPrintOnPluginLoad) {
gzdbg << __FUNCTION__ << "() called." << std::endl;
}
gzdbg << "_model = " << _model->GetName() << std::endl;
// Store the pointer to the model.
model_ = _model;
world_ = model_->GetWorld();
namespace_.clear();
// Get the robot namespace.
if (_sdf->HasElement("robotNamespace"))
namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>();
else
gzerr << "[gazebo_fw_dynamics_plugin] Please specify a robotNamespace.\n";
// Create the node handle.
node_handle_ = transport::NodePtr(new transport::Node());
// Initialise with default namespace (typically /gazebo/default/).
node_handle_->Init();
// Get the link name.
std::string link_name;
if (_sdf->HasElement("linkName"))
link_name = _sdf->GetElement("linkName")->Get<std::string>();
else
gzerr << "[gazebo_fw_dynamics_plugin] Please specify a linkName.\n";
// Get the pointer to the link.
link_ = model_->GetLink(link_name);
if (link_ == NULL) {
gzthrow("[gazebo_fw_dynamics_plugin] Couldn't find specified link \""
<< link_name << "\".");
}
// Get the path to fixed-wing aerodynamics parameters YAML file. If not
// provided, default Techpod parameters are used.
if (_sdf->HasElement("aeroParamsYAML")) {
std::string aero_params_yaml =
_sdf->GetElement("aeroParamsYAML")->Get<std::string>();
aero_params_.LoadAeroParamsYAML(aero_params_yaml);
} else {
gzwarn << "[gazebo_fw_dynamics_plugin] No aerodynamic paramaters YAML file"
<< " specified, using default Techpod parameters.\n";
}
// Get the path to fixed-wing vehicle parameters YAML file. If not provided,
// default Techpod parameters are used.
if (_sdf->HasElement("vehicleParamsYAML")) {
std::string vehicle_params_yaml =
_sdf->GetElement("vehicleParamsYAML")->Get<std::string>();
vehicle_params_.LoadVehicleParamsYAML(vehicle_params_yaml);
} else {
gzwarn << "[gazebo_fw_dynamics_plugin] No vehicle paramaters YAML file"
<< " specified, using default Techpod parameters.\n";
}
// Get the rest of the sdf parameters.
getSdfParam<bool>(_sdf, "isInputJoystick", is_input_joystick_,
kDefaultIsInputJoystick);
getSdfParam<std::string>(_sdf, "actuatorsSubTopic",
actuators_sub_topic_,
mav_msgs::default_topics::COMMAND_ACTUATORS);
getSdfParam<std::string>(_sdf, "rollPitchYawrateThrustSubTopic",
roll_pitch_yawrate_thrust_sub_topic_,
mav_msgs::default_topics::
COMMAND_ROLL_PITCH_YAWRATE_THRUST);
getSdfParam<std::string>(_sdf, "windSpeedSubTopic",
wind_speed_sub_topic_,
mav_msgs::default_topics::WIND_SPEED);
// Listen to the update event. This event is broadcast every
// simulation iteration.
this->updateConnection_ = event::Events::ConnectWorldUpdateBegin(
boost::bind(&GazeboFwDynamicsPlugin::OnUpdate, this, _1));
}
void GazeboPressurePlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) {
if (kPrintOnPluginLoad) {
gzdbg << __FUNCTION__ << "() called." << std::endl;
}
gzdbg << "_model = " << _model->GetName() << std::endl;
// Store the pointer to the model and the world.
model_ = _model;
world_ = model_->GetWorld();
//==============================================//
//========== READ IN PARAMS FROM SDF ===========//
//==============================================//
// Use the robot namespace to create the node handle.
if (_sdf->HasElement("robotNamespace"))
namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>();
else
gzerr << "[gazebo_pressure_plugin] Please specify a robotNamespace.\n";
// Get node handle.
node_handle_ = transport::NodePtr(new transport::Node());
// Initialise with default namespace (typically /gazebo/default/).
node_handle_->Init();
std::string link_name;
if (_sdf->HasElement("linkName"))
link_name = _sdf->GetElement("linkName")->Get<std::string>();
else
gzerr << "[gazebo_pressure_plugin] Please specify a linkName.\n";
// Get the pointer to the link.
link_ = model_->GetLink(link_name);
if (link_ == NULL)
gzthrow("[gazebo_pressure_plugin] Couldn't find specified link \"" << link_name << "\".");
frame_id_ = link_name;
// Retrieve the rest of the SDF parameters.
getSdfParam<std::string>(_sdf, "pressureTopic", pressure_topic_, kDefaultPressurePubTopic);
getSdfParam<double>(_sdf, "referenceAltitude", ref_alt_, kDefaultRefAlt);
getSdfParam<double>(_sdf, "pressureVariance", pressure_var_, kDefaultPressureVar);
CHECK(pressure_var_ >= 0.0);
// Initialize the normal distribution for pressure.
double mean = 0.0;
pressure_n_[0] = NormalDistribution(mean, sqrt(pressure_var_));
// Listen to the update event. This event is broadcast every simulation
// iteration.
this->updateConnection_ = event::Events::ConnectWorldUpdateBegin(
boost::bind(&GazeboPressurePlugin::OnUpdate, this, _1));
//==============================================//
//=== POPULATE STATIC PARTS OF PRESSURE MSG ====//
//==============================================//
pressure_message_.mutable_header()->set_frame_id(frame_id_);
pressure_message_.set_variance(pressure_var_);
}
void GazeboOdometryPlugin::Load(physics::ModelPtr _model,
sdf::ElementPtr _sdf) {
if (kPrintOnPluginLoad) {
gzdbg << __FUNCTION__ << "() called." << std::endl;
}
// Store the pointer to the model
model_ = _model;
world_ = model_->GetWorld();
SdfVector3 noise_normal_position;
SdfVector3 noise_normal_quaternion;
SdfVector3 noise_normal_linear_velocity;
SdfVector3 noise_normal_angular_velocity;
SdfVector3 noise_uniform_position;
SdfVector3 noise_uniform_quaternion;
SdfVector3 noise_uniform_linear_velocity;
SdfVector3 noise_uniform_angular_velocity;
const SdfVector3 zeros3(0.0, 0.0, 0.0);
odometry_queue_.clear();
if (_sdf->HasElement("robotNamespace"))
namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>();
else
gzerr << "[gazebo_odometry_plugin] Please specify a robotNamespace.\n";
node_handle_ = gazebo::transport::NodePtr(new transport::Node());
// Initialise with default namespace (typically /gazebo/default/)
node_handle_->Init();
if (_sdf->HasElement("linkName"))
link_name_ = _sdf->GetElement("linkName")->Get<std::string>();
else
gzerr << "[gazebo_odometry_plugin] Please specify a linkName.\n";
link_ = model_->GetLink(link_name_);
if (link_ == NULL)
gzthrow("[gazebo_odometry_plugin] Couldn't find specified link \""
<< link_name_ << "\".");
if (_sdf->HasElement("covarianceImage")) {
std::string image_name =
_sdf->GetElement("covarianceImage")->Get<std::string>();
covariance_image_ = cv::imread(image_name, CV_LOAD_IMAGE_GRAYSCALE);
if (covariance_image_.data == NULL)
gzerr << "loading covariance image " << image_name << " failed"
<< std::endl;
else
gzlog << "loading covariance image " << image_name << " successful"
<< std::endl;
}
if (_sdf->HasElement("randomEngineSeed")) {
random_generator_.seed(
_sdf->GetElement("randomEngineSeed")->Get<unsigned int>());
} else {
random_generator_.seed(
std::chrono::system_clock::now().time_since_epoch().count());
}
getSdfParam<std::string>(_sdf, "poseTopic", pose_pub_topic_, pose_pub_topic_);
getSdfParam<std::string>(_sdf, "poseWithCovarianceTopic",
pose_with_covariance_stamped_pub_topic_,
pose_with_covariance_stamped_pub_topic_);
getSdfParam<std::string>(_sdf, "positionTopic", position_stamped_pub_topic_,
position_stamped_pub_topic_);
getSdfParam<std::string>(_sdf, "transformTopic", transform_stamped_pub_topic_,
transform_stamped_pub_topic_);
getSdfParam<std::string>(_sdf, "odometryTopic", odometry_pub_topic_,
odometry_pub_topic_);
getSdfParam<std::string>(_sdf, "parentFrameId", parent_frame_id_,
parent_frame_id_);
getSdfParam<std::string>(_sdf, "childFrameId", child_frame_id_,
child_frame_id_);
getSdfParam<SdfVector3>(_sdf, "noiseNormalPosition", noise_normal_position,
zeros3);
getSdfParam<SdfVector3>(_sdf, "noiseNormalQuaternion",
noise_normal_quaternion, zeros3);
getSdfParam<SdfVector3>(_sdf, "noiseNormalLinearVelocity",
noise_normal_linear_velocity, zeros3);
getSdfParam<SdfVector3>(_sdf, "noiseNormalAngularVelocity",
noise_normal_angular_velocity, zeros3);
getSdfParam<SdfVector3>(_sdf, "noiseUniformPosition", noise_uniform_position,
zeros3);
getSdfParam<SdfVector3>(_sdf, "noiseUniformQuaternion",
noise_uniform_quaternion, zeros3);
getSdfParam<SdfVector3>(_sdf, "noiseUniformLinearVelocity",
noise_uniform_linear_velocity, zeros3);
getSdfParam<SdfVector3>(_sdf, "noiseUniformAngularVelocity",
noise_uniform_angular_velocity, zeros3);
getSdfParam<int>(_sdf, "measurementDelay", measurement_delay_,
measurement_delay_);
getSdfParam<int>(_sdf, "measurementDivisor", measurement_divisor_,
measurement_divisor_);
getSdfParam<double>(_sdf, "unknownDelay", unknown_delay_, unknown_delay_);
getSdfParam<double>(_sdf, "covarianceImageScale", covariance_image_scale_,
covariance_image_scale_);
parent_link_ = world_->GetEntity(parent_frame_id_);
if (parent_link_ == NULL && parent_frame_id_ != kDefaultParentFrameId) {
gzthrow("[gazebo_odometry_plugin] Couldn't find specified parent link \""
<< parent_frame_id_ << "\".");
}
position_n_[0] = NormalDistribution(0, noise_normal_position.X());
position_n_[1] = NormalDistribution(0, noise_normal_position.Y());
position_n_[2] = NormalDistribution(0, noise_normal_position.Z());
attitude_n_[0] = NormalDistribution(0, noise_normal_quaternion.X());
attitude_n_[1] = NormalDistribution(0, noise_normal_quaternion.Y());
attitude_n_[2] = NormalDistribution(0, noise_normal_quaternion.Z());
linear_velocity_n_[0] =
NormalDistribution(0, noise_normal_linear_velocity.X());
linear_velocity_n_[1] =
NormalDistribution(0, noise_normal_linear_velocity.Y());
linear_velocity_n_[2] =
NormalDistribution(0, noise_normal_linear_velocity.Z());
angular_velocity_n_[0] =
NormalDistribution(0, noise_normal_angular_velocity.X());
angular_velocity_n_[1] =
NormalDistribution(0, noise_normal_angular_velocity.Y());
angular_velocity_n_[2] =
NormalDistribution(0, noise_normal_angular_velocity.Z());
position_u_[0] = UniformDistribution(-noise_uniform_position.X(),
noise_uniform_position.X());
position_u_[1] = UniformDistribution(-noise_uniform_position.Y(),
noise_uniform_position.Y());
position_u_[2] = UniformDistribution(-noise_uniform_position.Z(),
noise_uniform_position.Z());
attitude_u_[0] = UniformDistribution(-noise_uniform_quaternion.X(),
noise_uniform_quaternion.X());
attitude_u_[1] = UniformDistribution(-noise_uniform_quaternion.Y(),
noise_uniform_quaternion.Y());
attitude_u_[2] = UniformDistribution(-noise_uniform_quaternion.Z(),
noise_uniform_quaternion.Z());
linear_velocity_u_[0] = UniformDistribution(
-noise_uniform_linear_velocity.X(), noise_uniform_linear_velocity.X());
linear_velocity_u_[1] = UniformDistribution(
-noise_uniform_linear_velocity.Y(), noise_uniform_linear_velocity.Y());
linear_velocity_u_[2] = UniformDistribution(
-noise_uniform_linear_velocity.Z(), noise_uniform_linear_velocity.Z());
angular_velocity_u_[0] = UniformDistribution(
-noise_uniform_angular_velocity.X(), noise_uniform_angular_velocity.X());
angular_velocity_u_[1] = UniformDistribution(
-noise_uniform_angular_velocity.Y(), noise_uniform_angular_velocity.Y());
angular_velocity_u_[2] = UniformDistribution(
-noise_uniform_angular_velocity.Z(), noise_uniform_angular_velocity.Z());
// Fill in covariance. We omit uniform noise here.
Eigen::Map<Eigen::Matrix<double, 6, 6> > pose_covariance(
pose_covariance_matrix_.data());
Eigen::Matrix<double, 6, 1> pose_covd;
pose_covd << noise_normal_position.X() * noise_normal_position.X(),
noise_normal_position.Y() * noise_normal_position.Y(),
noise_normal_position.Z() * noise_normal_position.Z(),
noise_normal_quaternion.X() * noise_normal_quaternion.X(),
noise_normal_quaternion.Y() * noise_normal_quaternion.Y(),
noise_normal_quaternion.Z() * noise_normal_quaternion.Z();
pose_covariance = pose_covd.asDiagonal();
// Fill in covariance. We omit uniform noise here.
Eigen::Map<Eigen::Matrix<double, 6, 6> > twist_covariance(
twist_covariance_matrix_.data());
Eigen::Matrix<double, 6, 1> twist_covd;
twist_covd << noise_normal_linear_velocity.X() *
noise_normal_linear_velocity.X(),
noise_normal_linear_velocity.Y() * noise_normal_linear_velocity.Y(),
noise_normal_linear_velocity.Z() * noise_normal_linear_velocity.Z(),
noise_normal_angular_velocity.X() * noise_normal_angular_velocity.X(),
noise_normal_angular_velocity.Y() * noise_normal_angular_velocity.Y(),
noise_normal_angular_velocity.Z() * noise_normal_angular_velocity.Z();
twist_covariance = twist_covd.asDiagonal();
// Listen to the update event. This event is broadcast every
// simulation iteration.
updateConnection_ = event::Events::ConnectWorldUpdateBegin(
boost::bind(&GazeboOdometryPlugin::OnUpdate, this, _1));
}
////////////////////////////////////////////////////////////////////////////////
// Load the controller
void GazeboRosRange::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf)
{
// load plugin
RayPlugin::Load(_parent, this->sdf);
// Get then name of the parent sensor
this->parent_sensor_ = _parent;
// Get the world name.
# if GAZEBO_MAJOR_VERSION >= 7
std::string worldName = _parent->WorldName();
# else
std::string worldName = _parent->GetWorldName();
# endif
this->world_ = physics::get_world(worldName);
// save pointers
this->sdf = _sdf;
this->last_update_time_ = common::Time(0);
GAZEBO_SENSORS_USING_DYNAMIC_POINTER_CAST;
this->parent_ray_sensor_ =
dynamic_pointer_cast<sensors::RaySensor>(this->parent_sensor_);
if (!this->parent_ray_sensor_)
gzthrow("GazeboRosRange controller requires a Ray Sensor as its parent");
this->robot_namespace_ = "";
if (this->sdf->HasElement("robotNamespace"))
this->robot_namespace_ = this->sdf->Get<std::string>("robotNamespace") + "/";
if (!this->sdf->HasElement("frameName"))
{
ROS_INFO("Range plugin missing <frameName>, defaults to /world");
this->frame_name_ = "/world";
}
else
this->frame_name_ = this->sdf->Get<std::string>("frameName");
if (!this->sdf->HasElement("topicName"))
{
ROS_INFO("Range plugin missing <topicName>, defaults to /range");
this->topic_name_ = "/range";
}
else
this->topic_name_ = this->sdf->Get<std::string>("topicName");
if (!this->sdf->HasElement("radiation"))
{
ROS_WARN("Range plugin missing <radiation>, defaults to ultrasound");
this->radiation_ = "ultrasound";
}
else
this->radiation_ = _sdf->GetElement("radiation")->Get<std::string>();
if (!this->sdf->HasElement("fov"))
{
ROS_WARN("Range plugin missing <fov>, defaults to 0.05");
this->fov_ = 0.05;
}
else
this->fov_ = _sdf->GetElement("fov")->Get<double>();
if (!this->sdf->HasElement("gaussianNoise"))
{
ROS_INFO("Range plugin missing <gaussianNoise>, defaults to 0.0");
this->gaussian_noise_ = 0;
}
else
this->gaussian_noise_ = this->sdf->Get<double>("gaussianNoise");
if (!this->sdf->HasElement("updateRate"))
{
ROS_INFO("Range plugin missing <updateRate>, defaults to 0");
this->update_rate_ = 0;
}
else
this->update_rate_ = this->sdf->Get<double>("updateRate");
// prepare to throttle this plugin at the same rate
// ideally, we should invoke a plugin update when the sensor updates,
// have to think about how to do that properly later
if (this->update_rate_ > 0.0)
this->update_period_ = 1.0/this->update_rate_;
else
this->update_period_ = 0.0;
this->range_connect_count_ = 0;
this->range_msg_.header.frame_id = this->frame_name_;
if (this->radiation_==std::string("ultrasound"))
this->range_msg_.radiation_type = sensor_msgs::Range::ULTRASOUND;
else
this->range_msg_.radiation_type = sensor_msgs::Range::INFRARED;
this->range_msg_.field_of_view = fov_;
# if GAZEBO_MAJOR_VERSION >= 7
this->range_msg_.max_range = this->parent_ray_sensor_->RangeMax();
this->range_msg_.min_range = this->parent_ray_sensor_->RangeMin();
# else
this->range_msg_.max_range = this->parent_ray_sensor_->GetRangeMax();
this->range_msg_.min_range = this->parent_ray_sensor_->GetRangeMin();
# endif
// Init ROS
if (ros::isInitialized())
{
// ros callback queue for processing subscription
this->deferred_load_thread_ = boost::thread(
boost::bind(&GazeboRosRange::LoadThread, this));
}
else
{
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";
}
}
void ServoPlugin::CalculateVelocities()
{
tf::StampedTransform transform;
boost::mutex::scoped_lock lock(mutex);
if(!current_cmd){
geometry_msgs::QuaternionStamped *default_cmd = new geometry_msgs::QuaternionStamped;
default_cmd->header.frame_id = "base_stabilized";
default_cmd->quaternion.w = 1;
current_cmd.reset(default_cmd);
}
try{
// ros::Time simTime(world->GetSimTime().sec, world->GetSimTime().nsec);
transform_listener_->lookupTransform("base_link", current_cmd->header.frame_id, ros::Time(0), transform);
}
catch (tf::TransformException ex){
ROS_DEBUG("%s",ex.what());
servo[FIRST].velocity = 0.0;
servo[SECOND].velocity = 0.0;
servo[THIRD].velocity = 0.0;
return;
}
rotation_.Set(current_cmd->quaternion.w, current_cmd->quaternion.x, current_cmd->quaternion.y, current_cmd->quaternion.z);
math::Quaternion quat(transform.getRotation().getW(),transform.getRotation().getX(),transform.getRotation().getY(),transform.getRotation().getZ());
rotation_ = quat * rotation_;
double temp[5];
double desAngle[3];
double actualAngle[3] = {0.0, 0.0, 0.0};
double actualVel[3] = {0.0, 0.0, 0.0};
//TODO use countOfServos for calculation
rotationConv = 99;
orderOfAxes[0] = 99;
orderOfAxes[1] = 99;
orderOfAxes[2] = 99;
switch(countOfServos) {
case 2:
if ((servo[FIRST].axis.z == 1) && (servo[SECOND].axis.y == 1)) {
rotationConv = zyx;
orderOfAxes[0] = 0;
orderOfAxes[1] = 1;
}
else {
if ((servo[FIRST].axis.x == 1) && (servo[SECOND].axis.y == 1)) {
rotationConv = xyz;
orderOfAxes[0] = 0;
orderOfAxes[1] = 1;
}
}
break;
case 3:
if ((servo[FIRST].axis.z == 1) && (servo[SECOND].axis.y == 1) && (servo[THIRD].axis.x == 1)) {
rotationConv = zyx;
orderOfAxes[0] = 0;
orderOfAxes[1] = 1;
orderOfAxes[2] = 2;
}
else if ((servo[FIRST].axis.x == 1) && (servo[SECOND].axis.y == 1) && (servo[THIRD].axis.z == 1)) {
rotationConv = xyz;
orderOfAxes[0] = 0;
orderOfAxes[1] = 1;
orderOfAxes[2] = 2;
}
break;
case 1:
if (servo[FIRST].axis.y == 1) {
rotationConv = xyz;
orderOfAxes[0] = 1;
}
break;
default:
gzthrow("Something went wrong. The count of servos is greater than 3");
break;
}
switch(rotationConv) {
case zyx:
temp[0] = 2*(rotation_.x*rotation_.y + rotation_.w*rotation_.z);
temp[1] = rotation_.w*rotation_.w + rotation_.x*rotation_.x - rotation_.y*rotation_.y - rotation_.z*rotation_.z;
temp[2] = -2*(rotation_.x*rotation_.z - rotation_.w*rotation_.y);
temp[3] = 2*(rotation_.y*rotation_.z + rotation_.w*rotation_.x);
temp[4] = rotation_.w*rotation_.w - rotation_.x*rotation_.x - rotation_.y*rotation_.y + rotation_.z*rotation_.z;
break;
case xyz:
temp[0] = -2*(rotation_.y*rotation_.z - rotation_.w*rotation_.x);
temp[1] = rotation_.w*rotation_.w - rotation_.x*rotation_.x - rotation_.y*rotation_.y + rotation_.z*rotation_.z;
temp[2] = 2*(rotation_.x*rotation_.z + rotation_.w*rotation_.y);
temp[3] = -2*(rotation_.x*rotation_.y - rotation_.w*rotation_.z);
temp[4] = rotation_.w*rotation_.w + rotation_.x*rotation_.x - rotation_.y*rotation_.y - rotation_.z*rotation_.z;
break;
default:
gzthrow("joint order " << rotationConv << " not supported");
break;
}
desAngle[0] = atan2(temp[0], temp[1]);
desAngle[1] = asin(temp[2]);
desAngle[2] = atan2(temp[3], temp[4]);
actualAngle[FIRST] = servo[FIRST].joint->GetAngle(0).RADIAN();
actualVel[FIRST] = servo[FIRST].joint->GetVelocity(0);
ROS_DEBUG_NAMED("servo_plugin", "%s servo angle: %f - %f", servo[FIRST].name.c_str(), desAngle[orderOfAxes[FIRST]], actualAngle[FIRST]);
servo[FIRST].velocity = ( proportionalControllerGain*(desAngle[orderOfAxes[FIRST]] - actualAngle[FIRST]) - derivativeControllerGain*actualVel[FIRST]);
if (maximumVelocity > 0.0 && fabs(servo[FIRST].velocity) > maximumVelocity) servo[FIRST].velocity = (servo[FIRST].velocity > 0 ? maximumVelocity : -maximumVelocity);
if (countOfServos > 1) {
actualAngle[SECOND] = servo[SECOND].joint->GetAngle(0).RADIAN();
actualVel[SECOND] = servo[SECOND].joint->GetVelocity(0);
ROS_DEBUG_NAMED("servo_plugin", "%s servo angle: %f - %f", servo[SECOND].name.c_str(), desAngle[orderOfAxes[SECOND]], actualAngle[SECOND]);
servo[SECOND].velocity = ( proportionalControllerGain*(desAngle[orderOfAxes[SECOND]] - actualAngle[SECOND]) - derivativeControllerGain*actualVel[SECOND]);
if (maximumVelocity > 0.0 && fabs(servo[SECOND].velocity) > maximumVelocity) servo[SECOND].velocity = (servo[SECOND].velocity > 0 ? maximumVelocity : -maximumVelocity);
if (countOfServos == 3) {
actualAngle[THIRD] = servo[THIRD].joint->GetAngle(0).RADIAN();
actualVel[THIRD] = servo[THIRD].joint->GetVelocity(0);
ROS_DEBUG_NAMED("servo_plugin", "%s servo angle: %f - %f", servo[THIRD].name.c_str(), desAngle[orderOfAxes[THIRD]], actualAngle[THIRD]);
servo[THIRD].velocity = ( proportionalControllerGain*(desAngle[orderOfAxes[THIRD]] - actualAngle[THIRD]) - derivativeControllerGain*actualVel[THIRD]);
if (maximumVelocity > 0.0 && fabs(servo[THIRD].velocity) > maximumVelocity) servo[THIRD].velocity = (servo[THIRD].velocity > 0 ? maximumVelocity : -maximumVelocity);
}
}
// Changed motors to be always on, which is probably what we want anyway
enableMotors = true; //myIface->data->cmdEnableMotors > 0;
}
