From 5497e9c343b247d1e07e79a80cb75b3dcbe44bf3 Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Thu, 4 Aug 2016 14:38:55 +0200 Subject: [PATCH] Fix draw ordering issue of the RViz visualization. (#5) Instead of drawing into a offscreen texture and blitting the result as an overlay (which shows up on top of everything else) we directly draw to the RViz scene. To get proper X-rays one should use a 0.5 gray background for now. Also fixes the configuration to enable 3D mapping from a bag. --- .../configuration_files/backpack_3d.lua | 2 +- cartographer_ros/launch/backpack_3d.launch | 59 -------- .../materials/glsl120/glsl120.program | 10 -- .../materials/glsl120/screen_blit.frag | 29 ---- .../materials/glsl120/screen_blit.vert | 28 ---- .../ogre_media/materials/glsl120/submap.frag | 7 +- .../materials/scripts/screen_blit.material | 31 ----- cartographer_ros/src/drawable_submap.cc | 3 + cartographer_ros/src/drawable_submap.h | 1 + cartographer_ros/src/submaps_display.cc | 128 ++---------------- cartographer_ros/src/submaps_display.h | 19 +-- cartographer_ros/urdf/backpack_2d.urdf | 18 +-- cartographer_ros/urdf/backpack_3d.urdf | 44 +++--- 13 files changed, 44 insertions(+), 335 deletions(-) delete mode 100644 cartographer_ros/ogre_media/materials/glsl120/screen_blit.frag delete mode 100644 cartographer_ros/ogre_media/materials/glsl120/screen_blit.vert delete mode 100644 cartographer_ros/ogre_media/materials/scripts/screen_blit.material diff --git a/cartographer_ros/configuration_files/backpack_3d.lua b/cartographer_ros/configuration_files/backpack_3d.lua index ae099e4..ce2f912 100644 --- a/cartographer_ros/configuration_files/backpack_3d.lua +++ b/cartographer_ros/configuration_files/backpack_3d.lua @@ -29,6 +29,6 @@ options.sparse_pose_graph.constraint_builder.adaptive_voxel_filter = TRAJECTORY_ options.sparse_pose_graph.constraint_builder.min_score = 0.62 options.sparse_pose_graph.constraint_builder.log_matches = true -options.trajectory_builder.scans_per_accumulation = 2 +options.trajectory_builder.scans_per_accumulation = 20 return options diff --git a/cartographer_ros/launch/backpack_3d.launch b/cartographer_ros/launch/backpack_3d.launch index 6801ef2..b679ecb 100644 --- a/cartographer_ros/launch/backpack_3d.launch +++ b/cartographer_ros/launch/backpack_3d.launch @@ -15,65 +15,6 @@ --> - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/cartographer_ros/ogre_media/materials/glsl120/glsl120.program b/cartographer_ros/ogre_media/materials/glsl120/glsl120.program index d8d6e6e..7272c21 100644 --- a/cartographer_ros/ogre_media/materials/glsl120/glsl120.program +++ b/cartographer_ros/ogre_media/materials/glsl120/glsl120.program @@ -23,13 +23,3 @@ vertex_program cartographer_ros/glsl120/submap.vert glsl { source submap.vert } - -fragment_program cartographer_ros/glsl120/screen_blit.frag glsl -{ - source screen_blit.frag -} - -vertex_program cartographer_ros/glsl120/screen_blit.vert glsl -{ - source screen_blit.vert -} diff --git a/cartographer_ros/ogre_media/materials/glsl120/screen_blit.frag b/cartographer_ros/ogre_media/materials/glsl120/screen_blit.frag deleted file mode 100644 index 195d813..0000000 --- a/cartographer_ros/ogre_media/materials/glsl120/screen_blit.frag +++ /dev/null @@ -1,29 +0,0 @@ -/* - * Copyright 2016 The Cartographer Authors - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#version 120 - -varying vec2 out_texture_coordinate; - -uniform sampler2D u_texture; - -void main() -{ - vec2 texture_color = texture2D(u_texture, out_texture_coordinate).rg; - float opacity = texture_color.g; - float value = texture_color.r; - gl_FragColor = vec4(value, value, value, opacity); -} diff --git a/cartographer_ros/ogre_media/materials/glsl120/screen_blit.vert b/cartographer_ros/ogre_media/materials/glsl120/screen_blit.vert deleted file mode 100644 index 74d1bba..0000000 --- a/cartographer_ros/ogre_media/materials/glsl120/screen_blit.vert +++ /dev/null @@ -1,28 +0,0 @@ -/* - * Copyright 2016 The Cartographer Authors - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#version 120 - -attribute vec4 vertex; -attribute vec4 uv0; - -varying vec2 out_texture_coordinate; - -void main() -{ - out_texture_coordinate = vec2(uv0); - gl_Position = vertex; -} diff --git a/cartographer_ros/ogre_media/materials/glsl120/submap.frag b/cartographer_ros/ogre_media/materials/glsl120/submap.frag index 1998f53..eb49e2f 100644 --- a/cartographer_ros/ogre_media/materials/glsl120/submap.frag +++ b/cartographer_ros/ogre_media/materials/glsl120/submap.frag @@ -24,8 +24,7 @@ uniform float u_alpha; void main() { vec2 texture_value = texture2D(u_submap, out_submap_texture_coordinate).rg; - float value = texture_value.r; - float alpha = texture_value.g; - float is_known = step(1e-3, alpha + value); - gl_FragColor = vec4(u_alpha * value, u_alpha * is_known, 0., u_alpha * alpha); + float value = u_alpha * texture_value.r; + float alpha = u_alpha * texture_value.g; + gl_FragColor = vec4(value, value, value, alpha); } diff --git a/cartographer_ros/ogre_media/materials/scripts/screen_blit.material b/cartographer_ros/ogre_media/materials/scripts/screen_blit.material deleted file mode 100644 index 2853689..0000000 --- a/cartographer_ros/ogre_media/materials/scripts/screen_blit.material +++ /dev/null @@ -1,31 +0,0 @@ -/* - * Copyright 2016 The Cartographer Authors - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -material cartographer_ros/ScreenBlit -{ - technique - { - pass - { - vertex_program_ref cartographer_ros/glsl120/screen_blit.vert {} - - fragment_program_ref cartographer_ros/glsl120/screen_blit.frag - { - param_named u_texture int 0 - } - } - } -} diff --git a/cartographer_ros/src/drawable_submap.cc b/cartographer_ros/src/drawable_submap.cc index c3276eb..bcfb159 100644 --- a/cartographer_ros/src/drawable_submap.cc +++ b/cartographer_ros/src/drawable_submap.cc @@ -60,6 +60,7 @@ DrawableSubmap::DrawableSubmap(const int submap_id, const int trajectory_id, ::rviz::FrameManager* const frame_manager, Ogre::SceneManager* const scene_manager) : frame_manager_(frame_manager), + scene_manager_(scene_manager), scene_node_(scene_manager->getRootSceneNode()->createChildSceneNode()), manual_object_(scene_manager->createManualObject( kManualObjectPrefix + GetSubmapIdentifier(trajectory_id, submap_id))), @@ -86,6 +87,8 @@ DrawableSubmap::~DrawableSubmap() { Ogre::TextureManager::getSingleton().remove(texture_->getHandle()); texture_.setNull(); } + scene_manager_->destroySceneNode(scene_node_); + scene_manager_->destroyManualObject(manual_object_); } bool DrawableSubmap::Update( diff --git a/cartographer_ros/src/drawable_submap.h b/cartographer_ros/src/drawable_submap.h index 8332a70..a05f5a9 100644 --- a/cartographer_ros/src/drawable_submap.h +++ b/cartographer_ros/src/drawable_submap.h @@ -89,6 +89,7 @@ class DrawableSubmap : public QObject { ::cartographer::common::Mutex mutex_; ::rviz::FrameManager* frame_manager_; + Ogre::SceneManager* const scene_manager_; Ogre::SceneNode* const scene_node_; Ogre::ManualObject* manual_object_; Ogre::TexturePtr texture_; diff --git a/cartographer_ros/src/submaps_display.cc b/cartographer_ros/src/submaps_display.cc index e59375d..35f8493 100644 --- a/cartographer_ros/src/submaps_display.cc +++ b/cartographer_ros/src/submaps_display.cc @@ -16,23 +16,8 @@ #include "submaps_display.h" -#include -#include -#include -#include -#include -#include -#include -#include #include -#include #include -#include -#include -#include -#include -#include -#include #include #include #include @@ -56,13 +41,6 @@ constexpr int kMaxOnGoingRequests = 6; constexpr char kMaterialsDirectory[] = "/ogre_media/materials"; constexpr char kGlsl120Directory[] = "/glsl120"; constexpr char kScriptsDirectory[] = "/scripts"; -constexpr char kScreenBlitMaterialName[] = "ScreenBlitMaterial"; -constexpr char kScreenBlitSourceMaterialName[] = "cartographer_ros/ScreenBlit"; -constexpr char kSubmapsRttPrefix[] = "SubmapsRtt"; -constexpr char kMapTextureName[] = "MapTexture"; -constexpr char kMapOverlayName[] = "MapOverlay"; -constexpr char kSubmapsSceneCameraName[] = "SubmapsSceneCamera"; -constexpr char kSubmapTexturesGroup[] = "SubmapTexturesGroup"; constexpr char kDefaultMapFrame[] = "map"; constexpr char kDefaultTrackingFrame[] = "base_link"; @@ -70,7 +48,6 @@ constexpr char kDefaultTrackingFrame[] = "base_link"; SubmapsDisplay::SubmapsDisplay() : Display(), - rtt_count_(0), scene_manager_listener_([this]() { UpdateMapTexture(); }), tf_listener_(tf_buffer_) { connect(this, SIGNAL(SubmapListUpdated()), this, SLOT(RequestNewSubmaps())); @@ -107,32 +84,6 @@ SubmapsDisplay::SubmapsDisplay() SubmapsDisplay::~SubmapsDisplay() { UnsubscribeAndClear(); } void SubmapsDisplay::onInitialize() { - submaps_scene_manager_ = - Ogre::Root::getSingletonPtr()->createSceneManager(Ogre::ST_GENERIC); - submaps_scene_camera_ = - submaps_scene_manager_->createCamera(kSubmapsSceneCameraName); - submap_scene_material_ = Ogre::MaterialManager::getSingleton().create( - kMapTextureName, Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); - - screen_blit_material_ = Ogre::MaterialManager::getSingleton().getByName( - kScreenBlitSourceMaterialName); - screen_blit_material_ = screen_blit_material_->clone(kScreenBlitMaterialName); - screen_blit_material_->setReceiveShadows(false); - screen_blit_material_->getTechnique(0)->setLightingEnabled(false); - screen_blit_material_->setDepthWriteEnabled(false); - - Ogre::OverlayManager& overlay_manager = Ogre::OverlayManager::getSingleton(); - overlay_ = overlay_manager.create(kMapOverlayName); - panel_ = static_cast( - overlay_manager.createOverlayElement("Panel", "PanelName")); - overlay_->add2D(panel_); - panel_->setPosition(0.0, 0.0); - panel_->setDimensions(1., 1.); - panel_->setMaterialName(kScreenBlitMaterialName); - - Ogre::ResourceGroupManager::getSingleton().createResourceGroup( - kSubmapTexturesGroup); - scene_manager_->addListener(&scene_manager_listener_); UpdateSubmapTopicOrService(); } @@ -166,9 +117,9 @@ void SubmapsDisplay::Subscribe() { &SubmapsDisplay::IncomingSubmapList, this, ros::TransportHints().reliable()); setStatus(::rviz::StatusProperty::Ok, "Topic", "OK"); - } catch (ros::Exception& e) { + } catch (const ros::Exception& ex) { setStatus(::rviz::StatusProperty::Error, "Topic", - QString("Error subscribing: ") + e.what()); + QString("Error subscribing: ") + ex.what()); } } } @@ -183,15 +134,7 @@ void SubmapsDisplay::UnsubscribeAndClear() { submap_list_subscriber_.shutdown(); client_.shutdown(); ::cartographer::common::MutexLocker locker(&mutex_); - submaps_scene_manager_->clearScene(); - if (!rttTexture_.isNull()) { - rttTexture_->unload(); - rttTexture_.setNull(); - } - Ogre::ResourceGroupManager::getSingleton().clearResourceGroup( - kSubmapTexturesGroup); trajectories_.clear(); - overlay_->hide(); } void SubmapsDisplay::RequestNewSubmaps() { @@ -210,7 +153,7 @@ void SubmapsDisplay::RequestNewSubmaps() { submap_id < submap_entries.size(); ++submap_id) { trajectories_[trajectory_id]->Add(submap_id, trajectory_id, context_->getFrameManager(), - submaps_scene_manager_); + context_->getSceneManager()); } } int num_ongoing_requests = 0; @@ -242,73 +185,22 @@ void SubmapsDisplay::RequestNewSubmaps() { } void SubmapsDisplay::UpdateMapTexture() { - if (trajectories_.empty()) { - return; - } - const int width = scene_manager_->getCurrentViewport()->getActualWidth(); - const int height = scene_manager_->getCurrentViewport()->getActualHeight(); - if (!rttTexture_.isNull()) { - rttTexture_->unload(); - rttTexture_.setNull(); - } - // If the rtt texture is freed every time UpdateMapTexture() is called, the - // code slows down a lot. Therefore, we assign them to a group and free them - // every 100th texture. - if (rtt_count_ % 100 == 0) { - Ogre::ResourceGroupManager::getSingleton().clearResourceGroup( - kSubmapTexturesGroup); - } - rttTexture_ = - Ogre::Root::getSingletonPtr()->getTextureManager()->createManual( - kSubmapsRttPrefix + std::to_string(rtt_count_), kSubmapTexturesGroup, - Ogre::TEX_TYPE_2D, width, height, 0, Ogre::PF_RG8, - Ogre::TU_RENDERTARGET); - rtt_count_++; - - Ogre::Pass* rtt_pass = submap_scene_material_->getTechnique(0)->getPass(0); - Ogre::TextureUnitState* const rtt_tex_unit = - rtt_pass->getNumTextureUnitStates() > 0 - ? rtt_pass->getTextureUnitState(0) - : rtt_pass->createTextureUnitState(); - rtt_tex_unit->setTexture(rttTexture_); - - Ogre::RenderTexture* const renderTexture = - rttTexture_->getBuffer()->getRenderTarget(); - renderTexture->addViewport(submaps_scene_camera_) - ->setBackgroundColour(Ogre::ColourValue(0.5f, 0.f, 0.f)); - { - ::cartographer::common::MutexLocker locker(&mutex_); - // TODO(pedrofernandez): Add support for more than one trajectory. - for (int i = 0; i < trajectories_.front()->Size(); ++i) { - trajectories_.front()->Get(i).Transform(ros::Time()); + ::cartographer::common::MutexLocker locker(&mutex_); + for (auto& trajectory : trajectories_) { + for (int i = 0; i < trajectory->Size(); ++i) { + trajectory->Get(i).Transform(ros::Time(0) /* latest */); try { const ::geometry_msgs::TransformStamped transform_stamped = tf_buffer_.lookupTransform(map_frame_property_->getStdString(), tracking_frame_property_->getStdString(), ros::Time(0) /* latest */); - trajectories_.front()->Get(i).SetAlpha( + trajectory->Get(i).SetAlpha( transform_stamped.transform.translation.z); - } catch (tf2::TransformException& e) { - ROS_WARN("Could not compute submap fading: %s", e.what()); + } catch (const tf2::TransformException& ex) { + ROS_WARN("Could not compute submap fading: %s", ex.what()); } } } - Ogre::Camera* const actual_camera = - scene_manager_->getCurrentViewport()->getCamera(); - submaps_scene_camera_->synchroniseBaseSettingsWith(actual_camera); - submaps_scene_camera_->setCustomProjectionMatrix( - true, actual_camera->getProjectionMatrix()); - renderTexture->update(); - - Ogre::Pass* const pass = screen_blit_material_->getTechnique(0)->getPass(0); - pass->setSceneBlending(Ogre::SBF_SOURCE_ALPHA, - Ogre::SBF_ONE_MINUS_SOURCE_ALPHA); - Ogre::TextureUnitState* const tex_unit = pass->getNumTextureUnitStates() > 0 - ? pass->getTextureUnitState(0) - : pass->createTextureUnitState(); - tex_unit->setTextureName(rttTexture_->getName()); - tex_unit->setTextureFiltering(Ogre::TFO_NONE); - overlay_->show(); } } // namespace rviz diff --git a/cartographer_ros/src/submaps_display.h b/cartographer_ros/src/submaps_display.h index 48a1585..b43b657 100644 --- a/cartographer_ros/src/submaps_display.h +++ b/cartographer_ros/src/submaps_display.h @@ -18,8 +18,6 @@ #define CARTOGRAPHER_ROS_GOOGLE_CARTOGRAPHER_SRC_SUBMAPS_DISPLAY_H_ #include -#include -#include #include #include #include @@ -46,12 +44,9 @@ namespace rviz { // RViz plugin used for displaying maps which are represented by a collection of // submaps. // -// We keep a separate Ogre scene to the one provided by rviz and in it we place -// every submap as a SceneNode. We show an X-ray view of the map which is -// achieved by shipping textures for every submap containing pre-multiplied -// alpha and grayscale values, these are then alpha blended together onto an -// offscreen texture. This offscreen texture is then screen blit onto the screen -// as a grayscale image. +// We show an X-ray view of the map which is achieved by shipping textures for +// every submap containing pre-multiplied alpha and grayscale values, these are +// then alpha blended together. class SubmapsDisplay : public ::rviz::Display { Q_OBJECT @@ -95,7 +90,6 @@ class SubmapsDisplay : public ::rviz::Display { void IncomingSubmapList( const ::cartographer_ros_msgs::SubmapList::ConstPtr& msg); - int rtt_count_; SceneManagerListener scene_manager_listener_; ::cartographer_ros_msgs::SubmapList submap_list_; ros::Subscriber submap_list_subscriber_; @@ -107,13 +101,6 @@ class SubmapsDisplay : public ::rviz::Display { ::rviz::StringProperty* map_frame_property_; ::rviz::StringProperty* tracking_frame_property_; std::vector> trajectories_ GUARDED_BY(mutex_); - Ogre::SceneManager* submaps_scene_manager_; - Ogre::Camera* submaps_scene_camera_; - Ogre::MaterialPtr submap_scene_material_; - Ogre::MaterialPtr screen_blit_material_; - Ogre::Overlay* overlay_; - Ogre::OverlayContainer* panel_; - Ogre::TexturePtr rttTexture_; ::cartographer::common::Mutex mutex_; }; diff --git a/cartographer_ros/urdf/backpack_2d.urdf b/cartographer_ros/urdf/backpack_2d.urdf index 6c88906..33842d3 100644 --- a/cartographer_ros/urdf/backpack_2d.urdf +++ b/cartographer_ros/urdf/backpack_2d.urdf @@ -27,9 +27,9 @@ - + - + @@ -37,7 +37,7 @@ - + @@ -47,7 +47,7 @@ - + @@ -55,15 +55,7 @@ - - - - - - - - - + diff --git a/cartographer_ros/urdf/backpack_3d.urdf b/cartographer_ros/urdf/backpack_3d.urdf index 1772dbb..9b6a538 100644 --- a/cartographer_ros/urdf/backpack_3d.urdf +++ b/cartographer_ros/urdf/backpack_3d.urdf @@ -25,61 +25,53 @@ - + - + - + - + - + - + - + - + - + - - - - - - - - - + - - - - + + + + - - + + - - + +