diff --git a/cartographer_rviz/cartographer_rviz/drawable_submap.cc b/cartographer_rviz/cartographer_rviz/drawable_submap.cc index 2027c2b..4dcf401 100644 --- a/cartographer_rviz/cartographer_rviz/drawable_submap.cc +++ b/cartographer_rviz/cartographer_rviz/drawable_submap.cc @@ -23,8 +23,6 @@ #include "Eigen/Core" #include "Eigen/Geometry" -#include "OgreGpuProgramParams.h" -#include "OgreImage.h" #include "cartographer/common/make_unique.h" #include "cartographer/common/port.h" #include "cartographer_ros/msg_conversion.h" @@ -37,10 +35,6 @@ namespace cartographer_rviz { namespace { constexpr std::chrono::milliseconds kMinQueryDelayInMs(250); -constexpr char kSubmapTexturePrefix[] = "SubmapTexture"; -constexpr char kManualObjectPrefix[] = "ManualObjectSubmap"; -constexpr char kSubmapMaterialPrefix[] = "SubmapMaterial"; -constexpr char kSubmapSourceMaterialName[] = "cartographer_ros/Submap"; // Distance before which the submap will be shown at full opacity, and distance // over which the submap will then fade out. @@ -48,20 +42,6 @@ constexpr double kFadeOutStartDistanceInMeters = 1.; constexpr double kFadeOutDistanceInMeters = 2.; constexpr float kAlphaUpdateThreshold = 0.2f; -std::string GetSubmapIdentifier( - const ::cartographer::mapping::SubmapId& submap_id) { - return std::to_string(submap_id.trajectory_id) + "-" + - std::to_string(submap_id.submap_index); -} - -Ogre::Vector3 ToOgre(const Eigen::Vector3d& v) { - return Ogre::Vector3(v.x(), v.y(), v.z()); -} - -Ogre::Quaternion ToOgre(const Eigen::Quaterniond& q) { - return Ogre::Quaternion(q.w(), q.x(), q.y(), q.z()); -} - } // namespace DrawableSubmap::DrawableSubmap(const ::cartographer::mapping::SubmapId& id, @@ -74,21 +54,10 @@ DrawableSubmap::DrawableSubmap(const ::cartographer::mapping::SubmapId& id, scene_node_(display_context->getSceneManager() ->getRootSceneNode() ->createChildSceneNode()), - submap_node_(scene_node_->createChildSceneNode()), - manual_object_(display_context->getSceneManager()->createManualObject( - kManualObjectPrefix + GetSubmapIdentifier(id))), + ogre_submap_(id, display_context->getSceneManager(), scene_node_), pose_axes_(display_context->getSceneManager(), scene_node_, pose_axes_length, pose_axes_radius), last_query_timestamp_(0) { - material_ = Ogre::MaterialManager::getSingleton().getByName( - kSubmapSourceMaterialName); - material_ = - material_->clone(kSubmapMaterialPrefix + GetSubmapIdentifier(id_)); - material_->setReceiveShadows(false); - material_->getTechnique(0)->setLightingEnabled(false); - material_->setCullingMode(Ogre::CULL_NONE); - material_->setDepthBias(-1.f, 0.f); - material_->setDepthWriteEnabled(false); // 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 @@ -96,7 +65,6 @@ DrawableSubmap::DrawableSubmap(const ::cartographer::mapping::SubmapId& id, visibility_ = ::cartographer::common::make_unique<::rviz::BoolProperty>( "" /* title */, visible, "" /* description */, submap_category, SLOT(ToggleVisibility()), this); - submap_node_->attachObject(manual_object_); scene_node_->setVisible(visible); connect(this, SIGNAL(RequestSucceeded()), this, SLOT(UpdateSceneNode())); } @@ -107,14 +75,7 @@ DrawableSubmap::~DrawableSubmap() { if (QueryInProgress()) { rpc_request_future_.wait(); } - Ogre::MaterialManager::getSingleton().remove(material_->getHandle()); - if (!texture_.isNull()) { - Ogre::TextureManager::getSingleton().remove(texture_->getHandle()); - texture_.setNull(); - } - display_context_->getSceneManager()->destroySceneNode(submap_node_); display_context_->getSceneManager()->destroySceneNode(scene_node_); - display_context_->getSceneManager()->destroyManualObject(manual_object_); } void DrawableSubmap::Update( @@ -132,9 +93,7 @@ void DrawableSubmap::Update( pose_ = ::cartographer_ros::ToRigid3d(metadata.pose); scene_node_->setPosition(ToOgre(pose_.translation())); scene_node_->setOrientation(ToOgre(pose_.rotation())); - if (submap_texture_ != nullptr) { - display_context_->queueRender(); - } + display_context_->queueRender(); visibility_->setName( QString("%1.%2").arg(id_.submap_index).arg(metadata_version_)); visibility_->setDescription( @@ -187,84 +146,26 @@ void DrawableSubmap::SetAlpha(const double current_tracking_z) { std::abs(pose_.translation().z() - current_tracking_z); const double fade_distance = std::max(distance_z - kFadeOutStartDistanceInMeters, 0.); - const float alpha = static_cast( + const float target_alpha = static_cast( std::max(0., 1. - fade_distance / kFadeOutDistanceInMeters)); - const Ogre::GpuProgramParametersSharedPtr parameters = - material_->getTechnique(0)->getPass(0)->getFragmentProgramParameters(); - parameters->setNamedConstant("u_alpha", UpdateAlpha(alpha)); -} - -void DrawableSubmap::UpdateSceneNode() { - ::cartographer::common::MutexLocker locker(&mutex_); - submap_node_->setPosition(ToOgre(submap_texture_->slice_pose.translation())); - submap_node_->setOrientation(ToOgre(submap_texture_->slice_pose.rotation())); - // The call to Ogre's loadRawData below does not work with an RG texture, - // therefore we create an RGB one whose blue channel is always 0. - std::vector rgb; - for (size_t i = 0; i < submap_texture_->intensity.size(); ++i) { - rgb.push_back(submap_texture_->intensity[i]); - rgb.push_back(submap_texture_->alpha[i]); - rgb.push_back(0.); - } - - manual_object_->clear(); - const float metric_width = - submap_texture_->resolution * submap_texture_->width; - const float metric_height = - submap_texture_->resolution * submap_texture_->height; - manual_object_->begin(material_->getName(), - Ogre::RenderOperation::OT_TRIANGLE_STRIP); - // Bottom left - manual_object_->position(-metric_height, 0.0f, 0.0f); - manual_object_->textureCoord(0.0f, 1.0f); - // Bottom right - manual_object_->position(-metric_height, -metric_width, 0.0f); - manual_object_->textureCoord(1.0f, 1.0f); - // Top left - manual_object_->position(0.0f, 0.0f, 0.0f); - manual_object_->textureCoord(0.0f, 0.0f); - // Top right - manual_object_->position(0.0f, -metric_width, 0.0f); - manual_object_->textureCoord(1.0f, 0.0f); - manual_object_->end(); - - Ogre::DataStreamPtr pixel_stream; - pixel_stream.bind(new Ogre::MemoryDataStream(rgb.data(), rgb.size())); - - if (!texture_.isNull()) { - Ogre::TextureManager::getSingleton().remove(texture_->getHandle()); - texture_.setNull(); - } - const std::string texture_name = - kSubmapTexturePrefix + GetSubmapIdentifier(id_); - texture_ = Ogre::TextureManager::getSingleton().loadRawData( - texture_name, Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, - pixel_stream, submap_texture_->width, submap_texture_->height, - Ogre::PF_BYTE_RGB, Ogre::TEX_TYPE_2D, 0); - - Ogre::Pass* const pass = material_->getTechnique(0)->getPass(0); - pass->setSceneBlending(Ogre::SBF_ONE, Ogre::SBF_ONE_MINUS_SOURCE_ALPHA); - Ogre::TextureUnitState* const texture_unit = - pass->getNumTextureUnitStates() > 0 ? pass->getTextureUnitState(0) - : pass->createTextureUnitState(); - - texture_unit->setTextureName(texture_->getName()); - texture_unit->setTextureFiltering(Ogre::TFO_NONE); - - display_context_->queueRender(); -} - -float DrawableSubmap::UpdateAlpha(const float target_alpha) { if (std::abs(target_alpha - current_alpha_) > kAlphaUpdateThreshold || target_alpha == 0.f || target_alpha == 1.f) { current_alpha_ = target_alpha; } - return current_alpha_; + ogre_submap_.SetAlpha(current_alpha_); + display_context_->queueRender(); +} + +void DrawableSubmap::UpdateSceneNode() { + ::cartographer::common::MutexLocker locker(&mutex_); + ogre_submap_.Update(*submap_texture_); + display_context_->queueRender(); } void DrawableSubmap::ToggleVisibility() { scene_node_->setVisible(visibility_->getBool()); + display_context_->queueRender(); } } // namespace cartographer_rviz diff --git a/cartographer_rviz/cartographer_rviz/drawable_submap.h b/cartographer_rviz/cartographer_rviz/drawable_submap.h index 8574077..c5a73ed 100644 --- a/cartographer_rviz/cartographer_rviz/drawable_submap.h +++ b/cartographer_rviz/cartographer_rviz/drawable_submap.h @@ -22,19 +22,15 @@ #include "Eigen/Core" #include "Eigen/Geometry" -#include "OgreManualObject.h" -#include "OgreMaterial.h" -#include "OgreQuaternion.h" #include "OgreSceneManager.h" #include "OgreSceneNode.h" -#include "OgreTexture.h" -#include "OgreVector3.h" #include "cartographer/common/mutex.h" #include "cartographer/mapping/id.h" #include "cartographer/transform/rigid_transform.h" #include "cartographer_ros/submap.h" #include "cartographer_ros_msgs/SubmapEntry.h" #include "cartographer_ros_msgs/SubmapQuery.h" +#include "cartographer_rviz/ogre_submap.h" #include "ros/ros.h" #include "rviz/display_context.h" #include "rviz/frame_manager.h" @@ -91,17 +87,12 @@ class DrawableSubmap : public QObject { void ToggleVisibility(); private: - float UpdateAlpha(float target_alpha); - const ::cartographer::mapping::SubmapId id_; ::cartographer::common::Mutex mutex_; ::rviz::DisplayContext* const display_context_; Ogre::SceneNode* const scene_node_; - Ogre::SceneNode* const submap_node_; - Ogre::ManualObject* const manual_object_; - Ogre::TexturePtr texture_; - Ogre::MaterialPtr material_; + OgreSubmap ogre_submap_; ::cartographer::transform::Rigid3d pose_ GUARDED_BY(mutex_); ::rviz::Axes pose_axes_; std::chrono::milliseconds last_query_timestamp_ GUARDED_BY(mutex_); diff --git a/cartographer_rviz/cartographer_rviz/ogre_submap.cc b/cartographer_rviz/cartographer_rviz/ogre_submap.cc new file mode 100644 index 0000000..af31faa --- /dev/null +++ b/cartographer_rviz/cartographer_rviz/ogre_submap.cc @@ -0,0 +1,145 @@ +/* + * 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. + */ + +#include "cartographer_rviz/ogre_submap.h" + +#include +#include + +#include "OgreGpuProgramParams.h" +#include "OgreImage.h" +#include "cartographer/common/port.h" + +namespace cartographer_rviz { + +namespace { + +constexpr char kManualObjectPrefix[] = "ManualObjectSubmap"; +constexpr char kSubmapSourceMaterialName[] = "cartographer_ros/Submap"; +constexpr char kSubmapMaterialPrefix[] = "SubmapMaterial"; +constexpr char kSubmapTexturePrefix[] = "SubmapTexture"; + +std::string GetSubmapIdentifier( + const ::cartographer::mapping::SubmapId& submap_id) { + return std::to_string(submap_id.trajectory_id) + "-" + + std::to_string(submap_id.submap_index); +} + +} // namespace + +Ogre::Vector3 ToOgre(const Eigen::Vector3d& v) { + return Ogre::Vector3(v.x(), v.y(), v.z()); +} + +Ogre::Quaternion ToOgre(const Eigen::Quaterniond& q) { + return Ogre::Quaternion(q.w(), q.x(), q.y(), q.z()); +} + +OgreSubmap::OgreSubmap(const ::cartographer::mapping::SubmapId& id, + Ogre::SceneManager* const scene_manager, + Ogre::SceneNode* const scene_node) + : id_(id), + scene_manager_(scene_manager), + scene_node_(scene_node), + submap_node_(scene_node_->createChildSceneNode()), + manual_object_(scene_manager_->createManualObject( + kManualObjectPrefix + GetSubmapIdentifier(id))) { + material_ = Ogre::MaterialManager::getSingleton().getByName( + kSubmapSourceMaterialName); + material_ = + material_->clone(kSubmapMaterialPrefix + GetSubmapIdentifier(id_)); + material_->setReceiveShadows(false); + material_->getTechnique(0)->setLightingEnabled(false); + material_->setCullingMode(Ogre::CULL_NONE); + material_->setDepthBias(-1.f, 0.f); + material_->setDepthWriteEnabled(false); + submap_node_->attachObject(manual_object_); +} + +OgreSubmap::~OgreSubmap() { + Ogre::MaterialManager::getSingleton().remove(material_->getHandle()); + if (!texture_.isNull()) { + Ogre::TextureManager::getSingleton().remove(texture_->getHandle()); + texture_.setNull(); + } + scene_manager_->destroySceneNode(submap_node_); + scene_manager_->destroyManualObject(manual_object_); +} + +void OgreSubmap::Update( + const ::cartographer_ros::SubmapTexture& submap_texture) { + submap_node_->setPosition(ToOgre(submap_texture.slice_pose.translation())); + submap_node_->setOrientation(ToOgre(submap_texture.slice_pose.rotation())); + // The call to Ogre's loadRawData below does not work with an RG texture, + // therefore we create an RGB one whose blue channel is always 0. + std::vector rgb; + CHECK_EQ(submap_texture.intensity.size(), submap_texture.alpha.size()); + for (size_t i = 0; i < submap_texture.intensity.size(); ++i) { + rgb.push_back(submap_texture.intensity[i]); + rgb.push_back(submap_texture.alpha[i]); + rgb.push_back(0); + } + + manual_object_->clear(); + const float metric_width = submap_texture.resolution * submap_texture.width; + const float metric_height = submap_texture.resolution * submap_texture.height; + manual_object_->begin(material_->getName(), + Ogre::RenderOperation::OT_TRIANGLE_STRIP); + // Bottom left + manual_object_->position(-metric_height, 0.0f, 0.0f); + manual_object_->textureCoord(0.0f, 1.0f); + // Bottom right + manual_object_->position(-metric_height, -metric_width, 0.0f); + manual_object_->textureCoord(1.0f, 1.0f); + // Top left + manual_object_->position(0.0f, 0.0f, 0.0f); + manual_object_->textureCoord(0.0f, 0.0f); + // Top right + manual_object_->position(0.0f, -metric_width, 0.0f); + manual_object_->textureCoord(1.0f, 0.0f); + manual_object_->end(); + + Ogre::DataStreamPtr pixel_stream; + pixel_stream.bind(new Ogre::MemoryDataStream(rgb.data(), rgb.size())); + + if (!texture_.isNull()) { + Ogre::TextureManager::getSingleton().remove(texture_->getHandle()); + texture_.setNull(); + } + const std::string texture_name = + kSubmapTexturePrefix + GetSubmapIdentifier(id_); + texture_ = Ogre::TextureManager::getSingleton().loadRawData( + texture_name, Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, + pixel_stream, submap_texture.width, submap_texture.height, + Ogre::PF_BYTE_RGB, Ogre::TEX_TYPE_2D, 0); + + Ogre::Pass* const pass = material_->getTechnique(0)->getPass(0); + pass->setSceneBlending(Ogre::SBF_ONE, Ogre::SBF_ONE_MINUS_SOURCE_ALPHA); + Ogre::TextureUnitState* const texture_unit = + pass->getNumTextureUnitStates() > 0 ? pass->getTextureUnitState(0) + : pass->createTextureUnitState(); + + texture_unit->setTextureName(texture_->getName()); + texture_unit->setTextureFiltering(Ogre::TFO_NONE); +} + +void OgreSubmap::SetAlpha(const float alpha) { + const Ogre::GpuProgramParametersSharedPtr parameters = + material_->getTechnique(0)->getPass(0)->getFragmentProgramParameters(); + parameters->setNamedConstant("u_alpha", alpha); +} + +} // namespace cartographer_rviz diff --git a/cartographer_rviz/cartographer_rviz/ogre_submap.h b/cartographer_rviz/cartographer_rviz/ogre_submap.h new file mode 100644 index 0000000..0b2038e --- /dev/null +++ b/cartographer_rviz/cartographer_rviz/ogre_submap.h @@ -0,0 +1,70 @@ +/* + * 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. + */ + +#ifndef CARTOGRAPHER_RVIZ_SRC_OGRE_SUBMAP_H_ +#define CARTOGRAPHER_RVIZ_SRC_OGRE_SUBMAP_H_ + +#include "Eigen/Core" +#include "Eigen/Geometry" +#include "OgreManualObject.h" +#include "OgreMaterial.h" +#include "OgreQuaternion.h" +#include "OgreSceneManager.h" +#include "OgreSceneNode.h" +#include "OgreTexture.h" +#include "OgreVector3.h" +#include "cartographer/mapping/id.h" +#include "cartographer_ros/submap.h" + +namespace cartographer_rviz { + +Ogre::Vector3 ToOgre(const Eigen::Vector3d& v); +Ogre::Quaternion ToOgre(const Eigen::Quaterniond& q); + +// A class containing the Ogre code to visualize submap data. Member functions +// are expected to be called from the Ogre thread. +class OgreSubmap { + public: + // Attaches a node visualizing the submap 'id' to the 'scene_node' which is + // expected to represent the submap frame. + OgreSubmap(const ::cartographer::mapping::SubmapId& id, + Ogre::SceneManager* const scene_manager, + Ogre::SceneNode* const scene_node); + ~OgreSubmap(); + + OgreSubmap(const OgreSubmap&) = delete; + OgreSubmap& operator=(const OgreSubmap&) = delete; + + // Updates the texture and pose of the submap using new data from + // 'submap_texture'. + void Update(const ::cartographer_ros::SubmapTexture& submap_texture); + + // Changes the opacity of the submap to 'alpha'. + void SetAlpha(float alpha); + + private: + const ::cartographer::mapping::SubmapId id_; + Ogre::SceneManager* const scene_manager_; + Ogre::SceneNode* const scene_node_; + Ogre::SceneNode* const submap_node_; + Ogre::ManualObject* const manual_object_; + Ogre::TexturePtr texture_; + Ogre::MaterialPtr material_; +}; + +} // namespace cartographer_rviz + +#endif // CARTOGRAPHER_RVIZ_SRC_OGRE_SUBMAP_H_