diff --git a/cartographer_ros/CMakeLists.txt b/cartographer_ros/CMakeLists.txt index 178bd90..f5a97ee 100644 --- a/cartographer_ros/CMakeLists.txt +++ b/cartographer_ros/CMakeLists.txt @@ -100,8 +100,6 @@ add_library(cartographer_rviz_submaps_visualization src/node_constants.h src/submaps_display.cc src/submaps_display.h - src/trajectory.cc - src/trajectory.h ) target_link_libraries(cartographer_rviz_submaps_visualization ${Boost_LIBRARIES} diff --git a/cartographer_ros/src/cartographer_node_main.cc b/cartographer_ros/src/cartographer_node_main.cc index 942d027..02f0048 100644 --- a/cartographer_ros/src/cartographer_node_main.cc +++ b/cartographer_ros/src/cartographer_node_main.cc @@ -496,7 +496,7 @@ bool Node::HandleSubmapQuery( return true; } -void Node::PublishSubmapList(int64 timestamp) { +void Node::PublishSubmapList(const int64 timestamp) { ::cartographer::common::MutexLocker lock(&mutex_); const ::cartographer::mapping::Submaps* submaps = trajectory_builder_->submaps(); @@ -513,12 +513,15 @@ void Node::PublishSubmapList(int64 timestamp) { } ::cartographer_ros_msgs::SubmapList ros_submap_list; + ros_submap_list.header.stamp = + ToRos(::cartographer::common::FromUniversal(timestamp)); + ros_submap_list.header.frame_id = map_frame_; ros_submap_list.trajectory.push_back(ros_trajectory); submap_list_publisher_.publish(ros_submap_list); last_submap_list_publish_timestamp_ = timestamp; } -void Node::PublishPose(int64 timestamp) { +void Node::PublishPose(const int64 timestamp) { const ::cartographer::common::Time time = ::cartographer::common::FromUniversal(timestamp); const Rigid3d tracking_to_map = trajectory_builder_->pose_estimate().pose; diff --git a/cartographer_ros/src/submaps_display.cc b/cartographer_ros/src/submaps_display.cc index 35f8493..f81be89 100644 --- a/cartographer_ros/src/submaps_display.cc +++ b/cartographer_ros/src/submaps_display.cc @@ -18,6 +18,7 @@ #include #include +#include #include #include #include @@ -47,21 +48,13 @@ constexpr char kDefaultTrackingFrame[] = "base_link"; } // namespace SubmapsDisplay::SubmapsDisplay() - : Display(), - scene_manager_listener_([this]() { UpdateMapTexture(); }), + : scene_manager_listener_([this]() { UpdateMapTexture(); }), tf_listener_(tf_buffer_) { connect(this, SIGNAL(SubmapListUpdated()), this, SLOT(RequestNewSubmaps())); - topic_property_ = new ::rviz::RosTopicProperty( - "Topic", QString("/cartographer/") + kSubmapListTopic, - QString::fromStdString( - ros::message_traits::datatype<::cartographer_ros_msgs::SubmapList>()), - "cartographer_ros_msgs::SubmapList topic to subscribe to.", this, - SLOT(UpdateSubmapTopicOrService())); submap_query_service_property_ = new ::rviz::StringProperty( "Submap query service", QString("/cartographer/") + kSubmapQueryServiceName, - "Submap query service to connect to.", this, - SLOT(UpdateSubmapTopicOrService())); + "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); @@ -81,60 +74,31 @@ SubmapsDisplay::SubmapsDisplay() Ogre::ResourceGroupManager::getSingleton().initialiseAllResourceGroups(); } -SubmapsDisplay::~SubmapsDisplay() { UnsubscribeAndClear(); } +SubmapsDisplay::~SubmapsDisplay() { client_.shutdown(); } void SubmapsDisplay::onInitialize() { + MFDClass::onInitialize(); scene_manager_->addListener(&scene_manager_listener_); - UpdateSubmapTopicOrService(); -} - -void SubmapsDisplay::UpdateSubmapTopicOrService() { - UnsubscribeAndClear(); - Subscribe(); + CreateClient(); } void SubmapsDisplay::reset() { - Display::reset(); - UpdateSubmapTopicOrService(); + MFDClass::reset(); + ::cartographer::common::MutexLocker locker(&mutex_); + client_.shutdown(); + trajectories_.clear(); + CreateClient(); } -void SubmapsDisplay::onEnable() { Subscribe(); } - -void SubmapsDisplay::onDisable() { UnsubscribeAndClear(); } - -void SubmapsDisplay::Subscribe() { - if (!isEnabled()) { - return; - } - - client_ = update_nh_.serviceClient<::cartographer_ros_msgs::SubmapQuery>( - submap_query_service_property_->getStdString()); - - if (!topic_property_->getTopic().isEmpty()) { - try { - submap_list_subscriber_ = - update_nh_.subscribe(topic_property_->getTopicStd(), 1, - &SubmapsDisplay::IncomingSubmapList, this, - ros::TransportHints().reliable()); - setStatus(::rviz::StatusProperty::Ok, "Topic", "OK"); - } catch (const ros::Exception& ex) { - setStatus(::rviz::StatusProperty::Error, "Topic", - QString("Error subscribing: ") + ex.what()); - } - } -} - -void SubmapsDisplay::IncomingSubmapList( +void SubmapsDisplay::processMessage( const ::cartographer_ros_msgs::SubmapList::ConstPtr& msg) { submap_list_ = *msg; Q_EMIT SubmapListUpdated(); } -void SubmapsDisplay::UnsubscribeAndClear() { - submap_list_subscriber_.shutdown(); - client_.shutdown(); - ::cartographer::common::MutexLocker locker(&mutex_); - trajectories_.clear(); +void SubmapsDisplay::CreateClient() { + client_ = update_nh_.serviceClient<::cartographer_ros_msgs::SubmapQuery>( + submap_query_service_property_->getStdString()); } void SubmapsDisplay::RequestNewSubmaps() { @@ -142,24 +106,25 @@ void SubmapsDisplay::RequestNewSubmaps() { for (int trajectory_id = 0; trajectory_id < submap_list_.trajectory.size(); ++trajectory_id) { if (trajectory_id >= trajectories_.size()) { - trajectories_.emplace_back(new Trajectory); + trajectories_.emplace_back(); } const std::vector<::cartographer_ros_msgs::SubmapEntry>& submap_entries = submap_list_.trajectory[trajectory_id].submap; if (submap_entries.empty()) { return; } - for (int submap_id = trajectories_[trajectory_id]->Size(); + for (int submap_id = trajectories_[trajectory_id].size(); submap_id < submap_entries.size(); ++submap_id) { - trajectories_[trajectory_id]->Add(submap_id, trajectory_id, - context_->getFrameManager(), - context_->getSceneManager()); + trajectories_[trajectory_id].push_back( + ::cartographer::common::make_unique( + submap_id, trajectory_id, context_->getFrameManager(), + context_->getSceneManager())); } } int num_ongoing_requests = 0; for (const auto& trajectory : trajectories_) { - for (int i = 0; i < trajectory->Size(); ++i) { - if (trajectory->Get(i).QueryInProgress()) { + for (const auto& submap : trajectory) { + if (submap->QueryInProgress()) { ++num_ongoing_requests; if (num_ongoing_requests == kMaxOnGoingRequests) { return; @@ -173,7 +138,7 @@ void SubmapsDisplay::RequestNewSubmaps() { submap_list_.trajectory[trajectory_id].submap; for (int submap_id = submap_entries.size() - 1; submap_id >= 0; --submap_id) { - if (trajectories_[trajectory_id]->Get(submap_id).Update( + if (trajectories_[trajectory_id][submap_id]->Update( submap_entries[submap_id], &client_)) { ++num_ongoing_requests; if (num_ongoing_requests == kMaxOnGoingRequests) { @@ -187,15 +152,14 @@ void SubmapsDisplay::RequestNewSubmaps() { void SubmapsDisplay::UpdateMapTexture() { ::cartographer::common::MutexLocker locker(&mutex_); for (auto& trajectory : trajectories_) { - for (int i = 0; i < trajectory->Size(); ++i) { - trajectory->Get(i).Transform(ros::Time(0) /* latest */); + for (auto& submap : trajectory) { + submap->Transform(ros::Time(0) /* latest */); try { const ::geometry_msgs::TransformStamped transform_stamped = tf_buffer_.lookupTransform(map_frame_property_->getStdString(), tracking_frame_property_->getStdString(), ros::Time(0) /* latest */); - trajectory->Get(i).SetAlpha( - transform_stamped.transform.translation.z); + submap->SetAlpha(transform_stamped.transform.translation.z); } catch (const tf2::TransformException& ex) { ROS_WARN("Could not compute submap fading: %s", ex.what()); } diff --git a/cartographer_ros/src/submaps_display.h b/cartographer_ros/src/submaps_display.h index b43b657..e629d97 100644 --- a/cartographer_ros/src/submaps_display.h +++ b/cartographer_ros/src/submaps_display.h @@ -27,8 +27,7 @@ #include #include #include -#include -#include +#include #include #include @@ -36,7 +35,6 @@ #include #include "drawable_submap.h" -#include "trajectory.h" namespace cartographer_ros { namespace rviz { @@ -47,7 +45,8 @@ namespace rviz { // We show an X-ray view of the map which is achieved by shipping textures for // every submap containing pre-multiplied alpha and grayscale values, these are // then alpha blended together. -class SubmapsDisplay : public ::rviz::Display { +class SubmapsDisplay + : public ::rviz::MessageFilterDisplay<::cartographer_ros_msgs::SubmapList> { Q_OBJECT public: @@ -57,17 +56,11 @@ class SubmapsDisplay : public ::rviz::Display { SubmapsDisplay(const SubmapsDisplay&) = delete; SubmapsDisplay& operator=(const SubmapsDisplay&) = delete; - // Called by RViz on initialization of the plugin. - void onInitialize() override; - // Called to tell the display to clear its state. - void reset() override; - Q_SIGNALS: void SubmapListUpdated(); private Q_SLOTS: void RequestNewSubmaps(); - void UpdateSubmapTopicOrService(); private: class SceneManagerListener : public Ogre::SceneManager::Listener { @@ -82,13 +75,13 @@ class SubmapsDisplay : public ::rviz::Display { std::function callback_; }; - void onEnable() override; - void onDisable() override; - void Subscribe(); - void UnsubscribeAndClear(); + void onInitialize() override; + void reset() override; + void processMessage( + const ::cartographer_ros_msgs::SubmapList::ConstPtr& msg) override; + + void CreateClient(); void UpdateMapTexture(); - void IncomingSubmapList( - const ::cartographer_ros_msgs::SubmapList::ConstPtr& msg); SceneManagerListener scene_manager_listener_; ::cartographer_ros_msgs::SubmapList submap_list_; @@ -96,11 +89,11 @@ class SubmapsDisplay : public ::rviz::Display { ::tf2_ros::Buffer tf_buffer_; ::tf2_ros::TransformListener tf_listener_; ros::ServiceClient client_; - ::rviz::RosTopicProperty* topic_property_; ::rviz::StringProperty* submap_query_service_property_; ::rviz::StringProperty* map_frame_property_; ::rviz::StringProperty* tracking_frame_property_; - std::vector> trajectories_ GUARDED_BY(mutex_); + using Trajectory = std::vector>; + std::vector trajectories_ GUARDED_BY(mutex_); ::cartographer::common::Mutex mutex_; }; diff --git a/cartographer_ros/src/trajectory.cc b/cartographer_ros/src/trajectory.cc deleted file mode 100644 index 995a605..0000000 --- a/cartographer_ros/src/trajectory.cc +++ /dev/null @@ -1,50 +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. - */ - -#include "trajectory.h" - -#include -#include -#include -#include - -#include -#include -#include - -#include "drawable_submap.h" - -namespace cartographer_ros { -namespace rviz { - -void Trajectory::Add(const int submap_id, const int trajectory_id, - ::rviz::FrameManager* const frame_manager, - Ogre::SceneManager* const scene_manager) { - std::lock_guard guard(mutex_); - drawable_submaps_.push_back( - ::cartographer::common::make_unique( - submap_id, trajectory_id, frame_manager, scene_manager)); -} - -DrawableSubmap& Trajectory::Get(const int index) { - std::lock_guard guard(mutex_); - return *drawable_submaps_[index]; -} - -int Trajectory::Size() { return drawable_submaps_.size(); } - -} // namespace rviz -} // namespace cartographer_ros diff --git a/cartographer_ros/src/trajectory.h b/cartographer_ros/src/trajectory.h deleted file mode 100644 index 12ff234..0000000 --- a/cartographer_ros/src/trajectory.h +++ /dev/null @@ -1,58 +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. - */ - -#ifndef CARTOGRAPHER_ROS_GOOGLE_CARTOGRAPHER_SRC_TRAJECTORY_H_ -#define CARTOGRAPHER_ROS_GOOGLE_CARTOGRAPHER_SRC_TRAJECTORY_H_ - -#include -#include -#include - -#include -#include -#include - -#include "drawable_submap.h" - -namespace cartographer_ros { -namespace rviz { - -// Contains a list of drawable submaps. -class Trajectory { - public: - Trajectory() = default; - - Trajectory(const Trajectory&) = delete; - Trajectory& operator=(const Trajectory&) = delete; - - // Creates a new DrawableSubmap and stores it as part of this trajectory. - void Add(int submap_id, int trajectory_id, - ::rviz::FrameManager* frame_manager, - Ogre::SceneManager* scene_manager); - // Gets the submap at 'index' and returns a non-const reference. - DrawableSubmap& Get(int index); - // Returns the number of DrawableSubmaps this trajectory contains. - int Size(); - - private: - std::mutex mutex_; - std::vector> drawable_submaps_; -}; - -} // namespace rviz -} // namespace cartographer_ros - -#endif // CARTOGRAPHER_ROS_GOOGLE_CARTOGRAPHER_SRC_TRAJECTORY_H_ diff --git a/cartographer_ros_msgs/msg/SubmapList.msg b/cartographer_ros_msgs/msg/SubmapList.msg index dbc7b08..9f8e34c 100644 --- a/cartographer_ros_msgs/msg/SubmapList.msg +++ b/cartographer_ros_msgs/msg/SubmapList.msg @@ -12,4 +12,5 @@ # See the License for the specific language governing permissions and # limitations under the License. +std_msgs/Header header TrajectorySubmapList[] trajectory