gazebo_ros_ir.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: IR controller.
00023  * Author: Saeed Anwar
00024  * Date: 22 Jul 2011
00025  */
00026 
00027 #include <algorithm>
00028 #include <assert.h>
00029 
00030 #include <range_gazebo_plugin/gazebo_ros_ir.h>
00031 
00032 #include <gazebo/Sensor.hh>
00033 #include <gazebo/Global.hh>
00034 #include <gazebo/XMLConfig.hh>
00035 #include <gazebo/HingeJoint.hh>
00036 #include <gazebo/World.hh>
00037 #include <gazebo/Simulator.hh>
00038 #include <gazebo/gazebo.h>
00039 #include <gazebo/GazeboError.hh>
00040 #include <gazebo/ControllerFactory.hh>
00041 #include <gazebo/IRSensor.hh>
00042 #include <gazebo/gz.h>
00043 
00044 namespace gazebo
00045 {
00046 
00047 GZ_REGISTER_DYNAMIC_CONTROLLER("gazebo_ros_ir", GazeboRosIr);
00048 
00050 // Constructor
00051 GazeboRosIr::GazeboRosIr(Entity *parent)
00052     : Controller(parent)
00053 
00054 {
00055   this->myParent = dynamic_cast<IRSensor*>(this->parent);
00056   if (!this->myParent)
00057     gzthrow("GazeboRosIr controller requires a IRSensor as its parent");
00058 
00059   Param::Begin(&this->parameters);
00060   this->robotNamespaceP = new ParamT<std::string>("robotNamespace", "/", 0);
00061   this->gaussianNoiseP = new ParamT<double>("gaussianNoise", 0.0, 0);
00062   this->topicNameP = new ParamT<std::string>("topicName", "", 1);
00063   this->deprecatedTopicNameP = new ParamT<std::string>("deprecatedTopicName", "", 0);
00064   this->frameNameP = new ParamT<std::string>("frameName", "default_gazebo_ros_ir_frame", 0);
00065   this->maxRangeP = new ParamT<double>("maxRange", 1.45, 0);
00066   this->minRangeP = new ParamT<double>("minRange", 0.1, 0);
00067   this->fovP = new ParamT<double>("fov", 0.4, 0);
00068  
00069   Param::End();
00070   this->irConnectCount = 0;
00071   this->deprecatedIrConnectCount = 0;
00072 }
00073 
00075 // Destructor
00076 GazeboRosIr::~GazeboRosIr()
00077 {
00078   delete this->robotNamespaceP;
00079   delete this->gaussianNoiseP;
00080   delete this->topicNameP;
00081   delete this->maxRangeP;
00082   delete this->minRangeP;
00083   delete this->fovP;
00084   delete this->deprecatedTopicNameP;
00085   delete this->frameNameP;
00086   delete this->rosnode_;
00087 }
00088 
00090 // Load the controller
00091 void GazeboRosIr::LoadChild(XMLConfigNode *node)
00092 {
00093     this->irIface = dynamic_cast<libgazebo::IRIface*>(this->GetIface("irarray"));
00094 
00095   this->robotNamespaceP->Load(node);
00096   this->robotNamespace = this->robotNamespaceP->GetValue();
00097   ROS_INFO("INFO: gazebo_ros_ir plugin loading" );
00098 
00099   if (!ros::isInitialized())
00100   {
00101     int argc = 0;
00102     char** argv = NULL;
00103     ros::init(argc,argv,"gazebo",ros::init_options::NoSigintHandler|ros::init_options::AnonymousName);
00104   }
00105 
00106   this->rosnode_ = new ros::NodeHandle(this->robotNamespace);
00107 
00108   this->topicNameP->Load(node);
00109   this->topicName = this->topicNameP->GetValue();
00110   
00111   this->deprecatedTopicNameP->Load(node);
00112   this->deprecatedTopicName = this->deprecatedTopicNameP->GetValue();
00113   
00114   this->frameNameP->Load(node);
00115   this->frameName = this->frameNameP->GetValue();
00116   
00117   this->gaussianNoiseP->Load(node);
00118   this->gaussianNoise = this->gaussianNoiseP->GetValue();
00119 
00120   this->maxRangeP->Load(node);
00121   this->maxRange = this->maxRangeP->GetValue();
00122 
00123   this->minRangeP->Load(node);
00124   this->minRange = this->minRangeP->GetValue();
00125 
00126   this->fovP->Load(node);
00127   this->fov = this->fovP->GetValue();
00128   
00129   if (this->topicName != "")
00130   {
00131 #ifdef USE_CBQ
00132     ros::AdvertiseOptions ao = ros::AdvertiseOptions::create<sensor_msgs::Range>(
00133       this->topicName,1,
00134       boost::bind( &GazeboRosIr::IrConnect,this),
00135       boost::bind( &GazeboRosIr::IrDisconnect,this), ros::VoidPtr(), &this->ir_queue_);
00136     this->pub_ = this->rosnode_->advertise(ao);
00137 #else
00138     this->pub_ = this->rosnode_->advertise<sensor_msgs::Range>(this->topicName,1,
00139       boost::bind( &GazeboRosIr::IrConnect, this),
00140       boost::bind( &GazeboRosIr::IrDisconnect, this));
00141 #endif
00142   }
00143 
00144   if (this->deprecatedTopicName != "")
00145   {
00146 #ifdef USE_CBQ
00147     ros::AdvertiseOptions ao = ros::AdvertiseOptions::create<sensor_msgs::Range>(
00148       this->deprecatedTopicName,1,
00149       boost::bind( &GazeboRosIr::DeprecatedIrConnect,this),
00150       boost::bind( &GazeboRosIr::DeprecatedIrDisconnect,this), ros::VoidPtr(), &this->ir_queue_);
00151     this->deprecated_pub_ = this->rosnode_->advertise(ao);
00152 #else
00153     this->deprecated_pub_ = this->rosnode_->advertise<sensor_msgs::Range>(this->deprecatedTopicName,1,
00154       boost::bind( &GazeboRosIr::DeprecatedIrConnect, this),
00155       boost::bind( &GazeboRosIr::DeprecatedIrDisconnect, this));
00156 #endif
00157   }
00158 }
00160 // Initialize the controller
00161 void GazeboRosIr::InitChild()
00162 {
00163   // sensor generation off by default
00164   this->myParent->SetActive(false);
00165 #ifdef USE_CBQ
00166   // start custom queue for IR
00167   this->callback_queue_thread_ = boost::thread( boost::bind( &GazeboRosIr::IrQueueThread,this ) );
00168 #endif
00169 }
00170 
00172 // Increment count
00173 void GazeboRosIr::IrConnect()
00174 {
00175   this->irConnectCount++;
00176 }
00177 
00179 // Decrement count
00180 void GazeboRosIr::IrDisconnect()
00181 {
00182   this->irConnectCount--;
00183 
00184   if (this->irConnectCount == 0 && this->deprecatedIrConnectCount == 0)
00185     this->myParent->SetActive(false);
00186 }
00187 
00189 // Increment count
00190 void GazeboRosIr::DeprecatedIrConnect()
00191 {
00192   ROS_WARN("you are subscribing to a deprecated ROS topic %s, please change your code/launch script to use new ROS topic %s",
00193            this->deprecatedTopicName.c_str(), this->topicName.c_str());
00194   this->deprecatedIrConnectCount++;
00195 }
00197 // Decrement count
00198 void GazeboRosIr::DeprecatedIrDisconnect()
00199 {
00200   this->deprecatedIrConnectCount--;
00201 
00202   if (this->irConnectCount == 0 && this->deprecatedIrConnectCount == 0)
00203     this->myParent->SetActive(false);
00204 }
00205 
00207 // Finalize the controller
00208 void GazeboRosIr::FiniChild()
00209 {
00210   this->rosnode_->shutdown();
00211   sleep(1);
00212 #ifdef USE_CBQ
00213   this->callback_queue_thread_.join();
00214 #endif
00215 }
00216 
00218 // Utility for adding noise
00219 double GazeboRosIr::GaussianKernel(double mu,double sigma)
00220 {
00221   // using Box-Muller transform to generate two independent standard normally disbributed normal variables
00222   // see wikipedia
00223   double U = (double)rand()/(double)RAND_MAX; // normalized uniform random variable
00224   double V = (double)rand()/(double)RAND_MAX; // normalized uniform random variable
00225   double X = sqrt(-2.0 * ::log(U)) * cos( 2.0*M_PI * V);
00226   //double Y = sqrt(-2.0 * ::log(U)) * sin( 2.0*M_PI * V); // the other indep. normal variable
00227   // we'll just use X
00228   // scale to our mu and sigma
00229   X = sigma * X + mu;
00230   return X;
00231 }
00232 
00233 #ifdef USE_CBQ
00234 
00235 // Put IR data to the interface
00236 void GazeboRosIr::IrQueueThread()
00237 {
00238   static const double timeout = 0.01;
00239 
00240   while (this->rosnode_->ok())
00241   {
00242     this->ir_queue_.callAvailable(ros::WallDuration(timeout));
00243   }
00244 }
00245 
00247 // Update the controller
00248 void GazeboRosIr::UpdateChild()
00249 {
00250   // as long as ros is connected, parent is active
00251   //ROS_ERROR("debug IR count %d",this->irConnectCount);
00252   if (!this->myParent->IsActive())
00253   {
00254     // do this first so there's chance for sensor to run 1 frame after activate
00255     if ((this->irConnectCount > 0 && this->topicName != "") ||
00256         (this->deprecatedIrConnectCount > 0 && this->deprecatedTopicName != ""))
00257       this->myParent->SetActive(true);
00258   }
00259   else
00260   {
00261     this->PutIRData();
00262   }
00263 }
00264 #endif
00265 
00267 // Put IR data to the interface
00268 void GazeboRosIr::PutIRData()
00269 {
00270   //double maxRange = 0.8; //this->myParent->maxRange; // this function is also not present in
00271   //double minRange = 0.1; //this->myParent->minRange;
00272   //double fieldofview = 0.4; //2*(this->myParent->maxAngle);
00273   //std::cout << maxRange << std::endl;
00274   
00275   int irCount = this->myParent->GetIRCount();
00276   //ROS_INFO("Nunber of IR: %i", irCount);
00277   
00278   double Range;
00279   float ranges[irCount];
00280   
00281   for (int i=0; i<irCount; i++)
00282   {
00283       ranges[i]=this->myParent->GetRange(i);
00284             
00285   }
00286   Range = ranges[0];
00287   for(int i = 0; i < irCount; i++)//loop through array
00288   {
00289         if(ranges[i] < Range) { Range = ranges[i]; } //find min
00290   }
00291 
00292 
00293   this->lock.lock();
00294   // Add Frame Name
00295   this->irMsg.header.frame_id = this->frameName;
00296   this->irMsg.header.stamp.sec = (Simulator::Instance()->GetSimTime()).sec;
00297   this->irMsg.header.stamp.nsec = (Simulator::Instance()->GetSimTime()).nsec;
00298 
00299   if (this->maxRange > 1.5)
00300         this->irMsg.radiation_type = sensor_msgs::Range::INFRARED;
00301   else
00302         this->irMsg.radiation_type = sensor_msgs::Range::ULTRASOUND;
00303 
00304   this->irMsg.field_of_view = this->fov;
00305   this->irMsg.min_range = this->minRange;
00306   this->irMsg.max_range = this->maxRange;
00307   this->irMsg.range = std::min(Range + this->GaussianKernel(0,this->gaussianNoise), maxRange);
00308 
00309   // send data out via ros message
00310   if (this->irConnectCount > 0 && this->topicName != "")
00311       this->pub_.publish(this->irMsg);
00312 
00313   if (this->deprecatedIrConnectCount > 0 && this->deprecatedTopicName != "")
00314       this->deprecated_pub_.publish(this->irMsg);
00315 
00316   this->lock.unlock();
00317 
00318 }
00319 
00320 }


range_gazebo_plugin
Author(s): Jose Capriles
autogenerated on Thu Jan 2 2014 11:37:21