00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025 #include <ros/ros.h>
00026 #include <cv_bridge/CvBridge.h>
00027 #include <image_transport/image_transport.h>
00028
00029
00030 #include <sensor_msgs/Image.h>
00031 #include <sensor_msgs/CameraInfo.h>
00032 #include <sensor_msgs/fill_image.h>
00033 #include <sensor_msgs/SetCameraInfo.h>
00034
00035
00036 #include <cob_camera_sensors/AbstractColorCamera.h>
00037 #include <cob_camera_sensors/AbstractRangeImagingSensor.h>
00038 #include <cob_vision_utils/GlobalDefines.h>
00039 #include <cob_vision_utils/CameraSensorToolbox.h>
00040
00041 using namespace ipa_CameraSensors;
00042
00045 class CobAllCamerasNode
00046 {
00047 private:
00048 ros::NodeHandle node_handle_;
00049
00050 AbstractColorCameraPtr left_color_camera_;
00051 AbstractColorCameraPtr right_color_camera_;
00052 AbstractRangeImagingSensorPtr tof_camera_;
00053
00054 std::string config_directory_;
00055
00056 std::string left_color_camera_ns_;
00057 std::string right_color_camera_ns_;
00058 std::string tof_camera_ns_;
00059
00060 int left_color_camera_intrinsic_id_;
00061 int right_color_camera_intrinsic_id_;
00062 int tof_camera_intrinsic_id_;
00063
00064 ipa_CameraSensors::t_cameraType left_color_camera_intrinsic_type_;
00065 ipa_CameraSensors::t_cameraType right_color_camera_intrinsic_type_;
00066 ipa_CameraSensors::t_cameraType tof_camera_intrinsic_type_;
00067
00068 sensor_msgs::CameraInfo left_color_camera_info_msg_;
00069 sensor_msgs::CameraInfo right_color_camera_info_msg_;
00070 sensor_msgs::CameraInfo tof_camera_info_msg_;
00071
00072 ros::ServiceServer left_color_camera_info_service_;
00073 ros::ServiceServer right_color_camera_info_service_;
00074 ros::ServiceServer tof_camera_info_service_;
00075
00076 cv::Mat left_color_image_8U3_;
00077 cv::Mat right_color_image_8U3_;
00078 cv::Mat xyz_tof_image_32F3_;
00079 cv::Mat grey_tof_image_32F1_;
00080
00081 image_transport::ImageTransport image_transport_;
00082 image_transport::CameraPublisher xyz_tof_image_publisher_;
00083 image_transport::CameraPublisher grey_tof_image_publisher_;
00084 image_transport::CameraPublisher left_color_image_publisher_;
00085 image_transport::CameraPublisher right_color_image_publisher_;
00086
00087 public:
00088 CobAllCamerasNode(const ros::NodeHandle& node_handle)
00089 : node_handle_(node_handle),
00090 left_color_camera_(AbstractColorCameraPtr()),
00091 right_color_camera_(AbstractColorCameraPtr()),
00092 tof_camera_(AbstractRangeImagingSensorPtr()),
00093 left_color_image_8U3_(cv::Mat()),
00094 right_color_image_8U3_(cv::Mat()),
00095 xyz_tof_image_32F3_(cv::Mat()),
00096 grey_tof_image_32F1_(cv::Mat()),
00097 image_transport_(node_handle)
00098 {
00100 }
00101
00102 ~CobAllCamerasNode()
00103 {
00104 ROS_INFO("[all_cameras] Shutting down cameras");
00105 if (left_color_camera_)
00106 {
00107 ROS_INFO("[all_cameras] Shutting down left color camera (1)");
00108 left_color_camera_->Close();
00109 }
00110 if (right_color_camera_)
00111 {
00112 ROS_INFO("[all_cameras] Shutting down right color camera (0)");
00113 right_color_camera_->Close();
00114 }
00115 if (tof_camera_)
00116 {
00117 ROS_INFO("[all_cameras] Shutting down tof camera (0)");
00118 tof_camera_->Close();
00119 }
00120 }
00121
00123 bool init()
00124 {
00125 if (loadParameters() == false)
00126 {
00127 ROS_ERROR("[all_cameras] Could not read parameters from launch file");
00128 return false;
00129 }
00130
00131 if (left_color_camera_ && (left_color_camera_->Init(config_directory_, 1) & ipa_CameraSensors::RET_FAILED))
00132 {
00133 ROS_WARN("[all_cameras] Initialization of left camera (1) failed");
00134 left_color_camera_ = AbstractColorCameraPtr();
00135 }
00136
00137 if (left_color_camera_ && (left_color_camera_->Open() & ipa_CameraSensors::RET_FAILED))
00138 {
00139 ROS_WARN("[all_cameras] Opening left color camera (1) failed");
00140 left_color_camera_ = AbstractColorCameraPtr();
00141 }
00142 if (left_color_camera_)
00143 {
00145 int camera_index = 1;
00146 ipa_CameraSensors::t_cameraProperty cameraProperty;
00147 cameraProperty.propertyID = ipa_CameraSensors::PROP_CAMERA_RESOLUTION;
00148 left_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_, left_color_camera_->GetCameraType(), camera_index, color_image_size);
00156
00157 cv::Mat d = color_sensor_toolbox->GetDistortionParameters(left_color_camera_intrinsic_type_, left_color_camera_intrinsic_id_);
00158 left_color_camera_info_msg_.header.stamp = ros::Time::now();
00159 left_color_camera_info_msg_.header.frame_id = "head_color_camera_l_link";
00160 left_color_camera_info_msg_.D[0] = d.at<double>(0, 0);
00161 left_color_camera_info_msg_.D[1] = d.at<double>(0, 1);
00162 left_color_camera_info_msg_.D[2] = d.at<double>(0, 2);
00163 left_color_camera_info_msg_.D[3] = d.at<double>(0, 3);
00164 left_color_camera_info_msg_.D[4] = 0;
00165
00166 cv::Mat k = color_sensor_toolbox->GetIntrinsicMatrix(left_color_camera_intrinsic_type_, left_color_camera_intrinsic_id_);
00167 left_color_camera_info_msg_.K[0] = k.at<double>(0, 0);
00168 left_color_camera_info_msg_.K[1] = k.at<double>(0, 1);
00169 left_color_camera_info_msg_.K[2] = k.at<double>(0, 2);
00170 left_color_camera_info_msg_.K[3] = k.at<double>(1, 0);
00171 left_color_camera_info_msg_.K[4] = k.at<double>(1, 1);
00172 left_color_camera_info_msg_.K[5] = k.at<double>(1, 2);
00173 left_color_camera_info_msg_.K[6] = k.at<double>(2, 0);
00174 left_color_camera_info_msg_.K[7] = k.at<double>(2, 1);
00175 left_color_camera_info_msg_.K[8] = k.at<double>(2, 2);
00176
00177 left_color_camera_info_msg_.width = color_sensor_width;
00178 left_color_camera_info_msg_.height = color_sensor_height;
00179 }
00180
00181 if (right_color_camera_ && (right_color_camera_->Init(config_directory_, 0) & ipa_CameraSensors::RET_FAILED))
00182 {
00183 ROS_WARN("[all_cameras] Initialization of right camera (0) failed");
00184 right_color_camera_ = AbstractColorCameraPtr();
00185 }
00186
00187 if (right_color_camera_ && (right_color_camera_->Open() & ipa_CameraSensors::RET_FAILED))
00188 {
00189 ROS_WARN("[all_cameras] Opening right color camera (0) failed");
00190 right_color_camera_ = AbstractColorCameraPtr();
00191 }
00192 if (right_color_camera_)
00193 {
00194 int camera_index = 0;
00196 ipa_CameraSensors::t_cameraProperty cameraProperty;
00197 cameraProperty.propertyID = ipa_CameraSensors::PROP_CAMERA_RESOLUTION;
00198 right_color_camera_->GetProperty(&cameraProperty);
00199 int color_sensor_width = cameraProperty.cameraResolution.xResolution;
00200 int color_sensor_height = cameraProperty.cameraResolution.yResolution;
00201 cv::Size color_image_size(color_sensor_width, color_sensor_height);
00202
00204 ipa_CameraSensors::CameraSensorToolboxPtr color_sensor_toolbox = ipa_CameraSensors::CreateCameraSensorToolbox();
00205 color_sensor_toolbox->Init(config_directory_, left_color_camera_->GetCameraType(), camera_index, color_image_size);
00206
00207 cv::Mat d = color_sensor_toolbox->GetDistortionParameters(right_color_camera_intrinsic_type_, right_color_camera_intrinsic_id_);
00208 right_color_camera_info_msg_.header.stamp = ros::Time::now();
00209 right_color_camera_info_msg_.header.frame_id = "head_color_camera_r_link";
00210 right_color_camera_info_msg_.D[0] = d.at<double>(0, 0);
00211 right_color_camera_info_msg_.D[1] = d.at<double>(0, 1);
00212 right_color_camera_info_msg_.D[2] = d.at<double>(0, 2);
00213 right_color_camera_info_msg_.D[3] = d.at<double>(0, 3);
00214 right_color_camera_info_msg_.D[4] = 0;
00215
00216 cv::Mat k = color_sensor_toolbox->GetIntrinsicMatrix(right_color_camera_intrinsic_type_, right_color_camera_intrinsic_id_);
00217 right_color_camera_info_msg_.K[0] = k.at<double>(0, 0);
00218 right_color_camera_info_msg_.K[1] = k.at<double>(0, 1);
00219 right_color_camera_info_msg_.K[2] = k.at<double>(0, 2);
00220 right_color_camera_info_msg_.K[3] = k.at<double>(1, 0);
00221 right_color_camera_info_msg_.K[4] = k.at<double>(1, 1);
00222 right_color_camera_info_msg_.K[5] = k.at<double>(1, 2);
00223 right_color_camera_info_msg_.K[6] = k.at<double>(2, 0);
00224 right_color_camera_info_msg_.K[7] = k.at<double>(2, 1);
00225 right_color_camera_info_msg_.K[8] = k.at<double>(2, 2);
00226
00227 right_color_camera_info_msg_.width = color_sensor_width;
00228 right_color_camera_info_msg_.height = color_sensor_height;
00229 }
00230
00231 if (tof_camera_ && (tof_camera_->Init(config_directory_) & ipa_CameraSensors::RET_FAILED))
00232 {
00233 ROS_WARN("[all_cameras] Initialization of tof camera (0) failed");
00234 tof_camera_ = AbstractRangeImagingSensorPtr();
00235 }
00236
00237 if (tof_camera_ && (tof_camera_->Open() & ipa_CameraSensors::RET_FAILED))
00238 {
00239 ROS_WARN("[all_cameras] Opening tof camera (0) failed");
00240 tof_camera_ = AbstractRangeImagingSensorPtr();
00241 }
00242 if (tof_camera_)
00243 {
00244 int camera_index = 0;
00246 ipa_CameraSensors::t_cameraProperty cameraProperty;
00247 cameraProperty.propertyID = ipa_CameraSensors::PROP_CAMERA_RESOLUTION;
00248 tof_camera_->GetProperty(&cameraProperty);
00249 int range_sensor_width = cameraProperty.cameraResolution.xResolution;
00250 int range_sensor_height = cameraProperty.cameraResolution.yResolution;
00251 cv::Size rangeImageSize(range_sensor_width, range_sensor_height);
00252
00254 ipa_CameraSensors::CameraSensorToolboxPtr tof_sensor_toolbox = ipa_CameraSensors::CreateCameraSensorToolbox();
00255 tof_sensor_toolbox->Init(config_directory_, tof_camera_->GetCameraType(), camera_index, rangeImageSize);
00256
00257 cv::Mat intrinsic_mat = tof_sensor_toolbox->GetIntrinsicMatrix(tof_camera_intrinsic_type_, tof_camera_intrinsic_id_);
00258 cv::Mat distortion_map_X = tof_sensor_toolbox->GetDistortionMapX(tof_camera_intrinsic_type_, tof_camera_intrinsic_id_);
00259 cv::Mat distortion_map_Y = tof_sensor_toolbox->GetDistortionMapY(tof_camera_intrinsic_type_, tof_camera_intrinsic_id_);
00260 tof_camera_->SetIntrinsics(intrinsic_mat, distortion_map_X, distortion_map_Y);
00261
00262 cv::Mat d = tof_sensor_toolbox->GetDistortionParameters(tof_camera_intrinsic_type_, tof_camera_intrinsic_id_);
00263 tof_camera_info_msg_.header.stamp = ros::Time::now();
00264 tof_camera_info_msg_.header.frame_id = "head_tof_link";
00265 tof_camera_info_msg_.D[0] = d.at<double>(0, 0);
00266 tof_camera_info_msg_.D[1] = d.at<double>(0, 1);
00267 tof_camera_info_msg_.D[2] = d.at<double>(0, 2);
00268 tof_camera_info_msg_.D[3] = d.at<double>(0, 3);
00269 tof_camera_info_msg_.D[4] = 0;
00270
00271 cv::Mat k = tof_sensor_toolbox->GetIntrinsicMatrix(tof_camera_intrinsic_type_, tof_camera_intrinsic_id_);
00272 tof_camera_info_msg_.K[0] = k.at<double>(0, 0);
00273 tof_camera_info_msg_.K[1] = k.at<double>(0, 1);
00274 tof_camera_info_msg_.K[2] = k.at<double>(0, 2);
00275 tof_camera_info_msg_.K[3] = k.at<double>(1, 0);
00276 tof_camera_info_msg_.K[4] = k.at<double>(1, 1);
00277 tof_camera_info_msg_.K[5] = k.at<double>(1, 2);
00278 tof_camera_info_msg_.K[6] = k.at<double>(2, 0);
00279 tof_camera_info_msg_.K[7] = k.at<double>(2, 1);
00280 tof_camera_info_msg_.K[8] = k.at<double>(2, 2);
00281
00282 tof_camera_info_msg_.width = range_sensor_width;
00283 tof_camera_info_msg_.height = range_sensor_height;
00284 }
00285
00287 if (left_color_camera_)
00288 {
00289
00290 left_color_image_publisher_ = image_transport_.advertiseCamera(left_color_camera_ns_ + "/left/image_color", 1);
00291 left_color_camera_info_service_ = node_handle_.advertiseService(left_color_camera_ns_ + "/left/set_camera_info", &CobAllCamerasNode::setCameraInfo, this);
00292 }
00293 if (right_color_camera_)
00294 {
00295
00296 right_color_image_publisher_ = image_transport_.advertiseCamera(right_color_camera_ns_ + "/right/image_color", 1);
00297 right_color_camera_info_service_ = node_handle_.advertiseService(right_color_camera_ns_ + "/right/set_camera_info", &CobAllCamerasNode::setCameraInfo, this);
00298 }
00299 if (tof_camera_)
00300 {
00301 grey_tof_image_publisher_ = image_transport_.advertiseCamera(tof_camera_ns_ + "/image_grey", 1);
00302 xyz_tof_image_publisher_ = image_transport_.advertiseCamera(tof_camera_ns_ + "/image_xyz", 1);
00303 tof_camera_info_service_ = node_handle_.advertiseService(tof_camera_ns_ + "/set_camera_info", &CobAllCamerasNode::setCameraInfo, this);
00304 }
00305
00306 return true;
00307 }
00308
00313 bool setCameraInfo(sensor_msgs::SetCameraInfo::Request& req,
00314 sensor_msgs::SetCameraInfo::Response& rsp)
00315 {
00317
00318
00319 rsp.success = false;
00320 rsp.status_message = "[all_cameras] Setting camera parameters through ROS not implemented";
00321
00322 return true;
00323 }
00324
00326 void spin()
00327 {
00328
00329 ros::Rate rate(30);
00330 while(node_handle_.ok())
00331 {
00332
00333 sensor_msgs::Image right_color_image_msg;
00334 sensor_msgs::Image left_color_image_msg;
00335 sensor_msgs::Image xyz_tof_image_msg;
00336 sensor_msgs::Image grey_tof_image_msg;
00337
00338
00339 sensor_msgs::CameraInfo right_color_image_info;
00340 sensor_msgs::CameraInfo left_color_image_info;
00341 sensor_msgs::CameraInfo tof_image_info;
00342
00343 ros::Time now = ros::Time::now();
00344
00345
00346 if (right_color_camera_)
00347 {
00348
00350 if (right_color_camera_->GetColorImage(&right_color_image_8U3_, false) & ipa_Utils::RET_FAILED)
00351 {
00352 ROS_ERROR("[all_cameras] Right color image acquisition failed");
00353 break;
00354 }
00355
00356 try
00357 {
00358 IplImage img = right_color_image_8U3_;
00359 right_color_image_msg = *(sensor_msgs::CvBridge::cvToImgMsg(&img, "bgr8"));
00360 }
00361 catch (sensor_msgs::CvBridgeException error)
00362 {
00363 ROS_ERROR("[all_cameras] Could not convert right IplImage to ROS message");
00364 break;
00365 }
00366 right_color_image_msg.header.stamp = now;
00367 right_color_image_msg.encoding = "bgr8";
00368 right_color_image_msg.header.frame_id = "head_color_camera_r_link";
00369
00370 right_color_image_info = right_color_camera_info_msg_;
00371 right_color_image_info.width = right_color_image_8U3_.cols;
00372 right_color_image_info.height = right_color_image_8U3_.rows;
00373 right_color_image_info.header.stamp = now;
00374 right_color_image_info.header.frame_id = "head_color_camera_r_link";
00375
00376 right_color_image_publisher_.publish(right_color_image_msg, right_color_image_info);
00377 }
00378
00379
00380 if (left_color_camera_)
00381 {
00382
00383
00385 if (left_color_camera_->GetColorImage(&left_color_image_8U3_, false) & ipa_Utils::RET_FAILED)
00386 {
00387 ROS_ERROR("[all_cameras] Left color image acquisition failed");
00388 break;
00389 }
00390
00391 try
00392 {
00393 IplImage img = left_color_image_8U3_;
00394 left_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 left IplImage to ROS message");
00399 break;
00400 }
00401 left_color_image_msg.header.stamp = now;
00402 left_color_image_msg.encoding = "bgr8";
00403 left_color_image_msg.header.frame_id = "head_color_camera_l_link";
00404
00405 left_color_image_info = left_color_camera_info_msg_;
00406 left_color_image_info.width = left_color_image_8U3_.cols;
00407 left_color_image_info.height = left_color_image_8U3_.rows;
00408 left_color_image_info.header.stamp = now;
00409 left_color_image_info.header.frame_id = "head_color_camera_l_link";
00410
00411 left_color_image_publisher_.publish(left_color_image_msg, left_color_image_info);
00412 }
00413
00414
00415 if (tof_camera_)
00416 {
00417
00418 if(tof_camera_->AcquireImages(0, &grey_tof_image_32F1_, &xyz_tof_image_32F3_, false, false, ipa_CameraSensors::INTENSITY_32F1) & ipa_Utils::RET_FAILED)
00419 {
00420 ROS_ERROR("[all_cameras] Tof image acquisition failed");
00421 tof_camera_->Close();
00422 tof_camera_ = AbstractRangeImagingSensorPtr();
00423 break;
00424 }
00425
00426 try
00427 {
00428 IplImage grey_img = grey_tof_image_32F1_;
00429 IplImage xyz_img = xyz_tof_image_32F3_;
00430 xyz_tof_image_msg = *(sensor_msgs::CvBridge::cvToImgMsg(&xyz_img, "passthrough"));
00431 grey_tof_image_msg = *(sensor_msgs::CvBridge::cvToImgMsg(&grey_img, "passthrough"));
00432 }
00433 catch (sensor_msgs::CvBridgeException error)
00434 {
00435 ROS_ERROR("[all_cameras] Could not convert tof IplImage to ROS message");
00436 break;
00437 }
00438
00439 xyz_tof_image_msg.header.stamp = now;
00440 xyz_tof_image_msg.header.frame_id = "head_tof_link";
00441 grey_tof_image_msg.header.stamp = now;
00442 grey_tof_image_msg.header.frame_id = "head_tof_link";
00443
00444 tof_image_info = tof_camera_info_msg_;
00445 tof_image_info.width = grey_tof_image_32F1_.cols;
00446 tof_image_info.height = grey_tof_image_32F1_.rows;
00447 tof_image_info.header.stamp = now;
00448 tof_image_info.header.frame_id = "head_tof_link";
00449
00450 grey_tof_image_publisher_.publish(grey_tof_image_msg, tof_image_info);
00451 xyz_tof_image_publisher_.publish(xyz_tof_image_msg, tof_image_info);
00452 }
00453
00454 ros::spinOnce();
00455 rate.sleep();
00456
00457 }
00458 }
00459
00460 bool loadParameters()
00461 {
00462 std::string tmp_string = "NULL";
00463
00465 if (node_handle_.getParam("all_cameras/configuration_files", config_directory_) == false)
00466 {
00467 ROS_ERROR("Path to xml configuration file not specified");
00468 return false;
00469 }
00470
00471 ROS_INFO("Configuration directory: %s", config_directory_.c_str());
00472
00473
00474 if (node_handle_.getParam("all_cameras/color_camera_type", tmp_string) == false)
00475 {
00476 ROS_ERROR("[all_cameras] Color camera type not specified");
00477 return false;
00478 }
00479 if (tmp_string == "CAM_AVTPIKE")
00480 {
00481 right_color_camera_ = ipa_CameraSensors::CreateColorCamera_AVTPikeCam();
00482 left_color_camera_ = ipa_CameraSensors::CreateColorCamera_AVTPikeCam();
00483
00484 left_color_camera_ns_ = "pike_145C";
00485 right_color_camera_ns_ = "pike_145C";
00486 }
00487 else if (tmp_string == "CAM_VIRTUAL")
00488 {
00489 right_color_camera_ = ipa_CameraSensors::CreateColorCamera_VirtualCam();
00490 left_color_camera_ = ipa_CameraSensors::CreateColorCamera_VirtualCam();
00491 left_color_camera_ns_ = "pike_145C";
00492 right_color_camera_ns_ = "pike_145C";
00493 }
00494 else if (tmp_string == "CAM_PROSILICA") ROS_ERROR("[all_cameras] Color camera type not CAM_PROSILICA not yet implemented");
00495 else
00496 {
00497 std::string str = "[all_cameras] Camera type '" + tmp_string + "' unknown, try 'CAM_AVTPIKE' or 'CAM_PROSILICA'";
00498 ROS_ERROR("%s", str.c_str());
00499 return false;
00500 }
00501
00502 ROS_INFO("Color camera type: %s", tmp_string.c_str());
00503
00504
00505 if (node_handle_.getParam("all_cameras/tof_camera_type", tmp_string) == false)
00506 {
00507 ROS_ERROR("[all_cameras] tof camera type not specified");
00508 return false;
00509 }
00510 if (tmp_string == "CAM_SWISSRANGER")
00511 {
00512 tof_camera_ = ipa_CameraSensors::CreateRangeImagingSensor_Swissranger();
00513 tof_camera_ns_ = "sr4000";
00514 }
00515 else if (tmp_string == "CAM_VIRTUAL")
00516 {
00517 tof_camera_ = ipa_CameraSensors::CreateRangeImagingSensor_VirtualCam();
00518 tof_camera_ns_ = "sr4000";
00519 }
00520 else
00521 {
00522 std::string str = "[all_cameras] Camera type '" + tmp_string + "' unknown, try 'CAM_SWISSRANGER'";
00523 ROS_ERROR("%s", str.c_str());
00524 }
00525
00526 ROS_INFO("Tof camera type: %s", tmp_string.c_str());
00527
00528
00529
00530 if (node_handle_.getParam("all_cameras/left_color_camera_intrinsic_type", tmp_string) == false)
00531 {
00532 ROS_ERROR("[all_cameras] Intrinsic camera type for left color camera not specified");
00533 return false;
00534 }
00535 if (tmp_string == "CAM_AVTPIKE")
00536 {
00537 left_color_camera_intrinsic_type_ = ipa_CameraSensors::CAM_AVTPIKE;
00538 }
00539 else if (tmp_string == "CAM_PROSILICA")
00540 {
00541 left_color_camera_intrinsic_type_ = ipa_CameraSensors::CAM_PROSILICA;
00542 }
00543 else if (tmp_string == "CAM_SWISSRANGER")
00544 {
00545 left_color_camera_intrinsic_type_ = ipa_CameraSensors::CAM_SWISSRANGER;
00546 }
00547 else if (tmp_string == "CAM_VIRTUALRANGE")
00548 {
00549 left_color_camera_intrinsic_type_ = ipa_CameraSensors::CAM_VIRTUALRANGE;
00550 }
00551 else if (tmp_string == "CAM_VIRTUALCOLOR")
00552 {
00553 left_color_camera_intrinsic_type_ = ipa_CameraSensors::CAM_VIRTUALCOLOR;
00554 }
00555 else
00556 {
00557 std::string str = "[all_cameras] Camera type '" + tmp_string + "' for intrinsics of left camera unknown, try 'CAM_AVTPIKE','CAM_PROSILICA' or 'CAM_SWISSRANGER'";
00558 ROS_ERROR("%s", str.c_str());
00559 return false;
00560 }
00561 if (node_handle_.getParam("all_cameras/left_color_camera_intrinsic_id", left_color_camera_intrinsic_id_) == false)
00562 {
00563 ROS_ERROR("[all_cameras] Intrinsic camera id for left color camera not specified");
00564 return false;
00565 }
00566
00567
00568 ROS_INFO("Intrinsic for left color camera: %s_%d", tmp_string.c_str(), left_color_camera_intrinsic_id_);
00569
00570
00571
00572 if (node_handle_.getParam("all_cameras/right_color_camera_intrinsic_type", tmp_string) == false)
00573 {
00574 ROS_ERROR("[all_cameras] Intrinsic camera type for right color camera not specified");
00575 return false;
00576 }
00577 if (tmp_string == "CAM_AVTPIKE")
00578 {
00579 right_color_camera_intrinsic_type_ = ipa_CameraSensors::CAM_AVTPIKE;
00580 }
00581 else if (tmp_string == "CAM_PROSILICA")
00582 {
00583 right_color_camera_intrinsic_type_ = ipa_CameraSensors::CAM_PROSILICA;
00584 }
00585 else if (tmp_string == "CAM_SWISSRANGER")
00586 {
00587 right_color_camera_intrinsic_type_ = ipa_CameraSensors::CAM_SWISSRANGER;
00588 }
00589 else if (tmp_string == "CAM_VIRTUALRANGE")
00590 {
00591 right_color_camera_intrinsic_type_ = ipa_CameraSensors::CAM_VIRTUALRANGE;
00592 }
00593 else if (tmp_string == "CAM_VIRTUALCOLOR")
00594 {
00595 right_color_camera_intrinsic_type_ = ipa_CameraSensors::CAM_VIRTUALCOLOR;
00596 }
00597 else
00598 {
00599 std::string str = "[all_cameras] Camera type '" + tmp_string + "' for intrinsics of right camera unknown, try 'CAM_AVTPIKE','CAM_PROSILICA' or 'CAM_SWISSRANGER'";
00600 ROS_ERROR("%s", str.c_str());
00601 return false;
00602 }
00603 if (node_handle_.getParam("all_cameras/right_color_camera_intrinsic_id", right_color_camera_intrinsic_id_) == false)
00604 {
00605 ROS_ERROR("[all_cameras] Intrinsic camera id for right color camera not specified");
00606 return false;
00607 }
00608
00609 ROS_INFO("Intrinsic for right color camera: %s_%d", tmp_string.c_str(), right_color_camera_intrinsic_id_);
00610
00611
00612
00613 if (node_handle_.getParam("all_cameras/tof_camera_intrinsic_type", tmp_string) == false)
00614 {
00615 ROS_ERROR("[all_cameras] Intrinsic camera type for tof camera not specified");
00616 return false;
00617 }
00618 if (tmp_string == "CAM_AVTPIKE")
00619 {
00620 tof_camera_intrinsic_type_ = ipa_CameraSensors::CAM_AVTPIKE;
00621 }
00622 else if (tmp_string == "CAM_PROSILICA")
00623 {
00624 tof_camera_intrinsic_type_ = ipa_CameraSensors::CAM_PROSILICA;
00625 }
00626 else if (tmp_string == "CAM_SWISSRANGER")
00627 {
00628 tof_camera_intrinsic_type_ = ipa_CameraSensors::CAM_SWISSRANGER;
00629 }
00630 else if (tmp_string == "CAM_VIRTUALRANGE")
00631 {
00632 tof_camera_intrinsic_type_ = ipa_CameraSensors::CAM_VIRTUALRANGE;
00633 }
00634 else if (tmp_string == "CAM_VIRTUALCOLOR")
00635 {
00636 tof_camera_intrinsic_type_ = ipa_CameraSensors::CAM_VIRTUALCOLOR;
00637 }
00638 else
00639 {
00640 std::string str = "[all_cameras] Camera type '" + tmp_string + "' for intrinsics unknown, try 'CAM_AVTPIKE','CAM_PROSILICA' or 'CAM_SWISSRANGER'";
00641 ROS_ERROR("%s", str.c_str());
00642 return false;
00643 }
00644 if (node_handle_.getParam("all_cameras/tof_camera_intrinsic_id", tof_camera_intrinsic_id_) == false)
00645 {
00646 ROS_ERROR("[all_cameras] Intrinsic camera id for tof camera not specified");
00647 return false;
00648 }
00649
00650 ROS_INFO("Intrinsic for tof camera: %s_%d", tmp_string.c_str(), tof_camera_intrinsic_id_);
00651
00652 return true;
00653 }
00654 };
00655
00656
00657
00658 int main(int argc, char** argv)
00659 {
00661 ros::init(argc, argv, "color_camera");
00662
00664 ros::NodeHandle nh;
00665
00667 CobAllCamerasNode camera_node(nh);
00668
00670 if (camera_node.init() == false)
00671 {
00672 ROS_ERROR("[all_cameras] Node initialization FAILED. Terminating");
00673 return 0;
00674 }
00675 else
00676 {
00677 ROS_INFO("[all_cameras] Node initialization OK. Enter spinning");
00678 }
00679
00680 camera_node.spin();
00681
00682 return 0;
00683 }