Пример #1
0
    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());
    }
Пример #2
0
    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);
        }
    }
Пример #3
0
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);
  }
}