gazebo_ros_camera_utils.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 #include <string>
00019 #include <algorithm>
00020 #include <assert.h>
00021 #include <boost/thread/thread.hpp>
00022 #include <boost/bind.hpp>
00023 
00024 #include <tf/tf.h>
00025 #include <tf/transform_listener.h>
00026 #include <sensor_msgs/Image.h>
00027 #include <sensor_msgs/fill_image.h>
00028 #include <image_transport/image_transport.h>
00029 #include <geometry_msgs/Point32.h>
00030 #include <sensor_msgs/ChannelFloat32.h>
00031 #include <camera_info_manager/camera_info_manager.h>
00032 
00033 #include <sdf/sdf.hh>
00034 
00035 #include <gazebo/physics/World.hh>
00036 #include <gazebo/physics/HingeJoint.hh>
00037 #include <gazebo/sensors/Sensor.hh>
00038 #include <gazebo/common/Exception.hh>
00039 #include <gazebo/sensors/CameraSensor.hh>
00040 #include <gazebo/sensors/SensorTypes.hh>
00041 #include <gazebo/rendering/Camera.hh>
00042 
00043 #include "gazebo_plugins/gazebo_ros_camera_utils.h"
00044 
00045 namespace gazebo
00046 {
00048 // Constructor
00049 GazeboRosCameraUtils::GazeboRosCameraUtils()
00050 {
00051   this->last_update_time_ = common::Time(0);
00052   this->last_info_update_time_ = common::Time(0);
00053   this->height_ = 0;
00054   this->width_ = 0;
00055   this->skip_ = 0;
00056   this->format_ = "";
00057   this->initialized_ = false;
00058 }
00059 
00060 void GazeboRosCameraUtils::configCallback(
00061   gazebo_plugins::GazeboRosCameraConfig &config, uint32_t level)
00062 {
00063   if (this->initialized_)
00064   {
00065     ROS_INFO("Reconfigure request for the gazebo ros camera_: %s. New rate: %.2f",
00066              this->camera_name_.c_str(), config.imager_rate);
00067     this->parentSensor_->SetUpdateRate(config.imager_rate);
00068   }
00069 }
00070 
00072 // Destructor
00073 GazeboRosCameraUtils::~GazeboRosCameraUtils()
00074 {
00075   this->parentSensor_->SetActive(false);
00076   this->rosnode_->shutdown();
00077   this->camera_queue_.clear();
00078   this->camera_queue_.disable();
00079   this->callback_queue_thread_.join();
00080   delete this->rosnode_;
00081 }
00082 
00084 // Load the controller
00085 void GazeboRosCameraUtils::Load(sensors::SensorPtr _parent,
00086   sdf::ElementPtr _sdf,
00087   const std::string &_camera_name_suffix,
00088   double _hack_baseline)
00089 {
00090   // default Load:
00091   // provide _camera_name_suffix to prevent LoadThread() creating the ros::NodeHandle with
00092   //an incomplete this->camera_name_ namespace. There was a race condition when the _camera_name_suffix
00093   //was appended in this function.
00094   this->Load(_parent, _sdf, _camera_name_suffix);
00095 
00096   // overwrite hack baseline if specified at load
00097   // example usage in gazebo_ros_multicamera
00098   this->hack_baseline_ = _hack_baseline;
00099 }
00100 
00102 // Load the controller
00103 void GazeboRosCameraUtils::Load(sensors::SensorPtr _parent,
00104   sdf::ElementPtr _sdf,
00105   const std::string &_camera_name_suffix)
00106 {
00107   // Get the world name.
00108   std::string world_name = _parent->GetWorldName();
00109 
00110   // Get the world_
00111   this->world_ = physics::get_world(world_name);
00112 
00113   // save pointers
00114   this->sdf = _sdf;
00115 
00116   // maintain for one more release for backwards compatibility with
00117   // pr2_gazebo_plugins
00118   this->world = this->world_;
00119 
00120   std::stringstream ss;
00121   this->robot_namespace_ =  GetRobotNamespace(_parent, _sdf, "Camera");
00122   
00123   this->image_topic_name_ = "image_raw";
00124   if (this->sdf->HasElement("imageTopicName"))
00125     this->image_topic_name_ = this->sdf->Get<std::string>("imageTopicName");
00126 
00127   this->camera_info_topic_name_ = "camera_info";
00128   if (this->sdf->HasElement("cameraInfoTopicName"))
00129     this->camera_info_topic_name_ =
00130       this->sdf->Get<std::string>("cameraInfoTopicName");
00131 
00132   if (!this->sdf->HasElement("cameraName"))
00133     ROS_DEBUG("Camera plugin missing <cameraName>, default to empty");
00134   else
00135     this->camera_name_ = this->sdf->Get<std::string>("cameraName");
00136 
00137   // overwrite camera suffix
00138   // example usage in gazebo_ros_multicamera
00139   this->camera_name_ += _camera_name_suffix;
00140 
00141   if (!this->sdf->HasElement("frameName"))
00142     ROS_DEBUG("Camera plugin missing <frameName>, defaults to /world");
00143   else
00144     this->frame_name_ = this->sdf->Get<std::string>("frameName");
00145 
00146   if (!this->sdf->HasElement("updateRate"))
00147   {
00148     ROS_DEBUG("Camera plugin missing <updateRate>, defaults to unlimited (0).");
00149     this->update_rate_ = 0;
00150   }
00151   else
00152     this->update_rate_ = this->sdf->Get<double>("updateRate");
00153 
00154   if (!this->sdf->HasElement("CxPrime"))
00155   {
00156     ROS_DEBUG("Camera plugin missing <CxPrime>, defaults to 0");
00157     this->cx_prime_ = 0;
00158   }
00159   else
00160     this->cx_prime_ = this->sdf->Get<double>("CxPrime");
00161 
00162   if (!this->sdf->HasElement("Cx"))
00163   {
00164     ROS_DEBUG("Camera plugin missing <Cx>, defaults to 0");
00165     this->cx_= 0;
00166   }
00167   else
00168     this->cx_ = this->sdf->Get<double>("Cx");
00169 
00170   if (!this->sdf->HasElement("Cy"))
00171   {
00172     ROS_DEBUG("Camera plugin missing <Cy>, defaults to 0");
00173     this->cy_= 0;
00174   }
00175   else
00176     this->cy_ = this->sdf->Get<double>("Cy");
00177 
00178   if (!this->sdf->HasElement("focalLength"))
00179   {
00180     ROS_DEBUG("Camera plugin missing <focalLength>, defaults to 0");
00181     this->focal_length_= 0;
00182   }
00183   else
00184     this->focal_length_ = this->sdf->Get<double>("focalLength");
00185 
00186   if (!this->sdf->HasElement("hackBaseline"))
00187   {
00188     ROS_DEBUG("Camera plugin missing <hackBaseline>, defaults to 0");
00189     this->hack_baseline_= 0;
00190   }
00191   else
00192     this->hack_baseline_ = this->sdf->Get<double>("hackBaseline");
00193 
00194   if (!this->sdf->HasElement("distortionK1"))
00195   {
00196     ROS_DEBUG("Camera plugin missing <distortionK1>, defaults to 0");
00197     this->distortion_k1_= 0;
00198   }
00199   else
00200     this->distortion_k1_ = this->sdf->Get<double>("distortionK1");
00201 
00202   if (!this->sdf->HasElement("distortionK2"))
00203   {
00204     ROS_DEBUG("Camera plugin missing <distortionK2>, defaults to 0");
00205     this->distortion_k2_= 0;
00206   }
00207   else
00208     this->distortion_k2_ = this->sdf->Get<double>("distortionK2");
00209 
00210   if (!this->sdf->HasElement("distortionK3"))
00211   {
00212     ROS_DEBUG("Camera plugin missing <distortionK3>, defaults to 0");
00213     this->distortion_k3_= 0;
00214   }
00215   else
00216     this->distortion_k3_ = this->sdf->Get<double>("distortionK3");
00217 
00218   if (!this->sdf->HasElement("distortionT1"))
00219   {
00220     ROS_DEBUG("Camera plugin missing <distortionT1>, defaults to 0");
00221     this->distortion_t1_= 0;
00222   }
00223   else
00224     this->distortion_t1_ = this->sdf->Get<double>("distortionT1");
00225 
00226   if (!this->sdf->HasElement("distortionT2"))
00227   {
00228     ROS_DEBUG("Camera plugin missing <distortionT2>, defaults to 0");
00229     this->distortion_t2_= 0;
00230   }
00231   else
00232     this->distortion_t2_ = this->sdf->Get<double>("distortionT2");
00233 
00234   if ((this->distortion_k1_ != 0.0) || (this->distortion_k2_ != 0.0) ||
00235       (this->distortion_k3_ != 0.0) || (this->distortion_t1_ != 0.0) ||
00236       (this->distortion_t2_ != 0.0))
00237   {
00238     ROS_WARN("gazebo_ros_camera_ simulation does not support non-zero"
00239              " distortion parameters right now, your simulation maybe wrong.");
00240   }
00241 
00242   // initialize shared_ptr members
00243   if (!this->image_connect_count_) this->image_connect_count_ = boost::shared_ptr<int>(new int(0));
00244   if (!this->image_connect_count_lock_) this->image_connect_count_lock_ = boost::shared_ptr<boost::mutex>(new boost::mutex);
00245   if (!this->was_active_) this->was_active_ = boost::shared_ptr<bool>(new bool(false));
00246 
00247   // ros callback queue for processing subscription
00248   this->deferred_load_thread_ = boost::thread(
00249     boost::bind(&GazeboRosCameraUtils::LoadThread, this));
00250 }
00251 
00252 event::ConnectionPtr GazeboRosCameraUtils::OnLoad(const boost::function<void()>& load_function)
00253 {
00254   return load_event_.Connect(load_function);
00255 }
00256 
00258 // Load the controller
00259 void GazeboRosCameraUtils::LoadThread()
00260 {
00261   // Exit if no ROS
00262   if (!ros::isInitialized())
00263   {
00264     gzerr << "Not loading plugin since ROS hasn't been "
00265           << "properly initialized.  Try starting gazebo with ros plugin:\n"
00266           << "  gazebo -s libgazebo_ros_api_plugin.so\n";
00267     return;
00268   }
00269 
00270   // Sensor generation off by default.  Must do this before advertising the
00271   // associated ROS topics.
00272   this->parentSensor_->SetActive(false);
00273 
00274   this->rosnode_ = new ros::NodeHandle(this->robot_namespace_ + "/" + this->camera_name_);
00275 
00276   // initialize camera_info_manager
00277   this->camera_info_manager_.reset(new camera_info_manager::CameraInfoManager(
00278           *this->rosnode_, this->camera_name_));
00279 
00280   // resolve tf prefix
00281   std::string key;
00282   if(this->rosnode_->searchParam("tf_prefix", key)){
00283     std::string prefix;
00284     this->rosnode_->getParam(key, prefix);
00285     this->frame_name_ = tf::resolve(prefix, this->frame_name_);
00286   }
00287 
00288   this->itnode_ = new image_transport::ImageTransport(*this->rosnode_);
00289   
00290   // resolve tf prefix
00291   this->tf_prefix_ = tf::getPrefixParam(*this->rosnode_);
00292   if(this->tf_prefix_.empty()) {
00293       this->tf_prefix_ = this->robot_namespace_;
00294       boost::trim_right_if(this->tf_prefix_,boost::is_any_of("/"));
00295   }
00296   this->frame_name_ = tf::resolve(this->tf_prefix_, this->frame_name_);
00297 
00298   ROS_INFO("Camera Plugin (ns = %s)  <tf_prefix_>, set to \"%s\"",
00299              this->robot_namespace_.c_str(), this->tf_prefix_.c_str());
00300   
00301   
00302   if (!this->camera_name_.empty())
00303   {
00304     dyn_srv_ =
00305       new dynamic_reconfigure::Server<gazebo_plugins::GazeboRosCameraConfig>
00306       (*this->rosnode_);
00307     dynamic_reconfigure::Server<gazebo_plugins::GazeboRosCameraConfig>
00308       ::CallbackType f =
00309       boost::bind(&GazeboRosCameraUtils::configCallback, this, _1, _2);
00310     dyn_srv_->setCallback(f);
00311   }
00312   else
00313   {
00314     ROS_WARN("dynamic reconfigure is not enabled for this image topic [%s]"
00315              " becuase <cameraName> is not specified",
00316              this->image_topic_name_.c_str());
00317   }
00318 
00319   this->image_pub_ = this->itnode_->advertise(
00320     this->image_topic_name_, 2,
00321     boost::bind(&GazeboRosCameraUtils::ImageConnect, this),
00322     boost::bind(&GazeboRosCameraUtils::ImageDisconnect, this),
00323     ros::VoidPtr(), &this->camera_queue_);
00324 
00325   // camera info publish rate will be synchronized to image sensor
00326   // publish rates.
00327   // If someone connects to camera_info, sensor will be activated
00328   // and camera_info will be published alongside image_raw with the
00329   // same timestamps.  This incurrs additional computational cost when
00330   // there are subscribers to camera_info, but better mimics behavior
00331   // of image_pipeline.
00332   ros::AdvertiseOptions cio =
00333     ros::AdvertiseOptions::create<sensor_msgs::CameraInfo>(
00334     this->camera_info_topic_name_, 2,
00335     boost::bind(&GazeboRosCameraUtils::ImageConnect, this),
00336     boost::bind(&GazeboRosCameraUtils::ImageDisconnect, this),
00337     ros::VoidPtr(), &this->camera_queue_);
00338   this->camera_info_pub_ = this->rosnode_->advertise(cio);
00339 
00340   /* disabling fov and rate setting for each camera
00341   ros::SubscribeOptions zoom_so =
00342     ros::SubscribeOptions::create<std_msgs::Float64>(
00343         "set_hfov", 1,
00344         boost::bind(&GazeboRosCameraUtils::SetHFOV, this, _1),
00345         ros::VoidPtr(), &this->camera_queue_);
00346   this->cameraHFOVSubscriber_ = this->rosnode_->subscribe(zoom_so);
00347 
00348   ros::SubscribeOptions rate_so =
00349     ros::SubscribeOptions::create<std_msgs::Float64>(
00350         "set_update_rate", 1,
00351         boost::bind(&GazeboRosCameraUtils::SetUpdateRate, this, _1),
00352         ros::VoidPtr(), &this->camera_queue_);
00353   this->cameraUpdateRateSubscriber_ = this->rosnode_->subscribe(rate_so);
00354   */
00355 
00356   this->Init();
00357 }
00358 
00360 // Set Horizontal Field of View
00361 void GazeboRosCameraUtils::SetHFOV(const std_msgs::Float64::ConstPtr& hfov)
00362 {
00363   this->camera_->SetHFOV(hfov->data);
00364 }
00365 
00367 // Set Update Rate
00368 void GazeboRosCameraUtils::SetUpdateRate(
00369   const std_msgs::Float64::ConstPtr& update_rate)
00370 {
00371   this->parentSensor_->SetUpdateRate(update_rate->data);
00372 }
00373 
00375 // Increment count
00376 void GazeboRosCameraUtils::ImageConnect()
00377 {
00378   boost::mutex::scoped_lock lock(*this->image_connect_count_lock_);
00379 
00380   // upon first connection, remember if camera was active.
00381   if ((*this->image_connect_count_) == 0)
00382     *this->was_active_ = this->parentSensor_->IsActive();
00383 
00384   (*this->image_connect_count_)++;
00385 
00386   this->parentSensor_->SetActive(true);
00387 }
00389 // Decrement count
00390 void GazeboRosCameraUtils::ImageDisconnect()
00391 {
00392   boost::mutex::scoped_lock lock(*this->image_connect_count_lock_);
00393 
00394   (*this->image_connect_count_)--;
00395 
00396   // if there are no more subscribers, but camera was active to begin with,
00397   // leave it active.  Use case:  this could be a multicamera, where
00398   // each camera shares the same parentSensor_.
00399   if ((*this->image_connect_count_) <= 0 && !*this->was_active_)
00400     this->parentSensor_->SetActive(false);
00401 }
00402 
00404 // Initialize the controller
00405 void GazeboRosCameraUtils::Init()
00406 {
00407   // prepare to throttle this plugin at the same rate
00408   // ideally, we should invoke a plugin update when the sensor updates,
00409   // have to think about how to do that properly later
00410   if (this->update_rate_ > 0.0)
00411     this->update_period_ = 1.0/this->update_rate_;
00412   else
00413     this->update_period_ = 0.0;
00414 
00415   // set buffer size
00416   if (this->format_ == "L8")
00417   {
00418     this->type_ = sensor_msgs::image_encodings::MONO8;
00419     this->skip_ = 1;
00420   }
00421   else if (this->format_ == "R8G8B8")
00422   {
00423     this->type_ = sensor_msgs::image_encodings::RGB8;
00424     this->skip_ = 3;
00425   }
00426   else if (this->format_ == "B8G8R8")
00427   {
00428     this->type_ = sensor_msgs::image_encodings::BGR8;
00429     this->skip_ = 3;
00430   }
00431   else if (this->format_ == "BAYER_RGGB8")
00432   {
00433     ROS_INFO("bayer simulation maybe computationally expensive.");
00434     this->type_ = sensor_msgs::image_encodings::BAYER_RGGB8;
00435     this->skip_ = 1;
00436   }
00437   else if (this->format_ == "BAYER_BGGR8")
00438   {
00439     ROS_INFO("bayer simulation maybe computationally expensive.");
00440     this->type_ = sensor_msgs::image_encodings::BAYER_BGGR8;
00441     this->skip_ = 1;
00442   }
00443   else if (this->format_ == "BAYER_GBRG8")
00444   {
00445     ROS_INFO("bayer simulation maybe computationally expensive.");
00446     this->type_ = sensor_msgs::image_encodings::BAYER_GBRG8;
00447     this->skip_ = 1;
00448   }
00449   else if (this->format_ == "BAYER_GRBG8")
00450   {
00451     ROS_INFO("bayer simulation maybe computationally expensive.");
00452     this->type_ = sensor_msgs::image_encodings::BAYER_GRBG8;
00453     this->skip_ = 1;
00454   }
00455   else
00456   {
00457     ROS_ERROR("Unsupported Gazebo ImageFormat\n");
00458     this->type_ = sensor_msgs::image_encodings::BGR8;
00459     this->skip_ = 3;
00460   }
00461 
00463   if (this->cx_prime_ == 0)
00464     this->cx_prime_ = (static_cast<double>(this->width_) + 1.0) /2.0;
00465   if (this->cx_ == 0)
00466     this->cx_ = (static_cast<double>(this->width_) + 1.0) /2.0;
00467   if (this->cy_ == 0)
00468     this->cy_ = (static_cast<double>(this->height_) + 1.0) /2.0;
00469 
00470 
00471   double computed_focal_length =
00472     (static_cast<double>(this->width_)) /
00473     (2.0 * tan(this->camera_->GetHFOV().Radian() / 2.0));
00474 
00475   if (this->focal_length_ == 0)
00476   {
00477     this->focal_length_ = computed_focal_length;
00478   }
00479   else
00480   {
00481     // check against float precision
00482     if (!gazebo::math::equal(this->focal_length_, computed_focal_length))
00483     {
00484       ROS_WARN("The <focal_length>[%f] you have provided for camera_ [%s]"
00485                " is inconsistent with specified image_width [%d] and"
00486                " HFOV [%f].   Please double check to see that"
00487                " focal_length = width_ / (2.0 * tan(HFOV/2.0)),"
00488                " the explected focal_lengtth value is [%f],"
00489                " please update your camera_ model description accordingly.",
00490                 this->focal_length_, this->parentSensor_->GetName().c_str(),
00491                 this->width_, this->camera_->GetHFOV().Radian(),
00492                 computed_focal_length);
00493     }
00494   }
00495 
00496   // fill CameraInfo
00497   sensor_msgs::CameraInfo camera_info_msg;
00498 
00499   camera_info_msg.header.frame_id = this->frame_name_;
00500 
00501   camera_info_msg.height = this->height_;
00502   camera_info_msg.width  = this->width_;
00503   // distortion
00504 #if ROS_VERSION_MINIMUM(1, 3, 0)
00505   camera_info_msg.distortion_model = "plumb_bob";
00506   camera_info_msg.D.resize(5);
00507 #endif
00508   camera_info_msg.D[0] = this->distortion_k1_;
00509   camera_info_msg.D[1] = this->distortion_k2_;
00510   camera_info_msg.D[2] = this->distortion_k3_;
00511   camera_info_msg.D[3] = this->distortion_t1_;
00512   camera_info_msg.D[4] = this->distortion_t2_;
00513   // original camera_ matrix
00514   camera_info_msg.K[0] = this->focal_length_;
00515   camera_info_msg.K[1] = 0.0;
00516   camera_info_msg.K[2] = this->cx_;
00517   camera_info_msg.K[3] = 0.0;
00518   camera_info_msg.K[4] = this->focal_length_;
00519   camera_info_msg.K[5] = this->cy_;
00520   camera_info_msg.K[6] = 0.0;
00521   camera_info_msg.K[7] = 0.0;
00522   camera_info_msg.K[8] = 1.0;
00523   // rectification
00524   camera_info_msg.R[0] = 1.0;
00525   camera_info_msg.R[1] = 0.0;
00526   camera_info_msg.R[2] = 0.0;
00527   camera_info_msg.R[3] = 0.0;
00528   camera_info_msg.R[4] = 1.0;
00529   camera_info_msg.R[5] = 0.0;
00530   camera_info_msg.R[6] = 0.0;
00531   camera_info_msg.R[7] = 0.0;
00532   camera_info_msg.R[8] = 1.0;
00533   // camera_ projection matrix (same as camera_ matrix due
00534   // to lack of distortion/rectification) (is this generated?)
00535   camera_info_msg.P[0] = this->focal_length_;
00536   camera_info_msg.P[1] = 0.0;
00537   camera_info_msg.P[2] = this->cx_;
00538   camera_info_msg.P[3] = -this->focal_length_ * this->hack_baseline_;
00539   camera_info_msg.P[4] = 0.0;
00540   camera_info_msg.P[5] = this->focal_length_;
00541   camera_info_msg.P[6] = this->cy_;
00542   camera_info_msg.P[7] = 0.0;
00543   camera_info_msg.P[8] = 0.0;
00544   camera_info_msg.P[9] = 0.0;
00545   camera_info_msg.P[10] = 1.0;
00546   camera_info_msg.P[11] = 0.0;
00547 
00548   this->camera_info_manager_->setCameraInfo(camera_info_msg);
00549 
00550   // start custom queue for camera_
00551   this->callback_queue_thread_ = boost::thread(
00552     boost::bind(&GazeboRosCameraUtils::CameraQueueThread, this));
00553 
00554   load_event_();
00555   this->initialized_ = true;
00556 }
00557 
00559 // Put camera_ data to the interface
00560 void GazeboRosCameraUtils::PutCameraData(const unsigned char *_src,
00561   common::Time &last_update_time)
00562 {
00563   this->sensor_update_time_ = last_update_time;
00564   this->PutCameraData(_src);
00565 }
00566 
00567 void GazeboRosCameraUtils::PutCameraData(const unsigned char *_src)
00568 {
00569   if (!this->initialized_ || this->height_ <=0 || this->width_ <=0)
00570     return;
00571 
00573   if ((*this->image_connect_count_) > 0)
00574   {
00575     boost::mutex::scoped_lock lock(this->lock_);
00576 
00577     // copy data into image
00578     this->image_msg_.header.frame_id = this->frame_name_;
00579     this->image_msg_.header.stamp.sec = this->sensor_update_time_.sec;
00580     this->image_msg_.header.stamp.nsec = this->sensor_update_time_.nsec;
00581 
00582     // copy from src to image_msg_
00583     fillImage(this->image_msg_, this->type_, this->height_, this->width_,
00584         this->skip_*this->width_, reinterpret_cast<const void*>(_src));
00585 
00586     // publish to ros
00587     this->image_pub_.publish(this->image_msg_);
00588   }
00589 }
00590 
00592 // Put camera_ data to the interface
00593 void GazeboRosCameraUtils::PublishCameraInfo(common::Time &last_update_time)
00594 {
00595   if (!this->initialized_ || this->height_ <=0 || this->width_ <=0)
00596     return;
00597 
00598   this->sensor_update_time_ = last_update_time;
00599   this->PublishCameraInfo();
00600 }
00601 
00602 void GazeboRosCameraUtils::PublishCameraInfo()
00603 {
00604   if (!this->initialized_ || this->height_ <=0 || this->width_ <=0)
00605     return;
00606 
00607   if (this->camera_info_pub_.getNumSubscribers() > 0)
00608   {
00609     this->sensor_update_time_ = this->parentSensor_->GetLastUpdateTime();
00610     common::Time cur_time = this->world_->GetSimTime();
00611     if (cur_time - this->last_info_update_time_ >= this->update_period_)
00612     {
00613       this->PublishCameraInfo(this->camera_info_pub_);
00614       this->last_info_update_time_ = cur_time;
00615     }
00616   }
00617 }
00618 
00619 void GazeboRosCameraUtils::PublishCameraInfo(
00620   ros::Publisher camera_info_publisher)
00621 {
00622   sensor_msgs::CameraInfo camera_info_msg = camera_info_manager_->getCameraInfo();
00623 
00624   camera_info_msg.header.stamp.sec = this->sensor_update_time_.sec;
00625   camera_info_msg.header.stamp.nsec = this->sensor_update_time_.nsec;
00626 
00627   camera_info_publisher.publish(camera_info_msg);
00628 }
00629 
00630 
00632 // Put camera_ data to the interface
00633 void GazeboRosCameraUtils::CameraQueueThread()
00634 {
00635   static const double timeout = 0.001;
00636 
00637   while (this->rosnode_->ok())
00638   {
00640     this->camera_queue_.callAvailable(ros::WallDuration(timeout));
00641   }
00642 }
00643 }


gazebo_plugins
Author(s): John Hsu
autogenerated on Fri Aug 28 2015 10:47:25