gazebo_ros_block_laser.cpp
Go to the documentation of this file.
1 /*
2  * Copyright 2013 Open Source Robotics Foundation
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  *
16 */
17 /*
18  * Desc: Ros Block Laser controller.
19  * Author: Nathan Koenig
20  * Date: 01 Feb 2007
21  */
22 
23 #include <algorithm>
24 #include <assert.h>
25 
28 
29 #include <gazebo/physics/World.hh>
30 #include <gazebo/physics/HingeJoint.hh>
31 #include <gazebo/sensors/Sensor.hh>
32 #include <sdf/sdf.hh>
33 #include <sdf/Param.hh>
34 #include <gazebo/common/Exception.hh>
35 #include <gazebo/sensors/RaySensor.hh>
36 #include <gazebo/sensors/SensorTypes.hh>
37 #include <gazebo/transport/Node.hh>
38 
39 #include <geometry_msgs/Point32.h>
40 #include <sensor_msgs/ChannelFloat32.h>
41 
42 #include <tf/tf.h>
43 
44 #define EPSILON_DIFF 0.000001
45 
46 namespace gazebo
47 {
48 // Register this plugin with the simulator
49 GZ_REGISTER_SENSOR_PLUGIN(GazeboRosBlockLaser)
50 
51 // Constructor
54 {
55 }
56 
58 // Destructor
60 {
62  // Finalize the controller / Custom Callback Queue
63  this->laser_queue_.clear();
64  this->laser_queue_.disable();
65  this->rosnode_->shutdown();
66  this->callback_laser_queue_thread_.join();
67 
68  delete this->rosnode_;
69 }
70 
72 // Load the controller
73 void GazeboRosBlockLaser::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf)
74 {
75  // load plugin
76  RayPlugin::Load(_parent, _sdf);
77 
78  // Get then name of the parent sensor
79  this->parent_sensor_ = _parent;
80 
81  // Get the world name.
82  std::string worldName = _parent->WorldName();
83  this->world_ = physics::get_world(worldName);
84 
85 #if GAZEBO_MAJOR_VERSION >= 8
86  last_update_time_ = this->world_->SimTime();
87 #else
88  last_update_time_ = this->world_->GetSimTime();
89 #endif
90 
91  this->node_ = transport::NodePtr(new transport::Node());
92  this->node_->Init(worldName);
93 
95  this->parent_ray_sensor_ = dynamic_pointer_cast<sensors::RaySensor>(this->parent_sensor_);
96 
97  if (!this->parent_ray_sensor_)
98  gzthrow("GazeboRosBlockLaser controller requires a Ray Sensor as its parent");
99 
100  this->robot_namespace_ = "";
101  if (_sdf->HasElement("robotNamespace"))
102  this->robot_namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>() + "/";
103 
104  if (!_sdf->HasElement("frameName"))
105  {
106  ROS_INFO_NAMED("block_laser", "Block laser plugin missing <frameName>, defaults to /world");
107  this->frame_name_ = "/world";
108  }
109  else
110  this->frame_name_ = _sdf->GetElement("frameName")->Get<std::string>();
111 
112  if (!_sdf->HasElement("topicName"))
113  {
114  ROS_INFO_NAMED("block_laser", "Block laser plugin missing <topicName>, defaults to /world");
115  this->topic_name_ = "/world";
116  }
117  else
118  this->topic_name_ = _sdf->GetElement("topicName")->Get<std::string>();
119 
120  if (!_sdf->HasElement("gaussianNoise"))
121  {
122  ROS_INFO_NAMED("block_laser", "Block laser plugin missing <gaussianNoise>, defaults to 0.0");
123  this->gaussian_noise_ = 0;
124  }
125  else
126  this->gaussian_noise_ = _sdf->GetElement("gaussianNoise")->Get<double>();
127 
128  if (!_sdf->HasElement("hokuyoMinIntensity"))
129  {
130  ROS_INFO_NAMED("block_laser", "Block laser plugin missing <hokuyoMinIntensity>, defaults to 101");
131  this->hokuyo_min_intensity_ = 101;
132  }
133  else
134  this->hokuyo_min_intensity_ = _sdf->GetElement("hokuyoMinIntensity")->Get<double>();
135 
136  ROS_DEBUG_NAMED("block_laser", "gazebo_ros_laser plugin should set minimum intensity to %f due to cutoff in hokuyo filters." , this->hokuyo_min_intensity_);
137 
138  if (!_sdf->HasElement("updateRate"))
139  {
140  ROS_INFO_NAMED("block_laser", "Block laser plugin missing <updateRate>, defaults to 0");
141  this->update_rate_ = 0;
142  }
143  else
144  this->update_rate_ = _sdf->GetElement("updateRate")->Get<double>();
145  // FIXME: update the update_rate_
146 
147 
148  this->laser_connect_count_ = 0;
149 
150  // Make sure the ROS node for Gazebo has already been initialized
151  if (!ros::isInitialized())
152  {
153  ROS_FATAL_STREAM_NAMED("block_laser", "A ROS node for Gazebo has not been initialized, unable to load plugin. "
154  << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
155  return;
156  }
157 
158  this->rosnode_ = new ros::NodeHandle(this->robot_namespace_);
159 
160  // resolve tf prefix
161  std::string prefix;
162  this->rosnode_->getParam(std::string("tf_prefix"), prefix);
163  this->frame_name_ = tf::resolve(prefix, this->frame_name_);
164 
165  // set size of cloud message, starts at 0!! FIXME: not necessary
166  this->cloud_msg_.points.clear();
167  this->cloud_msg_.channels.clear();
168  this->cloud_msg_.channels.push_back(sensor_msgs::ChannelFloat32());
169 
170  if (this->topic_name_ != "")
171  {
172  // Custom Callback Queue
173  ros::AdvertiseOptions ao = ros::AdvertiseOptions::create<sensor_msgs::PointCloud>(
174  this->topic_name_,1,
175  boost::bind( &GazeboRosBlockLaser::LaserConnect,this),
176  boost::bind( &GazeboRosBlockLaser::LaserDisconnect,this), ros::VoidPtr(), &this->laser_queue_);
177  this->pub_ = this->rosnode_->advertise(ao);
178  }
179 
180 
181  // Initialize the controller
182 
183  // sensor generation off by default
184  this->parent_ray_sensor_->SetActive(false);
185  // start custom queue for laser
186  this->callback_laser_queue_thread_ = boost::thread( boost::bind( &GazeboRosBlockLaser::LaserQueueThread,this ) );
187 
188 }
189 
191 // Increment count
193 {
194  this->laser_connect_count_++;
195  this->parent_ray_sensor_->SetActive(true);
196 }
198 // Decrement count
200 {
201  this->laser_connect_count_--;
202 
203  if (this->laser_connect_count_ == 0)
204  this->parent_ray_sensor_->SetActive(false);
205 }
206 
208 // Update the controller
210 {
211  if (this->topic_name_ != "")
212  {
213  common::Time sensor_update_time = this->parent_sensor_->LastUpdateTime();
214  if (sensor_update_time < last_update_time_)
215  {
216  ROS_WARN_NAMED("block_laser", "Negative sensor update time difference detected.");
217  last_update_time_ = sensor_update_time;
218  }
219 
220  if (last_update_time_ < sensor_update_time)
221  {
222  this->PutLaserData(sensor_update_time);
223  last_update_time_ = sensor_update_time;
224  }
225  }
226  else
227  {
228  ROS_INFO_NAMED("block_laser", "gazebo_ros_block_laser topic name not set");
229  }
230 }
231 
233 // Put laser data to the interface
234 void GazeboRosBlockLaser::PutLaserData(common::Time &_updateTime)
235 {
236  int i, hja, hjb;
237  int j, vja, vjb;
238  double vb, hb;
239  int j1, j2, j3, j4; // four corners indices
240  double r1, r2, r3, r4, r; // four corner values + interpolated range
241  double intensity;
242 
243  this->parent_ray_sensor_->SetActive(false);
244 
245  auto maxAngle = this->parent_ray_sensor_->AngleMax();
246  auto minAngle = this->parent_ray_sensor_->AngleMin();
247 
248  double maxRange = this->parent_ray_sensor_->RangeMax();
249  double minRange = this->parent_ray_sensor_->RangeMin();
250  int rayCount = this->parent_ray_sensor_->RayCount();
251  int rangeCount = this->parent_ray_sensor_->RangeCount();
252 
253  int verticalRayCount = this->parent_ray_sensor_->VerticalRayCount();
254  int verticalRangeCount = this->parent_ray_sensor_->VerticalRangeCount();
255  auto verticalMaxAngle = this->parent_ray_sensor_->VerticalAngleMax();
256  auto verticalMinAngle = this->parent_ray_sensor_->VerticalAngleMin();
257 
258  double yDiff = maxAngle.Radian() - minAngle.Radian();
259  double pDiff = verticalMaxAngle.Radian() - verticalMinAngle.Radian();
260 
261 
262  // set size of cloud message everytime!
263  //int r_size = rangeCount * verticalRangeCount;
264  this->cloud_msg_.points.clear();
265  this->cloud_msg_.channels.clear();
266  this->cloud_msg_.channels.push_back(sensor_msgs::ChannelFloat32());
267 
268  /***************************************************************/
269  /* */
270  /* point scan from laser */
271  /* */
272  /***************************************************************/
273  boost::mutex::scoped_lock sclock(this->lock);
274  // Add Frame Name
275  this->cloud_msg_.header.frame_id = this->frame_name_;
276  this->cloud_msg_.header.stamp.sec = _updateTime.sec;
277  this->cloud_msg_.header.stamp.nsec = _updateTime.nsec;
278 
279  for (j = 0; j<verticalRangeCount; j++)
280  {
281  // interpolating in vertical direction
282  vb = (verticalRangeCount == 1) ? 0 : (double) j * (verticalRayCount - 1) / (verticalRangeCount - 1);
283  vja = (int) floor(vb);
284  vjb = std::min(vja + 1, verticalRayCount - 1);
285  vb = vb - floor(vb); // fraction from min
286 
287  assert(vja >= 0 && vja < verticalRayCount);
288  assert(vjb >= 0 && vjb < verticalRayCount);
289 
290  for (i = 0; i<rangeCount; i++)
291  {
292  // Interpolate the range readings from the rays in horizontal direction
293  hb = (rangeCount == 1)? 0 : (double) i * (rayCount - 1) / (rangeCount - 1);
294  hja = (int) floor(hb);
295  hjb = std::min(hja + 1, rayCount - 1);
296  hb = hb - floor(hb); // fraction from min
297 
298  assert(hja >= 0 && hja < rayCount);
299  assert(hjb >= 0 && hjb < rayCount);
300 
301  // indices of 4 corners
302  j1 = hja + vja * rayCount;
303  j2 = hjb + vja * rayCount;
304  j3 = hja + vjb * rayCount;
305  j4 = hjb + vjb * rayCount;
306  // range readings of 4 corners
307  r1 = std::min(this->parent_ray_sensor_->LaserShape()->GetRange(j1) , maxRange-minRange);
308  r2 = std::min(this->parent_ray_sensor_->LaserShape()->GetRange(j2) , maxRange-minRange);
309  r3 = std::min(this->parent_ray_sensor_->LaserShape()->GetRange(j3) , maxRange-minRange);
310  r4 = std::min(this->parent_ray_sensor_->LaserShape()->GetRange(j4) , maxRange-minRange);
311 
312  // Range is linear interpolation if values are close,
313  // and min if they are very different
314  r = (1-vb)*((1 - hb) * r1 + hb * r2)
315  + vb *((1 - hb) * r3 + hb * r4);
316 
317  // Intensity is averaged
318  intensity = 0.25*(this->parent_ray_sensor_->LaserShape()->GetRetro(j1) +
319  this->parent_ray_sensor_->LaserShape()->GetRetro(j2) +
320  this->parent_ray_sensor_->LaserShape()->GetRetro(j3) +
321  this->parent_ray_sensor_->LaserShape()->GetRetro(j4));
322 
323  // std::cout << " block debug "
324  // << " ij("<<i<<","<<j<<")"
325  // << " j1234("<<j1<<","<<j2<<","<<j3<<","<<j4<<")"
326  // << " r1234("<<r1<<","<<r2<<","<<r3<<","<<r4<<")"
327  // << std::endl;
328 
329  // get angles of ray to get xyz for point
330  double yAngle = 0.5*(hja+hjb) * yDiff / (rayCount -1) + minAngle.Radian();
331  double pAngle = 0.5*(vja+vjb) * pDiff / (verticalRayCount -1) + verticalMinAngle.Radian();
332 
333  /***************************************************************/
334  /* */
335  /* point scan from laser */
336  /* */
337  /***************************************************************/
338  //compare 2 doubles
339  double diffRange = maxRange - minRange;
340  double diff = diffRange - r;
341  if (fabs(diff) < EPSILON_DIFF)
342  {
343  // no noise if at max range
344  geometry_msgs::Point32 point;
345  //pAngle is rotated by yAngle:
346  point.x = r * cos(pAngle) * cos(yAngle);
347  point.y = r * cos(pAngle) * sin(yAngle);
348  point.z = r * sin(pAngle);
349 
350  this->cloud_msg_.points.push_back(point);
351  }
352  else
353  {
354  geometry_msgs::Point32 point;
355  //pAngle is rotated by yAngle:
356  point.x = r * cos(pAngle) * cos(yAngle) + this->GaussianKernel(0,this->gaussian_noise_);
357  point.y = r * cos(pAngle) * sin(yAngle) + this->GaussianKernel(0,this->gaussian_noise_);
358  point.z = r * sin(pAngle) + this->GaussianKernel(0,this->gaussian_noise_);
359  this->cloud_msg_.points.push_back(point);
360  } // only 1 channel
361 
362  this->cloud_msg_.channels[0].values.push_back(intensity + this->GaussianKernel(0,this->gaussian_noise_)) ;
363  }
364  }
365  this->parent_ray_sensor_->SetActive(true);
366 
367  // send data out via ros message
368  this->pub_.publish(this->cloud_msg_);
369 
370 
371 
372 }
373 
374 
376 // Utility for adding noise
377 double GazeboRosBlockLaser::GaussianKernel(double mu,double sigma)
378 {
379  // using Box-Muller transform to generate two independent standard normally disbributed normal variables
380  // see wikipedia
381  double U = (double)rand()/(double)RAND_MAX; // normalized uniform random variable
382  double V = (double)rand()/(double)RAND_MAX; // normalized uniform random variable
383  double X = sqrt(-2.0 * ::log(U)) * cos( 2.0*M_PI * V);
384  //double Y = sqrt(-2.0 * ::log(U)) * sin( 2.0*M_PI * V); // the other indep. normal variable
385  // we'll just use X
386  // scale to our mu and sigma
387  X = sigma * X + mu;
388  return X;
389 }
390 
391 // Custom Callback Queue
393 // custom callback queue thread
395 {
396  static const double timeout = 0.01;
397 
398  while (this->rosnode_->ok())
399  {
401  }
402 }
403 
405 {
406  this->sim_time_ = msgs::Convert( _msg->sim_time() );
407 
408  ignition::math::Pose3d pose;
409  pose.Pos().X() = 0.5*sin(0.01*this->sim_time_.Double());
410  gzdbg << "plugin simTime [" << this->sim_time_.Double() << "] update pose [" << pose.Pos().X() << "]\n";
411 }
412 
413 
414 }
#define EPSILON_DIFF
int laser_connect_count_
Keep track of number of connctions.
void PutLaserData(common::Time &_updateTime)
Put laser data to the ROS topic.
#define ROS_INFO_NAMED(name,...)
void publish(const boost::shared_ptr< M > &message) const
#define ROS_WARN_NAMED(name,...)
ROSCPP_DECL bool isInitialized()
double hokuyo_min_intensity_
hack to mimic hokuyo intensity cutoff of 100
ros::NodeHandle * rosnode_
pointer to ros node
boost::mutex lock
A mutex to lock access to fields that are used in message callbacks.
std::string robot_namespace_
for setting ROS name space
std::string resolve(const std::string &prefix, const std::string &frame_name)
#define ROS_DEBUG_NAMED(name,...)
#define ROS_FATAL_STREAM_NAMED(name, args)
std::string frame_name_
frame transform name, should match link name
std::string topic_name_
topic name
double GaussianKernel(double mu, double sigma)
Gaussian noise generator.
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
sensors::SensorPtr parent_sensor_
The parent sensor.
double gaussian_noise_
Gaussian noise.
sensors::RaySensorPtr parent_ray_sensor_
bool getParam(const std::string &key, std::string &s) const
virtual void OnNewLaserScans()
Update the controller.
bool ok() const
#define GAZEBO_SENSORS_USING_DYNAMIC_POINTER_CAST
void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf)
Load the plugin.
double update_rate_
update rate of this sensor
boost::shared_ptr< void > VoidPtr
void OnStats(const boost::shared_ptr< msgs::WorldStatistics const > &_msg)
sensor_msgs::PointCloud cloud_msg_
ros message


gazebo_plugins
Author(s): John Hsu
autogenerated on Tue Apr 6 2021 02:19:39