$search
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 * Description: 00014 * 00015 * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 00016 * 00017 * Author: Jan Fischer, email:jan.fischer@ipa.fhg.de 00018 * Supervised by: Jan Fischer, email:jan.fischer@ipa.fhg.de 00019 * 00020 * Date of creation: Mai 2008 00021 * ToDo: 00022 * 00023 * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 00024 * 00025 * Redistribution and use in source and binary forms, with or without 00026 * modification, are permitted provided that the following conditions are met: 00027 * 00028 * * Redistributions of source code must retain the above copyright 00029 * notice, this list of conditions and the following disclaimer. 00030 * * Redistributions in binary form must reproduce the above copyright 00031 * notice, this list of conditions and the following disclaimer in the 00032 * documentation and/or other materials provided with the distribution. 00033 * * Neither the name of the Fraunhofer Institute for Manufacturing 00034 * Engineering and Automation (IPA) nor the names of its 00035 * contributors may be used to endorse or promote products derived from 00036 * this software without specific prior written permission. 00037 * 00038 * This program is free software: you can redistribute it and/or modify 00039 * it under the terms of the GNU Lesser General Public License LGPL as 00040 * published by the Free Software Foundation, either version 3 of the 00041 * License, or (at your option) any later version. 00042 * 00043 * This program is distributed in the hope that it will be useful, 00044 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00045 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00046 * GNU Lesser General Public License LGPL for more details. 00047 * 00048 * You should have received a copy of the GNU Lesser General Public 00049 * License LGPL along with this program. 00050 * If not, see <http://www.gnu.org/licenses/>. 00051 * 00052 ****************************************************************/ 00053 #include "../include/cob_camera_sensors/StdAfx.h" 00054 00055 #ifdef __LINUX__ 00056 #include "cob_camera_sensors/VirtualColorCam.h" 00057 #include "tinyxml.h" 00058 #else 00059 #include "cob_driver/cob_camera_sensors/common/include/cob_camera_sensors/VirtualColorCam.h" 00060 #include "cob_vision/windows/src/extern/TinyXml/tinyxml.h" 00061 #endif 00062 00063 #include <opencv/highgui.h> 00064 #include <iostream> 00065 00066 namespace fs = boost::filesystem; 00067 using namespace ipa_CameraSensors; 00068 00069 __DLL_LIBCAMERASENSORS__ AbstractColorCameraPtr ipa_CameraSensors::CreateColorCamera_VirtualCam() 00070 { 00071 return AbstractColorCameraPtr(new VirtualColorCam()); 00072 } 00073 00074 VirtualColorCam::VirtualColorCam() 00075 { 00076 m_initialized = false; 00077 m_open = false; 00078 00079 m_BufferSize = 1; 00080 00081 m_ImageWidth = 0; 00082 m_ImageHeight = 0; 00083 00084 m_ImageCounter = 0; 00085 } 00086 00087 VirtualColorCam::~VirtualColorCam() 00088 { 00089 if (isOpen()) 00090 { 00091 Close(); 00092 } 00093 } 00094 00095 00096 unsigned long VirtualColorCam::Init(std::string directory, int cameraIndex) 00097 { 00098 if (isInitialized()) 00099 { 00100 return (RET_OK | RET_CAMERA_ALREADY_INITIALIZED); 00101 } 00102 00103 m_CameraType = ipa_CameraSensors::CAM_VIRTUALCOLOR; 00104 00105 // It is important to put this before LoadParameters 00106 m_CameraDataDirectory = directory; 00107 if (LoadParameters((directory + "cameraSensorsIni.xml").c_str(), cameraIndex) & RET_FAILED) 00108 { 00109 return (RET_FAILED | RET_INIT_CAMERA_FAILED); 00110 } 00111 00112 m_CameraIndex = cameraIndex; 00113 00114 m_initialized = true; 00115 return RET_OK; 00116 00117 } 00118 00119 00120 unsigned long VirtualColorCam::Open() 00121 { 00122 if (!isInitialized()) 00123 { 00124 std::cerr << "ERROR - VirtualColorCam::Open:" << std::endl; 00125 std::cerr << "\t ... Camera not initialized." << std::endl; 00126 return (RET_FAILED); 00127 } 00128 m_open = false; 00129 00130 if (isOpen()) 00131 { 00132 return (RET_OK | RET_CAMERA_ALREADY_OPEN); 00133 } 00134 00135 // Convert camera ID to string 00136 std::stringstream ss; 00137 std::string sCameraIndex; 00138 ss << m_CameraIndex; 00139 ss >> sCameraIndex; 00140 00141 m_ImageWidth = -1; 00142 m_ImageHeight = -1; 00143 00144 // Create absolute filename and check if directory exists 00145 fs::path absoluteDirectoryName( m_CameraDataDirectory ); 00146 if ( !fs::exists( absoluteDirectoryName ) ) 00147 { 00148 std::cerr << "ERROR - VirtualColorCam::Open:" << std::endl; 00149 std::cerr << "\t ... Path '" << absoluteDirectoryName.file_string() << "' not found" << std::endl; 00150 return (ipa_CameraSensors::RET_FAILED | ipa_CameraSensors::RET_FAILED_OPEN_FILE); 00151 } 00152 00153 int colorImageCounter = 0; 00154 // Extract all image filenames from the directory 00155 if ( fs::is_directory( absoluteDirectoryName ) ) 00156 { 00157 std::cout << "INFO - VirtualColorCam::Open:" << std::endl; 00158 std::cout << "\t ... Parsing directory '" << absoluteDirectoryName.directory_string() << "'" << std::endl;; 00159 fs::directory_iterator end_iter; 00160 for ( fs::directory_iterator dir_itr( absoluteDirectoryName ); dir_itr != end_iter; ++dir_itr ) 00161 { 00162 try 00163 { 00164 if (fs::is_regular_file(dir_itr->status())) 00165 { 00166 std::string filename = dir_itr->path().string(); 00167 if ((dir_itr->path().extension() == ".jpg" || dir_itr->path().extension() == ".jpe" || 00168 dir_itr->path().extension() == ".jpeg" || dir_itr->path().extension() == ".bmp" || 00169 dir_itr->path().extension() == ".bmp" || dir_itr->path().extension() == ".dib" || 00170 dir_itr->path().extension() == ".png" || dir_itr->path().extension() == ".pgm" || 00171 dir_itr->path().extension() == ".ppm" || dir_itr->path().extension() == ".sr" || 00172 dir_itr->path().extension() == ".ras" || dir_itr->path().extension() == ".tiff" || 00173 dir_itr->path().extension() == ".exr" || dir_itr->path().extension() == ".jp2") && 00174 filename.find( "ColorCamRGB_8U3_" + sCameraIndex, 0 ) != std::string::npos) 00175 { 00176 ++colorImageCounter; 00177 //std::cout << "VirtualColorCam::Open(): Reading '" << dir_itr->path().string() << "\n"; 00178 m_ColorImageFileNames.push_back(dir_itr->path().string()); 00179 // Get image size 00180 if (m_ImageWidth == -1 || m_ImageHeight == -1) 00181 { 00182 cv::Mat image = cv::imread(m_ColorImageFileNames.back()); 00183 m_ImageWidth = image.cols; 00184 m_ImageHeight = image.rows; 00185 } 00186 } 00187 } 00188 } 00189 catch ( const std::exception &ex ) 00190 { 00191 std::cerr << "ERROR - VirtualColorCam::Open:" << std::endl; 00192 std::cerr << "\t ... Exception catch of '" << ex.what() << "'" << std::endl; 00193 } 00194 } 00195 std::sort(m_ColorImageFileNames.begin(),m_ColorImageFileNames.end()); 00196 std::cout << "INFO - VirtualColorCam::Open:" << std::endl; 00197 std::cerr << "\t ... Extracted '" << colorImageCounter << "' color images (8*3 bit/color)\n"; 00198 } 00199 else 00200 { 00201 std::cerr << "ERROR - VirtualColorCam::Open:" << std::endl; 00202 std::cerr << "\t .... Path '" << absoluteDirectoryName.file_string() << "' is not a directory." << std::endl; 00203 return ipa_CameraSensors::RET_FAILED; 00204 } 00205 00206 if (colorImageCounter == 0) 00207 { 00208 std::cerr << "ERROR - VirtualColorCam::Open:" << std::endl; 00209 std::cerr << "\t ... Could not detect any color images" << std::endl; 00210 std::cerr << "\t ... from the specified directory. Check directory" << std::endl; 00211 std::cerr << "\t ... and filenames (i.e. ColorCamRGB_8U3_*_*.jpg)." << std::endl; 00212 return ipa_CameraSensors::RET_FAILED; 00213 } 00214 00215 std::cout << "*******************************************************" << std::endl; 00216 std::cout << "VirtualColorCam::Open: Virtual color camera device OPEN" << std::endl; 00217 std::cout << "*******************************************************" << std::endl << std::endl; 00218 00219 m_open = true; 00220 return RET_OK; 00221 00222 } 00223 00224 int VirtualColorCam::GetNumberOfImages() 00225 { 00226 return (int)std::min(0.0f, (float)m_ColorImageFileNames.size()); 00227 } 00228 00229 unsigned long VirtualColorCam::SaveParameters(const char* filename) 00230 { 00231 return RET_FAILED; 00232 } 00233 00234 unsigned long VirtualColorCam::SetPathToImages(std::string path) 00235 { 00236 m_CameraDataDirectory = path; 00237 return RET_OK; 00238 } 00239 00240 00241 unsigned long VirtualColorCam::Close() 00242 { 00243 if (!isOpen()) 00244 { 00245 return RET_OK; 00246 } 00247 00248 m_open = false; 00249 return RET_OK; 00250 } 00251 00252 00253 unsigned long VirtualColorCam::SetProperty(t_cameraProperty* cameraProperty) 00254 { 00255 #ifdef __LINUX__ 00256 std::cerr << "VirtualColorCam::SetProperty: Function not implemented."; 00257 return RET_FAILED; 00258 #endif 00259 #ifndef __LINUX__ 00260 00261 switch (cameraProperty->propertyID) 00262 { 00263 case PROP_CAMERA_RESOLUTION: 00264 m_ImageWidth = cameraProperty->cameraResolution.xResolution; 00265 m_ImageHeight = cameraProperty->cameraResolution.yResolution; 00266 break; 00267 default: 00268 std::cerr << "ERROR - VirtualColorCam::SetProperty:" << std::endl; 00269 std::cerr << "\t ... Property " << cameraProperty->propertyID << " unspecified."; 00270 return RET_FAILED; 00271 break; 00272 } 00273 00274 return RET_OK; 00275 #endif 00276 } 00277 00278 unsigned long VirtualColorCam::SetPropertyDefaults() {return RET_FUNCTION_NOT_IMPLEMENTED;} 00279 00280 unsigned long VirtualColorCam::GetProperty(t_cameraProperty* cameraProperty) 00281 { 00282 switch (cameraProperty->propertyID) 00283 { 00284 case PROP_CAMERA_RESOLUTION: 00285 cameraProperty->cameraResolution.xResolution = m_ImageWidth; 00286 cameraProperty->cameraResolution.yResolution = m_ImageHeight; 00287 cameraProperty->propertyType = TYPE_CAMERA_RESOLUTION; 00288 return RET_OK; 00289 break; 00290 00291 case PROP_DMA_BUFFER_SIZE: 00292 cameraProperty->u_integerData = m_BufferSize; 00293 return RET_OK; 00294 break; 00295 00296 default: 00297 std::cerr << "ERROR - VirtualColorCam::SetProperty:" << std::endl; 00298 std::cerr << "\t ... Property " << cameraProperty->propertyID << " unspecified."; 00299 return RET_FAILED; 00300 break; 00301 00302 } 00303 00304 return RET_OK; 00305 } 00306 00307 00308 unsigned long VirtualColorCam::GetColorImage(char* colorImageData, bool getLatestFrame) 00309 { 00310 if (!isOpen()) 00311 { 00312 std::cerr << "ERROR - VirtualColorCam::GetColorImage:" << std::endl; 00313 std::cerr << "\t ... Color camera not open." << std::endl; 00314 return (RET_FAILED | RET_CAMERA_NOT_OPEN); 00315 } 00316 00317 IplImage* colorImage = (IplImage*) cvLoadImage(m_ColorImageFileNames[m_ImageCounter].c_str(), CV_LOAD_IMAGE_COLOR); 00318 00319 for(int row=0; row<m_ImageHeight; row++) 00320 { 00321 for (int col=0; col<m_ImageWidth; col++) 00322 { 00323 char* f_color_ptr = &((char*) (colorImage->imageData + row*colorImage->widthStep))[col*3]; 00324 ((char*) (colorImageData + row*colorImage->widthStep))[col*3 + 0] = f_color_ptr[0]; 00325 ((char*) (colorImageData + row*colorImage->widthStep))[col*3 + 1] = f_color_ptr[1]; 00326 ((char*) (colorImageData + row*colorImage->widthStep))[col*3 + 2] = f_color_ptr[2]; 00327 } 00328 } 00329 00330 cvReleaseImage(&colorImage); 00331 00332 m_ImageCounter++; 00333 if (m_ImageCounter >= m_ColorImageFileNames.size()) 00334 { 00335 // Reset image counter 00336 m_ImageCounter = 0; 00337 } 00338 00339 return RET_OK; 00340 } 00341 00342 00343 unsigned long VirtualColorCam::GetColorImage(cv::Mat* colorImage, bool getLatestFrame) 00344 { 00345 if (!isOpen()) 00346 { 00347 std::cerr << "ERROR - VirtualColorCam::GetColorImage:" << std::endl; 00348 std::cerr << "\t ... Color camera not open." << std::endl; 00349 return (RET_FAILED | RET_CAMERA_NOT_OPEN); 00350 } 00351 00352 CV_Assert(colorImage != 0); 00353 00354 colorImage->create(m_ImageHeight, m_ImageWidth, CV_8UC3); 00355 00356 return GetColorImage((char*)(colorImage->ptr<unsigned char>(0)), getLatestFrame); 00357 00358 return RET_FAILED; 00359 } 00360 00361 unsigned long VirtualColorCam::PrintCameraInformation() 00362 { 00363 return RET_FUNCTION_NOT_IMPLEMENTED; 00364 } 00365 00366 unsigned long VirtualColorCam::TestCamera(const char* filename) 00367 { 00368 if (AbstractColorCamera::TestCamera(filename) & RET_FAILED) 00369 { 00370 return RET_FAILED; 00371 } 00372 00373 return RET_OK; 00374 } 00375 00376 00377 unsigned long VirtualColorCam::LoadParameters(const char* filename, int cameraIndex) 00378 { //m_intrinsicMatrix; m_distortionParameters 00379 boost::shared_ptr<TiXmlDocument> p_configXmlDocument (new TiXmlDocument( filename )); 00380 if (!p_configXmlDocument->LoadFile()) 00381 { 00382 std::cerr << "ERROR - VirtualColorCam::LoadParameters:" << std::endl; 00383 std::cerr << "\t ... Error while loading xml configuration file (Check filename and syntax of the file):\n" << filename << std::endl; 00384 return (RET_FAILED | RET_FAILED_OPEN_FILE); 00385 } 00386 std::cout << "INFO - VirtualColorCam::LoadParameters:" << std::endl; 00387 std::cout << "\t ... Parsing xml configuration file:" << std::endl; 00388 std::cout << "\t ... '" << filename << "'" << std::endl; 00389 00390 if ( p_configXmlDocument ) 00391 { 00392 00393 //************************************************************************************ 00394 // BEGIN LibCameraSensors 00395 //************************************************************************************ 00396 // Tag element "LibCameraSensors" of Xml Inifile 00397 TiXmlElement *p_xmlElement_Root = NULL; 00398 p_xmlElement_Root = p_configXmlDocument->FirstChildElement( "LibCameraSensors" ); 00399 if ( p_xmlElement_Root ) 00400 { 00401 00402 //************************************************************************************ 00403 // BEGIN LibCameraSensors->VirtualColorCam 00404 //************************************************************************************ 00405 // Tag element "VirtualColorCam" of Xml Inifile 00406 TiXmlElement *p_xmlElement_Root_VirtualColorCam = NULL; 00407 std::stringstream ss; 00408 ss << "VirtualColorCam_" << cameraIndex; 00409 p_xmlElement_Root_VirtualColorCam = p_xmlElement_Root->FirstChildElement( ss.str() ); 00410 if ( p_xmlElement_Root_VirtualColorCam ) 00411 { 00412 00413 //************************************************************************************ 00414 // BEGIN LibCameraSensors->VirtualColorCam->CameraDataDirectory 00415 //************************************************************************************ 00416 // Subtag element "CameraDataDirectory" of Xml Inifile 00417 TiXmlElement *p_xmlElement_Child = NULL; 00418 p_xmlElement_Child = p_xmlElement_Root_VirtualColorCam->FirstChildElement( "CameraDataDirectory" ); 00419 if ( p_xmlElement_Child ) 00420 { 00421 // read and save value of attribute 00422 std::string tempString; 00423 if ( p_xmlElement_Child->QueryValueAttribute( "relativePath", &tempString ) != TIXML_SUCCESS) 00424 { 00425 std::cerr << "VirtualColorCam::LoadParameters: Can't find attribute 'relativePath' of tag 'CameraDataDirectory'." << std::endl; 00426 return (RET_FAILED | RET_XML_ATTR_NOT_FOUND); 00427 } 00428 00429 m_CameraDataDirectory = m_CameraDataDirectory + tempString + "/"; 00430 } 00431 else 00432 { 00433 std::cerr << "ERROR - VirtualColorCam::LoadParameters:" << std::endl; 00434 std::cerr << "\t ... Can't find tag 'CameraDataDirectory'." << std::endl; 00435 return (RET_FAILED | RET_XML_TAG_NOT_FOUND); 00436 } 00437 } 00438 //************************************************************************************ 00439 // END LibCameraSensors->VirtualColorCam 00440 //************************************************************************************ 00441 else 00442 { 00443 std::cerr << "ERROR - VirtualColorCam::LoadParameters:" << std::endl; 00444 std::cerr << "\t ... Can't find tag '" << ss.str() << "'" << std::endl; 00445 return (RET_FAILED | RET_XML_TAG_NOT_FOUND); 00446 } 00447 } 00448 00449 //************************************************************************************ 00450 // END LibCameraSensors 00451 //************************************************************************************ 00452 else 00453 { 00454 std::cerr << "ERROR - VirtualColorCam::LoadParameters:" << std::endl; 00455 std::cerr << "\t ... Can't find tag 'LibCameraSensors'." << std::endl; 00456 return (RET_FAILED | RET_XML_TAG_NOT_FOUND); 00457 } 00458 } 00459 00460 std::cout << "\t [OK] Parsing xml configuration file " << std::endl; 00461 00462 return RET_OK; 00463 }