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;
}