gazebo_ros_range.cpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (Modified BSD License)
3  *
4  * Copyright (c) 2013-2015, PAL Robotics, S.L.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of PAL Robotics, S.L. nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  */
34 
39 
40 #include <algorithm>
41 #include <string>
42 #include <assert.h>
43 
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>
50 
51 #include <sdf/sdf.hh>
52 #include <sdf/Param.hh>
53 
54 #include <tf/tf.h>
55 
56 namespace gazebo
57 {
58 // Register this plugin with the simulator
59 GZ_REGISTER_SENSOR_PLUGIN(GazeboRosRange)
60 
61 // Constructor
64 {
65  this->seed = 0;
66 }
67 
69 // Destructor
71 {
72  this->range_queue_.clear();
73  this->range_queue_.disable();
74  this->rosnode_->shutdown();
75  this->callback_queue_thread_.join();
76 
77  delete this->rosnode_;
78 }
79 
81 // Load the controller
82 void GazeboRosRange::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf)
83 {
84  // load plugin
85  RayPlugin::Load(_parent, this->sdf);
86  // Get then name of the parent sensor
87  this->parent_sensor_ = _parent;
88  // Get the world name.
89  std::string worldName = _parent->WorldName();
90  this->world_ = physics::get_world(worldName);
91  // save pointers
92  this->sdf = _sdf;
93 
94  this->last_update_time_ = common::Time(0);
95 
97  this->parent_ray_sensor_ =
98  dynamic_pointer_cast<sensors::RaySensor>(this->parent_sensor_);
99 
100  if (!this->parent_ray_sensor_)
101  gzthrow("GazeboRosRange controller requires a Ray Sensor as its parent");
102 
103  this->robot_namespace_ = "";
104  if (this->sdf->HasElement("robotNamespace"))
105  this->robot_namespace_ = this->sdf->Get<std::string>("robotNamespace") + "/";
106 
107  if (!this->sdf->HasElement("frameName"))
108  {
109  ROS_INFO_NAMED("range", "Range plugin missing <frameName>, defaults to /world");
110  this->frame_name_ = "/world";
111  }
112  else
113  this->frame_name_ = this->sdf->Get<std::string>("frameName");
114 
115  if (!this->sdf->HasElement("topicName"))
116  {
117  ROS_INFO_NAMED("range", "Range plugin missing <topicName>, defaults to /range");
118  this->topic_name_ = "/range";
119  }
120  else
121  this->topic_name_ = this->sdf->Get<std::string>("topicName");
122 
123  if (!this->sdf->HasElement("radiation"))
124  {
125  ROS_WARN_NAMED("range", "Range plugin missing <radiation>, defaults to ultrasound");
126  this->radiation_ = "ultrasound";
127 
128  }
129  else
130  this->radiation_ = _sdf->GetElement("radiation")->Get<std::string>();
131 
132  if (!this->sdf->HasElement("fov"))
133  {
134  ROS_WARN_NAMED("range", "Range plugin missing <fov>, defaults to 0.05");
135  this->fov_ = 0.05;
136  }
137  else
138  this->fov_ = _sdf->GetElement("fov")->Get<double>();
139  if (!this->sdf->HasElement("gaussianNoise"))
140  {
141  ROS_INFO_NAMED("range", "Range plugin missing <gaussianNoise>, defaults to 0.0");
142  this->gaussian_noise_ = 0;
143  }
144  else
145  this->gaussian_noise_ = this->sdf->Get<double>("gaussianNoise");
146 
147  if (!this->sdf->HasElement("updateRate"))
148  {
149  ROS_INFO_NAMED("range", "Range plugin missing <updateRate>, defaults to 0");
150  this->update_rate_ = 0;
151  }
152  else
153  this->update_rate_ = this->sdf->Get<double>("updateRate");
154 
155  // prepare to throttle this plugin at the same rate
156  // ideally, we should invoke a plugin update when the sensor updates,
157  // have to think about how to do that properly later
158  if (this->update_rate_ > 0.0)
159  this->update_period_ = 1.0/this->update_rate_;
160  else
161  this->update_period_ = 0.0;
162 
163  this->range_connect_count_ = 0;
164 
165  this->range_msg_.header.frame_id = this->frame_name_;
166  if (this->radiation_==std::string("ultrasound"))
167  this->range_msg_.radiation_type = sensor_msgs::Range::ULTRASOUND;
168  else
169  this->range_msg_.radiation_type = sensor_msgs::Range::INFRARED;
170 
171  this->range_msg_.field_of_view = fov_;
172  this->range_msg_.max_range = this->parent_ray_sensor_->RangeMax();
173  this->range_msg_.min_range = this->parent_ray_sensor_->RangeMin();
174 
175  // Init ROS
176  if (ros::isInitialized())
177  {
178  // ros callback queue for processing subscription
179  this->deferred_load_thread_ = boost::thread(
180  boost::bind(&GazeboRosRange::LoadThread, this));
181  }
182  else
183  {
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";
187  }
188 }
189 
191 // Load the controller
193 {
194  this->rosnode_ = new ros::NodeHandle(this->robot_namespace_);
195 
196  // resolve tf prefix
197  std::string prefix;
198  this->rosnode_->getParam(std::string("tf_prefix"), prefix);
199  this->frame_name_ = tf::resolve(prefix, this->frame_name_);
200 
201  if (this->topic_name_ != "")
202  {
204  ros::AdvertiseOptions::create<sensor_msgs::Range>(
205  this->topic_name_, 1,
206  boost::bind(&GazeboRosRange::RangeConnect, this),
207  boost::bind(&GazeboRosRange::RangeDisconnect, this),
208  ros::VoidPtr(), &this->range_queue_);
209  this->pub_ = this->rosnode_->advertise(ao);
210  }
211 
212 
213  // Initialize the controller
214 
215  // sensor generation off by default
216  this->parent_ray_sensor_->SetActive(false);
217  // start custom queue for range
218  this->callback_queue_thread_ =
219  boost::thread(boost::bind(&GazeboRosRange::RangeQueueThread, this));
220 }
221 
223 // Increment count
225 {
226  this->range_connect_count_++;
227  this->parent_ray_sensor_->SetActive(true);
228 }
230 // Decrement count
232 {
233  this->range_connect_count_--;
234 
235  if (this->range_connect_count_ == 0)
236  this->parent_ray_sensor_->SetActive(false);
237 }
238 
239 
241 // Update the plugin
243 {
244  if (this->topic_name_ != "")
245  {
246 #if GAZEBO_MAJOR_VERSION >= 8
247  common::Time cur_time = this->world_->SimTime();
248 #else
249  common::Time cur_time = this->world_->GetSimTime();
250 #endif
251  if (cur_time < this->last_update_time_)
252  {
253  ROS_WARN_NAMED("range", "Negative sensor update time difference detected.");
254  this->last_update_time_ = cur_time;
255  }
256 
257  if (cur_time - this->last_update_time_ >= this->update_period_)
258  {
259  common::Time sensor_update_time =
260  this->parent_sensor_->LastUpdateTime();
261  this->PutRangeData(sensor_update_time);
262  this->last_update_time_ = cur_time;
263  }
264  }
265  else
266  {
267  ROS_INFO_NAMED("range", "gazebo_ros_range topic name not set");
268  }
269 }
270 
272 // Put range data to the interface
273 void GazeboRosRange::PutRangeData(common::Time &_updateTime)
274 {
275  this->parent_ray_sensor_->SetActive(false);
276 
277  /***************************************************************/
278  /* */
279  /* point scan from ray sensor */
280  /* */
281  /***************************************************************/
282  {
283  boost::mutex::scoped_lock lock(this->lock_);
284  // Add Frame Name
285  this->range_msg_.header.frame_id = this->frame_name_;
286  this->range_msg_.header.stamp.sec = _updateTime.sec;
287  this->range_msg_.header.stamp.nsec = _updateTime.nsec;
288 
289  // find ray with minimal range
290  range_msg_.range = std::numeric_limits<sensor_msgs::Range::_range_type>::max();
291 
292  int num_ranges = parent_ray_sensor_->LaserShape()->GetSampleCount() * parent_ray_sensor_->LaserShape()->GetVerticalSampleCount();
293 
294  for(int i = 0; i < num_ranges; ++i)
295  {
296  double ray = parent_ray_sensor_->LaserShape()->GetRange(i);
297  if (ray < range_msg_.range)
298  range_msg_.range = ray;
299  }
300 
301  // add Gaussian noise and limit to min/max range
302  if (range_msg_.range < range_msg_.max_range)
303  range_msg_.range = std::min(range_msg_.range + this->GaussianKernel(0,gaussian_noise_), parent_ray_sensor_->RangeMax());
304 
305  this->parent_ray_sensor_->SetActive(true);
306 
307  // send data out via ros message
308  if (this->range_connect_count_ > 0 && this->topic_name_ != "")
309  this->pub_.publish(this->range_msg_);
310  }
311 }
312 
314 // Utility for adding noise
315 double GazeboRosRange::GaussianKernel(double mu, double sigma)
316 {
317  // using Box-Muller transform to generate two independent standard
318  // normally disbributed normal variables see wikipedia
319 
320  // normalized uniform random variable
321  double U = static_cast<double>(rand_r(&this->seed)) /
322  static_cast<double>(RAND_MAX);
323 
324  // normalized uniform random variable
325  double V = static_cast<double>(rand_r(&this->seed)) /
326  static_cast<double>(RAND_MAX);
327 
328  double X = sqrt(-2.0 * ::log(U)) * cos(2.0*M_PI * V);
329  // double Y = sqrt(-2.0 * ::log(U)) * sin(2.0*M_PI * V);
330 
331  // there are 2 indep. vars, we'll just use X
332  // scale to our mu and sigma
333  X = sigma * X + mu;
334  return X;
335 }
336 
338 // Put range data to the interface
340 {
341  static const double timeout = 0.01;
342 
343  while (this->rosnode_->ok())
344  {
346  }
347 }
348 }
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
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
physics::WorldPtr world_
bool ok() 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_


gazebo_plugins
Author(s): John Hsu
autogenerated on Tue Mar 26 2019 02:31:27