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

Add toggleable trajectories to RViz plugin (#456) 

Implementation of #422 by @ojura. Original feature idea and implementation by @jihoonl.
master
Holger Rapp 2017-07-28 13:46:58 +02:00 committed by GitHub
parent 2ee9a77a73
commit 7e272f2e2b
2 changed files with 64 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

75
cartographer_rviz/cartographer_rviz/submaps_display.cc
View File

@ -53,13 +53,13 @@ SubmapsDisplay::SubmapsDisplay() : tf_listener_(tf_buffer_) {
"Tracking frame", kDefaultTrackingFrame,
"Tracking frame, used for fading out submaps.", this);
client_ = update_nh_.serviceClient<::cartographer_ros_msgs::SubmapQuery>("");
submaps_category_ = new ::rviz::Property(
trajectories_category_ = new ::rviz::Property(
"Submaps", QVariant(), "List of all submaps, organized by trajectories.",
this);
visibility_all_enabled_ = new ::rviz::BoolProperty(
"All Enabled", true,
"Whether all the submaps should be displayed or not.", submaps_category_,
SLOT(AllEnabledToggled()), this);
"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);
Ogre::ResourceGroupManager::getSingleton().addResourceLocation(
package_path + kMaterialsDirectory, "FileSystem", ROS_PACKAGE_NAME);
@ -111,9 +111,9 @@ void SubmapsDisplay::processMessage(
if (trajectory_id >= trajectories_.size()) {
continue;
}
const auto& trajectory = trajectories_[trajectory_id].second;
const auto it = trajectory.find(submap_entry.submap_index);
if (it != trajectory.end() &&
const auto& trajectory_submaps = trajectories_[trajectory_id]->submaps;
const auto it = trajectory_submaps.find(submap_entry.submap_index);
if (it != trajectory_submaps.end() &&
it->second->version() > submap_entry.submap_version) {
// Versions should only increase unless Cartographer restarted.
trajectories_.clear();
@ -126,36 +126,40 @@ void SubmapsDisplay::processMessage(
const SubmapId id{submap_entry.trajectory_id, submap_entry.submap_index};
listed_submaps.insert(id);
while (id.trajectory_id >= static_cast<int>(trajectories_.size())) {
trajectories_.push_back(Trajectory(
::cartographer::common::make_unique<::rviz::Property>(
QString("Trajectory %1").arg(id.trajectory_id), QVariant(),
QString("List of all submaps in Trajectory %1.")
trajectories_.push_back(::cartographer::common::make_unique<Trajectory>(
::cartographer::common::make_unique<::rviz::BoolProperty>(
QString("Trajectory %1").arg(id.trajectory_id),
visibility_all_enabled_->getBool(),
QString("List of all submaps in Trajectory %1. The checkbox "
"controls whether all submaps in this trajectory should "
"be displayed or not.")
.arg(id.trajectory_id),
submaps_category_),
std::map<int, std::unique_ptr<DrawableSubmap>>()));
trajectories_category_)));
}
auto& trajectory_category = trajectories_[id.trajectory_id].first;
auto& trajectory = trajectories_[id.trajectory_id].second;
if (trajectory.count(id.submap_index) == 0) {
auto& trajectory_visibility = trajectories_[id.trajectory_id]->visibility;
auto& trajectory_submaps = trajectories_[id.trajectory_id]->submaps;
if (trajectory_submaps.count(id.submap_index) == 0) {
// TODO(ojura): Add RViz properties for adjusting submap pose axes
constexpr float kSubmapPoseAxesLength = 0.3f;
constexpr float kSubmapPoseAxesRadius = 0.06f;
trajectory.emplace(id.submap_index,
trajectory_submaps.emplace(
id.submap_index,
::cartographer::common::make_unique<DrawableSubmap>(
id, context_, map_node_, trajectory_category.get(),
visibility_all_enabled_->getBool(),
kSubmapPoseAxesLength, kSubmapPoseAxesRadius));
id, context_, map_node_, trajectory_visibility.get(),
trajectory_visibility->getBool(), kSubmapPoseAxesLength,
kSubmapPoseAxesRadius));
}
trajectory.at(id.submap_index)->Update(msg->header, submap_entry);
trajectory_submaps.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();
++trajectory_id) {
auto& trajectory = trajectories_[trajectory_id].second;
for (auto it = trajectory.begin(); it != trajectory.end();) {
auto& trajectory_submaps = trajectories_[trajectory_id]->submaps;
for (auto it = trajectory_submaps.begin();
it != trajectory_submaps.end();) {
if (listed_submaps.count(
SubmapId{static_cast<int>(trajectory_id), it->first}) == 0) {
it = trajectory.erase(it);
it = trajectory_submaps.erase(it);
} else {
++it;
}
@ -168,13 +172,13 @@ void SubmapsDisplay::update(const float wall_dt, const float ros_dt) {
// Schedule fetching of new submap textures.
for (const auto& trajectory : trajectories_) {
int num_ongoing_requests = 0;
for (const auto& submap_entry : trajectory.second) {
for (const auto& submap_entry : trajectory->submaps) {
if (submap_entry.second->QueryInProgress()) {
++num_ongoing_requests;
}
}
for (auto it = trajectory.second.rbegin();
it != trajectory.second.rend() &&
for (auto it = trajectory->submaps.rbegin();
it != trajectory->submaps.rend() &&
num_ongoing_requests < kMaxOnGoingRequestsPerTrajectory;
++it) {
if (it->second->MaybeFetchTexture(&client_)) {
@ -192,7 +196,7 @@ void SubmapsDisplay::update(const float wall_dt, const float ros_dt) {
tf_buffer_.lookupTransform(
*map_frame_, tracking_frame_property_->getStdString(), kLatest);
for (auto& trajectory : trajectories_) {
for (auto& submap_entry : trajectory.second) {
for (auto& submap_entry : trajectory->submaps) {
submap_entry.second->SetAlpha(
transform_stamped.transform.translation.z);
}
@ -213,14 +217,25 @@ void SubmapsDisplay::update(const float wall_dt, const float ros_dt) {
void SubmapsDisplay::AllEnabledToggled() {
::cartographer::common::MutexLocker locker(&mutex_);
const bool visibility = visibility_all_enabled_->getBool();
const bool visible = visibility_all_enabled_->getBool();
for (auto& trajectory : trajectories_) {
for (auto& submap_entry : trajectory.second) {
submap_entry.second->set_visibility(visibility);
trajectory->visibility->setBool(visible);
}
}
void Trajectory::AllEnabledToggled() {
const bool visible = visibility->getBool();
for (auto& submap_entry : submaps) {
submap_entry.second->set_visibility(visible);
}
}
Trajectory::Trajectory(std::unique_ptr<::rviz::BoolProperty> property)
: visibility(std::move(property)) {
::QObject::connect(visibility.get(), SIGNAL(changed()), this,
SLOT(AllEnabledToggled()));
}
} // namespace cartographer_rviz
PLUGINLIB_EXPORT_CLASS(cartographer_rviz::SubmapsDisplay, ::rviz::Display)

21
cartographer_rviz/cartographer_rviz/submaps_display.h
View File

@ -32,6 +32,21 @@
namespace cartographer_rviz {
// This should be a private class in SubmapsDisplay, unfortunately, QT does not
// allow for this.
struct Trajectory : public QObject {
Q_OBJECT
public:
Trajectory(std::unique_ptr<::rviz::BoolProperty> property);
std::unique_ptr<::rviz::BoolProperty> visibility;
std::map<int, std::unique_ptr<DrawableSubmap>> submaps;
private Q_SLOTS:
void AllEnabledToggled();
};
// RViz plugin used for displaying maps which are represented by a collection of
// submaps.
//
@ -70,11 +85,9 @@ class SubmapsDisplay
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_);
std::vector<std::unique_ptr<Trajectory>> trajectories_ GUARDED_BY(mutex_);
::cartographer::common::Mutex mutex_;
::rviz::Property* submaps_category_;
::rviz::Property* trajectories_category_;
::rviz::BoolProperty* visibility_all_enabled_;
};