void OrbitCameraController::update(double dt)
{
if (!isActive())
return;
if (!mInitialized)
initialize();
double rotDist = mOrbitSpeed * dt;
if (mFast ^ mFastAlternate)
rotDist *= mOrbitSpeedMult;
if (mLeft)
rotateHorizontal(-rotDist);
if (mRight)
rotateHorizontal(rotDist);
if (mUp)
rotateVertical(rotDist);
if (mDown)
rotateVertical(-rotDist);
if (mRollLeft)
roll(-rotDist);
if (mRollRight)
roll(rotDist);
// Normalize the matrix to counter drift
getCamera()->getViewMatrix().orthoNormal(getCamera()->getViewMatrix());
}
void OrbitCameraController::handleMouseMoveEvent(int x, int y)
{
if (!isActive())
return;
if (!mInitialized)
initialize();
if (mNaviPrimary)
{
double scalar = getCameraSensitivity() * (getInverted() ? -1.0 : 1.0);
rotateHorizontal(x * scalar);
rotateVertical(-y * scalar);
}
else if (mNaviSecondary)
{
osg::Vec3d movement;
movement += LocalLeft * x * getSecondaryMovementMultiplier();
movement += LocalUp * -y * getSecondaryMovementMultiplier();
translate(movement);
}
}
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);
}
}