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


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