$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 * 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 <polled_camera/publication_server.h> 00062 #include <cv_bridge/CvBridge.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_vision_utils/CameraSensorToolbox.h> 00073 #include <cob_vision_utils/GlobalDefines.h> 00074 00075 using namespace ipa_CameraSensors; 00076 00079 class CobColorCameraNode 00080 { 00081 private: 00082 ros::NodeHandle node_handle_; 00083 polled_camera::PublicationServer image_poll_server_; 00084 00085 AbstractColorCameraPtr color_camera_; 00086 00087 std::string config_directory_; 00088 int camera_index_; 00089 int color_camera_intrinsic_id_; 00090 ipa_CameraSensors::t_cameraType color_camera_intrinsic_type_; 00091 00092 sensor_msgs::CameraInfo camera_info_msg_; 00093 00094 ros::ServiceServer camera_info_service_; 00095 00096 cv::Mat color_image_8U3_; 00097 00098 public: 00099 CobColorCameraNode(const ros::NodeHandle& node_handle) 00100 : node_handle_(node_handle), 00101 color_camera_(AbstractColorCameraPtr()), 00102 color_image_8U3_(cv::Mat()) 00103 { 00105 } 00106 00107 ~CobColorCameraNode() 00108 { 00109 image_poll_server_.shutdown(); 00110 color_camera_->Close(); 00111 } 00112 00114 bool init() 00115 { 00116 if (loadParameters() == false) 00117 { 00118 ROS_ERROR("[color_camera] Could not read all parameters from launch file"); 00119 return false; 00120 } 00121 00122 if (color_camera_->Init(config_directory_, camera_index_) & ipa_CameraSensors::RET_FAILED) 00123 { 00124 std::stringstream ss; 00125 ss << "initialization of color camera "; 00126 ss << camera_index_; 00127 ss << " failed"; 00128 ROS_ERROR("[color_camera] %s", ss.str().c_str()); 00129 color_camera_->Close(); 00130 color_camera_ = AbstractColorCameraPtr(); 00131 return false; 00132 } 00133 00134 if (color_camera_ && (color_camera_->Open() & ipa_CameraSensors::RET_FAILED)) 00135 { 00136 std::stringstream ss; 00137 ss << "Could not open color camera "; 00138 ss << camera_index_; 00139 ROS_ERROR("[color_camera] %s", ss.str().c_str()); 00140 color_camera_->Close(); 00141 color_camera_ = AbstractColorCameraPtr(); 00142 return false; 00143 } 00144 00146 ipa_CameraSensors::t_cameraProperty cameraProperty; 00147 cameraProperty.propertyID = ipa_CameraSensors::PROP_CAMERA_RESOLUTION; 00148 color_camera_->GetProperty(&cameraProperty); 00149 int color_sensor_width = cameraProperty.cameraResolution.xResolution; 00150 int color_sensor_height = cameraProperty.cameraResolution.yResolution; 00151 cv::Size color_image_size(color_sensor_width, color_sensor_height); 00152 00154 ipa_CameraSensors::CameraSensorToolboxPtr color_sensor_toolbox = ipa_CameraSensors::CreateCameraSensorToolbox(); 00155 color_sensor_toolbox->Init(config_directory_, color_camera_->GetCameraType(), camera_index_, color_image_size); 00156 00157 cv::Mat d = color_sensor_toolbox->GetDistortionParameters(color_camera_intrinsic_type_, color_camera_intrinsic_id_); 00158 camera_info_msg_.header.stamp = ros::Time::now(); 00159 if (camera_index_ == 0) 00160 camera_info_msg_.header.frame_id = "head_color_camera_r_link"; 00161 else 00162 camera_info_msg_.header.frame_id = "head_color_camera_l_link"; 00163 camera_info_msg_.D[0] = d.at<double>(0, 0); 00164 camera_info_msg_.D[1] = d.at<double>(0, 1); 00165 camera_info_msg_.D[2] = d.at<double>(0, 2); 00166 camera_info_msg_.D[3] = d.at<double>(0, 3); 00167 camera_info_msg_.D[4] = 0; 00168 00169 cv::Mat k = color_sensor_toolbox->GetIntrinsicMatrix(color_camera_intrinsic_type_, color_camera_intrinsic_id_); 00170 camera_info_msg_.K[0] = k.at<double>(0, 0); 00171 camera_info_msg_.K[1] = k.at<double>(0, 1); 00172 camera_info_msg_.K[2] = k.at<double>(0, 2); 00173 camera_info_msg_.K[3] = k.at<double>(1, 0); 00174 camera_info_msg_.K[4] = k.at<double>(1, 1); 00175 camera_info_msg_.K[5] = k.at<double>(1, 2); 00176 camera_info_msg_.K[6] = k.at<double>(2, 0); 00177 camera_info_msg_.K[7] = k.at<double>(2, 1); 00178 camera_info_msg_.K[8] = k.at<double>(2, 2); 00179 00180 camera_info_msg_.width = color_sensor_width; 00181 camera_info_msg_.height = color_sensor_height; 00182 00184 camera_info_service_ = node_handle_.advertiseService("set_camera_info", &CobColorCameraNode::setCameraInfo, this); 00185 00187 image_poll_server_ = polled_camera::advertise(node_handle_, "request_image", &CobColorCameraNode::pollCallback, this); 00188 00189 return true; 00190 } 00191 00196 bool setCameraInfo(sensor_msgs::SetCameraInfo::Request& req, 00197 sensor_msgs::SetCameraInfo::Response& rsp) 00198 { 00200 camera_info_msg_ = req.camera_info; 00201 00202 rsp.success = false; 00203 rsp.status_message = "[color_camera] Setting camera parameters through ROS not implemented"; 00204 00205 return true; 00206 } 00207 00209 void pollCallback(polled_camera::GetPolledImage::Request& req, 00210 polled_camera::GetPolledImage::Response& res, 00211 sensor_msgs::Image& image_msg, sensor_msgs::CameraInfo& info) 00212 { 00214 if (color_camera_->GetColorImage(&color_image_8U3_) & ipa_Utils::RET_FAILED) 00215 { 00216 ROS_ERROR("[color_camera] Color image acquisition failed"); 00217 res.success = false; 00218 return; 00219 } 00220 00221 try 00222 { 00223 IplImage img = color_image_8U3_; 00224 image_msg = *(sensor_msgs::CvBridge::cvToImgMsg(&img, "bgr8")); 00225 } 00226 catch (sensor_msgs::CvBridgeException error) 00227 { 00228 ROS_ERROR("[color_camera] Could not convert IplImage to ROS message"); 00229 } 00230 00232 ros::Time now = ros::Time::now(); 00233 image_msg.header.stamp = now; 00234 if (camera_index_ == 0) 00235 image_msg.header.frame_id = "head_color_camera_r_link"; 00236 else 00237 image_msg.header.frame_id = "head_color_camera_l_link"; 00238 image_msg.encoding = "bgr8"; 00239 00240 info = camera_info_msg_; 00241 info.width = color_image_8U3_.cols; 00242 info.height = color_image_8U3_.rows; 00243 info.header.stamp = now; 00244 if (camera_index_ == 0) 00245 info.header.frame_id = "head_color_camera_r_link"; 00246 else 00247 info.header.frame_id = "head_color_camera_l_link"; 00248 00249 res.success = true; 00250 return; 00251 } 00252 00253 bool loadParameters() 00254 { 00255 std::string tmp_string = "NULL"; 00256 00257 if (node_handle_.getParam("configuration_files", config_directory_) == false) 00258 { 00259 ROS_ERROR("[color_camera] Path to xml configuration for color camera not specified"); 00260 return false; 00261 } 00262 00263 ROS_INFO("Configuration directory: %s", config_directory_.c_str()); 00264 00266 if (node_handle_.getParam("camera_index", camera_index_) == false) 00267 { 00268 ROS_ERROR("[color_camera] Color camera index (0 or 1) not specified"); 00269 return false; 00270 } 00271 00273 if (node_handle_.getParam("color_camera_type", tmp_string) == false) 00274 { 00275 ROS_ERROR("[color_camera] Color camera type not specified"); 00276 return false; 00277 } 00278 if (tmp_string == "CAM_AVTPIKE") color_camera_ = ipa_CameraSensors::CreateColorCamera_AVTPikeCam(); 00279 else if (tmp_string == "CAM_VIRTUAL") color_camera_ = ipa_CameraSensors::CreateColorCamera_VirtualCam(); 00280 else if (tmp_string == "CAM_PROSILICA") ROS_ERROR("[color_camera] Color camera type not CAM_PROSILICA not yet implemented"); 00281 else 00282 { 00283 std::string str = "[color_camera] Camera type '" + tmp_string + "' unknown, try 'CAM_AVTPIKE' or 'CAM_PROSILICA'"; 00284 ROS_ERROR("%s", str.c_str()); 00285 return false; 00286 } 00287 00288 ROS_INFO("Camera type: %s_%d", tmp_string.c_str(), camera_index_); 00289 00290 // There are several intrinsic matrices, optimized to different cameras 00291 // Here, we specified the desired intrinsic matrix for each camera 00292 if (node_handle_.getParam("color_camera_intrinsic_type", tmp_string) == false) 00293 { 00294 ROS_ERROR("[color_camera] Intrinsic camera type for color camera not specified"); 00295 return false; 00296 } 00297 if (tmp_string == "CAM_AVTPIKE") 00298 { 00299 color_camera_intrinsic_type_ = ipa_CameraSensors::CAM_AVTPIKE; 00300 } 00301 else if (tmp_string == "CAM_PROSILICA") 00302 { 00303 color_camera_intrinsic_type_ = ipa_CameraSensors::CAM_PROSILICA; 00304 } 00305 else if (tmp_string == "CAM_VIRTUALRANGE") 00306 { 00307 color_camera_intrinsic_type_ = ipa_CameraSensors::CAM_VIRTUALRANGE; 00308 } 00309 else 00310 { 00311 std::string str = "[color_camera] Camera type '" + tmp_string + "' for intrinsics unknown, try 'CAM_AVTPIKE','CAM_PROSILICA' or 'CAM_SWISSRANGER'"; 00312 ROS_ERROR("%s", str.c_str()); 00313 return false; 00314 } 00315 if (node_handle_.getParam("color_camera_intrinsic_id", color_camera_intrinsic_id_) == false) 00316 { 00317 ROS_ERROR("[color_camera] Intrinsic camera id for color camera not specified"); 00318 return false; 00319 } 00320 00321 ROS_INFO("Intrinsic for color camera: %s_%d", tmp_string.c_str(), color_camera_intrinsic_id_); 00322 00323 return true; 00324 } 00325 }; 00326 00327 //####################### 00328 //#### main programm #### 00329 int main(int argc, char** argv) 00330 { 00332 ros::init(argc, argv, "color_camera"); 00333 00335 ros::NodeHandle nh; 00336 00338 CobColorCameraNode camera_node(nh); 00339 00341 if (!camera_node.init()) return 0; 00342 00343 ros::spin(); 00344 00345 return 0; 00346 }