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