예제 #1
0
 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;
}