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