49 #include "view_controller_msgs/CameraPlacement.h" 50 #include "geometry_msgs/PointStamped.h" 52 #include <OGRE/OgreViewport.h> 53 #include <OGRE/OgreQuaternion.h> 54 #include <OGRE/OgreVector3.h> 55 #include <OGRE/OgreSceneNode.h> 56 #include <OGRE/OgreSceneManager.h> 57 #include <OGRE/OgreCamera.h> 62 #include <OGRE/OgreRenderWindow.h> 81 static inline Ogre::Vector3
vectorFromMsg(
const geometry_msgs::Point &m) {
return Ogre::Vector3(m.x, m.y, m.z); }
82 static inline Ogre::Vector3
vectorFromMsg(
const geometry_msgs::Vector3 &m) {
return Ogre::Vector3(m.x, m.y, m.z); }
85 geometry_msgs::Point m;
86 m.x = o.x; m.y = o.y; m.z = o.z;
89 static inline void pointOgreToMsg(
const Ogre::Vector3 &o, geometry_msgs::Point &m) { m.x = o.x; m.y = o.y; m.z = o.z; }
93 geometry_msgs::Vector3 m;
94 m.x = o.x; m.y = o.y; m.z = o.z;
97 static inline void vectorOgreToMsg(
const Ogre::Vector3 &o, geometry_msgs::Vector3 &m) { m.x = o.x; m.y = o.y; m.z = o.z; }
103 : nh_(
""), animate_(false), dragging_( false )
108 "Enables mouse control of the camera.",
111 "Select the style of mouse interaction.",
118 "If enabled, the camera is not allowed to roll side-to-side.",
122 "TF frame the camera is attached to.",
125 "Position of the camera.",
this );
127 "Position of the focus/orbit point.",
this );
129 "The vector which maps to \"up\" in the camera image plane.",
this );
131 "The distance between the camera position and the focus point.",
135 "The default time to use for camera transitions.",
138 QString::fromStdString(ros::message_traits::datatype<view_controller_msgs::CameraPlacement>() ),
139 "Topic for CameraPlacement messages",
this, SLOT(
updateTopics()));
142 QString::fromStdString(ros::message_traits::datatype<view_controller_msgs::CameraPlacement>() ),
146 QString::fromStdString(ros::message_traits::datatype<geometry_msgs::PointStamped>() ),
174 geometry_msgs::PointStamped
msg;
180 msg.point.x = (double)event.
x / window->getWidth();
181 msg.point.y = (double)event.
y / window->getHeight();
188 view_controller_msgs::CameraPlacement
msg;
193 msg.eye.header.stamp =
now;
194 msg.eye.header.frame_id = fixed_frame;
196 msg.eye.point.x = eye[0];
197 msg.eye.point.y = eye[1];
198 msg.eye.point.z = eye[2];
200 msg.focus.header.stamp =
now;
201 msg.focus.header.frame_id = fixed_frame;
203 msg.focus.point.x = focus[0];
204 msg.focus.point.y = focus[1];
205 msg.focus.point.z = focus[2];
207 msg.up.header.stamp =
now;
208 msg.up.header.frame_id = fixed_frame;
210 msg.up.vector.x = up[0];
211 msg.up.vector.y = up[1];
212 msg.up.vector.z = up[2];
234 camera_->setProjectionType( Ogre::PT_PERSPECTIVE );
309 camera_->setFixedYawAxis(
false);
326 Ogre::Vector3 new_reference_position;
327 Ogre::Quaternion new_reference_orientation;
331 new_reference_position, new_reference_orientation ))
391 setStatus(
"<b>Mouse interaction is disabled. You can enable it by checking the \"Mouse Enabled\" check-box in the Views panel." );
394 else if ( event.
shift() )
396 setStatus(
"TODO: Fix me! <b>Left-Click:</b> Move X/Y. <b>Right-Click:</b>: Move Z." );
400 setStatus(
"TODO: Fix me! <b>Left-Click:</b> Move X/Y. <b>Right-Click:</b>: Move Z." );
404 setStatus(
"TODO: Fix me! <b>Left-Click:</b> Rotate. <b>Middle-Click:</b> Move X/Y. <b>Right-Click:</b>: Zoom. <b>Shift</b>: More options." );
406 if (event.
type == QEvent::MouseButtonPress
407 || event.
type == QEvent::MouseButtonRelease
416 if( event.
type == QEvent::MouseButtonPress )
423 else if( event.
type == QEvent::MouseButtonRelease )
431 diff_x =
event.x -
event.last_x;
432 diff_y =
event.y -
event.last_y;
437 if( event.
left() && !
event.shift() )
443 else if( event.
middle() || (
event.shift() &&
event.left() ))
448 float fovY =
camera_->getFOVy().valueRadians();
449 float fovX = 2.0f *
atan(
tan( fovY / 2.0
f ) *
camera_->getAspectRatio() );
455 ((
float)diff_y / (
float)height) * distance *
tan( fovY / 2.0f ) * 2.0f,
463 else if( event.
right() )
473 move_eye( 0, 0, diff_y * 0.01 * distance );
483 int diff =
event.wheel_delta;
495 move_eye( 0, 0, -diff * 0.001 * distance );
500 if(event.
type == QEvent::MouseButtonPress && event.
left() &&
event.control() &&
event.shift())
528 Ogre::Vector3 direction = source_camera->getOrientation() * Ogre::Vector3::NEGATIVE_UNIT_Z;
543 if( target_frame.isValid() )
548 Ogre::Camera* source_camera = source_view->
getCamera();
549 Ogre::Vector3 position = source_camera->getPosition();
550 Ogre::Quaternion orientation = source_camera->getOrientation();
552 if( source_view->
getClassId() ==
"rviz/Orbit" )
621 CameraPlacement cp = *cp_ptr;
626 if(cp.mouse_interaction_mode != cp.NO_CHANGE)
628 std::string name =
"";
629 if(cp.mouse_interaction_mode == cp.ORBIT) name =
MODE_ORBIT;
630 else if(cp.mouse_interaction_mode == cp.FPS) name =
MODE_FPS;
634 if(cp.target_frame !=
"")
640 if(cp.time_from_start.toSec() >= 0)
692 Ogre::Vector3 position_fixed_eye, position_fixed_focus, position_fixed_up;
693 Ogre::Quaternion rotation_fixed_eye, rotation_fixed_focus, rotation_fixed_up;
765 float progress = 0.5*(1-
cos(fraction*M_PI));
796 Ogre::Quaternion old_camera_orientation =
camera_->getOrientation();
797 Ogre::Radian old_pitch = old_camera_orientation.getPitch(
false);
801 Ogre::Quaternion yaw_quat, pitch_quat, roll_quat;
802 yaw_quat.FromAngleAxis( Ogre::Radian( yaw ), Ogre::Vector3::UNIT_Y );
803 pitch_quat.FromAngleAxis( Ogre::Radian( pitch ), Ogre::Vector3::UNIT_X );
804 roll_quat.FromAngleAxis( Ogre::Radian( roll ), Ogre::Vector3::UNIT_Z );
805 Ogre::Quaternion orientation_change = yaw_quat * pitch_quat * roll_quat;
806 Ogre::Quaternion new_camera_orientation = old_camera_orientation * orientation_change;
807 Ogre::Radian new_pitch = new_camera_orientation.getPitch(
false);
810 ((new_pitch > PITCH_LIMIT_HIGH && new_pitch > old_pitch) || (new_pitch < PITCH_LIMIT_LOW && new_pitch < old_pitch)) )
812 orientation_change = yaw_quat * roll_quat;
813 new_camera_orientation = old_camera_orientation * orientation_change;
820 camera_->setOrientation( new_camera_orientation );
826 camera_->setPosition(new_eye_position);
838 return camera_->getOrientation();
843 Ogre::Vector3 translate( x, y, z );
850 Ogre::Vector3 translate( x, y, z );
void setPropertiesFromCamera(Ogre::Camera *source_camera)
virtual void onInitialize()
Do subclass-specific initialization. Called by ViewController::initialize after context_ and camera_ ...
virtual void reset()
Resets the camera parameters to a sane value.
rviz::VectorProperty * focus_point_property_
The point around which the camera "orbits".
void publishCurrentPlacement()
virtual void onAttachedFrameChanged(const Ogre::Vector3 &old_reference_position, const Ogre::Quaternion &old_reference_orientation)
Override to implement the change in properties which nullifies the change in attached frame...
An un-constrained "flying" camera, specified by an eye point, focus point, and up vector...
std::string getFrameStd() const
Ogre::SceneNode * attached_scene_node_
virtual void onUpPropertyChanged()
Called when up vector property is changed (does nothing for now...).
virtual bool setVector(const Ogre::Vector3 &vector)
void publishMouseEvent(rviz::ViewportMouseEvent &event)
void updateCamera()
Updates the Ogre camera properties from the view controller properties.
Ogre::Vector3 reference_position_
Used to store the position of the attached frame relative to <Fixed frame>="">
Ogre::Quaternion getOrientation()
Return a Quaternion.
void updatePublishTopics()
ros::Duration current_transition_duration_
QCursor makeIconCursor(QString url, bool fill_cache)
void updateAttachedSceneNode()
Update the position of the attached_scene_node_ from the TF frame specified in the Attached Frame pro...
rviz::TfFrameProperty * attached_frame_property_
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
PLUGINLIB_EXPORT_CLASS(jsk_rviz_plugins::PictogramArrayDisplay, rviz::Display)
void cameraPlacementCallback(const view_controller_msgs::CameraPlacementConstPtr &cp_ptr)
virtual void onActivate()
called by activate().
void disconnectPositionProperties()
Convenience function; disconnects the signals/slots for position properties.
void beginNewTransition(const Ogre::Vector3 &eye, const Ogre::Vector3 &focus, const Ogre::Vector3 &up, const ros::Duration &transition_time)
Begins a camera movement animation to the given goal points.
virtual ViewManager * getViewManager() const=0
void setScale(const Ogre::Vector3 &scale) override
void yaw_pitch_roll(float yaw, float pitch, float roll)
Applies a body-fixed-axes sequence of rotations; only accurate for small angles.
void move_eye(float x, float y, float z)
Applies a translation to only the eye point.
void cancelTransition()
Cancels any currently active camera movement.
Ogre::Vector3 fixedFrameToAttachedLocal(const Ogre::Vector3 &v)
ros::Subscriber placement_subscriber_
IMETHOD Vector diff(const Vector &p_w_a, const Vector &p_w_b, double dt=1)
void setCursor(CursorType cursor_type)
rviz::BoolProperty * mouse_enabled_property_
If True, most user changes to camera state are disabled.
Ogre::Camera * getCamera() const
ros::Time transition_start_time_
virtual Ogre::Vector3 getVector() const
virtual void updateAttachedFrame()
Called when Target Frame property changes while view is active. Purpose is to change values in the vi...
static geometry_msgs::Vector3 vectorOgreToMsg(const Ogre::Vector3 &o)
void addOptionStd(const std::string &option)
void setColor(float r, float g, float b, float a) override
bool setValue(const QVariant &new_value) override
bool add(const Ogre::Vector3 &offset)
static Ogre::Vector3 vectorFromMsg(const geometry_msgs::Point &m)
void orbitCameraTo(const Ogre::Vector3 &point)
Calls beginNewTransition() with the focus point fixed, moving the eye to the point given...
ros::Publisher mouse_point_publisher_
virtual QString getClassId() const
ros::Publisher placement_publisher_
std::string getStdString()
virtual void lookAt(const Ogre::Vector3 &point)
Calls beginNewTransition() to move the focus point to the point provided, assumed to be in the Rviz F...
virtual void handleMouseEvent(rviz::ViewportMouseEvent &evt)
bool setFloat(float new_value)
virtual Property * subProp(const QString &sub_name)
bool dragging_
A flag indicating the dragging state of the mouse.
void publish(const boost::shared_ptr< M > &message) const
rviz::RosTopicProperty * camera_placement_publish_topic_property_
rviz::EditableEnumProperty * interaction_mode_property_
Select between Orbit or FPS control style.
INLINE Rall1d< T, V, S > atan(const Rall1d< T, V, S > &x)
virtual void mimic(ViewController *source_view)
Configure the settings of this view controller to give, as much as possible, a similar view as that g...
rviz::RosTopicProperty * camera_placement_topic_property_
void updateMousePointPublishTopics()
rviz::BoolProperty * fixed_up_property_
If True, "up" is fixed to ... up.
RenderPanel * getRenderPanel() const
virtual ~TabletViewController()
virtual void transitionFrom(ViewController *previous_view)
Called by ViewManager when this ViewController is being made current.
void setStatus(const QString &message)
virtual FrameManager * getFrameManager() const=0
virtual void update(float dt, float ros_dt)
Called at 30Hz by ViewManager::update() while this view is active. Override with code that needs to r...
virtual void onDistancePropertyChanged()
Called when distance property is changed; computes new eye position.
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
Ogre::Vector3 start_focus_
#define ROS_DEBUG_STREAM(args)
virtual void onFocusPropertyChanged()
Called focus property is changed; computes new distance.
rviz::RosTopicProperty * mouse_point_publish_topic_property_
Ogre::Vector3 goal_focus_
void setPosition(const Ogre::Vector3 &position) override
rviz::Shape * focal_shape_
A small ellipsoid to show the focus point.
static const Ogre::Radian PITCH_LIMIT_LOW
Ogre::Vector3 start_position_
Ogre::Vector3 goal_position_
virtual Ogre::SceneManager * getSceneManager() const=0
virtual void queueRender()=0
void move_focus_and_eye(float x, float y, float z)
Applies a translation to the focus and eye points.
virtual QString getFixedFrame() const=0
static const QString FIXED_FRAME_STRING
virtual float getFloat() const
Ogre::Quaternion reference_orientation_
Used to store the orientation of the attached frame relative to <Fixed frame>=""> ...
bool getTransform(const Header &header, Ogre::Vector3 &position, Ogre::Quaternion &orientation)
void moveEyeWithFocusTo(const Ogre::Vector3 &point)
Calls beginNewTransition() to move the eye to the point given, keeping the direction fixed...
void setFrameManager(FrameManager *frame_manager)
rviz::VectorProperty * eye_point_property_
The position of the camera.
float getDistanceFromCameraToFocalPoint()
Return the distance between camera and focal point.
static const Ogre::Radian PITCH_LIMIT_HIGH
TFSIMD_FORCE_INLINE tfScalar length(const Quaternion &q)
virtual QVariant getValue() const
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
INLINE Rall1d< T, V, S > tan(const Rall1d< T, V, S > &arg)
Ogre::SceneNode * getRootNode()
static geometry_msgs::Point pointOgreToMsg(const Ogre::Vector3 &o)
void transformCameraPlacementToAttachedFrame(view_controller_msgs::CameraPlacement &cp)
virtual bool getBool() const
bool setStdString(const std::string &std_str)
DisplayContext * context_
QCursor interaction_disabled_cursor_
A cursor for indicating mouse interaction is disabled.
static const std::string MODE_ORBIT
void connectPositionProperties()
Convenience function; connects the signals/slots for position properties.
rviz::FloatProperty * default_transition_time_property_
A default time for any animation requests.
rviz::VectorProperty * up_vector_property_
The up vector for the camera.
rviz::FloatProperty * distance_property_
The camera's distance from the focal point.
virtual void onEyePropertyChanged()
Called when eye property is changed; computes new distance.
static const std::string MODE_FPS