gazebo_ros_prosilica.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: GazeboRosProsilica plugin for simulating cameras in Gazebo
00024    Author: John Hsu
00025    Date: 24 Sept 2008
00026    SVN info: $Id$
00027  @htmlinclude manifest.html
00028  @b GazeboRosProsilica plugin mimics after prosilica_camera package
00029  */
00030 
00031 #include <algorithm>
00032 #include <assert.h>
00033 
00034 #include <pr2_gazebo_plugins/gazebo_ros_prosilica.h>
00035 
00036 #include "physics/World.hh"
00037 #include "physics/HingeJoint.hh"
00038 #include "sensors/Sensor.hh"
00039 #include "sdf/interface/SDF.hh"
00040 #include "sdf/interface/Param.hh"
00041 #include "common/Exception.hh"
00042 #include "sensors/CameraSensor.hh"
00043 #include "sensors/SensorTypes.hh"
00044 #include "rendering/Camera.hh"
00045 
00046 #include <sensor_msgs/Image.h>
00047 #include <sensor_msgs/CameraInfo.h>
00048 #include <sensor_msgs/fill_image.h>
00049 #include <diagnostic_updater/diagnostic_updater.h>
00050 #include <sensor_msgs/RegionOfInterest.h>
00051 
00052 #include <opencv/cv.h>
00053 #include <opencv/highgui.h>
00054 
00055 #include <cv.h>
00056 #include <cvwimage.h>
00057 
00058 #include <boost/scoped_ptr.hpp>
00059 #include <boost/bind.hpp>
00060 #include <boost/tokenizer.hpp>
00061 #include <boost/thread.hpp>
00062 #include <boost/thread/recursive_mutex.hpp>
00063 #include <string>
00064 
00065 namespace gazebo
00066 {
00067 
00069 // Constructor
00070 GazeboRosProsilica::GazeboRosProsilica()
00071 {
00072 }
00073 
00075 // Destructor
00076 GazeboRosProsilica::~GazeboRosProsilica()
00077 {
00078   // Finalize the controller
00079   this->poll_srv_.shutdown();
00080 }
00081 
00083 // Load the controller
00084 void GazeboRosProsilica::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf)
00085 {
00086 
00087   DepthCameraPlugin::Load(_parent, _sdf);
00088   this->parentSensor_ = this->parentSensor;
00089   this->width_ = this->width;
00090   this->height_ = this->height;
00091   this->depth_ = this->depth;
00092   this->format_ = this->format;
00093   this->camera_ = this->depthCamera;
00094   GazeboRosCameraUtils::Load(_parent, _sdf);
00095 
00096   // camera mode for prosilica:
00097   // prosilica::AcquisitionMode mode_; /// @todo Make this property of Camera
00098 
00099   //ROS_ERROR("before trigger_mode %s %s",this->mode_param_name.c_str(),this->mode_.c_str());
00100 
00101   if (!this->rosnode_->searchParam("trigger_mode",this->mode_param_name)) 
00102       this->mode_param_name = "trigger_mode";
00103 
00104   if (!this->rosnode_->getParam(this->mode_param_name,this->mode_))
00105       this->mode_ = "streaming";
00106 
00107   ROS_INFO("trigger_mode %s %s",this->mode_param_name.c_str(),this->mode_.c_str());
00108 
00109 
00110   if (this->mode_ == "polled")
00111   {
00112       poll_srv_ = polled_camera::advertise(*this->rosnode_,this->pollServiceName,&GazeboRosProsilica::pollCallback,this);
00113   }
00114   else if (this->mode_ == "streaming")
00115   {
00116       ROS_DEBUG("do nothing here,mode: %s",this->mode_.c_str());
00117   }
00118   else
00119   {
00120       ROS_ERROR("trigger_mode is invalid: %s, using streaming mode",this->mode_.c_str());
00121   }
00122 }
00123 
00125 // Update the controller
00126 void GazeboRosProsilica::OnNewImageFrame(const unsigned char *_image, 
00127     unsigned int _width, unsigned int _height, unsigned int _depth, 
00128     const std::string &_format)
00129 {
00130   if (!this->rosnode_->getParam(this->mode_param_name,this->mode_))
00131       this->mode_ = "streaming";
00132 
00133   // should do nothing except turning camera on/off, as we are using service.
00135   common::Time sensor_update_time = this->parentSensor_->GetLastUpdateTime();
00136 
00137   // as long as ros is connected, parent is active
00138   //ROS_ERROR("debug image count %d",this->image_connect_count_);
00139   if (!this->parentSensor->IsActive())
00140   {
00141     if (this->image_connect_count_ > 0)
00142       // do this first so there's chance for sensor to run 1 frame after activate
00143       this->parentSensor->SetActive(true);
00144   }
00145   else
00146   {
00147     //ROS_ERROR("camera_ new frame %s %s",this->parentSensor_->GetName().c_str(),this->frame_name_.c_str());
00148 
00149     if (this->mode_ == "streaming")
00150     {
00151       if (this->image_connect_count_ > 0)
00152       {
00153         common::Time cur_time = this->world_->GetSimTime();
00154         if (cur_time - this->last_update_time_ >= this->update_period_)
00155         {
00156           this->PutCameraData(_image, sensor_update_time);
00157           this->last_update_time_ = cur_time;
00158         }
00159       }
00160     }
00161 
00162   }
00164   if (this->info_connect_count_ > 0)
00165   {
00166     common::Time cur_time = this->world_->GetSimTime();
00167     if (cur_time - this->last_info_update_time_ >= this->update_period_)
00168     {
00169       this->PublishCameraInfo(sensor_update_time);
00170       this->last_info_update_time_ = cur_time;
00171     }
00172   }
00173 }
00174 
00175 
00177 // new prosilica interface.
00178 void GazeboRosProsilica::pollCallback(polled_camera::GetPolledImage::Request& req,
00179                                       polled_camera::GetPolledImage::Response& rsp,
00180                                       sensor_msgs::Image& image, sensor_msgs::CameraInfo& info)
00181 {
00182   if (!this->rosnode_->getParam(this->mode_param_name,this->mode_))
00183       this->mode_ = "streaming";
00184 
00188 
00189   if (this->mode_ != "polled")
00190   {
00191     rsp.success = false;
00192     rsp.status_message = "Camera is not in triggered mode";
00193     return;
00194   }
00195 
00196   if (req.binning_x > 1 || req.binning_y > 1)
00197   {
00198     rsp.success = false;
00199     rsp.status_message = "Gazebo Prosilica plugin does not support binning";
00200     return;
00201   }
00202 
00203   // get region from request
00204   if (req.roi.x_offset <= 0 || req.roi.y_offset <= 0 || req.roi.width <= 0 || req.roi.height <= 0)
00205   {
00206     req.roi.x_offset = 0;
00207     req.roi.y_offset = 0;
00208     req.roi.width = this->width_;
00209     req.roi.height = this->height;
00210   }
00211   const unsigned char *src = NULL;
00212   ROS_ERROR("roidebug %d %d %d %d", req.roi.x_offset, req.roi.y_offset, req.roi.width, req.roi.height);
00213 
00214   // signal sensor to start update
00215   this->image_connect_count_++;
00216 
00217   // wait until an image has been returned
00218   while(!src)
00219   {
00220     {
00221       // Get a pointer to image data
00222       src = this->parentSensor->GetDepthCamera()->GetImageData(0);
00223 
00224       if (src)
00225       {
00226 
00227         // fill CameraInfo
00228         this->roiCameraInfoMsg = &info;
00229         this->roiCameraInfoMsg->header.frame_id = this->frame_name_;
00230 
00231         common::Time roiLastRenderTime = this->parentSensor_->GetLastUpdateTime();
00232         this->roiCameraInfoMsg->header.stamp.sec = roiLastRenderTime.sec;
00233         this->roiCameraInfoMsg->header.stamp.nsec = roiLastRenderTime.nsec;
00234 
00235         this->roiCameraInfoMsg->width  = req.roi.width; //this->parentSensor->GetImageWidth() ;
00236         this->roiCameraInfoMsg->height = req.roi.height; //this->parentSensor->GetImageHeight();
00237         // distortion
00238 #if ROS_VERSION_MINIMUM(1, 3, 0)
00239         this->roiCameraInfoMsg->distortion_model = "plumb_bob";
00240         this->roiCameraInfoMsg->D.resize(5);
00241 #endif
00242         this->roiCameraInfoMsg->D[0] = this->distortion_k1_;
00243         this->roiCameraInfoMsg->D[1] = this->distortion_k2_;
00244         this->roiCameraInfoMsg->D[2] = this->distortion_k3_;
00245         this->roiCameraInfoMsg->D[3] = this->distortion_t1_;
00246         this->roiCameraInfoMsg->D[4] = this->distortion_t2_;
00247         // original camera matrix
00248         this->roiCameraInfoMsg->K[0] = this->focal_length_;
00249         this->roiCameraInfoMsg->K[1] = 0.0;
00250         this->roiCameraInfoMsg->K[2] = this->cx_ - req.roi.x_offset;
00251         this->roiCameraInfoMsg->K[3] = 0.0;
00252         this->roiCameraInfoMsg->K[4] = this->focal_length_;
00253         this->roiCameraInfoMsg->K[5] = this->cy_ - req.roi.y_offset;
00254         this->roiCameraInfoMsg->K[6] = 0.0;
00255         this->roiCameraInfoMsg->K[7] = 0.0;
00256         this->roiCameraInfoMsg->K[8] = 1.0;
00257         // rectification
00258         this->roiCameraInfoMsg->R[0] = 1.0;
00259         this->roiCameraInfoMsg->R[1] = 0.0;
00260         this->roiCameraInfoMsg->R[2] = 0.0;
00261         this->roiCameraInfoMsg->R[3] = 0.0;
00262         this->roiCameraInfoMsg->R[4] = 1.0;
00263         this->roiCameraInfoMsg->R[5] = 0.0;
00264         this->roiCameraInfoMsg->R[6] = 0.0;
00265         this->roiCameraInfoMsg->R[7] = 0.0;
00266         this->roiCameraInfoMsg->R[8] = 1.0;
00267         // camera projection matrix (same as camera matrix due to lack of distortion/rectification) (is this generated?)
00268         this->roiCameraInfoMsg->P[0] = this->focal_length_;
00269         this->roiCameraInfoMsg->P[1] = 0.0;
00270         this->roiCameraInfoMsg->P[2] = this->cx_ - req.roi.x_offset;
00271         this->roiCameraInfoMsg->P[3] = -this->focal_length_ * this->hack_baseline_;
00272         this->roiCameraInfoMsg->P[4] = 0.0;
00273         this->roiCameraInfoMsg->P[5] = this->focal_length_;
00274         this->roiCameraInfoMsg->P[6] = this->cy_ - req.roi.y_offset;
00275         this->roiCameraInfoMsg->P[7] = 0.0;
00276         this->roiCameraInfoMsg->P[8] = 0.0;
00277         this->roiCameraInfoMsg->P[9] = 0.0;
00278         this->roiCameraInfoMsg->P[10] = 1.0;
00279         this->roiCameraInfoMsg->P[11] = 0.0;
00280         this->camera_info_pub_.publish(*this->roiCameraInfoMsg);
00281 
00282         // copy data into image_msg_, then convert to roiImageMsg(image)
00283         this->image_msg_.header.frame_id    = this->frame_name_;
00284 
00285         common::Time lastRenderTime = this->parentSensor_->GetLastUpdateTime();
00286         this->image_msg_.header.stamp.sec = lastRenderTime.sec;
00287         this->image_msg_.header.stamp.nsec = lastRenderTime.nsec;
00288 
00289         //unsigned char dst[this->width_*this->height];
00290 
00292 
00293         // copy from src to image_msg_
00294         fillImage(this->image_msg_,
00295                   this->type_,
00296                   this->height_,
00297                   this->width_,
00298                   this->skip_*this->width_,
00299                   (void*)src );
00300 
00302 
00303         this->image_pub_.publish(this->image_msg_);
00304 
00305         {
00306           // copy data into ROI image
00307           this->roiImageMsg = &image;
00308           this->roiImageMsg->header.frame_id = this->frame_name_;
00309           common::Time roiLastRenderTime = this->parentSensor_->GetLastUpdateTime();
00310           this->roiImageMsg->header.stamp.sec = roiLastRenderTime.sec;
00311           this->roiImageMsg->header.stamp.nsec = roiLastRenderTime.nsec;
00312 
00313           // convert image_msg_ to a CvImage using cv_bridge
00314           boost::shared_ptr<cv_bridge::CvImage> img_bridge_;
00315           img_bridge_ = cv_bridge::toCvCopy(this->image_msg_);
00316 
00317           // for debug
00318           //cvNamedWindow("showme",CV_WINDOW_AUTOSIZE);
00319           //cvSetMouseCallback("showme", &GazeboRosProsilica::mouse_cb, this);
00320           //cvStartWindowThread();
00321           //cvShowImage("showme",img_bridge_.toIpl());
00322 
00323           // crop image to roi
00324           cv::Mat roi(img_bridge_->image,
00325             cv::Rect(req.roi.x_offset, req.roi.y_offset,
00326                      req.roi.width, req.roi.height));
00327           img_bridge_->image = roi;
00328 
00329           // copy roi'd image into roiImageMsg
00330           img_bridge_->toImageMsg(*this->roiImageMsg);
00331         }
00332       }
00333     }
00334     usleep(100000);
00335   }
00336   this->image_connect_count_--;
00337   rsp.success = true;
00338   return;
00339 }
00340 
00341 
00342 /*
00343 void GazeboRosProsilica::OnStats( const boost::shared_ptr<msgs::WorldStatistics const> &_msg)
00344 {
00345   this->simTime  = msgs::Convert( _msg->sim_time() );
00346 
00347   math::Pose pose;
00348   pose.pos.x = 0.5*sin(0.01*this->simTime.Double());
00349   gzdbg << "plugin simTime [" << this->simTime.Double() << "] update pose [" << pose.pos.x << "]\n";
00350 }
00351 */
00352 
00353 // Register this plugin with the simulator
00354 GZ_REGISTER_SENSOR_PLUGIN(GazeboRosProsilica)
00355 
00356 
00357 }


pr2_gazebo_plugins
Author(s): Sachin Chitta, Stu Glaser, John Hsu
autogenerated on Thu Apr 24 2014 15:47:43