12345qiupeng
/
cartographer_ros
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Fix fixed frame issues of the RViz SubmapsDisplay. (#454) 

Adds a 'map_node_' which gets updated with the transform to RViz's
fixed frame. Before, submaps rendered incorrectly if a fixed frame
different from the map frame was chosen.

Removes the map frame property since this data is published on the
submap list topic.

Renames node names to reflect the frames they represent.
master
Wolfgang Hess 2017-07-28 12:10:26 +02:00 committed by GitHub
parent 4bad3c4702
commit 2ee9a77a73
8 changed files with 64 additions and 59 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
cartographer_ros/configuration_files/demo_2d.rviz
View File

@ -133,7 +133,6 @@ Visualization Manager:
Visual Enabled: true
- Class: Submaps
Enabled: true
Map frame: map
Name: Submaps
Submap query service: /submap_query
Topic: /submap_list

1
cartographer_ros/configuration_files/demo_3d.rviz
View File

@ -195,7 +195,6 @@ Visualization Manager:
Value: false
- Class: Submaps
Enabled: true
Map frame: map
Name: Submaps
Submap query service: /submap_query
Topic: /submap_list

28
cartographer_rviz/cartographer_rviz/drawable_submap.cc
View File

@ -46,16 +46,15 @@ constexpr float kAlphaUpdateThreshold = 0.2f;
DrawableSubmap::DrawableSubmap(const ::cartographer::mapping::SubmapId& id,
::rviz::DisplayContext* const display_context,
Ogre::SceneNode* const map_node,
::rviz::Property* const submap_category,
const bool visible, const float pose_axes_length,
const float pose_axes_radius)
: id_(id),
display_context_(display_context),
scene_node_(display_context->getSceneManager()
->getRootSceneNode()
->createChildSceneNode()),
ogre_submap_(id, display_context->getSceneManager(), scene_node_),
pose_axes_(display_context->getSceneManager(), scene_node_,
submap_node_(map_node->createChildSceneNode()),
ogre_submap_(id, display_context->getSceneManager(), submap_node_),
pose_axes_(display_context->getSceneManager(), submap_node_,
pose_axes_length, pose_axes_radius),
last_query_timestamp_(0) {
// DrawableSubmap creates and manages its visibility property object
@ -65,7 +64,7 @@ DrawableSubmap::DrawableSubmap(const ::cartographer::mapping::SubmapId& id,
visibility_ = ::cartographer::common::make_unique<::rviz::BoolProperty>(
"" /* title */, visible, "" /* description */, submap_category,
SLOT(ToggleVisibility()), this);
scene_node_->setVisible(visible);
submap_node_->setVisible(visible);
connect(this, SIGNAL(RequestSucceeded()), this, SLOT(UpdateSceneNode()));
}
@ -75,24 +74,17 @@ DrawableSubmap::~DrawableSubmap() {
if (QueryInProgress()) {
rpc_request_future_.wait();
}
display_context_->getSceneManager()->destroySceneNode(scene_node_);
display_context_->getSceneManager()->destroySceneNode(submap_node_);
}
void DrawableSubmap::Update(
const ::std_msgs::Header& header,
const ::cartographer_ros_msgs::SubmapEntry& metadata,
::rviz::FrameManager* const frame_manager) {
Ogre::Vector3 position;
Ogre::Quaternion orientation;
if (!frame_manager->transform(header, metadata.pose, position, orientation)) {
// We don't know where we would display the texture, so we stop here.
return;
}
const ::cartographer_ros_msgs::SubmapEntry& metadata) {
::cartographer::common::MutexLocker locker(&mutex_);
metadata_version_ = metadata.submap_version;
pose_ = ::cartographer_ros::ToRigid3d(metadata.pose);
scene_node_->setPosition(ToOgre(pose_.translation()));
scene_node_->setOrientation(ToOgre(pose_.rotation()));
submap_node_->setPosition(ToOgre(pose_.translation()));
submap_node_->setOrientation(ToOgre(pose_.rotation()));
display_context_->queueRender();
visibility_->setName(
QString("%1.%2").arg(id_.submap_index).arg(metadata_version_));
@ -164,7 +156,7 @@ void DrawableSubmap::UpdateSceneNode() {
}
void DrawableSubmap::ToggleVisibility() {
scene_node_->setVisible(visibility_->getBool());
submap_node_->setVisible(visibility_->getBool());
display_context_->queueRender();
}

9
cartographer_rviz/cartographer_rviz/drawable_submap.h
View File

@ -47,8 +47,8 @@ class DrawableSubmap : public QObject {
public:
DrawableSubmap(const ::cartographer::mapping::SubmapId& submap_id,
::rviz::DisplayContext* display_context,
::rviz::Property* submap_category, bool visible,
float pose_axes_length, float pose_axes_radius);
Ogre::SceneNode* map_node, ::rviz::Property* submap_category,
bool visible, float pose_axes_length, float pose_axes_radius);
~DrawableSubmap() override;
DrawableSubmap(const DrawableSubmap&) = delete;
DrawableSubmap& operator=(const DrawableSubmap&) = delete;
@ -56,8 +56,7 @@ class DrawableSubmap : public QObject {
// Updates the 'metadata' for this submap. If necessary, the next call to
// MaybeFetchTexture() will fetch a new submap texture.
void Update(const ::std_msgs::Header& header,
const ::cartographer_ros_msgs::SubmapEntry& metadata,
::rviz::FrameManager* frame_manager);
const ::cartographer_ros_msgs::SubmapEntry& metadata);
// If an update is needed, it will send an RPC using 'client' to request the
// new data for the submap and returns true.
@ -91,7 +90,7 @@ class DrawableSubmap : public QObject {
::cartographer::common::Mutex mutex_;
::rviz::DisplayContext* const display_context_;
Ogre::SceneNode* const scene_node_;
Ogre::SceneNode* const submap_node_;
OgreSubmap ogre_submap_;
::cartographer::transform::Rigid3d pose_ GUARDED_BY(mutex_);
::rviz::Axes pose_axes_;

14
cartographer_rviz/cartographer_rviz/ogre_submap.cc
View File

@ -50,11 +50,11 @@ Ogre::Quaternion ToOgre(const Eigen::Quaterniond& q) {
OgreSubmap::OgreSubmap(const ::cartographer::mapping::SubmapId& id,
Ogre::SceneManager* const scene_manager,
Ogre::SceneNode* const scene_node)
Ogre::SceneNode* const submap_node)
: id_(id),
scene_manager_(scene_manager),
scene_node_(scene_node),
submap_node_(scene_node_->createChildSceneNode()),
submap_node_(submap_node),
slice_node_(submap_node_->createChildSceneNode()),
manual_object_(scene_manager_->createManualObject(
kManualObjectPrefix + GetSubmapIdentifier(id))) {
material_ = Ogre::MaterialManager::getSingleton().getByName(
@ -66,7 +66,7 @@ OgreSubmap::OgreSubmap(const ::cartographer::mapping::SubmapId& id,
material_->setCullingMode(Ogre::CULL_NONE);
material_->setDepthBias(-1.f, 0.f);
material_->setDepthWriteEnabled(false);
submap_node_->attachObject(manual_object_);
slice_node_->attachObject(manual_object_);
}
OgreSubmap::~OgreSubmap() {
@ -75,14 +75,14 @@ OgreSubmap::~OgreSubmap() {
Ogre::TextureManager::getSingleton().remove(texture_->getHandle());
texture_.setNull();
}
scene_manager_->destroySceneNode(submap_node_);
scene_manager_->destroySceneNode(slice_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()));
slice_node_->setPosition(ToOgre(submap_texture.slice_pose.translation()));
slice_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;

6
cartographer_rviz/cartographer_rviz/ogre_submap.h
View File

@ -38,11 +38,11 @@ Ogre::Quaternion ToOgre(const Eigen::Quaterniond& q);
// 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
// Attaches a node visualizing the submap 'id' to the 'submap_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);
Ogre::SceneNode* const submap_node);
~OgreSubmap();
OgreSubmap(const OgreSubmap&) = delete;
@ -58,8 +58,8 @@ class OgreSubmap {
private:
const ::cartographer::mapping::SubmapId id_;
Ogre::SceneManager* const scene_manager_;
Ogre::SceneNode* const scene_node_;
Ogre::SceneNode* const submap_node_;
Ogre::SceneNode* const slice_node_;
Ogre::ManualObject* const manual_object_;
Ogre::TexturePtr texture_;
Ogre::MaterialPtr material_;

60
cartographer_rviz/cartographer_rviz/submaps_display.cc
View File

@ -49,9 +49,6 @@ SubmapsDisplay::SubmapsDisplay() : tf_listener_(tf_buffer_) {
submap_query_service_property_ = new ::rviz::StringProperty(
"Submap query service", kDefaultSubmapQueryServiceName,
"Submap query service to connect to.", this, SLOT(Reset()));
map_frame_property_ = new ::rviz::StringProperty(
"Map frame", kDefaultMapFrame, "Map frame, used for fading out submaps.",
this);
tracking_frame_property_ = new ::rviz::StringProperty(
"Tracking frame", kDefaultTrackingFrame,
"Tracking frame, used for fading out submaps.", this);
@ -75,7 +72,11 @@ SubmapsDisplay::SubmapsDisplay() : tf_listener_(tf_buffer_) {
Ogre::ResourceGroupManager::getSingleton().initialiseAllResourceGroups();
}
SubmapsDisplay::~SubmapsDisplay() { client_.shutdown(); }
SubmapsDisplay::~SubmapsDisplay() {
client_.shutdown();
trajectories_.clear();
scene_manager_->destroySceneNode(map_node_);
}
void SubmapsDisplay::Reset() { reset(); }
@ -86,6 +87,7 @@ void SubmapsDisplay::CreateClient() {
void SubmapsDisplay::onInitialize() {
MFDClass::onInitialize();
map_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode();
CreateClient();
}
@ -100,6 +102,8 @@ void SubmapsDisplay::reset() {
void SubmapsDisplay::processMessage(
const ::cartographer_ros_msgs::SubmapList::ConstPtr& msg) {
::cartographer::common::MutexLocker locker(&mutex_);
map_frame_ =
::cartographer::common::make_unique<std::string>(msg->header.frame_id);
// In case Cartographer node is relaunched, destroy trajectories from the
// previous instance.
for (const ::cartographer_ros_msgs::SubmapEntry& submap_entry : msg->submap) {
@ -138,12 +142,11 @@ void SubmapsDisplay::processMessage(
constexpr float kSubmapPoseAxesRadius = 0.06f;
trajectory.emplace(id.submap_index,
::cartographer::common::make_unique<DrawableSubmap>(
id, context_, trajectory_category.get(),
id, context_, map_node_, trajectory_category.get(),
visibility_all_enabled_->getBool(),
kSubmapPoseAxesLength, kSubmapPoseAxesRadius));
}
trajectory.at(id.submap_index)
->Update(msg->header, submap_entry, context_->getFrameManager());
trajectory.at(id.submap_index)->Update(msg->header, submap_entry);
}
// Remove all submaps not mentioned in the SubmapList.
for (size_t trajectory_id = 0; trajectory_id < trajectories_.size();
@ -162,22 +165,6 @@ void SubmapsDisplay::processMessage(
void SubmapsDisplay::update(const float wall_dt, const float ros_dt) {
::cartographer::common::MutexLocker locker(&mutex_);
// Update the fading by z distance.
try {
const ::geometry_msgs::TransformStamped transform_stamped =
tf_buffer_.lookupTransform(map_frame_property_->getStdString(),
tracking_frame_property_->getStdString(),
ros::Time(0) /* latest */);
for (auto& trajectory : trajectories_) {
for (auto& submap_entry : trajectory.second) {
submap_entry.second->SetAlpha(
transform_stamped.transform.translation.z);
}
}
} catch (const tf2::TransformException& ex) {
ROS_WARN("Could not compute submap fading: %s", ex.what());
}
// Schedule fetching of new submap textures.
for (const auto& trajectory : trajectories_) {
int num_ongoing_requests = 0;
@ -195,6 +182,33 @@ void SubmapsDisplay::update(const float wall_dt, const float ros_dt) {
}
}
}
if (map_frame_ == nullptr) {
return;
}
// Update the fading by z distance.
const ros::Time kLatest(0);
try {
const ::geometry_msgs::TransformStamped transform_stamped =
tf_buffer_.lookupTransform(
*map_frame_, tracking_frame_property_->getStdString(), kLatest);
for (auto& trajectory : trajectories_) {
for (auto& submap_entry : trajectory.second) {
submap_entry.second->SetAlpha(
transform_stamped.transform.translation.z);
}
}
} catch (const tf2::TransformException& ex) {
ROS_WARN("Could not compute submap fading: %s", ex.what());
}
// Update the map frame to fixed frame transform.
Ogre::Vector3 position;
Ogre::Quaternion orientation;
if (context_->getFrameManager()->getTransform(*map_frame_, kLatest, position,
orientation)) {
map_node_->setPosition(position);
map_node_->setOrientation(orientation);
context_->queueRender();
}
}
void SubmapsDisplay::AllEnabledToggled() {

4
cartographer_rviz/cartographer_rviz/submaps_display.h
View File

@ -19,6 +19,7 @@
#include <map>
#include <memory>
#include <string>
#include <vector>
#include "cartographer/common/mutex.h"
@ -66,8 +67,9 @@ class SubmapsDisplay
::tf2_ros::TransformListener tf_listener_;
ros::ServiceClient client_;
::rviz::StringProperty* submap_query_service_property_;
::rviz::StringProperty* map_frame_property_;
std::unique_ptr<std::string> map_frame_;
::rviz::StringProperty* tracking_frame_property_;
Ogre::SceneNode* map_node_ = nullptr; // Represents the map frame.
using Trajectory = std::pair<std::unique_ptr<::rviz::Property>,
std::map<int, std::unique_ptr<DrawableSubmap>>>;
std::vector<Trajectory> trajectories_ GUARDED_BY(mutex_);