diff --git a/cartographer_rviz/cartographer_rviz/drawable_submap.cc b/cartographer_rviz/cartographer_rviz/drawable_submap.cc index 7b0398c..b5ea213 100644 --- a/cartographer_rviz/cartographer_rviz/drawable_submap.cc +++ b/cartographer_rviz/cartographer_rviz/drawable_submap.cc @@ -45,6 +45,7 @@ constexpr float kAlphaUpdateThreshold = 0.2f; const Ogre::ColourValue kSubmapIdColor(Ogre::ColourValue::Red); const Eigen::Vector3d kSubmapIdPosition(0.0, 0.0, 0.3); constexpr float kSubmapIdCharHeight = 0.2f; +constexpr int kNumberOfSlicesPerSubmap = 2; } // namespace @@ -58,7 +59,6 @@ DrawableSubmap::DrawableSubmap(const ::cartographer::mapping::SubmapId& id, display_context_(display_context), submap_node_(map_node->createChildSceneNode()), submap_id_text_node_(submap_node_->createChildSceneNode()), - ogre_slice_(id, display_context->getSceneManager(), submap_node_), pose_axes_(display_context->getSceneManager(), submap_node_, pose_axes_length, pose_axes_radius), submap_id_text_(QString("(%1,%2)") @@ -66,6 +66,11 @@ DrawableSubmap::DrawableSubmap(const ::cartographer::mapping::SubmapId& id, .arg(id.submap_index) .toStdString()), last_query_timestamp_(0) { + for (int slice_index = 0; slice_index < kNumberOfSlicesPerSubmap; + ++slice_index) { + ogre_slices_.emplace_back(::cartographer::common::make_unique( + id, slice_index, display_context->getSceneManager(), submap_node_)); + } // DrawableSubmap creates and manages its visibility property object // (a unique_ptr is needed because the Qt parent of the visibility // property is the submap_category object - the BoolProperty needs @@ -80,7 +85,6 @@ DrawableSubmap::DrawableSubmap(const ::cartographer::mapping::SubmapId& id, // TODO(jihoonl): Make it toggleable. submap_id_text_node_->setPosition(ToOgre(kSubmapIdPosition)); submap_id_text_node_->attachObject(&submap_id_text_); - submap_node_->setVisible(visible); connect(this, SIGNAL(RequestSucceeded()), this, SLOT(UpdateSceneNode())); } @@ -162,19 +166,31 @@ void DrawableSubmap::SetAlpha(const double current_tracking_z) { target_alpha == 0.f || target_alpha == 1.f) { current_alpha_ = target_alpha; } - ogre_slice_.SetAlpha(current_alpha_); + for (auto& slice : ogre_slices_) { + slice->SetAlpha(current_alpha_); + } display_context_->queueRender(); } +void DrawableSubmap::SetSliceVisibility(size_t slice_index, bool visible) { + ogre_slices_.at(slice_index)->SetVisibility(visible); + ToggleVisibility(); +} + void DrawableSubmap::UpdateSceneNode() { ::cartographer::common::MutexLocker locker(&mutex_); - // TODO(gaschler): Add UI feature to show all textures. - ogre_slice_.Update(submap_textures_->textures[0]); + for (size_t slice_index = 0; slice_index < ogre_slices_.size() && + slice_index < submap_textures_->textures.size(); + ++slice_index) { + ogre_slices_[slice_index]->Update(submap_textures_->textures[slice_index]); + } display_context_->queueRender(); } void DrawableSubmap::ToggleVisibility() { - submap_node_->setVisible(visibility_->getBool()); + for (auto& ogre_slice : ogre_slices_) { + ogre_slice->UpdateOgreNodeVisibility(visibility_->getBool()); + } display_context_->queueRender(); } diff --git a/cartographer_rviz/cartographer_rviz/drawable_submap.h b/cartographer_rviz/cartographer_rviz/drawable_submap.h index df8df32..5320b74 100644 --- a/cartographer_rviz/cartographer_rviz/drawable_submap.h +++ b/cartographer_rviz/cartographer_rviz/drawable_submap.h @@ -70,6 +70,10 @@ class DrawableSubmap : public QObject { // 'current_tracking_z'. void SetAlpha(double current_tracking_z); + // Sets the visibility of a slice. It will be drawn if the parent submap + // is also visible. + void SetSliceVisibility(size_t slice_index, bool visible); + ::cartographer::mapping::SubmapId id() const { return id_; } int version() const { return metadata_version_; } bool visibility() const { return visibility_->getBool(); } @@ -93,7 +97,7 @@ class DrawableSubmap : public QObject { ::rviz::DisplayContext* const display_context_; Ogre::SceneNode* const submap_node_; Ogre::SceneNode* const submap_id_text_node_; - OgreSlice ogre_slice_; + std::vector> ogre_slices_; ::cartographer::transform::Rigid3d pose_ GUARDED_BY(mutex_); ::rviz::Axes pose_axes_; ::rviz::MovableText submap_id_text_; diff --git a/cartographer_rviz/cartographer_rviz/ogre_slice.cc b/cartographer_rviz/cartographer_rviz/ogre_slice.cc index 243c354..bf1aa33 100644 --- a/cartographer_rviz/cartographer_rviz/ogre_slice.cc +++ b/cartographer_rviz/cartographer_rviz/ogre_slice.cc @@ -32,10 +32,11 @@ constexpr char kSubmapSourceMaterialName[] = "cartographer_ros/Submap"; constexpr char kSubmapMaterialPrefix[] = "SubmapMaterial"; constexpr char kSubmapTexturePrefix[] = "SubmapTexture"; -std::string GetSubmapIdentifier( - const ::cartographer::mapping::SubmapId& submap_id) { +std::string GetSliceIdentifier( + const ::cartographer::mapping::SubmapId& submap_id, const int slice_id) { return std::to_string(submap_id.trajectory_id) + "-" + - std::to_string(submap_id.submap_index); + std::to_string(submap_id.submap_index) + "-" + + std::to_string(slice_id); } } // namespace @@ -48,19 +49,20 @@ Ogre::Quaternion ToOgre(const Eigen::Quaterniond& q) { return Ogre::Quaternion(q.w(), q.x(), q.y(), q.z()); } -OgreSlice::OgreSlice(const ::cartographer::mapping::SubmapId& id, +OgreSlice::OgreSlice(const ::cartographer::mapping::SubmapId& id, int slice_id, Ogre::SceneManager* const scene_manager, Ogre::SceneNode* const submap_node) : id_(id), + slice_id_(slice_id), scene_manager_(scene_manager), submap_node_(submap_node), slice_node_(submap_node_->createChildSceneNode()), manual_object_(scene_manager_->createManualObject( - kManualObjectPrefix + GetSubmapIdentifier(id))) { + kManualObjectPrefix + GetSliceIdentifier(id, slice_id))) { material_ = Ogre::MaterialManager::getSingleton().getByName( kSubmapSourceMaterialName); - material_ = - material_->clone(kSubmapMaterialPrefix + GetSubmapIdentifier(id_)); + material_ = material_->clone(kSubmapMaterialPrefix + + GetSliceIdentifier(id_, slice_id_)); material_->setReceiveShadows(false); material_->getTechnique(0)->setLightingEnabled(false); material_->setCullingMode(Ogre::CULL_NONE); @@ -120,7 +122,7 @@ void OgreSlice::Update( texture_.setNull(); } const std::string texture_name = - kSubmapTexturePrefix + GetSubmapIdentifier(id_); + kSubmapTexturePrefix + GetSliceIdentifier(id_, slice_id_); texture_ = Ogre::TextureManager::getSingleton().loadRawData( texture_name, Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, pixel_stream, submap_texture.width, submap_texture.height, @@ -142,4 +144,10 @@ void OgreSlice::SetAlpha(const float alpha) { parameters->setNamedConstant("u_alpha", alpha); } +void OgreSlice::SetVisibility(bool visibility) { visibility_ = visibility; } + +void OgreSlice::UpdateOgreNodeVisibility(bool submap_visibility) { + slice_node_->setVisible(submap_visibility && visibility_); +} + } // namespace cartographer_rviz diff --git a/cartographer_rviz/cartographer_rviz/ogre_slice.h b/cartographer_rviz/cartographer_rviz/ogre_slice.h index 6b89555..97605b4 100644 --- a/cartographer_rviz/cartographer_rviz/ogre_slice.h +++ b/cartographer_rviz/cartographer_rviz/ogre_slice.h @@ -40,7 +40,7 @@ class OgreSlice { public: // Attaches a node visualizing the submap 'id' to the 'submap_node' which is // expected to represent the submap frame. - OgreSlice(const ::cartographer::mapping::SubmapId& id, + OgreSlice(const ::cartographer::mapping::SubmapId& id, int slice_id, Ogre::SceneManager* const scene_manager, Ogre::SceneNode* const submap_node); ~OgreSlice(); @@ -55,14 +55,24 @@ class OgreSlice { // Changes the opacity of the submap to 'alpha'. void SetAlpha(float alpha); + // Sets the local visibility of this slice. + void SetVisibility(bool visibility); + + // Updates the SceneNode to be visible if the submap and this slice are + // visible. + void UpdateOgreNodeVisibility(bool submap_visibility); + private: + // TODO(gaschler): Pack both ids into a struct. const ::cartographer::mapping::SubmapId id_; + const int slice_id_; Ogre::SceneManager* const scene_manager_; Ogre::SceneNode* const submap_node_; Ogre::SceneNode* const slice_node_; Ogre::ManualObject* const manual_object_; Ogre::TexturePtr texture_; Ogre::MaterialPtr material_; + bool visibility_ = true; }; } // namespace cartographer_rviz diff --git a/cartographer_rviz/cartographer_rviz/submaps_display.cc b/cartographer_rviz/cartographer_rviz/submaps_display.cc index 3b8bb39..56f3101 100644 --- a/cartographer_rviz/cartographer_rviz/submaps_display.cc +++ b/cartographer_rviz/cartographer_rviz/submaps_display.cc @@ -52,12 +52,18 @@ SubmapsDisplay::SubmapsDisplay() : tf_listener_(tf_buffer_) { tracking_frame_property_ = new ::rviz::StringProperty( "Tracking frame", kDefaultTrackingFrame, "Tracking frame, used for fading out submaps.", this); + slice_high_resolution_enabled_ = new ::rviz::BoolProperty( + "High Resolution", true, "Display high resolution slices.", this, + SLOT(ResolutionToggled()), this); + slice_low_resolution_enabled_ = new ::rviz::BoolProperty( + "Low Resolution", false, "Display low resolution slices.", this, + SLOT(ResolutionToggled()), this); client_ = update_nh_.serviceClient<::cartographer_ros_msgs::SubmapQuery>(""); trajectories_category_ = new ::rviz::Property( "Submaps", QVariant(), "List of all submaps, organized by trajectories.", this); visibility_all_enabled_ = new ::rviz::BoolProperty( - "All Enabled", true, + "All", true, "Whether submaps from all trajectories should be displayed or not.", trajectories_category_, SLOT(AllEnabledToggled()), this); const std::string package_path = ::ros::package::getPath(ROS_PACKAGE_NAME); @@ -148,6 +154,10 @@ void SubmapsDisplay::processMessage( id, context_, map_node_, trajectory_visibility.get(), trajectory_visibility->getBool(), kSubmapPoseAxesLength, kSubmapPoseAxesRadius)); + trajectory_submaps.at(id.submap_index) + ->SetSliceVisibility(0, slice_high_resolution_enabled_->getBool()); + trajectory_submaps.at(id.submap_index) + ->SetSliceVisibility(1, slice_low_resolution_enabled_->getBool()); } trajectory_submaps.at(id.submap_index)->Update(msg->header, submap_entry); } @@ -223,6 +233,18 @@ void SubmapsDisplay::AllEnabledToggled() { } } +void SubmapsDisplay::ResolutionToggled() { + ::cartographer::common::MutexLocker locker(&mutex_); + for (auto& trajectory : trajectories_) { + for (auto& submap_entry : trajectory->submaps) { + submap_entry.second->SetSliceVisibility( + 0, slice_high_resolution_enabled_->getBool()); + submap_entry.second->SetSliceVisibility( + 1, slice_low_resolution_enabled_->getBool()); + } + } +} + void Trajectory::AllEnabledToggled() { const bool visible = visibility->getBool(); for (auto& submap_entry : submaps) { diff --git a/cartographer_rviz/cartographer_rviz/submaps_display.h b/cartographer_rviz/cartographer_rviz/submaps_display.h index 27a749e..779725e 100644 --- a/cartographer_rviz/cartographer_rviz/submaps_display.h +++ b/cartographer_rviz/cartographer_rviz/submaps_display.h @@ -32,8 +32,9 @@ namespace cartographer_rviz { -// This should be a private class in SubmapsDisplay, unfortunately, QT does not -// allow for this. +// TODO(gaschler): This should be a private class in SubmapsDisplay, +// unfortunately, QT does not allow for this. Move the logic out of the struct +// and use just one slot for all changes. struct Trajectory : public QObject { Q_OBJECT @@ -67,6 +68,7 @@ class SubmapsDisplay private Q_SLOTS: void Reset(); void AllEnabledToggled(); + void ResolutionToggled(); private: void CreateClient(); @@ -87,6 +89,8 @@ class SubmapsDisplay Ogre::SceneNode* map_node_ = nullptr; // Represents the map frame. std::vector> trajectories_ GUARDED_BY(mutex_); ::cartographer::common::Mutex mutex_; + ::rviz::BoolProperty* slice_high_resolution_enabled_; + ::rviz::BoolProperty* slice_low_resolution_enabled_; ::rviz::Property* trajectories_category_; ::rviz::BoolProperty* visibility_all_enabled_; };