Handle trimmed submaps in the visualization. (#389)

Related to googlecartographer/cartographer#283.

PAIR=SirVer
master
Wolfgang Hess 2017-06-21 16:36:01 +02:00 committed by GitHub
parent 9244ada458
commit 1eca669024
10 changed files with 72 additions and 63 deletions

View File

@ -20,7 +20,6 @@
#include "cartographer_ros/color.h" #include "cartographer_ros/color.h"
#include "cartographer_ros/msg_conversion.h" #include "cartographer_ros/msg_conversion.h"
#include "cartographer_ros/occupancy_grid.h" #include "cartographer_ros/occupancy_grid.h"
#include "cartographer_ros_msgs/TrajectorySubmapList.h"
namespace cartographer_ros { namespace cartographer_ros {
@ -133,14 +132,19 @@ cartographer_ros_msgs::SubmapList MapBuilderBridge::GetSubmapList() {
map_builder_.sparse_pose_graph()->GetAllSubmapData(); map_builder_.sparse_pose_graph()->GetAllSubmapData();
for (size_t trajectory_id = 0; trajectory_id < all_submap_data.size(); for (size_t trajectory_id = 0; trajectory_id < all_submap_data.size();
++trajectory_id) { ++trajectory_id) {
cartographer_ros_msgs::TrajectorySubmapList trajectory_submap_list; for (size_t submap_index = 0;
for (const auto& submap_data : all_submap_data[trajectory_id]) { submap_index < all_submap_data[trajectory_id].size(); ++submap_index) {
const auto& submap_data = all_submap_data[trajectory_id][submap_index];
if (submap_data.submap == nullptr) {
continue;
}
cartographer_ros_msgs::SubmapEntry submap_entry; cartographer_ros_msgs::SubmapEntry submap_entry;
submap_entry.trajectory_id = trajectory_id;
submap_entry.submap_index = submap_index;
submap_entry.submap_version = submap_data.submap->num_range_data(); submap_entry.submap_version = submap_data.submap->num_range_data();
submap_entry.pose = ToGeometryMsgPose(submap_data.pose); submap_entry.pose = ToGeometryMsgPose(submap_data.pose);
trajectory_submap_list.submap.push_back(submap_entry); submap_list.submap.push_back(submap_entry);
} }
submap_list.trajectory.push_back(trajectory_submap_list);
} }
return submap_list; return submap_list;
} }

View File

@ -31,7 +31,6 @@
#include "cartographer_ros_msgs/SubmapList.h" #include "cartographer_ros_msgs/SubmapList.h"
#include "cartographer_ros_msgs/SubmapQuery.h" #include "cartographer_ros_msgs/SubmapQuery.h"
#include "cartographer_ros_msgs/TrajectoryOptions.h" #include "cartographer_ros_msgs/TrajectoryOptions.h"
#include "cartographer_ros_msgs/TrajectorySubmapList.h"
#include "cartographer_ros_msgs/WriteAssets.h" #include "cartographer_ros_msgs/WriteAssets.h"
#include "ros/ros.h" #include "ros/ros.h"
#include "tf2_ros/transform_broadcaster.h" #include "tf2_ros/transform_broadcaster.h"

View File

@ -244,8 +244,8 @@ void Run(const std::vector<string>& bag_filenames) {
::ros::spinOnce(); ::ros::spinOnce();
LOG_EVERY_N(INFO, 100000) LOG_EVERY_N(INFO, 100000)
<< "Processed " << (delayed_msg.getTime() - begin_time).toSec() << " of " << "Processed " << (delayed_msg.getTime() - begin_time).toSec()
<< duration_in_seconds << " bag time seconds..."; << " of " << duration_in_seconds << " bag time seconds...";
delayed_messages.pop_front(); delayed_messages.pop_front();
} }
@ -256,7 +256,6 @@ void Run(const std::vector<string>& bag_filenames) {
continue; continue;
} }
delayed_messages.push_back(msg); delayed_messages.push_back(msg);
} }
bag.close(); bag.close();

View File

@ -25,7 +25,6 @@ find_package(catkin REQUIRED COMPONENTS message_generation ${PACKAGE_DEPENDENCIE
add_message_files( add_message_files(
FILES FILES
SubmapList.msg SubmapList.msg
TrajectorySubmapList.msg
SubmapEntry.msg SubmapEntry.msg
SensorTopics.msg SensorTopics.msg
TrajectoryOptions.msg TrajectoryOptions.msg

View File

@ -12,5 +12,7 @@
# See the License for the specific language governing permissions and # See the License for the specific language governing permissions and
# limitations under the License. # limitations under the License.
int32 trajectory_id
int32 submap_index
int32 submap_version int32 submap_version
geometry_msgs/Pose pose geometry_msgs/Pose pose

View File

@ -13,4 +13,4 @@
# limitations under the License. # limitations under the License.
std_msgs/Header header std_msgs/Header header
TrajectorySubmapList[] trajectory SubmapEntry[] submap

View File

@ -1,15 +0,0 @@
# 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.
SubmapEntry[] submap

View File

@ -72,6 +72,7 @@ class DrawableSubmap : public QObject {
int submap_index() const { return submap_index_; } int submap_index() const { return submap_index_; }
int trajectory_id() const { return trajectory_id_; } int trajectory_id() const { return trajectory_id_; }
int version() const { return metadata_version_; }
bool visibility() const { return visibility_->getBool(); } bool visibility() const { return visibility_->getBool(); }
void set_visibility(const bool visibility) { void set_visibility(const bool visibility) {
visibility_->setBool(visibility); visibility_->setBool(visibility);

View File

@ -19,6 +19,7 @@
#include "OgreResourceGroupManager.h" #include "OgreResourceGroupManager.h"
#include "cartographer/common/make_unique.h" #include "cartographer/common/make_unique.h"
#include "cartographer/common/mutex.h" #include "cartographer/common/mutex.h"
#include "cartographer/mapping/id.h"
#include "cartographer_ros_msgs/SubmapList.h" #include "cartographer_ros_msgs/SubmapList.h"
#include "cartographer_ros_msgs/SubmapQuery.h" #include "cartographer_ros_msgs/SubmapQuery.h"
#include "geometry_msgs/TransformStamped.h" #include "geometry_msgs/TransformStamped.h"
@ -99,44 +100,61 @@ void SubmapsDisplay::reset() {
void SubmapsDisplay::processMessage( void SubmapsDisplay::processMessage(
const ::cartographer_ros_msgs::SubmapList::ConstPtr& msg) { const ::cartographer_ros_msgs::SubmapList::ConstPtr& msg) {
::cartographer::common::MutexLocker locker(&mutex_); ::cartographer::common::MutexLocker locker(&mutex_);
// In case Cartographer node is relaunched, destroy // In case Cartographer node is relaunched, destroy trajectories from the
// trajectories from the previous instance // previous instance.
if (msg->trajectory.size() < trajectories_.size()) { for (const ::cartographer_ros_msgs::SubmapEntry& submap_entry : msg->submap) {
trajectories_.clear(); const size_t trajectory_id = submap_entry.trajectory_id;
}
for (size_t trajectory_id = 0; trajectory_id < msg->trajectory.size();
++trajectory_id) {
if (trajectory_id >= trajectories_.size()) { if (trajectory_id >= trajectories_.size()) {
// When a trajectory is destroyed, it also needs to delete its rviz continue;
// Property object, so we use a unique_ptr for it }
const auto& trajectory = trajectories_[trajectory_id].second;
const auto it = trajectory.find(submap_entry.submap_index);
if (it != trajectory.end() &&
it->second->version() > submap_entry.submap_version) {
// Versions should only increase unless Cartographer restarted.
trajectories_.clear();
break;
}
}
using ::cartographer::mapping::SubmapId;
std::set<SubmapId> listed_submaps;
for (const ::cartographer_ros_msgs::SubmapEntry& submap_entry : msg->submap) {
listed_submaps.insert(
SubmapId{submap_entry.trajectory_id, submap_entry.submap_index});
const size_t trajectory_id = submap_entry.trajectory_id;
while (trajectory_id >= trajectories_.size()) {
trajectories_.push_back(Trajectory( trajectories_.push_back(Trajectory(
::cartographer::common::make_unique<::rviz::Property>( ::cartographer::common::make_unique<::rviz::Property>(
QString("Trajectory %1").arg(trajectory_id), QVariant(), QString("Trajectory %1").arg(trajectory_id), QVariant(),
QString("List of all submaps in Trajectory %1.") QString("List of all submaps in Trajectory %1.")
.arg(trajectory_id), .arg(trajectory_id),
submaps_category_), submaps_category_),
std::vector<std::unique_ptr<DrawableSubmap>>())); std::map<int, std::unique_ptr<DrawableSubmap>>()));
} }
auto& trajectory_category = trajectories_[trajectory_id].first; auto& trajectory_category = trajectories_[trajectory_id].first;
auto& trajectory = trajectories_[trajectory_id].second; auto& trajectory = trajectories_[trajectory_id].second;
const std::vector<::cartographer_ros_msgs::SubmapEntry>& submap_entries = const int submap_index = submap_entry.submap_index;
msg->trajectory[trajectory_id].submap; if (trajectory.count(submap_index) == 0) {
// Same as above, destroy the whole trajectory if we detect that trajectory.emplace(
// we have more submaps than we should submap_index,
if (submap_entries.size() < trajectory.size()) {
trajectory.clear();
}
for (size_t submap_index = 0; submap_index < submap_entries.size();
++submap_index) {
if (submap_index >= trajectory.size()) {
trajectory.push_back(
::cartographer::common::make_unique<DrawableSubmap>( ::cartographer::common::make_unique<DrawableSubmap>(
trajectory_id, submap_index, context_->getSceneManager(), trajectory_id, submap_index, context_->getSceneManager(),
trajectory_category.get(), visibility_all_enabled_->getBool())); trajectory_category.get(), visibility_all_enabled_->getBool()));
} }
trajectory[submap_index]->Update(msg->header, trajectory.at(submap_index)
submap_entries[submap_index], ->Update(msg->header, submap_entry, context_->getFrameManager());
context_->getFrameManager()); }
// 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();) {
if (listed_submaps.count(
SubmapId{static_cast<int>(trajectory_id), it->first}) == 0) {
it = trajectory.erase(it);
} else {
++it;
}
} }
} }
} }
@ -150,8 +168,9 @@ void SubmapsDisplay::update(const float wall_dt, const float ros_dt) {
tracking_frame_property_->getStdString(), tracking_frame_property_->getStdString(),
ros::Time(0) /* latest */); ros::Time(0) /* latest */);
for (auto& trajectory : trajectories_) { for (auto& trajectory : trajectories_) {
for (auto& submap : trajectory.second) { for (auto& submap_entry : trajectory.second) {
submap->SetAlpha(transform_stamped.transform.translation.z); submap_entry.second->SetAlpha(
transform_stamped.transform.translation.z);
} }
} }
} catch (const tf2::TransformException& ex) { } catch (const tf2::TransformException& ex) {
@ -161,16 +180,16 @@ 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 : trajectory.second) { for (const auto& submap_entry : trajectory.second) {
if (submap->QueryInProgress()) { if (submap_entry.second->QueryInProgress()) {
++num_ongoing_requests; ++num_ongoing_requests;
} }
} }
for (int submap_index = static_cast<int>(trajectory.second.size()) - 1; for (auto it = trajectory.second.rbegin();
submap_index >= 0 && it != trajectory.second.rend() &&
num_ongoing_requests < kMaxOnGoingRequestsPerTrajectory; num_ongoing_requests < kMaxOnGoingRequestsPerTrajectory;
--submap_index) { ++it) {
if (trajectory.second[submap_index]->MaybeFetchTexture(&client_)) { if (it->second->MaybeFetchTexture(&client_)) {
++num_ongoing_requests; ++num_ongoing_requests;
} }
} }
@ -181,8 +200,8 @@ void SubmapsDisplay::AllEnabledToggled() {
::cartographer::common::MutexLocker locker(&mutex_); ::cartographer::common::MutexLocker locker(&mutex_);
const bool visibility = visibility_all_enabled_->getBool(); const bool visibility = visibility_all_enabled_->getBool();
for (auto& trajectory : trajectories_) { for (auto& trajectory : trajectories_) {
for (auto& submap : trajectory.second) { for (auto& submap_entry : trajectory.second) {
submap->set_visibility(visibility); submap_entry.second->set_visibility(visibility);
} }
} }
} }

View File

@ -17,6 +17,7 @@
#ifndef CARTOGRAPHER_RVIZ_SRC_SUBMAPS_DISPLAY_H_ #ifndef CARTOGRAPHER_RVIZ_SRC_SUBMAPS_DISPLAY_H_
#define CARTOGRAPHER_RVIZ_SRC_SUBMAPS_DISPLAY_H_ #define CARTOGRAPHER_RVIZ_SRC_SUBMAPS_DISPLAY_H_
#include <map>
#include <memory> #include <memory>
#include <vector> #include <vector>
@ -68,7 +69,7 @@ class SubmapsDisplay
::rviz::StringProperty* map_frame_property_; ::rviz::StringProperty* map_frame_property_;
::rviz::StringProperty* tracking_frame_property_; ::rviz::StringProperty* tracking_frame_property_;
using Trajectory = std::pair<std::unique_ptr<::rviz::Property>, using Trajectory = std::pair<std::unique_ptr<::rviz::Property>,
std::vector<std::unique_ptr<DrawableSubmap>>>; std::map<int, std::unique_ptr<DrawableSubmap>>>;
std::vector<Trajectory> trajectories_ GUARDED_BY(mutex_); std::vector<Trajectory> trajectories_ GUARDED_BY(mutex_);
::cartographer::common::Mutex mutex_; ::cartographer::common::Mutex mutex_;
::rviz::Property* submaps_category_; ::rviz::Property* submaps_category_;