gazebo_ros_camera_utils.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: GazeboRosCameraUtils plugin for simulating camera_s in Gazebo
00024    Author: John Hsu
00025    Date: 24 Sept 2008
00026    SVN info: $Id$
00027  @htmlinclude manifest.html
00028  @b GazeboRosCameraUtils 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 <gazebo_plugins/gazebo_ros_camera_utils.h>
00037 
00038 #include "physics/World.hh"
00039 #include "physics/HingeJoint.hh"
00040 #include "sensors/Sensor.hh"
00041 #include "sdf/interface/SDF.hh"
00042 #include "sdf/interface/Param.hh"
00043 #include "common/Exception.hh"
00044 #include "sensors/CameraSensor.hh"
00045 #include "sensors/SensorTypes.hh"
00046 #include "rendering/Camera.hh"
00047 
00048 #include "sensor_msgs/Image.h"
00049 #include "sensor_msgs/fill_image.h"
00050 #include "image_transport/image_transport.h"
00051 
00052 #include <geometry_msgs/Point32.h>
00053 #include <sensor_msgs/ChannelFloat32.h>
00054 
00055 #include "tf/tf.h"
00056 
00057 namespace gazebo
00058 {
00059 
00061 // Constructor
00062 GazeboRosCameraUtils::GazeboRosCameraUtils()
00063 {
00064   this->image_connect_count_ = 0;
00065   this->info_connect_count_ = 0;
00066 
00067   // maintain for one more release for backwards compatibility with pr2_gazebo_plugins
00068   this->imageConnectCount = this->image_connect_count_;
00069   this->infoConnectCount = this->info_connect_count_;
00070 
00071   this->last_update_time_ = common::Time(0);
00072   this->last_info_update_time_ = common::Time(0);
00073 }
00074 
00075 void GazeboRosCameraUtils::configCallback(gazebo_plugins::GazeboRosCameraConfig &config, uint32_t level)
00076 {
00077   ROS_INFO("Reconfigure request for the gazebo ros camera_: %s. New rate: %.2f", this->camera_name_.c_str(), config.imager_rate);
00078   this->parentSensor_->SetUpdateRate(config.imager_rate);
00079 }
00080 
00082 // Destructor
00083 GazeboRosCameraUtils::~GazeboRosCameraUtils()
00084 {
00085   this->parentSensor_->SetActive(false);
00086   this->rosnode_->shutdown();
00087   this->camera_queue_.clear();
00088   this->camera_queue_.disable();
00089   this->callback_queue_thread_.join();
00090   delete this->rosnode_;
00091 }
00092 
00094 // Load the controller
00095 void GazeboRosCameraUtils::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf)
00096 {
00097   // Get the world name.
00098   std::string world_name = _parent->GetWorldName();
00099 
00100   // Get the world_
00101   this->world_ = physics::get_world(world_name);
00102 
00103   // maintain for one more release for backwards compatibility with pr2_gazebo_plugins
00104   this->world = this->world_;
00105 
00106   this->robot_namespace_ = "";
00107   if (_sdf->HasElement("robotNamespace"))
00108     this->robot_namespace_ = _sdf->GetElement("robotNamespace")->GetValueString() + "/";
00109 
00110   this->image_topic_name_ = "image_raw";
00111   if (_sdf->GetElement("imageTopicName"))
00112     this->image_topic_name_ = _sdf->GetElement("imageTopicName")->GetValueString();
00113 
00114   this->camera_info_topic_name_ = "camera_info";
00115   if (_sdf->HasElement("cameraInfoTopicName"))
00116     this->camera_info_topic_name_ = _sdf->GetElement("cameraInfoTopicName")->GetValueString();
00117 
00118   if (!_sdf->HasElement("cameraName"))
00119     ROS_INFO("Camera plugin missing <cameraName>, default to empty");
00120   else
00121     this->camera_name_ = _sdf->GetElement("cameraName")->GetValueString();
00122 
00123   if (!_sdf->HasElement("frameName"))
00124     ROS_INFO("Camera plugin missing <frameName>, defaults to /world");
00125   else
00126     this->frame_name_ = _sdf->GetElement("frameName")->GetValueString();
00127 
00128   if (!_sdf->GetElement("updateRate"))
00129   {
00130     ROS_INFO("Camera plugin missing <updateRate>, defaults to 0");
00131     this->update_rate_ = 0;
00132   }
00133   else
00134     this->update_rate_ = _sdf->GetElement("updateRate")->GetValueDouble();
00135 
00136   if (!_sdf->GetElement("CxPrime"))
00137   {
00138     ROS_INFO("Camera plugin missing <CxPrime>, defaults to 0");
00139     this->cx_prime_ = 0;
00140   }
00141   else
00142     this->cx_prime_ = _sdf->GetElement("CxPrime")->GetValueDouble();
00143 
00144   if (!_sdf->HasElement("Cx"))
00145   {
00146     ROS_INFO("Camera plugin missing <Cx>, defaults to 0");
00147     this->cx_= 0;
00148   }
00149   else
00150     this->cx_ = _sdf->GetElement("Cx")->GetValueDouble();
00151 
00152   if (!_sdf->HasElement("Cy"))
00153   {
00154     ROS_INFO("Camera plugin missing <Cy>, defaults to 0");
00155     this->cy_= 0;
00156   }
00157   else
00158     this->cy_ = _sdf->GetElement("Cy")->GetValueDouble();
00159 
00160   if (!_sdf->HasElement("focalLength"))
00161   {
00162     ROS_INFO("Camera plugin missing <focalLength>, defaults to 0");
00163     this->focal_length_= 0;
00164   }
00165   else
00166     this->focal_length_ = _sdf->GetElement("focalLength")->GetValueDouble();
00167 
00168   if (!_sdf->HasElement("hackBaseline"))
00169   {
00170     ROS_INFO("Camera plugin missing <hackBaseline>, defaults to 0");
00171     this->hack_baseline_= 0;
00172   }
00173   else
00174     this->hack_baseline_ = _sdf->GetElement("hackBaseline")->GetValueDouble();
00175 
00176   if (!_sdf->HasElement("distortionK1"))
00177   {
00178     ROS_INFO("Camera plugin missing <distortionK1>, defaults to 0");
00179     this->distortion_k1_= 0;
00180   }
00181   else
00182     this->distortion_k1_ = _sdf->GetElement("distortionK1")->GetValueDouble();
00183 
00184   if (!_sdf->HasElement("distortionK2"))
00185   {
00186     ROS_INFO("Camera plugin missing <distortionK2>, defaults to 0");
00187     this->distortion_k2_= 0;
00188   }
00189   else
00190     this->distortion_k2_ = _sdf->GetElement("distortionK2")->GetValueDouble();
00191 
00192   if (!_sdf->HasElement("distortionK3"))
00193   {
00194     ROS_INFO("Camera plugin missing <distortionK3>, defaults to 0");
00195     this->distortion_k3_= 0;
00196   }
00197   else
00198     this->distortion_k3_ = _sdf->GetElement("distortionK3")->GetValueDouble();
00199 
00200   if (!_sdf->HasElement("distortionT1"))
00201   {
00202     ROS_INFO("Camera plugin missing <distortionT1>, defaults to 0");
00203     this->distortion_t1_= 0;
00204   }
00205   else
00206     this->distortion_t1_ = _sdf->GetElement("distortionT1")->GetValueDouble();
00207 
00208   if (!_sdf->HasElement("distortionT2"))
00209   {
00210     ROS_INFO("Camera plugin missing <distortionT2>, defaults to 0");
00211     this->distortion_t2_= 0;
00212   }
00213   else
00214     this->distortion_t2_ = _sdf->GetElement("distortionT2")->GetValueDouble();
00215 
00216   if ((this->distortion_k1_ != 0.0) || (this->distortion_k2_ != 0.0) ||
00217       (this->distortion_k3_ != 0.0) || (this->distortion_t1_ != 0.0) ||
00218       (this->distortion_t2_ != 0.0))
00219   {
00220     ROS_WARN("gazebo_ros_camera_ simulation does not support non-zero distortion parameters right now, your simulation maybe wrong.");
00221   }
00222 
00223 
00224   // Setup ROS
00225   if (!ros::isInitialized())
00226   {
00227     int argc = 0;
00228     char** argv = NULL;
00229     ros::init( argc, argv, "gazebo",
00230                ros::init_options::NoSigintHandler |
00231                ros::init_options::AnonymousName );
00232   }
00233 
00234   this->rosnode_ = new ros::NodeHandle(this->robot_namespace_+"/"+this->camera_name_);
00235 
00236   this->itnode_ = new image_transport::ImageTransport(*this->rosnode_);
00237 
00238   // resolve tf prefix
00239   std::string prefix;
00240   this->rosnode_->getParam(std::string("tf_prefix"), prefix);
00241   this->frame_name_ = tf::resolve(prefix, this->frame_name_);
00242 
00243   if (!this->camera_name_.empty())
00244   {
00245     dyn_srv_ = new dynamic_reconfigure::Server<gazebo_plugins::GazeboRosCameraConfig>(*this->rosnode_);
00246     dynamic_reconfigure::Server<gazebo_plugins::GazeboRosCameraConfig>::CallbackType f = boost::bind(&GazeboRosCameraUtils::configCallback, this, _1, _2);
00247     dyn_srv_->setCallback(f);
00248   }
00249   else
00250   {
00251     ROS_WARN("dynamic reconfigure is not enabled for this image topic [%s] becuase <cameraName> is not specified",this->image_topic_name_.c_str());
00252   }
00253 
00254   this->image_pub_ = this->itnode_->advertise(
00255     this->image_topic_name_,1,
00256     boost::bind( &GazeboRosCameraUtils::ImageConnect,this),
00257     boost::bind( &GazeboRosCameraUtils::ImageDisconnect,this),
00258     ros::VoidPtr(), &this->camera_queue_);
00259 
00260   ros::AdvertiseOptions camera_info_ao =
00261     ros::AdvertiseOptions::create<sensor_msgs::CameraInfo>(
00262         this->camera_info_topic_name_,1,
00263         boost::bind( &GazeboRosCameraUtils::InfoConnect,this),
00264         boost::bind( &GazeboRosCameraUtils::InfoDisconnect,this),
00265         ros::VoidPtr(), &this->camera_queue_);
00266   this->camera_info_pub_ = this->rosnode_->advertise(camera_info_ao);
00267 
00268   ros::SubscribeOptions zoom_so =
00269     ros::SubscribeOptions::create<std_msgs::Float64>(
00270         "set_hfov",1,
00271         boost::bind( &GazeboRosCameraUtils::SetHFOV,this,_1),
00272         ros::VoidPtr(), &this->camera_queue_);
00273   this->cameraHFOVSubscriber_ = this->rosnode_->subscribe(zoom_so);
00274 
00275   ros::SubscribeOptions rate_so =
00276     ros::SubscribeOptions::create<std_msgs::Float64>(
00277         "set_update_rate",1,
00278         boost::bind( &GazeboRosCameraUtils::SetUpdateRate,this,_1),
00279         ros::VoidPtr(), &this->camera_queue_);
00280   this->cameraUpdateRateSubscriber_ = this->rosnode_->subscribe(rate_so);
00281 
00282   this->Init();
00283 }
00284 
00286 // Increment count
00287 void GazeboRosCameraUtils::InfoConnect()
00288 {
00289   this->info_connect_count_++;
00290   // maintain for one more release for backwards compatibility with pr2_gazebo_plugins
00291   this->infoConnectCount = this->info_connect_count_;
00292 }
00294 // Decrement count
00295 void GazeboRosCameraUtils::InfoDisconnect()
00296 {
00297   this->info_connect_count_--;
00298   // maintain for one more release for backwards compatibility with pr2_gazebo_plugins
00299   this->infoConnectCount = this->info_connect_count_;
00300 }
00301 
00303 // Set Horizontal Field of View
00304 void GazeboRosCameraUtils::SetHFOV(const std_msgs::Float64::ConstPtr& hfov)
00305 {
00306   this->camera_->SetHFOV(hfov->data);
00307 }
00308 
00310 // Set Update Rate
00311 void GazeboRosCameraUtils::SetUpdateRate(const std_msgs::Float64::ConstPtr& update_rate)
00312 {
00313   this->parentSensor_->SetUpdateRate(update_rate->data);
00314 }
00315 
00317 // Increment count
00318 void GazeboRosCameraUtils::ImageConnect()
00319 {
00320   this->image_connect_count_++;
00321   // maintain for one more release for backwards compatibility with pr2_gazebo_plugins
00322   this->imageConnectCount = this->image_connect_count_;
00323   this->parentSensor_->SetActive(true);
00324 }
00326 // Decrement count
00327 void GazeboRosCameraUtils::ImageDisconnect()
00328 {
00329   this->image_connect_count_--;
00330   // maintain for one more release for backwards compatibility with pr2_gazebo_plugins
00331   this->imageConnectCount = this->image_connect_count_;
00332   if (this->image_connect_count_ <= 0)
00333     this->parentSensor_->SetActive(false);
00334 }
00335 
00337 // Initialize the controller
00338 void GazeboRosCameraUtils::Init()
00339 {
00340   // set parent sensor update rate
00341   this->parentSensor_->SetUpdateRate(this->update_rate_);
00342 
00343   // prepare to throttle this plugin at the same rate
00344   // ideally, we should invoke a plugin update when the sensor updates,
00345   // have to think about how to do that properly later
00346   if (this->update_rate_ > 0.0)
00347     this->update_period_ = 1.0/this->update_rate_;
00348   else
00349     this->update_period_ = 0.0;
00350 
00351 
00352   // sensor generation off by default
00353   this->parentSensor_->SetActive(false);
00354 
00355   // set buffer size
00356   if (this->format_ == "L8")
00357   {
00358     this->type_ = sensor_msgs::image_encodings::MONO8;
00359     this->skip_ = 1;
00360   }
00361   else if (this->format_ == "R8G8B8")
00362   {
00363     this->type_ = sensor_msgs::image_encodings::RGB8;
00364     this->skip_ = 3;
00365   }
00366   else if (this->format_ == "B8G8R8")
00367   {
00368     this->type_ = sensor_msgs::image_encodings::BGR8;
00369     this->skip_ = 3;
00370   }
00371   else if (this->format_ == "BAYER_RGGB8")
00372   {
00373     ROS_INFO("bayer simulation maybe computationally expensive.");
00374     this->type_ = sensor_msgs::image_encodings::BAYER_RGGB8;
00375     this->skip_ = 1;
00376   }
00377   else if (this->format_ == "BAYER_BGGR8")
00378   {
00379     ROS_INFO("bayer simulation maybe computationally expensive.");
00380     this->type_ = sensor_msgs::image_encodings::BAYER_BGGR8;
00381     this->skip_ = 1;
00382   }
00383   else if (this->format_ == "BAYER_GBRG8")
00384   {
00385     ROS_INFO("bayer simulation maybe computationally expensive.");
00386     this->type_ = sensor_msgs::image_encodings::BAYER_GBRG8;
00387     this->skip_ = 1;
00388   }
00389   else if (this->format_ == "BAYER_GRBG8")
00390   {
00391     ROS_INFO("bayer simulation maybe computationally expensive.");
00392     this->type_ = sensor_msgs::image_encodings::BAYER_GRBG8;
00393     this->skip_ = 1;
00394   }
00395   else
00396   {
00397     ROS_ERROR("Unsupported Gazebo ImageFormat\n");
00398     this->type_ = sensor_msgs::image_encodings::BGR8;
00399     this->skip_ = 3;
00400   }
00401 
00403   if (this->cx_prime_ == 0)
00404     this->cx_prime_ = ((double)this->width_+1.0) /2.0;
00405   if (this->cx_ == 0)
00406     this->cx_ = ((double)this->width_+1.0) /2.0;
00407   if (this->cy_ == 0)
00408     this->cy_ = ((double)this->height_+1.0) /2.0;
00409 
00410 
00411   double computed_focal_length = ((double)this->width_) / (2.0 *tan(this->camera_->GetHFOV().GetAsRadian()/2.0));
00412   if (this->focal_length_ == 0)
00413   {
00414     this->focal_length_ = computed_focal_length;
00415   }
00416   else
00417   {
00418     if (!gazebo::math::equal(this->focal_length_, computed_focal_length)) // check against float precision
00419     {
00420       ROS_WARN("The <focal_length>[%f] you have provided for camera_ [%s] is inconsistent with specified image_width [%d] and HFOV [%f].   Please double check to see that focal_length = width_ / (2.0 * tan( HFOV/2.0 )), the explected focal_lengtth value is [%f], please update your camera_ model description accordingly.",
00421                 this->focal_length_,this->parentSensor_->GetName().c_str(),this->width_,this->camera_->GetHFOV().GetAsRadian(),
00422                 computed_focal_length);
00423     }
00424   }
00425 
00426 
00427   // start custom queue for camera_
00428   this->callback_queue_thread_ = boost::thread( boost::bind( &GazeboRosCameraUtils::CameraQueueThread,this ) );
00429 }
00430 
00432 // Put camera_ data to the interface
00433 void GazeboRosCameraUtils::PutCameraData(const unsigned char *_src, common::Time &last_update_time)
00434 {
00435   this->sensor_update_time_ = last_update_time;
00436   this->PutCameraData(_src);
00437 }
00438 
00439 void GazeboRosCameraUtils::PutCameraData(const unsigned char *_src)
00440 {
00441   this->lock_.lock();
00442 
00443   // copy data into image
00444   this->image_msg_.header.frame_id = this->frame_name_;
00445   this->image_msg_.header.stamp.sec = this->sensor_update_time_.sec;
00446   this->image_msg_.header.stamp.nsec = this->sensor_update_time_.nsec;
00447 
00449   if (this->image_connect_count_ > 0)
00450   {
00451     // copy from src to image_msg_
00452     fillImage(this->image_msg_,
00453         this->type_,
00454         this->height_,
00455         this->width_,
00456         this->skip_*this->width_,
00457         (void*)_src );
00458 
00459     // publish to ros
00460     this->image_pub_.publish(this->image_msg_);
00461   }
00462 
00463   this->lock_.unlock();
00464 }
00465 
00466 
00467 
00469 // Put camera_ data to the interface
00470 void GazeboRosCameraUtils::PublishCameraInfo(common::Time &last_update_time)
00471 {
00472   this->sensor_update_time_ = last_update_time;
00473   this->PublishCameraInfo();
00474 }
00475 
00476 void GazeboRosCameraUtils::PublishCameraInfo()
00477 {
00478   if (this->info_connect_count_ > 0)
00479   {
00480     this->sensor_update_time_ = this->parentSensor_->GetLastUpdateTime();
00481     common::Time cur_time = this->world_->GetSimTime();
00482     if (cur_time - this->last_info_update_time_ >= this->update_period_)
00483     {
00484       this->PublishCameraInfo(this->camera_info_pub_);
00485       this->last_info_update_time_ = cur_time;
00486     }
00487   }
00488 }
00489 
00490 void GazeboRosCameraUtils::PublishCameraInfo(ros::Publisher camera_info_publisher)
00491 {
00492   sensor_msgs::CameraInfo camera_info_msg;
00493   // fill CameraInfo
00494   camera_info_msg.header.frame_id = this->frame_name_;
00495 
00496   camera_info_msg.header.stamp.sec = this->sensor_update_time_.sec;
00497   camera_info_msg.header.stamp.nsec = this->sensor_update_time_.nsec;
00498   camera_info_msg.height = this->height_;
00499   camera_info_msg.width  = this->width_;
00500   // distortion
00501 #if ROS_VERSION_MINIMUM(1, 3, 0)
00502   camera_info_msg.distortion_model = "plumb_bob";
00503   camera_info_msg.D.resize(5);
00504 #endif
00505   camera_info_msg.D[0] = this->distortion_k1_;
00506   camera_info_msg.D[1] = this->distortion_k2_;
00507   camera_info_msg.D[2] = this->distortion_k3_;
00508   camera_info_msg.D[3] = this->distortion_t1_;
00509   camera_info_msg.D[4] = this->distortion_t2_;
00510   // original camera_ matrix
00511   camera_info_msg.K[0] = this->focal_length_;
00512   camera_info_msg.K[1] = 0.0;
00513   camera_info_msg.K[2] = this->cx_;
00514   camera_info_msg.K[3] = 0.0;
00515   camera_info_msg.K[4] = this->focal_length_;
00516   camera_info_msg.K[5] = this->cy_;
00517   camera_info_msg.K[6] = 0.0;
00518   camera_info_msg.K[7] = 0.0;
00519   camera_info_msg.K[8] = 1.0;
00520   // rectification
00521   camera_info_msg.R[0] = 1.0;
00522   camera_info_msg.R[1] = 0.0;
00523   camera_info_msg.R[2] = 0.0;
00524   camera_info_msg.R[3] = 0.0;
00525   camera_info_msg.R[4] = 1.0;
00526   camera_info_msg.R[5] = 0.0;
00527   camera_info_msg.R[6] = 0.0;
00528   camera_info_msg.R[7] = 0.0;
00529   camera_info_msg.R[8] = 1.0;
00530   // camera_ projection matrix (same as camera_ matrix due to lack of distortion/rectification) (is this generated?)
00531   camera_info_msg.P[0] = this->focal_length_;
00532   camera_info_msg.P[1] = 0.0;
00533   camera_info_msg.P[2] = this->cx_;
00534   camera_info_msg.P[3] = -this->focal_length_ * this->hack_baseline_;
00535   camera_info_msg.P[4] = 0.0;
00536   camera_info_msg.P[5] = this->focal_length_;
00537   camera_info_msg.P[6] = this->cy_;
00538   camera_info_msg.P[7] = 0.0;
00539   camera_info_msg.P[8] = 0.0;
00540   camera_info_msg.P[9] = 0.0;
00541   camera_info_msg.P[10] = 1.0;
00542   camera_info_msg.P[11] = 0.0;
00543 
00544   camera_info_publisher.publish(camera_info_msg);
00545 }
00546 
00547 
00549 // Put camera_ data to the interface
00550 void GazeboRosCameraUtils::CameraQueueThread()
00551 {
00552   static const double timeout = 0.001;
00553 
00554   while (this->rosnode_->ok())
00555   {
00557     this->PublishCameraInfo();
00558 
00560     this->camera_queue_.callAvailable(ros::WallDuration(timeout));
00561   }
00562 }
00563 
00564 }


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