From 916b4fbac306e76f81033500a86c46afdd668ab6 Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Thu, 4 Aug 2016 17:59:03 +0200 Subject: [PATCH] Fixes bug not displaying all submaps in RViz. (#9) The 'version_' member of a DrawableSubmap is now properly initialized. The redundant 'submap_id' in the SubmapQuery response is removed. The missing slot for reset() is added. --- cartographer_ros/src/cartographer_node_main.cc | 1 - cartographer_ros/src/drawable_submap.cc | 1 + cartographer_ros/src/submaps_display.cc | 4 +++- cartographer_ros/src/submaps_display.h | 1 + cartographer_ros_msgs/srv/SubmapQuery.srv | 1 - 5 files changed, 5 insertions(+), 3 deletions(-) diff --git a/cartographer_ros/src/cartographer_node_main.cc b/cartographer_ros/src/cartographer_node_main.cc index 54a224b..93c14c2 100644 --- a/cartographer_ros/src/cartographer_node_main.cc +++ b/cartographer_ros/src/cartographer_node_main.cc @@ -470,7 +470,6 @@ bool Node::HandleSubmapQuery( sparse_pose_graph_->GetTrajectoryNodes(), submap_transforms[request.submap_id], &response_proto); - response.submap_id = response_proto.submap_id(); response.submap_version = response_proto.submap_version(); response.cells.insert(response.cells.begin(), response_proto.cells().begin(), response_proto.cells().end()); diff --git a/cartographer_ros/src/drawable_submap.cc b/cartographer_ros/src/drawable_submap.cc index bcfb159..7fea94c 100644 --- a/cartographer_ros/src/drawable_submap.cc +++ b/cartographer_ros/src/drawable_submap.cc @@ -68,6 +68,7 @@ DrawableSubmap::DrawableSubmap(const int submap_id, const int trajectory_id, trajectory_id_(trajectory_id), last_query_timestamp_(0), query_in_progress_(false), + version_(0), texture_count_(0), current_alpha_(0) { material_ = Ogre::MaterialManager::getSingleton().getByName( diff --git a/cartographer_ros/src/submaps_display.cc b/cartographer_ros/src/submaps_display.cc index f81be89..4a7d1f8 100644 --- a/cartographer_ros/src/submaps_display.cc +++ b/cartographer_ros/src/submaps_display.cc @@ -54,7 +54,7 @@ SubmapsDisplay::SubmapsDisplay() submap_query_service_property_ = new ::rviz::StringProperty( "Submap query service", QString("/cartographer/") + kSubmapQueryServiceName, - "Submap query service to connect to.", this, SLOT(reset())); + "Submap query service to connect to.", this, SLOT(Reset())); map_frame_property_ = new ::rviz::StringProperty( "Map frame", kDefaultMapFrame, "Map frame, used for fading out submaps.", this); @@ -101,6 +101,8 @@ void SubmapsDisplay::CreateClient() { submap_query_service_property_->getStdString()); } +void SubmapsDisplay::Reset() { reset(); } + void SubmapsDisplay::RequestNewSubmaps() { ::cartographer::common::MutexLocker locker(&mutex_); for (int trajectory_id = 0; trajectory_id < submap_list_.trajectory.size(); diff --git a/cartographer_ros/src/submaps_display.h b/cartographer_ros/src/submaps_display.h index e629d97..f0b032c 100644 --- a/cartographer_ros/src/submaps_display.h +++ b/cartographer_ros/src/submaps_display.h @@ -60,6 +60,7 @@ class SubmapsDisplay void SubmapListUpdated(); private Q_SLOTS: + void Reset(); void RequestNewSubmaps(); private: diff --git a/cartographer_ros_msgs/srv/SubmapQuery.srv b/cartographer_ros_msgs/srv/SubmapQuery.srv index 4463260..d116123 100644 --- a/cartographer_ros_msgs/srv/SubmapQuery.srv +++ b/cartographer_ros_msgs/srv/SubmapQuery.srv @@ -15,7 +15,6 @@ int32 submap_id int32 trajectory_id --- -int32 submap_id int32 submap_version uint8[] cells int32 width