From 1eca66902405f23498e58748fb704afd56dd0d0a Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Wed, 21 Jun 2017 16:36:01 +0200 Subject: [PATCH] Handle trimmed submaps in the visualization. (#389) Related to googlecartographer/cartographer#283. PAIR=SirVer --- .../cartographer_ros/map_builder_bridge.cc | 14 ++- cartographer_ros/cartographer_ros/node.h | 1 - .../cartographer_ros/offline_node_main.cc | 5 +- cartographer_ros_msgs/CMakeLists.txt | 1 - cartographer_ros_msgs/msg/SubmapEntry.msg | 2 + cartographer_ros_msgs/msg/SubmapList.msg | 2 +- .../msg/TrajectorySubmapList.msg | 15 --- .../cartographer_rviz/drawable_submap.h | 1 + .../cartographer_rviz/submaps_display.cc | 91 +++++++++++-------- .../cartographer_rviz/submaps_display.h | 3 +- 10 files changed, 72 insertions(+), 63 deletions(-) delete mode 100644 cartographer_ros_msgs/msg/TrajectorySubmapList.msg diff --git a/cartographer_ros/cartographer_ros/map_builder_bridge.cc b/cartographer_ros/cartographer_ros/map_builder_bridge.cc index 201f824..14e2c04 100644 --- a/cartographer_ros/cartographer_ros/map_builder_bridge.cc +++ b/cartographer_ros/cartographer_ros/map_builder_bridge.cc @@ -20,7 +20,6 @@ #include "cartographer_ros/color.h" #include "cartographer_ros/msg_conversion.h" #include "cartographer_ros/occupancy_grid.h" -#include "cartographer_ros_msgs/TrajectorySubmapList.h" namespace cartographer_ros { @@ -133,14 +132,19 @@ cartographer_ros_msgs::SubmapList MapBuilderBridge::GetSubmapList() { map_builder_.sparse_pose_graph()->GetAllSubmapData(); for (size_t trajectory_id = 0; trajectory_id < all_submap_data.size(); ++trajectory_id) { - cartographer_ros_msgs::TrajectorySubmapList trajectory_submap_list; - for (const auto& submap_data : all_submap_data[trajectory_id]) { + for (size_t submap_index = 0; + 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; + submap_entry.trajectory_id = trajectory_id; + submap_entry.submap_index = submap_index; submap_entry.submap_version = submap_data.submap->num_range_data(); 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; } diff --git a/cartographer_ros/cartographer_ros/node.h b/cartographer_ros/cartographer_ros/node.h index 8f0ab09..7dc792a 100644 --- a/cartographer_ros/cartographer_ros/node.h +++ b/cartographer_ros/cartographer_ros/node.h @@ -31,7 +31,6 @@ #include "cartographer_ros_msgs/SubmapList.h" #include "cartographer_ros_msgs/SubmapQuery.h" #include "cartographer_ros_msgs/TrajectoryOptions.h" -#include "cartographer_ros_msgs/TrajectorySubmapList.h" #include "cartographer_ros_msgs/WriteAssets.h" #include "ros/ros.h" #include "tf2_ros/transform_broadcaster.h" diff --git a/cartographer_ros/cartographer_ros/offline_node_main.cc b/cartographer_ros/cartographer_ros/offline_node_main.cc index 8d86601..fe94950 100644 --- a/cartographer_ros/cartographer_ros/offline_node_main.cc +++ b/cartographer_ros/cartographer_ros/offline_node_main.cc @@ -244,8 +244,8 @@ void Run(const std::vector& bag_filenames) { ::ros::spinOnce(); LOG_EVERY_N(INFO, 100000) - << "Processed " << (delayed_msg.getTime() - begin_time).toSec() << " of " - << duration_in_seconds << " bag time seconds..."; + << "Processed " << (delayed_msg.getTime() - begin_time).toSec() + << " of " << duration_in_seconds << " bag time seconds..."; delayed_messages.pop_front(); } @@ -256,7 +256,6 @@ void Run(const std::vector& bag_filenames) { continue; } delayed_messages.push_back(msg); - } bag.close(); diff --git a/cartographer_ros_msgs/CMakeLists.txt b/cartographer_ros_msgs/CMakeLists.txt index 8cb981a..e24ae77 100644 --- a/cartographer_ros_msgs/CMakeLists.txt +++ b/cartographer_ros_msgs/CMakeLists.txt @@ -25,7 +25,6 @@ find_package(catkin REQUIRED COMPONENTS message_generation ${PACKAGE_DEPENDENCIE add_message_files( FILES SubmapList.msg - TrajectorySubmapList.msg SubmapEntry.msg SensorTopics.msg TrajectoryOptions.msg diff --git a/cartographer_ros_msgs/msg/SubmapEntry.msg b/cartographer_ros_msgs/msg/SubmapEntry.msg index 6cbd4ea..4ea51f9 100644 --- a/cartographer_ros_msgs/msg/SubmapEntry.msg +++ b/cartographer_ros_msgs/msg/SubmapEntry.msg @@ -12,5 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. +int32 trajectory_id +int32 submap_index int32 submap_version geometry_msgs/Pose pose diff --git a/cartographer_ros_msgs/msg/SubmapList.msg b/cartographer_ros_msgs/msg/SubmapList.msg index 9f8e34c..6bab201 100644 --- a/cartographer_ros_msgs/msg/SubmapList.msg +++ b/cartographer_ros_msgs/msg/SubmapList.msg @@ -13,4 +13,4 @@ # limitations under the License. std_msgs/Header header -TrajectorySubmapList[] trajectory +SubmapEntry[] submap diff --git a/cartographer_ros_msgs/msg/TrajectorySubmapList.msg b/cartographer_ros_msgs/msg/TrajectorySubmapList.msg deleted file mode 100644 index 678a886..0000000 --- a/cartographer_ros_msgs/msg/TrajectorySubmapList.msg +++ /dev/null @@ -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 diff --git a/cartographer_rviz/cartographer_rviz/drawable_submap.h b/cartographer_rviz/cartographer_rviz/drawable_submap.h index 8824028..bbdfde4 100644 --- a/cartographer_rviz/cartographer_rviz/drawable_submap.h +++ b/cartographer_rviz/cartographer_rviz/drawable_submap.h @@ -72,6 +72,7 @@ class DrawableSubmap : public QObject { int submap_index() const { return submap_index_; } int trajectory_id() const { return trajectory_id_; } + int version() const { return metadata_version_; } bool visibility() const { return visibility_->getBool(); } void set_visibility(const bool visibility) { visibility_->setBool(visibility); diff --git a/cartographer_rviz/cartographer_rviz/submaps_display.cc b/cartographer_rviz/cartographer_rviz/submaps_display.cc index 338fe8e..9b23318 100644 --- a/cartographer_rviz/cartographer_rviz/submaps_display.cc +++ b/cartographer_rviz/cartographer_rviz/submaps_display.cc @@ -19,6 +19,7 @@ #include "OgreResourceGroupManager.h" #include "cartographer/common/make_unique.h" #include "cartographer/common/mutex.h" +#include "cartographer/mapping/id.h" #include "cartographer_ros_msgs/SubmapList.h" #include "cartographer_ros_msgs/SubmapQuery.h" #include "geometry_msgs/TransformStamped.h" @@ -99,44 +100,61 @@ void SubmapsDisplay::reset() { void SubmapsDisplay::processMessage( const ::cartographer_ros_msgs::SubmapList::ConstPtr& msg) { ::cartographer::common::MutexLocker locker(&mutex_); - // In case Cartographer node is relaunched, destroy - // trajectories from the previous instance - if (msg->trajectory.size() < trajectories_.size()) { - trajectories_.clear(); - } - for (size_t trajectory_id = 0; trajectory_id < msg->trajectory.size(); - ++trajectory_id) { + // In case Cartographer node is relaunched, destroy trajectories from the + // previous instance. + for (const ::cartographer_ros_msgs::SubmapEntry& submap_entry : msg->submap) { + const size_t trajectory_id = submap_entry.trajectory_id; if (trajectory_id >= trajectories_.size()) { - // When a trajectory is destroyed, it also needs to delete its rviz - // Property object, so we use a unique_ptr for it + continue; + } + 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 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( ::cartographer::common::make_unique<::rviz::Property>( QString("Trajectory %1").arg(trajectory_id), QVariant(), QString("List of all submaps in Trajectory %1.") .arg(trajectory_id), submaps_category_), - std::vector>())); + std::map>())); } auto& trajectory_category = trajectories_[trajectory_id].first; auto& trajectory = trajectories_[trajectory_id].second; - const std::vector<::cartographer_ros_msgs::SubmapEntry>& submap_entries = - msg->trajectory[trajectory_id].submap; - // Same as above, destroy the whole trajectory if we detect that - // we have more submaps than we should - if (submap_entries.size() < trajectory.size()) { - trajectory.clear(); + const int submap_index = submap_entry.submap_index; + if (trajectory.count(submap_index) == 0) { + trajectory.emplace( + submap_index, + ::cartographer::common::make_unique( + trajectory_id, submap_index, context_->getSceneManager(), + trajectory_category.get(), visibility_all_enabled_->getBool())); } - 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( - trajectory_id, submap_index, context_->getSceneManager(), - trajectory_category.get(), visibility_all_enabled_->getBool())); + trajectory.at(submap_index) + ->Update(msg->header, submap_entry, 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(trajectory_id), it->first}) == 0) { + it = trajectory.erase(it); + } else { + ++it; } - trajectory[submap_index]->Update(msg->header, - submap_entries[submap_index], - context_->getFrameManager()); } } } @@ -150,8 +168,9 @@ void SubmapsDisplay::update(const float wall_dt, const float ros_dt) { tracking_frame_property_->getStdString(), ros::Time(0) /* latest */); for (auto& trajectory : trajectories_) { - for (auto& submap : trajectory.second) { - submap->SetAlpha(transform_stamped.transform.translation.z); + for (auto& submap_entry : trajectory.second) { + submap_entry.second->SetAlpha( + transform_stamped.transform.translation.z); } } } 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. for (const auto& trajectory : trajectories_) { int num_ongoing_requests = 0; - for (const auto& submap : trajectory.second) { - if (submap->QueryInProgress()) { + for (const auto& submap_entry : trajectory.second) { + if (submap_entry.second->QueryInProgress()) { ++num_ongoing_requests; } } - for (int submap_index = static_cast(trajectory.second.size()) - 1; - submap_index >= 0 && + for (auto it = trajectory.second.rbegin(); + it != trajectory.second.rend() && num_ongoing_requests < kMaxOnGoingRequestsPerTrajectory; - --submap_index) { - if (trajectory.second[submap_index]->MaybeFetchTexture(&client_)) { + ++it) { + if (it->second->MaybeFetchTexture(&client_)) { ++num_ongoing_requests; } } @@ -181,8 +200,8 @@ void SubmapsDisplay::AllEnabledToggled() { ::cartographer::common::MutexLocker locker(&mutex_); const bool visibility = visibility_all_enabled_->getBool(); for (auto& trajectory : trajectories_) { - for (auto& submap : trajectory.second) { - submap->set_visibility(visibility); + for (auto& submap_entry : trajectory.second) { + submap_entry.second->set_visibility(visibility); } } } diff --git a/cartographer_rviz/cartographer_rviz/submaps_display.h b/cartographer_rviz/cartographer_rviz/submaps_display.h index 83e5238..a61cbd0 100644 --- a/cartographer_rviz/cartographer_rviz/submaps_display.h +++ b/cartographer_rviz/cartographer_rviz/submaps_display.h @@ -17,6 +17,7 @@ #ifndef CARTOGRAPHER_RVIZ_SRC_SUBMAPS_DISPLAY_H_ #define CARTOGRAPHER_RVIZ_SRC_SUBMAPS_DISPLAY_H_ +#include #include #include @@ -68,7 +69,7 @@ class SubmapsDisplay ::rviz::StringProperty* map_frame_property_; ::rviz::StringProperty* tracking_frame_property_; using Trajectory = std::pair, - std::vector>>; + std::map>>; std::vector trajectories_ GUARDED_BY(mutex_); ::cartographer::common::Mutex mutex_; ::rviz::Property* submaps_category_;