$search
00001 /* 00002 * gazebo thermal camera emulator plugin 00003 * Copyright (C) 2012 00004 * Stefan Kohlbrecher, TU Darmstadt 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 //based on: 00023 /* 00024 * Gazebo - Outdoor Multi-Robot Simulator 00025 * Copyright (C) 2003 00026 * Nate Koenig & Andrew Howard 00027 * 00028 * This program is free software; you can redistribute it and/or modify 00029 * it under the terms of the GNU General Public License as published by 00030 * the Free Software Foundation; either version 2 of the License, or 00031 * (at your option) any later version. 00032 * 00033 * This program is distributed in the hope that it will be useful, 00034 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00035 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00036 * GNU General Public License for more details. 00037 * 00038 * You should have received a copy of the GNU General Public License 00039 * along with this program; if not, write to the Free Software 00040 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 00041 * 00042 */ 00043 /* 00044 @mainpage 00045 Desc: GazeboRosThermalCamera plugin for simulating cameras in Gazebo 00046 Author: John Hsu 00047 Date: 24 Sept 2008 00048 SVN info: $Id$ 00049 @htmlinclude manifest.html 00050 @b GazeboRosThermalCamera plugin broadcasts ROS Image messages 00051 */ 00052 00053 #include <algorithm> 00054 #include <assert.h> 00055 #include <boost/thread/thread.hpp> 00056 #include <boost/bind.hpp> 00057 00058 #include <hector_gazebo_thermal_camera/gazebo_ros_thermal_camera.h> 00059 00060 #include <gazebo/Timer.hh> 00061 #include <gazebo/Sensor.hh> 00062 #include <gazebo/Model.hh> 00063 #include <gazebo/Global.hh> 00064 #include <gazebo/XMLConfig.hh> 00065 #include <gazebo/Simulator.hh> 00066 #include <gazebo/gazebo.h> 00067 #include <gazebo/GazeboError.hh> 00068 #include <gazebo/ControllerFactory.hh> 00069 #include <gazebo/MonoCameraSensor.hh> 00070 #include <gazebo/Pose3d.hh> 00071 00072 00073 #include "sensor_msgs/Image.h" 00074 #include "sensor_msgs/fill_image.h" 00075 #include "image_transport/image_transport.h" 00076 00077 #include <geometry_msgs/Point32.h> 00078 #include <sensor_msgs/ChannelFloat32.h> 00079 00080 namespace gazebo 00081 { 00082 00083 GZ_REGISTER_DYNAMIC_CONTROLLER("gazebo_ros_thermal_camera", GazeboRosThermalCamera); 00084 00086 // Constructor 00087 GazeboRosThermalCamera::GazeboRosThermalCamera(Entity *parent) 00088 : Controller(parent) 00089 { 00090 this->myParent = dynamic_cast<MonoCameraSensor*>(this->parent); 00091 00092 if (!this->myParent) 00093 gzthrow("GazeboRosThermalCamera controller requires a Camera Sensor as its parent"); 00094 00095 Param::Begin(&this->parameters); 00096 this->robotNamespaceP = new ParamT<std::string>("robotNamespace","/",0); 00097 this->imageTopicNameP = new ParamT<std::string>("imageTopicName","image_raw", 0); 00098 this->cameraInfoTopicNameP = new ParamT<std::string>("cameraInfoTopicName","camera_info", 0); 00099 this->pointCloudTopicNameP = new ParamT<std::string>("pointCloudTopicName","points", 0); 00100 this->cameraNameP = new ParamT<std::string>("cameraName","", 0); 00101 this->frameNameP = new ParamT<std::string>("frameName","generic_camera_link", 0); 00102 // camera parameters 00103 this->CxPrimeP = new ParamT<double>("CxPrime",0, 0); // default to 0 for compute on the fly 00104 this->CxP = new ParamT<double>("Cx" ,0, 0); // default to 0 for compute on the fly 00105 this->CyP = new ParamT<double>("Cy" ,0, 0); // default to 0 for compute on the fly 00106 this->focal_lengthP = new ParamT<double>("focal_length" ,0, 0); // == image_width(px) / (2*tan( hfov(radian) /2)), default to 0 for compute on the fly 00107 this->hack_baselineP = new ParamT<double>("hackBaseline" ,0, 0); // hack for right stereo camera 00108 this->pointCloudCutoffP = new ParamT<double>("pointCloudCutoff" ,0.4, 0); 00109 this->distortion_k1P = new ParamT<double>("distortion_k1" ,0, 0); 00110 this->distortion_k2P = new ParamT<double>("distortion_k2" ,0, 0); 00111 this->distortion_k3P = new ParamT<double>("distortion_k3" ,0, 0); 00112 this->distortion_t1P = new ParamT<double>("distortion_t1" ,0, 0); 00113 this->distortion_t2P = new ParamT<double>("distortion_t2" ,0, 0); 00114 Param::End(); 00115 00116 this->imageConnectCount = 0; 00117 this->infoConnectCount = 0; 00118 this->pointCloudConnectCount = 0; 00119 00120 last_image_pub_time_ = Time(0); 00121 last_camera_info_pub_time_ = Time(0); 00122 last_point_cloud_pub_time_ = Time(0); 00123 00124 // set sensor update rate to controller update rate? 00125 //(dynamic_cast<OgreCamera*>(this->myParent))->SetUpdateRate(this->updateRateP->GetValue()); 00126 } 00127 00128 void GazeboRosThermalCamera::configCallback(hector_gazebo_thermal_camera::GazeboRosThermalCameraConfig &config, uint32_t level) 00129 { 00130 ROS_INFO("Reconfigure request for the gazebo ros camera: %s. New rate: %.2f", this->cameraName.c_str(), config.imager_rate); 00131 00132 (dynamic_cast<OgreCamera*>(this->myParent))->SetUpdateRate(config.imager_rate); 00133 } 00134 00136 // Destructor 00137 GazeboRosThermalCamera::~GazeboRosThermalCamera() 00138 { 00139 delete this->robotNamespaceP; 00140 delete this->rosnode_; 00141 delete this->managernode_; 00142 delete this->manager_; 00143 delete this->imageTopicNameP; 00144 delete this->cameraInfoTopicNameP; 00145 delete this->pointCloudTopicNameP; 00146 delete this->frameNameP; 00147 delete this->CxPrimeP; 00148 delete this->CxP; 00149 delete this->CyP; 00150 delete this->focal_lengthP; 00151 delete this->hack_baselineP; 00152 delete this->pointCloudCutoffP; 00153 delete this->distortion_k1P; 00154 delete this->distortion_k2P; 00155 delete this->distortion_k3P; 00156 delete this->distortion_t1P; 00157 delete this->distortion_t2P; 00158 } 00159 00161 // Load the controller 00162 void GazeboRosThermalCamera::LoadChild(XMLConfigNode *node) 00163 { 00164 this->robotNamespaceP->Load(node); 00165 this->robotNamespace = this->robotNamespaceP->GetValue(); 00166 if (!ros::isInitialized()) 00167 { 00168 int argc = 0; 00169 char** argv = NULL; 00170 ros::init(argc,argv,"gazebo",ros::init_options::NoSigintHandler|ros::init_options::AnonymousName); 00171 } 00172 00173 this->imageTopicNameP->Load(node); 00174 this->cameraInfoTopicNameP->Load(node); 00175 this->pointCloudTopicNameP->Load(node); 00176 this->cameraNameP->Load(node); 00177 this->frameNameP->Load(node); 00178 this->CxPrimeP->Load(node); 00179 this->CxP->Load(node); 00180 this->CyP->Load(node); 00181 this->focal_lengthP->Load(node); 00182 this->hack_baselineP->Load(node); 00183 this->pointCloudCutoffP->Load(node); 00184 this->distortion_k1P->Load(node); 00185 this->distortion_k2P->Load(node); 00186 this->distortion_k3P->Load(node); 00187 this->distortion_t1P->Load(node); 00188 this->distortion_t2P->Load(node); 00189 this->imageTopicName = this->imageTopicNameP->GetValue(); 00190 this->cameraInfoTopicName = this->cameraInfoTopicNameP->GetValue(); 00191 this->pointCloudTopicName = this->pointCloudTopicNameP->GetValue(); 00192 this->frameName = this->frameNameP->GetValue(); 00193 this->CxPrime = this->CxPrimeP->GetValue(); 00194 this->Cx = this->CxP->GetValue(); 00195 this->Cy = this->CyP->GetValue(); 00196 this->focal_length = this->focal_lengthP->GetValue(); 00197 this->hack_baseline = this->hack_baselineP->GetValue(); 00198 this->pointCloudCutoff = this->pointCloudCutoffP->GetValue(); 00199 this->distortion_k1 = this->distortion_k1P->GetValue(); 00200 this->distortion_k2 = this->distortion_k2P->GetValue(); 00201 this->distortion_k3 = this->distortion_k3P->GetValue(); 00202 this->distortion_t1 = this->distortion_t1P->GetValue(); 00203 this->distortion_t2 = this->distortion_t2P->GetValue(); 00204 if ((this->distortion_k1 != 0.0) || (this->distortion_k2 != 0.0) || 00205 (this->distortion_k3 != 0.0) || (this->distortion_t1 != 0.0) || 00206 (this->distortion_t2 != 0.0)) 00207 ROS_WARN("gazebo_ros_camera simulation does not support non-zero distortion parameters right now, your simulation maybe wrong."); 00208 00209 this->cameraName = this->cameraNameP->GetValue(); 00210 this->rosnode_ = new ros::NodeHandle(this->robotNamespace+"/"+this->cameraName); 00211 if (!this->cameraName.empty()) 00212 this->managernode_ = new ros::NodeHandle(this->robotNamespace+"/"+this->cameraName+"_manager"); 00213 else 00214 this->managernode_ = new ros::NodeHandle(this->robotNamespace+"/"+this->GetName()+"_manager"); 00215 this->manager_ = new nodelet::Loader(*this->managernode_); 00216 this->itnode_ = new image_transport::ImageTransport(*this->rosnode_); 00217 00218 if (!this->cameraName.empty()) { 00219 dyn_srv_ = new dynamic_reconfigure::Server<hector_gazebo_thermal_camera::GazeboRosThermalCameraConfig>(*this->rosnode_); 00220 dynamic_reconfigure::Server<hector_gazebo_thermal_camera::GazeboRosThermalCameraConfig>::CallbackType f = boost::bind(&GazeboRosThermalCamera::configCallback, this, _1, _2); 00221 dyn_srv_->setCallback(f); 00222 } 00223 00224 00225 #ifdef USE_CBQ 00226 this->image_pub_ = this->itnode_->advertise( 00227 this->imageTopicName,1, 00228 boost::bind( &GazeboRosThermalCamera::ImageConnect,this), 00229 boost::bind( &GazeboRosThermalCamera::ImageDisconnect,this), ros::VoidPtr(), &this->camera_queue_); 00230 00231 ros::AdvertiseOptions camera_info_ao = ros::AdvertiseOptions::create<sensor_msgs::CameraInfo>( 00232 this->cameraInfoTopicName,1, 00233 boost::bind( &GazeboRosThermalCamera::InfoConnect,this), 00234 boost::bind( &GazeboRosThermalCamera::InfoDisconnect,this), ros::VoidPtr(), &this->camera_queue_); 00235 this->camera_info_pub_ = this->rosnode_->advertise(camera_info_ao); 00236 00237 ros::SubscribeOptions zoom_so = ros::SubscribeOptions::create<std_msgs::Float64>( 00238 "set_hfov",1, 00239 boost::bind( &GazeboRosThermalCamera::SetHFOV,this,_1), 00240 ros::VoidPtr(), &this->camera_queue_); 00241 this->cameraHFOVSubscriber_ = this->rosnode_->subscribe(zoom_so); 00242 00243 ros::SubscribeOptions rate_so = ros::SubscribeOptions::create<std_msgs::Float64>( 00244 "set_update_rate",1, 00245 boost::bind( &GazeboRosThermalCamera::SetUpdateRate,this,_1), 00246 ros::VoidPtr(), &this->camera_queue_); 00247 this->cameraUpdateRateSubscriber_ = this->rosnode_->subscribe(rate_so); 00248 00249 ros::AdvertiseOptions point_cloud_ao = ros::AdvertiseOptions::create<pcl::PointCloud<pcl::PointXYZ> >( 00250 this->pointCloudTopicName,1, 00251 boost::bind( &GazeboRosThermalCamera::PointCloudConnect,this), 00252 boost::bind( &GazeboRosThermalCamera::PointCloudDisconnect,this), ros::VoidPtr(), &this->camera_queue_); 00253 this->point_cloud_pub_ = this->rosnode_->advertise(point_cloud_ao); 00254 00255 #else 00256 this->image_pub_ = this->rosnode_->advertise<sensor_msgs::Image>(this->imageTopicName,1, 00257 boost::bind( &GazeboRosThermalCamera::ImageConnect, this), 00258 boost::bind( &GazeboRosThermalCamera::ImageDisconnect, this)); 00259 this->camera_info_pub_ = this->rosnode_->advertise<sensor_msgs::CameraInfo>(this->cameraInfoTopicName,1, 00260 boost::bind( &GazeboRosThermalCamera::InfoConnect, this), 00261 boost::bind( &GazeboRosThermalCamera::InfoDisconnect, this)); 00262 this->point_cloud_pub_ = this->rosnode_->advertise<pcl::PointCloud<pcl::PointXYZ> >(this->pointCloudTopicName,1, 00263 boost::bind( &GazeboRosThermalCamera::PointCloudConnect, this), 00264 boost::bind( &GazeboRosThermalCamera::PointCloudDisconnect, this)); 00265 #endif 00266 } 00267 00269 // Increment count 00270 void GazeboRosThermalCamera::InfoConnect() 00271 { 00272 this->infoConnectCount++; 00273 } 00275 // Decrement count 00276 void GazeboRosThermalCamera::InfoDisconnect() 00277 { 00278 this->infoConnectCount--; 00279 } 00280 00282 // Set Horizontal Field of View 00283 void GazeboRosThermalCamera::SetHFOV(const std_msgs::Float64::ConstPtr& hfov) 00284 { 00285 (dynamic_cast<OgreCamera*>(this->myParent))->SetFOV(hfov->data); 00286 } 00287 00289 // Set Update Rate 00290 void GazeboRosThermalCamera::SetUpdateRate(const std_msgs::Float64::ConstPtr& update_rate) 00291 { 00292 (dynamic_cast<OgreCamera*>(this->myParent))->SetUpdateRate(update_rate->data); 00293 } 00294 00296 // Increment count 00297 void GazeboRosThermalCamera::ImageConnect() 00298 { 00299 this->imageConnectCount++; 00300 this->myParent->SetActive(true); 00301 } 00303 // Decrement count 00304 void GazeboRosThermalCamera::ImageDisconnect() 00305 { 00306 this->imageConnectCount--; 00307 00308 if (this->pointCloudConnectCount == 0) 00309 { 00310 this->myParent->SimulateDepthData(false); 00311 if (this->imageConnectCount == 0) 00312 this->myParent->SetActive(false); 00313 } 00314 } 00315 00317 // Increment count 00318 void GazeboRosThermalCamera::PointCloudConnect() 00319 { 00320 this->pointCloudConnectCount++; 00321 this->myParent->SetActive(true); 00322 this->myParent->SimulateDepthData(true); 00323 } 00325 // Decrement count 00326 void GazeboRosThermalCamera::PointCloudDisconnect() 00327 { 00328 this->pointCloudConnectCount--; 00329 00330 if (this->pointCloudConnectCount == 0) 00331 { 00332 this->myParent->SimulateDepthData(false); 00333 if (this->imageConnectCount == 0) 00334 this->myParent->SetActive(false); 00335 } 00336 } 00337 00339 // Initialize the controller 00340 void GazeboRosThermalCamera::InitChild() 00341 { 00342 // sensor generation off by default 00343 this->myParent->SetActive(false); 00344 this->myParent->SimulateDepthData(false); 00345 00346 // set buffer size 00347 this->width = this->myParent->GetImageWidth(); 00348 this->height = this->myParent->GetImageHeight(); 00349 this->depth = this->myParent->GetImageDepth(); 00350 if (this->myParent->GetImageFormat() == "L8") 00351 { 00352 this->type = sensor_msgs::image_encodings::MONO8; 00353 this->skip = 1; 00354 } 00355 else if (this->myParent->GetImageFormat() == "R8G8B8") 00356 { 00357 this->type = sensor_msgs::image_encodings::RGB8; 00358 this->skip = 3; 00359 } 00360 else if (this->myParent->GetImageFormat() == "B8G8R8") 00361 { 00362 this->type = sensor_msgs::image_encodings::BGR8; 00363 this->skip = 3; 00364 } 00365 else if (this->myParent->GetImageFormat() == "BAYER_RGGB8") 00366 { 00367 ROS_WARN("bayer simulation maybe computationally expensive."); 00368 this->type = sensor_msgs::image_encodings::BAYER_RGGB8; 00369 this->skip = 1; 00370 } 00371 else if (this->myParent->GetImageFormat() == "BAYER_BGGR8") 00372 { 00373 ROS_WARN("bayer simulation maybe computationally expensive."); 00374 this->type = sensor_msgs::image_encodings::BAYER_BGGR8; 00375 this->skip = 1; 00376 } 00377 else if (this->myParent->GetImageFormat() == "BAYER_GBRG8") 00378 { 00379 ROS_WARN("bayer simulation maybe computationally expensive."); 00380 this->type = sensor_msgs::image_encodings::BAYER_GBRG8; 00381 this->skip = 1; 00382 } 00383 else if (this->myParent->GetImageFormat() == "BAYER_GRBG8") 00384 { 00385 ROS_WARN("bayer simulation maybe computationally expensive."); 00386 this->type = sensor_msgs::image_encodings::BAYER_GRBG8; 00387 this->skip = 1; 00388 } 00389 else 00390 { 00391 ROS_ERROR("Unsupported Gazebo ImageFormat\n"); 00392 this->type = sensor_msgs::image_encodings::BGR8; 00393 this->skip = 3; 00394 } 00395 00397 if (this->CxPrime == 0) 00398 this->CxPrime = ((double)this->width+1.0) /2.0; 00399 if (this->Cx == 0) 00400 this->Cx = ((double)this->width+1.0) /2.0; 00401 if (this->Cy == 0) 00402 this->Cy = ((double)this->height+1.0) /2.0; 00403 00404 00405 double computed_focal_length = ((double)this->width) / (2.0 *tan(this->myParent->GetHFOV().GetAsRadian()/2.0)); 00406 if (this->focal_length == 0) 00407 this->focal_length = computed_focal_length; 00408 else 00409 if (fabs(this->focal_length - computed_focal_length) > 1e-8) // check against float precision 00410 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.", 00411 this->focal_length,this->myParent->GetName().c_str(),this->width,this->myParent->GetHFOV().GetAsRadian(), 00412 computed_focal_length); 00413 00414 00415 #ifdef USE_CBQ 00416 // start custom queue for camera 00417 this->callback_queue_thread_ = boost::thread( boost::bind( &GazeboRosThermalCamera::CameraQueueThread,this ) ); 00418 #endif 00419 00420 00421 } 00422 00424 // Update the controller 00425 void GazeboRosThermalCamera::UpdateChild() 00426 { 00427 00428 // Only publish to ROS when the camera updates 00429 Time sensor_update_time = (dynamic_cast<OgreCamera*>(this->myParent))->GetLastRenderTime(); 00430 00431 // as long as ros is connected, parent is active 00432 if (this->myParent->IsActive()) 00433 { 00434 if (sensor_update_time > last_image_pub_time_) 00435 this->PutCameraData(); 00436 } 00437 00439 if (this->infoConnectCount > 0) //publish regardless camera image subscription 00440 this->PublishCameraInfo(); 00441 } 00442 00444 // Finalize the controller 00445 void GazeboRosThermalCamera::FiniChild() 00446 { 00447 this->myParent->SetActive(false); 00448 this->manager_->clear(); 00449 this->managernode_->shutdown(); 00450 this->rosnode_->shutdown(); 00451 #ifdef USE_CBQ 00452 this->camera_queue_.clear(); 00453 this->camera_queue_.disable(); 00454 this->callback_queue_thread_.join(); 00455 #endif 00456 00457 } 00458 00460 // Put laser data to the interface 00461 void GazeboRosThermalCamera::PutCameraData() 00462 { 00463 //ROS_ERROR("debug %d %s",this->imageConnectCount,this->GetName().c_str()); 00464 const unsigned char *src; 00465 const float* depths; 00466 00467 // Get a pointer to image data 00468 { 00469 // prevent parent from updating the image and the depth data while we are retrieving it 00470 boost::recursive_mutex::scoped_lock mr_lock(*Simulator::Instance()->GetMRMutex()); 00471 00472 //DIAGNOSTICTIMER(timer("gazebo_ros_camera: GetImageData",6)); 00473 src = this->myParent->GetImageData(0); 00474 if (src) 00475 { 00476 //double tmpT0 = Simulator::Instance()->GetWallTime(); 00477 00478 unsigned char dst[this->width*this->height]; 00479 00480 this->lock.lock(); 00481 // copy data into image 00482 this->imageMsg.header.frame_id = this->frameName; 00483 Time lastRenderTime = (dynamic_cast<OgreCamera*>(this->myParent))->GetLastRenderTime(); 00484 //Time lastRenderTime = Simulator::Instance()->GetSimTime(); 00485 //printf("name[%s] render[%f] vs. sim time[%f], diff[%f]\n",this->GetName().c_str(),lastRenderTime.Double(),Simulator::Instance()->GetSimTime().Double(),lastRenderTime.Double()-Simulator::Instance()->GetSimTime().Double()); 00486 //ROS_DEBUG("camera time %f %d %d",lastRenderTime.Double(),lastRenderTime.sec,lastRenderTime.nsec); 00487 this->imageMsg.header.stamp.sec = lastRenderTime.sec; 00488 this->imageMsg.header.stamp.nsec = lastRenderTime.nsec; 00489 00490 //double tmpT1 = Simulator::Instance()->GetWallTime(); 00491 //double tmpT2; 00492 00494 if (this->image_pub_.getNumSubscribers() > 0) 00495 { 00496 00497 //convertImageFormat(dst,src); 00498 00499 // copy from src to imageMsg 00500 /* 00501 fillImage(this->imageMsg, 00502 this->type, 00503 this->height, 00504 this->width, 00505 this->skip*this->width, 00506 (void*)src ); 00507 */ 00508 00509 this->imageMsg.encoding = sensor_msgs::image_encodings::MONO8; 00510 this->imageMsg.width = this->width; 00511 this->imageMsg.height = this->height; 00512 00513 size_t size = this->width * this->height; 00514 00515 std::vector<uint8_t>& data (this->imageMsg.data); 00516 data.resize(size); 00517 00518 size_t img_index = 0; 00519 00520 for (size_t i = 0; i < size; ++i){ 00521 if (src[img_index] >254 && src[img_index+1] < 1 && src[img_index+2 < 1]){ 00522 //RGB [255,0,0] translates to white (white hot) 00523 data[i]= 255; 00524 }else{ 00525 //Everything else is written to the MONO8 output image much darker 00526 data[i]= (src[img_index] + src[img_index+1] + src[img_index+2]) /8 ; 00527 } 00528 img_index += 3; 00529 } 00530 00531 00532 //tmpT2 = Simulator::Instance()->GetWallTime(); 00533 00534 // publish to ros 00535 last_image_pub_time_ = Simulator::Instance()->GetSimTime(); 00536 this->image_pub_.publish(this->imageMsg); 00537 } 00538 00539 //double tmpT3 = Simulator::Instance()->GetWallTime(); 00540 00541 this->lock.unlock(); 00542 } 00543 } 00544 00545 { 00546 // prevent parent from updating the image and the depth data while we are retrieving it 00547 boost::recursive_mutex::scoped_lock mr_lock(*Simulator::Instance()->GetMRMutex()); 00548 00549 depths = this->myParent->GetDepthData(0); 00550 if (depths) 00551 { 00552 //double tmpT0 = Simulator::Instance()->GetWallTime(); 00553 this->lock.lock(); 00554 this->pointCloudMsg.header.frame_id = this->frameName; 00555 Time lastRenderTime = (dynamic_cast<OgreCamera*>(this->myParent))->GetLastRenderTime(); 00556 this->pointCloudMsg.header.stamp.sec = lastRenderTime.sec; 00557 this->pointCloudMsg.header.stamp.nsec = lastRenderTime.nsec; 00558 this->pointCloudMsg.width = this->width; 00559 this->pointCloudMsg.height = this->height; 00560 00562 fillDepthImage(this->pointCloudMsg, 00563 this->height, 00564 this->width, 00565 this->skip, 00566 (void*)depths ); 00567 /* 00568 for (int i = 0 ; i < this->height; i++) 00569 for (int j = 0 ; j < this->width; i++) { 00570 ROS_ERROR("test %d %d %f",i,j,depths[j + i*this->width]); 00571 } 00572 */ 00573 00574 //tmpT2 = Simulator::Instance()->GetWallTime(); 00575 00576 // publish to ros 00577 this->point_cloud_pub_.publish(this->pointCloudMsg); 00578 00579 //double tmpT3 = Simulator::Instance()->GetWallTime(); 00580 00581 this->lock.unlock(); 00582 } 00583 } 00584 } 00585 00586 00588 // Convert image format from gazebo sensor provided foramt to desired ros format 00589 void GazeboRosThermalCamera::convertImageFormat(unsigned char *dst, const unsigned char *src) 00590 { 00591 // do last minute conversion if Bayer pattern is requested but not provided, go from R8G8B8 00592 // deprecated in gazebo2 branch, keep for backwards compatibility 00593 if (this->myParent->GetImageFormat() == "BAYER_RGGB8" && this->depth == 3) 00594 { 00595 for (int i=0;i<this->width;i++) 00596 { 00597 for (int j=0;j<this->height;j++) 00598 { 00599 // 00600 // RG 00601 // GB 00602 // 00603 // determine position 00604 if (j%2) // even column 00605 if (i%2) // even row, red 00606 dst[i+j*this->width] = src[i*3+j*this->width*3+0]; 00607 else // odd row, green 00608 dst[i+j*this->width] = src[i*3+j*this->width*3+1]; 00609 else // odd column 00610 if (i%2) // even row, green 00611 dst[i+j*this->width] = src[i*3+j*this->width*3+1]; 00612 else // odd row, blue 00613 dst[i+j*this->width] = src[i*3+j*this->width*3+2]; 00614 } 00615 } 00616 src=dst; 00617 } 00618 else if (this->myParent->GetImageFormat() == "BAYER_BGGR8" && this->depth == 3) 00619 { 00620 for (int i=0;i<this->width;i++) 00621 { 00622 for (int j=0;j<this->height;j++) 00623 { 00624 // 00625 // BG 00626 // GR 00627 // 00628 // determine position 00629 if (j%2) // even column 00630 if (i%2) // even row, blue 00631 dst[i+j*this->width] = src[i*3+j*this->width*3+2]; 00632 else // odd row, green 00633 dst[i+j*this->width] = src[i*3+j*this->width*3+1]; 00634 else // odd column 00635 if (i%2) // even row, green 00636 dst[i+j*this->width] = src[i*3+j*this->width*3+1]; 00637 else // odd row, red 00638 dst[i+j*this->width] = src[i*3+j*this->width*3+0]; 00639 } 00640 } 00641 src=dst; 00642 } 00643 else if (this->myParent->GetImageFormat() == "BAYER_GBRG8" && this->depth == 3) 00644 { 00645 for (int i=0;i<this->width;i++) 00646 { 00647 for (int j=0;j<this->height;j++) 00648 { 00649 // 00650 // GB 00651 // RG 00652 // 00653 // determine position 00654 if (j%2) // even column 00655 if (i%2) // even row, green 00656 dst[i+j*this->width] = src[i*3+j*this->width*3+1]; 00657 else // odd row, blue 00658 dst[i+j*this->width] = src[i*3+j*this->width*3+2]; 00659 else // odd column 00660 if (i%2) // even row, red 00661 dst[i+j*this->width] = src[i*3+j*this->width*3+0]; 00662 else // odd row, green 00663 dst[i+j*this->width] = src[i*3+j*this->width*3+1]; 00664 } 00665 } 00666 src=dst; 00667 } 00668 else if (this->myParent->GetImageFormat() == "BAYER_GRBG8" && this->depth == 3) 00669 { 00670 for (int i=0;i<this->width;i++) 00671 { 00672 for (int j=0;j<this->height;j++) 00673 { 00674 // 00675 // GR 00676 // BG 00677 // 00678 // determine position 00679 if (j%2) // even column 00680 if (i%2) // even row, green 00681 dst[i+j*this->width] = src[i*3+j*this->width*3+1]; 00682 else // odd row, red 00683 dst[i+j*this->width] = src[i*3+j*this->width*3+0]; 00684 else // odd column 00685 if (i%2) // even row, blue 00686 dst[i+j*this->width] = src[i*3+j*this->width*3+2]; 00687 else // odd row, green 00688 dst[i+j*this->width] = src[i*3+j*this->width*3+1]; 00689 } 00690 } 00691 src=dst; 00692 } 00693 } 00694 00696 // Fill depth information 00697 bool GazeboRosThermalCamera::fillDepthImage(pcl::PointCloud<pcl::PointXYZ>& point_cloud, 00698 uint32_t rows_arg, 00699 uint32_t cols_arg, 00700 uint32_t step_arg, 00701 void* data_arg) 00702 { 00703 point_cloud.points.resize(0); 00704 point_cloud.is_dense = true; 00705 //point_cloud.channels.resize(1); 00706 //point_cloud.channels[0].values.resize(0); 00707 00708 float* toCopyFrom = (float*)data_arg; 00709 int index = 0; 00710 00711 double hfov = this->myParent->GetHFOV().GetAsRadian(); 00712 double fl = ((double)this->width) / (2.0 *tan(hfov/2.0)); 00713 //ROS_ERROR("debug hfov: %f fl: %f w: %f h: %f",hfov, fl, (double)cols_arg, (double)rows_arg); 00714 //ROS_ERROR("debug %f %f", this->myParent->GetFarClip() , this->myParent->GetNearClip()); 00715 00716 // convert depth to point cloud 00717 for (uint32_t j=0; j<rows_arg; j++) 00718 { 00719 double pAngle; 00720 if (rows_arg>1) pAngle = atan2( (double)j - 0.5*(double)(rows_arg-1), fl); 00721 else pAngle = 0.0; 00722 00723 for (uint32_t i=0; i<cols_arg; i++) 00724 { 00725 double yAngle; 00726 if (cols_arg>1) yAngle = atan2( (double)i - 0.5*(double)(cols_arg-1), fl); 00727 else yAngle = 0.0; 00728 00729 //double depth = (this->myParent->GetFarClip() - this->myParent->GetNearClip()) * ( toCopyFrom[index++] ) + 1.0 * this->myParent->GetNearClip() ; 00730 //double depth = ( this->myParent->GetFarClip() + this->myParent->GetNearClip() + toCopyFrom[index++] ) 00731 // * 0.5 ; //this->myParent->GetNearClip() ; 00732 double depth = toCopyFrom[index++]; // + 0.0*this->myParent->GetNearClip(); 00733 00734 // in optical frame 00735 // hardcoded rotation rpy(-M_PI/2, 0, -M_PI/2) is built-in 00736 // to urdf, where the *_optical_frame should have above relative 00737 // rotation from the physical camera *_frame 00738 pcl::PointXYZ point; 00739 point.x = depth * tan(yAngle); 00740 point.y = depth * tan(pAngle); 00741 if(depth > this->pointCloudCutoff) 00742 { 00743 point.z = depth; 00744 } 00745 else //point in the unseeable range 00746 { 00747 point.x = point.y = point.z = std::numeric_limits<float>::quiet_NaN (); 00748 point_cloud.is_dense = false; 00749 } 00750 point_cloud.points.push_back(point); 00751 } 00752 } 00753 //this->pointCloudMsg.channels[0].values.push_back(1.0); 00754 return true; 00755 } 00757 // Put laser data to the interface 00758 void GazeboRosThermalCamera::PublishCameraInfo() 00759 { 00760 // fill CameraInfo 00761 this->cameraInfoMsg.header.frame_id = this->frameName; 00762 Time lastRenderTime = (dynamic_cast<OgreCamera*>(this->myParent))->GetLastRenderTime(); 00763 //Time lastRenderTime = Simulator::Instance()->GetSimTime(); 00764 this->cameraInfoMsg.header.stamp.sec = lastRenderTime.sec; 00765 this->cameraInfoMsg.header.stamp.nsec = lastRenderTime.nsec; 00766 this->cameraInfoMsg.height = this->height; 00767 this->cameraInfoMsg.width = this->width; 00768 // distortion 00769 #if ROS_VERSION_MINIMUM(1, 3, 0) 00770 this->cameraInfoMsg.distortion_model = "plumb_bob"; 00771 this->cameraInfoMsg.D.resize(5); 00772 #endif 00773 this->cameraInfoMsg.D[0] = this->distortion_k1; 00774 this->cameraInfoMsg.D[1] = this->distortion_k2; 00775 this->cameraInfoMsg.D[2] = this->distortion_k3; 00776 this->cameraInfoMsg.D[3] = this->distortion_t1; 00777 this->cameraInfoMsg.D[4] = this->distortion_t2; 00778 // original camera matrix 00779 this->cameraInfoMsg.K[0] = this->focal_length; 00780 this->cameraInfoMsg.K[1] = 0.0; 00781 this->cameraInfoMsg.K[2] = this->Cx; 00782 this->cameraInfoMsg.K[3] = 0.0; 00783 this->cameraInfoMsg.K[4] = this->focal_length; 00784 this->cameraInfoMsg.K[5] = this->Cy; 00785 this->cameraInfoMsg.K[6] = 0.0; 00786 this->cameraInfoMsg.K[7] = 0.0; 00787 this->cameraInfoMsg.K[8] = 1.0; 00788 // rectification 00789 this->cameraInfoMsg.R[0] = 1.0; 00790 this->cameraInfoMsg.R[1] = 0.0; 00791 this->cameraInfoMsg.R[2] = 0.0; 00792 this->cameraInfoMsg.R[3] = 0.0; 00793 this->cameraInfoMsg.R[4] = 1.0; 00794 this->cameraInfoMsg.R[5] = 0.0; 00795 this->cameraInfoMsg.R[6] = 0.0; 00796 this->cameraInfoMsg.R[7] = 0.0; 00797 this->cameraInfoMsg.R[8] = 1.0; 00798 // camera projection matrix (same as camera matrix due to lack of distortion/rectification) (is this generated?) 00799 this->cameraInfoMsg.P[0] = this->focal_length; 00800 this->cameraInfoMsg.P[1] = 0.0; 00801 this->cameraInfoMsg.P[2] = this->Cx; 00802 this->cameraInfoMsg.P[3] = -this->focal_length * this->hack_baseline; 00803 this->cameraInfoMsg.P[4] = 0.0; 00804 this->cameraInfoMsg.P[5] = this->focal_length; 00805 this->cameraInfoMsg.P[6] = this->Cy; 00806 this->cameraInfoMsg.P[7] = 0.0; 00807 this->cameraInfoMsg.P[8] = 0.0; 00808 this->cameraInfoMsg.P[9] = 0.0; 00809 this->cameraInfoMsg.P[10] = 1.0; 00810 this->cameraInfoMsg.P[11] = 0.0; 00811 00812 last_camera_info_pub_time_ = Simulator::Instance()->GetSimTime(); 00813 this->camera_info_pub_.publish(this->cameraInfoMsg); 00814 } 00815 00816 00817 #ifdef USE_CBQ 00818 00819 // Put laser data to the interface 00820 void GazeboRosThermalCamera::CameraQueueThread() 00821 { 00822 static const double timeout = 0.01; 00823 00824 while (this->rosnode_->ok()) 00825 { 00826 this->camera_queue_.callAvailable(ros::WallDuration(timeout)); 00827 } 00828 } 00829 #endif 00830 00831 }