gazebo_ros_laser.cpp
Go to the documentation of this file.
00001 /*
00002  *  Gazebo - Outdoor Multi-Robot Simulator
00003  *  Copyright (C) 2003
00004  *     Nate Koenig & Andrew Howard
00005  *
00006  *  This program is free software; you can redistribute it and/or modify
00007  *  it under the terms of the GNU General Public License as published by
00008  *  the Free Software Foundation; either version 2 of the License, or
00009  *  (at your option) any later version.
00010  *
00011  *  This program is distributed in the hope that it will be useful,
00012  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00013  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00014  *  GNU General Public License for more details.
00015  *
00016  *  You should have received a copy of the GNU General Public License
00017  *  along with this program; if not, write to the Free Software
00018  *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00019  *
00020  */
00021 /*
00022  * Desc: Ros Laser controller.
00023  * Author: Nathan Koenig
00024  * Date: 01 Feb 2007
00025  * SVN info: $Id: gazebo_ros_laser.cpp 6683 2008-06-25 19:12:30Z natepak $
00026  */
00027 
00028 #include <algorithm>
00029 #include <assert.h>
00030 
00031 #include <gazebo_plugins/gazebo_ros_laser.h>
00032 
00033 #include "physics/World.hh"
00034 #include "physics/HingeJoint.hh"
00035 #include "sensors/Sensor.hh"
00036 #include "sdf/interface/SDF.hh"
00037 #include "sdf/interface/Param.hh"
00038 #include "common/Exception.hh"
00039 #include "sensors/RaySensor.hh"
00040 #include "sensors/SensorTypes.hh"
00041 #include "tf/tf.h"
00042 
00043 namespace gazebo
00044 {
00045 
00047 // Constructor
00048 GazeboRosLaser::GazeboRosLaser()
00049 {
00050 }
00051 
00053 // Destructor
00054 GazeboRosLaser::~GazeboRosLaser()
00055 {
00056   this->laser_queue_.clear();
00057   this->laser_queue_.disable();
00058   this->rosnode_->shutdown();
00059   this->callback_queue_thread_.join();
00060 
00061   delete this->rosnode_;
00062 }
00063 
00065 // Load the controller
00066 void GazeboRosLaser::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf)
00067 {
00068   // load plugin
00069   RayPlugin::Load(_parent, _sdf);
00070 
00071   // Get then name of the parent sensor
00072   this->parent_sensor_ = _parent;
00073 
00074   // Get the world name.
00075   std::string worldName = _parent->GetWorldName();
00076   this->world_ = physics::get_world(worldName);
00077 
00078   this->last_update_time_ = common::Time(0);
00079 
00080   this->parent_ray_sensor_ = boost::shared_dynamic_cast<sensors::RaySensor>(this->parent_sensor_);
00081 
00082   if (!this->parent_ray_sensor_)
00083     gzthrow("GazeboRosLaser controller requires a Ray Sensor as its parent");
00084 
00085   this->robot_namespace_ = "";
00086   if (_sdf->HasElement("robotNamespace"))
00087     this->robot_namespace_ = _sdf->GetElement("robotNamespace")->GetValueString() + "/";
00088 
00089   if (!_sdf->HasElement("frameName"))
00090   {
00091     ROS_INFO("Laser plugin missing <frameName>, defaults to /world");
00092     this->frame_name_ = "/world";
00093   }
00094   else
00095     this->frame_name_ = _sdf->GetElement("frameName")->GetValueString();
00096 
00097   if (!_sdf->HasElement("topicName"))
00098   {
00099     ROS_INFO("Laser plugin missing <topicName>, defaults to /world");
00100     this->topic_name_ = "/world";
00101   }
00102   else
00103     this->topic_name_ = _sdf->GetElement("topicName")->GetValueString();
00104 
00105   if (!_sdf->HasElement("gaussianNoise"))
00106   {
00107     ROS_INFO("Laser plugin missing <gaussianNoise>, defaults to 0.0");
00108     this->gaussian_noise_ = 0;
00109   }
00110   else
00111     this->gaussian_noise_ = _sdf->GetElement("gaussianNoise")->GetValueDouble();
00112 
00113   if (!_sdf->HasElement("hokuyoMinIntensity"))
00114   {
00115     ROS_INFO("Laser plugin missing <hokuyoMinIntensity>, defaults to 101");
00116     this->hokuyo_min_intensity_ = 101;
00117   }
00118   else
00119     this->hokuyo_min_intensity_ = _sdf->GetElement("hokuyoMinIntensity")->GetValueDouble();
00120 
00121   ROS_INFO("INFO: gazebo_ros_laser plugin should set minimum intensity to %f due to cutoff in hokuyo filters." , this->hokuyo_min_intensity_);
00122 
00123   if (!_sdf->GetElement("updateRate"))
00124   {
00125     ROS_INFO("Laser plugin missing <updateRate>, defaults to 0");
00126     this->update_rate_ = 0;
00127   }
00128   else
00129     this->update_rate_ = _sdf->GetElement("updateRate")->GetValueDouble();
00130 
00131   // set parent sensor update rate
00132   this->parent_sensor_->SetUpdateRate(this->update_rate_);
00133 
00134   // prepare to throttle this plugin at the same rate
00135   // ideally, we should invoke a plugin update when the sensor updates,
00136   // have to think about how to do that properly later
00137   if (this->update_rate_ > 0.0)
00138     this->update_period_ = 1.0/this->update_rate_;
00139   else
00140     this->update_period_ = 0.0;
00141 
00142   this->laser_connect_count_ = 0;
00143 
00144 
00145   if (!ros::isInitialized())
00146   {
00147     ROS_FATAL("while loading gazebo_ros_force plugin, ros is not initialized, please load a gazebo system plugin that initializes ros (e.g. libgazebo_ros_api_plugin.so from gazebo ros package)\n");
00148     return;
00149   }
00150 
00151   this->rosnode_ = new ros::NodeHandle(this->robot_namespace_);
00152 
00153   // resolve tf prefix
00154   std::string prefix;
00155   this->rosnode_->getParam(std::string("tf_prefix"), prefix);
00156   this->frame_name_ = tf::resolve(prefix, this->frame_name_);
00157 
00158   if (this->topic_name_ != "")
00159   {
00160     ros::AdvertiseOptions ao = ros::AdvertiseOptions::create<sensor_msgs::LaserScan>(
00161       this->topic_name_,1,
00162       boost::bind( &GazeboRosLaser::LaserConnect,this),
00163       boost::bind( &GazeboRosLaser::LaserDisconnect,this), ros::VoidPtr(), &this->laser_queue_);
00164     this->pub_ = this->rosnode_->advertise(ao);
00165   }
00166 
00167 
00168   // Initialize the controller
00169 
00170   // sensor generation off by default
00171   this->parent_ray_sensor_->SetActive(false);
00172   // start custom queue for laser
00173   this->callback_queue_thread_ = boost::thread( boost::bind( &GazeboRosLaser::LaserQueueThread,this ) );
00174 
00175 }
00176 
00178 // Increment count
00179 void GazeboRosLaser::LaserConnect()
00180 {
00181   this->laser_connect_count_++;
00182   this->parent_ray_sensor_->SetActive(true);
00183 }
00185 // Decrement count
00186 void GazeboRosLaser::LaserDisconnect()
00187 {
00188   this->laser_connect_count_--;
00189 
00190   if (this->laser_connect_count_ == 0)
00191     this->parent_ray_sensor_->SetActive(false);
00192 }
00193 
00194 
00196 // Update the controller
00197 void GazeboRosLaser::OnNewLaserScans()
00198 {
00199   if (this->topic_name_ != "")
00200   {
00201     common::Time cur_time = this->world_->GetSimTime();
00202     if (cur_time - this->last_update_time_ >= this->update_period_)
00203     {
00204       common::Time sensor_update_time = this->parent_sensor_->GetLastUpdateTime();
00205       this->PutLaserData(sensor_update_time);
00206       this->last_update_time_ = cur_time;
00207     }
00208   }
00209   else
00210   {
00211     ROS_INFO("gazebo_ros_laser topic name not set");
00212   }
00213 }
00214 
00216 // Put laser data to the interface
00217 void GazeboRosLaser::PutLaserData(common::Time &_updateTime)
00218 {
00219   int i, ja, jb;
00220   double ra, rb, r, b;
00221   double intensity;
00222 
00223   this->parent_ray_sensor_->SetActive(false);
00224 
00225   math::Angle maxAngle = this->parent_ray_sensor_->GetAngleMax();
00226   math::Angle minAngle = this->parent_ray_sensor_->GetAngleMin();
00227 
00228   double maxRange = this->parent_ray_sensor_->GetRangeMax();
00229   double minRange = this->parent_ray_sensor_->GetRangeMin();
00230   int rayCount = this->parent_ray_sensor_->GetRayCount();
00231   int rangeCount = this->parent_ray_sensor_->GetRangeCount();
00232 
00233   /***************************************************************/
00234   /*                                                             */
00235   /*  point scan from laser                                      */
00236   /*                                                             */
00237   /***************************************************************/
00238   this->lock_.lock();
00239   // Add Frame Name
00240   this->laser_msg_.header.frame_id = this->frame_name_;
00241   this->laser_msg_.header.stamp.sec = _updateTime.sec;
00242   this->laser_msg_.header.stamp.nsec = _updateTime.nsec;
00243 
00244 
00245   double tmp_res_angle = (maxAngle.GetAsRadian() - minAngle.GetAsRadian())/((double)(rangeCount -1)); // for computing yaw
00246   this->laser_msg_.angle_min = minAngle.GetAsRadian();
00247   this->laser_msg_.angle_max = maxAngle.GetAsRadian();
00248   this->laser_msg_.angle_increment = tmp_res_angle;
00249   this->laser_msg_.time_increment  = 0; // instantaneous simulator scan
00250   this->laser_msg_.scan_time       = 0; // FIXME: what's this?
00251   this->laser_msg_.range_min = minRange;
00252   this->laser_msg_.range_max = maxRange;
00253   this->laser_msg_.ranges.clear();
00254   this->laser_msg_.intensities.clear();
00255 
00256   // Interpolate the range readings from the rays
00257   for (i = 0; i<rangeCount; i++)
00258   {
00259     b = (double) i * (rayCount - 1) / (rangeCount - 1);
00260     ja = (int) floor(b);
00261     jb = std::min(ja + 1, rayCount - 1);
00262     b = b - floor(b);
00263 
00264     assert(ja >= 0 && ja < rayCount);
00265     assert(jb >= 0 && jb < rayCount);
00266 
00267     ra = std::min(this->parent_ray_sensor_->GetLaserShape()->GetRange(ja) , maxRange-minRange); // length of ray
00268     rb = std::min(this->parent_ray_sensor_->GetLaserShape()->GetRange(jb) , maxRange-minRange); // length of ray
00269 
00270     // Range is linear interpolation if values are close,
00271     // and min if they are very different
00272     //if (fabs(ra - rb) < 0.10)
00273       r = (1 - b) * ra + b * rb;
00274     //else r = std::min(ra, rb);
00275 
00276     // Intensity is averaged
00277     intensity = 0.5*( this->parent_ray_sensor_->GetLaserShape()->GetRetro(ja)
00278                     + this->parent_ray_sensor_->GetLaserShape()->GetRetro(jb));
00279 
00280     /***************************************************************/
00281     /*                                                             */
00282     /*  point scan from laser                                      */
00283     /*                                                             */
00284     /***************************************************************/
00285     this->laser_msg_.ranges.push_back(std::min(r + minRange + this->GaussianKernel(0,this->gaussian_noise_), maxRange));
00286     this->laser_msg_.intensities.push_back(std::max(this->hokuyo_min_intensity_,intensity + this->GaussianKernel(0,this->gaussian_noise_)));
00287   }
00288 
00289   this->parent_ray_sensor_->SetActive(true);
00290 
00291   // send data out via ros message
00292   if (this->laser_connect_count_ > 0 && this->topic_name_ != "")
00293       this->pub_.publish(this->laser_msg_);
00294 
00295   this->lock_.unlock();
00296 
00297 }
00298 
00300 // Utility for adding noise
00301 double GazeboRosLaser::GaussianKernel(double mu,double sigma)
00302 {
00303   // using Box-Muller transform to generate two independent standard normally disbributed normal variables
00304   // see wikipedia
00305   double U = (double)rand()/(double)RAND_MAX; // normalized uniform random variable
00306   double V = (double)rand()/(double)RAND_MAX; // normalized uniform random variable
00307   double X = sqrt(-2.0 * ::log(U)) * cos( 2.0*M_PI * V);
00308   //double Y = sqrt(-2.0 * ::log(U)) * sin( 2.0*M_PI * V); // the other indep. normal variable
00309   // we'll just use X
00310   // scale to our mu and sigma
00311   X = sigma * X + mu;
00312   return X;
00313 }
00314 
00316 // Put laser data to the interface
00317 void GazeboRosLaser::LaserQueueThread()
00318 {
00319   static const double timeout = 0.01;
00320 
00321   while (this->rosnode_->ok())
00322   {
00323     this->laser_queue_.callAvailable(ros::WallDuration(timeout));
00324   }
00325 }
00326 
00327 // Register this plugin with the simulator
00328 GZ_REGISTER_SENSOR_PLUGIN(GazeboRosLaser)
00329 
00330 }


gazebo_plugins
Author(s): Sachin Chitta, Stu Glaser, John Hsu
autogenerated on Sun Jan 5 2014 11:34:58