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);
}
////////////////////////////////////////////////////////////////////////////////
// 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));
}
////////////////////////////////////////////////////////////////////////////////
// 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 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 GazeboRosThermalCameraUtils::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf)
{
// Get the world name.
std::string world_name = _parent->GetWorldName();
// Get the world_
this->world_ = physics::get_world(world_name);
// maintain for one more release for backwards compatibility with pr2_gazebo_plugins
this->world = this->world_;
this->robot_namespace_ = "";
if (_sdf->HasElement("robotNamespace"))
this->robot_namespace_ = _sdf->GetElement("robotNamespace")->GetValueString() + "/";
this->image_topic_name_ = "image_raw";
if (_sdf->GetElement("imageTopicName"))
this->image_topic_name_ = _sdf->GetElement("imageTopicName")->GetValueString();
this->camera_info_topic_name_ = "camera_info";
if (_sdf->HasElement("cameraInfoTopicName"))
this->camera_info_topic_name_ = _sdf->GetElement("cameraInfoTopicName")->GetValueString();
if (!_sdf->HasElement("cameraName"))
ROS_INFO("Camera plugin missing <cameraName>, default to empty");
else
this->camera_name_ = _sdf->GetElement("cameraName")->GetValueString();
if (!_sdf->HasElement("frameName"))
ROS_INFO("Camera plugin missing <frameName>, defaults to /world");
else
this->frame_name_ = _sdf->GetElement("frameName")->GetValueString();
if (!_sdf->GetElement("updateRate"))
{
ROS_INFO("Camera plugin missing <updateRate>, defaults to 0");
this->update_rate_ = 0;
}
else
this->update_rate_ = _sdf->GetElement("updateRate")->GetValueDouble();
if (!_sdf->GetElement("CxPrime"))
{
ROS_INFO("Camera plugin missing <CxPrime>, defaults to 0");
this->cx_prime_ = 0;
}
else
this->cx_prime_ = _sdf->GetElement("CxPrime")->GetValueDouble();
if (!_sdf->HasElement("Cx"))
{
ROS_INFO("Camera plugin missing <Cx>, defaults to 0");
this->cx_= 0;
}
else
this->cx_ = _sdf->GetElement("Cx")->GetValueDouble();
if (!_sdf->HasElement("Cy"))
{
ROS_INFO("Camera plugin missing <Cy>, defaults to 0");
this->cy_= 0;
}
else
this->cy_ = _sdf->GetElement("Cy")->GetValueDouble();
if (!_sdf->HasElement("focalLength"))
{
ROS_INFO("Camera plugin missing <focalLength>, defaults to 0");
this->focal_length_= 0;
}
else
this->focal_length_ = _sdf->GetElement("focalLength")->GetValueDouble();
if (!_sdf->HasElement("hackBaseline"))
{
ROS_INFO("Camera plugin missing <hackBaseline>, defaults to 0");
this->hack_baseline_= 0;
}
else
this->hack_baseline_ = _sdf->GetElement("hackBaseline")->GetValueDouble();
if (!_sdf->HasElement("distortionK1"))
{
ROS_INFO("Camera plugin missing <distortionK1>, defaults to 0");
this->distortion_k1_= 0;
}
else
this->distortion_k1_ = _sdf->GetElement("distortionK1")->GetValueDouble();
if (!_sdf->HasElement("distortionK2"))
{
ROS_INFO("Camera plugin missing <distortionK2>, defaults to 0");
this->distortion_k2_= 0;
}
else
this->distortion_k2_ = _sdf->GetElement("distortionK2")->GetValueDouble();
if (!_sdf->HasElement("distortionK3"))
{
ROS_INFO("Camera plugin missing <distortionK3>, defaults to 0");
this->distortion_k3_= 0;
}
else
this->distortion_k3_ = _sdf->GetElement("distortionK3")->GetValueDouble();
if (!_sdf->HasElement("distortionT1"))
{
ROS_INFO("Camera plugin missing <distortionT1>, defaults to 0");
this->distortion_t1_= 0;
}
else
this->distortion_t1_ = _sdf->GetElement("distortionT1")->GetValueDouble();
if (!_sdf->HasElement("distortionT2"))
{
ROS_INFO("Camera plugin missing <distortionT2>, defaults to 0");
this->distortion_t2_= 0;
}
else
this->distortion_t2_ = _sdf->GetElement("distortionT2")->GetValueDouble();
if ((this->distortion_k1_ != 0.0) || (this->distortion_k2_ != 0.0) ||
(this->distortion_k3_ != 0.0) || (this->distortion_t1_ != 0.0) ||
(this->distortion_t2_ != 0.0))
{
ROS_WARN("gazebo_ros_camera_ simulation does not support non-zero distortion parameters right now, your simulation maybe wrong.");
}
// Setup ROS
if (!ros::isInitialized())
{
int argc = 0;
char** argv = NULL;
ros::init( argc, argv, "gazebo",
ros::init_options::NoSigintHandler |
ros::init_options::AnonymousName );
}
this->rosnode_ = new ros::NodeHandle(this->robot_namespace_+"/"+this->camera_name_);
this->itnode_ = new image_transport::ImageTransport(*this->rosnode_);
// resolve tf prefix
std::string prefix;
this->rosnode_->getParam(std::string("tf_prefix"), prefix);
this->frame_name_ = tf::resolve(prefix, this->frame_name_);
if (!this->camera_name_.empty())
{
dyn_srv_ = new dynamic_reconfigure::Server<gazebo_plugins::GazeboRosCameraConfig>(*this->rosnode_);
dynamic_reconfigure::Server<gazebo_plugins::GazeboRosCameraConfig>::CallbackType f = boost::bind(&GazeboRosThermalCameraUtils::configCallback, this, _1, _2);
dyn_srv_->setCallback(f);
}
else
{
ROS_WARN("dynamic reconfigure is not enabled for this image topic [%s] becuase <cameraName> is not specified",this->image_topic_name_.c_str());
}
this->image_pub_ = this->itnode_->advertise(
this->image_topic_name_,1,
boost::bind( &GazeboRosThermalCameraUtils::ImageConnect,this),
boost::bind( &GazeboRosThermalCameraUtils::ImageDisconnect,this),
ros::VoidPtr(), &this->camera_queue_);
ros::AdvertiseOptions camera_info_ao =
ros::AdvertiseOptions::create<sensor_msgs::CameraInfo>(
this->camera_info_topic_name_,1,
boost::bind( &GazeboRosThermalCameraUtils::InfoConnect,this),
boost::bind( &GazeboRosThermalCameraUtils::InfoDisconnect,this),
ros::VoidPtr(), &this->camera_queue_);
this->camera_info_pub_ = this->rosnode_->advertise(camera_info_ao);
ros::SubscribeOptions zoom_so =
ros::SubscribeOptions::create<std_msgs::Float64>(
"set_hfov",1,
boost::bind( &GazeboRosThermalCameraUtils::SetHFOV,this,_1),
ros::VoidPtr(), &this->camera_queue_);
this->cameraHFOVSubscriber_ = this->rosnode_->subscribe(zoom_so);
ros::SubscribeOptions rate_so =
ros::SubscribeOptions::create<std_msgs::Float64>(
"set_update_rate",1,
boost::bind( &GazeboRosThermalCameraUtils::SetUpdateRate,this,_1),
ros::VoidPtr(), &this->camera_queue_);
this->cameraUpdateRateSubscriber_ = this->rosnode_->subscribe(rate_so);
this->Init();
}
////////////////////////////////////////////////////////////////////////////////
// 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
}