├── .gitignore ├── CHANGELOG.rst ├── plugin_description.xml ├── README.md ├── package.xml ├── LICENSE.md ├── include └── rviz_ortho_view_controller │ └── ortho_view_controller.h ├── CMakeLists.txt └── src └── ortho_view_controller.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | .idea/ 2 | .vscode/ 3 | 4 | *~ 5 | -------------------------------------------------------------------------------- /CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package rviz_ortho_view_controller 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.1.0 (2016-09-17) 6 | ------------------ 7 | * Update catkin_package 8 | * Initial commit 9 | * Contributors: Andrew Short 10 | -------------------------------------------------------------------------------- /plugin_description.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | An orthographic view controller 6 | 7 | 8 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # RViz Ortho View Controller 2 | 3 | An RViz plugin which provides an orthographic camera view controller. In contrast to the default top down ortho view 4 | included in RViz, the view controller allows arbitrary orientations, as well as the ability to lock the view to a 5 | specific plane. 6 | 7 | ## Requirements 8 | 9 | * ROS Jade or Kinetic 10 | 11 | ## Usage 12 | 13 | Once the plugin is installed, open RViz and select "Ortho" from the "Type" dropdown in the "Views" panel. If you want 14 | to lock the view to a specific plane, set the "Plane" option in the view properties. 15 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | rviz_ortho_view_controller 4 | 0.1.0 5 | An orthographic camera view controller for RViz 6 | Andrew Short 7 | 8 | MIT 9 | 10 | catkin 11 | 12 | pluginlib 13 | roscpp 14 | rviz 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /LICENSE.md: -------------------------------------------------------------------------------- 1 | # MIT License 2 | 3 | Copyright © 2016 Andrew Short 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /include/rviz_ortho_view_controller/ortho_view_controller.h: -------------------------------------------------------------------------------- 1 | #ifndef RVIZ_ORTHO_VIEW_CONTROLLER_ORTHO_VIEW_CONTROLLER_H 2 | #define RVIZ_ORTHO_VIEW_CONTROLLER_ORTHO_VIEW_CONTROLLER_H 3 | 4 | #include 5 | 6 | #include 7 | 8 | namespace rviz 9 | { 10 | class EnumProperty; 11 | class FloatProperty; 12 | class QuaternionProperty; 13 | class Shape; 14 | class VectorProperty; 15 | } 16 | 17 | namespace rviz_ortho_view_controller 18 | { 19 | /// An orthographic view controller which can be optionally locked to a plane. 20 | class OrthoViewController : public rviz::FramePositionTrackingViewController 21 | { 22 | Q_OBJECT 23 | 24 | public: 25 | OrthoViewController(); 26 | ~OrthoViewController(); 27 | 28 | void onInitialize() override; 29 | 30 | void handleMouseEvent(rviz::ViewportMouseEvent &e) override; 31 | 32 | void lookAt(const Ogre::Vector3 &p) override; 33 | 34 | void reset() override; 35 | 36 | void mimic(rviz::ViewController *source) override; 37 | 38 | void update(float dt, float ros_dt) override; 39 | 40 | private slots: 41 | void onPlaneChanged(); 42 | 43 | private: 44 | enum Plane 45 | { 46 | PLANE_NONE = 0, 47 | PLANE_XY, 48 | PLANE_XZ, 49 | PLANE_YZ 50 | }; 51 | 52 | /// Gets the plane this view controller is currently locked to. 53 | Plane getPlane() const; 54 | 55 | rviz::EnumProperty *plane_property_; 56 | rviz::VectorProperty *centre_property_; 57 | rviz::QuaternionProperty *orientation_property_; 58 | rviz::FloatProperty *scale_property_; 59 | 60 | bool dragging_ = false; 61 | 62 | std::unique_ptr centre_shape_; 63 | }; 64 | } 65 | 66 | #endif // RVIZ_ORTHO_VIEW_CONTROLLER_ORTHO_VIEW_CONTROLLER_H 67 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(rviz_ortho_view_controller) 3 | 4 | # Package 5 | set(CATKIN_DEPENDENCIES pluginlib roscpp rviz) 6 | 7 | find_package(catkin REQUIRED COMPONENTS ${CATKIN_DEPENDENCIES}) 8 | find_package(Qt5Core) 9 | find_package(Qt5Gui) 10 | 11 | catkin_package(INCLUDE_DIRS include 12 | LIBRARIES ${PROJECT_NAME} 13 | CATKIN_DEPENDS ${CATKIN_DEPENDENCIES}) 14 | 15 | # Build 16 | include_directories(include 17 | ${catkin_INCLUDE_DIRS} 18 | ${CMAKE_CURRENT_BINARY_DIR}) 19 | 20 | # Use the version that rviz used so they are compatible. 21 | if(rviz_QT_VERSION VERSION_LESS "5") 22 | message(STATUS "Using Qt4 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}") 23 | find_package(Qt4 ${rviz_QT_VERSION} EXACT REQUIRED QtCore QtGui) 24 | # pull in all required include dirs, define QT_LIBRARIES, etc. 25 | include(${QT_USE_FILE}) 26 | qt4_wrap_cpp(MOC_FILES include/${PROJECT_NAME}/ortho_view_controller.h) 27 | else() 28 | message(STATUS "Using Qt5 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}") 29 | find_package(Qt5 ${rviz_QT_VERSION} EXACT REQUIRED Core Widgets) 30 | # make target_link_libraries(${QT_LIBRARIES}) pull in all required dependencies 31 | set(QT_LIBRARIES Qt5::Widgets) 32 | qt5_wrap_cpp(MOC_FILES include/${PROJECT_NAME}/ortho_view_controller.h) 33 | endif() 34 | 35 | add_library(${PROJECT_NAME} src/ortho_view_controller.cpp ${MOC_FILES}) 36 | target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${QT_LIBRARIES}) 37 | set_target_properties(${PROJECT_NAME} PROPERTIES COMPILE_FLAGS "-std=c++11") 38 | 39 | # Install 40 | install(DIRECTORY include/${PROJECT_NAME}/ 41 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 42 | FILES_MATCHING PATTERN "*.h") 43 | 44 | install(FILES plugin_description.xml 45 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 46 | 47 | install(TARGETS ${PROJECT_NAME} 48 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 49 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 50 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 51 | -------------------------------------------------------------------------------- /src/ortho_view_controller.cpp: -------------------------------------------------------------------------------- 1 | #include "rviz_ortho_view_controller/ortho_view_controller.h" 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | #include 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | #include 22 | 23 | namespace rviz_ortho_view_controller 24 | { 25 | static const double VIEW_DISTANCE = 500.0; 26 | static const double DEFAULT_SCALE = 100.0; 27 | 28 | static const char *STATUS = "Left-Click: Rotate PY. Middle-Click: Move XY. " 29 | "Right-Click/Mouse Wheel: Zoom. Shift: More options."; 30 | 31 | static const char *STATUS_SHIFT = "Left-Click: Rotate R. Middle-Click: Move XY. " 32 | "Right-Click: Move Z. Mouse Wheel: Zoom."; 33 | 34 | OrthoViewController::OrthoViewController() 35 | : plane_property_(new rviz::EnumProperty("Plane", "none", "Optionally lock the view to a plane", this)) 36 | , centre_property_(new rviz::VectorProperty("Centre", Ogre::Vector3::ZERO, "The focal point of the camera", this)) 37 | , orientation_property_(new rviz::QuaternionProperty("Orientation", Ogre::Quaternion::IDENTITY, "", this)) 38 | , scale_property_(new rviz::FloatProperty("Scale", DEFAULT_SCALE, "How much to scale up the scene", this)) 39 | { 40 | plane_property_->addOption("none", PLANE_NONE); 41 | plane_property_->addOption("XY", PLANE_XY); 42 | plane_property_->addOption("XZ", PLANE_XZ); 43 | plane_property_->addOption("YZ", PLANE_YZ); 44 | 45 | connect(plane_property_, SIGNAL(changed()), this, SLOT(onPlaneChanged())); 46 | } 47 | 48 | OrthoViewController::~OrthoViewController() 49 | { 50 | } 51 | 52 | void OrthoViewController::onInitialize() 53 | { 54 | FramePositionTrackingViewController::onInitialize(); 55 | 56 | camera_->setProjectionType(Ogre::PT_ORTHOGRAPHIC); 57 | // camera_->setFixedYawAxis(false); 58 | 59 | centre_shape_.reset(new rviz::Shape(rviz::Shape::Sphere, context_->getSceneManager(), target_scene_node_)); 60 | centre_shape_->setScale(Ogre::Vector3(0.05f, 0.05f, 0.01f)); 61 | centre_shape_->setColor(1.0f, 1.0f, 0.0f, 0.5f); 62 | centre_shape_->getRootNode()->setVisible(false); 63 | } 64 | 65 | void OrthoViewController::handleMouseEvent(rviz::ViewportMouseEvent &e) 66 | { 67 | bool moved = false; 68 | 69 | int dx = 0; 70 | int dy = 0; 71 | 72 | if (e.shift()) 73 | setStatus(STATUS_SHIFT); 74 | else 75 | setStatus(STATUS); 76 | 77 | if (e.type == QEvent::MouseButtonPress) 78 | { 79 | dragging_ = true; 80 | centre_shape_->getRootNode()->setVisible(true); 81 | } 82 | else if (e.type == QEvent::MouseButtonRelease) 83 | { 84 | dragging_ = false; 85 | centre_shape_->getRootNode()->setVisible(false); 86 | } 87 | else if (e.type == QEvent::MouseMove && dragging_) 88 | { 89 | moved = true; 90 | 91 | dx = e.x - e.last_x; 92 | dy = e.y - e.last_y; 93 | } 94 | 95 | bool rotate_z = e.shift() || getPlane() != PLANE_NONE; 96 | auto rotate_cursor = rotate_z ? Rotate2D : Rotate3D; 97 | 98 | auto rotate = [this](double angle, const Ogre::Vector3 &axis) { 99 | const auto &orientation = orientation_property_->getQuaternion(); 100 | 101 | Ogre::Quaternion q; 102 | q.FromAngleAxis(Ogre::Radian(angle), orientation * axis); 103 | q.normalise(); 104 | 105 | orientation_property_->setQuaternion(q * orientation); 106 | }; 107 | 108 | if (e.left()) 109 | { 110 | setCursor(rotate_cursor); 111 | 112 | if (rotate_z) 113 | { 114 | rotate(0.005 * dx, Ogre::Vector3::UNIT_Z); 115 | } 116 | else 117 | { 118 | rotate(-0.005 * dx, Ogre::Vector3::UNIT_Y); 119 | rotate(-0.005 * dy, Ogre::Vector3::UNIT_X); 120 | } 121 | } 122 | else if (e.middle() || (e.left() && e.shift())) 123 | { 124 | setCursor(MoveXY); 125 | 126 | auto scale = scale_property_->getFloat(); 127 | auto movement = orientation_property_->getQuaternion() * Ogre::Vector3(-dx / scale, dy / scale, 0); 128 | 129 | centre_property_->add(movement); 130 | } 131 | else if (e.right() && !e.shift()) 132 | { 133 | setCursor(Zoom); 134 | scale_property_->multiply(1 - 0.01 * dy); 135 | } 136 | else if (e.right() && e.shift()) 137 | { 138 | setCursor(MoveZ); 139 | 140 | auto scale = scale_property_->getFloat(); 141 | auto movement = orientation_property_->getQuaternion() * Ogre::Vector3(0, 0, dy / scale); 142 | 143 | centre_property_->add(movement); 144 | } 145 | else 146 | { 147 | setCursor(e.shift() ? MoveXY : rotate_cursor); 148 | } 149 | 150 | if (e.wheel_delta) 151 | { 152 | moved = true; 153 | scale_property_->multiply(1 + 0.001 * e.wheel_delta); 154 | } 155 | 156 | if (moved) 157 | { 158 | context_->queueRender(); 159 | emitConfigChanged(); 160 | } 161 | } 162 | 163 | void OrthoViewController::lookAt(const Ogre::Vector3 &p) 164 | { 165 | centre_property_->setVector(p - target_scene_node_->getPosition()); 166 | } 167 | 168 | void OrthoViewController::reset() 169 | { 170 | plane_property_->setString("none"); 171 | centre_property_->setVector(Ogre::Vector3::ZERO); 172 | orientation_property_->setQuaternion(Ogre::Quaternion::IDENTITY); 173 | scale_property_->setFloat(DEFAULT_SCALE); 174 | } 175 | 176 | void OrthoViewController::mimic(rviz::ViewController *source) 177 | { 178 | FramePositionTrackingViewController::mimic(source); 179 | 180 | if (auto *source_ortho = qobject_cast(source)) 181 | { 182 | plane_property_->setString(source_ortho->plane_property_->getString()); 183 | centre_property_->setVector(source_ortho->centre_property_->getVector()); 184 | orientation_property_->setQuaternion(source_ortho->orientation_property_->getQuaternion()); 185 | scale_property_->setFloat(source_ortho->scale_property_->getFloat()); 186 | } 187 | else 188 | { 189 | centre_property_->setVector(source->getCamera()->getPosition()); 190 | } 191 | } 192 | 193 | void OrthoViewController::update(float dt, float ros_dt) 194 | { 195 | FramePositionTrackingViewController::update(dt, ros_dt); 196 | 197 | // Build the projection matrix. 198 | auto scale = scale_property_->getFloat(); 199 | auto width = camera_->getViewport()->getActualWidth() / scale / 2; 200 | auto height = camera_->getViewport()->getActualHeight() / scale / 2; 201 | 202 | auto near = camera_->getNearClipDistance(); 203 | auto far = camera_->getFarClipDistance(); 204 | 205 | Ogre::Matrix4 projection; 206 | rviz::buildScaledOrthoMatrix(projection, -width, width, -height, height, near, far); 207 | 208 | camera_->setCustomProjectionMatrix(true, projection); 209 | 210 | // Set the camera pose. 211 | auto centre = centre_property_->getVector(); 212 | auto orientation = orientation_property_->getQuaternion(); 213 | 214 | camera_->setOrientation(orientation); 215 | camera_->setPosition(centre + orientation * Ogre::Vector3::UNIT_Z * VIEW_DISTANCE); 216 | 217 | centre_shape_->setPosition(centre); 218 | } 219 | 220 | void OrthoViewController::onPlaneChanged() 221 | { 222 | auto plane = getPlane(); 223 | bool locked = plane != PLANE_NONE; 224 | 225 | orientation_property_->setReadOnly(locked); 226 | orientation_property_->setHidden(locked); 227 | 228 | if (locked) 229 | { 230 | auto orientation = Ogre::Quaternion::IDENTITY; 231 | 232 | // TODO fix XZ and YZ planes. 233 | if (plane == PLANE_XZ) 234 | orientation.FromAngleAxis(Ogre::Radian(M_PI / 2), Ogre::Vector3::UNIT_X); 235 | else if (plane == PLANE_YZ) 236 | orientation.FromAngleAxis(Ogre::Radian(M_PI / 2), Ogre::Vector3::UNIT_Y); 237 | 238 | orientation_property_->setQuaternion(orientation); 239 | } 240 | } 241 | 242 | OrthoViewController::Plane OrthoViewController::getPlane() const 243 | { 244 | return static_cast(plane_property_->getOptionInt()); 245 | } 246 | } 247 | 248 | PLUGINLIB_EXPORT_CLASS(rviz_ortho_view_controller::OrthoViewController, rviz::ViewController); 249 | --------------------------------------------------------------------------------