gazebo_ros_block_laser.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright 2013 Open Source Robotics Foundation
00003  *
00004  * Licensed under the Apache License, Version 2.0 (the "License");
00005  * you may not use this file except in compliance with the License.
00006  * You may obtain a copy of the License at
00007  *
00008  *     http://www.apache.org/licenses/LICENSE-2.0
00009  *
00010  * Unless required by applicable law or agreed to in writing, software
00011  * distributed under the License is distributed on an "AS IS" BASIS,
00012  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00013  * See the License for the specific language governing permissions and
00014  * limitations under the License.
00015  *
00016 */
00017 /*
00018  * Desc: Ros Block Laser controller.
00019  * Author: Nathan Koenig
00020  * Date: 01 Feb 2007
00021  */
00022 
00023 #include <algorithm>
00024 #include <assert.h>
00025 
00026 #include <gazebo_plugins/gazebo_ros_block_laser.h>
00027 #include <gazebo_plugins/gazebo_ros_utils.h>
00028 
00029 #include <gazebo/physics/World.hh>
00030 #include <gazebo/physics/HingeJoint.hh>
00031 #include <gazebo/sensors/Sensor.hh>
00032 #include <sdf/sdf.hh>
00033 #include <sdf/Param.hh>
00034 #include <gazebo/common/Exception.hh>
00035 #include <gazebo/sensors/RaySensor.hh>
00036 #include <gazebo/sensors/SensorTypes.hh>
00037 #include <gazebo/transport/Node.hh>
00038 
00039 #include <geometry_msgs/Point32.h>
00040 #include <sensor_msgs/ChannelFloat32.h>
00041 
00042 #include <tf/tf.h>
00043 
00044 #define EPSILON_DIFF 0.000001
00045 
00046 namespace gazebo
00047 {
00048 // Register this plugin with the simulator
00049 GZ_REGISTER_SENSOR_PLUGIN(GazeboRosBlockLaser)
00050 
00051 
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 # if GAZEBO_MAJOR_VERSION >= 7
00083   std::string worldName = _parent->WorldName();
00084 # else
00085   std::string worldName = _parent->GetWorldName();
00086 # endif
00087 
00088   this->world_ = physics::get_world(worldName);
00089 
00090   last_update_time_ = this->world_->GetSimTime();
00091 
00092   this->node_ = transport::NodePtr(new transport::Node());
00093   this->node_->Init(worldName);
00094 
00095   GAZEBO_SENSORS_USING_DYNAMIC_POINTER_CAST;
00096   this->parent_ray_sensor_ = dynamic_pointer_cast<sensors::RaySensor>(this->parent_sensor_);
00097 
00098   if (!this->parent_ray_sensor_)
00099     gzthrow("GazeboRosBlockLaser controller requires a Ray Sensor as its parent");
00100 
00101   this->robot_namespace_ = "";
00102   if (_sdf->HasElement("robotNamespace"))
00103     this->robot_namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>() + "/";
00104 
00105   if (!_sdf->HasElement("frameName"))
00106   {
00107     ROS_INFO("Block laser plugin missing <frameName>, defaults to /world");
00108     this->frame_name_ = "/world";
00109   }
00110   else
00111     this->frame_name_ = _sdf->GetElement("frameName")->Get<std::string>();
00112 
00113   if (!_sdf->HasElement("topicName"))
00114   {
00115     ROS_INFO("Block laser plugin missing <topicName>, defaults to /world");
00116     this->topic_name_ = "/world";
00117   }
00118   else
00119     this->topic_name_ = _sdf->GetElement("topicName")->Get<std::string>();
00120 
00121   if (!_sdf->HasElement("gaussianNoise"))
00122   {
00123     ROS_INFO("Block laser plugin missing <gaussianNoise>, defaults to 0.0");
00124     this->gaussian_noise_ = 0;
00125   }
00126   else
00127     this->gaussian_noise_ = _sdf->GetElement("gaussianNoise")->Get<double>();
00128 
00129   if (!_sdf->HasElement("hokuyoMinIntensity"))
00130   {
00131     ROS_INFO("Block laser plugin missing <hokuyoMinIntensity>, defaults to 101");
00132     this->hokuyo_min_intensity_ = 101;
00133   }
00134   else
00135     this->hokuyo_min_intensity_ = _sdf->GetElement("hokuyoMinIntensity")->Get<double>();
00136 
00137   ROS_INFO("INFO: gazebo_ros_laser plugin should set minimum intensity to %f due to cutoff in hokuyo filters." , this->hokuyo_min_intensity_);
00138 
00139   if (!_sdf->HasElement("updateRate"))
00140   {
00141     ROS_INFO("Block laser plugin missing <updateRate>, defaults to 0");
00142     this->update_rate_ = 0;
00143   }
00144   else
00145     this->update_rate_ = _sdf->GetElement("updateRate")->Get<double>();
00146   // FIXME:  update the update_rate_
00147 
00148 
00149   this->laser_connect_count_ = 0;
00150 
00151   // Make sure the ROS node for Gazebo has already been initialized
00152   if (!ros::isInitialized())
00153   {
00154     ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. "
00155       << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
00156     return;
00157   }
00158 
00159   this->rosnode_ = new ros::NodeHandle(this->robot_namespace_);
00160 
00161   // resolve tf prefix
00162   std::string prefix;
00163   this->rosnode_->getParam(std::string("tf_prefix"), prefix);
00164   this->frame_name_ = tf::resolve(prefix, this->frame_name_);
00165 
00166   // set size of cloud message, starts at 0!! FIXME: not necessary
00167   this->cloud_msg_.points.clear();
00168   this->cloud_msg_.channels.clear();
00169   this->cloud_msg_.channels.push_back(sensor_msgs::ChannelFloat32());
00170 
00171   if (this->topic_name_ != "")
00172   {
00173     // Custom Callback Queue
00174     ros::AdvertiseOptions ao = ros::AdvertiseOptions::create<sensor_msgs::PointCloud>(
00175       this->topic_name_,1,
00176       boost::bind( &GazeboRosBlockLaser::LaserConnect,this),
00177       boost::bind( &GazeboRosBlockLaser::LaserDisconnect,this), ros::VoidPtr(), &this->laser_queue_);
00178     this->pub_ = this->rosnode_->advertise(ao);
00179   }
00180 
00181 
00182   // Initialize the controller
00183 
00184   // sensor generation off by default
00185   this->parent_ray_sensor_->SetActive(false);
00186   // start custom queue for laser
00187   this->callback_laser_queue_thread_ = boost::thread( boost::bind( &GazeboRosBlockLaser::LaserQueueThread,this ) );
00188 
00189 }
00190 
00192 // Increment count
00193 void GazeboRosBlockLaser::LaserConnect()
00194 {
00195   this->laser_connect_count_++;
00196   this->parent_ray_sensor_->SetActive(true);
00197 }
00199 // Decrement count
00200 void GazeboRosBlockLaser::LaserDisconnect()
00201 {
00202   this->laser_connect_count_--;
00203 
00204   if (this->laser_connect_count_ == 0)
00205     this->parent_ray_sensor_->SetActive(false);
00206 }
00207 
00209 // Update the controller
00210 void GazeboRosBlockLaser::OnNewLaserScans()
00211 {
00212   if (this->topic_name_ != "")
00213   {
00214 # if GAZEBO_MAJOR_VERSION >= 7
00215     common::Time sensor_update_time = this->parent_sensor_->LastUpdateTime();
00216 # else
00217     common::Time sensor_update_time = this->parent_sensor_->GetLastUpdateTime();
00218 # endif
00219     if (last_update_time_ < sensor_update_time)
00220     {
00221       this->PutLaserData(sensor_update_time);
00222       last_update_time_ = sensor_update_time;
00223     }
00224   }
00225   else
00226   {
00227     ROS_INFO("gazebo_ros_block_laser topic name not set");
00228   }
00229 }
00230 
00232 // Put laser data to the interface
00233 void GazeboRosBlockLaser::PutLaserData(common::Time &_updateTime)
00234 {
00235   int i, hja, hjb;
00236   int j, vja, vjb;
00237   double vb, hb;
00238   int    j1, j2, j3, j4; // four corners indices
00239   double r1, r2, r3, r4, r; // four corner values + interpolated range
00240   double intensity;
00241 
00242   this->parent_ray_sensor_->SetActive(false);
00243 
00244 #if GAZEBO_MAJOR_VERSION >= 6
00245   auto maxAngle = this->parent_ray_sensor_->AngleMax();
00246   auto minAngle = this->parent_ray_sensor_->AngleMin();
00247 #else
00248   math::Angle maxAngle = this->parent_ray_sensor_->GetAngleMax();
00249   math::Angle minAngle = this->parent_ray_sensor_->GetAngleMin();
00250 #endif
00251 
00252 # if GAZEBO_MAJOR_VERSION >= 7
00253   double maxRange = this->parent_ray_sensor_->RangeMax();
00254   double minRange = this->parent_ray_sensor_->RangeMin();
00255   int rayCount = this->parent_ray_sensor_->RayCount();
00256   int rangeCount = this->parent_ray_sensor_->RangeCount();
00257 # else
00258   double maxRange = this->parent_ray_sensor_->GetRangeMax();
00259   double minRange = this->parent_ray_sensor_->GetRangeMin();
00260   int rayCount = this->parent_ray_sensor_->GetRayCount();
00261   int rangeCount = this->parent_ray_sensor_->GetRangeCount();
00262 # endif
00263 
00264 #if GAZEBO_MAJOR_VERSION >= 7
00265   int verticalRayCount = this->parent_ray_sensor_->VerticalRayCount();
00266   int verticalRangeCount = this->parent_ray_sensor_->VerticalRangeCount();
00267 # else
00268   int verticalRayCount = this->parent_ray_sensor_->GetVerticalRayCount();
00269   int verticalRangeCount = this->parent_ray_sensor_->GetVerticalRangeCount();
00270 # endif
00271 
00272 #if GAZEBO_MAJOR_VERSION >= 6
00273   auto verticalMaxAngle = this->parent_ray_sensor_->VerticalAngleMax();
00274   auto verticalMinAngle = this->parent_ray_sensor_->VerticalAngleMin();
00275 #else
00276   math::Angle verticalMaxAngle = this->parent_ray_sensor_->GetVerticalAngleMax();
00277   math::Angle verticalMinAngle = this->parent_ray_sensor_->GetVerticalAngleMin();
00278 #endif
00279 
00280   double yDiff = maxAngle.Radian() - minAngle.Radian();
00281   double pDiff = verticalMaxAngle.Radian() - verticalMinAngle.Radian();
00282 
00283 
00284   // set size of cloud message everytime!
00285   //int r_size = rangeCount * verticalRangeCount;
00286   this->cloud_msg_.points.clear();
00287   this->cloud_msg_.channels.clear();
00288   this->cloud_msg_.channels.push_back(sensor_msgs::ChannelFloat32());
00289 
00290   /***************************************************************/
00291   /*                                                             */
00292   /*  point scan from laser                                      */
00293   /*                                                             */
00294   /***************************************************************/
00295   boost::mutex::scoped_lock sclock(this->lock);
00296   // Add Frame Name
00297   this->cloud_msg_.header.frame_id = this->frame_name_;
00298   this->cloud_msg_.header.stamp.sec = _updateTime.sec;
00299   this->cloud_msg_.header.stamp.nsec = _updateTime.nsec;
00300 
00301   for (j = 0; j<verticalRangeCount; j++)
00302   {
00303     // interpolating in vertical direction
00304     vb = (verticalRangeCount == 1) ? 0 : (double) j * (verticalRayCount - 1) / (verticalRangeCount - 1);
00305     vja = (int) floor(vb);
00306     vjb = std::min(vja + 1, verticalRayCount - 1);
00307     vb = vb - floor(vb); // fraction from min
00308 
00309     assert(vja >= 0 && vja < verticalRayCount);
00310     assert(vjb >= 0 && vjb < verticalRayCount);
00311 
00312     for (i = 0; i<rangeCount; i++)
00313     {
00314       // Interpolate the range readings from the rays in horizontal direction
00315       hb = (rangeCount == 1)? 0 : (double) i * (rayCount - 1) / (rangeCount - 1);
00316       hja = (int) floor(hb);
00317       hjb = std::min(hja + 1, rayCount - 1);
00318       hb = hb - floor(hb); // fraction from min
00319 
00320       assert(hja >= 0 && hja < rayCount);
00321       assert(hjb >= 0 && hjb < rayCount);
00322 
00323       // indices of 4 corners
00324       j1 = hja + vja * rayCount;
00325       j2 = hjb + vja * rayCount;
00326       j3 = hja + vjb * rayCount;
00327       j4 = hjb + vjb * rayCount;
00328       // range readings of 4 corners
00329 # if GAZEBO_MAJOR_VERSION >= 7
00330       r1 = std::min(this->parent_ray_sensor_->LaserShape()->GetRange(j1) , maxRange-minRange);
00331       r2 = std::min(this->parent_ray_sensor_->LaserShape()->GetRange(j2) , maxRange-minRange);
00332       r3 = std::min(this->parent_ray_sensor_->LaserShape()->GetRange(j3) , maxRange-minRange);
00333       r4 = std::min(this->parent_ray_sensor_->LaserShape()->GetRange(j4) , maxRange-minRange);
00334 # else
00335       r1 = std::min(this->parent_ray_sensor_->GetLaserShape()->GetRange(j1) , maxRange-minRange);
00336       r2 = std::min(this->parent_ray_sensor_->GetLaserShape()->GetRange(j2) , maxRange-minRange);
00337       r3 = std::min(this->parent_ray_sensor_->GetLaserShape()->GetRange(j3) , maxRange-minRange);
00338       r4 = std::min(this->parent_ray_sensor_->GetLaserShape()->GetRange(j4) , maxRange-minRange);
00339 # endif
00340 
00341       // Range is linear interpolation if values are close,
00342       // and min if they are very different
00343       r = (1-vb)*((1 - hb) * r1 + hb * r2)
00344          +   vb *((1 - hb) * r3 + hb * r4);
00345 
00346       // Intensity is averaged
00347 #if GAZEBO_MAJOR_VERSION >= 7
00348       intensity = 0.25*(this->parent_ray_sensor_->LaserShape()->GetRetro(j1) +
00349                         this->parent_ray_sensor_->LaserShape()->GetRetro(j2) +
00350                         this->parent_ray_sensor_->LaserShape()->GetRetro(j3) +
00351                         this->parent_ray_sensor_->LaserShape()->GetRetro(j4));
00352 # else
00353       intensity = 0.25*(this->parent_ray_sensor_->GetLaserShape()->GetRetro(j1) +
00354                         this->parent_ray_sensor_->GetLaserShape()->GetRetro(j2) +
00355                         this->parent_ray_sensor_->GetLaserShape()->GetRetro(j3) +
00356                         this->parent_ray_sensor_->GetLaserShape()->GetRetro(j4));
00357 
00358 # endif
00359 
00360       // std::cout << " block debug "
00361       //           << "  ij("<<i<<","<<j<<")"
00362       //           << "  j1234("<<j1<<","<<j2<<","<<j3<<","<<j4<<")"
00363       //           << "  r1234("<<r1<<","<<r2<<","<<r3<<","<<r4<<")"
00364       //           << std::endl;
00365 
00366       // get angles of ray to get xyz for point
00367       double yAngle = 0.5*(hja+hjb) * yDiff / (rayCount -1) + minAngle.Radian();
00368       double pAngle = 0.5*(vja+vjb) * pDiff / (verticalRayCount -1) + verticalMinAngle.Radian();
00369 
00370       /***************************************************************/
00371       /*                                                             */
00372       /*  point scan from laser                                      */
00373       /*                                                             */
00374       /***************************************************************/
00375       //compare 2 doubles
00376       double diffRange = maxRange - minRange;
00377       double diff  = diffRange - r;
00378       if (fabs(diff) < EPSILON_DIFF)
00379       {
00380         // no noise if at max range
00381         geometry_msgs::Point32 point;
00382         //pAngle is rotated by yAngle:
00383         point.x = r * cos(pAngle) * cos(yAngle);
00384         point.y = r * cos(pAngle) * sin(yAngle);
00385         point.z = -r * sin(pAngle);
00386 
00387         this->cloud_msg_.points.push_back(point);
00388       }
00389       else
00390       {
00391         geometry_msgs::Point32 point;
00392         //pAngle is rotated by yAngle:
00393         point.x = r * cos(pAngle) * cos(yAngle) + this->GaussianKernel(0,this->gaussian_noise_);
00394         point.y = r * cos(pAngle) * sin(yAngle) + this->GaussianKernel(0,this->gaussian_noise_);
00395         point.z = -r * sin(pAngle) + this->GaussianKernel(0,this->gaussian_noise_);
00396         this->cloud_msg_.points.push_back(point);
00397       } // only 1 channel
00398 
00399       this->cloud_msg_.channels[0].values.push_back(intensity + this->GaussianKernel(0,this->gaussian_noise_)) ;
00400     }
00401   }
00402   this->parent_ray_sensor_->SetActive(true);
00403 
00404   // send data out via ros message
00405   this->pub_.publish(this->cloud_msg_);
00406 
00407 
00408 
00409 }
00410 
00411 
00413 // Utility for adding noise
00414 double GazeboRosBlockLaser::GaussianKernel(double mu,double sigma)
00415 {
00416   // using Box-Muller transform to generate two independent standard normally disbributed normal variables
00417   // see wikipedia
00418   double U = (double)rand()/(double)RAND_MAX; // normalized uniform random variable
00419   double V = (double)rand()/(double)RAND_MAX; // normalized uniform random variable
00420   double X = sqrt(-2.0 * ::log(U)) * cos( 2.0*M_PI * V);
00421   //double Y = sqrt(-2.0 * ::log(U)) * sin( 2.0*M_PI * V); // the other indep. normal variable
00422   // we'll just use X
00423   // scale to our mu and sigma
00424   X = sigma * X + mu;
00425   return X;
00426 }
00427 
00428 // Custom Callback Queue
00430 // custom callback queue thread
00431 void GazeboRosBlockLaser::LaserQueueThread()
00432 {
00433   static const double timeout = 0.01;
00434 
00435   while (this->rosnode_->ok())
00436   {
00437     this->laser_queue_.callAvailable(ros::WallDuration(timeout));
00438   }
00439 }
00440 
00441 void GazeboRosBlockLaser::OnStats( const boost::shared_ptr<msgs::WorldStatistics const> &_msg)
00442 {
00443   this->sim_time_  = msgs::Convert( _msg->sim_time() );
00444 
00445   math::Pose pose;
00446   pose.pos.x = 0.5*sin(0.01*this->sim_time_.Double());
00447   gzdbg << "plugin simTime [" << this->sim_time_.Double() << "] update pose [" << pose.pos.x << "]\n";
00448 }
00449 
00450 
00451 }
00452 


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