gazebo_ros_range.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (Modified BSD License)
00003  *
00004  *  Copyright (c) 2013-2015, PAL Robotics, S.L.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of PAL Robotics, S.L. nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  */
00034 
00037 #include "gazebo_plugins/gazebo_ros_range.h"
00038 #include "gazebo_plugins/gazebo_ros_utils.h"
00039 
00040 #include <algorithm>
00041 #include <string>
00042 #include <assert.h>
00043 
00044 #include <gazebo/physics/World.hh>
00045 #include <gazebo/physics/HingeJoint.hh>
00046 #include <gazebo/sensors/Sensor.hh>
00047 #include <gazebo/common/Exception.hh>
00048 #include <gazebo/sensors/RaySensor.hh>
00049 #include <gazebo/sensors/SensorTypes.hh>
00050 
00051 #include <sdf/sdf.hh>
00052 #include <sdf/Param.hh>
00053 
00054 #include <tf/tf.h>
00055 
00056 namespace gazebo
00057 {
00058 // Register this plugin with the simulator
00059 GZ_REGISTER_SENSOR_PLUGIN(GazeboRosRange)
00060 
00061 
00062 // Constructor
00063 GazeboRosRange::GazeboRosRange()
00064 {
00065   this->seed = 0;
00066 }
00067 
00069 // Destructor
00070 GazeboRosRange::~GazeboRosRange()
00071 {
00072   this->range_queue_.clear();
00073   this->range_queue_.disable();
00074   this->rosnode_->shutdown();
00075   this->callback_queue_thread_.join();
00076 
00077   delete this->rosnode_;
00078 }
00079 
00081 // Load the controller
00082 void GazeboRosRange::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf)
00083 {
00084   // load plugin
00085   RayPlugin::Load(_parent, this->sdf);
00086   // Get then name of the parent sensor
00087   this->parent_sensor_ = _parent;
00088   // Get the world name.
00089 # if GAZEBO_MAJOR_VERSION >= 7
00090   std::string worldName = _parent->WorldName();
00091 # else
00092   std::string worldName = _parent->GetWorldName();
00093 # endif
00094   this->world_ = physics::get_world(worldName);
00095   // save pointers
00096   this->sdf = _sdf;
00097 
00098   this->last_update_time_ = common::Time(0);
00099 
00100   GAZEBO_SENSORS_USING_DYNAMIC_POINTER_CAST;
00101   this->parent_ray_sensor_ =
00102     dynamic_pointer_cast<sensors::RaySensor>(this->parent_sensor_);
00103 
00104   if (!this->parent_ray_sensor_)
00105     gzthrow("GazeboRosRange controller requires a Ray Sensor as its parent");
00106 
00107   this->robot_namespace_ = "";
00108   if (this->sdf->HasElement("robotNamespace"))
00109     this->robot_namespace_ = this->sdf->Get<std::string>("robotNamespace") + "/";
00110 
00111   if (!this->sdf->HasElement("frameName"))
00112   {
00113     ROS_INFO("Range plugin missing <frameName>, defaults to /world");
00114     this->frame_name_ = "/world";
00115   }
00116   else
00117     this->frame_name_ = this->sdf->Get<std::string>("frameName");
00118 
00119   if (!this->sdf->HasElement("topicName"))
00120   {
00121     ROS_INFO("Range plugin missing <topicName>, defaults to /range");
00122     this->topic_name_ = "/range";
00123   }
00124   else
00125     this->topic_name_ = this->sdf->Get<std::string>("topicName");
00126 
00127   if (!this->sdf->HasElement("radiation"))
00128   {
00129       ROS_WARN("Range plugin missing <radiation>, defaults to ultrasound");
00130       this->radiation_ = "ultrasound";
00131 
00132   }
00133   else
00134       this->radiation_ = _sdf->GetElement("radiation")->Get<std::string>();
00135 
00136   if (!this->sdf->HasElement("fov"))
00137   {
00138       ROS_WARN("Range plugin missing <fov>, defaults to 0.05");
00139       this->fov_ = 0.05;
00140   }
00141   else
00142       this->fov_ = _sdf->GetElement("fov")->Get<double>();
00143   if (!this->sdf->HasElement("gaussianNoise"))
00144   {
00145     ROS_INFO("Range plugin missing <gaussianNoise>, defaults to 0.0");
00146     this->gaussian_noise_ = 0;
00147   }
00148   else
00149     this->gaussian_noise_ = this->sdf->Get<double>("gaussianNoise");
00150 
00151   if (!this->sdf->HasElement("updateRate"))
00152   {
00153     ROS_INFO("Range plugin missing <updateRate>, defaults to 0");
00154     this->update_rate_ = 0;
00155   }
00156   else
00157     this->update_rate_ = this->sdf->Get<double>("updateRate");
00158 
00159   // prepare to throttle this plugin at the same rate
00160   // ideally, we should invoke a plugin update when the sensor updates,
00161   // have to think about how to do that properly later
00162   if (this->update_rate_ > 0.0)
00163     this->update_period_ = 1.0/this->update_rate_;
00164   else
00165     this->update_period_ = 0.0;
00166 
00167   this->range_connect_count_ = 0;
00168 
00169   this->range_msg_.header.frame_id = this->frame_name_;
00170   if (this->radiation_==std::string("ultrasound"))
00171      this->range_msg_.radiation_type = sensor_msgs::Range::ULTRASOUND;
00172   else
00173       this->range_msg_.radiation_type = sensor_msgs::Range::INFRARED;
00174 
00175   this->range_msg_.field_of_view = fov_;
00176 # if GAZEBO_MAJOR_VERSION >= 7
00177   this->range_msg_.max_range = this->parent_ray_sensor_->RangeMax();
00178   this->range_msg_.min_range = this->parent_ray_sensor_->RangeMin();
00179 # else
00180   this->range_msg_.max_range = this->parent_ray_sensor_->GetRangeMax();
00181   this->range_msg_.min_range = this->parent_ray_sensor_->GetRangeMin();
00182 # endif
00183 
00184   // Init ROS
00185   if (ros::isInitialized())
00186   {
00187     // ros callback queue for processing subscription
00188     this->deferred_load_thread_ = boost::thread(
00189       boost::bind(&GazeboRosRange::LoadThread, this));
00190   }
00191   else
00192   {
00193     gzerr << "Not loading plugin since ROS hasn't been "
00194           << "properly initialized.  Try starting gazebo with ros plugin:\n"
00195           << "  gazebo -s libgazebo_ros_api_plugin.so\n";
00196   }
00197 }
00198 
00200 // Load the controller
00201 void GazeboRosRange::LoadThread()
00202 {
00203   this->rosnode_ = new ros::NodeHandle(this->robot_namespace_);
00204 
00205   // resolve tf prefix
00206   std::string prefix;
00207   this->rosnode_->getParam(std::string("tf_prefix"), prefix);
00208   this->frame_name_ = tf::resolve(prefix, this->frame_name_);
00209 
00210   if (this->topic_name_ != "")
00211   {
00212     ros::AdvertiseOptions ao =
00213       ros::AdvertiseOptions::create<sensor_msgs::Range>(
00214       this->topic_name_, 1,
00215       boost::bind(&GazeboRosRange::RangeConnect, this),
00216       boost::bind(&GazeboRosRange::RangeDisconnect, this),
00217       ros::VoidPtr(), &this->range_queue_);
00218     this->pub_ = this->rosnode_->advertise(ao);
00219   }
00220 
00221 
00222   // Initialize the controller
00223 
00224   // sensor generation off by default
00225   this->parent_ray_sensor_->SetActive(false);
00226   // start custom queue for range
00227   this->callback_queue_thread_ =
00228     boost::thread(boost::bind(&GazeboRosRange::RangeQueueThread, this));
00229 }
00230 
00232 // Increment count
00233 void GazeboRosRange::RangeConnect()
00234 {
00235   this->range_connect_count_++;
00236   this->parent_ray_sensor_->SetActive(true);
00237 }
00239 // Decrement count
00240 void GazeboRosRange::RangeDisconnect()
00241 {
00242   this->range_connect_count_--;
00243 
00244   if (this->range_connect_count_ == 0)
00245     this->parent_ray_sensor_->SetActive(false);
00246 }
00247 
00248 
00250 // Update the plugin
00251 void GazeboRosRange::OnNewLaserScans()
00252 {
00253   if (this->topic_name_ != "")
00254   {
00255     common::Time cur_time = this->world_->GetSimTime();
00256     if (cur_time - this->last_update_time_ >= this->update_period_)
00257     {
00258       common::Time sensor_update_time =
00259 # if GAZEBO_MAJOR_VERSION >= 7
00260         this->parent_sensor_->LastUpdateTime();
00261 # else
00262         this->parent_sensor_->GetLastUpdateTime();
00263 # endif
00264       this->PutRangeData(sensor_update_time);
00265       this->last_update_time_ = cur_time;
00266     }
00267   }
00268   else
00269   {
00270     ROS_INFO("gazebo_ros_range topic name not set");
00271   }
00272 }
00273 
00275 // Put range data to the interface
00276 void GazeboRosRange::PutRangeData(common::Time &_updateTime)
00277 {
00278   this->parent_ray_sensor_->SetActive(false);
00279 
00280   /***************************************************************/
00281   /*                                                             */
00282   /*  point scan from ray sensor                                 */
00283   /*                                                             */
00284   /***************************************************************/
00285   {
00286     boost::mutex::scoped_lock lock(this->lock_);
00287     // Add Frame Name
00288     this->range_msg_.header.frame_id = this->frame_name_;
00289     this->range_msg_.header.stamp.sec = _updateTime.sec;
00290     this->range_msg_.header.stamp.nsec = _updateTime.nsec;
00291 
00292     // find ray with minimal range
00293     range_msg_.range = std::numeric_limits<sensor_msgs::Range::_range_type>::max();
00294 
00295 # if GAZEBO_MAJOR_VERSION >= 7
00296     int num_ranges = parent_ray_sensor_->LaserShape()->GetSampleCount() * parent_ray_sensor_->LaserShape()->GetVerticalSampleCount();
00297 # else
00298     int num_ranges = parent_ray_sensor_->GetLaserShape()->GetSampleCount() * parent_ray_sensor_->GetLaserShape()->GetVerticalSampleCount();
00299 # endif
00300 
00301     for(int i = 0; i < num_ranges; ++i)
00302     {
00303 # if GAZEBO_MAJOR_VERSION >= 7
00304         double ray = parent_ray_sensor_->LaserShape()->GetRange(i);
00305 # else
00306         double ray = parent_ray_sensor_->GetLaserShape()->GetRange(i);
00307 # endif
00308         if (ray < range_msg_.range)
00309             range_msg_.range = ray;
00310     }
00311 
00312     // add Gaussian noise and limit to min/max range
00313     if (range_msg_.range < range_msg_.max_range)
00314 # if GAZEBO_MAJOR_VERSION >= 7
00315         range_msg_.range = std::min(range_msg_.range + this->GaussianKernel(0,gaussian_noise_), parent_ray_sensor_->RangeMax());
00316 # else
00317         range_msg_.range = std::min(range_msg_.range + this->GaussianKernel(0,gaussian_noise_), parent_ray_sensor_->GetRangeMax());
00318 # endif
00319 
00320     this->parent_ray_sensor_->SetActive(true);
00321 
00322     // send data out via ros message
00323     if (this->range_connect_count_ > 0 && this->topic_name_ != "")
00324         this->pub_.publish(this->range_msg_);
00325   }
00326 }
00327 
00329 // Utility for adding noise
00330 double GazeboRosRange::GaussianKernel(double mu, double sigma)
00331 {
00332   // using Box-Muller transform to generate two independent standard
00333   // normally disbributed normal variables see wikipedia
00334 
00335   // normalized uniform random variable
00336   double U = static_cast<double>(rand_r(&this->seed)) /
00337              static_cast<double>(RAND_MAX);
00338 
00339   // normalized uniform random variable
00340   double V = static_cast<double>(rand_r(&this->seed)) /
00341              static_cast<double>(RAND_MAX);
00342 
00343   double X = sqrt(-2.0 * ::log(U)) * cos(2.0*M_PI * V);
00344   // double Y = sqrt(-2.0 * ::log(U)) * sin(2.0*M_PI * V);
00345 
00346   // there are 2 indep. vars, we'll just use X
00347   // scale to our mu and sigma
00348   X = sigma * X + mu;
00349   return X;
00350 }
00351 
00353 // Put range data to the interface
00354 void GazeboRosRange::RangeQueueThread()
00355 {
00356   static const double timeout = 0.01;
00357 
00358   while (this->rosnode_->ok())
00359   {
00360     this->range_queue_.callAvailable(ros::WallDuration(timeout));
00361   }
00362 }
00363 }


gazebo_plugins
Author(s): John Hsu
autogenerated on Thu Jun 6 2019 18:41:09