Extract the Ogre related submap code into a separate class. (#444)
Wolfgang Hess
GitHub
parent
e49fecbf11
commit
44459e1810
|
|
@ -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();
|
||||
}
|
||||
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<float>(
|
||||
const float target_alpha = static_cast<float>(
|
||||
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<char> 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
|
||||
|
|
|
|||
|
|
@ -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_);
|
||||
|
|
|
|||
|
|
@ -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 <string>
|
||||
#include <vector>
|
||||
|
||||
#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<char> 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
|
||||
|
|
@ -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_
|
||||
Loading…
Reference in New Issue