static void image_move(dword key)
{
// cancel z mode
in_move_z_mode = false;
if ((key & config.imgkey[14] && !(key & config.imgkey[15]))
|| (key & config.imgkey2[14] && !(key & config.imgkey2[15]))
) {
image_left();
}
if ((key & config.imgkey[15] && !(key & config.imgkey[14])) ||
(key & config.imgkey2[15] && !(key & config.imgkey2[14]))
) {
image_right();
}
if ((key & config.imgkey[12] && !(key & config.imgkey[13])) ||
(key & config.imgkey2[12] && !(key & config.imgkey2[13]))
) {
image_up();
}
if ((key & config.imgkey[13] && !(key & config.imgkey[12])) ||
(key & config.imgkey2[13] && !(key & config.imgkey2[12]))) {
image_down();
}
thumb = (config.thumb == conf_thumb_scroll);
img_needrp = thumb | img_needrp;
}
void StereoCamera::feedImages() {
unsigned int pair_id = 0;
ros::Time prev_time = ros::Time::now();
ros::Duration force_wait_duration(1.0f / float(process_fps));
while (ok) {
unsigned char *frame_left = NULL, *frame_right = NULL;
uint32_t bytes_used_left, bytes_used_right;
ros::Time capture_time = ros::Time::now();
int left_idx = cam_left->grab(&frame_left, bytes_used_left);
int right_idx = cam_right->grab(&frame_right, bytes_used_right);
/* Read in every frame the camera generates, but only send each
* (skip_frames + 1)th frame.
* Alternatively, define this behavior with the process_fps parameter.
* process_fps is used when the camera will not respond to the FPS
* setting and it's actual FPS is unknown.
* These reduces effective frame rate, processing time and network usage
* while keeping the images synchronized within the true framerate.
*/
if ((skip_frames == 0 || frames_to_skip == 0) &&
((ros::Time::now() - prev_time) >= force_wait_duration)) {
if (frame_left && frame_right) {
ImagePtr image_left(new Image);
ImagePtr image_right(new Image);
image_left->height = height;
image_left->width = width;
image_left->step = 3 * width;
image_left->encoding = image_encodings::RGB8;
image_left->header.stamp = capture_time;
image_left->header.seq = pair_id;
image_right->height = height;
image_right->width = width;
image_right->step = 3 * width;
image_right->encoding = image_encodings::RGB8;
image_right->header.stamp = capture_time;
image_right->header.seq = pair_id;
image_left->header.frame_id = frame;
image_right->header.frame_id = frame;
image_left->data.resize(image_left->step * image_left->height);
image_right->data.resize(image_right->step * image_right->height);
if (rotate_left)
rotate(&image_left->data[0], frame_left, width * height);
else if (vertical)
rotateVertical(&image_left->data[0], frame_left, width, height);
else
memcpy(&image_left->data[0], frame_left, width * height * 3);
if (rotate_right)
rotate(&image_right->data[0], frame_right, width * height);
else if (vertical)
rotateVertical(&image_right->data[0], frame_right, width, height);
else
memcpy(&image_right->data[0], frame_right, width * height * 3);
left_pub.publish(image_left);
right_pub.publish(image_right);
sendInfo(capture_time);
++pair_id;
prev_time = capture_time;
}
frames_to_skip = skip_frames;
} else if (frames_to_skip > 0) {
frames_to_skip--;
}
if (frame_left)
cam_left->release(left_idx);
if (frame_right)
cam_right->release(right_idx);
}
}
/**
* Independent thread that grabs images and publishes them.
*/
void StereoCamera::feedImages() {
unsigned int pair_id = 0;
ROS_INFO("feedImages");
cv::Mat left, right;
// ok is a global that goes false in destructor
while (ok) {
//unsigned char *frame_left = NULL, *frame_right = NULL;
//uint32_t bytes_used_left, bytes_used_right;
ros::Time capture_time = ros::Time::now();
//int left_idx = cam_left->grab(&frame_left, bytes_used_left);
//int right_idx = cam_right->grab(&frame_right, bytes_used_right);
left = cam_left->getImage();
right = cam_left->getImage();
// Read in every frame the camera generates, but only send each
// (skip_frames + 1)th frame. This reduces effective frame
// rate, processing time and network usage while keeping the
// images synchronized within the true framerate.
//
if (skip_frames == 0 || frames_to_skip == 0) {
//if (frame_left && frame_right) {
if(left.data && right.data){
ImagePtr image_left(new Image);
ImagePtr image_right(new Image);
image_left->height = height;
image_left->width = width;
image_left->step = 3 * width;
image_left->encoding = image_encodings::RGB8;
image_left->header.stamp = capture_time;
image_left->header.seq = pair_id;
image_right->height = height;
image_right->width = width;
image_right->step = 3 * width;
image_right->encoding = image_encodings::RGB8;
image_right->header.stamp = capture_time;
image_right->header.seq = pair_id;
image_left->header.frame_id = frame;
image_right->header.frame_id = frame;
image_left->data.resize(image_left->step * image_left->height);
image_right->data.resize(image_right->step * image_right->height);
/*
if (rotate_left)
rotate(&image_left->data[0], frame_left, width * height);
else
memcpy(&image_left->data[0], frame_left, width * height * 3);
if (rotate_right)
rotate(&image_right->data[0], frame_right, width * height);
else
memcpy(&image_right->data[0], frame_right, width * height * 3);
*/
left_pub.publish(image_left);
right_pub.publish(image_right);
sendInfo(capture_time);
++pair_id;
}
frames_to_skip = skip_frames;
} else {
frames_to_skip--;
}
//if (frame_left) cam_left->release(left_idx);
//if (frame_right) cam_right->release(right_idx);
}
}
void StereoCamera::feedImages() {
unsigned int pair_id = 0;
FlyCapture2::Error error;
while (ok) {
// Retrieve an image
error = cam_left.RetrieveBuffer( &rawImage_l );
if (error != FlyCapture2::PGRERROR_OK) {
// PrintError( error );
continue;
}
error = cam_right.RetrieveBuffer( &rawImage_r );
if (error != FlyCapture2::PGRERROR_OK) {
// PrintError( error );
continue;
}
ros::Time capture_time = ros::Time::now();
// Convert the raw image
error = rawImage_l.Convert( FlyCapture2::PIXEL_FORMAT_RGB, &convertedImage_l );
if (error != FlyCapture2::PGRERROR_OK) {
// PrintError( error );
continue;
}
error = rawImage_r.Convert( FlyCapture2::PIXEL_FORMAT_RGB, &convertedImage_r );
if (error != FlyCapture2::PGRERROR_OK) {
// PrintError( error );
continue;
}
ImagePtr image_left(new Image);
ImagePtr image_right(new Image);
image_left->height = convertedImage_l.GetRows();
image_left->width = convertedImage_l.GetCols();
image_left->step = convertedImage_l.GetStride();
image_left->encoding = image_encodings::RGB8;
image_left->header.stamp = capture_time;
image_left->header.seq = pair_id;
image_left->header.frame_id = frame;
int data_size = convertedImage_l.GetDataSize();
image_left->data.resize(data_size);
memcpy(&image_left->data[0], convertedImage_l.GetData(), data_size);
image_right->height = convertedImage_r.GetRows();
image_right->width = convertedImage_r.GetCols();
image_right->step = convertedImage_r.GetStride();
image_right->encoding = image_encodings::RGB8;
image_right->header.stamp = capture_time;
image_right->header.seq = pair_id;
image_right->header.frame_id = frame;
data_size = convertedImage_r.GetDataSize();
image_right->data.resize(data_size);
memcpy(&image_right->data[0], convertedImage_r.GetData(), data_size);
left_pub.publish(image_left);
right_pub.publish(image_right);
sendInfo(capture_time);
++pair_id;
}
}
void StereoCamera::feedImages() {
unsigned int pair_id = 0;
dynamic_reconfigure::Server<see3cam::cameraParamsConfig> server;
dynamic_reconfigure::Server<see3cam::cameraParamsConfig>::CallbackType f;
f = boost::bind(&see3cam::StereoCamera::callback, this, _1, _2);
server.setCallback(f);
while (ok) {
unsigned char *frame_left = NULL, *frame_right = NULL;
uint32_t bytes_used_left, bytes_used_right;
int channels = 2;
if (encoding == "rgb8") {
channels = 3;
}
if (encoding == "mono8") {
channels = 1;
}
int left_idx = cam_left->grab(&frame_left, bytes_used_left);
int right_idx = cam_right->grab(&frame_right, bytes_used_right);
ros::Time capture_time = ros::Time::now();
if (frame_left && frame_right) {
ImagePtr image_left(new Image);
ImagePtr image_right(new Image);
image_left->height = height;
image_left->width = width;
image_left->step = channels * width;
image_left->encoding = encoding;
image_left->header.stamp = capture_time;
image_left->header.seq = pair_id;
image_right->height = height;
image_right->width = width;
image_right->step = channels * width;
image_right->encoding = encoding;
image_right->header.stamp = capture_time;
image_right->header.seq = pair_id;
image_left->header.frame_id = frame;
image_right->header.frame_id = frame;
image_left->data.resize(image_left->step * image_left->height);
image_right->data.resize(image_right->step * image_right->height);
if (rotate_left)
switch (channels) {
case 1:
RotateMono(&image_left->data[0], frame_left, width * height);
break;
case 3:
RotateRgb(&image_left->data[0], frame_left, width * height);
break;
default:
RotateYuv(&image_left->data[0], frame_left, width * height);
}
else
memcpy(&image_left->data[0], frame_left, image_left->step * height);
if (rotate_right)
switch (channels) {
case 1:
RotateMono(&image_right->data[0], frame_right, width * height);
break;
case 3:
RotateRgb(&image_right->data[0], frame_right, width * height);
break;
default:
RotateYuv(&image_right->data[0], frame_right, width * height);
}
else
memcpy(&image_right->data[0], frame_right, image_right->step * height);
left_pub.publish(image_left);
right_pub.publish(image_right);
sendInfo(capture_time);
++pair_id;
} else {
// Grab failure - need to restart camera driver.
if (cam_left)
delete cam_left;
if (cam_right)
delete cam_right;
sleep (1);
cam_left = new uvc_cam::Cam(left_device.c_str(), mode, width, height, fps);
cam_right = new uvc_cam::Cam(right_device.c_str(), mode, width, height, fps);
sleep (1);
// Force dynamic reconfigure to update
std::string cmd = "rosrun dynamic_reconfigure dynparam set " + ros::this_node::getName() + " exposureauto 1";
printf((cmd + "\n").c_str());
system(cmd.c_str());
}
if (frame_left)
cam_left->release(left_idx);
if (frame_right)
cam_right->release(right_idx);
}
}
