diff --git a/rviz_common/include/rviz_common/display.hpp b/rviz_common/include/rviz_common/display.hpp index 73a45c6f8..0e84dd8e3 100644 --- a/rviz_common/include/rviz_common/display.hpp +++ b/rviz_common/include/rviz_common/display.hpp @@ -303,6 +303,10 @@ class RVIZ_COMMON_PUBLIC Display : public rviz_common::properties::BoolProperty /// Get transform to the frame at the given time and update the scene node. True on success. bool updateFrame(const std::string & frame, rclcpp::Time time); + virtual + bool + allow_visibility() const; + Q_SIGNALS: void timeSignal(rviz_common::Display * display, rclcpp::Time time); diff --git a/rviz_common/include/rviz_common/frame_position_tracking_view_controller.hpp b/rviz_common/include/rviz_common/frame_position_tracking_view_controller.hpp index 1f3797298..9ea5cdd57 100644 --- a/rviz_common/include/rviz_common/frame_position_tracking_view_controller.hpp +++ b/rviz_common/include/rviz_common/frame_position_tracking_view_controller.hpp @@ -37,6 +37,7 @@ #include "rviz_common/view_controller.hpp" #include "rviz_common/visibility_control.hpp" +#include "rviz_common/properties/property.hpp" namespace rviz_common { @@ -56,6 +57,7 @@ class RVIZ_COMMON_PUBLIC FramePositionTrackingViewController : public ViewContro public: FramePositionTrackingViewController(); + FramePositionTrackingViewController(properties::Property * parent); ~FramePositionTrackingViewController() override; diff --git a/rviz_common/include/rviz_common/view_controller.hpp b/rviz_common/include/rviz_common/view_controller.hpp index f0d049b78..d0398bcb7 100644 --- a/rviz_common/include/rviz_common/view_controller.hpp +++ b/rviz_common/include/rviz_common/view_controller.hpp @@ -97,6 +97,7 @@ class RVIZ_COMMON_PUBLIC ViewController : public properties::Property public: ViewController(); + ViewController(properties::Property * parent); ~ViewController() override; /// Do all setup that can't be done in the constructor. @@ -271,6 +272,8 @@ private Q_SLOTS: bool is_active_; + properties::Property * parent_; + // this cursor will be displayed when the mouse is within the // window controlled by this view controller // use SetCursor to modify. diff --git a/rviz_common/include/rviz_common/visualization_manager.hpp b/rviz_common/include/rviz_common/visualization_manager.hpp index 54a16cfbc..433eab9c2 100644 --- a/rviz_common/include/rviz_common/visualization_manager.hpp +++ b/rviz_common/include/rviz_common/visualization_manager.hpp @@ -68,7 +68,7 @@ class Property; class PropertyTreeModel; class StatusList; class TfFrameProperty; - +class DisplayGroupVisibilityProperty; } // namespace properties class Display; @@ -294,6 +294,9 @@ class RVIZ_COMMON_PUBLIC VisualizationManager : public DisplayContext rclcpp::Clock::SharedPtr getClock() override; + int addRenderPanel(RenderPanel* rp); + void updateRenderMask(std::size_t id, bool mask); + public Q_SLOTS: /// Resets the wall and ROS elapsed time to zero and calls resetDisplays(). void resetTime(); @@ -360,6 +363,7 @@ protected Q_SLOTS: properties::TfFrameProperty * fixed_frame_property_; properties::StatusList * global_status_; properties::IntProperty * fps_property_; + properties::DisplayGroupVisibilityProperty * visibility_property_; RenderPanel * render_panel_; @@ -390,9 +394,13 @@ protected Q_SLOTS: rclcpp::Clock::SharedPtr clock_; rclcpp::JumpHandler::SharedPtr clock_jump_handler_; + std::vector render_panel_list_; + std::vector render_panel_render_mask_; + private Q_SLOTS: void updateFixedFrame(); void updateBackgroundColor(); + void updateVisibilityMask(); void updateFps(); private: diff --git a/rviz_common/src/rviz_common/display.cpp b/rviz_common/src/rviz_common/display.cpp index e87ee41ca..cbb1cc875 100644 --- a/rviz_common/src/rviz_common/display.cpp +++ b/rviz_common/src/rviz_common/display.cpp @@ -390,6 +390,11 @@ void Display::setVisibilityBits(uint32_t bits) rviz_rendering::applyVisibilityBits(visibility_bits_, scene_node_); } +bool Display::allow_visibility() const +{ + return true; +} + void Display::unsetVisibilityBits(uint32_t bits) { visibility_bits_ &= ~bits; diff --git a/rviz_common/src/rviz_common/frame_position_tracking_view_controller.cpp b/rviz_common/src/rviz_common/frame_position_tracking_view_controller.cpp index d80ee76a2..a4c705140 100644 --- a/rviz_common/src/rviz_common/frame_position_tracking_view_controller.cpp +++ b/rviz_common/src/rviz_common/frame_position_tracking_view_controller.cpp @@ -46,8 +46,9 @@ namespace rviz_common { -FramePositionTrackingViewController::FramePositionTrackingViewController() -: target_scene_node_(nullptr), +FramePositionTrackingViewController::FramePositionTrackingViewController(properties::Property * parent) +: rviz_common::ViewController(parent), + target_scene_node_(nullptr), reference_orientation_(Ogre::Quaternion::IDENTITY), reference_position_(Ogre::Vector3::ZERO), camera_scene_node_(nullptr) @@ -56,11 +57,17 @@ FramePositionTrackingViewController::FramePositionTrackingViewController() "Target Frame", rviz_common::properties::TfFrameProperty::FIXED_FRAME_STRING, "TF frame whose motion this view will follow.", - this, + parent_, nullptr, true); } +FramePositionTrackingViewController::FramePositionTrackingViewController() + : FramePositionTrackingViewController(nullptr) +{ + +} + void FramePositionTrackingViewController::onInitialize() { target_frame_property_->setFrameManager(context_->getFrameManager()); diff --git a/rviz_common/src/rviz_common/properties/display_group_visibility_property.cpp b/rviz_common/src/rviz_common/properties/display_group_visibility_property.cpp index 90dd31b85..b3e77e8af 100644 --- a/rviz_common/src/rviz_common/properties/display_group_visibility_property.cpp +++ b/rviz_common/src/rviz_common/properties/display_group_visibility_property.cpp @@ -115,9 +115,13 @@ void DisplayGroupVisibilityProperty::onDisplayAdded(Display * display) vis_bit_, display_group, parent_display_, "", true, "Uncheck to hide everything in this Display Group", this); } else { - vis_prop = new DisplayVisibilityProperty( - vis_bit_, display, "", true, - "Show or hide this Display", this); + if (display->allow_visibility()) { + vis_prop = new DisplayVisibilityProperty( + vis_bit_, display, "", true, + "Show or hide this Display", this); + } else { + return; + } } disp_vis_props_[display] = vis_prop; sortDisplayList(); diff --git a/rviz_common/src/rviz_common/view_controller.cpp b/rviz_common/src/rviz_common/view_controller.cpp index 28e4b6626..ded609568 100644 --- a/rviz_common/src/rviz_common/view_controller.cpp +++ b/rviz_common/src/rviz_common/view_controller.cpp @@ -57,16 +57,21 @@ namespace rviz_common using properties::BoolProperty; using properties::FloatProperty; -ViewController::ViewController() +ViewController::ViewController(properties::Property * parent) : context_(nullptr), camera_(nullptr), is_active_(false), + parent_(parent), type_property_(nullptr) { + if (parent_ == nullptr) + { + parent = this; + } near_clip_property_ = new FloatProperty( "Near Clip Distance", 0.01f, "Anything closer to the camera than this threshold will not get rendered.", - this, SLOT(updateNearClipDistance())); + parent, SLOT(updateNearClipDistance()), this); near_clip_property_->setMin(0.001f); near_clip_property_->setMax(10000); @@ -75,7 +80,7 @@ ViewController::ViewController() "Render the main view in stereo if supported." " On Linux this requires a recent version of Ogre and" " an NVIDIA Quadro card with 3DVision glasses.", - this, SLOT(updateStereoProperties())); + parent, SLOT(updateStereoProperties()), this); stereo_eye_swap_ = new BoolProperty( "Swap Stereo Eyes", false, "Swap eyes if the monitor shows the left eye on the right.", @@ -91,7 +96,12 @@ ViewController::ViewController() invert_z_ = new BoolProperty( "Invert Z Axis", false, "Invert camera's Z axis for Z-down environments/models.", - this, SLOT(updateStereoProperties())); + parent, SLOT(updateStereoProperties()), this); +} + +ViewController::ViewController() + : ViewController(nullptr) +{ } void ViewController::initialize(DisplayContext * context) diff --git a/rviz_common/src/rviz_common/visualization_manager.cpp b/rviz_common/src/rviz_common/visualization_manager.cpp index d021a78dd..996586ab8 100644 --- a/rviz_common/src/rviz_common/visualization_manager.cpp +++ b/rviz_common/src/rviz_common/visualization_manager.cpp @@ -69,6 +69,7 @@ #include "frame_manager.hpp" #include "rviz_common/load_resource.hpp" #include "rviz_common/properties/color_property.hpp" +#include "rviz_common/properties/display_group_visibility_property.hpp" #include "rviz_common/properties/int_property.hpp" #include "rviz_common/properties/parse_color.hpp" #include "rviz_common/properties/property.hpp" @@ -199,6 +200,17 @@ VisualizationManager::VisualizationManager( "RViz will try to render this many frames per second.", global_options_, SLOT(updateFps()), this); + rviz_rendering::RenderWindowOgreAdapter::setVisibilityMask( + getRenderPanel()->getRenderWindow(), + default_visibility_bit_); + + visibility_property_ = new rviz_common::properties::DisplayGroupVisibilityProperty( + default_visibility_bit_, getRootDisplayGroup(), nullptr, "Visibility", true, + "Changes the visibility of other Displays in the camera view.", global_options_, + SLOT(updateVisibilityMask()), this); + visibility_property_->setIcon( + rviz_common::loadPixmap("package://rviz_default_plugins/icons/visibility.svg", true)); + root_display_group_->initialize(this); // only initialize() a Display // after its sub-properties are created. root_display_group_->setEnabled(true); @@ -354,6 +366,8 @@ void VisualizationManager::onUpdate() Q_EMIT preUpdate(); + updateVisibilityMask(); + frame_manager_->update(); root_display_group_->update(wall_dt, ros_dt); @@ -396,6 +410,16 @@ void VisualizationManager::onUpdate() render_requested_ = 0; std::lock_guard lock(private_->render_mutex_); ogre_root_->renderOneFrame(); + + for (unsigned int i = 0; i < render_panel_list_.size(); i++) { + if(render_panel_render_mask_[i]) { + auto viewport = + rviz_rendering::RenderWindowOgreAdapter::getOgreViewport(render_panel_list_[i]->getRenderWindow()); + if (viewport) { + viewport->update(); + } + } + } } } @@ -413,6 +437,20 @@ void VisualizationManager::onTimeJump(const rcl_time_jump_t & jump) } } +int VisualizationManager::addRenderPanel( RenderPanel* rp ) +{ + render_panel_list_.push_back(rp); + render_panel_render_mask_.push_back(true); + return render_panel_list_.size()-1; +} + +void VisualizationManager::updateRenderMask(std::size_t id, bool mask) +{ + if(id < render_panel_render_mask_.size()) { + render_panel_render_mask_[id] = mask; + } +} + void VisualizationManager::updateTime() { rclcpp::Clock clock; // TODO(wjwwood): replace with clock attached to node for ROS Time @@ -581,6 +619,12 @@ double VisualizationManager::getROSTimeElapsed() return static_cast(ros_time_elapsed_) / 1e9; } +void VisualizationManager::updateVisibilityMask() +{ + visibility_property_->update(); + queueRender(); +} + void VisualizationManager::updateBackgroundColor() { using rviz_rendering::RenderWindowOgreAdapter; diff --git a/rviz_default_plugins/include/rviz_default_plugins/displays/camera/camera_display.hpp b/rviz_default_plugins/include/rviz_default_plugins/displays/camera/camera_display.hpp index ab97b62d0..194af69df 100644 --- a/rviz_default_plugins/include/rviz_default_plugins/displays/camera/camera_display.hpp +++ b/rviz_default_plugins/include/rviz_default_plugins/displays/camera/camera_display.hpp @@ -138,7 +138,9 @@ class RVIZ_DEFAULT_PLUGINS_PUBLIC CameraDisplay void processMessage(sensor_msgs::msg::Image::ConstSharedPtr msg) override; -private Q_SLOTS: + bool allow_visibility() const override; + + private Q_SLOTS: void updateAlpha(); private: diff --git a/rviz_default_plugins/include/rviz_default_plugins/displays/image/image_display.hpp b/rviz_default_plugins/include/rviz_default_plugins/displays/image/image_display.hpp index efdec89e7..2b6f24ce3 100644 --- a/rviz_default_plugins/include/rviz_default_plugins/displays/image/image_display.hpp +++ b/rviz_default_plugins/include/rviz_default_plugins/displays/image/image_display.hpp @@ -96,6 +96,8 @@ public Q_SLOTS: /* This is called by incomingMessage(). */ void processMessage(sensor_msgs::msg::Image::ConstSharedPtr msg) override; + bool allow_visibility() const override; + private: void setupScreenRectangle(); void setupRenderPanel(); diff --git a/rviz_default_plugins/include/rviz_default_plugins/view_controllers/orbit/orbit_view_controller.hpp b/rviz_default_plugins/include/rviz_default_plugins/view_controllers/orbit/orbit_view_controller.hpp index bb45225d8..58979d694 100644 --- a/rviz_default_plugins/include/rviz_default_plugins/view_controllers/orbit/orbit_view_controller.hpp +++ b/rviz_default_plugins/include/rviz_default_plugins/view_controllers/orbit/orbit_view_controller.hpp @@ -42,6 +42,7 @@ #include "rviz_common/frame_position_tracking_view_controller.hpp" #include "rviz_common/properties/bool_property.hpp" #include "rviz_common/properties/float_property.hpp" +#include "rviz_common/properties/property.hpp" #include "rviz_common/properties/vector_property.hpp" #include "rviz_rendering/objects/shape.hpp" #include "rviz_default_plugins/visibility_control.hpp" @@ -68,6 +69,8 @@ class RVIZ_DEFAULT_PLUGINS_PUBLIC OrbitViewController : public public: OrbitViewController(); + OrbitViewController(rviz_common::properties::Property * parent); + ~OrbitViewController() override; /// Do subclass-specific initialization. diff --git a/rviz_default_plugins/include/rviz_default_plugins/view_controllers/ortho/fixed_orientation_ortho_view_controller.hpp b/rviz_default_plugins/include/rviz_default_plugins/view_controllers/ortho/fixed_orientation_ortho_view_controller.hpp index 281876899..a35ab9629 100644 --- a/rviz_default_plugins/include/rviz_default_plugins/view_controllers/ortho/fixed_orientation_ortho_view_controller.hpp +++ b/rviz_default_plugins/include/rviz_default_plugins/view_controllers/ortho/fixed_orientation_ortho_view_controller.hpp @@ -36,6 +36,7 @@ #include #include "rviz_common/frame_position_tracking_view_controller.hpp" +#include "rviz_common/properties/property.hpp" #include "rviz_default_plugins/visibility_control.hpp" namespace rviz_rendering @@ -69,6 +70,8 @@ class RVIZ_DEFAULT_PLUGINS_PUBLIC FixedOrientationOrthoViewController : public public: FixedOrientationOrthoViewController(); + FixedOrientationOrthoViewController(rviz_common::properties::Property * parent); + ~FixedOrientationOrthoViewController() override = default; void onInitialize() override; diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/camera/camera_display.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/camera/camera_display.cpp index c15621460..ae4788120 100644 --- a/rviz_default_plugins/src/rviz_default_plugins/displays/camera/camera_display.cpp +++ b/rviz_default_plugins/src/rviz_default_plugins/displays/camera/camera_display.cpp @@ -221,6 +221,11 @@ void CameraDisplay::setupSceneNodes() updateAlpha(); } +bool CameraDisplay::allow_visibility() const +{ + return false; +} + void CameraDisplay::setupRenderPanel() { render_panel_ = std::make_unique(); diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/image/image_display.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/image/image_display.cpp index 489b1fdf2..b53169c9d 100644 --- a/rviz_default_plugins/src/rviz_default_plugins/displays/image/image_display.cpp +++ b/rviz_default_plugins/src/rviz_default_plugins/displays/image/image_display.cpp @@ -157,6 +157,11 @@ void ImageDisplay::clear() texture_->clear(); } +bool ImageDisplay::allow_visibility() const +{ + return false; +} + void ImageDisplay::update(std::chrono::nanoseconds wall_dt, std::chrono::nanoseconds ros_dt) { (void) wall_dt; 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 29f58c52e..ded389e38 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 @@ -345,6 +345,7 @@ void PointCloudCommon::insertNewClouds(float point_decay_time, const rclcpp::Tim cloud_info->position_, cloud_info->orientation_); + cloud_info->cloud_->setVisibilityFlags(display_->getVisibilityBits()); cloud_info->scene_node_->attachObject(cloud_info->cloud_.get()); cloud_info->setSelectable(selectable_property_->getBool(), getSelectionBoxSize(), context_); diff --git a/rviz_default_plugins/src/rviz_default_plugins/view_controllers/orbit/orbit_view_controller.cpp b/rviz_default_plugins/src/rviz_default_plugins/view_controllers/orbit/orbit_view_controller.cpp index 50645577d..cc2e77b74 100644 --- a/rviz_default_plugins/src/rviz_default_plugins/view_controllers/orbit/orbit_view_controller.cpp +++ b/rviz_default_plugins/src/rviz_default_plugins/view_controllers/orbit/orbit_view_controller.cpp @@ -66,8 +66,10 @@ using rviz_common::properties::FloatProperty; using rviz_common::properties::VectorProperty; using rviz_rendering::Shape; -OrbitViewController::OrbitViewController() -: focal_shape_(nullptr), dragging_(false) +OrbitViewController::OrbitViewController(rviz_common::properties::Property * parent) +: rviz_common::FramePositionTrackingViewController(parent), + focal_shape_(nullptr), + dragging_(false) { distance_property_ = new FloatProperty( "Distance", DISTANCE_START, @@ -98,6 +100,11 @@ OrbitViewController::OrbitViewController() "The center point which the camera orbits.", this); } +OrbitViewController::OrbitViewController() +: OrbitViewController(nullptr) +{ +} + void OrbitViewController::onInitialize() { rviz_common::FramePositionTrackingViewController::onInitialize(); diff --git a/rviz_default_plugins/src/rviz_default_plugins/view_controllers/ortho/fixed_orientation_ortho_view_controller.cpp b/rviz_default_plugins/src/rviz_default_plugins/view_controllers/ortho/fixed_orientation_ortho_view_controller.cpp index 4db913ec8..3c3d51a66 100644 --- a/rviz_default_plugins/src/rviz_default_plugins/view_controllers/ortho/fixed_orientation_ortho_view_controller.cpp +++ b/rviz_default_plugins/src/rviz_default_plugins/view_controllers/ortho/fixed_orientation_ortho_view_controller.cpp @@ -51,17 +51,22 @@ namespace view_controllers static const float ORTHO_VIEW_CONTROLLER_CAMERA_Z = 500; -FixedOrientationOrthoViewController::FixedOrientationOrthoViewController() -: dragging_(false) +FixedOrientationOrthoViewController::FixedOrientationOrthoViewController(rviz_common::properties::Property * parent) +: rviz_common::FramePositionTrackingViewController(parent), + dragging_(false) { scale_property_ = new rviz_common::properties::FloatProperty( - "Scale", 10, "How much to scale up the size of things in the scene.", this); + "Scale", 10, "How much to scale up the size of things in the scene.", parent_); angle_property_ = new rviz_common::properties::FloatProperty( - "Angle", 0, "Angle around the Z axis to rotate.", this); + "Angle", 0, "Angle around the Z axis to rotate.", parent_); x_property_ = new rviz_common::properties::FloatProperty( - "X", 0, "X component of camera position.", this); + "X", 0, "X component of camera position.", parent_); y_property_ = new rviz_common::properties::FloatProperty( - "Y", 0, "Y component of camera position.", this); + "Y", 0, "Y component of camera position.", parent_); +} +FixedOrientationOrthoViewController::FixedOrientationOrthoViewController() + : FixedOrientationOrthoViewController(nullptr) +{ } void FixedOrientationOrthoViewController::onInitialize() @@ -188,6 +193,9 @@ void FixedOrientationOrthoViewController::updateCamera() { orientCamera(); + if (!camera_->getViewport()) + return; + float width = camera_->getViewport()->getActualWidth(); float height = camera_->getViewport()->getActualHeight();