Add toggleable trajectories to RViz plugin (#456)
Implementation of #422 by @ojura. Original feature idea and implementation by @jihoonl.master
parent
2ee9a77a73
commit
7e272f2e2b
|
@ -53,13 +53,13 @@ SubmapsDisplay::SubmapsDisplay() : tf_listener_(tf_buffer_) {
|
||||||
"Tracking frame", kDefaultTrackingFrame,
|
"Tracking frame", kDefaultTrackingFrame,
|
||||||
"Tracking frame, used for fading out submaps.", this);
|
"Tracking frame, used for fading out submaps.", this);
|
||||||
client_ = update_nh_.serviceClient<::cartographer_ros_msgs::SubmapQuery>("");
|
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.",
|
"Submaps", QVariant(), "List of all submaps, organized by trajectories.",
|
||||||
this);
|
this);
|
||||||
visibility_all_enabled_ = new ::rviz::BoolProperty(
|
visibility_all_enabled_ = new ::rviz::BoolProperty(
|
||||||
"All Enabled", true,
|
"All Enabled", true,
|
||||||
"Whether all the submaps should be displayed or not.", submaps_category_,
|
"Whether submaps from all trajectories should be displayed or not.",
|
||||||
SLOT(AllEnabledToggled()), this);
|
trajectories_category_, SLOT(AllEnabledToggled()), this);
|
||||||
const std::string package_path = ::ros::package::getPath(ROS_PACKAGE_NAME);
|
const std::string package_path = ::ros::package::getPath(ROS_PACKAGE_NAME);
|
||||||
Ogre::ResourceGroupManager::getSingleton().addResourceLocation(
|
Ogre::ResourceGroupManager::getSingleton().addResourceLocation(
|
||||||
package_path + kMaterialsDirectory, "FileSystem", ROS_PACKAGE_NAME);
|
package_path + kMaterialsDirectory, "FileSystem", ROS_PACKAGE_NAME);
|
||||||
|
@ -111,9 +111,9 @@ void SubmapsDisplay::processMessage(
|
||||||
if (trajectory_id >= trajectories_.size()) {
|
if (trajectory_id >= trajectories_.size()) {
|
||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
const auto& trajectory = trajectories_[trajectory_id].second;
|
const auto& trajectory_submaps = trajectories_[trajectory_id]->submaps;
|
||||||
const auto it = trajectory.find(submap_entry.submap_index);
|
const auto it = trajectory_submaps.find(submap_entry.submap_index);
|
||||||
if (it != trajectory.end() &&
|
if (it != trajectory_submaps.end() &&
|
||||||
it->second->version() > submap_entry.submap_version) {
|
it->second->version() > submap_entry.submap_version) {
|
||||||
// Versions should only increase unless Cartographer restarted.
|
// Versions should only increase unless Cartographer restarted.
|
||||||
trajectories_.clear();
|
trajectories_.clear();
|
||||||
|
@ -126,36 +126,40 @@ void SubmapsDisplay::processMessage(
|
||||||
const SubmapId id{submap_entry.trajectory_id, submap_entry.submap_index};
|
const SubmapId id{submap_entry.trajectory_id, submap_entry.submap_index};
|
||||||
listed_submaps.insert(id);
|
listed_submaps.insert(id);
|
||||||
while (id.trajectory_id >= static_cast<int>(trajectories_.size())) {
|
while (id.trajectory_id >= static_cast<int>(trajectories_.size())) {
|
||||||
trajectories_.push_back(Trajectory(
|
trajectories_.push_back(::cartographer::common::make_unique<Trajectory>(
|
||||||
::cartographer::common::make_unique<::rviz::Property>(
|
::cartographer::common::make_unique<::rviz::BoolProperty>(
|
||||||
QString("Trajectory %1").arg(id.trajectory_id), QVariant(),
|
QString("Trajectory %1").arg(id.trajectory_id),
|
||||||
QString("List of all submaps in Trajectory %1.")
|
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),
|
.arg(id.trajectory_id),
|
||||||
submaps_category_),
|
trajectories_category_)));
|
||||||
std::map<int, std::unique_ptr<DrawableSubmap>>()));
|
|
||||||
}
|
}
|
||||||
auto& trajectory_category = trajectories_[id.trajectory_id].first;
|
auto& trajectory_visibility = trajectories_[id.trajectory_id]->visibility;
|
||||||
auto& trajectory = trajectories_[id.trajectory_id].second;
|
auto& trajectory_submaps = trajectories_[id.trajectory_id]->submaps;
|
||||||
if (trajectory.count(id.submap_index) == 0) {
|
if (trajectory_submaps.count(id.submap_index) == 0) {
|
||||||
// TODO(ojura): Add RViz properties for adjusting submap pose axes
|
// TODO(ojura): Add RViz properties for adjusting submap pose axes
|
||||||
constexpr float kSubmapPoseAxesLength = 0.3f;
|
constexpr float kSubmapPoseAxesLength = 0.3f;
|
||||||
constexpr float kSubmapPoseAxesRadius = 0.06f;
|
constexpr float kSubmapPoseAxesRadius = 0.06f;
|
||||||
trajectory.emplace(id.submap_index,
|
trajectory_submaps.emplace(
|
||||||
|
id.submap_index,
|
||||||
::cartographer::common::make_unique<DrawableSubmap>(
|
::cartographer::common::make_unique<DrawableSubmap>(
|
||||||
id, context_, map_node_, trajectory_category.get(),
|
id, context_, map_node_, trajectory_visibility.get(),
|
||||||
visibility_all_enabled_->getBool(),
|
trajectory_visibility->getBool(), kSubmapPoseAxesLength,
|
||||||
kSubmapPoseAxesLength, kSubmapPoseAxesRadius));
|
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.
|
// Remove all submaps not mentioned in the SubmapList.
|
||||||
for (size_t trajectory_id = 0; trajectory_id < trajectories_.size();
|
for (size_t trajectory_id = 0; trajectory_id < trajectories_.size();
|
||||||
++trajectory_id) {
|
++trajectory_id) {
|
||||||
auto& trajectory = trajectories_[trajectory_id].second;
|
auto& trajectory_submaps = trajectories_[trajectory_id]->submaps;
|
||||||
for (auto it = trajectory.begin(); it != trajectory.end();) {
|
for (auto it = trajectory_submaps.begin();
|
||||||
|
it != trajectory_submaps.end();) {
|
||||||
if (listed_submaps.count(
|
if (listed_submaps.count(
|
||||||
SubmapId{static_cast<int>(trajectory_id), it->first}) == 0) {
|
SubmapId{static_cast<int>(trajectory_id), it->first}) == 0) {
|
||||||
it = trajectory.erase(it);
|
it = trajectory_submaps.erase(it);
|
||||||
} else {
|
} else {
|
||||||
++it;
|
++it;
|
||||||
}
|
}
|
||||||
|
@ -168,13 +172,13 @@ void SubmapsDisplay::update(const float wall_dt, const float ros_dt) {
|
||||||
// Schedule fetching of new submap textures.
|
// Schedule fetching of new submap textures.
|
||||||
for (const auto& trajectory : trajectories_) {
|
for (const auto& trajectory : trajectories_) {
|
||||||
int num_ongoing_requests = 0;
|
int num_ongoing_requests = 0;
|
||||||
for (const auto& submap_entry : trajectory.second) {
|
for (const auto& submap_entry : trajectory->submaps) {
|
||||||
if (submap_entry.second->QueryInProgress()) {
|
if (submap_entry.second->QueryInProgress()) {
|
||||||
++num_ongoing_requests;
|
++num_ongoing_requests;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
for (auto it = trajectory.second.rbegin();
|
for (auto it = trajectory->submaps.rbegin();
|
||||||
it != trajectory.second.rend() &&
|
it != trajectory->submaps.rend() &&
|
||||||
num_ongoing_requests < kMaxOnGoingRequestsPerTrajectory;
|
num_ongoing_requests < kMaxOnGoingRequestsPerTrajectory;
|
||||||
++it) {
|
++it) {
|
||||||
if (it->second->MaybeFetchTexture(&client_)) {
|
if (it->second->MaybeFetchTexture(&client_)) {
|
||||||
|
@ -192,7 +196,7 @@ void SubmapsDisplay::update(const float wall_dt, const float ros_dt) {
|
||||||
tf_buffer_.lookupTransform(
|
tf_buffer_.lookupTransform(
|
||||||
*map_frame_, tracking_frame_property_->getStdString(), kLatest);
|
*map_frame_, tracking_frame_property_->getStdString(), kLatest);
|
||||||
for (auto& trajectory : trajectories_) {
|
for (auto& trajectory : trajectories_) {
|
||||||
for (auto& submap_entry : trajectory.second) {
|
for (auto& submap_entry : trajectory->submaps) {
|
||||||
submap_entry.second->SetAlpha(
|
submap_entry.second->SetAlpha(
|
||||||
transform_stamped.transform.translation.z);
|
transform_stamped.transform.translation.z);
|
||||||
}
|
}
|
||||||
|
@ -213,12 +217,23 @@ void SubmapsDisplay::update(const float wall_dt, const float ros_dt) {
|
||||||
|
|
||||||
void SubmapsDisplay::AllEnabledToggled() {
|
void SubmapsDisplay::AllEnabledToggled() {
|
||||||
::cartographer::common::MutexLocker locker(&mutex_);
|
::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& trajectory : trajectories_) {
|
||||||
for (auto& submap_entry : trajectory.second) {
|
trajectory->visibility->setBool(visible);
|
||||||
submap_entry.second->set_visibility(visibility);
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
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
|
} // namespace cartographer_rviz
|
||||||
|
|
|
@ -32,6 +32,21 @@
|
||||||
|
|
||||||
namespace cartographer_rviz {
|
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
|
// RViz plugin used for displaying maps which are represented by a collection of
|
||||||
// submaps.
|
// submaps.
|
||||||
//
|
//
|
||||||
|
@ -70,11 +85,9 @@ class SubmapsDisplay
|
||||||
std::unique_ptr<std::string> map_frame_;
|
std::unique_ptr<std::string> map_frame_;
|
||||||
::rviz::StringProperty* tracking_frame_property_;
|
::rviz::StringProperty* tracking_frame_property_;
|
||||||
Ogre::SceneNode* map_node_ = nullptr; // Represents the map frame.
|
Ogre::SceneNode* map_node_ = nullptr; // Represents the map frame.
|
||||||
using Trajectory = std::pair<std::unique_ptr<::rviz::Property>,
|
std::vector<std::unique_ptr<Trajectory>> trajectories_ GUARDED_BY(mutex_);
|
||||||
std::map<int, std::unique_ptr<DrawableSubmap>>>;
|
|
||||||
std::vector<Trajectory> trajectories_ GUARDED_BY(mutex_);
|
|
||||||
::cartographer::common::Mutex mutex_;
|
::cartographer::common::Mutex mutex_;
|
||||||
::rviz::Property* submaps_category_;
|
::rviz::Property* trajectories_category_;
|
||||||
::rviz::BoolProperty* visibility_all_enabled_;
|
::rviz::BoolProperty* visibility_all_enabled_;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue