27 #include <cv_bridge/CvBridge.h> 30 #include <sensor_msgs/Image.h> 31 #include <sensor_msgs/CameraInfo.h> 33 #include <sensor_msgs/SetCameraInfo.h> 65 : node_handle_(node_handle),
67 color_image_8U3_(cv::Mat())
75 color_camera_->Close();
81 if (loadParameters() ==
false)
83 ROS_ERROR(
"[color_camera] Could not read all parameters from launch file");
90 ss <<
"initialization of color camera ";
93 ROS_ERROR(
"[color_camera] %s", ss.str().c_str());
94 color_camera_->Close();
101 std::stringstream ss;
102 ss <<
"Could not open color camera ";
104 ROS_ERROR(
"[color_camera] %s", ss.str().c_str());
105 color_camera_->Close();
113 color_camera_->GetProperty(&cameraProperty);
116 cv::Size color_image_size(color_sensor_width, color_sensor_height);
120 color_sensor_toolbox->Init(config_directory_, color_camera_->GetCameraType(), camera_index_, color_image_size);
122 cv::Mat
d = color_sensor_toolbox->GetDistortionParameters(color_camera_intrinsic_type_, color_camera_intrinsic_id_);
124 if (camera_index_ == 0)
125 camera_info_msg_.header.frame_id =
"head_color_camera_r_link";
127 camera_info_msg_.header.frame_id =
"head_color_camera_l_link";
128 camera_info_msg_.D[0] = d.at<
double>(0, 0);
129 camera_info_msg_.D[1] = d.at<
double>(0, 1);
130 camera_info_msg_.D[2] = d.at<
double>(0, 2);
131 camera_info_msg_.D[3] = d.at<
double>(0, 3);
132 camera_info_msg_.D[4] = 0;
134 cv::Mat k = color_sensor_toolbox->GetIntrinsicMatrix(color_camera_intrinsic_type_, color_camera_intrinsic_id_);
135 camera_info_msg_.K[0] = k.at<
double>(0, 0);
136 camera_info_msg_.K[1] = k.at<
double>(0, 1);
137 camera_info_msg_.K[2] = k.at<
double>(0, 2);
138 camera_info_msg_.K[3] = k.at<
double>(1, 0);
139 camera_info_msg_.K[4] = k.at<
double>(1, 1);
140 camera_info_msg_.K[5] = k.at<
double>(1, 2);
141 camera_info_msg_.K[6] = k.at<
double>(2, 0);
142 camera_info_msg_.K[7] = k.at<
double>(2, 1);
143 camera_info_msg_.K[8] = k.at<
double>(2, 2);
145 camera_info_msg_.width = color_sensor_width;
146 camera_info_msg_.height = color_sensor_height;
162 sensor_msgs::SetCameraInfo::Response& rsp)
165 camera_info_msg_ = req.camera_info;
168 rsp.status_message =
"[color_camera] Setting camera parameters through ROS not implemented";
175 polled_camera::GetPolledImage::Response& res,
176 sensor_msgs::Image& image_msg, sensor_msgs::CameraInfo& info)
181 ROS_ERROR(
"[color_camera] Color image acquisition failed");
188 IplImage img = color_image_8U3_;
189 image_msg = *(sensor_msgs::CvBridge::cvToImgMsg(&img,
"bgr8"));
191 catch (sensor_msgs::CvBridgeException error)
193 ROS_ERROR(
"[color_camera] Could not convert IplImage to ROS message");
198 image_msg.header.stamp = now;
199 if (camera_index_ == 0)
200 image_msg.header.frame_id =
"head_color_camera_r_link";
202 image_msg.header.frame_id =
"head_color_camera_l_link";
203 image_msg.encoding =
"bgr8";
205 info = camera_info_msg_;
206 info.width = color_image_8U3_.cols;
207 info.height = color_image_8U3_.rows;
208 info.header.stamp = now;
209 if (camera_index_ == 0)
210 info.header.frame_id =
"head_color_camera_r_link";
212 info.header.frame_id =
"head_color_camera_l_link";
220 std::string tmp_string =
"NULL";
222 if (node_handle_.
getParam(
"configuration_files", config_directory_) ==
false)
224 ROS_ERROR(
"[color_camera] Path to xml configuration for color camera not specified");
228 ROS_INFO(
"Configuration directory: %s", config_directory_.c_str());
231 if (node_handle_.
getParam(
"camera_index", camera_index_) ==
false)
233 ROS_ERROR(
"[color_camera] Color camera index (0 or 1) not specified");
238 if (node_handle_.
getParam(
"color_camera_type", tmp_string) ==
false)
240 ROS_ERROR(
"[color_camera] Color camera type not specified");
245 else if (tmp_string ==
"CAM_PROSILICA")
ROS_ERROR(
"[color_camera] Color camera type not CAM_PROSILICA not yet implemented");
248 std::string str =
"[color_camera] Camera type '" + tmp_string +
"' unknown, try 'CAM_AVTPIKE' or 'CAM_PROSILICA'";
253 ROS_INFO(
"Camera type: %s_%d", tmp_string.c_str(), camera_index_);
257 if (node_handle_.
getParam(
"color_camera_intrinsic_type", tmp_string) ==
false)
259 ROS_ERROR(
"[color_camera] Intrinsic camera type for color camera not specified");
262 if (tmp_string ==
"CAM_AVTPIKE")
266 else if (tmp_string ==
"CAM_PROSILICA")
270 else if (tmp_string ==
"CAM_VIRTUALRANGE")
276 std::string str =
"[color_camera] Camera type '" + tmp_string +
"' for intrinsics unknown, try 'CAM_AVTPIKE','CAM_PROSILICA' or 'CAM_SWISSRANGER'";
280 if (node_handle_.
getParam(
"color_camera_intrinsic_id", color_camera_intrinsic_id_) ==
false)
282 ROS_ERROR(
"[color_camera] Intrinsic camera id for color camera not specified");
286 ROS_INFO(
"Intrinsic for color camera: %s_%d", tmp_string.c_str(), color_camera_intrinsic_id_);
294 int main(
int argc,
char** argv)
306 if (!camera_node.
init())
return 0;
AbstractColorCameraPtr color_camera_
Color camera instance.
bool setCameraInfo(sensor_msgs::SetCameraInfo::Request &req, sensor_msgs::SetCameraInfo::Response &rsp)
int main(int argc, char **argv)
ipa_CameraSensors::t_cameraType color_camera_intrinsic_type_
Instrinsic matrix type of left color camera.
t_cameraResolution cameraResolution
boost::shared_ptr< AbstractColorCamera > AbstractColorCameraPtr
ros::NodeHandle node_handle_
std::string config_directory_
Directory of related IPA configuration file.
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
__DLL_LIBCAMERASENSORS__ CameraSensorToolboxPtr CreateCameraSensorToolbox()
ros::ServiceServer camera_info_service_
ROSCPP_DECL void spin(Spinner &spinner)
polled_camera::PublicationServer image_poll_server_
PublicationServer advertise(ros::NodeHandle &nh, const std::string &service, const PublicationServer::DriverCallback &cb, const ros::VoidPtr &tracked_object=ros::VoidPtr())
__DLL_LIBCAMERASENSORS__ AbstractColorCameraPtr CreateColorCamera_VirtualCam()
int color_camera_intrinsic_id_
Instrinsic matrix id of left color camera.
void pollCallback(polled_camera::GetPolledImage::Request &req, polled_camera::GetPolledImage::Response &res, sensor_msgs::Image &image_msg, sensor_msgs::CameraInfo &info)
Callback function for image requests on topic 'request_image'.
CobColorCameraNode(const ros::NodeHandle &node_handle)
t_cameraPropertyID propertyID
sensor_msgs::CameraInfo camera_info_msg_
ROS camera information message (e.g. holding intrinsic parameters)
bool getParam(const std::string &key, std::string &s) const
bool init()
Opens the camera sensor.
int camera_index_
Camera index of the color camera for IPA configuration file.
__DLL_LIBCAMERASENSORS__ AbstractColorCameraPtr CreateColorCamera_AVTPikeCam()