rviz_animated_view_controller.h

Go to the documentation of this file.

/*
 * Copyright (c) 2012, 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.
 *
 * Author: Adam Leeper
 */

#ifndef RVIZ_ANIMATED_VIEW_CONTROLLER_H
#define RVIZ_ANIMATED_VIEW_CONTROLLER_H

#include <boost/circular_buffer.hpp>

#include <cv_bridge/cv_bridge.h>

#include <image_transport/image_transport.h>

#include "rviz/view_controller.h"
#include "rviz/view_manager.h"
#include "rviz/render_panel.h"

#include <ros/subscriber.h>
#include <ros/ros.h>

#include <geometry_msgs/Pose.h>

#include <std_msgs/Bool.h>
#include <std_msgs/Duration.h>

#include <view_controller_msgs/CameraMovement.h>
#include <view_controller_msgs/CameraPlacement.h>
#include <view_controller_msgs/CameraTrajectory.h>

#include <OGRE/OgreRenderWindow.h>
#include <OGRE/OgreVector3.h>
#include <OGRE/OgreQuaternion.h>

namespace rviz {
  class SceneNode;
  class Shape;
  class BoolProperty;
  class FloatProperty;
  class VectorProperty;
  class QuaternionProperty;
  class TfFrameProperty;
  class EditableEnumProperty;
  class RosTopicProperty;
}

namespace rviz_animated_view_controller
{

class AnimatedViewController : public rviz::ViewController
{
Q_OBJECT
public:

  enum { TRANSITION_LINEAR = 0,
         TRANSITION_SPHERICAL};

  struct OgreCameraMovement
  {
    OgreCameraMovement(){};

    OgreCameraMovement(const Ogre::Vector3& eye,
                       const Ogre::Vector3& focus,
                       const Ogre::Vector3& up,
                       const ros::Duration& transition_duration,
                       const uint8_t interpolation_speed)
      : eye(eye)
        , focus(focus)
        , up(up)
        , transition_duration(transition_duration)
        , interpolation_speed(interpolation_speed)
    {
    }

    Ogre::Vector3 eye;
    Ogre::Vector3 focus;
    Ogre::Vector3 up;

    ros::Duration transition_duration;
    uint8_t interpolation_speed;
  };

  typedef boost::circular_buffer<OgreCameraMovement> BufferCamMovements;

  AnimatedViewController();
  virtual ~AnimatedViewController();

  void initializePublishers();
  void initializeSubscribers();

  virtual void onInitialize();

  virtual void onActivate();


  void move_focus_and_eye( float x, float y, float z );

  void move_eye( float x, float y, float z );


  void yaw_pitch_roll( float yaw, float pitch, float roll );


  virtual void handleMouseEvent(rviz::ViewportMouseEvent& evt);

  void publishCameraPose();
  
  virtual void lookAt( const Ogre::Vector3& point );


  void orbitCameraTo( const Ogre::Vector3& point);

  void moveEyeWithFocusTo( const Ogre::Vector3& point);

  virtual void reset();

  virtual void mimic( ViewController* source_view );

  virtual void transitionFrom( ViewController* previous_view );


protected Q_SLOTS:
  virtual void updateAttachedFrame();
  
  virtual void onDistancePropertyChanged();

  virtual void onFocusPropertyChanged();
  
  virtual void onEyePropertyChanged();

  virtual void onUpPropertyChanged();

protected:  //methods
  void updateWindowSizeProperties();

  virtual void update(float dt, float ros_dt);

  void pauseAnimationOnRequest();

  bool isMovementAvailable(){ return cam_movements_buffer_.size() >= 2; };

  double computeRelativeProgressInTime(const ros::Duration& transition_duration);
  
  float computeRelativeProgressInSpace(double relative_progress_in_time,
                                       uint8_t interpolation_speed);

  void publishViewImage();

  void getViewImage(std::shared_ptr<Ogre::PixelBox>& pixel_box);

  void convertImage(std::shared_ptr<Ogre::PixelBox> input_image,
                    sensor_msgs::ImagePtr output_image);
  
  void prepareNextMovement(const ros::Duration& previous_transition_duration);
  
  void connectPositionProperties();

  void disconnectPositionProperties();

  virtual void onAttachedFrameChanged( const Ogre::Vector3& old_reference_position, const Ogre::Quaternion& old_reference_orientation );

  void updateAttachedSceneNode();

  void cameraPlacementCallback(const view_controller_msgs::CameraPlacementConstPtr &cp_ptr);
  
  void cameraTrajectoryCallback(const view_controller_msgs::CameraTrajectoryConstPtr& ct_ptr);
  
  void pauseAnimationCallback(const std_msgs::Duration::ConstPtr& pause_duration_msg);

  void transformCameraToAttachedFrame(geometry_msgs::PointStamped& eye,
                                      geometry_msgs::PointStamped& focus,
                                      geometry_msgs::Vector3Stamped& up);
  
  //void setUpVectorPropertyModeDependent( const Ogre::Vector3 &vector );

  void setPropertiesFromCamera( Ogre::Camera* source_camera );

  void beginNewTransition(const Ogre::Vector3& eye,
                          const Ogre::Vector3& focus,
                          const Ogre::Vector3& up,
                          ros::Duration transition_duration,
                          uint8_t interpolation_speed = view_controller_msgs::CameraMovement::WAVE);

  void cancelTransition();

  void updateCamera();

  Ogre::Vector3 fixedFrameToAttachedLocal(const Ogre::Vector3 &v) { return reference_orientation_.Inverse()*(v - reference_position_); }
  Ogre::Vector3 attachedLocalToFixedFrame(const Ogre::Vector3 &v) { return reference_position_ + (reference_orientation_*v); }

  float getDistanceFromCameraToFocalPoint(); 

  Ogre::Quaternion getOrientation(); 

protected Q_SLOTS:
  void updateTopics();


protected:    //members

  ros::NodeHandle nh_;

  rviz::BoolProperty* mouse_enabled_property_;            
  rviz::EditableEnumProperty* interaction_mode_property_; 
  rviz::BoolProperty* fixed_up_property_;                 

  rviz::FloatProperty* distance_property_;                
  rviz::VectorProperty* eye_point_property_;              
  rviz::VectorProperty* focus_point_property_;            
  rviz::VectorProperty* up_vector_property_;              
  rviz::FloatProperty* default_transition_time_property_; 

  rviz::RosTopicProperty* camera_placement_topic_property_;
  rviz::RosTopicProperty* camera_trajectory_topic_property_;

  rviz::FloatProperty* window_width_property_;            
  rviz::FloatProperty* window_height_property_;           

  rviz::BoolProperty* publish_view_images_property_;      

  rviz::TfFrameProperty* attached_frame_property_;
  Ogre::SceneNode* attached_scene_node_;

  Ogre::Quaternion reference_orientation_;    
  Ogre::Vector3 reference_position_;          

  // Variables used during animation
  bool animate_;
  ros::WallTime transition_start_time_;
  BufferCamMovements cam_movements_buffer_;

  rviz::Shape* focal_shape_;    
  bool dragging_;         

  QCursor interaction_disabled_cursor_;         
  
  ros::Subscriber placement_subscriber_;
  ros::Subscriber trajectory_subscriber_;
  ros::Subscriber pause_animation_duration_subscriber_;

  ros::Publisher current_camera_pose_publisher_;
  ros::Publisher finished_animation_publisher_;
  image_transport::Publisher camera_view_image_publisher_;

  bool render_frame_by_frame_;
  int target_fps_;
  int rendered_frames_counter_;

  ros::WallDuration pause_animation_duration_;
};

}  // namespace rviz_animated_view_controller

#endif // RVIZ_ANIMATED_VIEW_CONTROLLER_H