////////////////////////////////////////////////////////////////////////////////
// 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";
}
}
////////////////////////////////////////////////////////////////////////////////
// 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
}