44 #include <gazebo/physics/World.hh> 45 #include <gazebo/physics/HingeJoint.hh> 46 #include <gazebo/sensors/Sensor.hh> 47 #include <gazebo/common/Exception.hh> 48 #include <gazebo/sensors/RaySensor.hh> 49 #include <gazebo/sensors/SensorTypes.hh> 52 #include <sdf/Param.hh> 59 GZ_REGISTER_SENSOR_PLUGIN(GazeboRosRange)
85 RayPlugin::Load(_parent, this->
sdf);
89 std::string worldName = _parent->WorldName();
90 this->
world_ = physics::get_world(worldName);
101 gzthrow(
"GazeboRosRange controller requires a Ray Sensor as its parent");
104 if (this->
sdf->HasElement(
"robotNamespace"))
107 if (!this->
sdf->HasElement(
"frameName"))
109 ROS_INFO_NAMED(
"range",
"Range plugin missing <frameName>, defaults to /world");
115 if (!this->
sdf->HasElement(
"topicName"))
117 ROS_INFO_NAMED(
"range",
"Range plugin missing <topicName>, defaults to /range");
123 if (!this->
sdf->HasElement(
"radiation"))
125 ROS_WARN_NAMED(
"range",
"Range plugin missing <radiation>, defaults to ultrasound");
130 this->
radiation_ = _sdf->GetElement(
"radiation")->Get<std::string>();
132 if (!this->
sdf->HasElement(
"fov"))
134 ROS_WARN_NAMED(
"range",
"Range plugin missing <fov>, defaults to 0.05");
138 this->
fov_ = _sdf->GetElement(
"fov")->Get<
double>();
139 if (!this->
sdf->HasElement(
"gaussianNoise"))
141 ROS_INFO_NAMED(
"range",
"Range plugin missing <gaussianNoise>, defaults to 0.0");
147 if (!this->
sdf->HasElement(
"updateRate"))
149 ROS_INFO_NAMED(
"range",
"Range plugin missing <updateRate>, defaults to 0");
166 if (this->
radiation_==std::string(
"ultrasound"))
167 this->
range_msg_.radiation_type = sensor_msgs::Range::ULTRASOUND;
169 this->range_msg_.radiation_type = sensor_msgs::Range::INFRARED;
171 this->range_msg_.field_of_view =
fov_;
172 this->range_msg_.max_range = this->parent_ray_sensor_->RangeMax();
184 gzerr <<
"Not loading plugin since ROS hasn't been " 185 <<
"properly initialized. Try starting gazebo with ros plugin:\n" 186 <<
" gazebo -s libgazebo_ros_api_plugin.so\n";
204 ros::AdvertiseOptions::create<sensor_msgs::Range>(
246 #if GAZEBO_MAJOR_VERSION >= 8 247 common::Time cur_time = this->
world_->SimTime();
249 common::Time cur_time = this->
world_->GetSimTime();
253 ROS_WARN_NAMED(
"range",
"Negative sensor update time difference detected.");
254 this->last_update_time_ = cur_time;
259 common::Time sensor_update_time =
262 this->last_update_time_ = cur_time;
283 boost::mutex::scoped_lock lock(this->
lock_);
286 this->
range_msg_.header.stamp.sec = _updateTime.sec;
287 this->
range_msg_.header.stamp.nsec = _updateTime.nsec;
290 range_msg_.range = std::numeric_limits<sensor_msgs::Range::_range_type>::max();
294 for(
int i = 0; i < num_ranges; ++i)
321 double U =
static_cast<double>(rand_r(&this->
seed)) /
322 static_cast<double>(RAND_MAX);
325 double V =
static_cast<double>(rand_r(&this->
seed)) /
326 static_cast<double>(RAND_MAX);
328 double X = sqrt(-2.0 * ::log(U)) * cos(2.0*M_PI * V);
341 static const double timeout = 0.01;
std::string robot_namespace_
for setting ROS name space
double GaussianKernel(double mu, double sigma)
Gaussian noise generator.
sensor_msgs::Range range_msg_
ros message
#define ROS_INFO_NAMED(name,...)
void publish(const boost::shared_ptr< M > &message) const
void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf)
Load the plugin.
#define ROS_WARN_NAMED(name,...)
ros::NodeHandle * rosnode_
pointer to ros node
~GazeboRosRange()
Destructor.
ROSCPP_DECL bool isInitialized()
double update_rate_
update rate of this sensor
std::string resolve(const std::string &prefix, const std::string &frame_name)
sensors::RaySensorPtr parent_ray_sensor_
std::string radiation_
radiation type : ultrasound or infrared
std::string frame_name_
frame transform name, should match link name
int range_connect_count_
Keep track of number of connctions.
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
common::Time last_update_time_
boost::mutex lock_
mutex to lock access to fields that are used in message callbacks
double gaussian_noise_
Gaussian noise.
bool getParam(const std::string &key, std::string &s) const
#define GAZEBO_SENSORS_USING_DYNAMIC_POINTER_CAST
std::string topic_name_
topic name
ros::CallbackQueue range_queue_
sensors::SensorPtr parent_sensor_
The parent sensor.
boost::shared_ptr< void > VoidPtr
boost::thread callback_queue_thread_
virtual void OnNewLaserScans()
Update the controller.
void PutRangeData(common::Time &_updateTime)
Put range data to the ROS topic.
double fov_
sensor field of view
boost::thread deferred_load_thread_