gazebo_ros_block_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 Block Laser controller.
00023  * Author: Nathan Koenig
00024  * Date: 01 Feb 2007
00025  * SVN info: $Id: gazebo_ros_block_laser.cc 6683 2008-06-25 19:12:30Z natepak $
00026  */
00027 
00028 #include <algorithm>
00029 #include <assert.h>
00030 
00031 #include <gazebo_plugins/gazebo_ros_block_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 "transport/Node.hh"
00042 
00043 #include <geometry_msgs/Point32.h>
00044 #include <sensor_msgs/ChannelFloat32.h>
00045 
00046 #include "tf/tf.h"
00047 
00048 namespace gazebo
00049 {
00050 
00052 // Constructor
00053 GazeboRosBlockLaser::GazeboRosBlockLaser()
00054 {
00055 }
00056 
00058 // Destructor
00059 GazeboRosBlockLaser::~GazeboRosBlockLaser()
00060 {
00062   // Finalize the controller / Custom Callback Queue
00063   this->laser_queue_.clear();
00064   this->laser_queue_.disable();
00065   this->rosnode_->shutdown();
00066   this->callback_laser_queue_thread_.join();
00067 
00068   delete this->rosnode_;
00069 }
00070 
00072 // Load the controller
00073 void GazeboRosBlockLaser::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf)
00074 {
00075   // load plugin
00076   RayPlugin::Load(_parent, _sdf);
00077 
00078   // Get then name of the parent sensor
00079   this->parent_sensor_ = _parent;
00080 
00081   // Get the world name.
00082   std::string worldName = _parent->GetWorldName();
00083   this->world_ = physics::get_world(worldName);
00084 
00085   last_update_time_ = this->world_->GetSimTime();
00086 
00087   this->node_ = transport::NodePtr(new transport::Node());
00088   this->node_->Init(worldName);
00089 
00090   this->parent_ray_sensor_ = boost::shared_dynamic_cast<sensors::RaySensor>(this->parent_sensor_);
00091 
00092   if (!this->parent_ray_sensor_)
00093     gzthrow("GazeboRosBlockLaser controller requires a Ray Sensor as its parent");
00094 
00095   this->robot_namespace_ = "";
00096   if (_sdf->HasElement("robotNamespace"))
00097     this->robot_namespace_ = _sdf->GetElement("robotNamespace")->GetValueString() + "/";
00098 
00099   if (!_sdf->HasElement("frameName"))
00100   {
00101     ROS_INFO("Block laser plugin missing <frameName>, defaults to /world");
00102     this->frame_name_ = "/world";
00103   }
00104   else
00105     this->frame_name_ = _sdf->GetElement("frameName")->GetValueString();
00106 
00107   if (!_sdf->HasElement("topicName"))
00108   {
00109     ROS_INFO("Block laser plugin missing <topicName>, defaults to /world");
00110     this->topic_name_ = "/world";
00111   }
00112   else
00113     this->topic_name_ = _sdf->GetElement("topicName")->GetValueString();
00114 
00115   if (!_sdf->HasElement("gaussianNoise"))
00116   {
00117     ROS_INFO("Block laser plugin missing <gaussianNoise>, defaults to 0.0");
00118     this->gaussian_noise_ = 0;
00119   }
00120   else
00121     this->gaussian_noise_ = _sdf->GetElement("gaussianNoise")->GetValueDouble();
00122 
00123   if (!_sdf->HasElement("hokuyoMinIntensity"))
00124   {
00125     ROS_INFO("Block laser plugin missing <hokuyoMinIntensity>, defaults to 101");
00126     this->hokuyo_min_intensity_ = 101;
00127   }
00128   else
00129     this->hokuyo_min_intensity_ = _sdf->GetElement("hokuyoMinIntensity")->GetValueDouble();
00130 
00131   ROS_INFO("INFO: gazebo_ros_laser plugin should set minimum intensity to %f due to cutoff in hokuyo filters." , this->hokuyo_min_intensity_);
00132 
00133   if (!_sdf->GetElement("updateRate"))
00134   {
00135     ROS_INFO("Block laser plugin missing <updateRate>, defaults to 0");
00136     this->update_rate_ = 0;
00137   }
00138   else
00139     this->update_rate_ = _sdf->GetElement("updateRate")->GetValueDouble();
00140   // FIXME:  update the update_rate_
00141 
00142 
00143   this->laser_connect_count_ = 0;
00144 
00145   if (!ros::isInitialized())
00146   {
00147     int argc = 0;
00148     char** argv = NULL;
00149     ros::init(argc,argv,"gazebo",ros::init_options::NoSigintHandler|ros::init_options::AnonymousName);
00150   }
00151 
00152   this->rosnode_ = new ros::NodeHandle(this->robot_namespace_);
00153 
00154   // resolve tf prefix
00155   std::string prefix;
00156   this->rosnode_->getParam(std::string("tf_prefix"), prefix);
00157   this->frame_name_ = tf::resolve(prefix, this->frame_name_);
00158 
00159   // set size of cloud message, starts at 0!! FIXME: not necessary
00160   this->cloud_msg_.points.clear();
00161   this->cloud_msg_.channels.clear();
00162   this->cloud_msg_.channels.push_back(sensor_msgs::ChannelFloat32());
00163 
00164   if (this->topic_name_ != "")
00165   {
00166     // Custom Callback Queue
00167     ros::AdvertiseOptions ao = ros::AdvertiseOptions::create<sensor_msgs::PointCloud>(
00168       this->topic_name_,1,
00169       boost::bind( &GazeboRosBlockLaser::LaserConnect,this),
00170       boost::bind( &GazeboRosBlockLaser::LaserDisconnect,this), ros::VoidPtr(), &this->laser_queue_);
00171     this->pub_ = this->rosnode_->advertise(ao);
00172   }
00173 
00174 
00175   // Initialize the controller
00176 
00177   // sensor generation off by default
00178   this->parent_ray_sensor_->SetActive(false);
00179   // start custom queue for laser
00180   this->callback_laser_queue_thread_ = boost::thread( boost::bind( &GazeboRosBlockLaser::LaserQueueThread,this ) );
00181 
00182 }
00183 
00185 // Increment count
00186 void GazeboRosBlockLaser::LaserConnect()
00187 {
00188   this->laser_connect_count_++;
00189   this->parent_ray_sensor_->SetActive(true);
00190 }
00192 // Decrement count
00193 void GazeboRosBlockLaser::LaserDisconnect()
00194 {
00195   this->laser_connect_count_--;
00196 
00197   if (this->laser_connect_count_ == 0)
00198     this->parent_ray_sensor_->SetActive(false);
00199 }
00200 
00202 // Update the controller
00203 void GazeboRosBlockLaser::OnNewLaserScans()
00204 {
00205   if (this->topic_name_ != "")
00206   {
00207     common::Time sensor_update_time = this->parent_sensor_->GetLastUpdateTime();
00208     if (last_update_time_ < sensor_update_time)
00209     {
00210       this->PutLaserData(sensor_update_time);
00211       last_update_time_ = sensor_update_time;
00212     }
00213   }
00214   else
00215   {
00216     ROS_INFO("gazebo_ros_block_laser topic name not set");
00217   }
00218 }
00219 
00221 // Put laser data to the interface
00222 void GazeboRosBlockLaser::PutLaserData(common::Time &_updateTime)
00223 {
00224   int i, hja, hjb;
00225   int j, vja, vjb;
00226   double vb, hb;
00227   int    j1, j2, j3, j4; // four corners indices
00228   double r1, r2, r3, r4, r; // four corner values + interpolated range
00229   double intensity;
00230 
00231   this->parent_ray_sensor_->SetActive(false);
00232 
00233   math::Angle maxAngle = this->parent_ray_sensor_->GetAngleMax();
00234   math::Angle minAngle = this->parent_ray_sensor_->GetAngleMin();
00235 
00236   double maxRange = this->parent_ray_sensor_->GetRangeMax();
00237   double minRange = this->parent_ray_sensor_->GetRangeMin();
00238   int rayCount = this->parent_ray_sensor_->GetRayCount();
00239   int rangeCount = this->parent_ray_sensor_->GetRangeCount();
00240 
00241   int verticalRayCount = this->parent_ray_sensor_->GetVerticalRayCount();
00242   int verticalRangeCount = this->parent_ray_sensor_->GetVerticalRangeCount();
00243   math::Angle verticalMaxAngle = this->parent_ray_sensor_->GetVerticalAngleMax();
00244   math::Angle verticalMinAngle = this->parent_ray_sensor_->GetVerticalAngleMin();
00245 
00246   double yDiff = maxAngle.GetAsRadian() - minAngle.GetAsRadian();
00247   double pDiff = verticalMaxAngle.GetAsRadian() - verticalMinAngle.GetAsRadian();
00248 
00249 
00250   // set size of cloud message everytime!
00251   //int r_size = rangeCount * verticalRangeCount;
00252   this->cloud_msg_.points.clear();
00253   this->cloud_msg_.channels.clear();
00254   this->cloud_msg_.channels.push_back(sensor_msgs::ChannelFloat32());
00255 
00256   /***************************************************************/
00257   /*                                                             */
00258   /*  point scan from laser                                      */
00259   /*                                                             */
00260   /***************************************************************/
00261   boost::mutex::scoped_lock sclock(this->lock);
00262   // Add Frame Name
00263   this->cloud_msg_.header.frame_id = this->frame_name_;
00264   this->cloud_msg_.header.stamp.sec = _updateTime.sec;
00265   this->cloud_msg_.header.stamp.nsec = _updateTime.nsec;
00266 
00267   for (j = 0; j<verticalRangeCount; j++)
00268   {
00269     // interpolating in vertical direction
00270     vb = (verticalRangeCount == 1) ? 0 : (double) j * (verticalRayCount - 1) / (verticalRangeCount - 1);
00271     vja = (int) floor(vb);
00272     vjb = std::min(vja + 1, verticalRayCount - 1);
00273     vb = vb - floor(vb); // fraction from min
00274 
00275     assert(vja >= 0 && vja < verticalRayCount);
00276     assert(vjb >= 0 && vjb < verticalRayCount);
00277 
00278     for (i = 0; i<rangeCount; i++)
00279     {
00280       // Interpolate the range readings from the rays in horizontal direction
00281       hb = (rangeCount == 1)? 0 : (double) i * (rayCount - 1) / (rangeCount - 1);
00282       hja = (int) floor(hb);
00283       hjb = std::min(hja + 1, rayCount - 1);
00284       hb = hb - floor(hb); // fraction from min
00285 
00286       assert(hja >= 0 && hja < rayCount);
00287       assert(hjb >= 0 && hjb < rayCount);
00288 
00289       // indices of 4 corners
00290       j1 = hja + vja * rayCount;
00291       j2 = hjb + vja * rayCount;
00292       j3 = hja + vjb * rayCount;
00293       j4 = hjb + vjb * rayCount;
00294       // range readings of 4 corners
00295       r1 = std::min(this->parent_ray_sensor_->GetLaserShape()->GetRange(j1) , maxRange-minRange);
00296       r2 = std::min(this->parent_ray_sensor_->GetLaserShape()->GetRange(j2) , maxRange-minRange);
00297       r3 = std::min(this->parent_ray_sensor_->GetLaserShape()->GetRange(j3) , maxRange-minRange);
00298       r4 = std::min(this->parent_ray_sensor_->GetLaserShape()->GetRange(j4) , maxRange-minRange);
00299 
00300       // Range is linear interpolation if values are close,
00301       // and min if they are very different
00302       r = (1-vb)*((1 - hb) * r1 + hb * r2)
00303          +   vb *((1 - hb) * r3 + hb * r4);
00304 
00305       // Intensity is averaged
00306       intensity = 0.25*(this->parent_ray_sensor_->GetLaserShape()->GetRetro(j1) +
00307                         this->parent_ray_sensor_->GetLaserShape()->GetRetro(j2) +
00308                         this->parent_ray_sensor_->GetLaserShape()->GetRetro(j3) +
00309                         this->parent_ray_sensor_->GetLaserShape()->GetRetro(j4));
00310 
00311       // std::cout << " block debug "
00312       //           << "  ij("<<i<<","<<j<<")"
00313       //           << "  j1234("<<j1<<","<<j2<<","<<j3<<","<<j4<<")"
00314       //           << "  r1234("<<r1<<","<<r2<<","<<r3<<","<<r4<<")"
00315       //           << std::endl;
00316 
00317       // get angles of ray to get xyz for point
00318       double yAngle = 0.5*(hja+hjb) * yDiff / (rayCount -1) + minAngle.GetAsRadian();
00319       double pAngle = 0.5*(vja+vjb) * pDiff / (verticalRayCount -1) + verticalMinAngle.GetAsRadian();
00320 
00321       /***************************************************************/
00322       /*                                                             */
00323       /*  point scan from laser                                      */
00324       /*                                                             */
00325       /***************************************************************/
00326       if (r == maxRange - minRange)
00327       {
00328         // no noise if at max range
00329         geometry_msgs::Point32 point;
00330         point.x = (r+minRange) * cos(pAngle)*cos(yAngle);
00331         point.y = -(r+minRange) *             sin(yAngle);
00332         point.z = (r+minRange) * sin(pAngle)*cos(yAngle);
00333         this->cloud_msg_.points.push_back(point);
00334       }
00335       else
00336       {
00337         geometry_msgs::Point32 point;
00338         point.x      = (r+minRange) * cos(pAngle)*cos(yAngle) + this->GaussianKernel(0,this->gaussian_noise_) ;
00339         point.y      = -(r+minRange) *             sin(yAngle) + this->GaussianKernel(0,this->gaussian_noise_) ;
00340         point.z      = (r+minRange) * sin(pAngle)*cos(yAngle) + this->GaussianKernel(0,this->gaussian_noise_) ;
00341         this->cloud_msg_.points.push_back(point);
00342       }
00343       // only 1 channel
00344       this->cloud_msg_.channels[0].values.push_back(intensity + this->GaussianKernel(0,this->gaussian_noise_)) ;
00345     }
00346   }
00347   this->parent_ray_sensor_->SetActive(true);
00348 
00349   // send data out via ros message
00350   this->pub_.publish(this->cloud_msg_);
00351 
00352 
00353 
00354 }
00355 
00356 
00358 // Utility for adding noise
00359 double GazeboRosBlockLaser::GaussianKernel(double mu,double sigma)
00360 {
00361   // using Box-Muller transform to generate two independent standard normally disbributed normal variables
00362   // see wikipedia
00363   double U = (double)rand()/(double)RAND_MAX; // normalized uniform random variable
00364   double V = (double)rand()/(double)RAND_MAX; // normalized uniform random variable
00365   double X = sqrt(-2.0 * ::log(U)) * cos( 2.0*M_PI * V);
00366   //double Y = sqrt(-2.0 * ::log(U)) * sin( 2.0*M_PI * V); // the other indep. normal variable
00367   // we'll just use X
00368   // scale to our mu and sigma
00369   X = sigma * X + mu;
00370   return X;
00371 }
00372 
00373 // Custom Callback Queue
00375 // custom callback queue thread
00376 void GazeboRosBlockLaser::LaserQueueThread()
00377 {
00378   static const double timeout = 0.01;
00379 
00380   while (this->rosnode_->ok())
00381   {
00382     this->laser_queue_.callAvailable(ros::WallDuration(timeout));
00383   }
00384 }
00385 
00386 void GazeboRosBlockLaser::OnStats( const boost::shared_ptr<msgs::WorldStatistics const> &_msg)
00387 {
00388   this->sim_time_  = msgs::Convert( _msg->sim_time() );
00389 
00390   math::Pose pose;
00391   pose.pos.x = 0.5*sin(0.01*this->sim_time_.Double());
00392   gzdbg << "plugin simTime [" << this->sim_time_.Double() << "] update pose [" << pose.pos.x << "]\n";
00393 }
00394 
00395 // Register this plugin with the simulator
00396 GZ_REGISTER_SENSOR_PLUGIN(GazeboRosBlockLaser)
00397 
00398 }
00399 


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