diff --git a/rviz_common/CMakeLists.txt b/rviz_common/CMakeLists.txt index 83cf57023..da0440af8 100644 --- a/rviz_common/CMakeLists.txt +++ b/rviz_common/CMakeLists.txt @@ -108,8 +108,9 @@ set(rviz_common_headers_to_moc include/rviz_common/render_panel.hpp src/rviz_common/scaled_image_widget.hpp src/rviz_common/screenshot_dialog.hpp - include/rviz_common/selection/selection_manager.hpp - include/rviz_common/selection/selection_manager_iface.hpp + include/rviz_common/interaction/selection_manager.hpp + include/rviz_common/interaction/selection_manager_iface.hpp + include/rviz_common/interaction/selection_renderer.hpp src/rviz_common/splash_screen.hpp # src/rviz_common/time_panel.hpp include/rviz_common/tool.hpp @@ -185,9 +186,12 @@ set(rviz_common_source_files src/rviz_common/ros_integration/ros_node_storage.cpp src/rviz_common/scaled_image_widget.cpp src/rviz_common/screenshot_dialog.cpp - # src/rviz_common/selection_panel.cpp - src/rviz_common/selection/selection_handler.cpp - src/rviz_common/selection/selection_manager.cpp + src/rviz_common/selection_panel.cpp + src/rviz_common/interaction/handler_manager.cpp + src/rviz_common/interaction/selection_handler.cpp + src/rviz_common/interaction/selection_manager.cpp + src/rviz_common/interaction/selection_renderer.cpp + src/rviz_common/interaction/view_picker.cpp src/rviz_common/splash_screen.cpp # src/rviz_common/time_panel.cpp src/rviz_common/tool_manager.cpp @@ -346,6 +350,29 @@ if(BUILD_TESTING) if((APPLE AND NOT CMAKE_BUILD_TYPE STREQUAL "Debug") OR DISPLAYPRESENT OR EnableDisplayTests STREQUAL "True") message(STATUS "Enabling tests requiring a display") + ament_add_gmock(selection_manager_test + test/interaction/selection_manager_test.cpp + test/interaction/mock_selection_renderer.hpp + test/interaction/selection_test_fixture.hpp + test/display_context_fixture.cpp) + if(TARGET selection_manager_test) + target_link_libraries(selection_manager_test + rviz_common + rviz_ogre_vendor::OgreMain + rviz_ogre_vendor::OgreOverlay) + endif() + + ament_add_gmock(selection_handler_test + test/interaction/selection_handler_test.cpp + test/interaction/selection_test_fixture.hpp + test/display_context_fixture.cpp) + if(TARGET selection_handler_test) + target_link_libraries(selection_handler_test + rviz_common + rviz_ogre_vendor::OgreMain + rviz_ogre_vendor::OgreOverlay) + endif() + ament_add_gtest(rviz_common_display_test test/display_test.cpp test/mock_display.cpp diff --git a/rviz_common/default.rviz b/rviz_common/default.rviz index a55fc55b1..a228e9bcf 100644 --- a/rviz_common/default.rviz +++ b/rviz_common/default.rviz @@ -9,8 +9,8 @@ Panels: Splitter Ratio: 0.5 Tree Height: 464 # TODO(wjwwood): restore these panels when ported - # - Class: rviz/Selection - # Name: Selection + - Class: rviz_common/Selection + Name: Selection # - Class: rviz/Tool Properties # Expanded: # - /2D Pose Estimate1 @@ -58,7 +58,7 @@ Visualization Manager: # - Class: rviz/Interact # Hide Inactive Objects: true - Class: rviz_default_plugins/MoveCamera - # - Class: rviz/Select + - Class: rviz_default_plugins/Select # - Class: rviz/FocusCamera # - Class: rviz/Measure # - Class: rviz/SetInitialPose diff --git a/rviz_common/include/rviz_common/display_context.hpp b/rviz_common/include/rviz_common/display_context.hpp index dddf62e8e..213731067 100644 --- a/rviz_common/include/rviz_common/display_context.hpp +++ b/rviz_common/include/rviz_common/display_context.hpp @@ -63,12 +63,14 @@ class Node; namespace rviz_common { -namespace selection +namespace interaction { class SelectionManagerIface; +class HandlerManagerIface; +class ViewPickerIface; -} // namespace selection +} // namespace interaction class BitAllocator; class DisplayFactory; @@ -112,9 +114,19 @@ class RVIZ_COMMON_PUBLIC DisplayContext : public QObject /// Return a pointer to the SelectionManager. virtual - rviz_common::selection::SelectionManagerIface * + std::shared_ptr getSelectionManager() const = 0; + /// Return a pointer to the HandlerManager. + virtual + std::shared_ptr + getHandlerManager() const = 0; + + /// Return a pointer to the ViewPicker. + virtual + std::shared_ptr + getViewPicker() const = 0; + /// Return the FrameManager instance. virtual FrameManagerIface * @@ -200,6 +212,14 @@ class RVIZ_COMMON_PUBLIC DisplayContext : public QObject std::shared_ptr getClock() = 0; + virtual + void + lockRender() = 0; + + virtual + void + unlockRender() = 0; + public Q_SLOTS: /// Queue a render. /** diff --git a/rviz_common/include/rviz_common/selection/forwards.hpp b/rviz_common/include/rviz_common/interaction/forwards.hpp similarity index 84% rename from rviz_common/include/rviz_common/selection/forwards.hpp rename to rviz_common/include/rviz_common/interaction/forwards.hpp index 9e993b176..ec1f7975f 100644 --- a/rviz_common/include/rviz_common/selection/forwards.hpp +++ b/rviz_common/include/rviz_common/interaction/forwards.hpp @@ -28,8 +28,8 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef RVIZ_COMMON__SELECTION__FORWARDS_HPP_ -#define RVIZ_COMMON__SELECTION__FORWARDS_HPP_ +#ifndef RVIZ_COMMON__INTERACTION__FORWARDS_HPP_ +#define RVIZ_COMMON__INTERACTION__FORWARDS_HPP_ #include #include @@ -51,16 +51,16 @@ namespace rviz_common { -namespace selection +namespace interaction { -typedef uint32_t CollObjectHandle; -typedef std::vector V_CollObject; -typedef std::vector VV_CollObject; -typedef std::set S_CollObject; +using CollObjectHandle = uint32_t; +using V_CollObject = std::vector; +using VV_CollObject = std::vector; +using S_CollObject = std::set; -typedef std::set S_uint64; -typedef std::vector V_uint64; +using S_uint64 = std::set; +using V_uint64 = std::vector; struct Picked { @@ -74,7 +74,7 @@ struct Picked S_uint64 extra_handles; }; -typedef std::unordered_map M_Picked; +using M_Picked = std::unordered_map; inline uint32_t colorToHandle(Ogre::PixelFormat fmt, uint32_t col) { @@ -99,7 +99,7 @@ inline CollObjectHandle colorToHandle(const Ogre::ColourValue & color) } -} // namespace selection +} // namespace interaction } // namespace rviz_common -#endif // RVIZ_COMMON__SELECTION__FORWARDS_HPP_ +#endif // RVIZ_COMMON__INTERACTION__FORWARDS_HPP_ diff --git a/rviz_common/include/rviz_common/interaction/handler_manager.hpp b/rviz_common/include/rviz_common/interaction/handler_manager.hpp new file mode 100644 index 000000000..aaa18e08f --- /dev/null +++ b/rviz_common/include/rviz_common/interaction/handler_manager.hpp @@ -0,0 +1,99 @@ +/* + * Copyright (c) 2008, Willow Garage, Inc. + * Copyright (c) 2017, Open Source Robotics Foundation, Inc. + * Copyright (c) 2018, Bosch Software Innovations GmbH. + * 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 copyright holder 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. + */ + +#ifndef RVIZ_COMMON__INTERACTION__HANDLER_MANAGER_HPP_ +#define RVIZ_COMMON__INTERACTION__HANDLER_MANAGER_HPP_ + +#include "handler_manager_iface.hpp" + +#include +#include + +#include "rviz_common/interaction/forwards.hpp" +#include "rviz_common/interaction/handler_manager_listener.hpp" +#include "rviz_common/interaction/selection_handler.hpp" +#include "rviz_common/visibility_control.hpp" + + +namespace rviz_common +{ + +class DisplayContext; + +namespace interaction +{ + +class RVIZ_COMMON_PUBLIC HandlerManager + : public HandlerManagerIface +{ +public: + HandlerManager(); + + ~HandlerManager() override; + + void addHandler(CollObjectHandle handle, SelectionHandlerWeakPtr handler) override; + + void removeHandler(CollObjectHandle handle) override; + + SelectionHandlerPtr getHandler(CollObjectHandle handle) override; + + std::unique_lock lock() override; + + std::unique_lock lock(std::defer_lock_t defer_lock) override; + + void addListener(HandlerManagerListener * listener) override; + + void removeListener(HandlerManagerListener * listener) override; + + CollObjectHandle createHandle() override; + + void enableInteraction(bool enable) override; + + bool getInteractionEnabled() const override; + + HandlerRange handlers() override; + +private: + uint32_t uid_counter_; + + bool interaction_enabled_; + + std::recursive_mutex handlers_mutex_; + std::recursive_mutex uid_mutex_; + + M_ObjectHandleToSelectionHandler handlers_; + std::vector listeners_; +}; + +} // namespace interaction +} // namespace rviz_common + +#endif // RVIZ_COMMON__INTERACTION__HANDLER_MANAGER_HPP_ diff --git a/rviz_common/include/rviz_common/interaction/handler_manager_iface.hpp b/rviz_common/include/rviz_common/interaction/handler_manager_iface.hpp new file mode 100644 index 000000000..f969b1863 --- /dev/null +++ b/rviz_common/include/rviz_common/interaction/handler_manager_iface.hpp @@ -0,0 +1,195 @@ +/* + * Copyright (c) 2008, Willow Garage, Inc. + * Copyright (c) 2017, Open Source Robotics Foundation, Inc. + * Copyright (c) 2018, Bosch Software Innovations GmbH. + * 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 copyright holder 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. + */ + +#ifndef RVIZ_COMMON__INTERACTION__HANDLER_MANAGER_IFACE_HPP_ +#define RVIZ_COMMON__INTERACTION__HANDLER_MANAGER_IFACE_HPP_ + +#include +#include +#include +#include + +#include "rviz_common/interaction/handler_manager_listener.hpp" +#include "rviz_common/interaction/selection_handler.hpp" +#include "rviz_common/visibility_control.hpp" + +namespace rviz_common +{ + +class DisplayContext; + +namespace interaction +{ + +using M_ObjectHandleToSelectionHandler = + std::unordered_map; + +/** + * Light-weight container wrapper for M_ObjectHandleToSelectionHandler that allows iterating + * directly over handlers. + */ +class RVIZ_COMMON_PUBLIC HandlerRange +{ +public: + class iterator : public std::iterator + { +public: + // TODO(anhosi) uncrustify does not handle this identation correctly + reference operator*() const + { + return iterator_->second; + } + + bool operator!=(iterator other) + { + return iterator_ != other.iterator_; + } + + iterator & operator++() + { + ++iterator_; + return *this; + } + + iterator operator++(int) + { + return iterator(iterator_++); + } + +private: + explicit iterator(M_ObjectHandleToSelectionHandler::iterator it) + : iterator_(it) {} + + M_ObjectHandleToSelectionHandler::iterator iterator_; + + friend class HandlerRange; + }; + + explicit HandlerRange(M_ObjectHandleToSelectionHandler & handlers) + : handlers_(handlers) {} + + iterator begin() + { + return iterator(handlers_.begin()); + } + + iterator end() + { + return iterator(handlers_.end()); + } + +private: + M_ObjectHandleToSelectionHandler & handlers_; +}; + + +/** + * \brief The HandlerManagerIface manages selection handlers + * + * It is mainly used by the SelectionManager + * + * The HandlerManager must be locked (using one of the lock methods) if exclusive access to the + * handlers is necessary. + */ +class RVIZ_COMMON_PUBLIC HandlerManagerIface +{ +public: + virtual ~HandlerManagerIface() = default; + + /** + * Registers a new handle-handler pair. Locks internally to guarantee exclusive access. + * \param handle + * \param handler + */ + virtual void addHandler(CollObjectHandle handle, SelectionHandlerWeakPtr handler) = 0; + + /** + * Removes a handle-handler pair identified by its handle. Locks internally to guarantee + * exclusive access + * \param handle + */ + virtual void removeHandler(CollObjectHandle handle) = 0; + + /// obtains the handler for a handle + virtual SelectionHandlerPtr getHandler(CollObjectHandle handle) = 0; + + /** + * Locks the handlers container + * \return the lock for the handlers (already locked) + */ + virtual std::unique_lock lock() = 0; + + /** + * Returns a lock for the handlers container that is not yet locked + * \param defer_lock std::defer_lock + * \return the lock for the handlers (not yet locked) + */ + virtual std::unique_lock lock(std::defer_lock_t defer_lock) = 0; + + /** + * Registers a listener that is notified for every handle that is removed + * \param listener + */ + virtual void addListener(HandlerManagerListener * listener) = 0; + + /** + * Removes a listeners. + * \param listener + */ + virtual void removeListener(HandlerManagerListener * listener) = 0; + + /** + * Creates a new unique handle. + * \return new handle + */ + virtual CollObjectHandle createHandle() = 0; + + /// Tells all handlers that interactive mode is active/inactive. + virtual void enableInteraction(bool enable) = 0; + + /// Retrieves the current interaction mode (active/inactive). + virtual bool getInteractionEnabled() const = 0; + + /** + * Gives access to all managed handlers in form of a container that can be accessed via a + * forward iterator. + * Its usage needs to be protected by explicitly calling lock + * \return container of all handlers + */ + virtual HandlerRange handlers() = 0; +}; + +using HandlerManagerIfacePtr = std::shared_ptr; + +} // namespace interaction +} // namespace rviz_common + +#endif // RVIZ_COMMON__INTERACTION__HANDLER_MANAGER_IFACE_HPP_ diff --git a/rviz_common/include/rviz_common/interaction/handler_manager_listener.hpp b/rviz_common/include/rviz_common/interaction/handler_manager_listener.hpp new file mode 100644 index 000000000..13147442f --- /dev/null +++ b/rviz_common/include/rviz_common/interaction/handler_manager_listener.hpp @@ -0,0 +1,53 @@ +/* + * Copyright (c) 2018, Bosch Software Innovations GmbH. + * 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 copyright holder 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. + */ + +#ifndef RVIZ_COMMON__INTERACTION__HANDLER_MANAGER_LISTENER_HPP_ +#define RVIZ_COMMON__INTERACTION__HANDLER_MANAGER_LISTENER_HPP_ + +#include "rviz_common/interaction/forwards.hpp" +#include "rviz_common/visibility_control.hpp" + +namespace rviz_common +{ + +namespace interaction +{ + +class RVIZ_COMMON_PUBLIC HandlerManagerListener +{ +public: + virtual ~HandlerManagerListener() = default; + + virtual void onHandlerRemoved(CollObjectHandle handle) = 0; +}; + +} // namespace interaction +} // namespace rviz_common + +#endif // RVIZ_COMMON__INTERACTION__HANDLER_MANAGER_LISTENER_HPP_ diff --git a/rviz_common/include/rviz_common/selection/selection_handler.hpp b/rviz_common/include/rviz_common/interaction/selection_handler.hpp similarity index 56% rename from rviz_common/include/rviz_common/selection/selection_handler.hpp rename to rviz_common/include/rviz_common/interaction/selection_handler.hpp index 2a737e307..0b1d218d0 100644 --- a/rviz_common/include/rviz_common/selection/selection_handler.hpp +++ b/rviz_common/include/rviz_common/interaction/selection_handler.hpp @@ -28,8 +28,8 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef RVIZ_COMMON__SELECTION__SELECTION_HANDLER_HPP_ -#define RVIZ_COMMON__SELECTION__SELECTION_HANDLER_HPP_ +#ifndef RVIZ_COMMON__INTERACTION__SELECTION_HANDLER_HPP_ +#define RVIZ_COMMON__INTERACTION__SELECTION_HANDLER_HPP_ #include #include @@ -49,18 +49,19 @@ # pragma GCC diagnostic pop #endif -#include "./forwards.hpp" +#include "rviz_common/interaction/forwards.hpp" #include "rviz_common/interactive_object.hpp" #include "rviz_common/properties/property.hpp" #include "rviz_common/viewport_mouse_event.hpp" #include "rviz_common/visibility_control.hpp" + namespace Ogre { class WireBoundingBox; class SceneNode; class MovableObject; -} +} // namespace Ogre namespace rviz_common { @@ -68,27 +69,48 @@ namespace rviz_common class DisplayContext; class ViewportMouseEvent; -namespace selection +namespace interaction { -typedef std::vector V_AABB; +using V_AABB = std::vector; + +template +std::weak_ptr weak_from_this(T * pointer) +{ + return pointer->shared_from_this(); +} -class RVIZ_COMMON_PUBLIC SelectionHandler +/// Use this function to create a SelectionHandler +/** + * This function creates a shared_ptr for any SelectionHandler, registering a handle with the + * HandlerManager. + * + * Note: When migrating from rviz to rviz2, use this function to create a SelectionHandler + * instead of constructors. + * + * @tparam T template type of SelectionHandler to be created + * @tparam Args placeholder types for constructor parameter types + * @param arguments arguments used in the constructor of the SelectionHandler to be created + * @return + */ +template +std::shared_ptr createSelectionHandler(Args ... arguments) +{ + auto selection_handler = std::shared_ptr(new T(arguments ...)); + selection_handler->registerHandle(); + return selection_handler; +} + +class RVIZ_COMMON_PUBLIC SelectionHandler : public std::enable_shared_from_this { public: - explicit SelectionHandler(DisplayContext * context); virtual ~SelectionHandler(); - void - addTrackedObjects(Ogre::SceneNode * node); - void - addTrackedObject(Ogre::MovableObject * object); - void - removeTrackedObject(Ogre::MovableObject * object); + void addTrackedObjects(Ogre::SceneNode * node); + void addTrackedObject(Ogre::MovableObject * object); + void removeTrackedObject(Ogre::MovableObject * object); - virtual - void - updateTrackedBoxes(); + virtual void updateTrackedBoxes(); /// Override to create properties of the given picked object(s). /** @@ -97,9 +119,8 @@ class RVIZ_COMMON_PUBLIC SelectionHandler * * This base implementation does nothing. */ - virtual - void - createProperties(const Picked & obj, rviz_common::properties::Property * parent_property); + virtual void createProperties( + const Picked & obj, rviz_common::properties::Property * parent_property); /// Destroy all properties for the given picked object(s). /** @@ -108,9 +129,8 @@ class RVIZ_COMMON_PUBLIC SelectionHandler * If createProperties() adds all the top-level properties to properties_, * there is no need to override this in a subclass. */ - virtual - void - destroyProperties(const Picked & obj, rviz_common::properties::Property * parent_property); + virtual void destroyProperties( + const Picked & obj, rviz_common::properties::Property * parent_property); /** @brief Override to update property values. * @@ -122,45 +142,31 @@ class RVIZ_COMMON_PUBLIC SelectionHandler * * This base implementation does nothing. */ - virtual - void - updateProperties(); + virtual void updateProperties(); /// Override to indicate if an additional render pass is required. virtual bool needsAdditionalRenderPass(uint32_t pass); /// Override to hook before a render pass. - virtual - void - preRenderPass(uint32_t pass); + virtual void preRenderPass(uint32_t pass); /// Override to hook after a render pass. - virtual - void - postRenderPass(uint32_t pass); + virtual void postRenderPass(uint32_t pass); /// Get the AABBs. - virtual - void - getAABBs(const Picked & obj, V_AABB & aabbs); + virtual V_AABB getAABBs(const Picked & obj); /// Override to get called on selection. - virtual - void - onSelect(const Picked & obj); + virtual void onSelect(const Picked & obj); /// Override to get called on deselection. - virtual - void - onDeselect(const Picked & obj); + virtual void onDeselect(const Picked & obj); /// Set an object to listen to mouse events and other interaction calls. /** * Events occur during use of the 'interact' tool. */ - virtual - void - setInteractiveObject(InteractiveObjectWPtr object); + virtual void setInteractiveObject(InteractiveObjectWPtr object); /// Get the object to listen to mouse events and other interaction calls. /** @@ -169,65 +175,91 @@ class RVIZ_COMMON_PUBLIC SelectionHandler * long periods because it may cause something visual to stick * around after it was meant to be destroyed. */ - virtual - InteractiveObjectWPtr - getInteractiveObject(); + virtual InteractiveObjectWPtr getInteractiveObject(); /// Get CollObjectHandle. - CollObjectHandle - getHandle() const; + CollObjectHandle getHandle() const; + + struct Handles + { + Handles(CollObjectHandle _handle, uint64_t _extra_handle) + : handle(_handle), extra_handle(_extra_handle) {} + + bool operator==(const Handles & rhs) const + { + return handle == rhs.handle && extra_handle == rhs.extra_handle; + } + + bool operator<(const Handles & rhs) const + { + if (handle < rhs.handle) { + return true; + } else if (handle > rhs.handle) { + return false; + } else if (extra_handle < rhs.extra_handle) { + return true; + } + return false; + } + + bool operator<=(const Handles & rhs) const + { + return *this == rhs || *this < rhs; + } + + CollObjectHandle handle; + uint64_t extra_handle; + }; + + struct SelectionBox + { + SelectionBox(Ogre::SceneNode * _node, Ogre::WireBoundingBox * _box) + : scene_node(_node), box(_box) {} + + Ogre::SceneNode * scene_node; + Ogre::WireBoundingBox * box; + }; protected: + explicit SelectionHandler(DisplayContext * context); + + void registerHandle(); + /// Create or update a box for the given handle-int pair, with the box specified by aabb. - void - createBox( - const std::pair & handles, + void createBox( + const Handles & handles, const Ogre::AxisAlignedBox & aabb, const std::string & material_name); /// Destroy the box associated with the given handle-int pair, if there is one. - void - destroyBox(const std::pair & handles); + void destroyBox(const Handles & handles); + + void setBoxVisibility(bool visible); QList properties_; - typedef std::map, - std::pair> M_HandleToBox; + using M_HandleToBox = std::map; M_HandleToBox boxes_; DisplayContext * context_; - typedef std::set S_Movable; + using S_Movable = std::set; S_Movable tracked_objects_; - // TODO(wjwwood): move implementation to cpp file. class Listener : public Ogre::MovableObject::Listener { public: // TODO(wjwwood): uncrustify doesn't handle this indentation correctly. - explicit Listener(SelectionHandler * handler) - : handler_(handler) - {} + explicit Listener(SelectionHandler * handler); - virtual - void - objectMoved(Ogre::MovableObject * object) - { - Q_UNUSED(object); - handler_->updateTrackedBoxes(); - } + void objectMoved(Ogre::MovableObject * object) override; - virtual - void - objectDestroyed(Ogre::MovableObject * object) - { - handler_->removeTrackedObject(object); - } + void objectDestroyed(Ogre::MovableObject * object) override; SelectionHandler * handler_; }; - typedef std::shared_ptr ListenerPtr; + using ListenerPtr = std::shared_ptr; ListenerPtr listener_; InteractiveObjectWPtr interactive_object_; @@ -239,13 +271,17 @@ class RVIZ_COMMON_PUBLIC SelectionHandler CollObjectHandle pick_handle_; friend class SelectionManager; + template + friend typename std::shared_ptr // typename is used only to make uncrustify happy + rviz_common::interaction::createSelectionHandler(Args ... arguments); }; -typedef std::shared_ptr SelectionHandlerPtr; -typedef std::vector V_SelectionHandler; -typedef std::set S_SelectionHandler; +using SelectionHandlerPtr = std::shared_ptr; +using SelectionHandlerWeakPtr = std::weak_ptr; +using V_SelectionHandler = std::vector; +using S_SelectionHandler = std::set; -} // namespace selection +} // namespace interaction } // namespace rviz_common -#endif // RVIZ_COMMON__SELECTION__SELECTION_HANDLER_HPP_ +#endif // RVIZ_COMMON__INTERACTION__SELECTION_HANDLER_HPP_ diff --git a/rviz_common/include/rviz_common/interaction/selection_manager.hpp b/rviz_common/include/rviz_common/interaction/selection_manager.hpp new file mode 100644 index 000000000..89ec1715e --- /dev/null +++ b/rviz_common/include/rviz_common/interaction/selection_manager.hpp @@ -0,0 +1,246 @@ +/* + * Copyright (c) 2008, Willow Garage, Inc. + * Copyright (c) 2017, Open Source Robotics Foundation, Inc. + * Copyright (c) 2018, Bosch Software Innovations GmbH. + * 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. + */ + +#ifndef RVIZ_COMMON__INTERACTION__SELECTION_MANAGER_HPP_ +#define RVIZ_COMMON__INTERACTION__SELECTION_MANAGER_HPP_ + +#include "selection_manager_iface.hpp" + +#include +#include +#include +#include + +#ifndef _WIN32 +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wunused-parameter" +# pragma GCC diagnostic ignored "-Wpedantic" +#endif + +#include +#include + +#ifndef _WIN32 +# pragma GCC diagnostic pop +#endif + +#include // NOLINT: cpplint is unable to handle the include order here + +#include "rviz_common/interaction/forwards.hpp" +#include "rviz_common/interaction/handler_manager_iface.hpp" +#include "rviz_common/interaction/handler_manager_listener.hpp" +#include "rviz_common/interaction/selection_handler.hpp" +#include "rviz_common/visibility_control.hpp" + +namespace Ogre +{ +class Rectangle2D; +} + +namespace rviz_common +{ + +namespace properties +{ +class PropertyTreeModel; +} + +class VisualizationManager; + +namespace interaction +{ +class SelectionRenderer; +struct SelectionRectangle; +struct RenderTexture; + +class RVIZ_COMMON_PUBLIC SelectionManager + : public SelectionManagerIface, public HandlerManagerListener +{ + Q_OBJECT + +public: + SelectionManager(DisplayContext * manager, std::shared_ptr renderer); + + explicit SelectionManager(DisplayContext * manager); + + ~SelectionManager() override; + + void initialize() override; + + /// Control the highlight box being displayed while selecting. + void highlight(rviz_rendering::RenderWindow * window, int x1, int y1, int x2, int y2) override; + + void removeHighlight() override; + + /// Select all objects in bounding box. + void select( + rviz_rendering::RenderWindow * window, + int x1, + int y1, + int x2, + int y2, + SelectType type) override; + + void update() override; + + const M_Picked & getSelection() const override; + + static Ogre::ColourValue handleToColor(CollObjectHandle handle); + + // static CollObjectHandle colourToHandle( const Ogre::ColourValue & color ); + + // TODO(Martin-Idel-SI): maybe move those methods to SelectionRenderer + static void setPickColor(const Ogre::ColourValue & color, Ogre::SceneNode * node); + + static void setPickColor(const Ogre::ColourValue & color, Ogre::MovableObject * object); + + static void setPickHandle(CollObjectHandle handle, Ogre::SceneNode * node); + + static void setPickHandle(CollObjectHandle handle, Ogre::MovableObject * object); + + static void setPickData( + CollObjectHandle handle, + const Ogre::ColourValue & color, + Ogre::SceneNode * node); + + static void setPickData( + CollObjectHandle handle, + const Ogre::ColourValue & color, + Ogre::MovableObject * object); + + /// Tell the view controller to look at the selection. + void focusOnSelection() override; + + /// Change the size of the off-screen selection buffer texture. + void setTextureSize(unsigned size) override; + + rviz_common::properties::PropertyTreeModel * getPropertyModel() override; + + void onHandlerRemoved(CollObjectHandle handle) override; + +private Q_SLOTS: + /// Call updateProperties() on all SelectionHandlers in the current selection. + void updateProperties(); + +private: + /** + * \return handles of all objects in the given bounding box + * \param single_render_pass only perform one rendering pass + * (point cloud selecting won't work) + */ + void pick( + rviz_rendering::RenderWindow * window, + int x1, + int y1, + int x2, + int y2, + M_Picked & results, + bool single_render_pass = false); + + /// Set the list of currently selected objects. + void setSelection(const M_Picked & objs); + + void addSelection(const M_Picked & objs); + + void removeSelection(const M_Picked & objs); + + void selectionAdded(const M_Picked & added); + + void selectionRemoved(const M_Picked & removed); + + std::pair addSelectedObject(const Picked & obj); + + void removeSelectedObject(const Picked & obj); + + void setHighlightRect(Ogre::Viewport * viewport, int x1, int y1, int x2, int y2); + + /// Render to a texture for one of the picking passes and unpack the resulting pixels into + // pixel_buffer_. + void renderAndUnpack( + rviz_rendering::RenderWindow * window, + const SelectionRectangle & selection_rectangle, + uint32_t pass); + + /// Internal render function to render to a texture and read the pixels back out. + void render( + rviz_rendering::RenderWindow * window, + const SelectionRectangle & selection_rectangle, + const RenderTexture & render_texture, + Ogre::PixelBox & dst_box); + + /// Unpacks a pixelbox into pixel_buffer_ + void unpackColors(const Ogre::PixelBox & box); + + void setUpSlots(); + + + DisplayContext * context_; + std::shared_ptr handler_manager_; + + std::recursive_mutex selection_mutex_; + + bool highlight_enabled_; + + struct Highlight + { + int x1; + int y1; + int x2; + int y2; + Ogre::Viewport * viewport; + }; + Highlight highlight_; + + M_Picked selection_; + + // If you want to change this number to something > 3 you must provide more + // width for extra handles in the Picked structure (currently a u64) + static constexpr uint32_t kNumRenderTextures_ = 2; + std::array render_textures_; + std::array pixel_boxes_; + + Ogre::Rectangle2D * highlight_rectangle_; + Ogre::SceneNode * highlight_node_; + Ogre::Camera * camera_; + + V_CollObject pixel_buffer_; + + uint32_t texture_size_; + + rviz_common::properties::PropertyTreeModel * property_model_; + + std::shared_ptr renderer_; +}; + +} // namespace interaction +} // namespace rviz_common + +#endif // RVIZ_COMMON__INTERACTION__SELECTION_MANAGER_HPP_ diff --git a/rviz_common/include/rviz_common/interaction/selection_manager_iface.hpp b/rviz_common/include/rviz_common/interaction/selection_manager_iface.hpp new file mode 100644 index 000000000..1f02fd20c --- /dev/null +++ b/rviz_common/include/rviz_common/interaction/selection_manager_iface.hpp @@ -0,0 +1,106 @@ +/* + * Copyright (c) 2008, Willow Garage, Inc. + * Copyright (c) 2017, Open Source Robotics Foundation, 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. + */ + +#ifndef RVIZ_COMMON__INTERACTION__SELECTION_MANAGER_IFACE_HPP_ +#define RVIZ_COMMON__INTERACTION__SELECTION_MANAGER_IFACE_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include // NOLINT: cpplint is unable to handle the include order here + +#include "rviz_common/interaction/forwards.hpp" +#include "rviz_common/interaction/selection_handler.hpp" +#include "rviz_common/visibility_control.hpp" + +namespace rviz_rendering +{ +class RenderWindow; +} + +namespace rviz_common +{ +namespace properties +{ +class PropertyTreeModel; +} + +class DisplayContext; + +namespace interaction +{ + +class RVIZ_COMMON_PUBLIC SelectionManagerIface : public QObject +{ + Q_OBJECT + +public: + enum SelectType + { + Add, + Remove, + Replace + }; + + virtual void initialize() = 0; + + /// Control the highlight box being displayed while selecting. + virtual void + highlight(rviz_rendering::RenderWindow * window, int x1, int y1, int x2, int y2) = 0; + + virtual void removeHighlight() = 0; + + /// Select all objects in bounding box. + virtual void select( + rviz_rendering::RenderWindow * window, int x1, int y1, int x2, int y2, SelectType type) = 0; + + virtual void update() = 0; + + virtual const M_Picked & getSelection() const = 0; + + /// Tell the view controller to look at the selection. + virtual void focusOnSelection() = 0; + + /// Change the size of the off-screen selection buffer texture. + virtual void setTextureSize(unsigned size) = 0; + + virtual rviz_common::properties::PropertyTreeModel * getPropertyModel() = 0; +}; + +} // namespace interaction +} // namespace rviz_common + +#endif // RVIZ_COMMON__INTERACTION__SELECTION_MANAGER_IFACE_HPP_ diff --git a/rviz_common/include/rviz_common/interaction/selection_renderer.hpp b/rviz_common/include/rviz_common/interaction/selection_renderer.hpp new file mode 100644 index 000000000..aca7ed83f --- /dev/null +++ b/rviz_common/include/rviz_common/interaction/selection_renderer.hpp @@ -0,0 +1,186 @@ +/* + * Copyright (c) 2017, Open Source Robotics Foundation, Inc. + * Copyright (c) 2018, Bosch Software Innovations GmbH. + * 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 copyright holder 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. + */ + +#ifndef RVIZ_COMMON__INTERACTION__SELECTION_RENDERER_HPP_ +#define RVIZ_COMMON__INTERACTION__SELECTION_RENDERER_HPP_ + +#include +#include +#include + +#ifndef _WIN32 +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wunused-parameter" +#endif + +#include +#include + +#ifndef _WIN32 +# pragma GCC diagnostic pop +#endif + +#include "rviz_rendering/render_window.hpp" +#include "rviz_common/interaction/handler_manager_iface.hpp" +#include "rviz_common/visibility_control.hpp" + +namespace rviz_common +{ +class DisplayContext; + +namespace interaction +{ + +struct SelectionRectangle +{ + SelectionRectangle(int x1, int y1, int x2, int y2) + : x1(x1), x2(x2), y1(y1), y2(y2) + {} + + ~SelectionRectangle() = default; + + int x1; + int x2; + int y1; + int y2; +}; + +struct Dimensions +{ + Dimensions() + : width(0), height(0) {} + Dimensions(float width, float height) + : width(width), height(height) {} + + float width; + float height; +}; + +struct RenderTexture +{ + RenderTexture( + Ogre::TexturePtr tex, + Dimensions dimensions, + std::string material_scheme) + : tex(tex), + dimensions(dimensions), + material_scheme(material_scheme) + {} + + ~RenderTexture() = default; + + Ogre::TexturePtr tex; + Dimensions dimensions; + std::string material_scheme; +}; + +class SelectionRenderer + : public Ogre::MaterialManager::Listener, public Ogre::RenderQueueListener +{ +public: + RVIZ_COMMON_PUBLIC explicit SelectionRenderer(rviz_common::DisplayContext * context); + ~SelectionRenderer() override = default; + + RVIZ_COMMON_PUBLIC + virtual void initialize(Ogre::Camera * camera, Ogre::SceneManager * scene_manager); + + RVIZ_COMMON_PUBLIC + virtual void render( + rviz_rendering::RenderWindow * window, + SelectionRectangle rectangle, + RenderTexture texture, + HandlerRange handlers, + Ogre::PixelBox & dst_box); + + /// Implementation for Ogre::RenderQueueListener. + RVIZ_COMMON_PUBLIC + void renderQueueStarted( + uint8_t queueGroupId, + const std::string & invocation, + bool & skipThisInvocation) override; + + /// Implementation for Ogre::MaterialManager::Listener + /// If a material does not support the picking scheme, paint it black. + RVIZ_COMMON_PUBLIC + Ogre::Technique * handleSchemeNotFound( + unsigned short scheme_index, // NOLINT: Ogre decides the use of unsigned short + const Ogre::String & scheme_name, + Ogre::Material * original_material, + unsigned short lod_index, // NOLINT: Ogre decides the use of unsigned short + const Ogre::Renderable * rend) override; + +private: + void sanitizeRectangle(Ogre::Viewport * viewport, SelectionRectangle & rectangle); + + void configureCamera(Ogre::Viewport * viewport, const SelectionRectangle & rectangle) const; + float getRelativeCoordinate(float coord, int dimension) const; + + template + T clamp(T value, T min, T max) const; + + Ogre::RenderTexture * setupRenderTexture( + Ogre::HardwarePixelBufferSharedPtr pixel_buffer, + RenderTexture texture) const; + + Ogre::Viewport * setupRenderViewport( + Ogre::RenderTexture * render_texture, + Ogre::Viewport * viewport, + const SelectionRectangle & rectangle, + Dimensions texture_dimensions); + + Dimensions getRenderDimensions( + const SelectionRectangle & rectangle, + const Dimensions & texture_dim) const; + + void renderToTexture(Ogre::RenderTexture * render_texture, Ogre::Viewport * window_viewport); + + void blitToMemory( + Ogre::HardwarePixelBufferSharedPtr & pixel_buffer, + const Ogre::Viewport * render_viewport, + Ogre::PixelBox & dst_box) const; + + rviz_common::DisplayContext * context_; + + Ogre::Camera * camera_; + Ogre::SceneManager * scene_manager_; + + Ogre::MaterialPtr fallback_pick_material_; + Ogre::Technique * fallback_pick_technique_; + Ogre::Technique * fallback_black_technique_; + Ogre::Technique * fallback_depth_technique_; + Ogre::Technique * fallback_pick_cull_technique_; + Ogre::Technique * fallback_black_cull_technique_; + Ogre::Technique * fallback_depth_cull_technique_; +}; + +} // namespace interaction +} // namespace rviz_common + +#endif // RVIZ_COMMON__INTERACTION__SELECTION_RENDERER_HPP_ diff --git a/rviz_common/include/rviz_common/selection/selection_manager_iface.hpp b/rviz_common/include/rviz_common/interaction/view_picker.hpp similarity index 51% rename from rviz_common/include/rviz_common/selection/selection_manager_iface.hpp rename to rviz_common/include/rviz_common/interaction/view_picker.hpp index a8a93e57e..1f1135b24 100644 --- a/rviz_common/include/rviz_common/selection/selection_manager_iface.hpp +++ b/rviz_common/include/rviz_common/interaction/view_picker.hpp @@ -1,6 +1,7 @@ /* * Copyright (c) 2008, Willow Garage, Inc. * Copyright (c) 2017, Open Source Robotics Foundation, Inc. + * Copyright (c) 2018, Bosch Software Innovations GmbH. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -28,16 +29,12 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef RVIZ_COMMON__SELECTION__SELECTION_MANAGER_IFACE_HPP_ -#define RVIZ_COMMON__SELECTION__SELECTION_MANAGER_IFACE_HPP_ +#ifndef RVIZ_COMMON__INTERACTION__VIEW_PICKER_HPP_ +#define RVIZ_COMMON__INTERACTION__VIEW_PICKER_HPP_ + +#include "rviz_common/interaction/view_picker_iface.hpp" -#include #include -#include -#include -#include -#include -#include #include #ifndef _WIN32 @@ -55,17 +52,10 @@ #include // NOLINT: cpplint is unable to handle the include order here -#include "./forwards.hpp" -#include "./selection_handler.hpp" +#include "rviz_common/interaction/forwards.hpp" +#include "rviz_common/interaction/selection_handler.hpp" #include "rviz_common/visibility_control.hpp" -namespace rclcpp -{ - -class PublisherBase; - -} - namespace Ogre { @@ -73,123 +63,54 @@ class Rectangle2D; } // namespace Ogre -namespace rviz_common +namespace rviz_rendering { -namespace properties -{ +class RenderWindow; -class PropertyTreeModel; +} // namespace rviz_rendering -} +namespace rviz_common +{ -class VisualizationManager; +class DisplayContext; -namespace selection +namespace interaction { -class RVIZ_COMMON_PUBLIC SelectionManagerIface - : public QObject, - public Ogre::MaterialManager::Listener, - public Ogre::RenderQueueListener -{ - Q_OBJECT +class HandlerManagerIface; +class SelectionRenderer; +struct SelectionRectangle; +struct RenderTexture; +class RVIZ_COMMON_PUBLIC ViewPicker + : public ViewPickerIface +{ public: - enum SelectType - { - Add, - Remove, - Replace - }; - - virtual void - initialize() = 0; - - /// Enables or disables publishing of picking and depth rendering images. - virtual void - setDebugMode(bool debug) = 0; - - virtual void - clearHandlers() = 0; - - virtual void - addObject(CollObjectHandle obj, SelectionHandler * handler) = 0; - - virtual void - removeObject(CollObjectHandle obj) = 0; - - /// Control the highlight box being displayed while selecting. - virtual void - highlight(Ogre::Viewport * viewport, int x1, int y1, int x2, int y2) = 0; - - virtual void - removeHighlight() = 0; - - /// Select all objects in bounding box. - virtual void - select(Ogre::Viewport * viewport, int x1, int y1, int x2, int y2, SelectType type) = 0; - - /** - * \return handles of all objects in the given bounding box - * \param single_render_pass only perform one rendering pass - * (point cloud selecting won't work) - */ - virtual void - pick( - Ogre::Viewport * viewport, - int x1, - int y1, - int x2, - int y2, - M_Picked & results, - bool single_render_pass = false) = 0; - - virtual void update() = 0; - - /// Set the list of currently selected objects. - virtual void setSelection(const M_Picked & objs) = 0; - - virtual void addSelection(const M_Picked & objs) = 0; - - virtual void removeSelection(const M_Picked & objs) = 0; - - virtual const M_Picked & getSelection() const = 0; + ViewPicker(DisplayContext * manager, std::shared_ptr renderer); - virtual SelectionHandler * getHandler(CollObjectHandle obj) = 0; + explicit ViewPicker(DisplayContext * manager); - /// If a material does not support the picking scheme, paint it black. - Ogre::Technique * handleSchemeNotFound( - unsigned short scheme_index, // NOLINT: Ogre decides the use of unsigned short - const Ogre::String & scheme_name, - Ogre::Material * original_material, - unsigned short lod_index, // NOLINT: Ogre decides the use of unsigned short - const Ogre::Renderable * rend) override = 0; + ~ViewPicker() override; - /// Create a new unique handle. - virtual CollObjectHandle createHandle() = 0; - - /// Tell all handlers that interactive mode is active/inactive. - virtual void enableInteraction(bool enable) = 0; - - virtual bool getInteractionEnabled() const = 0; - - /// Tell the view controller to look at the selection. - virtual void focusOnSelection() = 0; - - /// Change the size of the off-screen selection buffer texture. - virtual void setTextureSize(unsigned size) = 0; + void + initialize() override; /// Return true if the point at x, y in the viewport is showing an object, false otherwise. /** * If it is showing an object, result will be changed to contain the 3D point * corresponding to it. */ - virtual bool get3DPoint( - Ogre::Viewport * viewport, + bool get3DPoint( + RenderPanel * panel, int x, int y, - Ogre::Vector3 & result_point) = 0; + Ogre::Vector3 & result_point) override; + +private: + void setDepthTextureSize(unsigned width, unsigned height); + + void capTextureSize(unsigned int & width, unsigned int & height); /// Gets the 3D points in a box around a point in a view port. /** @@ -206,19 +127,17 @@ class RVIZ_COMMON_PUBLIC SelectionManagerIface * so if skip_missing is false, this will always return true if * width and height are > 0. */ - virtual bool get3DPatch( - Ogre::Viewport * viewport, + bool get3DPatch( + RenderPanel * panel, int x, int y, unsigned width, unsigned height, bool skip_missing, - std::vector & result_points) = 0; - + std::vector & result_points); - /// Renders a depth image in a box around a point in a view port. /** - * \param[in] viewport Rendering area clicked on. + * \param[in] panel Rendering area clicked on. * \param[in] x x coordinate of upper-left corner of box. * \param[in] y y coordinate of upper-left corner of box. * \param[in] width The width of the rendered box in pixels. @@ -229,24 +148,42 @@ class RVIZ_COMMON_PUBLIC SelectionManagerIface * texture buffer succeeds. * Failure likely indicates a pretty serious problem. */ - virtual bool getPatchDepthImage( - Ogre::Viewport * viewport, + void getPatchDepthImage( + RenderPanel * panel, int x, int y, unsigned width, unsigned height, - std::vector & depth_vector) = 0; + std::vector & depth_vector); + + Ogre::Vector3 + computeForOrthogonalProjection(float depth, Ogre::Real screenx, Ogre::Real screeny) const; + + Ogre::Vector3 computeForPerspectiveProjection( + float depth, Ogre::Real screenx, Ogre::Real screeny) const; + /// Renders a depth image in a box around a point in a view port. + /// Internal render function to render to a texture and read the pixels back out. + void render( + rviz_rendering::RenderWindow * window, + const SelectionRectangle & selection_rectangle, + const RenderTexture & render_texture, + Ogre::PixelBox & dst_box); + + DisplayContext * context_; + std::shared_ptr handler_manager_; + + // Graphics card -based depth finding of clicked points. + Ogre::TexturePtr depth_render_texture_; + uint32_t depth_texture_width_, depth_texture_height_; + + Ogre::PixelBox depth_pixel_box_; - /// Implementation for Ogre::RenderQueueListener. - void renderQueueStarted( - uint8_t queueGroupId, - const std::string & invocation, - bool & skipThisInvocation) override = 0; + Ogre::Camera * camera_; - virtual rviz_common::properties::PropertyTreeModel * getPropertyModel() = 0; + std::shared_ptr renderer_; }; -} // namespace selection +} // namespace interaction } // namespace rviz_common -#endif // RVIZ_COMMON__SELECTION__SELECTION_MANAGER_IFACE_HPP_ +#endif // RVIZ_COMMON__INTERACTION__VIEW_PICKER_HPP_ diff --git a/rviz_common/include/rviz_common/interaction/view_picker_iface.hpp b/rviz_common/include/rviz_common/interaction/view_picker_iface.hpp new file mode 100644 index 000000000..107db4332 --- /dev/null +++ b/rviz_common/include/rviz_common/interaction/view_picker_iface.hpp @@ -0,0 +1,72 @@ +/* + * Copyright (c) 2008, Willow Garage, Inc. + * Copyright (c) 2017, Open Source Robotics Foundation, 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. + */ + +#ifndef RVIZ_COMMON__INTERACTION__VIEW_PICKER_IFACE_HPP_ +#define RVIZ_COMMON__INTERACTION__VIEW_PICKER_IFACE_HPP_ + +#include "rviz_common/visibility_control.hpp" + +namespace Ogre +{ +class Viewport; +class Vector3; +} // namespace Ogre + + +namespace rviz_common +{ +class RenderPanel; + +namespace interaction +{ + +class RVIZ_COMMON_PUBLIC ViewPickerIface +{ +public: + virtual ~ViewPickerIface() = default; + + virtual void initialize() = 0; + + /// Return true if the point at x, y in the viewport is showing an object, false otherwise. + /** + * If it is showing an object, result will be changed to contain the 3D point + * corresponding to it. + */ + virtual bool get3DPoint( + RenderPanel * panel, + int x, + int y, + Ogre::Vector3 & result_point) = 0; +}; + +} // namespace interaction +} // namespace rviz_common + +#endif // RVIZ_COMMON__INTERACTION__VIEW_PICKER_IFACE_HPP_ diff --git a/rviz_common/include/rviz_common/selection/selection_manager.hpp b/rviz_common/include/rviz_common/selection/selection_manager.hpp deleted file mode 100644 index 09eb7bc00..000000000 --- a/rviz_common/include/rviz_common/selection/selection_manager.hpp +++ /dev/null @@ -1,375 +0,0 @@ -/* - * Copyright (c) 2008, Willow Garage, Inc. - * Copyright (c) 2017, Open Source Robotics Foundation, 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. - */ - -#ifndef RVIZ_COMMON__SELECTION__SELECTION_MANAGER_HPP_ -#define RVIZ_COMMON__SELECTION__SELECTION_MANAGER_HPP_ - -#include "selection_manager_iface.hpp" - -#include -#include -#include -#include -#include -#include -#include -#include - -#ifndef _WIN32 -# pragma GCC diagnostic push -# pragma GCC diagnostic ignored "-Wunused-parameter" -# pragma GCC diagnostic ignored "-Wpedantic" -#endif - -#include -#include - -#ifndef _WIN32 -# pragma GCC diagnostic pop -#endif - -#include // NOLINT: cpplint is unable to handle the include order here - -#include "./forwards.hpp" -#include "./selection_handler.hpp" -#include "rviz_common/visibility_control.hpp" - -namespace rclcpp -{ - -class PublisherBase; - -} - -namespace Ogre -{ - -class Rectangle2D; - -} // namespace Ogre - -namespace rviz_common -{ - -namespace properties -{ - -class PropertyTreeModel; - -} - -class VisualizationManager; - -namespace selection -{ - -class RVIZ_COMMON_PUBLIC SelectionManager - : public SelectionManagerIface -{ - Q_OBJECT - -public: - explicit SelectionManager(VisualizationManager * manager); - - virtual ~SelectionManager(); - - void - initialize() override; - - /// Enables or disables publishing of picking and depth rendering images. - void - setDebugMode(bool debug) override; - - void - clearHandlers() override; - - void - addObject(CollObjectHandle obj, SelectionHandler * handler) override; - - void - removeObject(CollObjectHandle obj) override; - - /// Control the highlight box being displayed while selecting. - void - highlight(Ogre::Viewport * viewport, int x1, int y1, int x2, int y2) override; - - void - removeHighlight() override; - - /// Select all objects in bounding box. - void - select(Ogre::Viewport * viewport, int x1, int y1, int x2, int y2, SelectType type) override; - - /** - * \return handles of all objects in the given bounding box - * \param single_render_pass only perform one rendering pass - * (point cloud selecting won't work) - */ - void - pick( - Ogre::Viewport * viewport, - int x1, - int y1, - int x2, - int y2, - M_Picked & results, - bool single_render_pass = false) override; - - void update() override; - - /// Set the list of currently selected objects. - void setSelection(const M_Picked & objs) override; - - void addSelection(const M_Picked & objs) override; - - void removeSelection(const M_Picked & objs) override; - - const M_Picked & getSelection() const override; - - SelectionHandler * getHandler(CollObjectHandle obj) override; - - static Ogre::ColourValue handleToColor(CollObjectHandle handle); - - // static CollObjectHandle colourToHandle( const Ogre::ColourValue & color ); - - static void setPickColor(const Ogre::ColourValue & color, Ogre::SceneNode * node); - - static void setPickColor(const Ogre::ColourValue & color, Ogre::MovableObject * object); - - static void setPickHandle(CollObjectHandle handle, Ogre::SceneNode * node); - - static void setPickHandle(CollObjectHandle handle, Ogre::MovableObject * object); - - static void setPickData( - CollObjectHandle handle, - const Ogre::ColourValue & color, - Ogre::SceneNode * node); - - static void setPickData( - CollObjectHandle handle, - const Ogre::ColourValue & color, - Ogre::MovableObject * object); - - /// If a material does not support the picking scheme, paint it black. - Ogre::Technique * handleSchemeNotFound( - unsigned short scheme_index, // NOLINT: Ogre decides the use of unsigned short - const Ogre::String & scheme_name, - Ogre::Material * original_material, - unsigned short lod_index, // NOLINT: Ogre decides the use of unsigned short - const Ogre::Renderable * rend) override; - - /// Create a new unique handle. - CollObjectHandle createHandle() override; - - /// Tell all handlers that interactive mode is active/inactive. - void enableInteraction(bool enable) override; - - bool getInteractionEnabled() const override; - - /// Tell the view controller to look at the selection. - void focusOnSelection() override; - - /// Change the size of the off-screen selection buffer texture. - void setTextureSize(unsigned size) override; - - /// Return true if the point at x, y in the viewport is showing an object, false otherwise. - /** - * If it is showing an object, result will be changed to contain the 3D point - * corresponding to it. - */ - bool get3DPoint( - Ogre::Viewport * viewport, - int x, - int y, - Ogre::Vector3 & result_point) override; - - /// Gets the 3D points in a box around a point in a view port. - /** - * \param[in] viewport Rendering area clicked on. - * \param[in] x x coordinate of upper-left corner of box. - * \param[in] y y coordinate of upper-left corner of box. - * \param[in] width The width of the rendered box in pixels. - * \param[in] height The height of the rendered box in pixels. - * \param[in] skip_missing Whether to skip non-existing points or insert - * NaNs for them - * \param[out] result_points The vector of output points. - * - * \returns True if any valid point is rendered in the box. NaN points count, - * so if skip_missing is false, this will always return true if - * width and height are > 0. - */ - bool get3DPatch( - Ogre::Viewport * viewport, - int x, - int y, - unsigned width, - unsigned height, - bool skip_missing, - std::vector & result_points) override; - - - /// Renders a depth image in a box around a point in a view port. - /** - * \param[in] viewport Rendering area clicked on. - * \param[in] x x coordinate of upper-left corner of box. - * \param[in] y y coordinate of upper-left corner of box. - * \param[in] width The width of the rendered box in pixels. - * \param[in] height The height of the rendered box in pixels. - * \param[out] depth_vector The vector of depth values. - * - * \returns True if rendering operation to render depth data to the depth - * texture buffer succeeds. - * Failure likely indicates a pretty serious problem. - */ - bool getPatchDepthImage( - Ogre::Viewport * viewport, - int x, - int y, - unsigned width, - unsigned height, - std::vector & depth_vector) override; - - /// Implementation for Ogre::RenderQueueListener. - void renderQueueStarted( - uint8_t queueGroupId, - const std::string & invocation, - bool & skipThisInvocation) override; - - rviz_common::properties::PropertyTreeModel * getPropertyModel() override; - -private Q_SLOTS: - /// Call updateProperties() on all SelectionHandlers in the current selection. - void updateProperties(); - -private: - void selectionAdded(const M_Picked & added); - - void selectionRemoved(const M_Picked & removed); - - std::pair addSelectedObject(const Picked & obj); - - void removeSelectedObject(const Picked & obj); - - void setHighlightRect(Ogre::Viewport * viewport, int x1, int y1, int x2, int y2); - - /// Render to a texture for one of the picking passes and unpack the resulting pixels. - void renderAndUnpack( - Ogre::Viewport * viewport, - uint32_t pass, - int x1, - int y1, - int x2, - int y2, - V_CollObject & pixels); - - /// Internal render function to render to a texture and read the pixels back out. - bool render( - Ogre::Viewport * viewport, - Ogre::TexturePtr tex, - int x1, - int y1, - int x2, - int y2, - Ogre::PixelBox & dst_box, - std::string material_scheme, - unsigned texture_width, - unsigned textured_height); - - void unpackColors(const Ogre::PixelBox & box, V_CollObject & pixels); - - void setDepthTextureSize(unsigned width, unsigned height); - - void publishDebugImage(const Ogre::PixelBox & pixel_box, const std::string & label); - - VisualizationManager * vis_manager_; - - std::recursive_mutex global_mutex_; - - typedef std::unordered_map - M_CollisionObjectToSelectionHandler; - M_CollisionObjectToSelectionHandler objects_; - - bool highlight_enabled_; - - struct Highlight - { - int x1; - int y1; - int x2; - int y2; - Ogre::Viewport * viewport; - }; - Highlight highlight_; - - M_Picked selection_; - - // If you want to change this number to something > 3 you must provide more - // width for extra handles in the Picked structure (currently a u64) - static constexpr uint32_t kNumRenderTextures_ = 2; - Ogre::TexturePtr render_textures_[kNumRenderTextures_]; - Ogre::PixelBox pixel_boxes_[kNumRenderTextures_]; - - // Graphics card -based depth finding of clicked points. - Ogre::TexturePtr depth_render_texture_; - uint32_t depth_texture_width_, depth_texture_height_; - Ogre::PixelBox depth_pixel_box_; - - uint32_t uid_counter_; - - Ogre::Rectangle2D * highlight_rectangle_; - Ogre::SceneNode * highlight_node_; - Ogre::Camera * camera_; - - V_CollObject pixel_buffer_; - - bool interaction_enabled_; - - bool debug_mode_; - - Ogre::MaterialPtr fallback_pick_material_; - Ogre::Technique * fallback_pick_technique_; - Ogre::Technique * fallback_black_technique_; - Ogre::Technique * fallback_depth_technique_; - Ogre::Technique * fallback_pick_cull_technique_; - Ogre::Technique * fallback_black_cull_technique_; - Ogre::Technique * fallback_depth_cull_technique_; - - uint32_t texture_size_; - - rviz_common::properties::PropertyTreeModel * property_model_; - - typedef std::map> PublisherMap; - PublisherMap debug_publishers_; -}; - -} // namespace selection -} // namespace rviz_common - -#endif // RVIZ_COMMON__SELECTION__SELECTION_MANAGER_HPP_ diff --git a/rviz_common/include/rviz_common/tool.hpp b/rviz_common/include/rviz_common/tool.hpp index 36a472e44..d42712d57 100644 --- a/rviz_common/include/rviz_common/tool.hpp +++ b/rviz_common/include/rviz_common/tool.hpp @@ -77,7 +77,7 @@ class RVIZ_COMMON_PUBLIC Tool : public QObject */ Tool(); - virtual ~Tool(); + ~Tool() override; /// Initialize the tool. /** @@ -134,13 +134,13 @@ class RVIZ_COMMON_PUBLIC Tool : public QObject */ void setName(const QString & name); + /// Get the description. + QString getDescription() const; + /// Set the description of the tool. /** * This is called by ToolManager during tool initialization. */ - QString getDescription() const; - - /// Set the description. void setDescription(const QString & description); /// Return the class identifier which was used to create this instance. @@ -194,9 +194,8 @@ class RVIZ_COMMON_PUBLIC Tool : public QObject /// Override onInitialize to do any setup needed after the DisplayContext has been set. /** * This is called by Tool::initialize(). - * The base implementation here does nothing. */ - virtual void onInitialize(); + virtual void onInitialize() {} Ogre::SceneManager * scene_manager_; DisplayContext * context_; diff --git a/rviz_common/include/rviz_common/viewport_mouse_event.hpp b/rviz_common/include/rviz_common/viewport_mouse_event.hpp index 0f634230d..98e7fd306 100644 --- a/rviz_common/include/rviz_common/viewport_mouse_event.hpp +++ b/rviz_common/include/rviz_common/viewport_mouse_event.hpp @@ -88,14 +88,6 @@ class RVIZ_COMMON_PUBLIC ViewportMouseEvent bool rightDown(); RenderPanel * panel; - // TODO(wjwwood): this object did have a Ogre::Viewport as a member, but - // I think this needs to be encapsulated into some rviz_rendering - // construct. - // It is fairly commonly used, as an argument to get a 3D point - // based on a mouse event, but I think that can be moved into - // a function on the RenderPanel which can in turn ask a - // rviz_rendering class to get the info. - // Ogre::Viewport * viewport; QEvent::Type type; int x; int y; diff --git a/rviz_common/src/rviz_common/failed_tool.cpp b/rviz_common/src/rviz_common/failed_tool.cpp index ce4b40d2c..7a7aa11dc 100644 --- a/rviz_common/src/rviz_common/failed_tool.cpp +++ b/rviz_common/src/rviz_common/failed_tool.cpp @@ -44,12 +44,6 @@ FailedTool::FailedTool(const QString & desired_class_id, const QString & error_m setClassId(desired_class_id); } -QString FailedTool::getDescription() const -{ - return "The class required for this tool, '" + getClassId() + - "', could not be loaded.
Error:
" + error_message_; -} - void FailedTool::load(const Config & config) { saved_config_ = config; @@ -64,7 +58,7 @@ void FailedTool::save(Config config) const void FailedTool::activate() { - QWidget * parent = NULL; + QWidget * parent = nullptr; if (context_->getWindowManager()) { parent = context_->getWindowManager()->getParentWindow(); } @@ -74,10 +68,10 @@ void FailedTool::activate() void FailedTool::deactivate() {} -int FailedTool::processMouseEvent(ViewportMouseEvent & event) +void FailedTool::onInitialize() { - (void)event; - return 0; + setDescription("The class required for this tool, '" + getClassId() + + "', could not be loaded.
Error:
" + error_message_); } } // namespace rviz_common diff --git a/rviz_common/src/rviz_common/failed_tool.hpp b/rviz_common/src/rviz_common/failed_tool.hpp index cf4d99a8d..e8b097b75 100644 --- a/rviz_common/src/rviz_common/failed_tool.hpp +++ b/rviz_common/src/rviz_common/failed_tool.hpp @@ -53,19 +53,18 @@ class FailedTool : public Tool public: FailedTool(const QString & desired_class_id, const QString & error_message); - virtual QString getDescription() const; - void activate() override; void deactivate() override; - int processMouseEvent(ViewportMouseEvent & event) override; - /// Store the config data for later, so we can return it with save() when written back to a file. void load(const Config & config) override; /// Copy saved config data from last call to load() into config. void save(Config config) const override; +protected: + void onInitialize() override; + private: Config saved_config_; QString error_message_; diff --git a/rviz_common/src/rviz_common/interaction/handler_manager.cpp b/rviz_common/src/rviz_common/interaction/handler_manager.cpp new file mode 100644 index 000000000..ef15820c3 --- /dev/null +++ b/rviz_common/src/rviz_common/interaction/handler_manager.cpp @@ -0,0 +1,160 @@ +/* + * Copyright (c) 2008, Willow Garage, Inc. + * Copyright (c) 2017, Open Source Robotics Foundation, Inc. + * Copyright (c) 2018, Bosch Software Innovations GmbH. + * 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 copyright holder 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. + */ + +#include "rviz_common/interaction/handler_manager.hpp" + +#include +#include + + +namespace rviz_common +{ +namespace interaction +{ + +HandlerManager::HandlerManager() +: uid_counter_(0), + interaction_enabled_(false) +{} + +HandlerManager::~HandlerManager() +{ + std::lock_guard lock(handlers_mutex_); + handlers_.clear(); +} + +void HandlerManager::addHandler(CollObjectHandle handle, SelectionHandlerWeakPtr handler) +{ + if (!handle) { + return; + } + + std::lock_guard lock(handlers_mutex_); + + auto handler_shared_ptr = handler.lock(); + InteractiveObjectPtr object = handler_shared_ptr->getInteractiveObject().lock(); + if (object) { + object->enableInteraction(interaction_enabled_); + } + + bool inserted = handlers_.insert(std::make_pair(handle, handler)).second; + (void) inserted; + assert(inserted); +} + +void HandlerManager::removeHandler(CollObjectHandle handle) +{ + if (!handle) { + return; + } + + std::lock_guard lock(handlers_mutex_); + + handlers_.erase(handle); + + for (const auto & listener : listeners_) { + listener->onHandlerRemoved(handle); + } +} + +void HandlerManager::addListener(HandlerManagerListener * listener) +{ + listeners_.emplace_back(listener); +} + +void HandlerManager::removeListener(HandlerManagerListener * listener) +{ + listeners_.erase(std::remove(listeners_.begin(), listeners_.end(), listener), listeners_.end()); +} + +SelectionHandlerPtr HandlerManager::getHandler(CollObjectHandle handle) +{ + std::lock_guard lock(handlers_mutex_); + + auto item = handlers_.find(handle); + return item != handlers_.end() ? item->second.lock() : nullptr; +} + +std::unique_lock HandlerManager::lock() +{ + return std::unique_lock(handlers_mutex_); +} + +std::unique_lock HandlerManager::lock(std::defer_lock_t defer_lock) +{ + return std::unique_lock(handlers_mutex_, defer_lock); +} + +CollObjectHandle HandlerManager::createHandle() +{ + std::lock_guard lock(uid_mutex_); + + uid_counter_++; + if (uid_counter_ > 0x00ffffff) { + uid_counter_ = 0; + } + + CollObjectHandle handle = 0; + + // shuffle around the bits so we get lots of colors + // when we're displaying the selection buffer + for (unsigned int i = 0; i < 24; i++) { + uint32_t shift = (((23 - i) % 3) * 8) + (23 - i) / 3; + uint32_t bit = ( (uint32_t)(uid_counter_ >> i) & (uint32_t)1) << shift; + handle |= bit; + } + + return handle; +} + +void HandlerManager::enableInteraction(bool enable) +{ + interaction_enabled_ = enable; + std::lock_guard lock(handlers_mutex_); + for (auto handler : handlers_) { + if (InteractiveObjectPtr object = handler.second.lock()->getInteractiveObject().lock()) { + object->enableInteraction(enable); + } + } +} + +bool HandlerManager::getInteractionEnabled() const +{ + return interaction_enabled_; +} + +HandlerRange HandlerManager::handlers() +{ + return HandlerRange(handlers_); +} + +} // namespace interaction +} // namespace rviz_common diff --git a/rviz_common/src/rviz_common/selection/selection_handler.cpp b/rviz_common/src/rviz_common/interaction/selection_handler.cpp similarity index 61% rename from rviz_common/src/rviz_common/selection/selection_handler.cpp rename to rviz_common/src/rviz_common/interaction/selection_handler.cpp index e4850d4a3..8b2dce52c 100644 --- a/rviz_common/src/rviz_common/selection/selection_handler.cpp +++ b/rviz_common/src/rviz_common/interaction/selection_handler.cpp @@ -28,7 +28,7 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include "rviz_common/selection/selection_handler.hpp" +#include "rviz_common/interaction/selection_handler.hpp" #ifndef _WIN32 # pragma GCC diagnostic push @@ -36,12 +36,14 @@ #endif #ifdef _WIN32 -#pragma warning(push) -#pragma warning(disable : 4996) -#include -#pragma warning(pop) -#else -#include +# pragma warning(push) +# pragma warning(disable : 4996) +#endif + +#include // NOLINT + +#ifdef _WIN32 +# pragma warning(pop) #endif #include #include @@ -61,12 +63,13 @@ #include "rviz_common/logging.hpp" #include "rviz_common/properties/property.hpp" -#include "rviz_common/selection/selection_manager.hpp" +#include "rviz_common/interaction/selection_manager.hpp" +#include "rviz_common/interaction/handler_manager_iface.hpp" #include "../visualization_manager.hpp" namespace rviz_common { -namespace selection +namespace interaction { using rviz_common::properties::Property; @@ -74,48 +77,47 @@ using rviz_common::properties::Property; SelectionHandler::SelectionHandler(DisplayContext * context) : context_(context), listener_(new Listener(this)) -{ - pick_handle_ = context_->getSelectionManager()->createHandle(); - context_->getSelectionManager()->addObject(pick_handle_, this); -} +{} SelectionHandler::~SelectionHandler() { - S_Movable::iterator it = tracked_objects_.begin(); - S_Movable::iterator end = tracked_objects_.end(); - for (; it != end; ++it) { - Ogre::MovableObject * m = *it; - m->setListener(0); + for (const auto & object : tracked_objects_) { + object->setListener(nullptr); } while (!boxes_.empty()) { destroyBox(boxes_.begin()->first); } - context_->getSelectionManager()->removeObject(pick_handle_); + if (context_->getHandlerManager()) { + context_->getHandlerManager()->removeHandler(pick_handle_); + } +} + +void SelectionHandler::registerHandle() +{ + pick_handle_ = context_->getHandlerManager()->createHandle(); + context_->getHandlerManager()->addHandler( + pick_handle_, rviz_common::interaction::weak_from_this(this)); } void SelectionHandler::preRenderPass(uint32_t pass) { Q_UNUSED(pass); - M_HandleToBox::iterator it = boxes_.begin(); - M_HandleToBox::iterator end = boxes_.end(); - for (; it != end; ++it) { - Ogre::WireBoundingBox * box = it->second.second; - box->setVisible(false); - } + setBoxVisibility(false); } void SelectionHandler::postRenderPass(uint32_t pass) { Q_UNUSED(pass); - M_HandleToBox::iterator it = boxes_.begin(); - M_HandleToBox::iterator end = boxes_.end(); - for (; it != end; ++it) { - Ogre::WireBoundingBox * box = it->second.second; - box->setVisible(true); - } + setBoxVisibility(true); } +void SelectionHandler::setBoxVisibility(bool visible) +{ + for (const auto & handle_to_box : boxes_) { + handle_to_box.second.box->setVisible(visible); + } +} void SelectionHandler::addTrackedObjects(Ogre::SceneNode * node) { @@ -123,15 +125,15 @@ void SelectionHandler::addTrackedObjects(Ogre::SceneNode * node) return; } // Loop over all objects attached to this node. - Ogre::SceneNode::ObjectIterator obj_it = node->getAttachedObjectIterator(); + auto obj_it = node->getAttachedObjectIterator(); while (obj_it.hasMoreElements() ) { - Ogre::MovableObject * obj = obj_it.getNext(); + auto obj = obj_it.getNext(); addTrackedObject(obj); } // Loop over and recurse into all child nodes. - Ogre::SceneNode::ChildNodeIterator child_it = node->getChildIterator(); + auto child_it = node->getChildIterator(); while (child_it.hasMoreElements() ) { - Ogre::SceneNode * child = dynamic_cast( child_it.getNext() ); + auto child = dynamic_cast( child_it.getNext() ); addTrackedObjects(child); } } @@ -147,30 +149,25 @@ void SelectionHandler::addTrackedObject(Ogre::MovableObject * object) void SelectionHandler::removeTrackedObject(Ogre::MovableObject * object) { tracked_objects_.erase(object); - object->setListener(0); + object->setListener(nullptr); updateTrackedBoxes(); } void SelectionHandler::updateTrackedBoxes() { - M_HandleToBox::iterator it = boxes_.begin(); - M_HandleToBox::iterator end = boxes_.end(); - for (; it != end; ++it) { - V_AABB aabbs; - Picked p(it->first.first); - p.extra_handles.insert(it->first.second); - getAABBs(Picked(it->first.first), aabbs); + for (const auto & handle_to_box : boxes_) { + Picked picked(handle_to_box.first.handle); + picked.extra_handles.insert(handle_to_box.first.extra_handle); + auto aabbs = getAABBs(picked); if (!aabbs.empty()) { Ogre::AxisAlignedBox combined; - V_AABB::iterator aabb_it = aabbs.begin(); - V_AABB::iterator aabb_end = aabbs.end(); - for (; aabb_it != aabb_end; ++aabb_it) { - combined.merge(*aabb_it); + for (const auto & aabb : aabbs) { + combined.merge(aabb); } - createBox(std::make_pair(p.handle, it->first.second), combined, "RVIZ/Cyan"); + createBox(handle_to_box.first, combined, "RVIZ/Cyan"); } } } @@ -181,14 +178,21 @@ void SelectionHandler::createProperties(const Picked & obj, Property * parent_pr Q_UNUSED(parent_property); } -void SelectionHandler::getAABBs(const Picked & obj, V_AABB & aabbs) +V_AABB SelectionHandler::getAABBs(const Picked & obj) { Q_UNUSED(obj); - S_Movable::iterator it = tracked_objects_.begin(); - S_Movable::iterator end = tracked_objects_.end(); - for (; it != end; ++it) { - aabbs.push_back((*it)->getWorldBoundingBox()); + V_AABB aabbs; + + /** with 'derive_world_bounding_box' set to 'true', the WorldBoundingBox is derived each time. + setting it to 'false' may result in the wire box not properly following the tracked object, + but would be less computationally expensive. + */ + bool derive_world_bounding_box = true; + for (const auto & tracked_object : tracked_objects_) { + aabbs.push_back(tracked_object->getWorldBoundingBox(derive_world_bounding_box)); } + + return aabbs; } void SelectionHandler::destroyProperties(const Picked & obj, Property * parent_property) @@ -202,8 +206,7 @@ void SelectionHandler::destroyProperties(const Picked & obj, Property * parent_p } void SelectionHandler::updateProperties() -{ -} +{} bool SelectionHandler::needsAdditionalRenderPass(uint32_t pass) { @@ -212,25 +215,25 @@ bool SelectionHandler::needsAdditionalRenderPass(uint32_t pass) } void SelectionHandler::createBox( - const std::pair & handles, + const Handles & handles, const Ogre::AxisAlignedBox & aabb, const std::string & material_name) { - Ogre::WireBoundingBox * box = 0; - Ogre::SceneNode * node = 0; + Ogre::WireBoundingBox * box = nullptr; + Ogre::SceneNode * node = nullptr; - M_HandleToBox::iterator it = boxes_.find(handles); - if (it == boxes_.end()) { - Ogre::SceneManager * scene_manager = context_->getSceneManager(); + auto handle_to_box_iterator = boxes_.find(handles); + if (handle_to_box_iterator == boxes_.end()) { + auto scene_manager = context_->getSceneManager(); node = scene_manager->getRootSceneNode()->createChildSceneNode(); box = new Ogre::WireBoundingBox; - bool inserted = boxes_.insert(std::make_pair(handles, std::make_pair(node, box))).second; + bool inserted = boxes_.insert(std::make_pair(handles, SelectionBox(node, box))).second; (void) inserted; assert(inserted); } else { - node = it->second.first; - box = it->second.second; + node = handle_to_box_iterator->second.scene_node; + box = handle_to_box_iterator->second.box; } auto material = Ogre::MaterialManager::getSingleton().getByName(material_name); @@ -240,48 +243,44 @@ void SelectionHandler::createBox( } box->setMaterial(material); - box->setupBoundingBox(aabb); node->detachAllObjects(); node->attachObject(box); } -void SelectionHandler::destroyBox(const std::pair & handles) +void SelectionHandler::destroyBox(const Handles & handles) { - M_HandleToBox::iterator it = boxes_.find(handles); - if (it != boxes_.end()) { - Ogre::SceneNode * node = it->second.first; - Ogre::WireBoundingBox * box = it->second.second; + auto handle_to_box_iterator = boxes_.find(handles); + if (handle_to_box_iterator != boxes_.end()) { + auto node = handle_to_box_iterator->second.scene_node; + auto box = handle_to_box_iterator->second.box; node->detachAllObjects(); node->getParentSceneNode()->removeAndDestroyChild(node->getName()); delete box; - - boxes_.erase(it); + box = nullptr; + boxes_.erase(handle_to_box_iterator); } } void SelectionHandler::onSelect(const Picked & obj) { - V_AABB aabbs; - getAABBs(obj, aabbs); + auto aabbs = getAABBs(obj); if (!aabbs.empty()) { Ogre::AxisAlignedBox combined; - V_AABB::iterator it = aabbs.begin(); - V_AABB::iterator end = aabbs.end(); - for (; it != end; ++it) { - combined.merge(*it); + for (const auto & aabb : aabbs) { + combined.merge(aabb); } - createBox(std::make_pair(obj.handle, 0ULL), combined, "RVIZ/Cyan"); + createBox(Handles(obj.handle, 0ULL), combined, "RVIZ/Cyan"); } } void SelectionHandler::onDeselect(const Picked & obj) { - destroyBox(std::make_pair(obj.handle, 0ULL)); + destroyBox(Handles(obj.handle, 0ULL)); } void SelectionHandler::setInteractiveObject(InteractiveObjectWPtr object) @@ -299,5 +298,20 @@ CollObjectHandle SelectionHandler::getHandle() const return pick_handle_; } -} // namespace selection +SelectionHandler::Listener::Listener(SelectionHandler * handler) +: handler_(handler) +{} + +void SelectionHandler::Listener::objectMoved(Ogre::MovableObject * object) +{ + Q_UNUSED(object); + handler_->updateTrackedBoxes(); +} + +void SelectionHandler::Listener::objectDestroyed(Ogre::MovableObject * object) +{ + handler_->removeTrackedObject(object); +} + +} // namespace interaction } // namespace rviz_common diff --git a/rviz_common/src/rviz_common/interaction/selection_manager.cpp b/rviz_common/src/rviz_common/interaction/selection_manager.cpp new file mode 100644 index 000000000..479889546 --- /dev/null +++ b/rviz_common/src/rviz_common/interaction/selection_manager.cpp @@ -0,0 +1,716 @@ +/* + * Copyright (c) 2008, Willow Garage, Inc. + * Copyright (c) 2017, Open Source Robotics Foundation, Inc. + * Copyright (c) 2018, Bosch Software Innovations GmbH. + * 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. + */ + +#include "rviz_common/interaction/selection_manager.hpp" + +#include +#include +#include +#include +#include +#include + +#ifndef _WIN32 +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wunused-parameter" +#endif + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#ifndef _WIN32 +# pragma GCC diagnostic pop +#endif + +#include // NOLINT: cpplint is unable to handle the include order here + +#include "rviz_rendering/custom_parameter_indices.hpp" +#include "rviz_rendering/render_window.hpp" + +#include "rviz_common/display_context.hpp" +#include "rviz_common/logging.hpp" +#include "rviz_common/render_panel.hpp" +#include "rviz_common/view_manager.hpp" +#include "rviz_common/interaction/handler_manager_iface.hpp" +#include "rviz_common/interaction/selection_renderer.hpp" +#include "rviz_common/properties/property.hpp" +#include "rviz_common/properties/property_tree_model.hpp" + + +namespace rviz_common +{ +namespace interaction +{ + +using rviz_common::properties::Property; +using rviz_common::properties::PropertyTreeModel; + +SelectionManager::SelectionManager( + DisplayContext * context, std::shared_ptr renderer) +: context_(context), + highlight_enabled_(false), + property_model_(new PropertyTreeModel(new Property("root"))), + renderer_(renderer) +{ + // TODO(Martin-Idel-SI): I would like to just use setUpSlots(), but timer needs a QThread, + // which is not easily available in tests + for (auto & pixel_box : pixel_boxes_) { + pixel_box.data = nullptr; + } +} + +SelectionManager::SelectionManager(DisplayContext * context) +: context_(context), + highlight_enabled_(false), + property_model_(new PropertyTreeModel(new Property("root"))), + renderer_(std::make_shared(context)) +{ + setUpSlots(); +} + +void SelectionManager::setUpSlots() +{ + for (auto & pixel_box : pixel_boxes_) { + pixel_box.data = nullptr; + } + + auto timer = new QTimer(this); + connect(timer, SIGNAL(timeout()), this, SLOT(updateProperties())); + timer->start(200); +} + +SelectionManager::~SelectionManager() +{ + std::lock_guard lock(selection_mutex_); + + setSelection(M_Picked()); + + highlight_node_->getParentSceneNode()->removeAndDestroyChild(highlight_node_); + delete highlight_rectangle_; + + for (auto & pixel_box : pixel_boxes_) { + delete[] reinterpret_cast(pixel_box.data); + } + + delete property_model_; + + handler_manager_->removeListener(this); +} + +void SelectionManager::initialize() +{ + // Create our render textures + setTextureSize(1); + + // Create our highlight rectangle + auto scene_manager = context_->getSceneManager(); + highlight_node_ = scene_manager->getRootSceneNode()->createChildSceneNode(); + + std::string name("SelectionRect"); + static int count = 0; + name += std::to_string(count++); + highlight_rectangle_ = new Ogre::Rectangle2D(true); + + static const uint32_t texture_data[1] = {0xffff0080}; + Ogre::DataStreamPtr pixel_stream; + pixel_stream.reset(new Ogre::MemoryDataStream( + reinterpret_cast(const_cast(&texture_data[0])), + 4 + )); + + Ogre::TexturePtr tex = Ogre::TextureManager::getSingleton().loadRawData( + name + "Texture", + Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, + pixel_stream, + 1, + 1, + Ogre::PF_R8G8B8A8, + Ogre::TEX_TYPE_2D, + 0 + ); + + Ogre::MaterialPtr material = Ogre::MaterialManager::getSingleton().create( + name, Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); + material->setLightingEnabled(false); + // material->getTechnique(0)->getPass(0)->setPolygonMode(Ogre::PM_WIREFRAME); + highlight_rectangle_->setMaterial(material); + Ogre::AxisAlignedBox aabInf; + aabInf.setInfinite(); + highlight_rectangle_->setBoundingBox(aabInf); + highlight_rectangle_->setRenderQueueGroup(Ogre::RENDER_QUEUE_OVERLAY - 1); + material->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA); + material->setCullingMode(Ogre::CULL_NONE); + + Ogre::TextureUnitState * tex_unit = + material->getTechnique(0)->getPass(0)->createTextureUnitState(); + tex_unit->setTextureName(tex->getName()); + tex_unit->setTextureFiltering(Ogre::TFO_NONE); + + highlight_node_->attachObject(highlight_rectangle_); + + // create picking camera + camera_ = scene_manager->createCamera(name + "_camera"); + + renderer_->initialize(camera_, scene_manager); + + handler_manager_ = context_->getHandlerManager(); + handler_manager_->addListener(this); +} + +void SelectionManager::setTextureSize(unsigned size) +{ + if (size > 1024) { + size = 1024; + } + + texture_size_ = size; + + for (auto & render_texture : render_textures_) { + // check if we need to change the texture size + if (!render_texture.get() || render_texture->getWidth() != size) { + std::string tex_name; + if (render_texture.get()) { + tex_name = render_texture->getName(); + + // destroy old + Ogre::TextureManager::getSingleton().remove( + tex_name, + Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); + } else { + static int count = 0; + tex_name = "SelectionTexture" + std::to_string(count++); + } + + // create new texture + render_texture = Ogre::TextureManager::getSingleton().createManual(tex_name, + Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, Ogre::TEX_TYPE_2D, size, size, 0, + Ogre::PF_R8G8B8A8, Ogre::TU_STATIC | Ogre::TU_RENDERTARGET); + + render_texture->getBuffer()->getRenderTarget()->setAutoUpdated(false); + } + } +} + +void SelectionManager::update() +{ + std::lock_guard lock(selection_mutex_); + + highlight_node_->setVisible(highlight_enabled_); + + if (highlight_enabled_) { + setHighlightRect( + highlight_.viewport, highlight_.x1, highlight_.y1, highlight_.x2, highlight_.y2); + +#if 0 + M_Picked results; + highlight_node_->setVisible(false); + pick(highlight_.viewport, highlight_.x1, highlight_.y1, highlight_.x2, highlight_.y2, results); + highlight_node_->setVisible(true); +#endif + } +} + +void SelectionManager::removeHighlight() +{ + std::lock_guard lock(selection_mutex_); + + highlight_enabled_ = false; +} + +void SelectionManager::setHighlightRect(Ogre::Viewport * viewport, int x1, int y1, int x2, int y2) +{ + float nx1 = (static_cast(x1) / viewport->getActualWidth()) * 2 - 1; + float nx2 = (static_cast(x2) / viewport->getActualWidth()) * 2 - 1; + float ny1 = -((static_cast(y1) / viewport->getActualHeight()) * 2 - 1); + float ny2 = -((static_cast(y2) / viewport->getActualHeight()) * 2 - 1); + + nx1 = nx1 < -1 ? -1 : (nx1 > 1 ? 1 : nx1); + ny1 = ny1 < -1 ? -1 : (ny1 > 1 ? 1 : ny1); + nx2 = nx2 < -1 ? -1 : (nx2 > 1 ? 1 : nx2); + ny2 = ny2 < -1 ? -1 : (ny2 > 1 ? 1 : ny2); + + highlight_rectangle_->setCorners(nx1, ny1, nx2, ny2); +} + +void SelectionManager::unpackColors(const Ogre::PixelBox & box) +{ + auto w = box.getWidth(); + auto h = box.getHeight(); + + pixel_buffer_.clear(); + pixel_buffer_.reserve(w * h); + + for (uint32_t y = 0; y < h; ++y) { + for (uint32_t x = 0; x < w; ++x) { + uint32_t pos = (x + y * w) * 4; + + uint32_t pix_val = *reinterpret_cast(reinterpret_cast(box.data) + pos); + uint32_t handle = colorToHandle(box.format, pix_val); + + pixel_buffer_.push_back(handle); + } + } +} + +void SelectionManager::renderAndUnpack( + rviz_rendering::RenderWindow * window, + const SelectionRectangle & selection_rectangle, + uint32_t pass) +{ + assert(pass < render_textures_.size()); + + std::stringstream scheme; + scheme << "Pick"; + if (pass > 0) { + scheme << pass; + } + + auto tex = RenderTexture( + render_textures_[pass], + Dimensions(texture_size_, texture_size_), + scheme.str()); + + render(window, selection_rectangle, tex, pixel_boxes_[pass]); + unpackColors(pixel_boxes_[pass]); +} + +void SelectionManager::render( + rviz_rendering::RenderWindow * window, + const SelectionRectangle & selection_rectangle, + const RenderTexture & render_texture, + Ogre::PixelBox & dst_box) +{ + auto handler_lock = handler_manager_->lock(); + renderer_->render( + window, selection_rectangle, + render_texture, + handler_manager_->handlers(), + dst_box); +} + +PropertyTreeModel * SelectionManager::getPropertyModel() +{ + return property_model_; +} + +Ogre::ColourValue SelectionManager::handleToColor(CollObjectHandle handle) +{ + float r = ((handle >> 16) & 0xff) / 255.0f; + float g = ((handle >> 8) & 0xff) / 255.0f; + float b = (handle & 0xff) / 255.0f; + return Ogre::ColourValue(r, g, b, 1.0f); +} + +void SelectionManager::setPickColor(const Ogre::ColourValue & color, Ogre::SceneNode * node) +{ + setPickData(colorToHandle(color), color, node); +} + +void SelectionManager::setPickColor(const Ogre::ColourValue & color, Ogre::MovableObject * object) +{ + setPickData(colorToHandle(color), color, object); +} + +void SelectionManager::setPickHandle(CollObjectHandle handle, Ogre::SceneNode * node) +{ + setPickData(handle, handleToColor(handle), node); +} + +void SelectionManager::setPickHandle(CollObjectHandle handle, Ogre::MovableObject * object) +{ + setPickData(handle, handleToColor(handle), object); +} + +void SelectionManager::setPickData( + CollObjectHandle handle, const Ogre::ColourValue & color, Ogre::SceneNode * node) +{ + if (!node) { + return; + } + // Loop over all objects attached to this node. + auto obj_it = node->getAttachedObjectIterator(); + while (obj_it.hasMoreElements()) { + auto obj = obj_it.getNext(); + setPickData(handle, color, obj); + } + // Loop over and recurse into all child nodes. + auto child_it = node->getChildIterator(); + while (child_it.hasMoreElements()) { + auto child = dynamic_cast(child_it.getNext()); + setPickData(handle, color, child); + } +} + +class PickColorSetter : public Ogre::Renderable::Visitor +{ +public: + PickColorSetter(CollObjectHandle handle, const Ogre::ColourValue & color) + : color_vector_(color.r, color.g, color.b, 1.0), handle_(handle) {} + + void visit(Ogre::Renderable * rend, ushort lodIndex, bool isDebug, Ogre::Any * pAny = 0) override + { + Q_UNUSED(lodIndex); + Q_UNUSED(isDebug); + Q_UNUSED(pAny); + rend->setCustomParameter(RVIZ_RENDERING_PICK_COLOR_PARAMETER, color_vector_); + rend->getUserObjectBindings().setUserAny("pick_handle", Ogre::Any(handle_)); + } + + Ogre::Vector4 color_vector_; + CollObjectHandle handle_; +}; + +void SelectionManager::setPickData( + CollObjectHandle handle, const Ogre::ColourValue & color, Ogre::MovableObject * object) +{ + PickColorSetter visitor(handle, color); + object->visitRenderables(&visitor); + object->getUserObjectBindings().setUserAny("pick_handle", Ogre::Any(handle)); +} + +void SelectionManager::removeSelection(const M_Picked & objs) +{ + std::lock_guard lock(selection_mutex_); + + for (const auto & obj : objs) { + removeSelectedObject(obj.second); + } + + selectionRemoved(objs); +} + +const M_Picked & SelectionManager::getSelection() const +{ + return selection_; +} + +void SelectionManager::addSelection(const M_Picked & objs) +{ + std::lock_guard lock(selection_mutex_); + + M_Picked added; + for (const auto & obj : objs) { + auto ppb = addSelectedObject(obj.second); + if (ppb.second) { + added.insert(std::make_pair(obj.first, ppb.first)); + } + } + + selectionAdded(added); +} + +void SelectionManager::setSelection(const M_Picked & objs) +{ + std::lock_guard lock(selection_mutex_); + + M_Picked original(selection_.begin(), selection_.end()); + + removeSelection(original); + addSelection(objs); +} + +std::pair SelectionManager::addSelectedObject(const Picked & obj) +{ + std::lock_guard lock(selection_mutex_); + + std::pair pib = selection_.insert(std::make_pair(obj.handle, obj)); + + auto handler = handler_manager_->getHandler(obj.handle); + + if (pib.second) { + handler->onSelect(obj); + return std::make_pair(obj, true); + } else { + Picked & cur = pib.first->second; + Picked added(cur.handle); + + for (const auto & extra_handle : obj.extra_handles) { + if (cur.extra_handles.insert(extra_handle).second) { + added.extra_handles.insert(extra_handle); + } + } + + if (!added.extra_handles.empty()) { + handler->onSelect(added); + + return std::make_pair(added, true); + } + } + + return std::make_pair(Picked(0), false); +} + +void SelectionManager::removeSelectedObject(const Picked & obj) +{ + std::lock_guard lock(selection_mutex_); + + auto sel_it = selection_.find(obj.handle); + if (sel_it != selection_.end()) { + for (const auto & extra_handle : obj.extra_handles) { + sel_it->second.extra_handles.erase(extra_handle); + } + + if (sel_it->second.extra_handles.empty()) { + selection_.erase(sel_it); + } + } + + handler_manager_->getHandler(obj.handle)->onDeselect(obj); +} + +void SelectionManager::focusOnSelection() +{ + std::lock_guard lock(selection_mutex_); + + if (selection_.empty()) { + return; + } + + Ogre::AxisAlignedBox combined; + + for (const auto & selection_item : selection_) { + const Picked & p = selection_item.second; + + auto handler = handler_manager_->getHandler(p.handle); + + auto aabbs = handler->getAABBs(p); + + for (const auto & aabb : aabbs) { + combined.merge(aabb); + } + } + + if (!combined.isInfinite() && !combined.isNull()) { + Ogre::Vector3 center = combined.getCenter(); + ViewController * controller = context_->getViewManager()->getCurrent(); + if (controller) { + controller->lookAt(center); + } + } +} + +void SelectionManager::selectionRemoved(const M_Picked & removed) +{ + for (const auto & removed_item : removed) { + const Picked & picked = removed_item.second; + auto handler = handler_manager_->getHandler(picked.handle); + assert(handler); + + handler->destroyProperties(picked, property_model_->getRoot()); + } +} + +void SelectionManager::selectionAdded(const M_Picked & added) +{ + for (const auto & added_item : added) { + const Picked & picked = added_item.second; + auto handler = handler_manager_->getHandler(picked.handle); + assert(handler); + + handler->createProperties(picked, property_model_->getRoot()); + } + property_model_->sort(0, Qt::AscendingOrder); +} + +void SelectionManager::updateProperties() +{ + std::lock_guard lock(selection_mutex_); + + for (const auto & selection_item : selection_) { + handler_manager_->getHandler(selection_item.first)->updateProperties(); + } +} + +void SelectionManager::highlight( + rviz_rendering::RenderWindow * window, int x1, int y1, int x2, int y2) +{ + Ogre::Viewport * viewport = rviz_rendering::RenderWindowOgreAdapter::getOgreViewport(window); + std::lock_guard lock(selection_mutex_); + + highlight_enabled_ = true; + + highlight_.viewport = viewport; + highlight_.x1 = x1; + highlight_.y1 = y1; + highlight_.x2 = x2; + highlight_.y2 = y2; +} + +void SelectionManager::select( + rviz_rendering::RenderWindow * window, int x1, int y1, int x2, int y2, SelectType type) +{ + std::lock_guard lock(selection_mutex_); + + highlight_enabled_ = false; + highlight_node_->setVisible(false); + + M_Picked results; + pick(window, x1, y1, x2, y2, results); + + if (type == Add) { + addSelection(results); + } else if (type == Remove) { + removeSelection(results); + } else if (type == Replace) { + setSelection(results); + } +} + +void SelectionManager::pick( + rviz_rendering::RenderWindow * window, + int x1, + int y1, + int x2, + int y2, + M_Picked & results, + bool single_render_pass) +{ + auto handler_lock = handler_manager_->lock(std::defer_lock); + std::lock(selection_mutex_, handler_lock); + std::lock_guard lock(selection_mutex_, std::adopt_lock); + + bool need_additional_render = false; + + V_CollObject handles_by_pixel; + S_CollObject need_additional; + + auto rectangle = SelectionRectangle(x1, y1, x2, y2); + // First render is special... does the initial object picking, determines + // which objects have been selected. + // After that, individual handlers can specify that they need additional + // renders (max # defined in kNumRenderTextures_). + { + renderAndUnpack(window, rectangle, 0); + + handles_by_pixel.reserve(pixel_buffer_.size()); + for (const auto & handle : pixel_buffer_) { + handles_by_pixel.push_back(handle); + + if (handle == 0) { + continue; + } + + auto handler = handler_manager_->getHandler(handle); + + if (handler) { + std::pair insert_result = + results.insert(std::make_pair(handle, Picked(handle))); + if (insert_result.second) { + if (handler->needsAdditionalRenderPass(1) && !single_render_pass) { + need_additional.insert(handle); + need_additional_render = true; + } + } else { + insert_result.first->second.pixel_count++; + } + } + } + } + + uint32_t pass = 1; + + V_uint64 extra_by_pixel; + extra_by_pixel.resize(handles_by_pixel.size()); + while (need_additional_render && pass < render_textures_.size()) { + for (const auto & handle : need_additional) { + handler_manager_->getHandler(handle)->preRenderPass(pass); + } + + renderAndUnpack(window, rectangle, pass); + + for (const auto & handle : need_additional) { + handler_manager_->getHandler(handle)->postRenderPass(pass); + } + + for (size_t i = 0; i != handles_by_pixel.size(); ++i) { + if (pass == 1) { + extra_by_pixel[i] = 0; + } + + if (need_additional.find(handles_by_pixel[i]) != need_additional.end()) { + auto extra_handle = pixel_buffer_[i]; + extra_by_pixel[i] |= extra_handle << (32 * (pass - 1)); + } else { + extra_by_pixel[i] = 0; + } + } + + need_additional_render = false; + need_additional.clear(); + for (const auto & result : results) { + CollObjectHandle handle = result.first; + + if (handler_manager_->getHandler(handle)->needsAdditionalRenderPass(pass + 1)) { + need_additional_render = true; + need_additional.insert(handle); + } + } + } + + for (size_t i = 0; i != handles_by_pixel.size(); ++i) { + auto handle = handles_by_pixel[i]; + + if (handle == 0) { + continue; + } + + auto picked_it = results.find(handle); + if (picked_it == results.end()) { + continue; + } + + Picked & picked = picked_it->second; + + if (extra_by_pixel[i]) { + picked.extra_handles.insert(extra_by_pixel[i]); + } + } +} + +void SelectionManager::onHandlerRemoved(CollObjectHandle handle) +{ + std::lock_guard lock(selection_mutex_); + + auto it = selection_.find(handle); + if (it != selection_.end()) { + selection_.erase(it); + } +} + +} // namespace interaction +} // namespace rviz_common diff --git a/rviz_common/src/rviz_common/interaction/selection_renderer.cpp b/rviz_common/src/rviz_common/interaction/selection_renderer.cpp new file mode 100644 index 000000000..9aa0db150 --- /dev/null +++ b/rviz_common/src/rviz_common/interaction/selection_renderer.cpp @@ -0,0 +1,362 @@ +/* + * Copyright (c) 2008, Willow Garage, Inc. + * Copyright (c) 2017, Open Source Robotics Foundation, Inc. + * Copyright (c) 2018, Bosch Software Innovations GmbH. + * 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. + */ + +#include "rviz_common/interaction/selection_renderer.hpp" + +#include +#include + +#ifndef _WIN32 +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wunused-parameter" +#endif + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#ifndef _WIN32 +# pragma GCC diagnostic pop +#endif + +#include // NOLINT: cpplint is unable to handle the include order here + +#include "rviz_rendering/render_window.hpp" + +#include "rviz_common/display_context.hpp" +#include "rviz_common/logging.hpp" +#include "rviz_common/view_manager.hpp" + +namespace rviz_common +{ +namespace interaction +{ +SelectionRenderer::SelectionRenderer(rviz_common::DisplayContext * context) +: context_(context), + camera_(nullptr), + scene_manager_(nullptr) +{} + +void SelectionRenderer::initialize(Ogre::Camera * camera, Ogre::SceneManager * scene_manager) +{ + camera_ = camera; + scene_manager_ = scene_manager; + + fallback_pick_material_ = Ogre::MaterialManager::getSingleton().getByName( + "rviz/DefaultPickAndDepth"); + if (fallback_pick_material_) { + fallback_pick_material_->load(); + + fallback_pick_cull_technique_ = fallback_pick_material_->getTechnique("PickCull"); + fallback_black_cull_technique_ = fallback_pick_material_->getTechnique("BlackCull"); + fallback_depth_cull_technique_ = fallback_pick_material_->getTechnique("DepthCull"); + + fallback_pick_technique_ = fallback_pick_material_->getTechnique("Pick"); + fallback_black_technique_ = fallback_pick_material_->getTechnique("Black"); + fallback_depth_technique_ = fallback_pick_material_->getTechnique("Depth"); + } else { + RVIZ_COMMON_LOG_ERROR("failed to load material 'rviz/DefaultPickAndDepth'"); + } +} + +void SelectionRenderer::render( + rviz_rendering::RenderWindow * window, + SelectionRectangle rectangle, + RenderTexture texture, + HandlerRange handlers, + Ogre::PixelBox & dst_box) +{ + context_->lockRender(); + for (const auto & handler : handlers) { + handler.lock()->preRenderPass(0); + } + + auto window_viewport = rviz_rendering::RenderWindowOgreAdapter::getOgreViewport(window); + + sanitizeRectangle(window_viewport, rectangle); + + configureCamera(window_viewport, rectangle); + + Ogre::HardwarePixelBufferSharedPtr pixel_buffer = texture.tex->getBuffer(); + auto render_texture = setupRenderTexture(pixel_buffer, texture); + auto render_viewport = + setupRenderViewport(render_texture, window_viewport, rectangle, texture.dimensions); + + renderToTexture(render_texture, window_viewport); + + blitToMemory(pixel_buffer, render_viewport, dst_box); + + context_->unlockRender(); + for (const auto & handler : handlers) { + handler.lock()->postRenderPass(0); + } +} + + +void SelectionRenderer::sanitizeRectangle(Ogre::Viewport * viewport, SelectionRectangle & rectangle) +{ + int & x1 = rectangle.x1; + int & x2 = rectangle.x2; + int & y1 = rectangle.y1; + int & y2 = rectangle.y2; + + if (x1 > x2) {std::swap(x1, x2);} + if (y1 > y2) {std::swap(y1, y2);} + + x1 = clamp(x1, 0, viewport->getActualWidth() - 2); + x2 = clamp(x2, 0, viewport->getActualWidth() - 2); + y1 = clamp(y1, 0, viewport->getActualHeight() - 2); + y2 = clamp(y2, 0, viewport->getActualHeight() - 2); + + if (x2 == x1) {x2++;} + if (y2 == y1) {y2++;} +} + +template +T SelectionRenderer::clamp(T value, T min, T max) const +{ + if (value < min) {value = min;} else if (value > max) {value = max;} + return value; +} + +void SelectionRenderer::configureCamera( + Ogre::Viewport * viewport, const SelectionRectangle & rectangle) const +{ + Ogre::Matrix4 proj_matrix = viewport->getCamera()->getProjectionMatrix(); + Ogre::Matrix4 scale_matrix = Ogre::Matrix4::IDENTITY; + Ogre::Matrix4 trans_matrix = Ogre::Matrix4::IDENTITY; + + float x1_rel = getRelativeCoordinate(rectangle.x1, viewport->getActualWidth()); + float y1_rel = getRelativeCoordinate(rectangle.y1, viewport->getActualHeight()); + float x2_rel = getRelativeCoordinate(rectangle.x2, viewport->getActualWidth()); + float y2_rel = getRelativeCoordinate(rectangle.y2, viewport->getActualHeight()); + + scale_matrix[0][0] = 1.f / (x2_rel - x1_rel); + scale_matrix[1][1] = 1.f / (y2_rel - y1_rel); + + trans_matrix[0][3] -= x1_rel + x2_rel; + trans_matrix[1][3] += y1_rel + y2_rel; + + camera_->setCustomProjectionMatrix(true, scale_matrix * trans_matrix * proj_matrix); + + camera_->setPosition(viewport->getCamera()->getDerivedPosition()); + camera_->setOrientation(viewport->getCamera()->getDerivedOrientation()); +} + +float SelectionRenderer::getRelativeCoordinate(float coordinate, int dimension) const +{ + return coordinate / static_cast(dimension - 1) - 0.5f; +} + +Ogre::RenderTexture * SelectionRenderer::setupRenderTexture( + Ogre::HardwarePixelBufferSharedPtr pixel_buffer, + RenderTexture texture) const +{ + Ogre::RenderTexture * render_texture = pixel_buffer->getRenderTarget(); + + // create a viewport if there is none + if (render_texture->getNumViewports() == 0) { + render_texture->removeAllViewports(); + render_texture->addViewport(camera_); + Ogre::Viewport * render_viewport = render_texture->getViewport(0); + render_viewport->setClearEveryFrame(true); + render_viewport->setBackgroundColour(Ogre::ColourValue::Black); + render_viewport->setOverlaysEnabled(false); + render_viewport->setMaterialScheme(texture.material_scheme); + } + return render_texture; +} + +Ogre::Viewport * SelectionRenderer::setupRenderViewport( + Ogre::RenderTexture * render_texture, + Ogre::Viewport * viewport, + const SelectionRectangle & rectangle, + Dimensions texture_dimensions) +{ + Dimensions render_dimensions = getRenderDimensions(rectangle, texture_dimensions); + + // set viewport to render to a subwindow of the texture + Ogre::Viewport * render_viewport = render_texture->getViewport(0); + render_viewport->setDimensions( + 0, + 0, + render_dimensions.width / texture_dimensions.width, + render_dimensions.height / texture_dimensions.height + ); + + // make sure the same objects are visible as in the original viewport + render_viewport->setVisibilityMask(viewport->getVisibilityMask()); + return render_viewport; +} + +Dimensions SelectionRenderer::getRenderDimensions( + const SelectionRectangle & rectangle, + const Dimensions & texture_dim) const +{ + Dimensions selection( + rectangle.x2 - rectangle.x1, + rectangle.y2 - rectangle.y1 + ); + + Dimensions render_dimensions = selection; + + if (selection.width > selection.height) { + if (render_dimensions.width > texture_dim.width) { + render_dimensions.width = texture_dim.width; + render_dimensions.height = round(selection.height * texture_dim.width / selection.width); + } + } else { + if (render_dimensions.height > texture_dim.height) { + render_dimensions.height = texture_dim.height; + render_dimensions.width = round(selection.width * texture_dim.height / selection.height); + } + } + + // safety clamping in case of rounding errors + render_dimensions.width = clamp(render_dimensions.width, 0.f, texture_dim.width); + render_dimensions.height = clamp(render_dimensions.height, 0.f, texture_dim.height); + return render_dimensions; +} + +void SelectionRenderer::renderToTexture( + Ogre::RenderTexture * render_texture, Ogre::Viewport * window_viewport) +{ + // update & force ogre to render the scene + Ogre::MaterialManager::getSingleton().addListener(this); + + render_texture->update(); + + // For some reason we need to pretend to render the main window in + // order to get the picking render to show up in the pixelbox below. + // If we don't do this, it will show up there the *next* time we + // pick something, but not this time. This object as a + // render queue listener tells the scene manager to skip every + // render step, so nothing actually gets drawn. + // + // TODO(unknown): find out what part of _renderScene() actually makes this work. + scene_manager_->addRenderQueueListener(this); + scene_manager_->_renderScene(window_viewport->getCamera(), window_viewport, false); + scene_manager_->removeRenderQueueListener(this); + + Ogre::MaterialManager::getSingleton().removeListener(this); +} + +void SelectionRenderer::blitToMemory( + Ogre::HardwarePixelBufferSharedPtr & pixel_buffer, + const Ogre::Viewport * render_viewport, + Ogre::PixelBox & dst_box) const +{ + auto viewport_w = static_cast(render_viewport->getActualWidth()); + auto viewport_h = static_cast(render_viewport->getActualHeight()); + + auto format = pixel_buffer->getFormat(); + auto size = Ogre::PixelUtil::getMemorySize(viewport_w, viewport_h, 1, format); + auto data = new uint8_t[size]; + + delete[] reinterpret_cast(dst_box.data); + dst_box = Ogre::PixelBox(viewport_w, viewport_h, 1, format, data); + + pixel_buffer->blitToMemory(dst_box, dst_box); +} + +void SelectionRenderer::renderQueueStarted( + uint8_t queueGroupId, + const std::string & invocation, + bool & skipThisInvocation) +{ + Q_UNUSED(queueGroupId); + Q_UNUSED(invocation); + // This render queue listener function tells the scene manager to + // skip every render step, so nothing actually gets drawn. + skipThisInvocation = true; +} + +Ogre::Technique * SelectionRenderer::handleSchemeNotFound( + unsigned short scheme_index, // NOLINT: Ogre decides the use of unsigned short + const Ogre::String & scheme_name, + Ogre::Material * original_material, + unsigned short lod_index, // NOLINT: Ogre decides the use of unsigned short + const Ogre::Renderable * rend) +{ + Q_UNUSED(scheme_index); + Q_UNUSED(lod_index); + // Find the original culling mode + Ogre::CullingMode culling_mode = Ogre::CULL_CLOCKWISE; + Ogre::Technique * orig_tech = original_material->getTechnique(0); + if (orig_tech && orig_tech->getNumPasses() > 0) { + culling_mode = orig_tech->getPass(0)->getCullingMode(); + } + + // find out if the renderable has the picking param set + bool has_pick_param = rend->getUserObjectBindings().getUserAny("pick_handle").has_value(); + + // NOTE: it is important to avoid changing the culling mode of the + // fallback techniques here, because that change then propagates to + // other uses of these fallback techniques in unpredictable ways. + // If you want to change the technique programmatically (like with + // Pass::setCullingMode()), make sure you do it on a cloned material + // which doesn't get shared with other objects. + + // Use the technique with the right name and culling mode. + if (culling_mode == Ogre::CULL_CLOCKWISE) { + if (scheme_name == "Pick") { + return has_pick_param ? fallback_pick_cull_technique_ : fallback_black_cull_technique_; + } else if (scheme_name == "Depth") { + return fallback_depth_cull_technique_; + } + if (scheme_name == "Pick1") { + return fallback_black_cull_technique_; + } else { + return nullptr; + } + } else { // Must be CULL_NONE because we never use CULL_ANTICLOCKWISE + if (scheme_name == "Pick") { + return has_pick_param ? fallback_pick_technique_ : fallback_black_technique_; + } else if (scheme_name == "Depth") { + return fallback_depth_technique_; + } + if (scheme_name == "Pick1") { + return fallback_black_technique_; + } else { + return nullptr; + } + } +} + +} // namespace interaction +} // namespace rviz_common diff --git a/rviz_common/src/rviz_common/interaction/view_picker.cpp b/rviz_common/src/rviz_common/interaction/view_picker.cpp new file mode 100644 index 000000000..a8bc3e523 --- /dev/null +++ b/rviz_common/src/rviz_common/interaction/view_picker.cpp @@ -0,0 +1,289 @@ +/* + * Copyright (c) 2008, Willow Garage, Inc. + * Copyright (c) 2017, Open Source Robotics Foundation, Inc. + * Copyright (c) 2018, Bosch Software Innovations GmbH. + * 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. + */ + +#include "rviz_common/interaction/view_picker.hpp" + +#include +#include +#include + +#ifndef _WIN32 +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wunused-parameter" +#endif + +#include +#include +#include +#include +#include +#include +#include +#include + +#ifndef _WIN32 +# pragma GCC diagnostic pop +#endif + +#include "rviz_common/logging.hpp" + +#include "rviz_common/display_context.hpp" +#include "rviz_common/interaction/handler_manager_iface.hpp" +#include "rviz_common/interaction/selection_renderer.hpp" +#include "rviz_common/render_panel.hpp" + + +namespace rviz_common +{ +namespace interaction +{ + +ViewPicker::ViewPicker( + DisplayContext * context, std::shared_ptr renderer) +: context_(context), + renderer_(renderer) +{ + depth_pixel_box_.data = nullptr; +} + +ViewPicker::ViewPicker(DisplayContext * context) +: ViewPicker(context, std::make_shared(context)) +{} + +ViewPicker::~ViewPicker() +{ + delete[] reinterpret_cast(depth_pixel_box_.data); +} + +void ViewPicker::initialize() +{ + auto scene_manager = context_->getSceneManager(); + + camera_ = scene_manager->createCamera("ViewPicker_camera"); + + renderer_->initialize(camera_, scene_manager); + + handler_manager_ = context_->getHandlerManager(); +} + +bool ViewPicker::get3DPoint( + RenderPanel * panel, + int x, + int y, + Ogre::Vector3 & result_point) +{ + std::vector result_points_temp; + get3DPatch(panel, x, y, 1, 1, true, result_points_temp); + if (result_points_temp.empty()) { + return false; + } + result_point = result_points_temp[0]; + + return true; +} + +bool ViewPicker::get3DPatch( + RenderPanel * panel, + int x, + int y, + unsigned int width, + unsigned int height, + bool skip_missing, + std::vector & result_points) +{ + auto handler_lock = handler_manager_->lock(); + + std::vector depth_vector; + + getPatchDepthImage(panel, x, y, width, height, depth_vector); + + unsigned int pixel_counter = 0; + Ogre::Matrix4 projection = camera_->getProjectionMatrix(); + float depth; + + for (unsigned int y_iter = 0; y_iter < height; ++y_iter) { + for (unsigned int x_iter = 0; x_iter < width; ++x_iter) { + depth = depth_vector[pixel_counter]; + + // Deal with missing or invalid points + if ((depth > camera_->getFarClipDistance()) || (depth == 0)) { + ++pixel_counter; + if (!skip_missing) { + result_points.emplace_back(NAN, NAN, NAN); + } + continue; + } + + Ogre::Vector3 result_point; + // We want to shoot rays through the center of pixels, not the corners, + // so add .5 pixels to the x and y coordinate to get to the center + // instead of the top left of the pixel. + Ogre::Real screenx = static_cast(x_iter + .5) / static_cast(width); + Ogre::Real screeny = static_cast(y_iter + .5) / static_cast(height); + result_point = projection[3][3] == 0.0 ? + computeForPerspectiveProjection(depth, screenx, screeny) : + computeForOrthogonalProjection(depth, screenx, screeny); + + result_points.push_back(result_point); + ++pixel_counter; + } + } + + return !result_points.empty(); +} + +void ViewPicker::getPatchDepthImage( + RenderPanel * panel, int x, int y, unsigned width, + unsigned height, std::vector & depth_vector) +{ + unsigned int num_pixels = width * height; + depth_vector.reserve(num_pixels); + + setDepthTextureSize(width, height); + + render( + panel->getRenderWindow(), + SelectionRectangle(x, y, x + width, y + height), + RenderTexture( + depth_render_texture_, Dimensions(depth_texture_width_, depth_texture_height_), "Depth"), + depth_pixel_box_); + + auto data_ptr = reinterpret_cast(depth_pixel_box_.data); + + for (uint32_t pixel = 0; pixel < num_pixels; ++pixel) { + uint8_t a = data_ptr[4 * pixel]; + uint8_t b = data_ptr[4 * pixel + 1]; + uint8_t c = data_ptr[4 * pixel + 2]; + + int int_depth = (c << 16) | (b << 8) | a; + float normalized_depth = (static_cast(int_depth)) / static_cast(0xffffff); + depth_vector.push_back(normalized_depth * camera_->getFarClipDistance()); + } +} + +Ogre::Vector3 +ViewPicker::computeForOrthogonalProjection( + float depth, Ogre::Real screenx, Ogre::Real screeny) const +{ + Ogre::Ray ray; + camera_->getCameraToViewportRay(screenx, screeny, &ray); + + return ray.getPoint(depth); +} + +Ogre::Vector3 +ViewPicker::computeForPerspectiveProjection( + float depth, Ogre::Real screenx, Ogre::Real screeny) const +{ + // get world-space ray from camera & mouse coord + Ogre::Ray vp_ray = camera_->getCameraToViewportRay(screenx, screeny); + + // transform ray direction back into camera coords + Ogre::Vector3 dir_cam = camera_->getDerivedOrientation().Inverse() * vp_ray.getDirection(); + + // normalize, so dir_cam.z == -depth + dir_cam = dir_cam / dir_cam.z * depth * -1; + + // compute 3d point from camera origin and direction*/ + return camera_->getDerivedPosition() + camera_->getDerivedOrientation() * dir_cam; +} + +void ViewPicker::setDepthTextureSize(unsigned width, unsigned height) +{ + // Cap and store requested texture sizecapTextureSize(width, height); + capTextureSize(width, height); + + if (!depth_render_texture_.get() || depth_render_texture_->getWidth() != width || + depth_render_texture_->getHeight() != height) + { + std::string tex_name = "DepthTexture"; + if (depth_render_texture_.get()) { + tex_name = depth_render_texture_->getName(); + + // destroy old + Ogre::TextureManager::getSingleton().remove( + tex_name, + Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); + } + + depth_render_texture_ = + Ogre::TextureManager::getSingleton().createManual(tex_name, + Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, + Ogre::TEX_TYPE_2D, depth_texture_width_, depth_texture_height_, 0, + Ogre::PF_R8G8B8, + Ogre::TU_RENDERTARGET); + + depth_render_texture_->getBuffer()->getRenderTarget()->setAutoUpdated(false); + } +} + +void ViewPicker::capTextureSize(unsigned int & width, unsigned int & height) +{ + // It's probably an error if an invalid size is requested. + if (width > 1024) { + width = 1024; + RVIZ_COMMON_LOG_ERROR_STREAM( + "SelectionManager::setDepthTextureSize invalid width requested. " + "Max Width: 1024 -- Width requested: " << width << ". Capping Width at 1024."); + } + + if (depth_texture_width_ != width) { + depth_texture_width_ = width; + } + + if (height > 1024) { + height = 1024; + RVIZ_COMMON_LOG_ERROR_STREAM( + "SelectionManager::setDepthTextureSize invalid height requested. " + "Max Height: 1024 -- Height requested: " << height << ". Capping Height at 1024."); + } + + if (depth_texture_height_ != height) { + depth_texture_height_ = height; + } +} + +void ViewPicker::render( + rviz_rendering::RenderWindow * window, + const SelectionRectangle & selection_rectangle, + const RenderTexture & render_texture, + Ogre::PixelBox & dst_box) +{ + auto handler_lock = handler_manager_->lock(); + return renderer_->render( + window, selection_rectangle, + render_texture, + handler_manager_->handlers(), + dst_box); +} + +} // namespace interaction +} // namespace rviz_common diff --git a/rviz_common/src/rviz_common/panel_factory.cpp b/rviz_common/src/rviz_common/panel_factory.cpp index e76c97404..db185d313 100644 --- a/rviz_common/src/rviz_common/panel_factory.cpp +++ b/rviz_common/src/rviz_common/panel_factory.cpp @@ -33,18 +33,18 @@ #include // TODO(wjwwood): reenable the remaining panels -#include "./displays_panel.hpp" +#include "displays_panel.hpp" // #include "./help_panel.hpp" -// #include "./selection_panel.hpp" +#include "selection_panel.hpp" // #include "./time_panel.hpp" // #include "./tool_properties_panel.hpp" -#include "./views_panel.hpp" +#include "views_panel.hpp" namespace rviz_common { // static Panel * newHelpPanel() {return new HelpPanel();} -// static Panel * newSelectionPanel() {return new SelectionPanel();} +static Panel * newSelectionPanel() {return new SelectionPanel();} // static Panel * newTimePanel() {return new TimePanel();} // static Panel * newToolPropertiesPanel() {return new ToolPropertiesPanel();} static Panel * newViewsPanel() {return new ViewsPanel();} @@ -59,9 +59,9 @@ PanelFactory::PanelFactory(const std::string & node_name) }); // addBuiltInClass("rviz_common", "Help", // "Show the key and mouse bindings", &newHelpPanel); - // addBuiltInClass("rviz_common", "Selection", - // "Show properties of selected objects", &newSelectionPanel); - // addBuiltInClass("rviz_common", "Time", + addBuiltInClass("rviz_common", "Selection", + "Show properties of selected objects", &newSelectionPanel); + // addBuiltInClass("rviz", "Time", // "Show the current time", &newTimePanel); // addBuiltInClass("rviz_common", "Tool Properties", // "Show and edit properties of tools", &newToolPropertiesPanel); diff --git a/rviz_common/src/rviz_common/selection/selection_manager.cpp b/rviz_common/src/rviz_common/selection/selection_manager.cpp deleted file mode 100644 index 2134ae09d..000000000 --- a/rviz_common/src/rviz_common/selection/selection_manager.cpp +++ /dev/null @@ -1,1329 +0,0 @@ -/* - * Copyright (c) 2008, Willow Garage, Inc. - * Copyright (c) 2017, Open Source Robotics Foundation, 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. - */ - -#include "rviz_common/selection/selection_manager.hpp" - -#include -#include -#include -#include -#include - -#ifndef _WIN32 -# pragma GCC diagnostic push -# pragma GCC diagnostic ignored "-Wunused-parameter" -#endif - -#include -// #include -#include -// #include -// #include -#include -// #include -#include -// #include -#include -#include -// #include -// #include -#include -#include -#include -// #include - -#ifndef _WIN32 -# pragma GCC diagnostic pop -#endif - -#include // NOLINT: cpplint is unable to handle the include order here - -#include -#include - -#include "rviz_rendering/custom_parameter_indices.hpp" -#include "rviz_rendering/render_window.hpp" - -#include "rviz_common/logging.hpp" - -// #include "../ogre_helpers/arrow.hpp" -// #include "../ogre_helpers/axes.hpp" -// #include "../ogre_helpers/qt_ogre_render_window.hpp" -// #include "../ogre_helpers/shape.hpp" -#include "rviz_common/properties/property.hpp" -#include "rviz_common/properties/property_tree_model.hpp" -#include "rviz_common/render_panel.hpp" -// #include "../view_controller.hpp" -#include "rviz_common/view_manager.hpp" -#include "../visualization_manager.hpp" - - -namespace rviz_common -{ -namespace selection -{ - -using rviz_common::properties::Property; -using rviz_common::properties::PropertyTreeModel; - -SelectionManager::SelectionManager(VisualizationManager * manager) -: vis_manager_(manager), - highlight_enabled_(false), - uid_counter_(0), - interaction_enabled_(false), - debug_mode_(false), - property_model_(new PropertyTreeModel(new Property("root"))) -{ - for (uint32_t i = 0; i < kNumRenderTextures_; ++i) { - pixel_boxes_[i].data = 0; - } - depth_pixel_box_.data = 0; - - QTimer * timer = new QTimer(this); - connect(timer, SIGNAL(timeout()), this, SLOT(updateProperties())); - timer->start(200); -} - -SelectionManager::~SelectionManager() -{ - std::lock_guard lock(global_mutex_); - - setSelection(M_Picked()); - - highlight_node_->getParentSceneNode()->removeAndDestroyChild(highlight_node_->getName()); - delete highlight_rectangle_; - - for (uint32_t i = 0; i < kNumRenderTextures_; ++i) { - delete[] reinterpret_cast(pixel_boxes_[i].data); - } - delete[] reinterpret_cast(depth_pixel_box_.data); - - delete property_model_; -} - -void SelectionManager::setDebugMode(bool debug) -{ - debug_mode_ = debug; -} - -void SelectionManager::initialize() -{ - // Create our render textures - setTextureSize(1); - - // Create our highlight rectangle - Ogre::SceneManager * scene_manager = vis_manager_->getSceneManager(); - highlight_node_ = scene_manager->getRootSceneNode()->createChildSceneNode(); - - std::stringstream ss; - static int count = 0; - ss << "SelectionRect" << count++; - highlight_rectangle_ = new Ogre::Rectangle2D(true); - - static const uint32_t texture_data[1] = {0xffff0080}; - Ogre::DataStreamPtr pixel_stream; - pixel_stream.reset(new Ogre::MemoryDataStream( - reinterpret_cast(const_cast(&texture_data[0])), - 4 - )); - - Ogre::TexturePtr tex = Ogre::TextureManager::getSingleton().loadRawData( - ss.str() + "Texture", - Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, - pixel_stream, - 1, - 1, - Ogre::PF_R8G8B8A8, - Ogre::TEX_TYPE_2D, - 0 - ); - - Ogre::MaterialPtr material = Ogre::MaterialManager::getSingleton().create( - ss.str(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); - material->setLightingEnabled(false); - // material->getTechnique(0)->getPass(0)->setPolygonMode(Ogre::PM_WIREFRAME); - highlight_rectangle_->setMaterial(material); - Ogre::AxisAlignedBox aabInf; - aabInf.setInfinite(); - highlight_rectangle_->setBoundingBox(aabInf); - highlight_rectangle_->setRenderQueueGroup(Ogre::RENDER_QUEUE_OVERLAY - 1); - material->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA); - material->setCullingMode(Ogre::CULL_NONE); - - Ogre::TextureUnitState * tex_unit = - material->getTechnique(0)->getPass(0)->createTextureUnitState(); - tex_unit->setTextureName(tex->getName()); - tex_unit->setTextureFiltering(Ogre::TFO_NONE); - - highlight_node_->attachObject(highlight_rectangle_); - - // create picking camera - camera_ = scene_manager->createCamera(ss.str() + "_camera"); - - // create fallback picking material - fallback_pick_material_ = Ogre::MaterialManager::getSingleton().getByName( - "rviz/DefaultPickAndDepth"); - // TODO(wjwwood): see why this fails to load - if (fallback_pick_material_) { - fallback_pick_material_->load(); - - fallback_pick_cull_technique_ = fallback_pick_material_->getTechnique("PickCull"); - fallback_black_cull_technique_ = fallback_pick_material_->getTechnique("BlackCull"); - fallback_depth_cull_technique_ = fallback_pick_material_->getTechnique("DepthCull"); - - fallback_pick_technique_ = fallback_pick_material_->getTechnique("Pick"); - fallback_black_technique_ = fallback_pick_material_->getTechnique("Black"); - fallback_depth_technique_ = fallback_pick_material_->getTechnique("Depth"); - } else { - RVIZ_COMMON_LOG_ERROR("failed to load material 'rviz/DefaultPickAndDepth'"); - } -} - - -bool SelectionManager::get3DPoint( - Ogre::Viewport * viewport, - int x, - int y, - Ogre::Vector3 & result_point) -{ - std::vector result_points_temp; - bool success = get3DPatch(viewport, x, y, 1, 1, true, result_points_temp); - if (result_points_temp.size() == 0) { - // return result_point unmodified if get point fails. - return false; - } - result_point = result_points_temp[0]; - - return success; -} - - -bool SelectionManager::getPatchDepthImage( - Ogre::Viewport * viewport, int x, int y, unsigned width, - unsigned height, std::vector & depth_vector) -{ - unsigned int num_pixels = width * height; - depth_vector.reserve(num_pixels); - - setDepthTextureSize(width, height); - - - M_CollisionObjectToSelectionHandler::iterator handler_it = objects_.begin(); - M_CollisionObjectToSelectionHandler::iterator handler_end = objects_.end(); - - for (; handler_it != handler_end; ++handler_it) { - handler_it->second->preRenderPass(0); - } - - if (render(viewport, depth_render_texture_, x, y, x + width, - y + height, depth_pixel_box_, "Depth", depth_texture_width_, depth_texture_height_)) - { - uint8_t * data_ptr = reinterpret_cast(depth_pixel_box_.data); - - for (uint32_t pixel = 0; pixel < num_pixels; ++pixel) { - uint8_t a = data_ptr[4 * pixel]; - uint8_t b = data_ptr[4 * pixel + 1]; - uint8_t c = data_ptr[4 * pixel + 2]; - - int int_depth = (c << 16) | (b << 8) | a; - float normalized_depth = (static_cast(int_depth)) / static_cast(0xffffff); - depth_vector.push_back(normalized_depth * camera_->getFarClipDistance()); - } - } else { - RVIZ_COMMON_LOG_WARNING("Failed to render depth patch\n"); - return false; - } - - handler_it = objects_.begin(); - handler_end = objects_.end(); - for (; handler_it != handler_end; ++handler_it) { - handler_it->second->postRenderPass(0); - } - - return true; -} - - -bool SelectionManager::get3DPatch( - Ogre::Viewport * viewport, - int x, - int y, - unsigned int width, - unsigned int height, - bool skip_missing, - std::vector & result_points) -{ - std::lock_guard lock(global_mutex_); - - std::vector depth_vector; - - if (!getPatchDepthImage(viewport, x, y, width, height, depth_vector)) { - return false; - } - - unsigned int pixel_counter = 0; - Ogre::Matrix4 projection = camera_->getProjectionMatrix(); - float depth; - - for (unsigned int y_iter = 0; y_iter < height; ++y_iter) { - for (unsigned int x_iter = 0; x_iter < width; ++x_iter) { - depth = depth_vector[pixel_counter]; - - // Deal with missing or invalid points - if ( ( depth > camera_->getFarClipDistance()) || ( depth == 0)) { - ++pixel_counter; - if (!skip_missing) { - result_points.push_back(Ogre::Vector3(NAN, NAN, NAN)); - } - continue; - } - - - Ogre::Vector3 result_point; - // We want to shoot rays through the center of pixels, not the corners, - // so add .5 pixels to the x and y coordinate to get to the center - // instead of the top left of the pixel. - Ogre::Real screenx = static_cast(x_iter + .5) / static_cast(width); - Ogre::Real screeny = static_cast(y_iter + .5) / static_cast(height); - if (projection[3][3] == 0.0) { // If this is a perspective projection - // get world-space ray from camera & mouse coord - Ogre::Ray vp_ray = camera_->getCameraToViewportRay(screenx, screeny); - - // transform ray direction back into camera coords - Ogre::Vector3 dir_cam = camera_->getDerivedOrientation().Inverse() * vp_ray.getDirection(); - - // normalize, so dir_cam.z == -depth - dir_cam = dir_cam / dir_cam.z * depth * -1; - - // compute 3d point from camera origin and direction*/ - result_point = camera_->getDerivedPosition() + camera_->getDerivedOrientation() * dir_cam; - } else { // else this must be an orthographic projection. - // For orthographic projection, getCameraToViewportRay() does - // the right thing for us, and the above math does not work. - Ogre::Ray ray; - camera_->getCameraToViewportRay(screenx, screeny, &ray); - - result_point = ray.getPoint(depth); - } - - result_points.push_back(result_point); - ++pixel_counter; - } - } - - return result_points.size() > 0; -} - - -void SelectionManager::setDepthTextureSize(unsigned width, unsigned height) -{ - // Cap and store requested texture size - // It's probably an error if an invalid size is requested. - if (width > 1024) { - width = 1024; - RVIZ_COMMON_LOG_ERROR_STREAM( - "SelectionManager::setDepthTextureSize invalid width requested. " - "Max Width: 1024 -- Width requested: " << width << ". Capping Width at 1024."); - } - - if (depth_texture_width_ != width) { - depth_texture_width_ = width; - } - - if (height > 1024) { - height = 1024; - RVIZ_COMMON_LOG_ERROR_STREAM( - "SelectionManager::setDepthTextureSize invalid height requested. " - "Max Height: 1024 -- Height requested: " << width << ". Capping Height at 1024."); - } - - if (depth_texture_height_ != height) { - depth_texture_height_ = height; - } - - if (!depth_render_texture_.get() || depth_render_texture_->getWidth() != width || - depth_render_texture_->getHeight() != height) - { - std::string tex_name = "DepthTexture"; - if (depth_render_texture_.get()) { - tex_name = depth_render_texture_->getName(); - - // destroy old - Ogre::TextureManager::getSingleton().remove( - tex_name, - Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); - } - - depth_render_texture_ = - Ogre::TextureManager::getSingleton().createManual(tex_name, - Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, - Ogre::TEX_TYPE_2D, depth_texture_width_, depth_texture_height_, 0, - Ogre::PF_R8G8B8, - Ogre::TU_RENDERTARGET); - - Ogre::RenderTexture * render_texture = depth_render_texture_->getBuffer()->getRenderTarget(); - render_texture->setAutoUpdated(false); - } -} - - -void SelectionManager::setTextureSize(unsigned size) -{ - if (size > 1024) { - size = 1024; - } - - texture_size_ = size; - - for (uint32_t pass = 0; pass < kNumRenderTextures_; ++pass) { - // check if we need to change the texture size - if (!render_textures_[pass].get() || render_textures_[pass]->getWidth() != size) { - std::string tex_name; - if (render_textures_[pass].get()) { - tex_name = render_textures_[pass]->getName(); - - // destroy old - Ogre::TextureManager::getSingleton().remove( - tex_name, - Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); - } else { - std::stringstream ss; - static int count = 0; - ss << "SelectionTexture" << count++; - tex_name = ss.str(); - } - - // create new texture - render_textures_[pass] = Ogre::TextureManager::getSingleton().createManual(tex_name, - Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, Ogre::TEX_TYPE_2D, size, size, 0, - Ogre::PF_R8G8B8, Ogre::TU_STATIC | Ogre::TU_RENDERTARGET); - - Ogre::RenderTexture * render_texture = render_textures_[pass]->getBuffer()->getRenderTarget(); - render_texture->setAutoUpdated(false); - } - } -} - -void SelectionManager::clearHandlers() -{ - std::lock_guard lock(global_mutex_); - - objects_.clear(); -} - -void SelectionManager::enableInteraction(bool enable) -{ - interaction_enabled_ = enable; - M_CollisionObjectToSelectionHandler::iterator handler_it = objects_.begin(); - M_CollisionObjectToSelectionHandler::iterator handler_end = objects_.end(); - for (; handler_it != handler_end; ++handler_it) { - if (InteractiveObjectPtr object = handler_it->second->getInteractiveObject().lock()) { - object->enableInteraction(enable); - } - } -} - -bool SelectionManager::getInteractionEnabled() const -{ - return interaction_enabled_; -} - -CollObjectHandle SelectionManager::createHandle() -{ - uid_counter_++; - if (uid_counter_ > 0x00ffffff) { - uid_counter_ = 0; - } - - uint32_t handle = 0; - - // shuffle around the bits so we get lots of colors - // when we're displaying the selection buffer - for (unsigned int i = 0; i < 24; i++) { - uint32_t shift = (((23 - i) % 3) * 8) + (23 - i) / 3; - uint32_t bit = ( (uint32_t)(uid_counter_ >> i) & (uint32_t)1) << shift; - handle |= bit; - } - - return handle; -} - -void SelectionManager::addObject(CollObjectHandle obj, SelectionHandler * handler) -{ - if (!obj) { -// ROS_BREAK(); - return; - } - - std::lock_guard lock(global_mutex_); - - InteractiveObjectPtr object = handler->getInteractiveObject().lock(); - if (object) { - object->enableInteraction(interaction_enabled_); - } - - bool inserted = objects_.insert(std::make_pair(obj, handler)).second; - (void) inserted; - assert(inserted); -} - -void SelectionManager::removeObject(CollObjectHandle obj) -{ - if (!obj) { - return; - } - - std::lock_guard lock(global_mutex_); - - M_Picked::iterator it = selection_.find(obj); - if (it != selection_.end()) { - M_Picked objs; - objs.insert(std::make_pair(it->first, it->second)); - - removeSelection(objs); - } - - objects_.erase(obj); -} - -void SelectionManager::update() -{ - std::lock_guard lock(global_mutex_); - - highlight_node_->setVisible(highlight_enabled_); - - if (highlight_enabled_) { - setHighlightRect(highlight_.viewport, highlight_.x1, highlight_.y1, highlight_.x2, - highlight_.y2); - -#if 0 - M_Picked results; - highlight_node_->setVisible(false); - pick(highlight_.viewport, highlight_.x1, highlight_.y1, highlight_.x2, highlight_.y2, results); - highlight_node_->setVisible(true); -#endif - } -} - -void SelectionManager::highlight(Ogre::Viewport * viewport, int x1, int y1, int x2, int y2) -{ - std::lock_guard lock(global_mutex_); - - highlight_enabled_ = true; - - highlight_.viewport = viewport; - highlight_.x1 = x1; - highlight_.y1 = y1; - highlight_.x2 = x2; - highlight_.y2 = y2; -} - -void SelectionManager::removeHighlight() -{ - std::lock_guard lock(global_mutex_); - - highlight_enabled_ = false; -} - -void SelectionManager::select( - Ogre::Viewport * viewport, int x1, int y1, int x2, int y2, - SelectType type) -{ - std::lock_guard lock(global_mutex_); - - highlight_enabled_ = false; - highlight_node_->setVisible(false); - - M_Picked results; - pick(viewport, x1, y1, x2, y2, results); - - if (type == Add) { - addSelection(results); - } else if (type == Remove) { - removeSelection(results); - } else if (type == Replace) { - setSelection(results); - } -} - -void SelectionManager::setHighlightRect(Ogre::Viewport * viewport, int x1, int y1, int x2, int y2) -{ - float nx1 = (static_cast(x1) / viewport->getActualWidth()) * 2 - 1; - float nx2 = (static_cast(x2) / viewport->getActualWidth()) * 2 - 1; - float ny1 = -((static_cast(y1) / viewport->getActualHeight()) * 2 - 1); - float ny2 = -((static_cast(y2) / viewport->getActualHeight()) * 2 - 1); - - nx1 = nx1 < -1 ? -1 : (nx1 > 1 ? 1 : nx1); - ny1 = ny1 < -1 ? -1 : (ny1 > 1 ? 1 : ny1); - nx2 = nx2 < -1 ? -1 : (nx2 > 1 ? 1 : nx2); - ny2 = ny2 < -1 ? -1 : (ny2 > 1 ? 1 : ny2); - - highlight_rectangle_->setCorners(nx1, ny1, nx2, ny2); -} - -void SelectionManager::unpackColors(const Ogre::PixelBox & box, V_CollObject & pixels) -{ - int w = box.getWidth(); - int h = box.getHeight(); - - pixels.clear(); - pixels.reserve(w * h); - - for (int y = 0; y < h; y++) { - for (int x = 0; x < w; x++) { - uint32_t pos = (x + y * w) * 4; - - uint32_t pix_val = - *reinterpret_cast(reinterpret_cast(box.data) + pos); - uint32_t handle = colorToHandle(box.format, pix_val); - - pixels.push_back(handle); - } - } -} - -void SelectionManager::renderAndUnpack( - Ogre::Viewport * viewport, uint32_t pass, int x1, int y1, - int x2, int y2, V_CollObject & pixels) -{ - assert(pass < kNumRenderTextures_); - - std::stringstream scheme; - scheme << "Pick"; - if (pass > 0) { - scheme << pass; - } - - if (render(viewport, render_textures_[pass], x1, y1, x2, y2, pixel_boxes_[pass], scheme.str(), - texture_size_, texture_size_)) - { - unpackColors(pixel_boxes_[pass], pixels); - } -} - - -bool SelectionManager::render( - Ogre::Viewport * viewport, Ogre::TexturePtr tex, - int x1, int y1, int x2, int y2, - Ogre::PixelBox & dst_box, std::string material_scheme, - unsigned texture_width, unsigned texture_height) -{ - vis_manager_->lockRender(); - - if (x1 > x2) {std::swap(x1, x2);} - if (y1 > y2) {std::swap(y1, y2);} - - if (x1 < 0) {x1 = 0;} - if (y1 < 0) {y1 = 0;} - if (x1 > viewport->getActualWidth() - 2) {x1 = viewport->getActualWidth() - 2;} - if (y1 > viewport->getActualHeight() - 2) {y1 = viewport->getActualHeight() - 2;} - if (x2 < 0) {x2 = 0;} - if (y2 < 0) {y2 = 0;} - if (x2 > viewport->getActualWidth() - 2) {x2 = viewport->getActualWidth() - 2;} - if (y2 > viewport->getActualHeight() - 2) {y2 = viewport->getActualHeight() - 2;} - - if (x2 == x1) {x2++;} - if (y2 == y1) {y2++;} - - if (x2 == x1 || y2 == y1) { - RVIZ_COMMON_LOG_WARNING("SelectionManager::render(): not rendering 0 size area."); - vis_manager_->unlockRender(); - return false; - } - - unsigned w = x2 - x1; - unsigned h = y2 - y1; - - Ogre::HardwarePixelBufferSharedPtr pixel_buffer = tex->getBuffer(); - Ogre::RenderTexture * render_texture = pixel_buffer->getRenderTarget(); - - Ogre::Matrix4 proj_matrix = viewport->getCamera()->getProjectionMatrix(); - Ogre::Matrix4 scale_matrix = Ogre::Matrix4::IDENTITY; - Ogre::Matrix4 trans_matrix = Ogre::Matrix4::IDENTITY; - - float x1_rel = static_cast(x1) / static_cast(viewport->getActualWidth() - 1) - 0.5f; - float y1_rel = static_cast(y1) / static_cast(viewport->getActualHeight() - 1) - - 0.5f; - float x2_rel = static_cast(x2) / static_cast(viewport->getActualWidth() - 1) - 0.5f; - float y2_rel = static_cast(y2) / static_cast(viewport->getActualHeight() - 1) - - 0.5f; - - scale_matrix[0][0] = 1.0 / (x2_rel - x1_rel); - scale_matrix[1][1] = 1.0 / (y2_rel - y1_rel); - - trans_matrix[0][3] -= x1_rel + x2_rel; - trans_matrix[1][3] += y1_rel + y2_rel; - - camera_->setCustomProjectionMatrix(true, scale_matrix * trans_matrix * proj_matrix); - - camera_->setPosition(viewport->getCamera()->getDerivedPosition()); - camera_->setOrientation(viewport->getCamera()->getDerivedOrientation()); - - // create a viewport if there is none - if (render_texture->getNumViewports() == 0) { - render_texture->removeAllViewports(); - render_texture->addViewport(camera_); - Ogre::Viewport * render_viewport = render_texture->getViewport(0); - render_viewport->setClearEveryFrame(true); - render_viewport->setBackgroundColour(Ogre::ColourValue::Black); - render_viewport->setOverlaysEnabled(false); - render_viewport->setMaterialScheme(material_scheme); - } - - unsigned render_w = w; - unsigned render_h = h; - - if (w > h) { - if (render_w > texture_width) { - render_w = texture_width; - render_h = round( - static_cast(h) * static_cast(texture_width) / static_cast(w) - ); - } - } else { - if (render_h > texture_height) { - render_h = texture_height; - render_w = round( - static_cast(w) * static_cast(texture_height) / static_cast(h) - ); - } - } - - // safety clamping in case of rounding errors - if (render_w > texture_width) {render_w = texture_width;} - if (render_h > texture_height) {render_h = texture_height;} - - // set viewport to render to a subwindow of the texture - Ogre::Viewport * render_viewport = render_texture->getViewport(0); - render_viewport->setDimensions( - 0, - 0, - static_cast(render_w) / static_cast(texture_width), - static_cast(render_h) / static_cast(texture_height) - ); - - // make sure the same objects are visible as in the original viewport - render_viewport->setVisibilityMask(viewport->getVisibilityMask()); - - // update & force ogre to render the scene - Ogre::MaterialManager::getSingleton().addListener(this); - - render_texture->update(); - - // For some reason we need to pretend to render the main window in - // order to get the picking render to show up in the pixelbox below. - // If we don't do this, it will show up there the *next* time we - // pick something, but not this time. This object as a - // render queue listener tells the scene manager to skip every - // render step, so nothing actually gets drawn. - // - // TODO(unknown): find out what part of _renderScene() actually makes this work. - using rviz_rendering::RenderWindowOgreAdapter; - Ogre::Viewport * main_view = - RenderWindowOgreAdapter::getOgreViewport(vis_manager_->getRenderPanel()->getRenderWindow()); - vis_manager_->getSceneManager()->addRenderQueueListener(this); - vis_manager_->getSceneManager()->_renderScene(main_view->getCamera(), main_view, false); - vis_manager_->getSceneManager()->removeRenderQueueListener(this); - - Ogre::MaterialManager::getSingleton().removeListener(this); - - render_w = render_viewport->getActualWidth(); - render_h = render_viewport->getActualHeight(); - - Ogre::PixelFormat format = pixel_buffer->getFormat(); - - int size = static_cast(Ogre::PixelUtil::getMemorySize(render_w, render_h, 1, format)); - uint8_t * data = new uint8_t[size]; - - delete[] reinterpret_cast(dst_box.data); - dst_box = Ogre::PixelBox(render_w, render_h, 1, format, data); - - pixel_buffer->blitToMemory(dst_box, dst_box); - - vis_manager_->unlockRender(); - - if (debug_mode_) { - // TODO(wjwwood): determine if this is still needed. -#if 0 - publishDebugImage(dst_box, material_scheme); -#endif - RVIZ_COMMON_LOG_ERROR("publishDebugImage not implemented"); - } - - return true; -} - -// TODO(wjwwood): determine if this is still needed. -#if 0 -void SelectionManager::publishDebugImage( - const Ogre::PixelBox & pixel_box, - const std::string & label) -{ - ros::Publisher pub; - ros::NodeHandle nh; - PublisherMap::const_iterator iter = debug_publishers_.find(label); - if (iter == debug_publishers_.end()) { - pub = nh.advertise("/rviz_debug/" + label, 2); - debug_publishers_[label] = pub; - } else { - pub = iter->second; - } - - sensor_msgs::Image msg; - msg.header.stamp = ros::Time::now(); - msg.width = pixel_box.getWidth(); - msg.height = pixel_box.getHeight(); - msg.encoding = sensor_msgs::image_encodings::RGB8; - msg.is_bigendian = false; - msg.step = msg.width * 3; - int dest_byte_count = msg.width * msg.height * 3; - msg.data.resize(dest_byte_count); - int dest_index = 0; - uint8_t * source_ptr = reinterpret_cast(pixel_box.data); - int pre_pixel_padding = 0; - int post_pixel_padding = 0; - switch (pixel_box.format) { - case Ogre::PF_A8R8G8B8: - case Ogre::PF_X8R8G8B8: - post_pixel_padding = 1; - break; - case Ogre::PF_R8G8B8A8: - pre_pixel_padding = 1; - break; - default: - ROS_ERROR("SelectionManager::publishDebugImage(): Incompatible pixel format [%d]", - pixel_box.format); - return; - } - uint8_t r, g, b; - while (dest_index < dest_byte_count) { - source_ptr += pre_pixel_padding; - b = *source_ptr++; - g = *source_ptr++; - r = *source_ptr++; - source_ptr += post_pixel_padding; - msg.data[dest_index++] = r; - msg.data[dest_index++] = g; - msg.data[dest_index++] = b; - } - - pub.publish(msg); -} -#endif - -void SelectionManager::renderQueueStarted( - uint8_t queueGroupId, - const std::string & invocation, - bool & skipThisInvocation) -{ - Q_UNUSED(queueGroupId); - Q_UNUSED(invocation); - // This render queue listener function tells the scene manager to - // skip every render step, so nothing actually gets drawn. - - // ROS_DEBUG( - // "SelectionManager renderQueueStarted(%d, '%s') returning skip = true.", - // (int)queueGroupId, - // invocation.c_str()); - skipThisInvocation = true; -} - -PropertyTreeModel * SelectionManager::getPropertyModel() -{ - return property_model_; -} - -void SelectionManager::pick( - Ogre::Viewport * viewport, int x1, int y1, int x2, int y2, - M_Picked & results, bool single_render_pass) -{ - std::lock_guard lock(global_mutex_); - - bool need_additional_render = false; - - V_CollObject handles_by_pixel; - S_CollObject need_additional; - - V_CollObject & pixels = pixel_buffer_; - - // First render is special... does the initial object picking, determines - // which objects have been selected. - // After that, individual handlers can specify that they need additional - // renders (max # defined in kNumRenderTextures_). - { - M_CollisionObjectToSelectionHandler::iterator handler_it = objects_.begin(); - M_CollisionObjectToSelectionHandler::iterator handler_end = objects_.end(); - for (; handler_it != handler_end; ++handler_it) { - handler_it->second->preRenderPass(0); - } - - renderAndUnpack(viewport, 0, x1, y1, x2, y2, pixels); - - handler_it = objects_.begin(); - handler_end = objects_.end(); - for (; handler_it != handler_end; ++handler_it) { - handler_it->second->postRenderPass(0); - } - - handles_by_pixel.reserve(pixels.size()); - V_CollObject::iterator it = pixels.begin(); - V_CollObject::iterator end = pixels.end(); - for (; it != end; ++it) { - const CollObjectHandle & p = *it; - - CollObjectHandle handle = p; - - handles_by_pixel.push_back(handle); - - if (handle == 0) { - continue; - } - - SelectionHandler * handler = getHandler(handle); - - if (handler) { - std::pair insert_result = results.insert(std::make_pair(handle, Picked(handle))); - if (insert_result.second) { - if (handler->needsAdditionalRenderPass(1) && !single_render_pass) { - need_additional.insert(handle); - need_additional_render = true; - } - } else { - insert_result.first->second.pixel_count++; - } - } - } - } - - uint32_t pass = 1; - - V_uint64 extra_by_pixel; - extra_by_pixel.resize(handles_by_pixel.size()); - while (need_additional_render && pass < kNumRenderTextures_) { - { - S_CollObject::iterator need_it = need_additional.begin(); - S_CollObject::iterator need_end = need_additional.end(); - for (; need_it != need_end; ++need_it) { - SelectionHandler * handler = getHandler(*need_it); - assert(handler); - - handler->preRenderPass(pass); - } - } - - renderAndUnpack(viewport, pass, x1, y1, x2, y2, pixels); - - { - S_CollObject::iterator need_it = need_additional.begin(); - S_CollObject::iterator need_end = need_additional.end(); - for (; need_it != need_end; ++need_it) { - SelectionHandler * handler = getHandler(*need_it); - assert(handler); - - handler->postRenderPass(pass); - } - } - - int i = 0; - V_CollObject::iterator pix_it = pixels.begin(); - V_CollObject::iterator pix_end = pixels.end(); - for (; pix_it != pix_end; ++pix_it, ++i) { - const CollObjectHandle & p = *pix_it; - - CollObjectHandle handle = handles_by_pixel[i]; - - if (pass == 1) { - extra_by_pixel[i] = 0; - } - - if (need_additional.find(handle) != need_additional.end()) { - CollObjectHandle extra_handle = p; - extra_by_pixel[i] |= extra_handle << (32 * (pass - 1)); - } else { - extra_by_pixel[i] = 0; - } - } - - need_additional_render = false; - need_additional.clear(); - M_Picked::iterator handle_it = results.begin(); - M_Picked::iterator handle_end = results.end(); - for (; handle_it != handle_end; ++handle_it) { - CollObjectHandle handle = handle_it->first; - - if (getHandler(handle)->needsAdditionalRenderPass(pass + 1)) { - need_additional_render = true; - need_additional.insert(handle); - } - } - } - - int i = 0; - V_uint64::iterator pix_2_it = extra_by_pixel.begin(); - V_uint64::iterator pix_2_end = extra_by_pixel.end(); - for (; pix_2_it != pix_2_end; ++pix_2_it, ++i) { - CollObjectHandle handle = handles_by_pixel[i]; - - if (handle == 0) { - continue; - } - - M_Picked::iterator picked_it = results.find(handle); - if (picked_it == results.end()) { - continue; - } - - Picked & picked = picked_it->second; - - if (*pix_2_it) { - picked.extra_handles.insert(*pix_2_it); - } - } -} - -Ogre::Technique * SelectionManager::handleSchemeNotFound( - unsigned short scheme_index, // NOLINT: Ogre decides the use of unsigned short - const Ogre::String & scheme_name, - Ogre::Material * original_material, - unsigned short lod_index, // NOLINT: Ogre decides the use of unsigned short - const Ogre::Renderable * rend) -{ - Q_UNUSED(scheme_index); - Q_UNUSED(lod_index); - // Find the original culling mode - Ogre::CullingMode culling_mode = Ogre::CULL_CLOCKWISE; - Ogre::Technique * orig_tech = original_material->getTechnique(0); - if (orig_tech && orig_tech->getNumPasses() > 0) { - culling_mode = orig_tech->getPass(0)->getCullingMode(); - } - - // find out if the renderable has the picking param set - bool has_pick_param = rend->getUserObjectBindings().getUserAny("pick_handle").has_value(); - - // NOTE: it is important to avoid changing the culling mode of the - // fallback techniques here, because that change then propagates to - // other uses of these fallback techniques in unpredictable ways. - // If you want to change the technique programmatically (like with - // Pass::setCullingMode()), make sure you do it on a cloned material - // which doesn't get shared with other objects. - - // Use the technique with the right name and culling mode. - if (culling_mode == Ogre::CULL_CLOCKWISE) { - if (scheme_name == "Pick") { - return has_pick_param ? fallback_pick_cull_technique_ : fallback_black_cull_technique_; - } else if (scheme_name == "Depth") { - return fallback_depth_cull_technique_; - } - if (scheme_name == "Pick1") { - return fallback_black_cull_technique_; - } else { - return NULL; - } - } else { // Must be CULL_NONE because we never use CULL_ANTICLOCKWISE - if (scheme_name == "Pick") { - return has_pick_param ? fallback_pick_technique_ : fallback_black_technique_; - } else if (scheme_name == "Depth") { - return fallback_depth_technique_; - } - if (scheme_name == "Pick1") { - return fallback_black_technique_; - } else { - return NULL; - } - } -} - -Ogre::ColourValue SelectionManager::handleToColor(CollObjectHandle handle) -{ - float r = ((handle >> 16) & 0xff) / 255.0f; - float g = ((handle >> 8) & 0xff) / 255.0f; - float b = (handle & 0xff) / 255.0f; - return Ogre::ColourValue(r, g, b, 1.0f); -} - -void SelectionManager::setPickColor(const Ogre::ColourValue & color, Ogre::SceneNode * node) -{ - setPickData(colorToHandle(color), color, node); -} - -void SelectionManager::setPickColor(const Ogre::ColourValue & color, Ogre::MovableObject * object) -{ - setPickData(colorToHandle(color), color, object); -} - -void SelectionManager::setPickHandle(CollObjectHandle handle, Ogre::SceneNode * node) -{ - setPickData(handle, handleToColor(handle), node); -} - -void SelectionManager::setPickHandle(CollObjectHandle handle, Ogre::MovableObject * object) -{ - setPickData(handle, handleToColor(handle), object); -} - -void SelectionManager::setPickData( - CollObjectHandle handle, const Ogre::ColourValue & color, - Ogre::SceneNode * node) -{ - if (!node) { - return; - } - // Loop over all objects attached to this node. - Ogre::SceneNode::ObjectIterator obj_it = node->getAttachedObjectIterator(); - while (obj_it.hasMoreElements()) { - Ogre::MovableObject * obj = obj_it.getNext(); - setPickData(handle, color, obj); - } - // Loop over and recurse into all child nodes. - Ogre::SceneNode::ChildNodeIterator child_it = node->getChildIterator(); - while (child_it.hasMoreElements()) { - Ogre::SceneNode * child = dynamic_cast( child_it.getNext()); - setPickData(handle, color, child); - } -} - -class PickColorSetter : public Ogre::Renderable::Visitor -{ -public: - PickColorSetter(CollObjectHandle handle, const Ogre::ColourValue & color) - : color_vector_(color.r, color.g, color.b, 1.0), handle_(handle) {} - - virtual void visit(Ogre::Renderable * rend, ushort lodIndex, bool isDebug, Ogre::Any * pAny = 0) - { - Q_UNUSED(lodIndex); - Q_UNUSED(isDebug); - Q_UNUSED(pAny); - rend->setCustomParameter(RVIZ_RENDERING_PICK_COLOR_PARAMETER, color_vector_); - rend->getUserObjectBindings().setUserAny("pick_handle", Ogre::Any(handle_)); - } - - Ogre::Vector4 color_vector_; - CollObjectHandle handle_; -}; - -void SelectionManager::setPickData( - CollObjectHandle handle, const Ogre::ColourValue & color, - Ogre::MovableObject * object) -{ - PickColorSetter visitor(handle, color); - object->visitRenderables(&visitor); - object->getUserObjectBindings().setUserAny("pick_handle", Ogre::Any(handle)); -} - -SelectionHandler * SelectionManager::getHandler(CollObjectHandle obj) -{ - std::lock_guard lock(global_mutex_); - - M_CollisionObjectToSelectionHandler::iterator it = objects_.find(obj); - if (it != objects_.end()) { - return it->second; - } - - return NULL; -} - -void SelectionManager::removeSelection(const M_Picked & objs) -{ - std::lock_guard lock(global_mutex_); - - M_Picked::const_iterator it = objs.begin(); - M_Picked::const_iterator end = objs.end(); - for (; it != end; ++it) { - removeSelectedObject(it->second); - } - - selectionRemoved(objs); -} - -const M_Picked & SelectionManager::getSelection() const -{ - return selection_; -} - -void SelectionManager::addSelection(const M_Picked & objs) -{ - std::lock_guard lock(global_mutex_); - - M_Picked added; - M_Picked::const_iterator it = objs.begin(); - M_Picked::const_iterator end = objs.end(); - for (; it != end; ++it) { - std::pair ppb = addSelectedObject(it->second); - if (ppb.second) { - added.insert(std::make_pair(it->first, ppb.first)); - } - } - - selectionAdded(added); -} - -void SelectionManager::setSelection(const M_Picked & objs) -{ - std::lock_guard lock(global_mutex_); - - M_Picked original(selection_.begin(), selection_.end()); - - removeSelection(original); - addSelection(objs); -} - -std::pair SelectionManager::addSelectedObject(const Picked & obj) -{ - std::lock_guard lock(global_mutex_); - - std::pair pib = selection_.insert(std::make_pair(obj.handle, obj)); - - SelectionHandler * handler = getHandler(obj.handle); - - if (pib.second) { - handler->onSelect(obj); - return std::make_pair(obj, true); - } else { - Picked & cur = pib.first->second; - Picked added(cur.handle); - - S_uint64::iterator it = obj.extra_handles.begin(); - S_uint64::iterator end = obj.extra_handles.end(); - for (; it != end; ++it) { - if (cur.extra_handles.insert(*it).second) { - added.extra_handles.insert(*it); - } - } - - if (!added.extra_handles.empty()) { - handler->onSelect(added); - - return std::make_pair(added, true); - } - } - - return std::make_pair(Picked(0), false); -} - -void SelectionManager::removeSelectedObject(const Picked & obj) -{ - std::lock_guard lock(global_mutex_); - - M_Picked::iterator sel_it = selection_.find(obj.handle); - if (sel_it != selection_.end()) { - S_uint64::iterator extra_it = obj.extra_handles.begin(); - S_uint64::iterator extra_end = obj.extra_handles.end(); - for (; extra_it != extra_end; ++extra_it) { - sel_it->second.extra_handles.erase(*extra_it); - } - - if (sel_it->second.extra_handles.empty()) { - selection_.erase(sel_it); - } - } - - SelectionHandler * handler = getHandler(obj.handle); - handler->onDeselect(obj); -} - -void SelectionManager::focusOnSelection() -{ - std::lock_guard lock(global_mutex_); - - if (selection_.empty()) { - return; - } - - Ogre::AxisAlignedBox combined; - - M_Picked::iterator it = selection_.begin(); - M_Picked::iterator end = selection_.end(); - for (; it != end; ++it) { - const Picked & p = it->second; - - SelectionHandler * handler = getHandler(p.handle); - - V_AABB aabbs; - handler->getAABBs(p, aabbs); - - V_AABB::iterator aabb_it = aabbs.begin(); - V_AABB::iterator aabb_end = aabbs.end(); - for (; aabb_it != aabb_end; ++aabb_it) { - combined.merge(*aabb_it); - } - } - - if (!combined.isInfinite() && !combined.isNull()) { - Ogre::Vector3 center = combined.getCenter(); - ViewController * controller = vis_manager_->getViewManager()->getCurrent(); - if (controller) { - controller->lookAt(center); - } - } -} - -void SelectionManager::selectionRemoved(const M_Picked & removed) -{ - M_Picked::const_iterator it = removed.begin(); - M_Picked::const_iterator end = removed.end(); - for (; it != end; ++it) { - const Picked & picked = it->second; - SelectionHandler * handler = getHandler(picked.handle); - assert(handler); - - handler->destroyProperties(picked, property_model_->getRoot()); - } -} - -void SelectionManager::selectionAdded(const M_Picked & added) -{ - M_Picked::const_iterator it = added.begin(); - M_Picked::const_iterator end = added.end(); - for (; it != end; ++it) { - const Picked & picked = it->second; - SelectionHandler * handler = getHandler(picked.handle); - assert(handler); - - handler->createProperties(picked, property_model_->getRoot()); - } - property_model_->sort(0, Qt::AscendingOrder); -} - -void SelectionManager::updateProperties() -{ - M_Picked::const_iterator it = selection_.begin(); - M_Picked::const_iterator end = selection_.end(); - for (; it != end; ++it) { - CollObjectHandle handle = it->first; - SelectionHandler * handler = getHandler(handle); - - handler->updateProperties(); - } -} - - -} // namespace selection -} // namespace rviz_common diff --git a/rviz_common/src/rviz_common/selection_panel.cpp b/rviz_common/src/rviz_common/selection_panel.cpp index 1d9e1690e..0055d1c13 100644 --- a/rviz_common/src/rviz_common/selection_panel.cpp +++ b/rviz_common/src/rviz_common/selection_panel.cpp @@ -27,30 +27,30 @@ * POSSIBILITY OF SUCH DAMAGE. */ +#include "selection_panel.hpp" + #include #include "rviz_common/properties/property_tree_widget.hpp" -#include "rviz_common/selection/selection_manager.hpp" -#include "rviz_common/visualization_manager.hpp" - -#include "rviz_common/selection_panel.hpp" +#include "rviz_common/interaction/selection_manager.hpp" +#include "visualization_manager.hpp" -namespace rviz +namespace rviz_common { SelectionPanel::SelectionPanel(QWidget * parent) -: Panel(parent) +: rviz_common::Panel(parent) { - QVBoxLayout * layout = new QVBoxLayout(); + auto layout = new QVBoxLayout(); layout->setContentsMargins(0, 0, 0, 0); - tree_widget_ = new PropertyTreeWidget(); + tree_widget_ = new properties::PropertyTreeWidget(); layout->addWidget(tree_widget_); setLayout(layout); } void SelectionPanel::onInitialize() { - tree_widget_->setModel(vis_manager_->getSelectionManager()->getPropertyModel() ); + tree_widget_->setModel(vis_manager_->getSelectionManager()->getPropertyModel()); } -} // namespace rviz +} // namespace rviz_common diff --git a/rviz_common/src/rviz_common/selection_panel.hpp b/rviz_common/src/rviz_common/selection_panel.hpp index f43fc3c9b..e7f7396c0 100644 --- a/rviz_common/src/rviz_common/selection_panel.hpp +++ b/rviz_common/src/rviz_common/selection_panel.hpp @@ -30,26 +30,28 @@ #ifndef RVIZ_COMMON__SELECTION_PANEL_HPP_ #define RVIZ_COMMON__SELECTION_PANEL_HPP_ -#include "rviz_common/panel.hpp" +#include "panel.hpp" -namespace rviz +namespace rviz_common +{ +namespace properties { - class PropertyTreeWidget; +} class SelectionPanel : public Panel { Q_OBJECT public: - explicit SelectionPanel(QWidget * parent = 0); + explicit SelectionPanel(QWidget * parent = nullptr); - virtual void onInitialize(); + void onInitialize() override; private: - PropertyTreeWidget * tree_widget_; + properties::PropertyTreeWidget * tree_widget_; }; -} // namespace rviz +} // namespace rviz_common #endif // RVIZ_COMMON__SELECTION_PANEL_HPP_ diff --git a/rviz_common/src/rviz_common/tool.cpp b/rviz_common/src/rviz_common/tool.cpp index 0bef8b803..c9e539192 100644 --- a/rviz_common/src/rviz_common/tool.cpp +++ b/rviz_common/src/rviz_common/tool.cpp @@ -62,10 +62,6 @@ void Tool::initialize(DisplayContext * context) onInitialize(); } -void Tool::onInitialize() -{ -} - rviz_common::properties::Property * Tool::getPropertyContainer() const { return property_container_; diff --git a/rviz_common/src/rviz_common/view_controller.cpp b/rviz_common/src/rviz_common/view_controller.cpp index 818832200..5bd91071a 100644 --- a/rviz_common/src/rviz_common/view_controller.cpp +++ b/rviz_common/src/rviz_common/view_controller.cpp @@ -56,7 +56,7 @@ #include "rviz_common/load_resource.hpp" #include "rviz_common/properties/bool_property.hpp" #include "rviz_common/properties/float_property.hpp" -#include "rviz_common/selection/selection_manager.hpp" +#include "rviz_common/interaction/view_picker_iface.hpp" #include "rviz_common/render_panel.hpp" namespace rviz_common @@ -227,12 +227,10 @@ void ViewController::save(Config config) const void ViewController::handleKeyEvent(QKeyEvent * event, RenderPanel * panel) { - Ogre::Viewport * viewport = - rviz_rendering::RenderWindowOgreAdapter::getOgreViewport(panel->getRenderWindow()); - if (event->key() == Qt::Key_F && viewport && context_->getSelectionManager()) { + if (event->key() == Qt::Key_F && context_->getViewPicker()) { QPoint mouse_rel_panel = panel->mapFromGlobal(QCursor::pos()); Ogre::Vector3 point_rel_world; // output of get3DPoint(). - if (context_->getSelectionManager()->get3DPoint(viewport, + if (context_->getViewPicker()->get3DPoint(panel, mouse_rel_panel.x(), mouse_rel_panel.y(), point_rel_world)) { diff --git a/rviz_common/src/rviz_common/visualization_frame.cpp b/rviz_common/src/rviz_common/visualization_frame.cpp index b2f64bd13..f1f8224e7 100644 --- a/rviz_common/src/rviz_common/visualization_frame.cpp +++ b/rviz_common/src/rviz_common/visualization_frame.cpp @@ -94,7 +94,7 @@ // #include "./displays_panel.hpp" // #include "./help_panel.hpp" -// #include "./selection/selection_manager.hpp" +// #include "./interaction/selection_manager.hpp" // #include "./selection_panel.hpp" // #include "./time_panel.hpp" // #include "./tool_properties_panel.hpp" diff --git a/rviz_common/src/rviz_common/visualization_manager.cpp b/rviz_common/src/rviz_common/visualization_manager.cpp index 9d40fead0..a0a4304c2 100644 --- a/rviz_common/src/rviz_common/visualization_manager.cpp +++ b/rviz_common/src/rviz_common/visualization_manager.cpp @@ -86,8 +86,12 @@ #include "rviz_common/properties/status_list.hpp" #include "rviz_common/properties/tf_frame_property.hpp" #include "rviz_common/render_panel.hpp" -#include "rviz_common/selection/selection_manager.hpp" -#include "rviz_common/selection/selection_manager_iface.hpp" +#include "rviz_common/interaction/handler_manager.hpp" +#include "rviz_common/interaction/handler_manager_iface.hpp" +#include "rviz_common/interaction/selection_manager.hpp" +#include "rviz_common/interaction/selection_manager_iface.hpp" +#include "rviz_common/interaction/view_picker.hpp" +#include "rviz_common/interaction/view_picker_iface.hpp" #include "rviz_common/tool.hpp" #include "./tool_manager.hpp" // #include "rviz_common/view_controller.hpp" @@ -105,9 +109,10 @@ using rviz_common::properties::PropertyTreeModel; using rviz_common::properties::StatusList; using rviz_common::properties::StatusProperty; using rviz_common::properties::TfFrameProperty; -using rviz_common::selection::SelectionManager; -using rviz_common::selection::SelectionManagerIface; -using rviz_common::selection::M_Picked; +using rviz_common::interaction::HandlerManager; +using rviz_common::interaction::SelectionManager; +using rviz_common::interaction::ViewPicker; +using rviz_common::interaction::M_Picked; // helper class needed to display an icon besides "Global Options" class IconizedProperty : public rviz_common::properties::Property @@ -222,7 +227,9 @@ VisualizationManager::VisualizationManager( rviz_rendering::MaterialManager::createDefaultColorMaterials(); - selection_manager_ = new SelectionManager(this); + handler_manager_ = std::make_shared(); + selection_manager_ = std::make_shared(this); + view_picker_ = std::make_shared(this); // TODO(wjwwood): redo with executors? #if 0 @@ -251,14 +258,9 @@ VisualizationManager::~VisualizationManager() private_->threaded_queue_threads_.join_all(); #endif - if (selection_manager_) { - selection_manager_->setSelection(M_Picked()); - } - delete display_property_tree_model_; delete tool_manager_; delete display_factory_; - delete selection_manager_; delete frame_manager_; delete private_; @@ -274,6 +276,7 @@ void VisualizationManager::initialize() view_manager_->initialize(); selection_manager_->initialize(); + view_picker_->initialize(); tool_manager_->initialize(); last_update_ros_time_ = clock_->now(); @@ -498,11 +501,24 @@ void VisualizationManager::resetTime() queueRender(); } -SelectionManagerIface * VisualizationManager::getSelectionManager() const +std::shared_ptr +VisualizationManager::getHandlerManager() const +{ + return handler_manager_; +} + +std::shared_ptr +VisualizationManager::getSelectionManager() const { return selection_manager_; } +std::shared_ptr +VisualizationManager::getViewPicker() const +{ + return view_picker_; +} + ToolManager * VisualizationManager::getToolManager() const { return tool_manager_; diff --git a/rviz_common/src/rviz_common/visualization_manager.hpp b/rviz_common/src/rviz_common/visualization_manager.hpp index b30ce1d90..67ea6699f 100644 --- a/rviz_common/src/rviz_common/visualization_manager.hpp +++ b/rviz_common/src/rviz_common/visualization_manager.hpp @@ -80,7 +80,7 @@ class VisualizationManagerPrivate; * It keeps the current view controller for the main render window. * It has a timer which calls update() on all the displays. * It creates and holds pointers to the other manager objects: - * SelectionManager, FrameManager, the PropertyManagers, and Ogre::SceneManager. + * HandlerManager, SelectionManager, FrameManager, the PropertyManagers, and Ogre::SceneManager. * * The "protected" members should probably all be "private", as * VisualizationManager is not intended to be subclassed. @@ -110,7 +110,7 @@ class VisualizationManager : public DisplayContext /** * Stops the update of timers and destroys all displays, tools, and managers. */ - virtual ~VisualizationManager(); + ~VisualizationManager() override; /// Do initialization that was not done in constructor. /** @@ -217,8 +217,15 @@ class VisualizationManager : public DisplayContext /// Resets the wall and ROS elapsed time to zero and calls resetDisplays(). void resetTime(); + /// Return a pointer to the HandlerManager + std::shared_ptr getHandlerManager() const override; + /// Return a pointer to the SelectionManager. - rviz_common::selection::SelectionManagerIface * getSelectionManager() const override; + std::shared_ptr + getSelectionManager() const override; + + /// Return a pointer to the ViewPicker. + std::shared_ptr getViewPicker() const override; /// Return a pointer to the ToolManager. ToolManager * getToolManager() const override; @@ -227,10 +234,10 @@ class VisualizationManager : public DisplayContext ViewManager * getViewManager() const override; /// Lock a mutex to delay calls to Ogre::Root::renderOneFrame(). - void lockRender(); + void lockRender() override; /// Unlock a mutex, allowing calls to Ogre::Root::renderOneFrame(). - void unlockRender(); + void unlockRender() override; /// Queues a render. /** @@ -365,7 +372,9 @@ protected Q_SLOTS: float time_update_timer_; float frame_update_timer_; - rviz_common::selection::SelectionManagerIface * selection_manager_; + std::shared_ptr handler_manager_; + std::shared_ptr selection_manager_; + std::shared_ptr view_picker_; uint32_t render_requested_; uint64_t frame_count_; diff --git a/rviz_common/src/rviz_common/visualizer_app.cpp b/rviz_common/src/rviz_common/visualizer_app.cpp index b145f11ef..6e7b33aea 100644 --- a/rviz_common/src/rviz_common/visualizer_app.cpp +++ b/rviz_common/src/rviz_common/visualizer_app.cpp @@ -43,7 +43,7 @@ #include "rviz_common/logging.hpp" -#include "rviz_common/selection/selection_manager.hpp" +#include "rviz_common/interaction/selection_manager.hpp" #include "./visualization_frame.hpp" #include "./visualization_manager.hpp" @@ -169,13 +169,11 @@ bool VisualizerApp::init(int argc, char ** argv) // "Force OpenGL version (use '--opengl 210' for OpenGL 2.1 compatibility mode)") // ("disable-anti-aliasing", "Prevent rviz from trying to use anti-aliasing when rendering.") // ("no-stereo", "Disable the use of stereo rendering.") - // ("verbose,v", "Enable debug visualizations") // ("log-level-debug", "Sets the ROS logger level to debug."); // po::variables_map vm; // std::string display_config, fixed_frame, splash_path, help_path; // bool enable_ogre_log = false; // bool in_mc_wrapper = false; - // bool verbose = false; // int force_gl_version = 0; // bool disable_anti_aliasing = false; // bool disable_stereo = false; @@ -240,10 +238,6 @@ bool VisualizerApp::init(int argc, char ** argv) // disable_anti_aliasing = true; // } // - // if (vm.count("verbose")) { - // verbose = true; - // } - // // if (vm.count("log-level-debug")) { // if ( // ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug)) @@ -304,9 +298,6 @@ bool VisualizerApp::init(int argc, char ** argv) // frame_->getManager()->setFixedFrame(QString::fromStdString(fixed_frame)); // } - // frame_->getManager()->getSelectionManager()->setDebugMode(verbose); - frame_->getManager()->getSelectionManager()->setDebugMode(false); - frame_->show(); // TODO(wjwwood): reenable the ROS service to reload the shaders via the ros_integration API diff --git a/rviz_common/test/display_context_fixture.cpp b/rviz_common/test/display_context_fixture.cpp new file mode 100644 index 000000000..b213644b1 --- /dev/null +++ b/rviz_common/test/display_context_fixture.cpp @@ -0,0 +1,69 @@ +/* + * Copyright (c) 2018, Bosch Software Innovations GmbH. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted (subject to the limitations in the disclaimer + * below) 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 copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. 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 HOLDER 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. + */ + +#include "display_context_fixture.hpp" + +#include + +#include + +void DisplayContextFixture::SetUpTestCase() +{ + testing_environment_ = std::make_shared(); + testing_environment_->setUpOgreTestEnvironment(); + + scene_manager_ = Ogre::Root::getSingletonPtr()->createSceneManager(); +} + +DisplayContextFixture::DisplayContextFixture() +{ + context_ = std::make_shared>(); + window_manager_ = std::make_shared>(); + clock_ = std::make_shared(); + + EXPECT_CALL(*context_, getClock()).WillRepeatedly(testing::Return(clock_)); + EXPECT_CALL(*context_, getWindowManager()).WillRepeatedly(testing::Return(window_manager_.get())); + EXPECT_CALL(*context_, getSceneManager()).WillRepeatedly(testing::Return(scene_manager_)); +} + +DisplayContextFixture::~DisplayContextFixture() +{ + scene_manager_->getRootSceneNode()->removeAndDestroyAllChildren(); +} + +void DisplayContextFixture::TearDownTestCase() +{ + Ogre::Root::getSingletonPtr()->destroySceneManager(scene_manager_); +} + +Ogre::SceneManager * DisplayContextFixture::scene_manager_ = nullptr; +std::shared_ptr +DisplayContextFixture::testing_environment_ = nullptr; diff --git a/rviz_common/test/display_context_fixture.hpp b/rviz_common/test/display_context_fixture.hpp new file mode 100644 index 000000000..326443287 --- /dev/null +++ b/rviz_common/test/display_context_fixture.hpp @@ -0,0 +1,74 @@ +/* + * Copyright (c) 2018, Bosch Software Innovations GmbH. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted (subject to the limitations in the disclaimer + * below) 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 copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. 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 HOLDER 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. + */ + +#ifndef DISPLAY_CONTEXT_FIXTURE_HPP_ +#define DISPLAY_CONTEXT_FIXTURE_HPP_ + +#include +#include + +#include +#include + +#include // NOLINT + +#include + +#include "rclcpp/clock.hpp" + +#include "test/rviz_rendering/ogre_testing_environment.hpp" + +#include "mock_display_context.hpp" +#include "mock_window_manager_interface.hpp" + +class DisplayContextFixture : public testing::Test +{ +public: + static void SetUpTestCase(); + + DisplayContextFixture(); + + ~DisplayContextFixture(); + + static void TearDownTestCase(); + + static std::shared_ptr testing_environment_; + static Ogre::SceneManager * scene_manager_; + + std::shared_ptr context_; + std::shared_ptr window_manager_; + std::shared_ptr clock_; + + std::string fixed_frame = "fixed_frame"; +}; + + +#endif // DISPLAY_CONTEXT_FIXTURE_HPP_ diff --git a/rviz_common/test/interaction/mock_selection_renderer.hpp b/rviz_common/test/interaction/mock_selection_renderer.hpp new file mode 100644 index 000000000..70a04221a --- /dev/null +++ b/rviz_common/test/interaction/mock_selection_renderer.hpp @@ -0,0 +1,115 @@ +/* + * Copyright (c) 2018, Bosch Software Innovations GmbH. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted (subject to the limitations in the disclaimer + * below) 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 copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. 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 HOLDER 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. + */ + +#ifndef INTERACTION__MOCK_SELECTION_RENDERER_HPP_ +#define INTERACTION__MOCK_SELECTION_RENDERER_HPP_ + +#include +#include +#include + +#include +#include +#include + +#include "rviz_common/interaction/selection_manager.hpp" +#include "rviz_common/interaction/selection_renderer.hpp" + +#include "../display_context_fixture.hpp" + +class VisibleObject +{ +public: + VisibleObject(int x, int y, rviz_common::DisplayContext * context) + : x(x), y(y), handler_( + rviz_common::interaction::createSelectionHandler + (context)) + {} + + uint32_t getARGBColor() + { + return handler_->getHandle() & 0xFFFFFFFF; + } + + rviz_common::interaction::CollObjectHandle getHandle() + { + return handler_->getHandle(); + } + + int x; + int y; + +private: + rviz_common::interaction::SelectionHandlerPtr handler_; +}; + +class MockSelectionRenderer : public rviz_common::interaction::SelectionRenderer +{ +public: + explicit MockSelectionRenderer(rviz_common::DisplayContext * context) + : SelectionRenderer(context) {} + + void render( + rviz_rendering::RenderWindow * window, + rviz_common::interaction::SelectionRectangle rectangle, + rviz_common::interaction::RenderTexture texture, + rviz_common::interaction::HandlerRange handlers, + Ogre::PixelBox & dst_box) override + { + (void) window; + (void) texture; + (void) handlers; + + auto width = static_cast(rectangle.x2 - rectangle.x1); + auto height = static_cast(rectangle.y2 - rectangle.y1); + + auto data = new uint8_t[Ogre::PixelUtil::getMemorySize(width, height, 1, Ogre::PF_A8R8G8B8)](); + dst_box = Ogre::PixelBox(width, height, 1, Ogre::PF_A8R8G8B8, data); + + auto dstptr = static_cast(dst_box.data); + for (auto & object : objects_) { + int x = object.x - rectangle.x1; + int y = object.y - rectangle.y1; + if (x < rectangle.x2 && y < rectangle.y2) { + dstptr[y * dst_box.rowPitch + x] = object.getARGBColor(); + } + } + } + + void addVisibleObject(VisibleObject object) + { + objects_.emplace_back(object); + } + + std::vector objects_; +}; + +#endif // INTERACTION__MOCK_SELECTION_RENDERER_HPP_ diff --git a/rviz_common/test/interaction/selection_handler_test.cpp b/rviz_common/test/interaction/selection_handler_test.cpp new file mode 100644 index 000000000..602692564 --- /dev/null +++ b/rviz_common/test/interaction/selection_handler_test.cpp @@ -0,0 +1,159 @@ +/* + * Copyright (c) 2018, Bosch Software Innovations GmbH. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted (subject to the limitations in the disclaimer + * below) 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 copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. 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 HOLDER 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. + */ + +#include +#include + +#include +#include +#include +#include + +#include +#include + +#include "rviz_common/interaction/selection_handler.hpp" +#include "rviz_common/display_context.hpp" + +#include "selection_test_fixture.hpp" + +MATCHER_P(ContainsWireBoxWithBoundingBox, AABB, "") { + for (const auto & box : arg) { + if (box->getBoundingBox() == AABB) { + return true; + } + } + return false; +} + +class SelectionHandlerFixture : public SelectionTestFixture +{ +public: + SelectionHandlerFixture() + { + using rviz_common::interaction::SelectionHandler; + handler_ = rviz_common::interaction::createSelectionHandler(context_.get()); + } + + Ogre::ManualObject * createManualObject() + { + static int count = 0; + auto object = scene_manager_->createManualObject("ManualObject" + std::to_string(count++)); + + object->begin("BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_STRIP); + object->position(-2.0f, -2.0f, 0.0f); + object->position(2.0f, -2.0f, 0.0f); + object->position(2.0f, 2.0f, 0.0f); + object->position(-2.0f, 2.0f, 0.0f); + object->position(-2.0f, -2.0f, 0.0f); + object->end(); + + return object; + } + + void findSimpleRenderablesAttached( + Ogre::SceneNode * scene_node, std::vector & objects) + { + auto it = scene_node->getAttachedObjectIterator(); + while (it.hasMoreElements()) { + auto movable_object = it.getNext(); + if (movable_object->getMovableType() == "SimpleRenderable") { + objects.push_back(movable_object); + } + } + } + + std::vector findAllSimpleRenderables(Ogre::SceneNode * scene_node) + { + std::vector objects; + findSimpleRenderablesAttached(scene_node, objects); + + auto child_it = scene_node->getChildIterator(); + while (child_it.hasMoreElements()) { + auto child_node = child_it.getNext(); + auto child_scene_node = dynamic_cast(child_node); + findSimpleRenderablesAttached(child_scene_node, objects); + } + + return objects; + } + + std::shared_ptr handler_; +}; + +TEST_F(SelectionHandlerFixture, addTrackedObject_adds_object_correctly) { + auto manual_object = createManualObject(); + scene_manager_->getRootSceneNode()->createChildSceneNode()->attachObject(manual_object); + + handler_->addTrackedObject(manual_object); + + rviz_common::interaction::Picked object(handler_->getHandle()); + auto aabbs = handler_->getAABBs(object); + EXPECT_THAT(aabbs, SizeIs(1)); +} + +TEST_F(SelectionHandlerFixture, onSelect_draws_wirebox_around_selected_object) { + auto manual_object = createManualObject(); + scene_manager_->getRootSceneNode()->createChildSceneNode()->attachObject(manual_object); + + handler_->addTrackedObject(manual_object); + + rviz_common::interaction::Picked object(handler_->getHandle()); + + Ogre::MaterialManager::getSingletonPtr()->load("RVIZ/Cyan", "rviz_rendering"); + + handler_->onSelect(object); + + auto found_objects = findAllSimpleRenderables(scene_manager_->getRootSceneNode()); + EXPECT_THAT( + found_objects, + ContainsWireBoxWithBoundingBox(Ogre::AxisAlignedBox(-2.0f, -2.0f, 0.0f, 2.0f, 2.0f, 0.0f))); +} + +TEST_F(SelectionHandlerFixture, onDeselect_removes_wirebox_around_object) { + auto manual_object = createManualObject(); + scene_manager_->getRootSceneNode()->createChildSceneNode()->attachObject(manual_object); + + handler_->addTrackedObject(manual_object); + + rviz_common::interaction::Picked object(handler_->getHandle()); + + Ogre::MaterialManager::getSingletonPtr()->load("RVIZ/Cyan", "rviz_rendering"); + + handler_->onSelect(object); + handler_->onDeselect(object); + + auto found_objects = findAllSimpleRenderables(scene_manager_->getRootSceneNode()); + EXPECT_THAT( + found_objects, + Not( + ContainsWireBoxWithBoundingBox(Ogre::AxisAlignedBox(-2.0f, -2.0f, 0.0f, 2.0f, 2.0f, 0.0f)))); +} diff --git a/rviz_common/test/interaction/selection_manager_test.cpp b/rviz_common/test/interaction/selection_manager_test.cpp new file mode 100644 index 000000000..275cc0c93 --- /dev/null +++ b/rviz_common/test/interaction/selection_manager_test.cpp @@ -0,0 +1,114 @@ +/* + * Copyright (c) 2018, Bosch Software Innovations GmbH. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted (subject to the limitations in the disclaimer + * below) 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 copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. 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 HOLDER 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. + */ + +#include +#include + +#include +#include +#include + +#include "rviz_common/interaction/selection_manager.hpp" +#include "rviz_common/interaction/selection_handler.hpp" +#include "rviz_common/display_context.hpp" + +#include "selection_test_fixture.hpp" +#include "mock_selection_renderer.hpp" + +using namespace ::testing; // NOLINT + +class SelectionManagerTestFixture : public SelectionTestFixture +{ +public: + VisibleObject addVisibleObject(int x, int y) + { + VisibleObject object(x, y, context_.get()); + renderer_->addVisibleObject(object); + return object; + } + + rviz_rendering::RenderWindow * render_window_ = nullptr; +}; + +TEST_F(SelectionManagerTestFixture, select_selects_objects_inside_selection) { + auto o1 = addVisibleObject(10, 10); + auto o2 = addVisibleObject(150, 150); + + selection_manager_->select( + render_window_, 0, 0, 100, 100, rviz_common::interaction::SelectionManager::Replace); + + auto selection = selection_manager_->getSelection(); + EXPECT_THAT(selection, SizeIs(1)); + EXPECT_THAT(selection, Contains(Key(o1.getHandle()))); + EXPECT_THAT(selection, Not(Contains(Key(o2.getHandle())))); +} + +TEST_F(SelectionManagerTestFixture, adding_a_new_selection) { + auto o1 = addVisibleObject(10, 10); + auto o2 = addVisibleObject(20, 20); + selection_manager_->select( + render_window_, 0, 0, 15, 15, rviz_common::interaction::SelectionManager::Replace); + + selection_manager_->select( + render_window_, 15, 15, 25, 25, rviz_common::interaction::SelectionManager::Add); + + auto selection = selection_manager_->getSelection(); + EXPECT_THAT(selection, SizeIs(2)); + EXPECT_THAT(selection, Contains(Key(o1.getHandle()))); + EXPECT_THAT(selection, Contains(Key(o2.getHandle()))); +} + +TEST_F(SelectionManagerTestFixture, adding_an_exising_selection_has_no_effect) { + auto o1 = addVisibleObject(10, 10); + selection_manager_->select( + render_window_, 0, 0, 15, 15, rviz_common::interaction::SelectionManager::Replace); + + selection_manager_->select( + render_window_, 0, 0, 15, 15, rviz_common::interaction::SelectionManager::Add); + + auto selection = selection_manager_->getSelection(); + EXPECT_THAT(selection, SizeIs(1)); + EXPECT_THAT(selection, Contains(Key(o1.getHandle()))); +} + +TEST_F(SelectionManagerTestFixture, subtracting_from_a_selection) { + auto o1 = addVisibleObject(10, 10); + auto o2 = addVisibleObject(20, 20); + selection_manager_->select( + render_window_, 0, 0, 25, 25, rviz_common::interaction::SelectionManager::Replace); + + selection_manager_->select( + render_window_, 0, 0, 15, 15, rviz_common::interaction::SelectionManager::Remove); + + auto selection = selection_manager_->getSelection(); + EXPECT_THAT(selection, SizeIs(1)); + EXPECT_THAT(selection, Contains(Key(o2.getHandle()))); +} diff --git a/rviz_common/test/interaction/selection_test_fixture.hpp b/rviz_common/test/interaction/selection_test_fixture.hpp new file mode 100644 index 000000000..d4908e42c --- /dev/null +++ b/rviz_common/test/interaction/selection_test_fixture.hpp @@ -0,0 +1,74 @@ +/* + * Copyright (c) 2018, Bosch Software Innovations GmbH. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted (subject to the limitations in the disclaimer + * below) 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 copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. 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 HOLDER 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. + */ + +#ifndef INTERACTION__SELECTION_TEST_FIXTURE_HPP_ +#define INTERACTION__SELECTION_TEST_FIXTURE_HPP_ + +#include +#include + +#include + +#include "rviz_common/display_context.hpp" +#include "rviz_common/interaction/selection_manager.hpp" +#include "rviz_common/interaction/handler_manager.hpp" + +#include "../display_context_fixture.hpp" +#include "mock_selection_renderer.hpp" + +using namespace ::testing; // NOLINT + +class SelectionTestFixture : public DisplayContextFixture +{ +public: + SelectionTestFixture() + { + renderer_ = std::make_shared(context_.get()); + handler_manager_ = std::make_shared(); + std::weak_ptr + handler_manager_weak_ptr(handler_manager_); + selection_manager_ = std::make_shared( + context_.get(), renderer_); + std::weak_ptr + selection_manager_weak_ptr(selection_manager_); + EXPECT_CALL(*context_, getHandlerManager()).WillRepeatedly( + Invoke([handler_manager_weak_ptr]() {return handler_manager_weak_ptr.lock();})); + EXPECT_CALL(*context_, getSelectionManager()).WillRepeatedly( + Invoke([selection_manager_weak_ptr]() {return selection_manager_weak_ptr.lock();})); + selection_manager_->initialize(); + } + + std::shared_ptr renderer_; + std::shared_ptr selection_manager_; + std::shared_ptr handler_manager_; +}; + +#endif // INTERACTION__SELECTION_TEST_FIXTURE_HPP_ diff --git a/rviz_common/test/mock_display_context.hpp b/rviz_common/test/mock_display_context.hpp new file mode 100644 index 000000000..c8c546669 --- /dev/null +++ b/rviz_common/test/mock_display_context.hpp @@ -0,0 +1,83 @@ +/* + * Copyright (c) 2017, Bosch Software Innovations GmbH. + * 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. + */ + +#ifndef MOCK_DISPLAY_CONTEXT_HPP_ +#define MOCK_DISPLAY_CONTEXT_HPP_ + +#include +#include + +#include + +#include "rviz_common/display_context.hpp" +#include "rviz_common/panel_dock_widget.hpp" +#include "rviz_common/viewport_mouse_event.hpp" +#include "rviz_common/window_manager_interface.hpp" +#include "rviz_common/interaction/handler_manager_iface.hpp" +#include "rviz_common/interaction/selection_manager_iface.hpp" + +class MockDisplayContext : public rviz_common::DisplayContext +{ +public: + MOCK_CONST_METHOD0(getSceneManager, Ogre::SceneManager * ()); + MOCK_CONST_METHOD0(getWindowManager, rviz_common::WindowManagerInterface * ()); + MOCK_CONST_METHOD0( + getHandlerManager, std::shared_ptr()); + MOCK_CONST_METHOD0( + getSelectionManager, std::shared_ptr()); + MOCK_CONST_METHOD0(getViewPicker, std::shared_ptr()); + MOCK_CONST_METHOD0(getFrameManager, rviz_common::FrameManagerIface * ()); + MOCK_CONST_METHOD0(getFixedFrame, QString()); + MOCK_CONST_METHOD0(getFrameCount, uint64_t()); + MOCK_CONST_METHOD0(getDisplayFactory, rviz_common::DisplayFactory * ()); + + MOCK_METHOD1(addNodeToMainExecutor, void(std::shared_ptr node)); + MOCK_METHOD1(removeNodeFromMainExecutor, void(std::shared_ptr node)); + + MOCK_METHOD2(handleChar, void(QKeyEvent * event, rviz_common::RenderPanel * panel)); + MOCK_METHOD1(handleMouseEvent, void(const rviz_common::ViewportMouseEvent & even)); + + MOCK_CONST_METHOD0(getToolManager, rviz_common::ToolManager * ()); + MOCK_CONST_METHOD0(getViewManager, rviz_common::ViewManager * ()); + MOCK_CONST_METHOD0(getRootDisplayGroup, rviz_common::DisplayGroup * ()); + + MOCK_CONST_METHOD0(getDefaultVisibilityBit, uint32_t()); + MOCK_METHOD0(visibilityBits, rviz_common::BitAllocator * ()); + + MOCK_METHOD1(setStatus, void(const QString &message)); + MOCK_METHOD0(getClock, std::shared_ptr()); + + MOCK_METHOD0(queueRender, void()); + + MOCK_METHOD0(lockRender, void()); + MOCK_METHOD0(unlockRender, void()); + MOCK_CONST_METHOD0(getRenderPanel, rviz_common::RenderPanel * ()); +}; + +#endif // MOCK_DISPLAY_CONTEXT_HPP_ diff --git a/rviz_common/test/mock_window_manager_interface.hpp b/rviz_common/test/mock_window_manager_interface.hpp new file mode 100644 index 000000000..80fe9b8dc --- /dev/null +++ b/rviz_common/test/mock_window_manager_interface.hpp @@ -0,0 +1,53 @@ +/* + * Copyright (c) 2017, Bosch Software Innovations GmbH. + * 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. + */ + +#ifndef MOCK_WINDOW_MANAGER_INTERFACE_HPP_ +#define MOCK_WINDOW_MANAGER_INTERFACE_HPP_ + +#include +#include + +#include "rviz_common/panel_dock_widget.hpp" + +#include "rviz_common/window_manager_interface.hpp" + +class MockWindowManagerInterface : public rviz_common::WindowManagerInterface +{ +public: + MOCK_METHOD0(getParentWindow, QWidget * ()); + + MOCK_METHOD4(addPane, + rviz_common::PanelDockWidget *( + const QString &name, QWidget * pane, Qt::DockWidgetArea area, bool floating)); + + MOCK_METHOD1(setStatus, void(const QString &message)); +}; + + +#endif // MOCK_WINDOW_MANAGER_INTERFACE_HPP_ diff --git a/rviz_common/test/ros_node_abstraction_test.cpp b/rviz_common/test/ros_node_abstraction_test.cpp index ab5b507fe..1e7fec729 100644 --- a/rviz_common/test/ros_node_abstraction_test.cpp +++ b/rviz_common/test/ros_node_abstraction_test.cpp @@ -66,7 +66,7 @@ class RosNodeAbstractionTestFixture : public Test rclcpp::shutdown(); } - void SetUp() override + RosNodeAbstractionTestFixture() { ros_node_storage_ = std::make_unique(); } diff --git a/rviz_common/test_resources/single_point_Pick.png b/rviz_common/test_resources/single_point_Pick.png new file mode 100644 index 000000000..1b8987a58 Binary files /dev/null and b/rviz_common/test_resources/single_point_Pick.png differ diff --git a/rviz_common/test_resources/single_point_Pick1.png b/rviz_common/test_resources/single_point_Pick1.png new file mode 100644 index 000000000..1298e581c Binary files /dev/null and b/rviz_common/test_resources/single_point_Pick1.png differ diff --git a/rviz_default_plugins/CMakeLists.txt b/rviz_default_plugins/CMakeLists.txt index 8540e44b5..6147c9c4b 100644 --- a/rviz_default_plugins/CMakeLists.txt +++ b/rviz_default_plugins/CMakeLists.txt @@ -112,6 +112,7 @@ set(rviz_default_plugins_source_files src/rviz_default_plugins/robot/robot_element_base_class.cpp src/rviz_default_plugins/robot/tf_link_updater.cpp src/rviz_default_plugins/tools/move/move_tool.cpp + src/rviz_default_plugins/tools/select/selection_tool.cpp src/rviz_default_plugins/view_controllers/orbit/orbit_view_controller.cpp ) @@ -361,6 +362,25 @@ if(BUILD_TESTING) target_link_libraries(path_display_test ${TEST_LINK_LIBRARIES}) endif() + + ament_add_gmock(pointcloud_selection_handler_test + test/rviz_default_plugins/displays/pointcloud/point_cloud_selection_handler_test.cpp + test/rviz_default_plugins/displays/pointcloud/message_creators.cpp + test/rviz_default_plugins/displays/display_test_fixture.cpp + test/rviz_default_plugins/scene_graph_introspection.hpp + APPEND_ENV AMENT_PREFIX_PATH=${CMAKE_INSTALL_PREFIX} PATH=${CMAKE_INSTALL_PREFIX}/bin;${CMAKE_INSTALL_PREFIX}/opt/rviz_assimp_vendor/bin;${CMAKE_INSTALL_PREFIX}/opt/rviz_yaml_cpp_vendor/bin;${CMAKE_INSTALL_PREFIX}/opt/rviz_ogre_vendor/bin) + if(TARGET pointcloud_selection_handler_test) + target_include_directories(pointcloud_selection_handler_test PUBLIC ${TEST_INCLUDE_DIRS}) + target_link_libraries(pointcloud_selection_handler_test ${TEST_LINK_LIBRARIES}) + endif() + + ament_add_gmock(selection_tool_test + test/rviz_default_plugins/tools/select/selection_tool_test.cpp + APPEND_ENV AMENT_PREFIX_PATH=${CMAKE_INSTALL_PREFIX} PATH=${CMAKE_INSTALL_PREFIX}/bin;${CMAKE_INSTALL_PREFIX}/opt/rviz_assimp_vendor/bin;${CMAKE_INSTALL_PREFIX}/opt/rviz_yaml_cpp_vendor/bin;${CMAKE_INSTALL_PREFIX}/opt/rviz_ogre_vendor/bin) + if(TARGET selection_tool_test) + target_include_directories(selection_tool_test PUBLIC ${TEST_INCLUDE_DIRS}) + target_link_libraries(selection_tool_test ${TEST_LINK_LIBRARIES}) + endif() endif() endif() diff --git a/rviz_default_plugins/plugins_description.xml b/rviz_default_plugins/plugins_description.xml index ca92c22ae..0a8bcff4d 100644 --- a/rviz_default_plugins/plugins_description.xml +++ b/rviz_default_plugins/plugins_description.xml @@ -155,6 +155,16 @@ + + + Drag with the left button to select objects in the 3D scene. + Hold the Alt key to change viewpoint as in the Move tool. + + (context_->getSceneManager(), scene_node_); setDefaultProportions(); - handler_.reset( - new MarkerSelectionHandler(this, MarkerID(new_message->ns, new_message->id), context_)); + handler_ = rviz_common::interaction::createSelectionHandler( + this, MarkerID(new_message->ns, new_message->id), context_); handler_->addTrackedObjects(arrow_->getSceneNode()); } diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/line_list_marker.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/line_list_marker.cpp index 355d3d95c..513ba98f7 100644 --- a/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/line_list_marker.cpp +++ b/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/line_list_marker.cpp @@ -52,7 +52,6 @@ #endif #include "../marker_display.hpp" -#include "marker_selection_handler.hpp" #include "rviz_rendering/objects/billboard_line.hpp" diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/line_marker_base.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/line_marker_base.cpp index de009e547..168f0d99d 100644 --- a/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/line_marker_base.cpp +++ b/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/line_marker_base.cpp @@ -111,8 +111,8 @@ void LineMarkerBase::onNewMessage( convertNewMessageToBillboardLine(new_message); - handler_.reset(new MarkerSelectionHandler(this, MarkerID(new_message->ns, new_message->id), - context_)); + handler_ = rviz_common::interaction::createSelectionHandler( + this, MarkerID(new_message->ns, new_message->id), context_); handler_->addTrackedObjects(lines_->getSceneNode()); } diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/line_strip_marker.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/line_strip_marker.cpp index de1024ebd..709ef663d 100644 --- a/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/line_strip_marker.cpp +++ b/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/line_strip_marker.cpp @@ -52,7 +52,6 @@ #endif #include "../marker_display.hpp" -#include "marker_selection_handler.hpp" #include "rviz_common/display_context.hpp" #include "rviz_rendering/objects/billboard_line.hpp" diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/marker_base.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/marker_base.cpp index 8a07349c8..cb84797b7 100644 --- a/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/marker_base.cpp +++ b/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/marker_base.cpp @@ -59,7 +59,6 @@ #include "../marker_display.hpp" #include "marker_selection_handler.hpp" #include "rviz_common/display_context.hpp" -#include "rviz_common/selection/selection_manager.hpp" #include "rviz_common/frame_manager_iface.hpp" #include "rviz_common/properties/status_property.hpp" diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/marker_base.hpp b/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/marker_base.hpp index 8891bc65d..9662d3f9b 100644 --- a/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/marker_base.hpp +++ b/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/marker_base.hpp @@ -39,7 +39,7 @@ #include "visualization_msgs/msg/marker.hpp" #include "rclcpp/rclcpp.hpp" -#include "rviz_common/selection/forwards.hpp" +#include "rviz_common/interaction/forwards.hpp" namespace Ogre { diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/marker_selection_handler.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/marker_selection_handler.cpp index 3b46fe291..d5c7a537d 100644 --- a/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/marker_selection_handler.cpp +++ b/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/marker_selection_handler.cpp @@ -89,7 +89,7 @@ Ogre::Quaternion MarkerSelectionHandler::getOrientation() } void MarkerSelectionHandler::createProperties( - const rviz_common::selection::Picked & obj, rviz_common::properties::Property * parent_property) + const rviz_common::interaction::Picked & obj, rviz_common::properties::Property * parent_property) { (void) obj; diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/marker_selection_handler.hpp b/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/marker_selection_handler.hpp index fd296eb69..42480fa39 100644 --- a/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/marker_selection_handler.hpp +++ b/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/marker_selection_handler.hpp @@ -31,11 +31,12 @@ #ifndef RVIZ_DEFAULT_PLUGINS__DISPLAYS__MARKER__MARKERS__MARKER_SELECTION_HANDLER_HPP_ #define RVIZ_DEFAULT_PLUGINS__DISPLAYS__MARKER__MARKERS__MARKER_SELECTION_HANDLER_HPP_ +#include #include #include -#include "rviz_common/selection/forwards.hpp" -#include "rviz_common/selection/selection_manager.hpp" +#include "rviz_common/interaction/forwards.hpp" +#include "rviz_common/interaction/selection_manager.hpp" namespace rviz_common { @@ -56,7 +57,7 @@ class InteractiveMarkerControl; class MarkerBase; typedef std::pair MarkerID; -class MarkerSelectionHandler : public rviz_common::selection::SelectionHandler +class MarkerSelectionHandler : public rviz_common::interaction::SelectionHandler { public: MarkerSelectionHandler( @@ -67,7 +68,7 @@ class MarkerSelectionHandler : public rviz_common::selection::SelectionHandler Ogre::Quaternion getOrientation(); void createProperties( - const rviz_common::selection::Picked & obj, + const rviz_common::interaction::Picked & obj, rviz_common::properties::Property * parent_property) override; void updateProperties() override; @@ -76,6 +77,10 @@ class MarkerSelectionHandler : public rviz_common::selection::SelectionHandler QString marker_id_; rviz_common::properties::VectorProperty * position_property_; rviz_common::properties::QuaternionProperty * orientation_property_; + + template + friend typename std::shared_ptr + rviz_common::interaction::createSelectionHandler(Args ... arguments); }; } // namespace markers diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/mesh_resource_marker.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/mesh_resource_marker.cpp index b8e928f45..ee9a42b87 100644 --- a/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/mesh_resource_marker.cpp +++ b/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/mesh_resource_marker.cpp @@ -126,8 +126,8 @@ void MeshResourceMarker::onNewMessage( createMeshWithMaterials(new_message); - handler_.reset(new MarkerSelectionHandler(this, MarkerID(new_message->ns, new_message->id), - context_)); + handler_ = rviz_common::interaction::createSelectionHandler( + this, MarkerID(new_message->ns, new_message->id), context_); handler_->addTrackedObject(entity_); } else { // underlying mesh resource has not changed but if the color has then we need to update the diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/points_marker.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/points_marker.cpp index e09968122..2e18067aa 100644 --- a/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/points_marker.cpp +++ b/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/points_marker.cpp @@ -32,6 +32,8 @@ #include +#include "rviz_common/interaction/selection_manager.hpp" + #include "../marker_display.hpp" #include "marker_selection_handler.hpp" @@ -67,10 +69,10 @@ void PointsMarker::onNewMessage( points_ = new rviz_rendering::PointCloud(); scene_node_->attachObject(points_); - handler_.reset( - new MarkerSelectionHandler(this, MarkerID(new_message->ns, new_message->id), context_)); + handler_ = rviz_common::interaction::createSelectionHandler( + this, MarkerID(new_message->ns, new_message->id), context_); points_->setPickColor( - rviz_common::selection::SelectionManager::handleToColor(handler_->getHandle())); + rviz_common::interaction::SelectionManager::handleToColor(handler_->getHandle())); } Ogre::Vector3 pose, scale; diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/shape_marker.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/shape_marker.cpp index 6404a2059..ae579090d 100644 --- a/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/shape_marker.cpp +++ b/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/shape_marker.cpp @@ -36,7 +36,6 @@ #include "../marker_display.hpp" #include "rviz_common/display_context.hpp" #include "rviz_common/properties/status_property.hpp" -#include "rviz_common/selection/selection_manager.hpp" #include "rviz_rendering/objects/shape.hpp" namespace rviz_default_plugins @@ -109,8 +108,8 @@ void ShapeMarker::resetShapeForMessage(const MarkerBase::MarkerConstSharedPtr & shape_ = std::make_shared( shape_type, this->context_->getSceneManager(), this->scene_node_); - handler_.reset(new MarkerSelectionHandler( - this, MarkerID(new_message->ns, new_message->id), context_)); + handler_ = rviz_common::interaction::createSelectionHandler( + this, MarkerID(new_message->ns, new_message->id), context_); handler_->addTrackedObjects(shape_->getRootNode()); } diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/text_view_facing_marker.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/text_view_facing_marker.cpp index ddc69b103..3d4947ca9 100644 --- a/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/text_view_facing_marker.cpp +++ b/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/text_view_facing_marker.cpp @@ -84,8 +84,8 @@ void TextViewFacingMarker::onNewMessage( rviz_rendering::MovableText::H_CENTER, rviz_rendering::MovableText::V_CENTER); scene_node_->attachObject(text_); - handler_.reset( - new MarkerSelectionHandler(this, MarkerID(new_message->ns, new_message->id), context_)); + handler_ = rviz_common::interaction::createSelectionHandler( + this, MarkerID(new_message->ns, new_message->id), context_); handler_->addTrackedObject(text_); } diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/triangle_list_marker.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/triangle_list_marker.cpp index e252c077e..03e40022e 100644 --- a/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/triangle_list_marker.cpp +++ b/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/triangle_list_marker.cpp @@ -56,7 +56,6 @@ #include "marker_selection_handler.hpp" #include "../marker_display.hpp" -#include "rviz_common/selection/selection_manager.hpp" #include "rviz_common/properties/status_property.hpp" #include "rviz_common/uniform_string_stream.hpp" #include "rviz_common/display_context.hpp" @@ -161,9 +160,8 @@ void TriangleListMarker::initializeManualObject( material_->setReceiveShadows(false); material_->getTechnique(0)->setLightingEnabled(true); material_->setCullingMode(Ogre::CULL_NONE); - - handler_.reset(new MarkerSelectionHandler(this, MarkerID(new_message->ns, new_message->id), - context_)); + handler_ = rviz_common::interaction::createSelectionHandler( + this, MarkerID(new_message->ns, new_message->id), context_); } void TriangleListMarker::updateManualObject( diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/pointcloud/point_cloud_common.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/pointcloud/point_cloud_common.cpp index 92d73b65b..1328dd28e 100644 --- a/rviz_default_plugins/src/rviz_default_plugins/displays/pointcloud/point_cloud_common.cpp +++ b/rviz_default_plugins/src/rviz_default_plugins/displays/pointcloud/point_cloud_common.cpp @@ -53,6 +53,7 @@ #include "rviz_common/display.hpp" #include "rviz_common/display_context.hpp" #include "rviz_common/frame_manager_iface.hpp" +#include "rviz_common/interaction/selection_manager.hpp" #include "rviz_rendering/objects/point_cloud.hpp" #include "rviz_common/properties/bool_property.hpp" #include "rviz_common/properties/enum_property.hpp" @@ -90,6 +91,20 @@ void CloudInfo::clear() } } +void CloudInfo::setSelectable( + bool selectable, float selection_box_size, rviz_common::DisplayContext * context) +{ + if (selectable) { + selection_handler_ = rviz_common::interaction::createSelectionHandler + (selection_box_size, this, context); + cloud_->setPickColor(rviz_common::interaction::SelectionManager::handleToColor( + selection_handler_->getHandle())); + } else { + selection_handler_.reset(); + cloud_->setPickColor(Ogre::ColourValue(0.0f, 0.0f, 0.0f, 0.0f)); + } +} + const std::string PointCloudCommon::message_status_name_ = "Message"; // NOLINT allow std::string PointCloudCommon::PointCloudCommon(rviz_common::Display * display) @@ -279,19 +294,8 @@ void PointCloudCommon::updateSelectable() { bool selectable = selectable_property_->getBool(); - if (selectable) { - for (auto const & cloud_info : cloud_infos_) { - cloud_info->selection_handler_.reset( - new PointCloudSelectionHandler(getSelectionBoxSize(), cloud_info.get(), context_)); - cloud_info->cloud_->setPickColor( - rviz_common::selection::SelectionManager::handleToColor( - cloud_info->selection_handler_->getHandle())); - } - } else { - for (auto const & cloud_info : cloud_infos_) { - cloud_info->selection_handler_.reset(); - cloud_info->cloud_->setPickColor(Ogre::ColourValue(0.0f, 0.0f, 0.0f, 0.0f)); - } + for (auto const & cloud_info : cloud_infos_) { + cloud_info->setSelectable(selectable, getSelectionBoxSize(), context_); } } @@ -344,6 +348,7 @@ void PointCloudCommon::update(float wall_dt, float ros_dt) (void) wall_dt; (void) ros_dt; auto mode = static_cast(style_property_->getOptionInt()); + bool selectable = selectable_property_->getBool(); float point_decay_time = decay_time_property_->getFloat(); if (needs_retransform_) { @@ -429,8 +434,7 @@ void PointCloudCommon::update(float wall_dt, float ros_dt) cloud_info->scene_node_->attachObject(cloud_info->cloud_.get()); - cloud_info->selection_handler_.reset(new PointCloudSelectionHandler(getSelectionBoxSize(), - cloud_info.get(), context_)); + cloud_info->setSelectable(selectable, getSelectionBoxSize(), context_); cloud_infos_.push_back(*it); } diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/pointcloud/point_cloud_common.hpp b/rviz_default_plugins/src/rviz_default_plugins/displays/pointcloud/point_cloud_common.hpp index 5103b15e1..b50d19550 100644 --- a/rviz_default_plugins/src/rviz_default_plugins/displays/pointcloud/point_cloud_common.hpp +++ b/rviz_default_plugins/src/rviz_default_plugins/displays/pointcloud/point_cloud_common.hpp @@ -52,12 +52,13 @@ #include "sensor_msgs/msg/point_cloud.hpp" #include "sensor_msgs/msg/point_cloud2.hpp" -#include "rviz_common/selection/selection_manager.hpp" -#include "point_cloud_transformer.hpp" -#include "point_cloud_selection_handler.hpp" +#include "rviz_common/interaction/forwards.hpp" +#include "rviz_common/interaction/selection_manager.hpp" #include "rviz_common/properties/color_property.hpp" #include "rviz_rendering/objects/point_cloud.hpp" -#include "rviz_common/selection/forwards.hpp" + +#include "point_cloud_transformer.hpp" +#include "point_cloud_selection_handler.hpp" #endif @@ -95,6 +96,9 @@ struct CloudInfo // clear the point cloud, but keep selection handler around void clear(); + void setSelectable( + bool selectable, float selection_box_size, rviz_common::DisplayContext * context); + rclcpp::Time receive_time_; Ogre::SceneManager * manager_; diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/pointcloud/point_cloud_helpers.hpp b/rviz_default_plugins/src/rviz_default_plugins/displays/pointcloud/point_cloud_helpers.hpp index 8d61f84bd..467b6fecc 100644 --- a/rviz_default_plugins/src/rviz_default_plugins/displays/pointcloud/point_cloud_helpers.hpp +++ b/rviz_default_plugins/src/rviz_default_plugins/displays/pointcloud/point_cloud_helpers.hpp @@ -77,7 +77,7 @@ inline T valueFromCloud( uint32_t offset, uint8_t type, uint32_t point_step, - uint32_t index) + uint64_t index) { const uint8_t * data = &cloud->data[(point_step * index) + offset]; T ret = 0; diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/pointcloud/point_cloud_selection_handler.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/pointcloud/point_cloud_selection_handler.cpp index 51950c6ca..15c1cf12a 100644 --- a/rviz_default_plugins/src/rviz_default_plugins/displays/pointcloud/point_cloud_selection_handler.cpp +++ b/rviz_default_plugins/src/rviz_default_plugins/displays/pointcloud/point_cloud_selection_handler.cpp @@ -28,6 +28,7 @@ */ #include "point_cloud_selection_handler.hpp" + #include #include #include @@ -41,38 +42,29 @@ #include #include #include + #ifndef _WIN32 # pragma GCC diagnostic pop #endif -// #include - -// TODO(wjwwood): revist file when pluginlib is available -// #include - -#include "./point_cloud_transformer.hpp" -#include "point_cloud_helpers.hpp" +#include "rviz_rendering/objects/point_cloud.hpp" #include "rviz_common/display.hpp" #include "rviz_common/display_context.hpp" -#include "rviz_common/frame_manager_iface.hpp" -#include "rviz_rendering/objects/point_cloud.hpp" +#include "rviz_common/interaction/forwards.hpp" #include "rviz_common/properties/bool_property.hpp" #include "rviz_common/properties/enum_property.hpp" #include "rviz_common/properties/float_property.hpp" #include "rviz_common/properties/vector_property.hpp" -#include "rviz_common/uniform_string_stream.hpp" -#include "rviz_common/validate_floats.hpp" + +#include "./point_cloud_helpers.hpp" #include "./point_cloud_common.hpp" namespace rviz_default_plugins { -uint qHash(IndexAndMessage iam) +uint64_t qHash(IndexAndMessage iam) { - return - ((uint) iam.index) + - ((uint) (iam.message >> 32)) + - ((uint) (iam.message & 0xffffffff)); + return iam.index + (iam.message >> 32) + (iam.message & 0xffffffff); } bool operator==(IndexAndMessage a, IndexAndMessage b) @@ -92,20 +84,18 @@ PointCloudSelectionHandler::PointCloudSelectionHandler( PointCloudSelectionHandler::~PointCloudSelectionHandler() { - // delete all the Property objects on our way out. - QHash::const_iterator iter; - for (iter = property_hash_.begin(); iter != property_hash_.end(); iter++) { - delete iter.value(); + for (auto property_hash : property_hash_) { + delete property_hash; } } void PointCloudSelectionHandler::preRenderPass(uint32_t pass) { - rviz_common::selection::SelectionHandler::preRenderPass(pass); + rviz_common::interaction::SelectionHandler::preRenderPass(pass); switch (pass) { case 0: - cloud_info_->cloud_->setPickColor(rviz_common::selection::SelectionManager::handleToColor( + cloud_info_->cloud_->setPickColor(rviz_common::interaction::SelectionManager::handleToColor( getHandle())); break; case 1: @@ -118,173 +108,71 @@ void PointCloudSelectionHandler::preRenderPass(uint32_t pass) void PointCloudSelectionHandler::postRenderPass(uint32_t pass) { - rviz_common::selection::SelectionHandler::postRenderPass(pass); + rviz_common::interaction::SelectionHandler::postRenderPass(pass); if (pass == 1) { cloud_info_->cloud_->setColorByIndex(false); } } -Ogre::Vector3 pointFromCloud( - const sensor_msgs::msg::PointCloud2::ConstSharedPtr & cloud, - uint32_t index) -{ - int32_t xi = findChannelIndex(cloud, "x"); - int32_t yi = findChannelIndex(cloud, "y"); - int32_t zi = findChannelIndex(cloud, "z"); - - const uint32_t xoff = cloud->fields[xi].offset; - const uint32_t yoff = cloud->fields[yi].offset; - const uint32_t zoff = cloud->fields[zi].offset; - const uint8_t type = cloud->fields[xi].datatype; - const uint32_t point_step = cloud->point_step; - float x = valueFromCloud(cloud, xoff, type, point_step, index); - float y = valueFromCloud(cloud, yoff, type, point_step, index); - float z = valueFromCloud(cloud, zoff, type, point_step, index); - return Ogre::Vector3(x, y, z); -} - void PointCloudSelectionHandler::createProperties( - const rviz_common::selection::Picked & obj, + const rviz_common::interaction::Picked & obj, rviz_common::properties::Property * parent_property) { - typedef std::set S_int; - S_int indices; - { - rviz_common::selection::S_uint64::const_iterator it = obj.extra_handles.begin(); - rviz_common::selection::S_uint64::const_iterator end = obj.extra_handles.end(); - for (; it != end; ++it) { - uint64_t handle = *it; - indices.insert((handle & 0xffffffff) - 1); - } - } + S_int indices = getIndicesOfSelectedPoints(obj); + + for (auto index : indices) { + const sensor_msgs::msg::PointCloud2::ConstSharedPtr & message = cloud_info_->message_; + + IndexAndMessage hash_key(index, message.get()); + if (!property_hash_.contains(hash_key)) { + rviz_common::properties::Property * parent = createParentPropertyForPoint( + parent_property, index, message); + property_hash_.insert(hash_key, parent); - { - S_int::iterator it = indices.begin(); - S_int::iterator end = indices.end(); - for (; it != end; ++it) { - int index = *it; - const sensor_msgs::msg::PointCloud2::ConstSharedPtr & message = cloud_info_->message_; - - IndexAndMessage hash_key(index, message.get()); - if (!property_hash_.contains(hash_key)) { - rviz_common::properties::Property * cat = new rviz_common::properties::Property( - QString("Point %1 [cloud 0x%2]").arg(index).arg((uint64_t) message.get()), - QVariant(), - "", - parent_property); - property_hash_.insert(hash_key, cat); - - // First add the position. - rviz_common::properties::VectorProperty * pos_prop = - new rviz_common::properties::VectorProperty( - "Position", - cloud_info_->transformed_points_[index].position, - "", - cat); - pos_prop->setReadOnly(true); - - // Then add all other fields as well. - for (size_t field = 0; field < message->fields.size(); ++field) { - const sensor_msgs::msg::PointField & f = message->fields[field]; - const std::string & name = f.name; - - if (name == "x" || name == "y" || name == "z" || name == "X" || name == "Y" || - name == "Z") - { - continue; - } - if (name == "rgb" || name == "rgba") { - float float_val = valueFromCloud(message, f.offset, f.datatype, - message->point_step, index); - // Convertion hack because rgb are stored int float (datatype=7) and valueFromCloud - // can't cast float to uint32_t - float * float_val_ptr = &float_val; - uint32_t val = *reinterpret_cast(float_val_ptr); - rviz_common::properties::ColorProperty * prop = - new rviz_common::properties::ColorProperty( - QString("%1: %2").arg(field).arg(QString::fromStdString(name)), - QColor((val >> 16) & 0xff, (val >> 8) & 0xff, val & 0xff), - "", - cat); - prop->setReadOnly(true); - - rviz_common::properties::FloatProperty * aprop = - new rviz_common::properties::FloatProperty( - QString("alpha"), ((val >> 24) / 255.0), "", cat); - aprop->setReadOnly(true); - } else { - float val = valueFromCloud(message, f.offset, f.datatype, message->point_step, - index); - rviz_common::properties::FloatProperty * prop = - new rviz_common::properties::FloatProperty( - QString("%1: %2").arg(field).arg(QString::fromStdString(name)), - val, - "", - cat); - prop->setReadOnly(true); - } - } - // suppressing this memleak warning from cppcheck below - // cppcheck-suppress memleak - } + addPositionProperty(parent, index); + + addAdditionalProperties(parent, index, message); } } } void PointCloudSelectionHandler::destroyProperties( - const rviz_common::selection::Picked & obj, + const rviz_common::interaction::Picked & obj, rviz_common::properties::Property * parent_property) { (void) parent_property; - typedef std::set S_int; - S_int indices; - { - rviz_common::selection::S_uint64::const_iterator it = obj.extra_handles.begin(); - rviz_common::selection::S_uint64::const_iterator end = obj.extra_handles.end(); - for (; it != end; ++it) { - uint64_t handle = *it; - indices.insert((handle & 0xffffffff) - 1); - } - } - - { - S_int::iterator it = indices.begin(); - S_int::iterator end = indices.end(); - for (; it != end; ++it) { - int index = *it; - const sensor_msgs::msg::PointCloud2::ConstSharedPtr & message = cloud_info_->message_; + S_int indices = getIndicesOfSelectedPoints(obj); + for (auto index : indices) { + const sensor_msgs::msg::PointCloud2::ConstSharedPtr & message = cloud_info_->message_; - IndexAndMessage hash_key(index, message.get()); + IndexAndMessage hash_key(index, message.get()); - rviz_common::properties::Property * prop = property_hash_.take(hash_key); - delete prop; - } + rviz_common::properties::Property * prop = property_hash_.take(hash_key); + delete prop; } } -void PointCloudSelectionHandler::getAABBs( - const rviz_common::selection::Picked & obj, - rviz_common::selection::V_AABB & aabbs) + +rviz_common::interaction::V_AABB PointCloudSelectionHandler::getAABBs( + const rviz_common::interaction::Picked & obj) { - rviz_common::selection::S_uint64::iterator it = obj.extra_handles.begin(); - rviz_common::selection::S_uint64::iterator end = obj.extra_handles.end(); - for (; it != end; ++it) { - M_HandleToBox::iterator find_it = boxes_.find(std::make_pair(obj.handle, *it - 1)); + rviz_common::interaction::V_AABB aabbs; + for (auto handle : obj.extra_handles) { + auto find_it = boxes_.find(Handles(obj.handle, handle - 1)); if (find_it != boxes_.end()) { - Ogre::WireBoundingBox * box = find_it->second.second; + Ogre::WireBoundingBox * box = find_it->second.box; - aabbs.push_back(box->getWorldBoundingBox()); + aabbs.push_back(box->getWorldBoundingBox(true)); // calculate bounding boxes if absent } } + return aabbs; } -void PointCloudSelectionHandler::onSelect(const rviz_common::selection::Picked & obj) +void PointCloudSelectionHandler::onSelect(const rviz_common::interaction::Picked & obj) { - rviz_common::selection::S_uint64::iterator it = obj.extra_handles.begin(); - rviz_common::selection::S_uint64::iterator end = obj.extra_handles.end(); - for (; it != end; ++it) { - int index = (*it & 0xffffffff) - 1; + for (auto handle : obj.extra_handles) { + uint64_t index = handleToIndex(handle); sensor_msgs::msg::PointCloud2::ConstSharedPtr message = cloud_info_->message_; @@ -295,19 +183,120 @@ void PointCloudSelectionHandler::onSelect(const rviz_common::selection::Picked & Ogre::AxisAlignedBox aabb(pos - size, pos + size); - createBox(std::make_pair(obj.handle, index), aabb, "RVIZ/Cyan"); + createBox(Handles(obj.handle, index), aabb, "RVIZ/Cyan"); + } +} + +void PointCloudSelectionHandler::onDeselect(const rviz_common::interaction::Picked & obj) +{ + for (auto handle : obj.extra_handles) { + destroyBox(Handles(obj.handle, handleToIndex(handle))); } } -void PointCloudSelectionHandler::onDeselect(const rviz_common::selection::Picked & obj) +rviz_common::properties::Property * PointCloudSelectionHandler::createParentPropertyForPoint( + rviz_common::properties::Property * parent_property, + uint64_t index, + const sensor_msgs::msg::PointCloud2::ConstSharedPtr & message) { - rviz_common::selection::S_uint64::iterator it = obj.extra_handles.begin(); - rviz_common::selection::S_uint64::iterator end = obj.extra_handles.end(); - for (; it != end; ++it) { - int global_index = (*it & 0xffffffff) - 1; + return new rviz_common::properties::Property( + QString("Point %1 [cloud 0x%2]").arg(index).arg((uint64_t) message.get()), + QVariant(), + "", + parent_property); +} - destroyBox(std::make_pair(obj.handle, global_index)); +void PointCloudSelectionHandler::addPositionProperty( + rviz_common::properties::Property * parent, uint64_t index) const +{ + rviz_common::properties::VectorProperty * pos_prop = + new rviz_common::properties::VectorProperty( + "Position", + cloud_info_->transformed_points_[index].position, + "", + parent); + pos_prop->setReadOnly(true); + // suppressing this memleak warning from cppcheck below + // cppcheck-suppress memleak +} + +void PointCloudSelectionHandler::addAdditionalProperties( + rviz_common::properties::Property * parent, + uint64_t index, + const sensor_msgs::msg::PointCloud2::ConstSharedPtr & message) const +{ + for (size_t field = 0; field < message->fields.size(); ++field) { + const sensor_msgs::msg::PointField & f = message->fields[field]; + const std::string & name = f.name; + + if (name == "x" || name == "y" || name == "z" || name == "X" || name == "Y" || + name == "Z") + { + continue; + } + if (name == "rgb" || name == "rgba") { + uint32_t val = convertValueToColor(index, message, f); + addColorProperty(parent, field, name, val); + + addAlphaProperty(parent, val); + } else { + auto val = valueFromCloud( + message, f.offset, f.datatype, message->point_step, index); + addIntensityProperty(parent, field, name, val); + } } } +uint32_t PointCloudSelectionHandler::convertValueToColor( + uint64_t index, + const sensor_msgs::msg::PointCloud2::ConstSharedPtr & message, + const sensor_msgs::msg::PointField & f) const +{ + auto float_val = valueFromCloud( + message, f.offset, f.datatype, message->point_step, index); + // Conversion hack because rgb are stored in float (datatype=7) and valueFromCloud + // can't cast float to uint32_t + float * float_val_ptr = &float_val; + uint32_t val = *reinterpret_cast(float_val_ptr); + return val; +} + +void PointCloudSelectionHandler::addColorProperty( + rviz_common::properties::Property * parent, + size_t field, + const std::string & name, + uint32_t val) const +{ + rviz_common::properties::ColorProperty * prop = + new rviz_common::properties::ColorProperty( + QString("%1: %2").arg(field).arg(QString::fromStdString(name)), + QColor((val >> 16) & 0xff, (val >> 8) & 0xff, val & 0xff), + "", + parent); + prop->setReadOnly(true); +} + +void PointCloudSelectionHandler::addAlphaProperty( + rviz_common::properties::Property * parent, uint32_t val) const +{ + rviz_common::properties::FloatProperty * aprop = + new rviz_common::properties::FloatProperty( + QString("alpha"), ((val >> 24) / 255.0), "", parent); + aprop->setReadOnly(true); +} + +void PointCloudSelectionHandler::addIntensityProperty( + rviz_common::properties::Property * parent, + size_t field, + const std::string & name, + float val) const +{ + rviz_common::properties::FloatProperty * prop = + new rviz_common::properties::FloatProperty( + QString("%1: %2").arg(field).arg(QString::fromStdString(name)), + val, + "", + parent); + prop->setReadOnly(true); +} } // namespace rviz_default_plugins diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/pointcloud/point_cloud_selection_handler.hpp b/rviz_default_plugins/src/rviz_default_plugins/displays/pointcloud/point_cloud_selection_handler.hpp index cd57b009d..23c7a42a1 100644 --- a/rviz_default_plugins/src/rviz_default_plugins/displays/pointcloud/point_cloud_selection_handler.hpp +++ b/rviz_default_plugins/src/rviz_default_plugins/displays/pointcloud/point_cloud_selection_handler.hpp @@ -32,15 +32,16 @@ #ifndef Q_MOC_RUN // See: https://bugreports.qt-project.org/browse/QTBUG-22829 # include +# include # include # include "sensor_msgs/msg/point_cloud.hpp" # include "sensor_msgs/msg/point_cloud2.hpp" -# include "rviz_common/selection/selection_manager.hpp" +# include "rviz_common/interaction/forwards.hpp" +# include "rviz_common/interaction/selection_manager.hpp" # include "rviz_common/properties/color_property.hpp" # include "rviz_rendering/objects/point_cloud.hpp" -# include "rviz_common/selection/forwards.hpp" #endif namespace rviz_default_plugins @@ -50,61 +51,106 @@ struct CloudInfo; struct IndexAndMessage { - IndexAndMessage(int _index, const void * _message) + IndexAndMessage(uint64_t _index, const void * _message) : index(_index), message( (uint64_t) _message) {} - int index; + uint64_t index; uint64_t message; }; -class PointCloudSelectionHandler : public rviz_common::selection::SelectionHandler +class PointCloudSelectionHandler : public rviz_common::interaction::SelectionHandler { public: PointCloudSelectionHandler( float box_size, CloudInfo * cloud_info, rviz_common::DisplayContext * context); - virtual ~PointCloudSelectionHandler(); + ~PointCloudSelectionHandler() override; - virtual void createProperties( - const rviz_common::selection::Picked & obj, - rviz_common::properties::Property * parent_property); - virtual void destroyProperties( - const rviz_common::selection::Picked & obj, - rviz_common::properties::Property * parent_property); + void createProperties( + const rviz_common::interaction::Picked & obj, + rviz_common::properties::Property * parent_property) override; + void destroyProperties( + const rviz_common::interaction::Picked & obj, + rviz_common::properties::Property * parent_property) override; - virtual bool needsAdditionalRenderPass(uint32_t pass) + bool needsAdditionalRenderPass(uint32_t pass) override { - if (pass < 2) { - return true; - } - - return false; + return pass < 2; } - virtual void preRenderPass(uint32_t pass); - virtual void postRenderPass(uint32_t pass); + void preRenderPass(uint32_t pass) override; + void postRenderPass(uint32_t pass) override; - virtual void onSelect(const rviz_common::selection::Picked & obj); - virtual void onDeselect(const rviz_common::selection::Picked & obj); + void onSelect(const rviz_common::interaction::Picked & obj) override; + void onDeselect(const rviz_common::interaction::Picked & obj) override; - virtual void getAABBs( - const rviz_common::selection::Picked & obj, - rviz_common::selection::V_AABB & aabbs); + rviz_common::interaction::V_AABB getAABBs(const rviz_common::interaction::Picked & obj) override; void setBoxSize(float size) {box_size_ = size;} bool hasSelections() {return !boxes_.empty();} private: + typedef std::set S_int; + + uint64_t handleToIndex(uint64_t handle) const + { + return (handle & 0xffffffff) - 1; + } + + S_int getIndicesOfSelectedPoints(const rviz_common::interaction::Picked & obj) + { + S_int indices; + for (auto handle : obj.extra_handles) { + indices.insert(handleToIndex(handle)); + } + return indices; + } + + rviz_common::properties::Property * createParentPropertyForPoint( + rviz_common::properties::Property * parent_property, + uint64_t index, + const sensor_msgs::msg::PointCloud2::ConstSharedPtr & message); + + void + addPositionProperty(rviz_common::properties::Property * parent, uint64_t index) const; + + void addAdditionalProperties( + rviz_common::properties::Property * parent, + uint64_t index, + const sensor_msgs::msg::PointCloud2::ConstSharedPtr & message) const; + + uint32_t convertValueToColor( + uint64_t index, + const sensor_msgs::msg::PointCloud2::ConstSharedPtr & message, + const sensor_msgs::msg::PointField & f) const; + + void addColorProperty( + rviz_common::properties::Property * parent, + size_t field, + const std::string & name, + uint32_t val) const; + + void addAlphaProperty(rviz_common::properties::Property * parent, uint32_t val) const; + + void addIntensityProperty( + rviz_common::properties::Property * parent, + size_t field, + const std::string & name, + float val) const; + CloudInfo * cloud_info_; QHash property_hash_; float box_size_; + + template + friend typename std::shared_ptr + rviz_common::interaction::createSelectionHandler(Args ... arguments); }; } // namespace rviz_default_plugins - #endif // RVIZ_DEFAULT_PLUGINS__DISPLAYS__POINTCLOUD__POINT_CLOUD_SELECTION_HANDLER_HPP_ diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/pose/pose_display.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/pose/pose_display.cpp index 0b8786b62..05ac8f155 100644 --- a/rviz_default_plugins/src/rviz_default_plugins/displays/pose/pose_display.cpp +++ b/rviz_default_plugins/src/rviz_default_plugins/displays/pose/pose_display.cpp @@ -44,7 +44,7 @@ #include "rviz_common/properties/float_property.hpp" #include "rviz_common/properties/quaternion_property.hpp" #include "rviz_common/properties/vector_property.hpp" -#include "rviz_common/selection/selection_manager.hpp" +#include "rviz_common/interaction/selection_manager.hpp" #include "rviz_common/validate_floats.hpp" #include "pose_display_selection_handler.hpp" @@ -115,7 +115,8 @@ void PoseDisplay::onInitialize() updateShapeChoice(); updateColorAndAlpha(); - coll_handler_ = std::make_shared(this, context_); + coll_handler_ = rviz_common::interaction::createSelectionHandler + (this, context_); coll_handler_->addTrackedObjects(arrow_->getSceneNode()); coll_handler_->addTrackedObjects(axes_->getSceneNode()); } diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/pose/pose_display.hpp b/rviz_default_plugins/src/rviz_default_plugins/displays/pose/pose_display.hpp index f2e8f745d..e8b2fdca6 100644 --- a/rviz_default_plugins/src/rviz_default_plugins/displays/pose/pose_display.hpp +++ b/rviz_default_plugins/src/rviz_default_plugins/displays/pose/pose_display.hpp @@ -36,7 +36,7 @@ #include "geometry_msgs/msg/pose_stamped.hpp" #include "rviz_common/ros_topic_display.hpp" -#include "rviz_common/selection/forwards.hpp" +#include "rviz_common/interaction/forwards.hpp" namespace rviz_rendering { diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/pose/pose_display_selection_handler.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/pose/pose_display_selection_handler.cpp index 25168b2ef..24fd10ca7 100644 --- a/rviz_default_plugins/src/rviz_default_plugins/displays/pose/pose_display_selection_handler.cpp +++ b/rviz_default_plugins/src/rviz_default_plugins/displays/pose/pose_display_selection_handler.cpp @@ -50,7 +50,7 @@ #include "rviz_rendering/objects/axes.hpp" #include "rviz_rendering/objects/arrow.hpp" #include "rviz_rendering/objects/shape.hpp" -#include "rviz_common/selection/selection_handler.hpp" +#include "rviz_common/interaction/selection_handler.hpp" #include "rviz_common/properties/vector_property.hpp" #include "rviz_common/properties/string_property.hpp" #include "rviz_common/properties/quaternion_property.hpp" @@ -73,7 +73,7 @@ PoseDisplaySelectionHandler::PoseDisplaySelectionHandler( {} void PoseDisplaySelectionHandler::createProperties( - const rviz_common::selection::Picked & obj, + const rviz_common::interaction::Picked & obj, rviz_common::properties::Property * parent_property) { (void) obj; @@ -93,20 +93,29 @@ void PoseDisplaySelectionHandler::createProperties( orientation_property_->setReadOnly(true); } -void PoseDisplaySelectionHandler::getAABBs( - const rviz_common::selection::Picked & obj, rviz_common::selection::V_AABB & aabbs) +rviz_common::interaction::V_AABB PoseDisplaySelectionHandler::getAABBs( + const rviz_common::interaction::Picked & obj) { (void) obj; + rviz_common::interaction::V_AABB aabbs; if (display_->pose_valid_) { + /** with 'derive_world_bounding_box' set to 'true', the WorldBoundingBox is derived each time. + setting it to 'false' results in the wire box not properly following the pose arrow, but it + would be less computationally expensive. + */ + bool derive_world_bounding_box = true; if (display_->shape_property_->getOptionInt() == PoseDisplay::Arrow) { - aabbs.push_back(display_->arrow_->getHead()->getEntity()->getWorldBoundingBox()); - aabbs.push_back(display_->arrow_->getShaft()->getEntity()->getWorldBoundingBox()); + aabbs.push_back( + display_->arrow_->getHead()->getEntity()->getWorldBoundingBox(derive_world_bounding_box)); + aabbs.push_back( + display_->arrow_->getShaft()->getEntity()->getWorldBoundingBox(derive_world_bounding_box)); } else { aabbs.push_back(display_->axes_->getXShape()->getEntity()->getWorldBoundingBox()); aabbs.push_back(display_->axes_->getYShape()->getEntity()->getWorldBoundingBox()); aabbs.push_back(display_->axes_->getZShape()->getEntity()->getWorldBoundingBox()); } } + return aabbs; } void PoseDisplaySelectionHandler::setMessage( diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/pose/pose_display_selection_handler.hpp b/rviz_default_plugins/src/rviz_default_plugins/displays/pose/pose_display_selection_handler.hpp index 0f85d60fc..e94620c5f 100644 --- a/rviz_default_plugins/src/rviz_default_plugins/displays/pose/pose_display_selection_handler.hpp +++ b/rviz_default_plugins/src/rviz_default_plugins/displays/pose/pose_display_selection_handler.hpp @@ -31,9 +31,11 @@ #ifndef RVIZ_DEFAULT_PLUGINS__DISPLAYS__POSE__POSE_DISPLAY_SELECTION_HANDLER_HPP_ #define RVIZ_DEFAULT_PLUGINS__DISPLAYS__POSE__POSE_DISPLAY_SELECTION_HANDLER_HPP_ +#include + #include "geometry_msgs/msg/pose_stamped.hpp" -#include "rviz_common/selection/selection_handler.hpp" +#include "rviz_common/interaction/selection_handler.hpp" #include "pose_display.hpp" namespace rviz_common @@ -51,25 +53,28 @@ namespace rviz_default_plugins namespace displays { -class PoseDisplaySelectionHandler : public rviz_common::selection::SelectionHandler +class PoseDisplaySelectionHandler : public rviz_common::interaction::SelectionHandler { public: - PoseDisplaySelectionHandler(PoseDisplay * display, rviz_common::DisplayContext * context); - void createProperties( - const rviz_common::selection::Picked & obj, + const rviz_common::interaction::Picked & obj, rviz_common::properties::Property * parent_property) override; - void getAABBs( - const rviz_common::selection::Picked & obj, rviz_common::selection::V_AABB & aabbs) override; + rviz_common::interaction::V_AABB getAABBs(const rviz_common::interaction::Picked & obj) override; void setMessage(geometry_msgs::msg::PoseStamped::ConstSharedPtr message); private: + PoseDisplaySelectionHandler(PoseDisplay * display, rviz_common::DisplayContext * context); + PoseDisplay * display_; rviz_common::properties::StringProperty * frame_property_; rviz_common::properties::VectorProperty * position_property_; rviz_common::properties::QuaternionProperty * orientation_property_; + + template + friend typename std::shared_ptr + rviz_common::interaction::createSelectionHandler(Args ... arguments); }; } // namespace displays diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/tf/frame_info.hpp b/rviz_default_plugins/src/rviz_default_plugins/displays/tf/frame_info.hpp index 5a7f9147a..889792f6f 100644 --- a/rviz_default_plugins/src/rviz_default_plugins/displays/tf/frame_info.hpp +++ b/rviz_default_plugins/src/rviz_default_plugins/displays/tf/frame_info.hpp @@ -99,7 +99,7 @@ public Q_SLOTS: std::string name_; std::string parent_; rviz_rendering::Axes * axes_; - rviz_common::selection::CollObjectHandle axes_coll_; + rviz_common::interaction::CollObjectHandle axes_coll_; FrameSelectionHandlerPtr selection_handler_; rviz_rendering::Arrow * parent_arrow_; rviz_rendering::MovableText * name_text_; diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/tf/frame_selection_handler.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/tf/frame_selection_handler.cpp index 2df1837e8..b12533079 100644 --- a/rviz_default_plugins/src/rviz_default_plugins/displays/tf/frame_selection_handler.cpp +++ b/rviz_default_plugins/src/rviz_default_plugins/displays/tf/frame_selection_handler.cpp @@ -38,7 +38,7 @@ #include "rviz_common/properties/string_property.hpp" #include "rviz_common/properties/vector_property.hpp" -using rviz_common::selection::SelectionHandler; +using rviz_common::interaction::SelectionHandler; using rviz_common::properties::BoolProperty; using rviz_common::properties::FloatProperty; using rviz_common::properties::StatusProperty; @@ -46,7 +46,7 @@ using rviz_common::properties::StringProperty; using rviz_common::properties::Property; using rviz_common::properties::QuaternionProperty; using rviz_common::properties::VectorProperty; -using rviz_common::selection::Picked; +using rviz_common::interaction::Picked; namespace rviz_default_plugins { diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/tf/frame_selection_handler.hpp b/rviz_default_plugins/src/rviz_default_plugins/displays/tf/frame_selection_handler.hpp index 09f049de3..8405c5439 100644 --- a/rviz_default_plugins/src/rviz_default_plugins/displays/tf/frame_selection_handler.hpp +++ b/rviz_default_plugins/src/rviz_default_plugins/displays/tf/frame_selection_handler.hpp @@ -30,9 +30,10 @@ #ifndef RVIZ_DEFAULT_PLUGINS__DISPLAYS__TF__FRAME_SELECTION_HANDLER_HPP_ #define RVIZ_DEFAULT_PLUGINS__DISPLAYS__TF__FRAME_SELECTION_HANDLER_HPP_ +#include #include -#include "rviz_common/selection/selection_handler.hpp" +#include "rviz_common/interaction/selection_handler.hpp" #include "tf_display.hpp" #include "frame_info.hpp" @@ -55,22 +56,17 @@ namespace rviz_default_plugins namespace displays { -class FrameSelectionHandler : public rviz_common::selection::SelectionHandler +class FrameSelectionHandler : public rviz_common::interaction::SelectionHandler { public: - FrameSelectionHandler( - FrameInfo * frame, - TFDisplay * display, - rviz_common::DisplayContext * context); - ~FrameSelectionHandler() override = default; void createProperties( - const rviz_common::selection::Picked & obj, + const rviz_common::interaction::Picked & obj, rviz_common::properties::Property * parent_property) override; void destroyProperties( - const rviz_common::selection::Picked & obj, + const rviz_common::interaction::Picked & obj, rviz_common::properties::Property * parent_property) override; bool getEnabled(); @@ -84,6 +80,11 @@ class FrameSelectionHandler : public rviz_common::selection::SelectionHandler void setOrientation(const Ogre::Quaternion & orientation); private: + FrameSelectionHandler( + FrameInfo * frame, + TFDisplay * display, + rviz_common::DisplayContext * context); + FrameInfo * frame_; TFDisplay * display_; rviz_common::properties::Property * category_property_; @@ -91,6 +92,10 @@ class FrameSelectionHandler : public rviz_common::selection::SelectionHandler rviz_common::properties::StringProperty * parent_property_; rviz_common::properties::VectorProperty * position_property_; rviz_common::properties::QuaternionProperty * orientation_property_; + + template + friend typename std::shared_ptr + rviz_common::interaction::createSelectionHandler(Args ... arguments); }; } // namespace displays diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/tf/tf_display.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/tf/tf_display.cpp index 849bc4c5c..98a57aea7 100644 --- a/rviz_default_plugins/src/rviz_default_plugins/displays/tf/tf_display.cpp +++ b/rviz_default_plugins/src/rviz_default_plugins/displays/tf/tf_display.cpp @@ -62,13 +62,13 @@ #include "rviz_common/properties/quaternion_property.hpp" #include "rviz_common/properties/string_property.hpp" #include "rviz_common/properties/vector_property.hpp" -#include "rviz_common/selection/forwards.hpp" -#include "rviz_common/selection/selection_manager.hpp" +#include "rviz_common/interaction/forwards.hpp" +#include "rviz_common/interaction/selection_manager.hpp" #include "rviz_common/uniform_string_stream.hpp" #include "frame_info.hpp" #include "frame_selection_handler.hpp" -using rviz_common::selection::SelectionHandler; +using rviz_common::interaction::SelectionHandler; using rviz_common::properties::BoolProperty; using rviz_common::properties::FloatProperty; using rviz_common::properties::StatusProperty; @@ -76,7 +76,7 @@ using rviz_common::properties::StringProperty; using rviz_common::properties::Property; using rviz_common::properties::QuaternionProperty; using rviz_common::properties::VectorProperty; -using rviz_common::selection::Picked; +using rviz_common::interaction::Picked; using rviz_rendering::Axes; using rviz_rendering::Arrow; using rviz_rendering::MovableText; @@ -345,7 +345,8 @@ FrameInfo * TFDisplay::createFrame(const std::string & frame) info->last_update_ = tf2::get_now(); info->axes_ = new Axes(scene_manager_, axes_node_, 0.2f, 0.02f); info->axes_->getSceneNode()->setVisible(show_axes_property_->getBool()); - info->selection_handler_.reset(new FrameSelectionHandler(info, this, context_)); + info->selection_handler_ = + rviz_common::interaction::createSelectionHandler(info, this, context_); info->selection_handler_->addTrackedObjects(info->axes_->getSceneNode()); info->name_text_ = new MovableText(frame, "Liberation Sans", 0.1f); @@ -573,7 +574,7 @@ void TFDisplay::deleteFrame(FrameInfo * frame, bool delete_properties) frames_.erase(it); delete frame->axes_; - context_->getSelectionManager()->removeObject(frame->axes_coll_); + context_->getHandlerManager()->removeHandler(frame->axes_coll_); delete frame->parent_arrow_; delete frame->name_text_; scene_manager_->destroySceneNode(frame->name_node_->getName()); diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/tf/tf_display.hpp b/rviz_default_plugins/src/rviz_default_plugins/displays/tf/tf_display.hpp index 3b88483f3..5071665ef 100644 --- a/rviz_default_plugins/src/rviz_default_plugins/displays/tf/tf_display.hpp +++ b/rviz_default_plugins/src/rviz_default_plugins/displays/tf/tf_display.hpp @@ -52,7 +52,7 @@ #include "tf2/buffer_core.h" #include "tf2/time.h" -#include "rviz_common/selection/forwards.hpp" +#include "rviz_common/interaction/forwards.hpp" #include "rviz_common/display.hpp" namespace Ogre diff --git a/rviz_default_plugins/src/rviz_default_plugins/robot/robot_element_base_class.hpp b/rviz_default_plugins/src/rviz_default_plugins/robot/robot_element_base_class.hpp index fe936ab2e..43a1b5f07 100644 --- a/rviz_default_plugins/src/rviz_default_plugins/robot/robot_element_base_class.hpp +++ b/rviz_default_plugins/src/rviz_default_plugins/robot/robot_element_base_class.hpp @@ -58,7 +58,7 @@ #include #include "rviz_rendering/objects/object.hpp" -#include "rviz_common/selection/forwards.hpp" +#include "rviz_common/interaction/forwards.hpp" namespace Ogre { diff --git a/rviz_default_plugins/src/rviz_default_plugins/robot/robot_joint.hpp b/rviz_default_plugins/src/rviz_default_plugins/robot/robot_joint.hpp index 97d404228..d127a30af 100644 --- a/rviz_default_plugins/src/rviz_default_plugins/robot/robot_joint.hpp +++ b/rviz_default_plugins/src/rviz_default_plugins/robot/robot_joint.hpp @@ -58,7 +58,7 @@ #include "urdf_model/pose.h" #include "rviz_rendering/objects/object.hpp" -#include "rviz_common/selection/forwards.hpp" +#include "rviz_common/interaction/forwards.hpp" #include "robot_element_base_class.hpp" namespace Ogre diff --git a/rviz_default_plugins/src/rviz_default_plugins/robot/robot_link.cpp b/rviz_default_plugins/src/rviz_default_plugins/robot/robot_link.cpp index 245165165..ec8d3cc58 100644 --- a/rviz_default_plugins/src/rviz_default_plugins/robot/robot_link.cpp +++ b/rviz_default_plugins/src/rviz_default_plugins/robot/robot_link.cpp @@ -80,7 +80,7 @@ #include "rviz_common/properties/property.hpp" #include "rviz_common/properties/quaternion_property.hpp" #include "rviz_common/properties/vector_property.hpp" -#include "rviz_common/selection/selection_manager.hpp" +#include "rviz_common/interaction/selection_manager.hpp" using rviz_rendering::Axes; using rviz_rendering::Shape; @@ -95,14 +95,14 @@ using rviz_common::properties::FloatProperty; using rviz_common::properties::QuaternionProperty; using rviz_common::properties::VectorProperty; -class RobotLinkSelectionHandler : public rviz_common::selection::SelectionHandler +class RobotLinkSelectionHandler : public rviz_common::interaction::SelectionHandler { public: RobotLinkSelectionHandler(RobotLink * link, rviz_common::DisplayContext * context); ~RobotLinkSelectionHandler() override; void createProperties( - const rviz_common::selection::Picked & obj, + const rviz_common::interaction::Picked & obj, Property * parent_property) override; void updateProperties() override; @@ -113,6 +113,10 @@ class RobotLinkSelectionHandler : public rviz_common::selection::SelectionHandle RobotLink * link_; rviz_common::properties::VectorProperty * position_property_; rviz_common::properties::QuaternionProperty * orientation_property_; + + template + friend typename std::shared_ptr rviz_common::interaction::createSelectionHandler( + Args ... arguments); }; RobotLinkSelectionHandler::RobotLinkSelectionHandler( @@ -127,7 +131,7 @@ RobotLinkSelectionHandler::RobotLinkSelectionHandler( RobotLinkSelectionHandler::~RobotLinkSelectionHandler() = default; void RobotLinkSelectionHandler::createProperties( - const rviz_common::selection::Picked & obj, + const rviz_common::interaction::Picked & obj, rviz_common::properties::Property * parent_property) { (void) obj; @@ -785,7 +789,8 @@ void RobotLink::createVisual(const urdf::LinkConstSharedPtr & link) void RobotLink::createSelection() { - selection_handler_.reset(new RobotLinkSelectionHandler(this, context_)); + selection_handler_ = rviz_common::interaction::createSelectionHandler( + this, context_); for (auto & visual_mesh : visual_meshes_) { selection_handler_->addTrackedObject(visual_mesh); } diff --git a/rviz_default_plugins/src/rviz_default_plugins/robot/robot_link.hpp b/rviz_default_plugins/src/rviz_default_plugins/robot/robot_link.hpp index 2da406ed2..f5c7f6404 100644 --- a/rviz_default_plugins/src/rviz_default_plugins/robot/robot_link.hpp +++ b/rviz_default_plugins/src/rviz_default_plugins/robot/robot_link.hpp @@ -61,7 +61,7 @@ #include "robot_element_base_class.hpp" #include "rviz_rendering/objects/object.hpp" -#include "rviz_common/selection/forwards.hpp" +#include "rviz_common/interaction/forwards.hpp" namespace Ogre { diff --git a/rviz/src/rviz/default_plugin/tools/selection_tool.cpp b/rviz_default_plugins/src/rviz_default_plugins/tools/select/selection_tool.cpp similarity index 52% rename from rviz/src/rviz/default_plugin/tools/selection_tool.cpp rename to rviz_default_plugins/src/rviz_default_plugins/tools/select/selection_tool.cpp index 7852e8132..5d0ff2784 100644 --- a/rviz/src/rviz/default_plugin/tools/selection_tool.cpp +++ b/rviz_default_plugins/src/rviz_default_plugins/tools/select/selection_tool.cpp @@ -27,7 +27,16 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include +#include "selection_tool.hpp" + +#ifndef _WIN32 +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wunused-parameter" +# pragma GCC diagnostic ignored "-Wpedantic" +#else +#pragma warning(push) +#pragma warning(disable : 4996) +#endif #include #include @@ -40,31 +49,37 @@ #include #include -#include +#ifndef _WIN32 +# pragma GCC diagnostic pop +#else +# pragma warning(pop) +#endif -#include "move_tool.h" +#include // NOLINT cpplint cannot handle include order -#include "rviz/ogre_helpers/camera_base.h" -#include "rviz/ogre_helpers/qt_ogre_render_window.h" -#include "rviz/selection/selection_manager.h" -#include "rviz/visualization_manager.h" -#include "rviz/render_panel.h" -#include "rviz/display.h" -#include "rviz/viewport_mouse_event.h" -#include "rviz/load_resource.h" +#include "../move/move_tool.hpp" -#include "selection_tool.h" +#include "rviz_common/interaction/selection_manager.hpp" +#include "rviz_common/display_context.hpp" +#include "rviz_common/render_panel.hpp" +#include "rviz_common/display.hpp" +#include "rviz_common/tool.hpp" +#include "rviz_common/viewport_mouse_event.hpp" +#include "rviz_common/load_resource.hpp" -namespace rviz +namespace rviz_default_plugins +{ +namespace tools { + SelectionTool::SelectionTool() - : Tool() - , move_tool_( new MoveTool() ) - , selecting_( false ) - , sel_start_x_( 0 ) - , sel_start_y_( 0 ) - , moving_( false ) +: Tool(), + move_tool_(new MoveTool()), + selecting_(false), + sel_start_x_(0), + sel_start_y_(0), + moving_(false) { shortcut_key_ = 's'; access_all_keys_ = true; @@ -77,12 +92,12 @@ SelectionTool::~SelectionTool() void SelectionTool::onInitialize() { - move_tool_->initialize( context_ ); + move_tool_->initialize(context_); } void SelectionTool::activate() { - setStatus( "Click and drag to select objects on the screen." ); + setStatus("Click and drag to select objects on the screen."); context_->getSelectionManager()->setTextureSize(512); selecting_ = false; moving_ = false; @@ -96,31 +111,28 @@ void SelectionTool::deactivate() void SelectionTool::update(float wall_dt, float ros_dt) { - SelectionManager* sel_manager = context_->getSelectionManager(); + (void) wall_dt; + (void) ros_dt; + auto sel_manager = context_->getSelectionManager(); - if (!selecting_) - { + if (!selecting_) { sel_manager->removeHighlight(); } } -int SelectionTool::processMouseEvent( ViewportMouseEvent& event ) +int SelectionTool::processMouseEvent(rviz_common::ViewportMouseEvent & event) { - SelectionManager* sel_manager = context_->getSelectionManager(); + auto sel_manager = context_->getSelectionManager(); int flags = 0; - if( event.alt() ) - { + if (event.alt()) { moving_ = true; selecting_ = false; - } - else - { + } else { moving_ = false; - if( event.leftDown() ) - { + if (event.leftDown()) { selecting_ = true; sel_start_x_ = event.x; @@ -128,64 +140,72 @@ int SelectionTool::processMouseEvent( ViewportMouseEvent& event ) } } - if( selecting_ ) - { - sel_manager->highlight( event.viewport, sel_start_x_, sel_start_y_, event.x, event.y ); + if (selecting_) { + sel_manager->highlight( + event.panel->getRenderWindow(), + sel_start_x_, + sel_start_y_, + event.x, + event.y); - if( event.leftUp() ) - { - SelectionManager::SelectType type = SelectionManager::Replace; + if (event.leftUp()) { + rviz_common::interaction::SelectionManager::SelectType type = + rviz_common::interaction::SelectionManager::Replace; - M_Picked selection; + rviz_common::interaction::M_Picked selection; - if( event.shift() ) - { - type = SelectionManager::Add; - } - else if( event.control() ) - { - type = SelectionManager::Remove; + if (event.shift()) { + type = rviz_common::interaction::SelectionManager::Add; + } else if (event.control()) { + type = rviz_common::interaction::SelectionManager::Remove; } - sel_manager->select( event.viewport, sel_start_x_, sel_start_y_, event.x, event.y, type ); + sel_manager->select( + event.panel->getRenderWindow(), + sel_start_x_, + sel_start_y_, + event.x, + event.y, + type); selecting_ = false; } flags |= Render; - } - else if( moving_ ) - { + } else if (moving_) { sel_manager->removeHighlight(); - flags = move_tool_->processMouseEvent( event ); + flags = move_tool_->processMouseEvent(event); - if( event.type == QEvent::MouseButtonRelease ) - { + if (event.type == QEvent::MouseButtonRelease) { moving_ = false; } - } - else - { - sel_manager->highlight( event.viewport, event.x, event.y, event.x, event.y ); + } else { + sel_manager->highlight( + event.panel->getRenderWindow(), + event.x, + event.y, + event.x, + event.y); } return flags; } -int SelectionTool::processKeyEvent( QKeyEvent* event, RenderPanel* panel ) +int SelectionTool::processKeyEvent(QKeyEvent * event, rviz_common::RenderPanel * panel) { - SelectionManager* sel_manager = context_->getSelectionManager(); + (void) panel; + auto sel_manager = context_->getSelectionManager(); - if( event->key() == Qt::Key_F ) - { + if (event->key() == Qt::Key_F) { sel_manager->focusOnSelection(); } return Render; } -} // end namespace rviz +} // namespace tools +} // namespace rviz_default_plugins -#include -PLUGINLIB_EXPORT_CLASS( rviz::SelectionTool, rviz::Tool ) +#include // NOLINT +PLUGINLIB_EXPORT_CLASS(rviz_default_plugins::tools::SelectionTool, rviz_common::Tool) diff --git a/rviz/src/rviz/default_plugin/tools/selection_tool.h b/rviz_default_plugins/src/rviz_default_plugins/tools/select/selection_tool.hpp similarity index 74% rename from rviz/src/rviz/default_plugin/tools/selection_tool.h rename to rviz_default_plugins/src/rviz_default_plugins/tools/select/selection_tool.hpp index 0e227fad9..fa058cfe8 100644 --- a/rviz/src/rviz/default_plugin/tools/selection_tool.h +++ b/rviz_default_plugins/src/rviz_default_plugins/tools/select/selection_tool.hpp @@ -27,25 +27,27 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef RVIZ_SELECTION_TOOL_H -#define RVIZ_SELECTION_TOOL_H - -#include "rviz/tool.h" -#include "rviz/selection/forwards.h" +#ifndef RVIZ_DEFAULT_PLUGINS__TOOLS__SELECT__SELECTION_TOOL_HPP_ +#define RVIZ_DEFAULT_PLUGINS__TOOLS__SELECT__SELECTION_TOOL_HPP_ #include +#include "rviz_common/tool.hpp" +#include "rviz_common/interaction/forwards.hpp" + namespace Ogre { class Viewport; } -namespace rviz +namespace rviz_default_plugins +{ +namespace tools { class MoveTool; -class SelectionTool : public Tool +class SelectionTool : public rviz_common::Tool { public: SelectionTool(); @@ -56,25 +58,24 @@ class SelectionTool : public Tool virtual void activate(); virtual void deactivate(); - virtual int processMouseEvent( ViewportMouseEvent& event ); - virtual int processKeyEvent( QKeyEvent* event, RenderPanel* panel ); + virtual int processMouseEvent(rviz_common::ViewportMouseEvent & event); + virtual int processKeyEvent(QKeyEvent * event, rviz_common::RenderPanel * panel); virtual void update(float wall_dt, float ros_dt); private: - - MoveTool* move_tool_; + MoveTool * move_tool_; bool selecting_; int sel_start_x_; int sel_start_y_; - M_Picked highlight_; + rviz_common::interaction::M_Picked highlight_; bool moving_; }; -} - -#endif +} // namespace tools +} // namespace rviz_default_plugins +#endif // RVIZ_DEFAULT_PLUGINS__TOOLS__SELECT__SELECTION_TOOL_HPP_ diff --git a/rviz_default_plugins/test/rviz_default_plugins/displays/display_test_fixture.cpp b/rviz_default_plugins/test/rviz_default_plugins/displays/display_test_fixture.cpp index 92ec9cd60..240a3617b 100644 --- a/rviz_default_plugins/test/rviz_default_plugins/displays/display_test_fixture.cpp +++ b/rviz_default_plugins/test/rviz_default_plugins/displays/display_test_fixture.cpp @@ -43,11 +43,12 @@ void DisplayTestFixture::SetUpTestCase() scene_manager_ = Ogre::Root::getSingletonPtr()->createSceneManager(); } -void DisplayTestFixture::SetUp() +DisplayTestFixture::DisplayTestFixture() { context_ = std::make_shared>(); frame_manager_ = std::make_shared>(); selection_manager_ = std::make_shared>(); + handler_manager_ = std::make_shared>(); clock_ = std::make_shared(); EXPECT_CALL(*frame_manager_, getFixedFrame()).WillRepeatedly(testing::ReturnRef(fixed_frame)); @@ -56,10 +57,12 @@ void DisplayTestFixture::SetUp() EXPECT_CALL(*context_, getSceneManager()).WillRepeatedly(testing::Return(scene_manager_)); EXPECT_CALL(*context_, getFrameManager()).WillRepeatedly(testing::Return(frame_manager_.get())); EXPECT_CALL(*context_, getSelectionManager()).WillRepeatedly( - testing::Return(selection_manager_.get())); + testing::Return(selection_manager_)); + EXPECT_CALL(*context_, getHandlerManager()).WillRepeatedly( + testing::Return(handler_manager_)); } -void DisplayTestFixture::TearDown() +DisplayTestFixture::~DisplayTestFixture() { scene_manager_->getRootSceneNode()->removeAndDestroyAllChildren(); } diff --git a/rviz_default_plugins/test/rviz_default_plugins/displays/display_test_fixture.hpp b/rviz_default_plugins/test/rviz_default_plugins/displays/display_test_fixture.hpp index 8914c8ea7..e5bb5179e 100644 --- a/rviz_default_plugins/test/rviz_default_plugins/displays/display_test_fixture.hpp +++ b/rviz_default_plugins/test/rviz_default_plugins/displays/display_test_fixture.hpp @@ -49,15 +49,16 @@ #include "../mock_display_context.hpp" #include "../mock_frame_manager.hpp" #include "../mock_selection_manager.hpp" +#include "../mock_handler_manager.hpp" class DisplayTestFixture : public testing::Test { public: static void SetUpTestCase(); - void SetUp() override; + DisplayTestFixture(); - void TearDown() override; + ~DisplayTestFixture(); static void TearDownTestCase(); @@ -71,6 +72,7 @@ class DisplayTestFixture : public testing::Test std::shared_ptr context_; std::shared_ptr frame_manager_; std::shared_ptr selection_manager_; + std::shared_ptr handler_manager_; std::shared_ptr clock_; std::string fixed_frame = "fixed_frame"; diff --git a/rviz_default_plugins/test/rviz_default_plugins/displays/image/image_display_test.cpp b/rviz_default_plugins/test/rviz_default_plugins/displays/image/image_display_test.cpp index 7f8585208..58607a736 100644 --- a/rviz_default_plugins/test/rviz_default_plugins/displays/image/image_display_test.cpp +++ b/rviz_default_plugins/test/rviz_default_plugins/displays/image/image_display_test.cpp @@ -61,7 +61,7 @@ class ImageDisplayTestFixture : public Test testing_environment_->setUpOgreTestEnvironment(); } - void SetUp() override + ImageDisplayTestFixture() { texture_ = std::make_unique(); ON_CALL(*texture_, getName()).WillByDefault(Return("texture")); diff --git a/rviz_default_plugins/test/rviz_default_plugins/displays/marker/marker_display_test.cpp b/rviz_default_plugins/test/rviz_default_plugins/displays/marker/marker_display_test.cpp index 86087d188..4c5a42ed7 100644 --- a/rviz_default_plugins/test/rviz_default_plugins/displays/marker/marker_display_test.cpp +++ b/rviz_default_plugins/test/rviz_default_plugins/displays/marker/marker_display_test.cpp @@ -61,10 +61,8 @@ using namespace ::testing; // NOLINT class MarkerDisplayFixture : public DisplayTestFixture { public: - void SetUp() override + MarkerDisplayFixture() { - DisplayTestFixture::SetUp(); - factory_ = std::make_unique(); factory_->initialize( nullptr, context_.get(), scene_manager_->getRootSceneNode()->createChildSceneNode()); @@ -73,12 +71,6 @@ class MarkerDisplayFixture : public DisplayTestFixture std::move(factory_), context_.get()); } - void TearDown() override - { - display_.reset(nullptr); - DisplayTestFixture::TearDown(); - } - std::unique_ptr factory_; std::unique_ptr display_; }; diff --git a/rviz_default_plugins/test/rviz_default_plugins/displays/marker/markers/markers_test_fixture.cpp b/rviz_default_plugins/test/rviz_default_plugins/displays/marker/markers/markers_test_fixture.cpp index d93c1fc16..297072977 100644 --- a/rviz_default_plugins/test/rviz_default_plugins/displays/marker/markers/markers_test_fixture.cpp +++ b/rviz_default_plugins/test/rviz_default_plugins/displays/marker/markers/markers_test_fixture.cpp @@ -34,15 +34,7 @@ #include #include -void MarkersTestFixture::SetUp() +MarkersTestFixture::MarkersTestFixture() { - DisplayTestFixture::SetUp(); marker_display_ = std::make_shared(); } - -void MarkersTestFixture::TearDown() -{ - marker_display_.reset(); - marker_.reset(); - DisplayTestFixture::TearDown(); -} diff --git a/rviz_default_plugins/test/rviz_default_plugins/displays/marker/markers/markers_test_fixture.hpp b/rviz_default_plugins/test/rviz_default_plugins/displays/marker/markers/markers_test_fixture.hpp index 152e0abfe..330af84b1 100644 --- a/rviz_default_plugins/test/rviz_default_plugins/displays/marker/markers/markers_test_fixture.hpp +++ b/rviz_default_plugins/test/rviz_default_plugins/displays/marker/markers/markers_test_fixture.hpp @@ -54,9 +54,7 @@ class MarkersTestFixture : public DisplayTestFixture { public: - void SetUp() override; - - void TearDown() override; + MarkersTestFixture(); template std::unique_ptr makeMarker() diff --git a/rviz_default_plugins/test/rviz_default_plugins/displays/marker/markers/mesh_resource_marker_test.cpp b/rviz_default_plugins/test/rviz_default_plugins/displays/marker/markers/mesh_resource_marker_test.cpp index fae4ad343..777b7cdee 100644 --- a/rviz_default_plugins/test/rviz_default_plugins/displays/marker/markers/mesh_resource_marker_test.cpp +++ b/rviz_default_plugins/test/rviz_default_plugins/displays/marker/markers/mesh_resource_marker_test.cpp @@ -43,6 +43,7 @@ #else #include #endif +#include #include #include #include diff --git a/rviz_default_plugins/test/rviz_default_plugins/displays/path/path_display_test.cpp b/rviz_default_plugins/test/rviz_default_plugins/displays/path/path_display_test.cpp index 15dba1729..98c244ea0 100644 --- a/rviz_default_plugins/test/rviz_default_plugins/displays/path/path_display_test.cpp +++ b/rviz_default_plugins/test/rviz_default_plugins/displays/path/path_display_test.cpp @@ -61,18 +61,11 @@ using namespace ::testing; // NOLINT class PathTestFixture : public DisplayTestFixture { public: - void SetUp() override + PathTestFixture() { - DisplayTestFixture::SetUp(); path_display_ = std::make_shared(context_.get()); } - void TearDown() override - { - path_display_.reset(); - DisplayTestFixture::TearDown(); - } - std::shared_ptr path_display_; }; diff --git a/rviz_default_plugins/test/rviz_default_plugins/displays/pointcloud/point_cloud_selection_handler_test.cpp b/rviz_default_plugins/test/rviz_default_plugins/displays/pointcloud/point_cloud_selection_handler_test.cpp new file mode 100644 index 000000000..deeaae787 --- /dev/null +++ b/rviz_default_plugins/test/rviz_default_plugins/displays/pointcloud/point_cloud_selection_handler_test.cpp @@ -0,0 +1,258 @@ +/* + * Copyright (c) 2018, Bosch Software Innovations GmbH. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted (subject to the limitations in the disclaimer + * below) 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 copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. 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 HOLDER 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. + */ + +#include +#include + +#include +#include + +#include + +#include "sensor_msgs/msg/point_cloud2.hpp" + +#include "rviz_rendering/objects/point_cloud.hpp" +#include "rviz_common/interaction/selection_handler.hpp" + +#include "../../../../src/rviz_default_plugins/displays/pointcloud/point_cloud_common.hpp" +#include "../../../../src/rviz_default_plugins/displays/pointcloud/point_cloud2_display.hpp" + +#include "../../scene_graph_introspection.hpp" +#include "../display_test_fixture.hpp" +#include "./message_creators.hpp" + +using namespace ::testing; // NOLINT +using namespace rviz_default_plugins::displays; // NOLINT + +MATCHER_P(ContainsWireBoxWithBoundingBox, AABB, "") { + for (const auto & box : arg) { + if (box->getBoundingBox() == AABB) { + return true; + } + } + return false; +} + +MATCHER_P(HasPositionPropertyWithPosition, position, "") { + return arg->getNameStd() == "Position" && + arg->numChildren() == 3 && + abs(arg->childAt(0)->getValue().toFloat() - position.x) < 0.001 && + abs(arg->childAt(1)->getValue().toFloat() - position.y) < 0.001 && + abs(arg->childAt(2)->getValue().toFloat() - position.z) < 0.001; +} + +MATCHER_P(HasNumberOfSubproperties, number, "") { + return arg->numChildren() == number; +} + +MATCHER_P2(HasColorProperty, color_string, alpha, "") { + return arg->numChildren() == 3 && + arg->childAt(1)->getValue().toString().toStdString() == color_string && + abs(arg->childAt(2)->getValue().toFloat() - alpha) < 0.001; +} + +MATCHER_P(HasIntensityProperty, intensity, "") { + return arg->numChildren() == 2 && abs(arg->childAt(1)->getValue().toFloat() - intensity) < 0.001; +} + +class PointCloudSelectionHandlerFixture : public DisplayTestFixture +{ +public: + PointCloudSelectionHandlerFixture() + { + Ogre::MaterialManager::getSingletonPtr()->load("RVIZ/Cyan", "rviz_rendering"); + } +}; + +std::shared_ptr createCloudInfoWithSquare( + Ogre::SceneManager * scene_manager, rviz_common::DisplayContext * context, + sensor_msgs::msg::PointCloud2::ConstSharedPtr message) +{ + std::vector points; + points.push_back({Ogre::Vector3(1, 1, 1), Ogre::ColourValue(0, 0, 0, 1)}); + points.push_back({Ogre::Vector3(-1, -1, 1), Ogre::ColourValue(0, 0, 0, 1)}); + points.push_back({Ogre::Vector3(-1, 1, 1), Ogre::ColourValue(0, 0, 0, 1)}); + points.push_back({Ogre::Vector3(1, -1, 1), Ogre::ColourValue(0, 0, 0, 1)}); + + auto cloud = std::make_shared(); + cloud->addPoints(points.begin(), points.end()); + + auto cloud_info = std::make_shared(); + cloud_info->message_ = message; + cloud_info->receive_time_ = message->header.stamp; + cloud_info->manager_ = scene_manager; + cloud_info->scene_node_ = scene_manager->getRootSceneNode()->createChildSceneNode(); + cloud_info->cloud_ = cloud; + cloud_info->transformed_points_ = points; + cloud_info->orientation_ = Ogre::Quaternion::IDENTITY; + cloud_info->position_ = Ogre::Vector3::ZERO; + cloud_info->setSelectable(true, 1, context); + cloud_info->scene_node_ = scene_manager + ->getRootSceneNode() + ->createChildSceneNode() + ->createChildSceneNode(cloud_info->position_, cloud_info->orientation_); + return cloud_info; +} + +TEST_F(PointCloudSelectionHandlerFixture, onSelect_selects_only_points_actually_picked) +{ + std::vector message_points = + {{1, 1, 1}, {-1, -1, 1}, {-1, 1, 1}, {1, -1, 1}}; + auto message = rviz_default_plugins::createPointCloud2WithPoints(message_points); + auto cloud_info = createCloudInfoWithSquare(scene_manager_, context_.get(), message); + + rviz_common::interaction::Picked picked_object(cloud_info->selection_handler_->getHandle()); + picked_object.extra_handles.clear(); + picked_object.extra_handles.insert(1); + picked_object.extra_handles.insert(4); + + cloud_info->selection_handler_->onSelect(picked_object); + rviz_common::interaction::V_AABB aabbs = cloud_info->selection_handler_->getAABBs(picked_object); + + EXPECT_THAT(aabbs, SizeIs(2)); + auto found_objects = rviz_default_plugins::findAllOgreObjectByType( + scene_manager_->getRootSceneNode(), "SimpleRenderable"); + EXPECT_THAT( + found_objects, + ContainsWireBoxWithBoundingBox(Ogre::AxisAlignedBox(0.5f, 0.5f, 0.5f, 1.5f, 1.5f, 1.5f))); + EXPECT_THAT( + found_objects, + ContainsWireBoxWithBoundingBox(Ogre::AxisAlignedBox(0.5f, -1.5f, 0.5f, 1.5f, -0.5f, 1.5f))); +} + +TEST_F(PointCloudSelectionHandlerFixture, + onDeselect_destroys_wired_bounding_boxes_for_unpicked_objects) +{ + std::vector message_points = + {{1, 1, 1}, {-1, -1, 1}, {-1, 1, 1}, {1, -1, 1}}; + auto message = rviz_default_plugins::createPointCloud2WithPoints(message_points); + auto cloud_info = createCloudInfoWithSquare(scene_manager_, context_.get(), message); + + rviz_common::interaction::Picked picked_object(cloud_info->selection_handler_->getHandle()); + picked_object.extra_handles.clear(); + picked_object.extra_handles.insert(1); + picked_object.extra_handles.insert(4); + cloud_info->selection_handler_->onSelect(picked_object); + + rviz_common::interaction::Picked unpicked_object(cloud_info->selection_handler_->getHandle()); + unpicked_object.extra_handles.clear(); + unpicked_object.extra_handles.insert(1); + cloud_info->selection_handler_->onDeselect(unpicked_object); + + rviz_common::interaction::V_AABB aabbs = cloud_info->selection_handler_->getAABBs(picked_object); + + EXPECT_THAT(aabbs, SizeIs(1)); + auto found_objects = rviz_default_plugins::findAllOgreObjectByType( + scene_manager_->getRootSceneNode(), "SimpleRenderable"); + EXPECT_THAT( + found_objects, + ContainsWireBoxWithBoundingBox(Ogre::AxisAlignedBox(0.5f, -1.5f, 0.5f, 1.5f, -0.5f, 1.5f))); +} + +TEST_F(PointCloudSelectionHandlerFixture, + createProperties_creates_tree_of_properties_without_color_for_simple_clouds) +{ + std::vector message_points = + {{1, 1, 1}, {-1, -1, 1}, {-1, 1, 1}, {1, -1, 1}}; + auto message = rviz_default_plugins::createPointCloud2WithPoints(message_points); + auto cloud_info = createCloudInfoWithSquare(scene_manager_, context_.get(), message); + + rviz_common::interaction::Picked picked_object(cloud_info->selection_handler_->getHandle()); + picked_object.extra_handles.clear(); + picked_object.extra_handles.insert(1); + picked_object.extra_handles.insert(4); + cloud_info->selection_handler_->onSelect(picked_object); + + rviz_common::properties::Property * parent = new rviz_common::properties::Property(); + cloud_info->selection_handler_->createProperties(picked_object, parent); + + ASSERT_THAT(parent, HasNumberOfSubproperties(2)); + + auto first_point_property = parent->childAt(0); + ASSERT_THAT(first_point_property, HasNumberOfSubproperties(1)); + EXPECT_THAT(first_point_property->getNameStd(), StartsWith("Point 0 [cloud")); + EXPECT_THAT(first_point_property->childAt(0), + HasPositionPropertyWithPosition(Ogre::Vector3(1, 1, 1))); + + auto second_point_property = parent->childAt(1); + ASSERT_THAT(second_point_property, HasNumberOfSubproperties(1)); + EXPECT_THAT(second_point_property->getNameStd(), StartsWith("Point 3 [cloud")); + EXPECT_THAT(second_point_property->childAt(0), + HasPositionPropertyWithPosition(Ogre::Vector3(1, -1, 1))); +} + +TEST_F(PointCloudSelectionHandlerFixture, + createProperties_creates_tree_of_properties_with_color_for_clouds_with_rgb) +{ + std::vector message_points = + {{1, 1, 1, 0, 0, 0}, + {-1, -1, 1, 50, 50, 50}, + {-1, 1, 1, 100, 100, 100}, + {1, -1, 1, 255, 255, 255}}; + auto message = rviz_default_plugins::create8BitColoredPointCloud2(message_points); + auto cloud_info = createCloudInfoWithSquare(scene_manager_, context_.get(), message); + + rviz_common::interaction::Picked picked_object(cloud_info->selection_handler_->getHandle()); + picked_object.extra_handles.clear(); + picked_object.extra_handles.insert(1); + picked_object.extra_handles.insert(4); + cloud_info->selection_handler_->onSelect(picked_object); + + rviz_common::properties::Property * parent = new rviz_common::properties::Property(); + cloud_info->selection_handler_->createProperties(picked_object, parent); + + ASSERT_THAT(parent, HasNumberOfSubproperties(2)); + EXPECT_THAT(parent->childAt(0), HasColorProperty("0; 0; 0", 0.0f)); + EXPECT_THAT(parent->childAt(1), HasColorProperty("1; 1; 1", 0.0f)); +} + +TEST_F(PointCloudSelectionHandlerFixture, + createProperties_creates_tree_of_properties_with_intensity_for_clouds_with_intensity) +{ + std::vector message_points = + {{1, 1, 1, 0}, {-1, -1, 1, 0.5f}, {-1, 1, 1, 0.5f}, {1, -1, 1, 1}}; + auto message = rviz_default_plugins::createPointCloud2WithIntensity(message_points); + Ogre::MaterialManager::getSingletonPtr()->load("RVIZ/Cyan", "rviz_rendering"); + auto cloud_info = createCloudInfoWithSquare(scene_manager_, context_.get(), message); + + rviz_common::interaction::Picked picked_object(cloud_info->selection_handler_->getHandle()); + picked_object.extra_handles.clear(); + picked_object.extra_handles.insert(1); + picked_object.extra_handles.insert(4); + cloud_info->selection_handler_->onSelect(picked_object); + + rviz_common::properties::Property * parent = new rviz_common::properties::Property(); + cloud_info->selection_handler_->createProperties(picked_object, parent); + + ASSERT_THAT(parent, HasNumberOfSubproperties(2)); + ASSERT_THAT(parent->childAt(0), HasIntensityProperty(0.0f)); + ASSERT_THAT(parent->childAt(1), HasIntensityProperty(1.0f)); +} diff --git a/rviz_default_plugins/test/rviz_default_plugins/displays/pose_array/pose_array_display_test.cpp b/rviz_default_plugins/test/rviz_default_plugins/displays/pose_array/pose_array_display_test.cpp index 45bdbf90a..ad6073276 100644 --- a/rviz_default_plugins/test/rviz_default_plugins/displays/pose_array/pose_array_display_test.cpp +++ b/rviz_default_plugins/test/rviz_default_plugins/displays/pose_array/pose_array_display_test.cpp @@ -66,10 +66,8 @@ using namespace ::testing; // NOLINT class PoseArrayDisplayFixture : public DisplayTestFixture { public: - void SetUp() override + PoseArrayDisplayFixture() { - DisplayTestFixture::SetUp(); - display_ = std::make_unique( context_.get(), scene_manager_->getRootSceneNode()->createChildSceneNode()); @@ -87,12 +85,6 @@ class PoseArrayDisplayFixture : public DisplayTestFixture } } - void TearDown() override - { - display_.reset(); - DisplayTestFixture::TearDown(); - } - std::unique_ptr display_; rviz_common::properties::Property * arrow_2d_length_property_; std::vector common_arrow_properties_; diff --git a/rviz_default_plugins/test/rviz_default_plugins/mock_display_context.hpp b/rviz_default_plugins/test/rviz_default_plugins/mock_display_context.hpp index 872ca2310..0adcb49d0 100644 --- a/rviz_default_plugins/test/rviz_default_plugins/mock_display_context.hpp +++ b/rviz_default_plugins/test/rviz_default_plugins/mock_display_context.hpp @@ -39,13 +39,19 @@ #include "rviz_common/panel_dock_widget.hpp" #include "rviz_common/viewport_mouse_event.hpp" #include "rviz_common/window_manager_interface.hpp" +#include "rviz_common/interaction/handler_manager_iface.hpp" +#include "rviz_common/interaction/selection_manager_iface.hpp" class MockDisplayContext : public rviz_common::DisplayContext { public: MOCK_CONST_METHOD0(getSceneManager, Ogre::SceneManager * ()); MOCK_CONST_METHOD0(getWindowManager, rviz_common::WindowManagerInterface * ()); - MOCK_CONST_METHOD0(getSelectionManager, rviz_common::selection::SelectionManagerIface * ()); + MOCK_CONST_METHOD0( + getHandlerManager, std::shared_ptr()); + MOCK_CONST_METHOD0( + getSelectionManager, std::shared_ptr()); + MOCK_CONST_METHOD0(getViewPicker, std::shared_ptr()); MOCK_CONST_METHOD0(getFrameManager, rviz_common::FrameManagerIface * ()); MOCK_CONST_METHOD0(getFixedFrame, QString()); MOCK_CONST_METHOD0(getFrameCount, uint64_t()); @@ -68,6 +74,9 @@ class MockDisplayContext : public rviz_common::DisplayContext MOCK_METHOD0(getClock, std::shared_ptr()); MOCK_METHOD0(queueRender, void()); + + MOCK_METHOD0(lockRender, void()); + MOCK_METHOD0(unlockRender, void()); }; #endif // RVIZ_DEFAULT_PLUGINS__MOCK_DISPLAY_CONTEXT_HPP_ diff --git a/rviz_default_plugins/test/rviz_default_plugins/mock_handler_manager.hpp b/rviz_default_plugins/test/rviz_default_plugins/mock_handler_manager.hpp new file mode 100644 index 000000000..92b0bf5a8 --- /dev/null +++ b/rviz_default_plugins/test/rviz_default_plugins/mock_handler_manager.hpp @@ -0,0 +1,68 @@ +/* + * Copyright (c) 2018, Bosch Software Innovations GmbH. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted (subject to the limitations in the disclaimer + * below) 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 copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. 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 HOLDER 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. + */ + +#ifndef RVIZ_DEFAULT_PLUGINS__MOCK_HANDLER_MANAGER_HPP_ +#define RVIZ_DEFAULT_PLUGINS__MOCK_HANDLER_MANAGER_HPP_ + +#include +#include + +#include +#include +#include +#include + +#include "rviz_common/interaction/forwards.hpp" +#include "rviz_common/interaction/handler_manager_iface.hpp" +#include "rviz_common/interaction/selection_handler.hpp" + +class MockHandlerManager : public rviz_common::interaction::HandlerManagerIface +{ +public: + MOCK_METHOD0(createHandle, rviz_common::interaction::CollObjectHandle()); + + MOCK_METHOD2(addHandler, void(rviz_common::interaction::CollObjectHandle, + rviz_common::interaction::SelectionHandlerWeakPtr)); + MOCK_METHOD1(removeHandler, void(rviz_common::interaction::CollObjectHandle)); + MOCK_METHOD1(addListener, void(rviz_common::interaction::HandlerManagerListener *)); + MOCK_METHOD1(removeListener, void(rviz_common::interaction::HandlerManagerListener *)); + MOCK_METHOD1(getHandler, + rviz_common::interaction::SelectionHandlerPtr(rviz_common::interaction::CollObjectHandle)); + + MOCK_METHOD1(enableInteraction, void(bool)); + MOCK_CONST_METHOD0(getInteractionEnabled, bool()); + MOCK_METHOD0(handlers, rviz_common::interaction::HandlerRange()); + + MOCK_METHOD0(lock, std::unique_lock()); + MOCK_METHOD1(lock, std::unique_lock(std::defer_lock_t)); +}; + +#endif // RVIZ_DEFAULT_PLUGINS__MOCK_HANDLER_MANAGER_HPP_ diff --git a/rviz_default_plugins/test/rviz_default_plugins/mock_selection_manager.hpp b/rviz_default_plugins/test/rviz_default_plugins/mock_selection_manager.hpp index 406756d79..c47d624c8 100644 --- a/rviz_default_plugins/test/rviz_default_plugins/mock_selection_manager.hpp +++ b/rviz_default_plugins/test/rviz_default_plugins/mock_selection_manager.hpp @@ -39,54 +39,30 @@ #include #include -#include "rviz_common/selection/forwards.hpp" -#include "rviz_common/selection/selection_manager_iface.hpp" +#include "rviz_common/interaction/forwards.hpp" +#include "rviz_common/interaction/selection_manager_iface.hpp" -using rviz_common::selection::CollObjectHandle; -using rviz_common::selection::M_Picked; -using rviz_common::selection::SelectionHandler; +using rviz_common::interaction::CollObjectHandle; +using rviz_common::interaction::M_Picked; +using rviz_common::interaction::SelectionHandler; -class MockSelectionManager : public rviz_common::selection::SelectionManagerIface +class MockSelectionManager : public rviz_common::interaction::SelectionManagerIface { public: - MOCK_METHOD0(createHandle, rviz_common::selection::CollObjectHandle()); - MOCK_METHOD3(renderQueueStarted, void(uint8_t, const std::string &, bool &)); - MOCK_METHOD0(initialize, void()); MOCK_METHOD1(setDebugMode, void(bool)); - MOCK_METHOD0(clearHandlers, void()); - MOCK_METHOD2(addObject, void(CollObjectHandle, SelectionHandler *)); - MOCK_METHOD1(removeObject, void(rviz_common::selection::CollObjectHandle)); - MOCK_METHOD5(highlight, void(Ogre::Viewport *, int, int, int, int)); + MOCK_METHOD5(highlight, void(rviz_rendering::RenderWindow *, int, int, int, int)); MOCK_METHOD0(removeHighlight, void()); - MOCK_METHOD6(select, void(Ogre::Viewport *, int, int, int, int, SelectType)); - MOCK_METHOD7(pick, void(Ogre::Viewport *, int, int, int, int, M_Picked &, bool)); + MOCK_METHOD6(select, void(rviz_rendering::RenderWindow *, int, int, int, int, SelectType)); MOCK_METHOD0(update, void()); - MOCK_METHOD1(setSelection, void(const M_Picked &)); - MOCK_METHOD1(addSelection, void(const M_Picked &)); - MOCK_METHOD1(removeSelection, void(const M_Picked &)); MOCK_CONST_METHOD0(getSelection, const M_Picked &()); - MOCK_METHOD1(getHandler, SelectionHandler * (CollObjectHandle)); - MOCK_METHOD5(handleSchemeNotFound, Ogre::Technique *( - unsigned short int, // NOLINT: Ogre decides the use of unsigned short - const Ogre::String &, - Ogre::Material *, - unsigned short int, // NOLINT: Ogre decides the use of unsigned short - const Ogre::Renderable *)); - - MOCK_METHOD1(enableInteraction, void(bool)); - MOCK_CONST_METHOD0(getInteractionEnabled, bool()); MOCK_METHOD0(focusOnSelection, void()); MOCK_METHOD1(setTextureSize, void(unsigned int)); MOCK_METHOD4(get3DPoint, bool(Ogre::Viewport *, int, int, Ogre::Vector3 &)); - MOCK_METHOD7(get3DPatch, bool(Ogre::Viewport *, int, int, unsigned int, unsigned int, bool, - std::vector&)); - MOCK_METHOD6(getPatchDepthImage, bool(Ogre::Viewport *, int, int, unsigned int, unsigned int, - std::vector>&)); MOCK_METHOD0(getPropertyModel, rviz_common::properties::PropertyTreeModel *()); }; diff --git a/rviz_default_plugins/test/rviz_default_plugins/robot/robot_test.cpp b/rviz_default_plugins/test/rviz_default_plugins/robot/robot_test.cpp index 606ea6805..056285640 100644 --- a/rviz_default_plugins/test/rviz_default_plugins/robot/robot_test.cpp +++ b/rviz_default_plugins/test/rviz_default_plugins/robot/robot_test.cpp @@ -46,6 +46,7 @@ #include "test/rviz_rendering/ogre_testing_environment.hpp" #include "./mock_link_updater.hpp" #include "../mock_display_context.hpp" +#include "../mock_handler_manager.hpp" #include "../mock_selection_manager.hpp" #include "../scene_graph_introspection.hpp" @@ -72,13 +73,16 @@ class RobotTestFixture : public Test rviz_rendering::MaterialManager::createDefaultColorMaterials(); } - void SetUp() override + + RobotTestFixture() { selection_manager_ = std::make_shared>(); + handle_manager_ = std::make_unique>(); context_ = std::make_shared>(); EXPECT_CALL(*context_, getSceneManager()).WillRepeatedly(Return(scene_manager_)); - EXPECT_CALL(*context_, getSelectionManager()).WillRepeatedly(Return(selection_manager_.get())); + EXPECT_CALL(*context_, getSelectionManager()).WillRepeatedly(Return(selection_manager_)); + EXPECT_CALL(*context_, getHandlerManager()).WillRepeatedly(Return(handle_manager_)); resource_retriever::Retriever retriever; auto file = retriever.get("package://rviz_rendering_tests/test_meshes/test.urdf"); @@ -99,6 +103,7 @@ class RobotTestFixture : public Test static Ogre::SceneManager * scene_manager_; urdf::Model urdf_model_; + std::shared_ptr handle_manager_; std::shared_ptr context_; std::shared_ptr selection_manager_; diff --git a/rviz_default_plugins/test/rviz_default_plugins/tools/select/selection_tool_test.cpp b/rviz_default_plugins/test/rviz_default_plugins/tools/select/selection_tool_test.cpp new file mode 100644 index 000000000..ca610a22e --- /dev/null +++ b/rviz_default_plugins/test/rviz_default_plugins/tools/select/selection_tool_test.cpp @@ -0,0 +1,175 @@ +/* + * Copyright (c) 2018, Bosch Software Innovations GmbH. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted (subject to the limitations in the disclaimer + * below) 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 copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. 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 HOLDER 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. + */ + +#include +#include + +#include + +#include // NOLINT cpplint cannot handle include order +#include // NOLINT cpplint cannot handle include order + +#include "rviz_common/render_panel.hpp" +#include "rviz_common/tool.hpp" + +#include "../../../../src/rviz_default_plugins/tools/select/selection_tool.hpp" + +#include "../../mock_display_context.hpp" +#include "../../mock_selection_manager.hpp" + +using namespace ::testing; // NOLINT + +class SelectionToolTestFixture : public Test +{ +public: + static void SetUpTestCase() + { + render_panel_ = new rviz_common::RenderPanel(nullptr); + } + + SelectionToolTestFixture() + { + context_ = std::make_shared>(); + selection_manager_ = std::make_shared>(); + + EXPECT_CALL(*context_, getSelectionManager()).WillRepeatedly(Return(selection_manager_)); + + selection_tool_ = std::make_shared(); + selection_tool_->initialize(context_.get()); + } + + static void TearDownTestCase() + { + delete render_panel_; + render_panel_ = nullptr; + } + + rviz_common::ViewportMouseEvent generateMouseMoveEvent(int x, int y) + { + return generateMouseEvent(x, y, QMouseEvent::MouseMove, Qt::NoButton, Qt::NoModifier); + } + + rviz_common::ViewportMouseEvent generateMousePressEvent(int x, int y) + { + return generateMouseEvent(x, y, QMouseEvent::MouseButtonPress, Qt::LeftButton, Qt::NoModifier); + } + + rviz_common::ViewportMouseEvent generateMouseReleaseEvent( + int x, int y, Qt::KeyboardModifiers modifiers = Qt::NoModifier) + { + return generateMouseEvent(x, y, QMouseEvent::MouseButtonRelease, Qt::LeftButton, modifiers); + } + + rviz_common::ViewportMouseEvent generateMouseEvent( + int x, int y, QMouseEvent::Type type, Qt::MouseButton button, Qt::KeyboardModifiers modifiers) + { + auto mouseEvent = new QMouseEvent(type, QPointF(x, y), button, button, modifiers); + return {render_panel_, mouseEvent, x, y}; + } + + std::shared_ptr context_; + std::shared_ptr selection_manager_; + + std::shared_ptr selection_tool_; + + static rviz_common::RenderPanel * render_panel_; +}; + +rviz_common::RenderPanel * SelectionToolTestFixture::render_panel_ = nullptr; + +TEST_F(SelectionToolTestFixture, processMouseEvent_does_not_render_on_mouse_move) { + auto event = generateMouseMoveEvent(10, 20); + auto result = selection_tool_->processMouseEvent(event); + + EXPECT_THAT(result, Eq(0)); +} + +TEST_F(SelectionToolTestFixture, processMouseEvent_starts_highlighting_on_left_mouse_down) { + auto event = generateMousePressEvent(10, 20); + EXPECT_CALL(*selection_manager_, highlight(_, event.x, event.y, event.x, event.y)); + + auto result = selection_tool_->processMouseEvent(event); + + EXPECT_THAT(result, Eq(rviz_common::Tool::Render)); +} + +TEST_F(SelectionToolTestFixture, processMouseEvent_replaces_selection_on_mouse_release) { + auto click_event = generateMousePressEvent(10, 20); + selection_tool_->processMouseEvent(click_event); + + auto release_event = generateMouseReleaseEvent(100, 200); + EXPECT_CALL(*selection_manager_, select( + _, + click_event.x, click_event.y, + release_event.x, release_event.y, + rviz_common::interaction::SelectionManagerIface::Replace)); + selection_tool_->processMouseEvent(release_event); +} + +TEST_F(SelectionToolTestFixture, processMouseEvent_adds_to_selection_when_holding_shift) { + auto click_event = generateMousePressEvent(10, 20); + selection_tool_->processMouseEvent(click_event); + + auto release_event = generateMouseReleaseEvent(100, 200, Qt::ShiftModifier); + EXPECT_CALL(*selection_manager_, select( + _, + click_event.x, click_event.y, + release_event.x, release_event.y, + rviz_common::interaction::SelectionManagerIface::Add)); + selection_tool_->processMouseEvent(release_event); +} + +TEST_F(SelectionToolTestFixture, processMouseEvent_removes_from_selection_when_holding_ctrl) { + auto click_event = generateMousePressEvent(10, 20); + selection_tool_->processMouseEvent(click_event); + + auto release_event = generateMouseReleaseEvent(100, 200, Qt::ControlModifier); + EXPECT_CALL(*selection_manager_, select( + _, + click_event.x, click_event.y, + release_event.x, release_event.y, + rviz_common::interaction::SelectionManagerIface::Remove)); + selection_tool_->processMouseEvent(release_event); +} + +TEST_F(SelectionToolTestFixture, processKeyEvent_F_key_should_focus_on_selection) { + EXPECT_CALL(*selection_manager_, focusOnSelection()).Times(1); + + QKeyEvent * keyEvent = new QKeyEvent(QKeyEvent::KeyPress, Qt::Key_F, Qt::NoModifier); + selection_tool_->processKeyEvent(keyEvent, nullptr); +} + +int main(int argc, char ** argv) +{ + QApplication app(argc, argv); + InitGoogleMock(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/rviz_rendering/src/rviz_rendering/render_window.cpp b/rviz_rendering/src/rviz_rendering/render_window.cpp index 52c0b9e4c..7af119141 100644 --- a/rviz_rendering/src/rviz_rendering/render_window.cpp +++ b/rviz_rendering/src/rviz_rendering/render_window.cpp @@ -241,7 +241,7 @@ RenderWindowOgreAdapter::getOgreCamera(RenderWindow * render_window) Ogre::Viewport * RenderWindowOgreAdapter::getOgreViewport(RenderWindow * render_window) { - return render_window->impl_->getViewport(); + return render_window ? render_window->impl_->getViewport() : nullptr; } void diff --git a/rviz_rendering/test/point_cloud_renderable_test.cpp b/rviz_rendering/test/point_cloud_renderable_test.cpp index f56b5d664..ee53c6cc4 100644 --- a/rviz_rendering/test/point_cloud_renderable_test.cpp +++ b/rviz_rendering/test/point_cloud_renderable_test.cpp @@ -60,7 +60,7 @@ class PointCloudRenderableTestFixture : public ::testing::Test testing_environment_->setUpOgreTestEnvironment(); } - void SetUp() override + PointCloudRenderableTestFixture() { cloud_ = std::make_shared(); auto points = std::vector(