camera_following_camera.cpp

Go to the documentation of this file.

/*
 * Copyright (c) 2010, Willow Garage, Inc.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the Willow Garage, Inc. nor the names of its
 *       contributors may be used to endorse or promote products derived from
 *       this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

#include <rve_common_transformers/camera_following_camera.h>
#include <rve_transformer/transformer_manager.h>
#include <rve_transformer/frame_manager.h>
#include <rve_render_client/camera.h>
#include <rve_render_client/camera_info_projection_factory.h>
#include <rve_properties/property_node.h>

#include <rve_msgs/MouseEvent.h>

#include <rve_common/eigen_conversions.h>
#include <geometry_msgs/TransformStamped.h>
#include <ros/callback_queue.h>
#include <geometry_msgs/Pose.h>

#include <sys/time.h>

namespace rve_common_transformers
{

CameraFollowingCamera::CameraFollowingCamera()
: orientation_(Eigen::Quaternionf::Identity())
, position_(0.0, 0.0, 0.0)
, camera_projection_( new rve_render_client::CameraInfoProjectionFactory() )
, camera_info_changed_( false )
{
}

CameraFollowingCamera::~CameraFollowingCamera()
{
}

void CameraFollowingCamera::onInit()
{
  addSubscriber("camera_info", 1, &CameraFollowingCamera::cameraInfoCallback, this);
}

void CameraFollowingCamera::cameraInfoCallback(const sensor_msgs::CameraInfoConstPtr& camera_info)
{
//  boost::mutex::scoped_lock lock(caminfo_mutex_);
  camera_info_ = camera_info;
  camera_info_changed_ = true;
}

void CameraFollowingCamera::onUpdate()
{
  updateCamera();
}

void CameraFollowingCamera::onCameraSet()
{
  getCamera()->setPosition(rve_common::eigenToMsg(position_));
  getCamera()->setOrientation(rve_common::eigenToMsg(orientation_));
  getCamera()->setProjectionMatrixFactory( camera_projection_ );
}

void CameraFollowingCamera::updateCamera()
{
  sensor_msgs::CameraInfo info;
  {
//    boost::mutex::scoped_lock lock(caminfo_mutex_);
    if( !camera_info_changed_ || !camera_info_ )
    {
      return;
    }
    info = *camera_info_;
    camera_info_changed_ = false;
  }

  // Read the target frame relative to the fixed frame.
  geometry_msgs::TransformStamped target = getManager()->getFrameManager()->lookupTransform( info.header.frame_id, ros::Time());

  geometry_msgs::Vector3 p = target.transform.translation;
  position_ = Eigen::Vector3f(p.x, p.y, p.z);

  geometry_msgs::Quaternion q = target.transform.rotation;
  orientation_ = Eigen::Quaternionf(q.w, q.x, q.y, q.z);

  // convert vision (Z-forward, X-right) frame to rve scene frame (Z-up, X-forward)
  orientation_ = orientation_ *
    Eigen::AngleAxisf(-M_PI/2, Eigen::Vector3f::UnitZ()) *
    Eigen::AngleAxisf(-M_PI/2, Eigen::Vector3f::UnitY()) *
    Eigen::AngleAxisf(M_PI, Eigen::Vector3f::UnitX());
  
  float width = info.width;
  float height = info.height;

  // If the image width is 0 due to a malformed caminfo, try to grab the width from the image.
  if( width == 0 )
  {
// Not actually listening for images just yet...
//    width = texture_.getWidth();
  }

  if( height == 0 )
  {
// Not actually listening for images just yet...
//    height = texture_.getHeight();
  }

  if( height == 0 || width == 0 )
  {
    ROS_ERROR( "CameraInfo: bad size: width = %f, height = %f.", width, height );
    return;
  }

  double fx = info.P[0];
  double fy = info.P[5];

  // Add the camera's translation relative to the left camera (from P[3]);
  double tx = -1 * (info.P[3] / fx);
  Eigen::Vector3f right = orientation_ * Eigen::Vector3f::UnitX();
  position_ = position_ + (right * tx);

  double ty = -1 * (info.P[7] / fy);
  Eigen::Vector3f down = orientation_ * Eigen::Vector3f::UnitY();
  position_ = position_ + (down * ty);

  getCamera()->setPosition( rve_common::eigenToMsg( position_ ));
  getCamera()->setOrientation( rve_common::eigenToMsg( orientation_ ));

  double cx = info.P[2];
  double cy = info.P[6];
  camera_projection_->setCameraInfo( cx, cy, fx, fy, width, height );
}

} // namespace rve_common_transformers