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 # if GAZEBO_MAJOR_VERSION >= 7
00109   std::string world_name = _parent->WorldName();
00110 # else
00111   std::string world_name = _parent->GetWorldName();
00112 # endif
00113 
00114   // Get the world_
00115   this->world_ = physics::get_world(world_name);
00116 
00117   // save pointers
00118   this->sdf = _sdf;
00119 
00120   // maintain for one more release for backwards compatibility with
00121   // pr2_gazebo_plugins
00122   this->world = this->world_;
00123 
00124   std::stringstream ss;
00125   this->robot_namespace_ =  GetRobotNamespace(_parent, _sdf, "Camera");
00126 
00127   this->image_topic_name_ = "image_raw";
00128   if (this->sdf->HasElement("imageTopicName"))
00129     this->image_topic_name_ = this->sdf->Get<std::string>("imageTopicName");
00130 
00131   this->camera_info_topic_name_ = "camera_info";
00132   if (this->sdf->HasElement("cameraInfoTopicName"))
00133     this->camera_info_topic_name_ =
00134       this->sdf->Get<std::string>("cameraInfoTopicName");
00135 
00136   if (!this->sdf->HasElement("cameraName"))
00137     ROS_DEBUG("Camera plugin missing <cameraName>, default to empty");
00138   else
00139     this->camera_name_ = this->sdf->Get<std::string>("cameraName");
00140 
00141   // overwrite camera suffix
00142   // example usage in gazebo_ros_multicamera
00143   this->camera_name_ += _camera_name_suffix;
00144 
00145   if (!this->sdf->HasElement("frameName"))
00146     ROS_DEBUG("Camera plugin missing <frameName>, defaults to /world");
00147   else
00148     this->frame_name_ = this->sdf->Get<std::string>("frameName");
00149 
00150   if (!this->sdf->HasElement("updateRate"))
00151   {
00152     ROS_DEBUG("Camera plugin missing <updateRate>, defaults to unlimited (0).");
00153     this->update_rate_ = 0;
00154   }
00155   else
00156     this->update_rate_ = this->sdf->Get<double>("updateRate");
00157 
00158   if (!this->sdf->HasElement("CxPrime"))
00159   {
00160     ROS_DEBUG("Camera plugin missing <CxPrime>, defaults to 0");
00161     this->cx_prime_ = 0;
00162   }
00163   else
00164     this->cx_prime_ = this->sdf->Get<double>("CxPrime");
00165 
00166   if (!this->sdf->HasElement("Cx"))
00167   {
00168     ROS_DEBUG("Camera plugin missing <Cx>, defaults to 0");
00169     this->cx_= 0;
00170   }
00171   else
00172     this->cx_ = this->sdf->Get<double>("Cx");
00173 
00174   if (!this->sdf->HasElement("Cy"))
00175   {
00176     ROS_DEBUG("Camera plugin missing <Cy>, defaults to 0");
00177     this->cy_= 0;
00178   }
00179   else
00180     this->cy_ = this->sdf->Get<double>("Cy");
00181 
00182   if (!this->sdf->HasElement("focalLength"))
00183   {
00184     ROS_DEBUG("Camera plugin missing <focalLength>, defaults to 0");
00185     this->focal_length_= 0;
00186   }
00187   else
00188     this->focal_length_ = this->sdf->Get<double>("focalLength");
00189 
00190   if (!this->sdf->HasElement("hackBaseline"))
00191   {
00192     ROS_DEBUG("Camera plugin missing <hackBaseline>, defaults to 0");
00193     this->hack_baseline_= 0;
00194   }
00195   else
00196     this->hack_baseline_ = this->sdf->Get<double>("hackBaseline");
00197 
00198   if (!this->sdf->HasElement("distortionK1"))
00199   {
00200     ROS_DEBUG("Camera plugin missing <distortionK1>, defaults to 0");
00201     this->distortion_k1_= 0;
00202   }
00203   else
00204     this->distortion_k1_ = this->sdf->Get<double>("distortionK1");
00205 
00206   if (!this->sdf->HasElement("distortionK2"))
00207   {
00208     ROS_DEBUG("Camera plugin missing <distortionK2>, defaults to 0");
00209     this->distortion_k2_= 0;
00210   }
00211   else
00212     this->distortion_k2_ = this->sdf->Get<double>("distortionK2");
00213 
00214   if (!this->sdf->HasElement("distortionK3"))
00215   {
00216     ROS_DEBUG("Camera plugin missing <distortionK3>, defaults to 0");
00217     this->distortion_k3_= 0;
00218   }
00219   else
00220     this->distortion_k3_ = this->sdf->Get<double>("distortionK3");
00221 
00222   if (!this->sdf->HasElement("distortionT1"))
00223   {
00224     ROS_DEBUG("Camera plugin missing <distortionT1>, defaults to 0");
00225     this->distortion_t1_= 0;
00226   }
00227   else
00228     this->distortion_t1_ = this->sdf->Get<double>("distortionT1");
00229 
00230   if (!this->sdf->HasElement("distortionT2"))
00231   {
00232     ROS_DEBUG("Camera plugin missing <distortionT2>, defaults to 0");
00233     this->distortion_t2_= 0;
00234   }
00235   else
00236     this->distortion_t2_ = this->sdf->Get<double>("distortionT2");
00237 
00238   if ((this->distortion_k1_ != 0.0) || (this->distortion_k2_ != 0.0) ||
00239       (this->distortion_k3_ != 0.0) || (this->distortion_t1_ != 0.0) ||
00240       (this->distortion_t2_ != 0.0))
00241   {
00242     ROS_WARN("gazebo_ros_camera_ simulation does not support non-zero"
00243              " distortion parameters right now, your simulation maybe wrong.");
00244   }
00245 
00246   // initialize shared_ptr members
00247   if (!this->image_connect_count_) this->image_connect_count_ = boost::shared_ptr<int>(new int(0));
00248   if (!this->image_connect_count_lock_) this->image_connect_count_lock_ = boost::shared_ptr<boost::mutex>(new boost::mutex);
00249   if (!this->was_active_) this->was_active_ = boost::shared_ptr<bool>(new bool(false));
00250 
00251   // ros callback queue for processing subscription
00252   this->deferred_load_thread_ = boost::thread(
00253     boost::bind(&GazeboRosCameraUtils::LoadThread, this));
00254 }
00255 
00256 event::ConnectionPtr GazeboRosCameraUtils::OnLoad(const boost::function<void()>& load_function)
00257 {
00258   return load_event_.Connect(load_function);
00259 }
00260 
00262 // Load the controller
00263 void GazeboRosCameraUtils::LoadThread()
00264 {
00265   // Exit if no ROS
00266   if (!ros::isInitialized())
00267   {
00268     gzerr << "Not loading plugin since ROS hasn't been "
00269           << "properly initialized.  Try starting gazebo with ros plugin:\n"
00270           << "  gazebo -s libgazebo_ros_api_plugin.so\n";
00271     return;
00272   }
00273 
00274   // Sensor generation off by default.  Must do this before advertising the
00275   // associated ROS topics.
00276   this->parentSensor_->SetActive(false);
00277 
00278   this->rosnode_ = new ros::NodeHandle(this->robot_namespace_ + "/" + this->camera_name_);
00279 
00280   // initialize camera_info_manager
00281   this->camera_info_manager_.reset(new camera_info_manager::CameraInfoManager(
00282           *this->rosnode_, this->camera_name_));
00283 
00284   this->itnode_ = new image_transport::ImageTransport(*this->rosnode_);
00285 
00286   // resolve tf prefix
00287   this->tf_prefix_ = tf::getPrefixParam(*this->rosnode_);
00288   if(this->tf_prefix_.empty()) {
00289       this->tf_prefix_ = this->robot_namespace_;
00290       boost::trim_right_if(this->tf_prefix_,boost::is_any_of("/"));
00291   }
00292   this->frame_name_ = tf::resolve(this->tf_prefix_, this->frame_name_);
00293 
00294   ROS_INFO("Camera Plugin (ns = %s)  <tf_prefix_>, set to \"%s\"",
00295              this->robot_namespace_.c_str(), this->tf_prefix_.c_str());
00296 
00297 
00298   if (!this->camera_name_.empty())
00299   {
00300     dyn_srv_ =
00301       new dynamic_reconfigure::Server<gazebo_plugins::GazeboRosCameraConfig>
00302       (*this->rosnode_);
00303     dynamic_reconfigure::Server<gazebo_plugins::GazeboRosCameraConfig>
00304       ::CallbackType f =
00305       boost::bind(&GazeboRosCameraUtils::configCallback, this, _1, _2);
00306     dyn_srv_->setCallback(f);
00307   }
00308   else
00309   {
00310     ROS_WARN("dynamic reconfigure is not enabled for this image topic [%s]"
00311              " becuase <cameraName> is not specified",
00312              this->image_topic_name_.c_str());
00313   }
00314 
00315   this->image_pub_ = this->itnode_->advertise(
00316     this->image_topic_name_, 2,
00317     boost::bind(&GazeboRosCameraUtils::ImageConnect, this),
00318     boost::bind(&GazeboRosCameraUtils::ImageDisconnect, this),
00319     ros::VoidPtr(), true);
00320 
00321   // camera info publish rate will be synchronized to image sensor
00322   // publish rates.
00323   // If someone connects to camera_info, sensor will be activated
00324   // and camera_info will be published alongside image_raw with the
00325   // same timestamps.  This incurrs additional computational cost when
00326   // there are subscribers to camera_info, but better mimics behavior
00327   // of image_pipeline.
00328   ros::AdvertiseOptions cio =
00329     ros::AdvertiseOptions::create<sensor_msgs::CameraInfo>(
00330     this->camera_info_topic_name_, 2,
00331     boost::bind(&GazeboRosCameraUtils::ImageConnect, this),
00332     boost::bind(&GazeboRosCameraUtils::ImageDisconnect, this),
00333     ros::VoidPtr(), &this->camera_queue_);
00334   this->camera_info_pub_ = this->rosnode_->advertise(cio);
00335 
00336   /* disabling fov and rate setting for each camera
00337   ros::SubscribeOptions zoom_so =
00338     ros::SubscribeOptions::create<std_msgs::Float64>(
00339         "set_hfov", 1,
00340         boost::bind(&GazeboRosCameraUtils::SetHFOV, this, _1),
00341         ros::VoidPtr(), &this->camera_queue_);
00342   this->cameraHFOVSubscriber_ = this->rosnode_->subscribe(zoom_so);
00343 
00344   ros::SubscribeOptions rate_so =
00345     ros::SubscribeOptions::create<std_msgs::Float64>(
00346         "set_update_rate", 1,
00347         boost::bind(&GazeboRosCameraUtils::SetUpdateRate, this, _1),
00348         ros::VoidPtr(), &this->camera_queue_);
00349   this->cameraUpdateRateSubscriber_ = this->rosnode_->subscribe(rate_so);
00350   */
00351 
00352   this->Init();
00353 }
00354 
00356 // Set Horizontal Field of View
00357 void GazeboRosCameraUtils::SetHFOV(const std_msgs::Float64::ConstPtr& hfov)
00358 {
00359 #if GAZEBO_MAJOR_VERSION >= 7
00360   this->camera_->SetHFOV(ignition::math::Angle(hfov->data));
00361 #else
00362   this->camera_->SetHFOV(gazebo::math::Angle(hfov->data));
00363 #endif
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 # if GAZEBO_MAJOR_VERSION >= 7
00472   double computed_focal_length =
00473     (static_cast<double>(this->width_)) /
00474     (2.0 * tan(this->camera_->HFOV().Radian() / 2.0));
00475 # else
00476   double computed_focal_length =
00477     (static_cast<double>(this->width_)) /
00478     (2.0 * tan(this->camera_->GetHFOV().Radian() / 2.0));
00479 # endif
00480 
00481   if (this->focal_length_ == 0)
00482   {
00483     this->focal_length_ = computed_focal_length;
00484   }
00485   else
00486   {
00487     // check against float precision
00488     if (!gazebo::math::equal(this->focal_length_, computed_focal_length))
00489     {
00490 # if GAZEBO_MAJOR_VERSION >= 7
00491       ROS_WARN("The <focal_length>[%f] you have provided for camera_ [%s]"
00492                " is inconsistent with specified image_width [%d] and"
00493                " HFOV [%f].   Please double check to see that"
00494                " focal_length = width_ / (2.0 * tan(HFOV/2.0)),"
00495                " the explected focal_lengtth value is [%f],"
00496                " please update your camera_ model description accordingly.",
00497                 this->focal_length_, this->parentSensor_->Name().c_str(),
00498                 this->width_, this->camera_->HFOV().Radian(),
00499                 computed_focal_length);
00500 # else
00501       ROS_WARN("The <focal_length>[%f] you have provided for camera_ [%s]"
00502                " is inconsistent with specified image_width [%d] and"
00503                " HFOV [%f].   Please double check to see that"
00504                " focal_length = width_ / (2.0 * tan(HFOV/2.0)),"
00505                " the explected focal_lengtth value is [%f],"
00506                " please update your camera_ model description accordingly.",
00507                 this->focal_length_, this->parentSensor_->GetName().c_str(),
00508                 this->width_, this->camera_->GetHFOV().Radian(),
00509                 computed_focal_length);
00510 
00511 # endif
00512     }
00513   }
00514 
00515   // fill CameraInfo
00516   sensor_msgs::CameraInfo camera_info_msg;
00517 
00518   camera_info_msg.header.frame_id = this->frame_name_;
00519 
00520   camera_info_msg.height = this->height_;
00521   camera_info_msg.width  = this->width_;
00522   // distortion
00523 #if ROS_VERSION_MINIMUM(1, 3, 0)
00524   camera_info_msg.distortion_model = "plumb_bob";
00525   camera_info_msg.D.resize(5);
00526 #endif
00527   // D = {k1, k2, t1, t2, k3}, as specified in:
00528   // - sensor_msgs/CameraInfo: http://docs.ros.org/api/sensor_msgs/html/msg/CameraInfo.html
00529   // - OpenCV: http://docs.opencv.org/2.4/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html
00530   camera_info_msg.D[0] = this->distortion_k1_;
00531   camera_info_msg.D[1] = this->distortion_k2_;
00532   camera_info_msg.D[2] = this->distortion_t1_;
00533   camera_info_msg.D[3] = this->distortion_t2_;
00534   camera_info_msg.D[4] = this->distortion_k3_;
00535   // original camera_ matrix
00536   camera_info_msg.K[0] = this->focal_length_;
00537   camera_info_msg.K[1] = 0.0;
00538   camera_info_msg.K[2] = this->cx_;
00539   camera_info_msg.K[3] = 0.0;
00540   camera_info_msg.K[4] = this->focal_length_;
00541   camera_info_msg.K[5] = this->cy_;
00542   camera_info_msg.K[6] = 0.0;
00543   camera_info_msg.K[7] = 0.0;
00544   camera_info_msg.K[8] = 1.0;
00545   // rectification
00546   camera_info_msg.R[0] = 1.0;
00547   camera_info_msg.R[1] = 0.0;
00548   camera_info_msg.R[2] = 0.0;
00549   camera_info_msg.R[3] = 0.0;
00550   camera_info_msg.R[4] = 1.0;
00551   camera_info_msg.R[5] = 0.0;
00552   camera_info_msg.R[6] = 0.0;
00553   camera_info_msg.R[7] = 0.0;
00554   camera_info_msg.R[8] = 1.0;
00555   // camera_ projection matrix (same as camera_ matrix due
00556   // to lack of distortion/rectification) (is this generated?)
00557   camera_info_msg.P[0] = this->focal_length_;
00558   camera_info_msg.P[1] = 0.0;
00559   camera_info_msg.P[2] = this->cx_;
00560   camera_info_msg.P[3] = -this->focal_length_ * this->hack_baseline_;
00561   camera_info_msg.P[4] = 0.0;
00562   camera_info_msg.P[5] = this->focal_length_;
00563   camera_info_msg.P[6] = this->cy_;
00564   camera_info_msg.P[7] = 0.0;
00565   camera_info_msg.P[8] = 0.0;
00566   camera_info_msg.P[9] = 0.0;
00567   camera_info_msg.P[10] = 1.0;
00568   camera_info_msg.P[11] = 0.0;
00569 
00570   this->camera_info_manager_->setCameraInfo(camera_info_msg);
00571 
00572   // start custom queue for camera_
00573   this->callback_queue_thread_ = boost::thread(
00574     boost::bind(&GazeboRosCameraUtils::CameraQueueThread, this));
00575 
00576   load_event_();
00577   this->initialized_ = true;
00578 }
00579 
00581 // Put camera_ data to the interface
00582 void GazeboRosCameraUtils::PutCameraData(const unsigned char *_src,
00583   common::Time &last_update_time)
00584 {
00585   this->sensor_update_time_ = last_update_time;
00586   this->PutCameraData(_src);
00587 }
00588 
00589 void GazeboRosCameraUtils::PutCameraData(const unsigned char *_src)
00590 {
00591   if (!this->initialized_ || this->height_ <=0 || this->width_ <=0)
00592     return;
00593 
00595   if ((*this->image_connect_count_) > 0)
00596   {
00597     boost::mutex::scoped_lock lock(this->lock_);
00598 
00599     // copy data into image
00600     this->image_msg_.header.frame_id = this->frame_name_;
00601     this->image_msg_.header.stamp.sec = this->sensor_update_time_.sec;
00602     this->image_msg_.header.stamp.nsec = this->sensor_update_time_.nsec;
00603 
00604     // copy from src to image_msg_
00605     fillImage(this->image_msg_, this->type_, this->height_, this->width_,
00606         this->skip_*this->width_, reinterpret_cast<const void*>(_src));
00607 
00608     // publish to ros
00609     this->image_pub_.publish(this->image_msg_);
00610   }
00611 }
00612 
00614 // Put camera_ data to the interface
00615 void GazeboRosCameraUtils::PublishCameraInfo(common::Time &last_update_time)
00616 {
00617   if (!this->initialized_ || this->height_ <=0 || this->width_ <=0)
00618     return;
00619 
00620   this->sensor_update_time_ = last_update_time;
00621   this->PublishCameraInfo();
00622 }
00623 
00624 void GazeboRosCameraUtils::PublishCameraInfo()
00625 {
00626   if (!this->initialized_ || this->height_ <=0 || this->width_ <=0)
00627     return;
00628 
00629   if (this->camera_info_pub_.getNumSubscribers() > 0)
00630   {
00631 # if GAZEBO_MAJOR_VERSION >= 7
00632     this->sensor_update_time_ = this->parentSensor_->LastMeasurementTime();
00633 # else
00634     this->sensor_update_time_ = this->parentSensor_->GetLastMeasurementTime();
00635 # endif
00636     if (this->sensor_update_time_ - this->last_info_update_time_ >= this->update_period_)
00637     {
00638       this->PublishCameraInfo(this->camera_info_pub_);
00639       this->last_info_update_time_ = this->sensor_update_time_;
00640     }
00641   }
00642 }
00643 
00644 void GazeboRosCameraUtils::PublishCameraInfo(
00645   ros::Publisher camera_info_publisher)
00646 {
00647   sensor_msgs::CameraInfo camera_info_msg = camera_info_manager_->getCameraInfo();
00648 
00649   camera_info_msg.header.stamp.sec = this->sensor_update_time_.sec;
00650   camera_info_msg.header.stamp.nsec = this->sensor_update_time_.nsec;
00651 
00652   camera_info_publisher.publish(camera_info_msg);
00653 }
00654 
00655 
00657 // Put camera_ data to the interface
00658 void GazeboRosCameraUtils::CameraQueueThread()
00659 {
00660   static const double timeout = 0.001;
00661 
00662   while (this->rosnode_->ok())
00663   {
00665     this->camera_queue_.callAvailable(ros::WallDuration(timeout));
00666   }
00667 }
00668 }


gazebo_plugins
Author(s): John Hsu
autogenerated on Thu Feb 23 2017 03:43:22