Skip to content

Instantly share code, notes, and snippets.

@ajshort

ajshort/ortho.patch

Created July 10, 2017 23:31
Show Gist options
  • Save ajshort/87b8bd2a48222436166922bfc8f5272b to your computer and use it in GitHub Desktop.
Save ajshort/87b8bd2a48222436166922bfc8f5272b to your computer and use it in GitHub Desktop.
Download ZIP
Raw
ortho.patch
diff --git a/plugin_description.xml b/plugin_description.xml
index fb0152d..b5951c2 100644
--- a/plugin_description.xml
+++ b/plugin_description.xml
@@ -269,6 +269,11 @@
Control the camera like in a First Person Shooter game: drag left to look left, etc.
</description>
</class>
+ <class name="rviz/Ortho" type="rviz::OrthoViewController" base_class_type="rviz::ViewController">
+ <description>
+ Orthographic projection.
+ </description>
+ </class>
<class name="rviz/TopDownOrtho" type="rviz::FixedOrientationOrthoViewController" base_class_type="rviz::ViewController">
<description>
Orthographic projection, seen from the top.
diff --git a/src/rviz/default_plugin/CMakeLists.txt b/src/rviz/default_plugin/CMakeLists.txt
index 989ebc1..5439e30 100644
--- a/src/rviz/default_plugin/CMakeLists.txt
+++ b/src/rviz/default_plugin/CMakeLists.txt
@@ -61,6 +61,7 @@ set(SOURCE_FILES
tools/interaction_tool.cpp
view_controllers/orbit_view_controller.cpp
view_controllers/xy_orbit_view_controller.cpp
+ view_controllers/ortho_view_controller.cpp
view_controllers/third_person_follower_view_controller.cpp
view_controllers/fixed_orientation_ortho_view_controller.cpp
view_controllers/fps_view_controller.cpp
diff --git a/src/rviz/default_plugin/view_controllers/fixed_orientation_ortho_view_controller.h b/src/rviz/default_plugin/view_controllers/fixed_orientation_ortho_view_controller.h
index 4a6c362..6aa679c 100644
--- a/src/rviz/default_plugin/view_controllers/fixed_orientation_ortho_view_controller.h
+++ b/src/rviz/default_plugin/view_controllers/fixed_orientation_ortho_view_controller.h
@@ -30,7 +30,7 @@
#ifndef RVIZ_FIXED_ORIENTATION_ORTHO_VIEW_CONTROLLER_H
#define RVIZ_FIXED_ORIENTATION_ORTHO_VIEW_CONTROLLER_H
-#include "rviz/frame_position_tracking_view_controller.h"
+#include <rviz/frame_position_tracking_view_controller.h>
#include <OgreQuaternion.h>
diff --git a/src/rviz/default_plugin/view_controllers/ortho_view_controller.cpp b/src/rviz/default_plugin/view_controllers/ortho_view_controller.cpp
new file mode 100644
index 0000000..2bd1fe6
--- /dev/null
+++ b/src/rviz/default_plugin/view_controllers/ortho_view_controller.cpp
@@ -0,0 +1,251 @@
+#include "rviz/default_plugin/view_controllers/ortho_view_controller.h"
+
+#include "rviz/display_context.h"
+#include "rviz/geometry.h"
+#include "rviz/ogre_helpers/orthographic.h"
+#include "rviz/ogre_helpers/shape.h"
+#include "rviz/properties/enum_property.h"
+#include "rviz/properties/float_property.h"
+#include "rviz/properties/quaternion_property.h"
+#include "rviz/properties/vector_property.h"
+#include "rviz/viewport_mouse_event.h"
+
+#include <OgreCamera.h>
+#include <OgreSceneNode.h>
+#include <OgreViewport.h>
+
+#include <pluginlib/class_list_macros.h>
+
+#include <QEvent>
+
+#include <ros/console.h>
+
+namespace rviz
+{
+static const double VIEW_DISTANCE = 500.0;
+static const double DEFAULT_SCALE = 100.0;
+
+static const char *STATUS = "<b>Left-Click:</b> Rotate PY. <b>Middle-Click:</b> Move XY. "
+ "<b>Right-Click/Mouse Wheel:</b> Zoom. <b>Shift</b>: More options.";
+
+static const char *STATUS_SHIFT = "<b>Left-Click:</b> Rotate R. <b>Middle-Click:</b> Move XY. "
+ "<b>Right-Click:</b> Move Z. <b>Mouse Wheel:</b> Zoom.";
+
+OrthoViewController::OrthoViewController()
+ : plane_property_(new rviz::EnumProperty("Plane", "none", "Optionally lock the view to a plane", this))
+ , center_property_(new rviz::VectorProperty("Center", Ogre::Vector3::ZERO, "The focal point of the camera", this))
+ , orientation_property_(new rviz::QuaternionProperty("Orientation", Ogre::Quaternion::IDENTITY, "", this))
+ , scale_property_(new rviz::FloatProperty("Scale", DEFAULT_SCALE, "How much to scale up the scene", this))
+{
+ plane_property_->addOption("none", PLANE_NONE);
+ plane_property_->addOption("XY", PLANE_XY);
+ plane_property_->addOption("XZ", PLANE_XZ);
+ plane_property_->addOption("YZ", PLANE_YZ);
+
+ connect(plane_property_, SIGNAL(changed()), this, SLOT(onPlaneChanged()));
+}
+
+OrthoViewController::~OrthoViewController() = default;
+
+void OrthoViewController::onInitialize()
+{
+ FramePositionTrackingViewController::onInitialize();
+
+ camera_->setProjectionType(Ogre::PT_ORTHOGRAPHIC);
+ // camera_->setFixedYawAxis(false);
+
+ center_shape_.reset(new rviz::Shape(rviz::Shape::Sphere, context_->getSceneManager(), target_scene_node_));
+ center_shape_->setScale(Ogre::Vector3(0.05f, 0.05f, 0.01f));
+ center_shape_->setColor(1.0f, 1.0f, 0.0f, 0.5f);
+ center_shape_->getRootNode()->setVisible(false);
+}
+
+void OrthoViewController::handleMouseEvent(rviz::ViewportMouseEvent &e)
+{
+ bool moved = false;
+
+ int dx = 0;
+ int dy = 0;
+
+ if (e.shift())
+ setStatus(STATUS_SHIFT);
+ else
+ setStatus(STATUS);
+
+ if (e.type == QEvent::MouseButtonPress)
+ {
+ dragging_ = true;
+ center_shape_->getRootNode()->setVisible(true);
+ }
+ else if (e.type == QEvent::MouseButtonRelease)
+ {
+ dragging_ = false;
+ center_shape_->getRootNode()->setVisible(false);
+ }
+ else if (e.type == QEvent::MouseMove && dragging_)
+ {
+ moved = true;
+
+ dx = e.x - e.last_x;
+ dy = e.y - e.last_y;
+ }
+
+ bool rotate_z = e.shift() || getPlane() != PLANE_NONE;
+ auto rotate_cursor = rotate_z ? Rotate2D : Rotate3D;
+
+ auto rotate = [this](double angle, const Ogre::Vector3 &axis) {
+ const auto &orientation = orientation_property_->getQuaternion();
+
+ Ogre::Quaternion q;
+ q.FromAngleAxis(Ogre::Radian(angle), orientation * axis);
+ q.normalise();
+
+ orientation_property_->setQuaternion(q * orientation);
+ };
+
+ if (e.left())
+ {
+ setCursor(rotate_cursor);
+
+ if (rotate_z)
+ {
+ rotate(0.005 * dx, Ogre::Vector3::UNIT_Z);
+ }
+ else
+ {
+ rotate(-0.005 * dx, Ogre::Vector3::UNIT_Y);
+ rotate(-0.005 * dy, Ogre::Vector3::UNIT_X);
+ }
+ }
+ else if (e.middle() || (e.left() && e.shift()))
+ {
+ setCursor(MoveXY);
+
+ auto scale = scale_property_->getFloat();
+ auto movement = orientation_property_->getQuaternion() * Ogre::Vector3(-dx / scale, dy / scale, 0);
+
+ center_property_->add(movement);
+ }
+ else if (e.right() && !e.shift())
+ {
+ setCursor(Zoom);
+ scale_property_->multiply(1 - 0.01 * dy);
+ }
+ else if (e.right() && e.shift())
+ {
+ setCursor(MoveZ);
+
+ auto scale = scale_property_->getFloat();
+ auto movement = orientation_property_->getQuaternion() * Ogre::Vector3(0, 0, dy / scale);
+
+ center_property_->add(movement);
+ }
+ else
+ {
+ setCursor(e.shift() ? MoveXY : rotate_cursor);
+ }
+
+ if (e.wheel_delta)
+ {
+ moved = true;
+ scale_property_->multiply(1 + 0.001 * e.wheel_delta);
+ }
+
+ if (moved)
+ {
+ context_->queueRender();
+ emitConfigChanged();
+ }
+}
+
+void OrthoViewController::lookAt(const Ogre::Vector3 &p)
+{
+ center_property_->setVector(p - target_scene_node_->getPosition());
+}
+
+void OrthoViewController::reset()
+{
+ plane_property_->setString("none");
+ center_property_->setVector(Ogre::Vector3::ZERO);
+ orientation_property_->setQuaternion(Ogre::Quaternion::IDENTITY);
+ scale_property_->setFloat(DEFAULT_SCALE);
+}
+
+void OrthoViewController::mimic(rviz::ViewController *source)
+{
+ FramePositionTrackingViewController::mimic(source);
+
+ if (auto *source_ortho = qobject_cast<OrthoViewController *>(source))
+ {
+ plane_property_->setString(source_ortho->plane_property_->getString());
+ center_property_->setVector(source_ortho->center_property_->getVector());
+ orientation_property_->setQuaternion(source_ortho->orientation_property_->getQuaternion());
+ scale_property_->setFloat(source_ortho->scale_property_->getFloat());
+ }
+ else
+ {
+ center_property_->setVector(source->getCamera()->getPosition());
+ }
+}
+
+void OrthoViewController::update(float dt, float ros_dt)
+{
+ FramePositionTrackingViewController::update(dt, ros_dt);
+
+ // Build the projection matrix.
+ auto scale = scale_property_->getFloat();
+ auto width = camera_->getViewport()->getActualWidth() / scale / 2;
+ auto height = camera_->getViewport()->getActualHeight() / scale / 2;
+
+ auto near = camera_->getNearClipDistance();
+ auto far = camera_->getFarClipDistance();
+
+ Ogre::Matrix4 projection;
+ rviz::buildScaledOrthoMatrix(projection, -width, width, -height, height, near, far);
+
+ camera_->setCustomProjectionMatrix(true, projection);
+
+ // Set the camera pose.
+ auto center = center_property_->getVector();
+ auto orientation = orientation_property_->getQuaternion();
+
+ camera_->setOrientation(orientation);
+ camera_->setPosition(center + orientation * Ogre::Vector3::UNIT_Z * VIEW_DISTANCE);
+
+ center_shape_->setPosition(center);
+}
+
+void OrthoViewController::onTargetFrameChanged(const Ogre::Vector3 &old_pos, const Ogre::Quaternion &old_orient)
+{
+ // TODO
+}
+
+void OrthoViewController::onPlaneChanged()
+{
+ auto plane = getPlane();
+ bool locked = plane != PLANE_NONE;
+
+ orientation_property_->setReadOnly(locked);
+ orientation_property_->setHidden(locked);
+
+ if (locked)
+ {
+ auto orientation = Ogre::Quaternion::IDENTITY;
+
+ // TODO fix XZ and YZ planes.
+ if (plane == PLANE_XZ)
+ orientation.FromAngleAxis(Ogre::Radian(M_PI / 2), Ogre::Vector3::UNIT_X);
+ else if (plane == PLANE_YZ)
+ orientation.FromAngleAxis(Ogre::Radian(M_PI / 2), Ogre::Vector3::UNIT_Y);
+
+ orientation_property_->setQuaternion(orientation);
+ }
+}
+
+OrthoViewController::Plane OrthoViewController::getPlane() const
+{
+ return static_cast<Plane>(plane_property_->getOptionInt());
+}
+}
+
+PLUGINLIB_EXPORT_CLASS(rviz::OrthoViewController, rviz::ViewController);
diff --git a/src/rviz/default_plugin/view_controllers/ortho_view_controller.h b/src/rviz/default_plugin/view_controllers/ortho_view_controller.h
new file mode 100644
index 0000000..3b4b5b7
--- /dev/null
+++ b/src/rviz/default_plugin/view_controllers/ortho_view_controller.h
@@ -0,0 +1,66 @@
+#ifndef RVIZ_ORTHO_VIEW_CONTROLLER_ORTHO_VIEW_CONTROLLER_H
+#define RVIZ_ORTHO_VIEW_CONTROLLER_ORTHO_VIEW_CONTROLLER_H
+
+#include "rviz/frame_position_tracking_view_controller.h"
+
+#include <memory>
+
+namespace rviz
+{
+class EnumProperty;
+class FloatProperty;
+class QuaternionProperty;
+class Shape;
+class VectorProperty;
+
+/// An orthographic view controller which can be optionally locked to a plane.
+class OrthoViewController : public rviz::FramePositionTrackingViewController
+{
+ Q_OBJECT
+
+public:
+ OrthoViewController();
+ ~OrthoViewController();
+
+ void onInitialize() override;
+
+ void handleMouseEvent(rviz::ViewportMouseEvent &e) override;
+
+ void lookAt(const Ogre::Vector3 &p) override;
+
+ void reset() override;
+
+ void mimic(rviz::ViewController *source) override;
+
+ void update(float dt, float ros_dt) override;
+
+protected:
+ void onTargetFrameChanged(const Ogre::Vector3 &old_pos, const Ogre::Quaternion &old_orient) override;
+
+private Q_SLOTS:
+ void onPlaneChanged();
+
+private:
+ enum Plane
+ {
+ PLANE_NONE = 0,
+ PLANE_XY,
+ PLANE_XZ,
+ PLANE_YZ
+ };
+
+ /// Gets the plane this view controller is currently locked to.
+ Plane getPlane() const;
+
+ rviz::EnumProperty *plane_property_;
+ rviz::VectorProperty *center_property_;
+ rviz::QuaternionProperty *orientation_property_;
+ rviz::FloatProperty *scale_property_;
+
+ bool dragging_ = false;
+
+ std::unique_ptr<rviz::Shape> center_shape_;
+};
+}
+
+#endif // RVIZ_ORTHO_VIEW_CONTROLLER_ORTHO_VIEW_CONTROLLER_H
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment