gazebo_ros_gpu_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  @mainpage
00023    Desc: GazeboRosGpuLaser plugin for simulating ray sensors in Gazebo
00024    Author: Mihai Emanuel Dolha
00025    Date: 29 March 2012
00026    SVN info: $Id$
00027  @htmlinclude manifest.html
00028  @b GazeboRosGpuLaser plugin broadcasts ROS Image messages
00029  */
00030 
00031 #include <algorithm>
00032 #include <assert.h>
00033 #include <boost/thread/thread.hpp>
00034 #include <boost/bind.hpp>
00035 
00036 #include "sensor_msgs/fill_image.h"
00037 #include "image_transport/image_transport.h"
00038 
00039 #include <gazebo_plugins/gazebo_ros_gpu_laser.h>
00040 
00041 #include "sensors/Sensor.hh"
00042 #include "sensors/GpuRaySensor.hh"
00043 #include "sdf/interface/SDF.hh"
00044 #include "sensors/SensorTypes.hh"
00045 
00046 #include "tf/tf.h"
00047 
00048 #include <iostream>
00049 #include <fstream>
00050 
00051 namespace gazebo
00052 {
00053 
00055 // Constructor
00056 GazeboRosGpuLaser::GazeboRosGpuLaser()
00057 {
00058   this->laser_connect_count_ = 0;
00059 }
00060 
00062 // Destructor
00063 GazeboRosGpuLaser::~GazeboRosGpuLaser()
00064 {
00065   this->parentSensor->SetActive(false);
00066   this->rosnode_->shutdown();
00067   this->queue_.clear();
00068   this->queue_.disable();
00069   this->callback_queue_thread_.join();
00070 //  this->timelog_.close();
00071   delete this->rosnode_;
00072 }
00073 
00075 // Load the controller
00076 void GazeboRosGpuLaser::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf)
00077 {
00078   GpuRayPlugin::Load(_parent, _sdf);
00079 
00080   //GazeboRosCameraUtils::Load(_parent, _sdf);
00081   this->robot_namespace_ = "";
00082   if (_sdf->HasElement("robotNamespace"))
00083     this->robot_namespace_ = _sdf->GetElement("robotNamespace")->GetValueString() + "/";
00084 
00085   // point cloud stuff
00086   if (!_sdf->GetElement("topicName"))
00087     this->laser_topic_name_ = "scan";
00088   else
00089     this->laser_topic_name_ = _sdf->GetElement("topicName")->GetValueString();
00090 
00091   if (!_sdf->GetElement("pointCloudCutoff"))
00092     this->point_cloud_cutoff_ = 0.4;
00093   else
00094     this->point_cloud_cutoff_ = _sdf->GetElement("pointCloudCutoff")->GetValueDouble();
00095 
00096   this->frame_name_ = "/world";
00097   if (_sdf->HasElement("frameName"))
00098     this->frame_name_ = _sdf->GetElement("frameName")->GetValueString();
00099 
00100   if (!_sdf->HasElement("gaussianNoise"))
00101   {
00102     ROS_INFO("Block laser plugin missing <gaussianNoise>, defaults to 0.0");
00103     this->gaussian_noise_ = 0;
00104   }
00105   else
00106     this->gaussian_noise_ = _sdf->GetElement("gaussianNoise")->GetValueDouble();
00107 
00108   if (!_sdf->HasElement("hokuyoMinIntensity"))
00109   {
00110     ROS_INFO("Block laser plugin missing <hokuyoMinIntensity>, defaults to 101");
00111     this->hokuyo_min_intensity_ = 101;
00112   }
00113   else
00114     this->hokuyo_min_intensity_ = _sdf->GetElement("hokuyoMinIntensity")->GetValueDouble();
00115 
00116   ROS_INFO("INFO: gazebo_ros_laser plugin should set minimum intensity to %f due to cutoff in hokuyo filters." , this->hokuyo_min_intensity_);
00117 
00118   if (!_sdf->GetElement("updateRate"))
00119   {
00120     ROS_INFO("Camera plugin missing <updateRate>, defaults to 0");
00121     this->update_rate_ = 0;
00122   }
00123   else
00124     this->update_rate_ = _sdf->GetElement("updateRate")->GetValueDouble();
00125 
00126 
00127   // Setup ROS
00128   if (!ros::isInitialized())
00129   {
00130     int argc = 0;
00131     char** argv = NULL;
00132     ros::init( argc, argv, "gazebo",
00133                ros::init_options::NoSigintHandler |
00134                ros::init_options::AnonymousName );
00135   }
00136 
00137   this->rosnode_ = new ros::NodeHandle(this->robot_namespace_);
00138 
00139   // resolve tf prefix
00140   std::string prefix;
00141   this->rosnode_->getParam(std::string("tf_prefix"), prefix);
00142   this->frame_name_ = tf::resolve(prefix, this->frame_name_);
00143 
00144   ros::AdvertiseOptions laser_scan_ao;
00145 
00146   if (this->parentSensor->GetVerticalRangeCount() != 1)
00147     laser_scan_ao = ros::AdvertiseOptions::create<pcl::PointCloud<pcl::PointXYZI> >(
00148       this->laser_topic_name_,1,
00149       boost::bind( &GazeboRosGpuLaser::LaserConnect,this),
00150       boost::bind( &GazeboRosGpuLaser::LaserDisconnect,this),
00151       ros::VoidPtr(), &this->queue_);
00152   else
00153     laser_scan_ao = ros::AdvertiseOptions::create<sensor_msgs::LaserScan>(
00154       this->laser_topic_name_,1,
00155       boost::bind( &GazeboRosGpuLaser::LaserConnect,this),
00156       boost::bind( &GazeboRosGpuLaser::LaserDisconnect,this),
00157       ros::VoidPtr(), &this->queue_);
00158   this->laser_scan_pub_ = this->rosnode_->advertise(laser_scan_ao);
00159 
00160 //  std::stringstream logName;
00161 //  logName << "/tmp/" << this->parentSensor->GetName() << "_log.txt";
00162 
00163 //  this->timelog_.open(logName.str().c_str());
00164 
00165   //this->itnode_ = new image_transport::ImageTransport(*this->rosnode_);
00166   //this->image_pub_ = this->itnode_->advertise(
00167   //  "target1",1,
00168   //  boost::bind( &GazeboRosGpuLaser::ImageConnect,this),
00169   //  boost::bind( &GazeboRosGpuLaser::ImageDisconnect,this),
00170   //  ros::VoidPtr(), &this->queue_);
00171 
00172   //this->image2_pub_ = this->itnode_->advertise(
00173   //  "target2",1,
00174   //  boost::bind( &GazeboRosGpuLaser::ImageConnect,this),
00175   //  boost::bind( &GazeboRosGpuLaser::ImageDisconnect,this),
00176   //  ros::VoidPtr(), &this->queue_);
00177 
00178   //this->image3_pub_ = this->itnode_->advertise(
00179   //  "target3",1,
00180   //  boost::bind( &GazeboRosGpuLaser::ImageConnect,this),
00181   //  boost::bind( &GazeboRosGpuLaser::ImageDisconnect,this),
00182   //  ros::VoidPtr(), &this->queue_);
00183 
00184   //this->image4_pub_ = this->itnode_->advertise(
00185   //  "final_image",1,
00186   //  boost::bind( &GazeboRosGpuLaser::ImageConnect,this),
00187   //  boost::bind( &GazeboRosGpuLaser::ImageDisconnect,this),
00188   //  ros::VoidPtr(), &this->queue_);
00189 
00190 
00191   this->Init();
00192 }
00193 
00194 //void GazeboRosGpuLaser::PutCameraData(const unsigned char *_src, unsigned int w, unsigned int h, unsigned int d, image_transport::Publisher *pub_)
00195 //{
00196 //  // copy data into image
00197 //  sensor_msgs::Image msg_;
00198 //  msg_.header.frame_id = this->frame_name_;
00199 //  msg_.header.stamp.sec = this->sensor_update_time_.sec;
00200 //  msg_.header.stamp.nsec = this->sensor_update_time_.nsec;
00201 //
00202 //  /// @todo: don't bother if there are no subscribers
00203 //  if (pub_->getNumSubscribers() > 0)
00204 //  {
00205 //    // copy from src to image_msg_
00206 //    fillImage(msg_,
00207 //        sensor_msgs::image_encodings::RGB8,
00208 //        h,
00209 //        w,
00210 //        d*w,
00211 //        (void*)_src );
00212 //
00213 //    // publish to ros
00214 //    pub_->publish(msg_);
00215 //  }
00216 //}
00217 //
00220 //void GazeboRosGpuLaser::ImageConnect()
00221 //{
00222 //  this->imageConnectCount++;
00223 //  this->parentSensor->SetActive(true);
00224 //}
00227 //void GazeboRosGpuLaser::ImageDisconnect()
00228 //{
00229 //  this->imageConnectCount--;
00230 //  if (this->imageConnectCount <= 0)
00231 //    this->parentSensor->SetActive(false);
00232 //}
00233 //
00236 //void GazeboRosGpuLaser::OnNewImageFrame(const unsigned char *_image,
00237 //    unsigned int _width, unsigned int _height, unsigned int _depth,
00238 //    unsigned int cam)
00239 //{
00240 //  //ROS_ERROR("camera_ new frame %s %s",this->parentSensor_->GetName().c_str(),this->frame_name_.c_str());
00241 //  this->sensor_update_time_ = this->parentSensor->GetLastUpdateTime();
00242 //
00243 //  if (!this->parentSensor->IsActive())
00244 //  {
00245 //    if (this->imageConnectCount > 0)
00246 //      // do this first so there's chance for sensor to run 1 frame after activate
00247 //      this->parentSensor->SetActive(true);
00248 //  }
00249 //  else
00250 //  {
00251 //    if (this->imageConnectCount > 0)
00252 //    {
00253 //      switch (cam)
00254 //      {
00255 //        case 1: this->PutCameraData(_image, _width, _height, _depth, &this->image_pub_);break;
00256 //        case 2: this->PutCameraData(_image, _width, _height, _depth, &this->image2_pub_);break;
00257 //        case 3: this->PutCameraData(_image, _width, _height, _depth, &this->image3_pub_);break;
00258 //        case 4: this->PutCameraData(_image, _width, _height, _depth, &this->image4_pub_);break;
00259 //      }
00260 //    }
00261 //  }
00262 //}
00263 
00265 void GazeboRosGpuLaser::Init()
00266 {
00267   // set parent sensor update rate
00268   this->parentSensor->SetUpdateRate(this->update_rate_);
00269 
00270   // prepare to throttle this plugin at the same rate
00271   // ideally, we should invoke a plugin update when the sensor updates,
00272   // have to think about how to do that properly later
00273   if (this->update_rate_ > 0.0)
00274     this->update_period_ = 1.0/this->update_rate_;
00275   else
00276     this->update_period_ = 0.0;
00277 
00278   // start custom queue for camera_
00279   this->callback_queue_thread_ = boost::thread( boost::bind( &GazeboRosGpuLaser::QueueThread,this ) );
00280 
00281   this->last_publish_ = ros::WallTime::now();
00282   // this->logCount_ = 0;
00283 }
00284 
00286 // Increment count
00287 void GazeboRosGpuLaser::LaserConnect()
00288 {
00289   this->laser_connect_count_++;
00290   this->parentSensor->SetActive(true);
00291 }
00293 // Decrement count
00294 void GazeboRosGpuLaser::LaserDisconnect()
00295 {
00296   this->laser_connect_count_--;
00297 
00298   if (this->laser_connect_count_ <= 0)
00299     this->parentSensor->SetActive(false);
00300 }
00301 
00303 // Update the controller
00304 void GazeboRosGpuLaser::OnNewLaserFrame(const float *_image,
00305     unsigned int _width, unsigned int _height, unsigned int _depth,
00306     const std::string &_format)
00307 {
00308   this->sensor_update_time_ = this->parentSensor->GetLastUpdateTime();
00309   if (!this->parentSensor->IsActive())
00310   {
00311     if (this->laser_connect_count_ > 0)
00312       // do this first so there's chance for sensor to run 1 frame after activate
00313       this->parentSensor->SetActive(true);
00314   }
00315   else
00316   {
00317     if (this->laser_connect_count_ > 0)
00318     {
00319       if (this->parentSensor->GetVerticalRangeCount() == 1)
00320         PublishLaserScan(_image, width);
00321       else
00322         PublishPointCloud(_image, width, height);
00323     }
00324   }
00325 }
00326 
00328 void GazeboRosGpuLaser::PublishLaserScan(const float *_scan,
00329     unsigned int _width)
00330 {
00331   math::Angle maxAngle = this->parentSensor->GetAngleMax();
00332   math::Angle minAngle = this->parentSensor->GetAngleMin();
00333 
00334   this->laser_scan_msg_.header.frame_id = this->frame_name_;
00335   this->laser_scan_msg_.header.stamp.sec = this->sensor_update_time_.sec;
00336   this->laser_scan_msg_.header.stamp.nsec = this->sensor_update_time_.nsec;
00337 
00338   this->laser_scan_msg_.angle_min = minAngle.Radian();
00339   this->laser_scan_msg_.angle_max = maxAngle.Radian();
00340   this->laser_scan_msg_.angle_increment = (maxAngle.Radian() - minAngle.Radian())/((double)(_width -1)); // for computing yaw
00341   this->laser_scan_msg_.time_increment  = 0; // instantaneous simulator scan
00342   this->laser_scan_msg_.scan_time       = 0; // FIXME: what's this?
00343   this->laser_scan_msg_.range_min = this->parentSensor->GetRangeMin();
00344   this->laser_scan_msg_.range_max = this->parentSensor->GetRangeMax();
00345   this->laser_scan_msg_.ranges.clear();
00346   this->laser_scan_msg_.intensities.clear();
00347 
00348   for(unsigned int i=0; i < width; i++)
00349   {
00350     float range = _scan[3 * i];
00351     if (range < this->parentSensor->GetRangeMin())
00352       range = this->parentSensor->GetRangeMax();
00353     this->laser_scan_msg_.ranges.push_back(range + this->GaussianKernel(0,this->gaussian_noise_));
00354     this->laser_scan_msg_.intensities.push_back(_scan[3*i+1]);
00355   }
00356 
00357 //  unsigned int logLimit = 101;
00358 
00359 //  if (this->logCount_ < logLimit)
00360 //  {
00361 //    ros::WallTime curr_time = ros::WallTime::now();
00362 //    ros::WallDuration duration = curr_time - this->last_publish_;
00363 //    if (this->logCount_ != 0)
00364 //      this->timelog_ << (duration.toSec() * 1000) << "\n";
00365 //    ROS_WARN("%f", duration.toSec() * 1000);
00366     this->laser_scan_pub_.publish(this->laser_scan_msg_);
00367 //    this->last_publish_ = curr_time;
00368 //    this->logCount_++;
00369 //  }
00370 //  else
00371 //    if (this->logCount_ == logLimit)
00372 //      this->timelog_.close();
00373 
00374 }
00375 
00377 void GazeboRosGpuLaser::PublishPointCloud(const float *_scan,
00378     unsigned int _width, unsigned int _height)
00379 {
00380   this->point_cloud_msg_.header.frame_id = this->frame_name_;
00381   this->point_cloud_msg_.header.stamp.sec = this->sensor_update_time_.sec;
00382   this->point_cloud_msg_.header.stamp.nsec = this->sensor_update_time_.nsec;
00383 
00384   this->point_cloud_msg_.points.clear();
00385   this->point_cloud_msg_.is_dense = true;
00386 
00387   math::Angle maxAngle = this->parentSensor->GetAngleMax();
00388   math::Angle minAngle = this->parentSensor->GetAngleMin();
00389 
00390   math::Angle verticalMaxAngle = this->parentSensor->GetVerticalAngleMax();
00391   math::Angle verticalMinAngle = this->parentSensor->GetVerticalAngleMin();
00392 
00393   double dH = (maxAngle.Radian() - minAngle.Radian()) / (_width - 1) ;
00394   double dV = (verticalMaxAngle.Radian() - verticalMinAngle.Radian()) / (_height - 1);
00395 
00396   double alpha = ((minAngle + maxAngle) / 2.0).Radian();
00397 
00398   for (unsigned int j = 0; j < _height; j++)
00399     for (unsigned int i = 0; i < _width; i++)
00400     {
00401       double hAngle = (i * dH) + minAngle.Radian() - alpha;
00402       double vAngle = (j * dV) + verticalMinAngle.Radian();
00403       float r = _scan[3 * (i + j * _width)];
00404 
00405       pcl::PointXYZI pr;
00406 
00407       if ((r < this->parentSensor->GetRangeMin()) ||
00408           (r >= this->parentSensor->GetRangeMax()))
00409         r = this->parentSensor->GetRangeMax();
00410 
00411       pcl::PointXYZI p;
00412 
00413       if (this->parentSensor->IsHorizontal())
00414       {
00415         p.x = r * cos(vAngle) * cos(hAngle) + this->GaussianKernel(0,this->gaussian_noise_);
00416         p.y = r *               sin(hAngle) + this->GaussianKernel(0,this->gaussian_noise_);
00417         p.z = r * sin(vAngle) * cos(hAngle) + this->GaussianKernel(0,this->gaussian_noise_);
00418       }
00419       else
00420       {
00421         p.x = r * cos(vAngle) * cos(hAngle) + this->GaussianKernel(0,this->gaussian_noise_);
00422         p.y = r * cos(vAngle) * sin(hAngle) + this->GaussianKernel(0,this->gaussian_noise_);
00423         p.z = r * sin(vAngle) + this->GaussianKernel(0,this->gaussian_noise_);
00424       }
00425 
00426       pr.x = cos(alpha)*p.x - sin(alpha)*p.y;
00427       pr.y = sin(alpha)*p.x + cos(alpha)*p.y;
00428       pr.z = p.z;
00429       pr.intensity = _scan[3 * (i + j * _width) + 1];
00430 
00431       this->point_cloud_msg_.points.push_back(pr);
00432     }
00433 
00434     //std::cerr<<"--------------------------------\n";
00435 
00436 //  if (this->logCount_ < 10001)
00437 //  {
00438 //    ros::WallTime curr_time = ros::WallTime::now();
00439 //    ros::WallDuration duration = curr_time - this->last_publish_;
00440 //    if (this->logCount_ != 0)
00441 //      this->timelog_ << duration << "\n";
00442     this->laser_scan_pub_.publish(this->point_cloud_msg_);
00443 //    this->last_publish_ = curr_time;
00444 //  }
00445 }
00446 
00447 
00449 // Utility for adding noise
00450 double GazeboRosGpuLaser::GaussianKernel(double mu,double sigma)
00451 {
00452   // using Box-Muller transform to generate two independent standard normally disbributed normal variables
00453   // see wikipedia
00454   double U = (double)rand()/(double)RAND_MAX; // normalized uniform random variable
00455   double V = (double)rand()/(double)RAND_MAX; // normalized uniform random variable
00456   double X = sqrt(-2.0 * ::log(U)) * cos( 2.0*M_PI * V);
00457   //double Y = sqrt(-2.0 * ::log(U)) * sin( 2.0*M_PI * V); // the other indep. normal variable
00458   // we'll just use X
00459   // scale to our mu and sigma
00460   X = sigma * X + mu;
00461   return X;
00462 }
00463 
00465 // Put camera_ data to the interface
00466 void GazeboRosGpuLaser::QueueThread()
00467 {
00468   static const double timeout = 0.001;
00469 
00470   while (this->rosnode_->ok())
00471   {
00473     this->queue_.callAvailable(ros::WallDuration(timeout));
00474   }
00475 }
00476 
00477 // Register this plugin with the simulator
00478 GZ_REGISTER_SENSOR_PLUGIN(GazeboRosGpuLaser)
00479 
00480 }


gazebo_plugins
Author(s): Sachin Chitta, Stu Glaser, John Hsu
autogenerated on Mon Oct 6 2014 12:15:44