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

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
cartographer_ros/cartographer_ros/map_builder_bridge.cc
View File

@ -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;
}

1
cartographer_ros/cartographer_ros/node.h
View File

@ -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"

5
cartographer_ros/cartographer_ros/offline_node_main.cc
View File

@ -244,8 +244,8 @@ void Run(const std::vector<string>& 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<string>& bag_filenames) {
continue;
}
delayed_messages.push_back(msg);
}
bag.close();

1
cartographer_ros_msgs/CMakeLists.txt
View File

@ -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

2
cartographer_ros_msgs/msg/SubmapEntry.msg
View File

@ -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

2
cartographer_ros_msgs/msg/SubmapList.msg
View File

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

15
cartographer_ros_msgs/msg/TrajectorySubmapList.msg
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

1
cartographer_rviz/cartographer_rviz/drawable_submap.h
View File

@ -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);

91
cartographer_rviz/cartographer_rviz/submaps_display.cc
View File

@ -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<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(
::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::unique_ptr<DrawableSubmap>>()));
std::map<int, std::unique_ptr<DrawableSubmap>>()));
}
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<DrawableSubmap>(
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<DrawableSubmap>(
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<int>(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<int>(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);
}
}
}

3
cartographer_rviz/cartographer_rviz/submaps_display.h
View File

@ -17,6 +17,7 @@
#ifndef CARTOGRAPHER_RVIZ_SRC_SUBMAPS_DISPLAY_H_
#define CARTOGRAPHER_RVIZ_SRC_SUBMAPS_DISPLAY_H_
#include <map>
#include <memory>
#include <vector>
@ -68,7 +69,7 @@ class SubmapsDisplay
::rviz::StringProperty* map_frame_property_;
::rviz::StringProperty* tracking_frame_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_);
::cartographer::common::Mutex mutex_;
::rviz::Property* submaps_category_;