CameraWrapper::CameraWrapper(int frames) { // init camera e = 0; ctx = rs_create_context(RS_API_VERSION, &e); check_error(); printf("There are %d connected RealSense devices.\n", rs_get_device_count(ctx, &e)); check_error(); if(rs_get_device_count(ctx, &e) == 0) { std::cout << "camera init broken" << std::cout; } else { dev = rs_get_device(ctx, 0, &e); check_error(); printf("\nUsing device 0, an %s\n", rs_get_device_name(dev, &e)); check_error(); printf(" Serial number: %s\n", rs_get_device_serial(dev, &e)); check_error(); printf(" Firmware version: %s\n", rs_get_device_firmware_version(dev, &e)); check_error(); // set controls // NOTE find good laser power, max 15 //rs_enable_stream_preset(dev, RS_STREAM_DEPTH, RS_PRESET_BEST_QUALITY, &e); rs_enable_stream(dev, RS_STREAM_DEPTH, 640, 480, RS_FORMAT_Z16, 30, &e); check_error(); rs_set_device_option(dev, RS_OPTION_F200_ACCURACY, 1, &e); check_error(); rs_set_device_option(dev, RS_OPTION_F200_FILTER_OPTION, 0, &e); check_error(); rs_enable_stream(dev, RS_STREAM_COLOR, 1920, 1080, RS_FORMAT_RGB8, 30, &e); // rs_enable_stream_preset(dev, RS_STREAM_COLOR, RS_PRESET_BEST_QUALITY, &e); check_error(); rs_enable_stream(dev, RS_STREAM_INFRARED, 640,480, RS_FORMAT_Y8, 30, &e); //rs_enable_stream_preset(dev, RS_STREAM_INFRARED, RS_PRESET_BEST_QUALITY, &e); check_error(); rs_start_device(dev, &e); check_error(); // get intrinsics and convert them to opencv mat // IR and DEPTH are the same for f200, so this is basically overhead rs_get_stream_intrinsics(dev, RS_STREAM_DEPTH, &depth_intrin, &e); check_error(); rs_get_device_extrinsics(dev, RS_STREAM_DEPTH, RS_STREAM_COLOR, &depth_to_color, &e); check_error(); rs_get_stream_intrinsics(dev, RS_STREAM_COLOR, &color_intrin, &e); check_error(); rs_get_stream_intrinsics(dev, RS_STREAM_INFRARED, &ir_intrin, &e); check_error(); depthScale = rs_get_device_depth_scale(dev, &e); check_error(); } // set number of frames to record setStackSize(frames); }
/* * Get the dynamic param values. */ void F200Nodelet::configCallback(realsense_camera::f200_paramsConfig &config, uint32_t level) { ROS_INFO_STREAM(nodelet_name_ << " - Setting dynamic camera options"); // set the depth enable BaseNodelet::setDepthEnable(config.enable_depth); // Set common options rs_set_device_option(rs_device_, RS_OPTION_COLOR_BACKLIGHT_COMPENSATION, config.color_backlight_compensation, 0); rs_set_device_option(rs_device_, RS_OPTION_COLOR_BRIGHTNESS, config.color_brightness, 0); rs_set_device_option(rs_device_, RS_OPTION_COLOR_CONTRAST, config.color_contrast, 0); rs_set_device_option(rs_device_, RS_OPTION_COLOR_GAIN, config.color_gain, 0); rs_set_device_option(rs_device_, RS_OPTION_COLOR_GAMMA, config.color_gamma, 0); rs_set_device_option(rs_device_, RS_OPTION_COLOR_HUE, config.color_hue, 0); rs_set_device_option(rs_device_, RS_OPTION_COLOR_SATURATION, config.color_saturation, 0); rs_set_device_option(rs_device_, RS_OPTION_COLOR_SHARPNESS, config.color_sharpness, 0); rs_set_device_option(rs_device_, RS_OPTION_COLOR_ENABLE_AUTO_EXPOSURE, config.color_enable_auto_exposure, 0); if (config.color_enable_auto_exposure == 0) { rs_set_device_option(rs_device_, RS_OPTION_COLOR_EXPOSURE, config.color_exposure, 0); } rs_set_device_option(rs_device_, RS_OPTION_COLOR_ENABLE_AUTO_WHITE_BALANCE, config.color_enable_auto_white_balance, 0); if (config.color_enable_auto_white_balance == 0) { rs_set_device_option(rs_device_, RS_OPTION_COLOR_WHITE_BALANCE, config.color_white_balance, 0); } // Set F200 specific options rs_set_device_option(rs_device_, RS_OPTION_F200_LASER_POWER, config.f200_laser_power, 0); rs_set_device_option(rs_device_, RS_OPTION_F200_ACCURACY, config.f200_accuracy, 0); rs_set_device_option(rs_device_, RS_OPTION_F200_MOTION_RANGE, config.f200_motion_range, 0); rs_set_device_option(rs_device_, RS_OPTION_F200_FILTER_OPTION, config.f200_filter_option, 0); rs_set_device_option(rs_device_, RS_OPTION_F200_CONFIDENCE_THRESHOLD, config.f200_confidence_threshold, 0); }