void Camera1394Driver::checkCameraInfo(const sensor_msgs::ImagePtr &image, sensor_msgs::CameraInfoPtr &ci, bool &calibration_matches, std::string camera_name) { if (!dev_->checkCameraInfo(*image, *ci)) { // image size does not match: publish a matching uncalibrated // CameraInfo instead if (calibration_matches) { // warn user once calibration_matches = false; ROS_WARN_STREAM("[" << camera_name << "] calibration does not match video mode " << "(publishing uncalibrated data)"); } ci.reset(new sensor_msgs::CameraInfo()); ci->height = image->height; ci->width = image->width; } else if (!calibration_matches) { // calibration OK now calibration_matches = true; ROS_WARN_STREAM("[" << camera_name << "] calibration matches video mode now"); } }
bool processFrame (FlyCapture2::Image * frame, sensor_msgs::Image & img, sensor_msgs::CameraInfoPtr & cam_info) { // cam_->saveImageToFile(frame); // Just a test: comment this out if not needed if (!frameToImage (frame, img)) return false; /// @todo Use time from frame? img.header.stamp = cam_info->header.stamp = ros::Time::now (); // Throw out any CamInfo that's not calibrated to this camera mode if (cam_info->K[0] != 0.0 && (img.width != cam_info->width || img.height != cam_info->height)) { cam_info.reset(new sensor_msgs::CameraInfo()); } // If we don't have a calibration, set the image dimensions if (cam_info->K[0] == 0.0) { cam_info->width = img.width = width_; cam_info->height = img.height = height_; } // TF frame img.header.frame_id = cam_info->header.frame_id = frame_id_; //frame->GetDimensions(&cam_info.height, &cam_info.width); //cam_info.width = frame->GetCols(); // // cam_info.roi.x_offset = frame->RegionX; // cam_info.roi.y_offset = frame->RegionY; // cam_info.roi.height = frame->Height; // cam_info.roi.width = frame->Width; count_++; //ROS_INFO("count = %d", count_); return true; }
int main(int argc, char* argv[]) { ros::init(argc, argv, "realsense_camera_node"); ros::NodeHandle n; image_transport::ImageTransport image_transport(n); ros::NodeHandle private_node_handle_("~"); private_node_handle_.param("realsense_camera_type", realsense_camera_type, std::string("Intel(R) RealSense(TM) 3D Camer")); private_node_handle_.param("rgb_frame_id", rgb_frame_id, std::string("_rgb_optical_frame")); private_node_handle_.param("depth_frame_id", depth_frame_id, std::string("_depth_optical_frame")); private_node_handle_.param("rgb_frame_w", rgb_frame_w, 1280); private_node_handle_.param("rgb_frame_h", rgb_frame_h, 720); double depth_unit_d, depth_scale_d; private_node_handle_.param("depth_unit", depth_unit_d, 31.25); private_node_handle_.param("depth_scale", depth_scale_d, 0.001); depth_unit = depth_unit_d; depth_scale = depth_scale_d; double depth_fx, depth_fy; private_node_handle_.param("depth_fx", depth_fx, 463.888885); private_node_handle_.param("depth_fy", depth_fy, 463.888885); depth_fxinv = 1.0f / depth_fx; depth_fyinv = 1.0f / depth_fy; double depth_cx_d, depth_cy_d; private_node_handle_.param("depth_cx", depth_cx_d, 320.0); private_node_handle_.param("depth_cy", depth_cy_d, 240.0); depth_cx = depth_cx_d; depth_cy = depth_cy_d; private_node_handle_.param("depth_uv_enable_min", depth_uv_enable_min, 0); private_node_handle_.param("depth_uv_enable_max", depth_uv_enable_max, 2047); private_node_handle_.param("topic_depth_points_id", topic_depth_points_id, std::string("/depth/points")); private_node_handle_.param("topic_depth_registered_points_id", topic_depth_registered_points_id, std::string("/depth_registered/points")); private_node_handle_.param("topic_image_rgb_raw_id", topic_image_rgb_raw_id, std::string("/rgb/image_raw")); private_node_handle_.param("topic_image_depth_raw_id", topic_image_depth_raw_id, std::string("/depth/image_raw")); private_node_handle_.param("topic_image_infrared_raw_id", topic_image_infrared_raw_id, std::string("/ir/image_raw")); private_node_handle_.param("debug_depth_unit", debug_depth_unit, false); std::string rgb_info_url; private_node_handle_.param("rgb_camera_info_url", rgb_info_url, std::string()); std::string ir_camera_info_url; private_node_handle_.param("ir_camera_info_url", ir_camera_info_url, std::string()); printf("\n\n===================\n" "realsense_camera_type = %s\n" "rgb_frame_id = %s\n" "depth_frame_id = %s\n" "depth_unit = %f\n" "depth_scale = %f\n" "depth_fxinv = %f\n" "depth_fyinv = %f\n" "depth_cx = %f\n" "depth_cy = %f\n" "depth_uv_enable_min = %d\n" "depth_uv_enable_max = %d\n" "topic_depth_points_id = %s\n" "topic_depth_registered_points_id = %s\n" "topic_image_rgb_raw_id = %s\n" "topic_image_depth_raw_id = %s\n" "topic_image_infrared_raw_id = %s\n" "debug_depth_unit = %d\n" "rgb_camera_info_url = %s\n" "ir_camera_info_url = %s\n" "=======================\n\n", realsense_camera_type.c_str(), rgb_frame_id.c_str(), depth_frame_id.c_str(), depth_unit, depth_scale, depth_fxinv, depth_fyinv, depth_cx, depth_cy, depth_uv_enable_min, depth_uv_enable_max, topic_depth_points_id.c_str(), topic_depth_registered_points_id.c_str(), topic_image_rgb_raw_id.c_str(), topic_image_depth_raw_id.c_str(), topic_image_infrared_raw_id.c_str(), debug_depth_unit, rgb_info_url.c_str(), ir_camera_info_url.c_str() ); #ifdef V4L2_PIX_FMT_INZI printf("\ndepthWithIRStream - YEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEES\n"); #else printf("\ndepthWithIRStream - NOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOO\n"); printf("if you want IR stream, please visit\n" "http://solsticlipse.com/2015/03/31/intel-real-sense-3d-on-linux-macos.html\n" "https://github.com/teknotus/depthview/tree/kernelpatchfmt\n"); #endif //find realsense video device std::vector<VIDEO_DEVICE> video_lists; list_devices(realsense_camera_type, video_lists); if(video_lists.empty()) { printf("\n\n""can not find Intel(R) RealSense(TM) 3D Camera video device!!!!!!!!! - %s\n\n", realsense_camera_type.c_str()); ROS_ERROR("can not find Intel(R) RealSense(TM) 3D Camera video device!!!!!!!!! - %s", realsense_camera_type.c_str()); ros::shutdown(); return 0; } if(1) { printf("\n===========================================\n"); printf("Intel(R) RealSense(TM) 3D Camera lists\n"); for(int i=0; i<video_lists.size(); ++i) { printf("\nPCI: %s\n", video_lists[i].card_name.c_str()); printf("Serial: %s\n", video_lists[i].serial_number.c_str()); for(int j=0; j<video_lists[i].video_names.size(); ++j) { printf("\t%s\n", video_lists[i].video_names[j].c_str()); } } printf("===========================================\n\n"); } //return 0; if(video_lists[0].video_names.size() < 2) { printf("Intel(R) RealSense(TM) 3D Camera video device count error!!!!!!!!!!!\n"); ros::shutdown(); return 0; } else { useDeviceSerialNum = video_lists[0].serial_number; printf("use camera %s\n", useDeviceSerialNum.c_str()); } initDepthToRGBUVMap(); initVideoStream(); strncpy(rgb_stream.videoName, video_lists[0].video_names[0].c_str(), video_lists[0].video_names[0].length()); strncpy(depth_stream.videoName, video_lists[0].video_names[1].c_str(), video_lists[0].video_names[1].length()); printf("video rgb name is %s\n", rgb_stream.videoName); printf("video depth name is %s\n", depth_stream.videoName); if(capturer_mmap_init(&rgb_stream)) { printf("open %s error!!!!!!!!\n", rgb_stream.videoName); ros::shutdown(); return 0; } else { printf("video rgb w,h - %d, %d\n", rgb_stream.width, rgb_stream.height); } if(capturer_mmap_init(&depth_stream)) { printf("open %s error!!!!!!!!\n", depth_stream.videoName); ros::shutdown(); return 0; } else { printf("video depth w,h - %d, %d\n", depth_stream.width, depth_stream.height); } if (!rgb_info_url.empty()) { std::string camera_name_rgb = "realsense_camera_rgb_" + useDeviceSerialNum; camera_info_manager::CameraInfoManager rgb_info_manager(n, camera_name_rgb, rgb_info_url); if (rgb_info_manager.isCalibrated()) { rgb_camera_info = boost::make_shared<sensor_msgs::CameraInfo>(rgb_info_manager.getCameraInfo()); if (rgb_camera_info->width != rgb_frame_w || rgb_camera_info->height != rgb_frame_h) { ROS_WARN("RGB image resolution does not match calibration file"); rgb_camera_info.reset(new sensor_msgs::CameraInfo()); rgb_camera_info->width = rgb_frame_w; rgb_camera_info->height = rgb_frame_h; } } } if (!rgb_camera_info) { rgb_camera_info.reset(new sensor_msgs::CameraInfo()); rgb_camera_info->width = rgb_frame_w; rgb_camera_info->height = rgb_frame_h; } if (!ir_camera_info_url.empty()) { std::string camera_name_ir = "realsense_camera_ir_" + useDeviceSerialNum; camera_info_manager::CameraInfoManager ir_camera_info_manager(n, camera_name_ir, ir_camera_info_url); if (ir_camera_info_manager.isCalibrated()) { ir_camera_info = boost::make_shared<sensor_msgs::CameraInfo>(ir_camera_info_manager.getCameraInfo()); if (ir_camera_info->width != depth_stream.width || ir_camera_info->height != depth_stream.height) { ROS_WARN("IR image resolution does not match calibration file"); ir_camera_info.reset(new sensor_msgs::CameraInfo()); ir_camera_info->width = depth_stream.width; ir_camera_info->height = depth_stream.height; } } } if (!ir_camera_info) { ir_camera_info.reset(new sensor_msgs::CameraInfo()); ir_camera_info->width = depth_stream.width; ir_camera_info->height = depth_stream.height; } if(debug_depth_unit && realsense_camera_type == "Intel(R) RealSense(TM) 3D Camer") { getRealsenseUSBHandle(usbContext, usbHandle, useDeviceSerialNum); if(usbContext && usbHandle) { printf("getRealsenseUSBHandle OK!\n"); } } printf("RealSense Camera is running!\n"); #if USE_BGR24 rgb_frame_buffer = new unsigned char[rgb_stream.width * rgb_stream.height * 3]; #else rgb_frame_buffer = new unsigned char[rgb_stream.width * rgb_stream.height * 2]; #endif depth_frame_buffer = new unsigned char[depth_stream.width * depth_stream.height]; #ifdef V4L2_PIX_FMT_INZI ir_frame_buffer = new unsigned char[depth_stream.width * depth_stream.height]; #endif realsense_points_pub = n.advertise<sensor_msgs::PointCloud2> (topic_depth_points_id, 1); realsense_reg_points_pub = n.advertise<sensor_msgs::PointCloud2>(topic_depth_registered_points_id, 1); realsense_rgb_image_pub = image_transport.advertiseCamera(topic_image_rgb_raw_id, 1); realsense_depth_image_pub = image_transport.advertiseCamera(topic_image_depth_raw_id, 1); //pub_depth_info = n.advertise<sensor_msgs::CameraInfo>("depth/camera_info", 1); //pub_rgb_info = n.advertise<sensor_msgs::CameraInfo>("rgb/camera_info", 1); #ifdef V4L2_PIX_FMT_INZI realsense_infrared_image_pub = image_transport.advertiseCamera(topic_image_infrared_raw_id, 1); #endif ros::Subscriber config_sub = n.subscribe("/realsense_camera_config", 1, realsenseConfigCallback); getRGBUVService = n.advertiseService("get_rgb_uv", getRGBUV); capturer_mmap_init_v4l2_controls(); dynamic_reconfigure::Server<realsense_camera::RealsenseCameraConfig> dynamic_reconfigure_server; dynamic_reconfigure_server.setCallback(boost::bind(&dynamicReconfigCallback, _1, _2)); ros::Rate loop_rate(60); ros::Rate idle_rate(1); while(ros::ok()) { while ((getNumRGBSubscribers() + getNumDepthSubscribers()) == 0 && ros::ok()) { ros::spinOnce(); idle_rate.sleep(); } processRGBD(); #if SHOW_RGBD_FRAME cv::waitKey(10); #endif ros::spinOnce(); loop_rate.sleep(); } capturer_mmap_exit(&rgb_stream); capturer_mmap_exit(&depth_stream); delete[] rgb_frame_buffer; delete[] depth_frame_buffer; #ifdef V4L2_PIX_FMT_INZI delete[] ir_frame_buffer; #endif if(debug_depth_unit) { libusb_close(usbHandle); libusb_exit(usbContext); } printf("RealSense Camera is shutdown!\n"); return 0; }