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

Change the RViz plugin to use MessageFilterDisplay. (#6)

master
Wolfgang Hess 2016-08-04 15:58:41 +02:00 committed by GitHub
parent 5497e9c343
commit 7ead7400d4
7 changed files with 44 additions and 193 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
cartographer_ros/CMakeLists.txt
View File

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

7
cartographer_ros/src/cartographer_node_main.cc
View File

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

90
cartographer_ros/src/submaps_display.cc
View File

@ -18,6 +18,7 @@
#include <OgreResourceGroupManager.h>
#include <OgreSceneManager.h>
#include <cartographer/common/make_unique.h>
#include <cartographer/common/mutex.h>
#include <cartographer_ros_msgs/SubmapList.h>
#include <cartographer_ros_msgs/SubmapQuery.h>
@ -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<DrawableSubmap>(
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());
}

29
cartographer_ros/src/submaps_display.h
View File

@ -27,8 +27,7 @@
#include <cartographer_ros_msgs/SubmapList.h>
#include <nav_msgs/MapMetaData.h>
#include <ros/time.h>
#include <rviz/display.h>
#include <rviz/properties/ros_topic_property.h>
#include <rviz/message_filter_display.h>
#include <tf/tfMessage.h>
#include <tf2_ros/transform_listener.h>
@ -36,7 +35,6 @@
#include <vector>
#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<void()> 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<std::unique_ptr<Trajectory>> trajectories_ GUARDED_BY(mutex_);
using Trajectory = std::vector<std::unique_ptr<DrawableSubmap>>;
std::vector<Trajectory> trajectories_ GUARDED_BY(mutex_);
::cartographer::common::Mutex mutex_;
};

50
cartographer_ros/src/trajectory.cc
View File

@ -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 <OgreSceneManager.h>
#include <OgreSceneNode.h>
#include <cartographer/common/make_unique.h>
#include <rviz/frame_manager.h>
#include <memory>
#include <mutex>
#include <vector>
#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<std::mutex> guard(mutex_);
drawable_submaps_.push_back(
::cartographer::common::make_unique<DrawableSubmap>(
submap_id, trajectory_id, frame_manager, scene_manager));
}
DrawableSubmap& Trajectory::Get(const int index) {
std::lock_guard<std::mutex> guard(mutex_);
return *drawable_submaps_[index];
}
int Trajectory::Size() { return drawable_submaps_.size(); }
} // namespace rviz
} // namespace cartographer_ros

58
cartographer_ros/src/trajectory.h
View File

@ -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 <OgreSceneManager.h>
#include <OgreSceneNode.h>
#include <rviz/frame_manager.h>
#include <memory>
#include <mutex>
#include <vector>
#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<std::unique_ptr<DrawableSubmap>> drawable_submaps_;
};
} // namespace rviz
} // namespace cartographer_ros
#endif // CARTOGRAPHER_ROS_GOOGLE_CARTOGRAPHER_SRC_TRAJECTORY_H_

1
cartographer_ros_msgs/msg/SubmapList.msg
View File

@ -12,4 +12,5 @@
# See the License for the specific language governing permissions and
# limitations under the License.
std_msgs/Header header
TrajectorySubmapList[] trajectory