cob_camera_sensors: cob_camera_sensors: all

00001 /****************************************************************
00002  *
00003  * Copyright (c) 2010
00004  *
00005  * Fraunhofer Institute for Manufacturing Engineering   
00006  * and Automation (IPA)
00007  *
00008  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00009  *
00010  * Project name: care-o-bot
00011  * ROS stack name: cob_driver
00012  * ROS package name: cob_camera_sensors
00013  *                                                              
00014  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00015  *                      
00016  * Author: Jan Fischer, email:jan.fischer@ipa.fhg.de
00017  * Supervised by: Jan Fischer, email:jan.fischer@ipa.fhg.de
00018  *
00019  * Date of creation: Jan 2010
00020  * ToDo:
00021  *
00022  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00023  *
00024  * Redistribution and use in source and binary forms, with or without
00025  * modification, are permitted provided that the following conditions are met:
00026  *
00027  *     * Redistributions of source code must retain the above copyright
00028  *       notice, this list of conditions and the following disclaimer.
00029  *     * Redistributions in binary form must reproduce the above copyright
00030  *       notice, this list of conditions and the following disclaimer in the
00031  *       documentation and/or other materials provided with the distribution.
00032  *     * Neither the name of the Fraunhofer Institute for Manufacturing 
00033  *       Engineering and Automation (IPA) nor the names of its
00034  *       contributors may be used to endorse or promote products derived from
00035  *       this software without specific prior written permission.
00036  *
00037  * This program is free software: you can redistribute it and/or modify
00038  * it under the terms of the GNU Lesser General Public License LGPL as 
00039  * published by the Free Software Foundation, either version 3 of the 
00040  * License, or (at your option) any later version.
00041  * 
00042  * This program is distributed in the hope that it will be useful,
00043  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00044  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00045  * GNU Lesser General Public License LGPL for more details.
00046  * 
00047  * You should have received a copy of the GNU Lesser General Public 
00048  * License LGPL along with this program. 
00049  * If not, see <http://www.gnu.org/licenses/>.
00050  *
00051  ****************************************************************/
00052 
00053 //##################
00054 //#### includes ####
00055 
00056 // standard includes
00057 //--
00058 
00059 // ROS includes
00060 #include <ros/ros.h>
00061 #include <cv_bridge/CvBridge.h>
00062 #include <image_transport/image_transport.h>
00063 
00064 // ROS message includes
00065 #include <sensor_msgs/Image.h>
00066 #include <sensor_msgs/CameraInfo.h>
00067 #include <sensor_msgs/fill_image.h>
00068 #include <sensor_msgs/SetCameraInfo.h>
00069 
00070 // external includes
00071 #include <cob_camera_sensors/AbstractColorCamera.h>
00072 #include <cob_camera_sensors/AbstractRangeImagingSensor.h>
00073 #include <cob_vision_utils/GlobalDefines.h>
00074 #include <cob_vision_utils/CameraSensorToolbox.h>
00075 
00076 using namespace ipa_CameraSensors;
00077 
00080 class CobAllCamerasNode
00081 {
00082 private:
00083         ros::NodeHandle node_handle_;
00084 
00085         AbstractColorCameraPtr left_color_camera_;      
00086         AbstractColorCameraPtr right_color_camera_;     
00087         AbstractRangeImagingSensorPtr tof_camera_;      
00088         
00089         std::string config_directory_;  
00090                 
00091         std::string left_color_camera_ns_; 
00092         std::string right_color_camera_ns_; 
00093         std::string tof_camera_ns_; 
00094 
00095         int left_color_camera_intrinsic_id_;    
00096         int right_color_camera_intrinsic_id_;   
00097         int tof_camera_intrinsic_id_;   
00098 
00099         ipa_CameraSensors::t_cameraType left_color_camera_intrinsic_type_;      
00100         ipa_CameraSensors::t_cameraType right_color_camera_intrinsic_type_;     
00101         ipa_CameraSensors::t_cameraType tof_camera_intrinsic_type_;     
00102 
00103         sensor_msgs::CameraInfo left_color_camera_info_msg_;    
00104         sensor_msgs::CameraInfo right_color_camera_info_msg_;   
00105         sensor_msgs::CameraInfo tof_camera_info_msg_;   
00106 
00107         ros::ServiceServer left_color_camera_info_service_;
00108         ros::ServiceServer right_color_camera_info_service_;
00109         ros::ServiceServer tof_camera_info_service_;
00110 
00111         cv::Mat left_color_image_8U3_;  
00112         cv::Mat right_color_image_8U3_; 
00113         cv::Mat xyz_tof_image_32F3_;      
00114         cv::Mat grey_tof_image_32F1_;     
00115 
00116         image_transport::ImageTransport image_transport_;       
00117         image_transport::CameraPublisher xyz_tof_image_publisher_;      
00118         image_transport::CameraPublisher grey_tof_image_publisher_;     
00119         image_transport::CameraPublisher left_color_image_publisher_;   
00120         image_transport::CameraPublisher right_color_image_publisher_;  
00121 
00122 public:
00123         CobAllCamerasNode(const ros::NodeHandle& node_handle)
00124         : node_handle_(node_handle),
00125           left_color_camera_(AbstractColorCameraPtr()),
00126           right_color_camera_(AbstractColorCameraPtr()),
00127           tof_camera_(AbstractRangeImagingSensorPtr()),
00128           left_color_image_8U3_(cv::Mat()),
00129           right_color_image_8U3_(cv::Mat()),
00130           xyz_tof_image_32F3_(cv::Mat()),
00131           grey_tof_image_32F1_(cv::Mat()),
00132           image_transport_(node_handle)
00133         {
00135         }
00136 
00137         ~CobAllCamerasNode()
00138         {
00139                 ROS_INFO("[all_cameras] Shutting down cameras");
00140                 if (left_color_camera_)
00141                 {
00142                         ROS_INFO("[all_cameras] Shutting down left color camera (1)");
00143                         left_color_camera_->Close();
00144                 }
00145                 if (right_color_camera_)
00146                 {
00147                         ROS_INFO("[all_cameras] Shutting down right color camera (0)");
00148                         right_color_camera_->Close();
00149                 }
00150                 if (tof_camera_)
00151                 {
00152                         ROS_INFO("[all_cameras] Shutting down tof camera (0)");
00153                         tof_camera_->Close();
00154                 }
00155         } 
00156 
00158         bool init()
00159         {
00160                 if (loadParameters() == false)
00161                 {
00162                         ROS_ERROR("[all_cameras] Could not read parameters from launch file");
00163                         return false;
00164                 }
00165                 
00166                 if (left_color_camera_ && (left_color_camera_->Init(config_directory_, 1) & ipa_CameraSensors::RET_FAILED))
00167                 {
00168                         ROS_WARN("[all_cameras] Initialization of left camera (1) failed");
00169                         left_color_camera_ = AbstractColorCameraPtr();
00170                 }
00171 
00172                 if (left_color_camera_ && (left_color_camera_->Open() & ipa_CameraSensors::RET_FAILED))
00173                 {
00174                         ROS_WARN("[all_cameras] Opening left color camera (1) failed");
00175                         left_color_camera_ = AbstractColorCameraPtr();
00176                 }
00177                 if (left_color_camera_)
00178                 {
00180                         int camera_index = 1;
00181                         ipa_CameraSensors::t_cameraProperty cameraProperty;
00182                         cameraProperty.propertyID = ipa_CameraSensors::PROP_CAMERA_RESOLUTION;
00183                         left_color_camera_->GetProperty(&cameraProperty);
00184                         int color_sensor_width = cameraProperty.cameraResolution.xResolution;
00185                         int color_sensor_height = cameraProperty.cameraResolution.yResolution;
00186                         cv::Size color_image_size(color_sensor_width, color_sensor_height);
00187                         
00189                         ipa_CameraSensors::CameraSensorToolboxPtr color_sensor_toolbox = ipa_CameraSensors::CreateCameraSensorToolbox();
00190                         color_sensor_toolbox->Init(config_directory_, left_color_camera_->GetCameraType(), camera_index, color_image_size);
00191         
00192                         cv::Mat d = color_sensor_toolbox->GetDistortionParameters(left_color_camera_intrinsic_type_, left_color_camera_intrinsic_id_);
00193                         left_color_camera_info_msg_.header.stamp = ros::Time::now();
00194                         left_color_camera_info_msg_.header.frame_id = "head_color_camera_l_link";
00195                         left_color_camera_info_msg_.D[0] = d.at<double>(0, 0);
00196                         left_color_camera_info_msg_.D[1] = d.at<double>(0, 1);
00197                         left_color_camera_info_msg_.D[2] = d.at<double>(0, 2);
00198                         left_color_camera_info_msg_.D[3] = d.at<double>(0, 3);
00199                         left_color_camera_info_msg_.D[4] = 0;
00200         
00201                         cv::Mat k = color_sensor_toolbox->GetIntrinsicMatrix(left_color_camera_intrinsic_type_, left_color_camera_intrinsic_id_);
00202                         left_color_camera_info_msg_.K[0] = k.at<double>(0, 0);
00203                         left_color_camera_info_msg_.K[1] = k.at<double>(0, 1);
00204                         left_color_camera_info_msg_.K[2] = k.at<double>(0, 2);
00205                         left_color_camera_info_msg_.K[3] = k.at<double>(1, 0);
00206                         left_color_camera_info_msg_.K[4] = k.at<double>(1, 1);
00207                         left_color_camera_info_msg_.K[5] = k.at<double>(1, 2);
00208                         left_color_camera_info_msg_.K[6] = k.at<double>(2, 0);
00209                         left_color_camera_info_msg_.K[7] = k.at<double>(2, 1);
00210                         left_color_camera_info_msg_.K[8] = k.at<double>(2, 2);
00211         
00212                         left_color_camera_info_msg_.width = color_sensor_width;         
00213                         left_color_camera_info_msg_.height = color_sensor_height;               
00214                 }
00215 
00216                 if (right_color_camera_ && (right_color_camera_->Init(config_directory_, 0) & ipa_CameraSensors::RET_FAILED))
00217                 {
00218                         ROS_WARN("[all_cameras] Initialization of right camera (0) failed");
00219                         right_color_camera_ = AbstractColorCameraPtr();
00220                 }
00221 
00222                 if (right_color_camera_ && (right_color_camera_->Open() & ipa_CameraSensors::RET_FAILED))
00223                 {
00224                         ROS_WARN("[all_cameras] Opening right color camera (0) failed");
00225                         right_color_camera_ = AbstractColorCameraPtr();
00226                 }
00227                 if (right_color_camera_)
00228                 {
00229                         int camera_index = 0;
00231                         ipa_CameraSensors::t_cameraProperty cameraProperty;
00232                         cameraProperty.propertyID = ipa_CameraSensors::PROP_CAMERA_RESOLUTION;
00233                         right_color_camera_->GetProperty(&cameraProperty);
00234                         int color_sensor_width = cameraProperty.cameraResolution.xResolution;
00235                         int color_sensor_height = cameraProperty.cameraResolution.yResolution;
00236                         cv::Size color_image_size(color_sensor_width, color_sensor_height);
00237 
00239                         ipa_CameraSensors::CameraSensorToolboxPtr color_sensor_toolbox = ipa_CameraSensors::CreateCameraSensorToolbox();
00240                         color_sensor_toolbox->Init(config_directory_, left_color_camera_->GetCameraType(), camera_index, color_image_size);
00241         
00242                         cv::Mat d = color_sensor_toolbox->GetDistortionParameters(right_color_camera_intrinsic_type_, right_color_camera_intrinsic_id_);
00243                         right_color_camera_info_msg_.header.stamp = ros::Time::now();
00244                         right_color_camera_info_msg_.header.frame_id = "head_color_camera_r_link";
00245                         right_color_camera_info_msg_.D[0] = d.at<double>(0, 0);
00246                         right_color_camera_info_msg_.D[1] = d.at<double>(0, 1);
00247                         right_color_camera_info_msg_.D[2] = d.at<double>(0, 2);
00248                         right_color_camera_info_msg_.D[3] = d.at<double>(0, 3);
00249                         right_color_camera_info_msg_.D[4] = 0;
00250         
00251                         cv::Mat k = color_sensor_toolbox->GetIntrinsicMatrix(right_color_camera_intrinsic_type_, right_color_camera_intrinsic_id_);
00252                         right_color_camera_info_msg_.K[0] = k.at<double>(0, 0);
00253                         right_color_camera_info_msg_.K[1] = k.at<double>(0, 1);
00254                         right_color_camera_info_msg_.K[2] = k.at<double>(0, 2);
00255                         right_color_camera_info_msg_.K[3] = k.at<double>(1, 0);
00256                         right_color_camera_info_msg_.K[4] = k.at<double>(1, 1);
00257                         right_color_camera_info_msg_.K[5] = k.at<double>(1, 2);
00258                         right_color_camera_info_msg_.K[6] = k.at<double>(2, 0);
00259                         right_color_camera_info_msg_.K[7] = k.at<double>(2, 1);
00260                         right_color_camera_info_msg_.K[8] = k.at<double>(2, 2);
00261         
00262                         right_color_camera_info_msg_.width = color_sensor_width;                
00263                         right_color_camera_info_msg_.height = color_sensor_height;              
00264                 }
00265 
00266                 if (tof_camera_ && (tof_camera_->Init(config_directory_) & ipa_CameraSensors::RET_FAILED))
00267                 {
00268                         ROS_WARN("[all_cameras] Initialization of tof camera (0) failed");
00269                         tof_camera_ = AbstractRangeImagingSensorPtr();
00270                 }
00271 
00272                 if (tof_camera_ && (tof_camera_->Open() & ipa_CameraSensors::RET_FAILED))
00273                 {
00274                         ROS_WARN("[all_cameras] Opening tof camera (0) failed");
00275                         tof_camera_ = AbstractRangeImagingSensorPtr();
00276                 }
00277                 if (tof_camera_)
00278                 {
00279                         int camera_index = 0;
00281                         ipa_CameraSensors::t_cameraProperty cameraProperty;
00282                         cameraProperty.propertyID = ipa_CameraSensors::PROP_CAMERA_RESOLUTION;
00283                         tof_camera_->GetProperty(&cameraProperty);
00284                         int range_sensor_width = cameraProperty.cameraResolution.xResolution;
00285                         int range_sensor_height = cameraProperty.cameraResolution.yResolution;
00286                         cv::Size rangeImageSize(range_sensor_width, range_sensor_height);
00287         
00289                         ipa_CameraSensors::CameraSensorToolboxPtr tof_sensor_toolbox = ipa_CameraSensors::CreateCameraSensorToolbox();
00290                         tof_sensor_toolbox->Init(config_directory_, tof_camera_->GetCameraType(), camera_index, rangeImageSize);
00291 
00292                         cv::Mat intrinsic_mat = tof_sensor_toolbox->GetIntrinsicMatrix(tof_camera_intrinsic_type_, tof_camera_intrinsic_id_);
00293                         cv::Mat distortion_map_X = tof_sensor_toolbox->GetDistortionMapX(tof_camera_intrinsic_type_, tof_camera_intrinsic_id_);
00294                         cv::Mat distortion_map_Y = tof_sensor_toolbox->GetDistortionMapY(tof_camera_intrinsic_type_, tof_camera_intrinsic_id_);
00295                         tof_camera_->SetIntrinsics(intrinsic_mat, distortion_map_X, distortion_map_Y);
00296 
00297                         cv::Mat d = tof_sensor_toolbox->GetDistortionParameters(tof_camera_intrinsic_type_, tof_camera_intrinsic_id_);
00298                         tof_camera_info_msg_.header.stamp = ros::Time::now();
00299                         tof_camera_info_msg_.header.frame_id = "head_tof_link";
00300                         tof_camera_info_msg_.D[0] = d.at<double>(0, 0);
00301                         tof_camera_info_msg_.D[1] = d.at<double>(0, 1);
00302                         tof_camera_info_msg_.D[2] = d.at<double>(0, 2);
00303                         tof_camera_info_msg_.D[3] = d.at<double>(0, 3);
00304                         tof_camera_info_msg_.D[4] = 0;
00305         
00306                         cv::Mat k = tof_sensor_toolbox->GetIntrinsicMatrix(tof_camera_intrinsic_type_, tof_camera_intrinsic_id_);
00307                         tof_camera_info_msg_.K[0] = k.at<double>(0, 0);
00308                         tof_camera_info_msg_.K[1] = k.at<double>(0, 1);
00309                         tof_camera_info_msg_.K[2] = k.at<double>(0, 2);
00310                         tof_camera_info_msg_.K[3] = k.at<double>(1, 0);
00311                         tof_camera_info_msg_.K[4] = k.at<double>(1, 1);
00312                         tof_camera_info_msg_.K[5] = k.at<double>(1, 2);
00313                         tof_camera_info_msg_.K[6] = k.at<double>(2, 0);
00314                         tof_camera_info_msg_.K[7] = k.at<double>(2, 1);
00315                         tof_camera_info_msg_.K[8] = k.at<double>(2, 2);
00316 
00317                         tof_camera_info_msg_.width = range_sensor_width;                
00318                         tof_camera_info_msg_.height = range_sensor_height;              
00319                 }
00320         
00322                 if (left_color_camera_) 
00323                 {
00324                         // Adapt name according to camera type
00325                         left_color_image_publisher_ = image_transport_.advertiseCamera(left_color_camera_ns_ + "/left/image_color", 1);
00326                         left_color_camera_info_service_ = node_handle_.advertiseService(left_color_camera_ns_ + "/left/set_camera_info", &CobAllCamerasNode::setCameraInfo, this);
00327                 }
00328                 if (right_color_camera_)
00329                 {
00330                         // Adapt name according to camera type
00331                         right_color_image_publisher_ = image_transport_.advertiseCamera(right_color_camera_ns_ + "/right/image_color", 1);
00332                         right_color_camera_info_service_ = node_handle_.advertiseService(right_color_camera_ns_ + "/right/set_camera_info", &CobAllCamerasNode::setCameraInfo, this);
00333                 }
00334                 if (tof_camera_)
00335                 {
00336                         grey_tof_image_publisher_ = image_transport_.advertiseCamera(tof_camera_ns_ + "/image_grey", 1);
00337                         xyz_tof_image_publisher_ = image_transport_.advertiseCamera(tof_camera_ns_ + "/image_xyz", 1);
00338                         tof_camera_info_service_ = node_handle_.advertiseService(tof_camera_ns_ + "/set_camera_info", &CobAllCamerasNode::setCameraInfo, this);
00339                 }
00340         
00341                 return true;
00342         }
00343 
00348         bool setCameraInfo(sensor_msgs::SetCameraInfo::Request& req,
00349                         sensor_msgs::SetCameraInfo::Response& rsp)
00350         {
00352                 //camera_info_message_ = req.camera_info;
00353     
00354                 rsp.success = false;
00355                 rsp.status_message = "[all_cameras] Setting camera parameters through ROS not implemented";
00356 
00357                 return true;
00358         }
00359 
00361         void spin()
00362         {
00363                 // Set maximal spinning rate
00364                 ros::Rate rate(30);
00365                 while(node_handle_.ok())
00366                 {
00367                         // ROS images
00368                         sensor_msgs::Image right_color_image_msg;
00369                         sensor_msgs::Image left_color_image_msg;
00370                         sensor_msgs::Image xyz_tof_image_msg;
00371                         sensor_msgs::Image grey_tof_image_msg;
00372                 
00373                         // ROS camera information messages
00374                         sensor_msgs::CameraInfo right_color_image_info;
00375                         sensor_msgs::CameraInfo left_color_image_info;
00376                         sensor_msgs::CameraInfo tof_image_info;
00377         
00378                         ros::Time now = ros::Time::now();
00379         
00380                         // Acquire right color image
00381                         if (right_color_camera_)
00382                         {
00383                                 //ROS_INFO("[all_cameras] RIGHT");
00385                                 if (right_color_camera_->GetColorImage(&right_color_image_8U3_, false) & ipa_Utils::RET_FAILED)
00386                                 {
00387                                         ROS_ERROR("[all_cameras] Right color image acquisition failed");
00388                                         break;
00389                                 }
00390 
00391                                 try
00392                                 {
00393                                         IplImage img = right_color_image_8U3_;
00394                                         right_color_image_msg = *(sensor_msgs::CvBridge::cvToImgMsg(&img, "bgr8"));
00395                                 }
00396                                 catch (sensor_msgs::CvBridgeException error)
00397                                 {
00398                                         ROS_ERROR("[all_cameras] Could not convert right IplImage to ROS message");
00399                                         break;
00400                                 }
00401                                 right_color_image_msg.header.stamp = now; 
00402                                 right_color_image_msg.encoding = "bgr8"; 
00403                                 right_color_image_msg.header.frame_id = "head_color_camera_r_link";   
00404                 
00405                                 right_color_image_info = right_color_camera_info_msg_;
00406                                 right_color_image_info.width = right_color_image_8U3_.cols;
00407                                 right_color_image_info.height = right_color_image_8U3_.rows;
00408                                 right_color_image_info.header.stamp = now;
00409                                 right_color_image_info.header.frame_id = "head_color_camera_r_link";
00410         
00411                                 right_color_image_publisher_.publish(right_color_image_msg, right_color_image_info);
00412                         }
00413                 
00414                         // Acquire left color image
00415                         if (left_color_camera_)
00416                         {
00417                                 //ROS_INFO("[all_cameras] LEFT");
00418                 
00420                                 if (left_color_camera_->GetColorImage(&left_color_image_8U3_, false) & ipa_Utils::RET_FAILED)
00421                                 {
00422                                         ROS_ERROR("[all_cameras] Left color image acquisition failed");
00423                                         break;
00424                                 }
00425 
00426                                 try
00427                                 {
00428                                         IplImage img = left_color_image_8U3_;
00429                                         left_color_image_msg = *(sensor_msgs::CvBridge::cvToImgMsg(&img, "bgr8"));
00430                                 }
00431                                 catch (sensor_msgs::CvBridgeException error)
00432                                 {
00433                                         ROS_ERROR("[all_cameras] Could not convert left IplImage to ROS message");
00434                                         break;
00435                                 }
00436                                 left_color_image_msg.header.stamp = now;    
00437                                 left_color_image_msg.encoding = "bgr8";
00438                                 left_color_image_msg.header.frame_id = "head_color_camera_l_link"; 
00439                 
00440                                 left_color_image_info = left_color_camera_info_msg_;
00441                                 left_color_image_info.width = left_color_image_8U3_.cols;
00442                                 left_color_image_info.height = left_color_image_8U3_.rows;
00443                                 left_color_image_info.header.stamp = now;
00444                                 left_color_image_info.header.frame_id = "head_color_camera_l_link"; 
00445         
00446                                 left_color_image_publisher_.publish(left_color_image_msg, left_color_image_info);
00447                         }
00448         
00449                         // Acquire image from tof camera        
00450                         if (tof_camera_)
00451                         {
00452                                 //ROS_INFO("[all_cameras] TOF");
00453                                 if(tof_camera_->AcquireImages(0, &grey_tof_image_32F1_, &xyz_tof_image_32F3_, false, false, ipa_CameraSensors::INTENSITY_32F1) & ipa_Utils::RET_FAILED)
00454                                 {
00455                                         ROS_ERROR("[all_cameras] Tof image acquisition failed");
00456                                         tof_camera_->Close();
00457                       tof_camera_ = AbstractRangeImagingSensorPtr();
00458                                         break;  
00459                                 }
00460         
00461                                 try
00462                                 {
00463                                         IplImage grey_img = grey_tof_image_32F1_; 
00464                                         IplImage xyz_img = xyz_tof_image_32F3_; 
00465                                         xyz_tof_image_msg = *(sensor_msgs::CvBridge::cvToImgMsg(&xyz_img, "passthrough"));
00466                                         grey_tof_image_msg = *(sensor_msgs::CvBridge::cvToImgMsg(&grey_img, "passthrough"));
00467                                 }
00468                                 catch (sensor_msgs::CvBridgeException error)
00469                                 {
00470                                         ROS_ERROR("[all_cameras] Could not convert tof IplImage to ROS message");
00471                                         break;
00472                                 }
00473         
00474                                 xyz_tof_image_msg.header.stamp = now;   
00475                                 xyz_tof_image_msg.header.frame_id = "head_tof_link"; 
00476                                 grey_tof_image_msg.header.stamp = now;    
00477                                 grey_tof_image_msg.header.frame_id = "head_tof_link";
00478                 
00479                                 tof_image_info = tof_camera_info_msg_;
00480                                 tof_image_info.width = grey_tof_image_32F1_.cols;
00481                                 tof_image_info.height = grey_tof_image_32F1_.rows;
00482                                 tof_image_info.header.stamp = now;
00483                                 tof_image_info.header.frame_id = "head_tof_link";
00484                                 
00485                                 grey_tof_image_publisher_.publish(grey_tof_image_msg, tof_image_info);
00486                                 xyz_tof_image_publisher_.publish(xyz_tof_image_msg, tof_image_info);
00487                         }
00488                         
00489                         ros::spinOnce();
00490                         rate.sleep();
00491         
00492                 } // END while-loop
00493         }
00494 
00495         bool loadParameters()
00496         {
00497                 std::string tmp_string = "NULL";
00498 
00500                 if (node_handle_.getParam("all_cameras/configuration_files", config_directory_) == false)
00501                 {
00502                         ROS_ERROR("Path to xml configuration file not specified");
00503                         return false;
00504                 }
00505 
00506                 ROS_INFO("Configuration directory: %s", config_directory_.c_str());
00507 
00508                 // Color camera type
00509                 if (node_handle_.getParam("all_cameras/color_camera_type", tmp_string) == false)
00510                 {
00511                         ROS_ERROR("[all_cameras] Color camera type not specified");
00512                         return false;
00513                 }
00514                 if (tmp_string == "CAM_AVTPIKE")
00515                 {
00516                         right_color_camera_ = ipa_CameraSensors::CreateColorCamera_AVTPikeCam();
00517                         left_color_camera_ = ipa_CameraSensors::CreateColorCamera_AVTPikeCam();
00518 
00519                         left_color_camera_ns_ = "pike_145C";
00520                         right_color_camera_ns_ = "pike_145C";
00521                 }
00522                 else if (tmp_string == "CAM_VIRTUAL") 
00523                 {
00524                         right_color_camera_ = ipa_CameraSensors::CreateColorCamera_VirtualCam();
00525                         left_color_camera_ = ipa_CameraSensors::CreateColorCamera_VirtualCam();
00526                         left_color_camera_ns_ = "pike_145C";
00527                         right_color_camera_ns_ = "pike_145C";
00528                 }
00529                 else if (tmp_string == "CAM_PROSILICA") ROS_ERROR("[all_cameras] Color camera type not CAM_PROSILICA not yet implemented");
00530                 else
00531                 {
00532                         std::string str = "[all_cameras] Camera type '" + tmp_string + "' unknown, try 'CAM_AVTPIKE' or 'CAM_PROSILICA'";
00533                         ROS_ERROR("%s", str.c_str());
00534                         return false;
00535                 }
00536 
00537                 ROS_INFO("Color camera type: %s", tmp_string.c_str());
00538 
00539                 // Tof camera type
00540                 if (node_handle_.getParam("all_cameras/tof_camera_type", tmp_string) == false)
00541                 {
00542                         ROS_ERROR("[all_cameras] tof camera type not specified");
00543                         return false;
00544                 }
00545                 if (tmp_string == "CAM_SWISSRANGER") 
00546                 {
00547                         tof_camera_ = ipa_CameraSensors::CreateRangeImagingSensor_Swissranger();
00548                         tof_camera_ns_ = "sr4000";
00549                 }
00550                 else if (tmp_string == "CAM_VIRTUAL") 
00551                 {
00552                         tof_camera_ = ipa_CameraSensors::CreateRangeImagingSensor_VirtualCam();
00553                         tof_camera_ns_ = "sr4000";
00554                 }
00555                 else
00556                 {
00557                         std::string str = "[all_cameras] Camera type '" + tmp_string + "' unknown, try 'CAM_SWISSRANGER'";
00558                         ROS_ERROR("%s", str.c_str());
00559                 }
00560                 
00561                 ROS_INFO("Tof camera type: %s", tmp_string.c_str());
00562 
00563                 // There are several intrinsic matrices, optimized to different cameras
00564                 // Here, we specified the desired intrinsic matrix for each camera
00565                 if (node_handle_.getParam("all_cameras/left_color_camera_intrinsic_type", tmp_string) == false)
00566                 {
00567                         ROS_ERROR("[all_cameras] Intrinsic camera type for left color camera not specified");
00568                         return false;
00569                 }
00570                 if (tmp_string == "CAM_AVTPIKE")
00571                 {
00572                         left_color_camera_intrinsic_type_ = ipa_CameraSensors::CAM_AVTPIKE;
00573                 }
00574                 else if (tmp_string == "CAM_PROSILICA")
00575                 {
00576                         left_color_camera_intrinsic_type_ = ipa_CameraSensors::CAM_PROSILICA;
00577                 } 
00578                 else if (tmp_string == "CAM_SWISSRANGER")
00579                 {
00580                         left_color_camera_intrinsic_type_ = ipa_CameraSensors::CAM_SWISSRANGER;
00581                 } 
00582                 else if (tmp_string == "CAM_VIRTUALRANGE")
00583                 {
00584                         left_color_camera_intrinsic_type_ = ipa_CameraSensors::CAM_VIRTUALRANGE;
00585                 } 
00586                 else if (tmp_string == "CAM_VIRTUALCOLOR")
00587                 {
00588                         left_color_camera_intrinsic_type_ = ipa_CameraSensors::CAM_VIRTUALCOLOR;
00589                 } 
00590                 else
00591                 {
00592                         std::string str = "[all_cameras] Camera type '" + tmp_string + "' for intrinsics  of left camera unknown, try 'CAM_AVTPIKE','CAM_PROSILICA' or 'CAM_SWISSRANGER'";
00593                         ROS_ERROR("%s", str.c_str());
00594                         return false;
00595                 }
00596                 if (node_handle_.getParam("all_cameras/left_color_camera_intrinsic_id", left_color_camera_intrinsic_id_) == false)
00597                 {
00598                         ROS_ERROR("[all_cameras] Intrinsic camera id for left color camera not specified");
00599                         return false;
00600                 }
00601 
00602                 
00603                 ROS_INFO("Intrinsic for left color camera: %s_%d", tmp_string.c_str(), left_color_camera_intrinsic_id_);
00604 
00605                 // There are several intrinsic matrices, optimized to different cameras
00606                 // Here, we specified the desired intrinsic matrix for each camera
00607                 if (node_handle_.getParam("all_cameras/right_color_camera_intrinsic_type", tmp_string) == false)
00608                 {
00609                         ROS_ERROR("[all_cameras] Intrinsic camera type for right color camera not specified");
00610                         return false;
00611                 }
00612                 if (tmp_string == "CAM_AVTPIKE")
00613                 {
00614                         right_color_camera_intrinsic_type_ = ipa_CameraSensors::CAM_AVTPIKE;
00615                 }
00616                 else if (tmp_string == "CAM_PROSILICA")
00617                 {
00618                         right_color_camera_intrinsic_type_ = ipa_CameraSensors::CAM_PROSILICA;
00619                 } 
00620                 else if (tmp_string == "CAM_SWISSRANGER")
00621                 {
00622                         right_color_camera_intrinsic_type_ = ipa_CameraSensors::CAM_SWISSRANGER;
00623                 } 
00624                 else if (tmp_string == "CAM_VIRTUALRANGE")
00625                 {
00626                         right_color_camera_intrinsic_type_ = ipa_CameraSensors::CAM_VIRTUALRANGE;
00627                 } 
00628                 else if (tmp_string == "CAM_VIRTUALCOLOR")
00629                 {
00630                         right_color_camera_intrinsic_type_ = ipa_CameraSensors::CAM_VIRTUALCOLOR;
00631                 } 
00632                 else
00633                 {
00634                         std::string str = "[all_cameras] Camera type '" + tmp_string + "' for intrinsics  of right camera unknown, try 'CAM_AVTPIKE','CAM_PROSILICA' or 'CAM_SWISSRANGER'";
00635                         ROS_ERROR("%s", str.c_str());
00636                         return false;
00637                 }
00638                 if (node_handle_.getParam("all_cameras/right_color_camera_intrinsic_id", right_color_camera_intrinsic_id_) == false)
00639                 {
00640                         ROS_ERROR("[all_cameras] Intrinsic camera id for right color camera not specified");
00641                         return false;
00642                 }
00643 
00644                 ROS_INFO("Intrinsic for right color camera: %s_%d", tmp_string.c_str(), right_color_camera_intrinsic_id_);
00645 
00646                 // There are several intrinsic matrices, optimized to different cameras
00647                 // Here, we specified the desired intrinsic matrix for each camera
00648                 if (node_handle_.getParam("all_cameras/tof_camera_intrinsic_type", tmp_string) == false)
00649                 {
00650                         ROS_ERROR("[all_cameras] Intrinsic camera type for tof camera not specified");
00651                         return false;
00652                 }
00653                 if (tmp_string == "CAM_AVTPIKE")
00654                 {
00655                         tof_camera_intrinsic_type_ = ipa_CameraSensors::CAM_AVTPIKE;
00656                 }
00657                 else if (tmp_string == "CAM_PROSILICA")
00658                 {
00659                         tof_camera_intrinsic_type_ = ipa_CameraSensors::CAM_PROSILICA;
00660                 } 
00661                 else if (tmp_string == "CAM_SWISSRANGER")
00662                 {
00663                         tof_camera_intrinsic_type_ = ipa_CameraSensors::CAM_SWISSRANGER;
00664                 } 
00665                 else if (tmp_string == "CAM_VIRTUALRANGE")
00666                 {
00667                         tof_camera_intrinsic_type_ = ipa_CameraSensors::CAM_VIRTUALRANGE;
00668                 } 
00669                 else if (tmp_string == "CAM_VIRTUALCOLOR")
00670                 {
00671                         tof_camera_intrinsic_type_ = ipa_CameraSensors::CAM_VIRTUALCOLOR;
00672                 } 
00673                 else
00674                 {
00675                         std::string str = "[all_cameras] Camera type '" + tmp_string + "' for intrinsics  unknown, try 'CAM_AVTPIKE','CAM_PROSILICA' or 'CAM_SWISSRANGER'";
00676                         ROS_ERROR("%s", str.c_str());
00677                         return false;
00678                 }
00679                 if (node_handle_.getParam("all_cameras/tof_camera_intrinsic_id", tof_camera_intrinsic_id_) == false)
00680                 {
00681                         ROS_ERROR("[all_cameras] Intrinsic camera id for tof camera not specified");
00682                         return false;
00683                 }
00684 
00685                 ROS_INFO("Intrinsic for tof camera: %s_%d", tmp_string.c_str(), tof_camera_intrinsic_id_);
00686         
00687                 return true;
00688         }
00689 };
00690 
00691 //#######################
00692 //#### main programm ####
00693 int main(int argc, char** argv)
00694 {
00696         ros::init(argc, argv, "color_camera");
00697 
00699         ros::NodeHandle nh;
00700         
00702         CobAllCamerasNode camera_node(nh);
00703 
00705         if (camera_node.init() == false)
00706         {
00707                 ROS_ERROR("[all_cameras] Node initialization FAILED. Terminating");
00708                 return 0;
00709         }
00710         else
00711         {
00712                 ROS_INFO("[all_cameras] Node initialization OK. Enter spinning");
00713         }
00714 
00715         camera_node.spin();
00716         
00717         return 0;
00718 }
all_cameras.cpp
