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
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056
00057
00058
00059
00060 #include <ros/ros.h>
00061 #include <polled_camera/publication_server.h>
00062 #include <cv_bridge/CvBridge.h>
00063
00064
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
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
00291
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
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 }