feat(slam): add cartographer_ros
This commit is contained in:
X-lanni
@@ -0,0 +1,2 @@
|
||||
BasedOnStyle: Google
|
||||
DerivePointerAlignment: false
|
||||
@@ -0,0 +1,207 @@
|
||||
/*
|
||||
* 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 "cartographer_rviz/drawable_submap.h"
|
||||
|
||||
#include <chrono>
|
||||
#include <future>
|
||||
#include <sstream>
|
||||
#include <string>
|
||||
|
||||
#include "Eigen/Core"
|
||||
#include "Eigen/Geometry"
|
||||
#include "absl/memory/memory.h"
|
||||
#include "cartographer/common/port.h"
|
||||
#include "cartographer_ros/msg_conversion.h"
|
||||
#include "cartographer_ros_msgs/srv/submap_query.hpp"
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
|
||||
|
||||
namespace cartographer_rviz {
|
||||
|
||||
namespace {
|
||||
|
||||
constexpr std::chrono::milliseconds kMinQueryDelayInMs(250);
|
||||
constexpr float kAlphaUpdateThreshold = 0.2f;
|
||||
|
||||
const Ogre::ColourValue kSubmapIdColor(Ogre::ColourValue::Red);
|
||||
const Eigen::Vector3d kSubmapIdPosition(0.0, 0.0, 0.3);
|
||||
constexpr float kSubmapIdCharHeight = 0.2f;
|
||||
constexpr int kNumberOfSlicesPerSubmap = 2;
|
||||
|
||||
} // namespace
|
||||
|
||||
|
||||
DrawableSubmap::DrawableSubmap(const ::cartographer::mapping::SubmapId& id,
|
||||
::rviz_common::DisplayContext* const display_context,
|
||||
Ogre::SceneNode* const map_node,
|
||||
::rviz_common::properties::Property* const submap_category,
|
||||
const bool visible, const bool pose_axes_visible,
|
||||
const float pose_axes_length,
|
||||
const float pose_axes_radius)
|
||||
: id_(id),
|
||||
display_context_(display_context),
|
||||
submap_node_(map_node->createChildSceneNode()),
|
||||
submap_id_text_node_(submap_node_->createChildSceneNode()),
|
||||
pose_axes_(display_context->getSceneManager(), submap_node_,
|
||||
pose_axes_length, pose_axes_radius),
|
||||
pose_axes_visible_(pose_axes_visible),
|
||||
submap_id_text_(QString("(%1,%2)")
|
||||
.arg(id.trajectory_id)
|
||||
.arg(id.submap_index)
|
||||
.toStdString()),
|
||||
last_query_timestamp_(0){
|
||||
for (int slice_index = 0; slice_index < kNumberOfSlicesPerSubmap;
|
||||
++slice_index) {
|
||||
ogre_slices_.emplace_back(absl::make_unique<OgreSlice>(
|
||||
id, slice_index, display_context->getSceneManager(), submap_node_));
|
||||
}
|
||||
// DrawableSubmap creates and manages its visibility property object
|
||||
// (a unique_ptr is needed because the Qt parent of the visibility
|
||||
// property is the submap_category object - the BoolProperty needs
|
||||
// to be destroyed along with the DrawableSubmap)
|
||||
visibility_ = absl::make_unique<::rviz_common::properties::BoolProperty>(
|
||||
"" /* title */, visible, "" /* description */, submap_category,
|
||||
SLOT(ToggleVisibility()), this);
|
||||
submap_id_text_.setCharacterHeight(kSubmapIdCharHeight);
|
||||
submap_id_text_.setColor(kSubmapIdColor);
|
||||
submap_id_text_.setTextAlignment(::rviz_rendering::MovableText::H_CENTER,
|
||||
::rviz_rendering::MovableText::V_ABOVE);
|
||||
submap_id_text_node_->setPosition(ToOgre(kSubmapIdPosition));
|
||||
submap_id_text_node_->attachObject(&submap_id_text_);
|
||||
TogglePoseMarkerVisibility();
|
||||
connect(this, SIGNAL(RequestSucceeded()), this, SLOT(UpdateSceneNode()));
|
||||
}
|
||||
|
||||
DrawableSubmap::~DrawableSubmap() {
|
||||
// 'query_in_progress_' must be true until the Q_EMIT has happened. Qt then
|
||||
// makes sure that 'RequestSucceeded' is not called after our destruction.
|
||||
if (QueryInProgress()) {
|
||||
rpc_request_future_.wait();
|
||||
}
|
||||
display_context_->getSceneManager()->destroySceneNode(submap_node_);
|
||||
display_context_->getSceneManager()->destroySceneNode(submap_id_text_node_);
|
||||
}
|
||||
|
||||
void DrawableSubmap::Update(
|
||||
const ::std_msgs::msg::Header& header,
|
||||
const ::cartographer_ros_msgs::msg::SubmapEntry& metadata) {
|
||||
(void) header; // TODO: remove unused arg ?
|
||||
absl::MutexLock locker(&mutex_);
|
||||
metadata_version_ = metadata.submap_version;
|
||||
pose_ = ::cartographer_ros::ToRigid3d(metadata.pose);
|
||||
submap_node_->setPosition(ToOgre(pose_.translation()));
|
||||
submap_node_->setOrientation(ToOgre(pose_.rotation()));
|
||||
display_context_->queueRender();
|
||||
visibility_->setName(
|
||||
QString("%1.%2").arg(id_.submap_index).arg(metadata_version_));
|
||||
visibility_->setDescription(
|
||||
QString("Toggle visibility of this individual submap.<br><br>"
|
||||
"Trajectory %1, submap %2, submap version %3")
|
||||
.arg(id_.trajectory_id)
|
||||
.arg(id_.submap_index)
|
||||
.arg(metadata_version_));
|
||||
}
|
||||
|
||||
bool DrawableSubmap::MaybeFetchTexture(
|
||||
rclcpp::Client<cartographer_ros_msgs::srv::SubmapQuery>::SharedPtr const client,
|
||||
rclcpp::executors::SingleThreadedExecutor::SharedPtr callback_group_executor) {
|
||||
absl::MutexLock locker(&mutex_);
|
||||
// Received metadata version can also be lower if we restarted Cartographer.
|
||||
const bool newer_version_available =
|
||||
submap_textures_ == nullptr ||
|
||||
submap_textures_->version != metadata_version_;
|
||||
const std::chrono::milliseconds now =
|
||||
std::chrono::duration_cast<std::chrono::milliseconds>(
|
||||
std::chrono::system_clock::now().time_since_epoch());
|
||||
const bool recently_queried =
|
||||
last_query_timestamp_ + kMinQueryDelayInMs > now;
|
||||
if (!newer_version_available || recently_queried || query_in_progress_) {
|
||||
return false;
|
||||
}
|
||||
query_in_progress_ = true;
|
||||
last_query_timestamp_ = now;
|
||||
|
||||
rpc_request_future_ = std::async(std::launch::async, [this, client, callback_group_executor]() {
|
||||
std::unique_ptr<::cartographer::io::SubmapTextures> submap_textures =
|
||||
::cartographer_ros::FetchSubmapTextures(id_, client, callback_group_executor, std::chrono::milliseconds(10000));
|
||||
absl::MutexLock locker(&mutex_);
|
||||
query_in_progress_ = false;
|
||||
if (submap_textures != nullptr) {
|
||||
// We emit a signal to update in the right thread, and pass via the
|
||||
// 'submap_texture_' member to simplify the signal-slot connection
|
||||
// slightly.
|
||||
submap_textures_ = std::move(submap_textures);
|
||||
Q_EMIT RequestSucceeded();
|
||||
}
|
||||
});
|
||||
return true;
|
||||
}
|
||||
|
||||
bool DrawableSubmap::QueryInProgress() {
|
||||
absl::MutexLock locker(&mutex_);
|
||||
return query_in_progress_;
|
||||
}
|
||||
|
||||
void DrawableSubmap::SetAlpha(const double current_tracking_z,
|
||||
const float fade_out_start_distance_in_meters) {
|
||||
const float fade_out_distance_in_meters =
|
||||
2.f * fade_out_start_distance_in_meters;
|
||||
const double distance_z =
|
||||
std::abs(pose_.translation().z() - current_tracking_z);
|
||||
const double fade_distance =
|
||||
std::max(distance_z - fade_out_start_distance_in_meters, 0.);
|
||||
const float target_alpha = static_cast<float>(
|
||||
std::max(0., 1. - fade_distance / fade_out_distance_in_meters));
|
||||
|
||||
if (std::abs(target_alpha - current_alpha_) > kAlphaUpdateThreshold ||
|
||||
target_alpha == 0.f || target_alpha == 1.f) {
|
||||
current_alpha_ = target_alpha;
|
||||
}
|
||||
for (auto& slice : ogre_slices_) {
|
||||
slice->SetAlpha(current_alpha_);
|
||||
}
|
||||
display_context_->queueRender();
|
||||
}
|
||||
|
||||
void DrawableSubmap::SetSliceVisibility(size_t slice_index, bool visible) {
|
||||
ogre_slices_.at(slice_index)->SetVisibility(visible);
|
||||
ToggleVisibility();
|
||||
}
|
||||
|
||||
void DrawableSubmap::UpdateSceneNode() {
|
||||
absl::MutexLock locker(&mutex_);
|
||||
for (size_t slice_index = 0; slice_index < ogre_slices_.size() &&
|
||||
slice_index < submap_textures_->textures.size();
|
||||
++slice_index) {
|
||||
ogre_slices_[slice_index]->Update(submap_textures_->textures[slice_index]);
|
||||
}
|
||||
display_context_->queueRender();
|
||||
}
|
||||
|
||||
void DrawableSubmap::ToggleVisibility() {
|
||||
for (auto& ogre_slice : ogre_slices_) {
|
||||
ogre_slice->UpdateOgreNodeVisibility(visibility_->getBool());
|
||||
}
|
||||
display_context_->queueRender();
|
||||
}
|
||||
|
||||
void DrawableSubmap::TogglePoseMarkerVisibility() {
|
||||
submap_id_text_node_->setVisible(pose_axes_visible_);
|
||||
pose_axes_.getSceneNode()->setVisible(pose_axes_visible_);
|
||||
}
|
||||
|
||||
} // namespace cartographer_rviz
|
||||
@@ -0,0 +1,154 @@
|
||||
/*
|
||||
* 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 "cartographer_rviz/ogre_slice.h"
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include "OgreGpuProgramParams.h"
|
||||
#include "OgreImage.h"
|
||||
#include "OgreMaterialManager.h"
|
||||
#include "OgreTechnique.h"
|
||||
#include "OgreTextureManager.h"
|
||||
#include "cartographer/common/port.h"
|
||||
|
||||
namespace cartographer_rviz {
|
||||
|
||||
namespace {
|
||||
|
||||
constexpr char kManualObjectPrefix[] = "ManualObjectSubmap";
|
||||
constexpr char kSubmapSourceMaterialName[] = "cartographer_ros/Submap";
|
||||
constexpr char kSubmapMaterialPrefix[] = "SubmapMaterial";
|
||||
constexpr char kSubmapTexturePrefix[] = "SubmapTexture";
|
||||
|
||||
std::string GetSliceIdentifier(
|
||||
const ::cartographer::mapping::SubmapId& submap_id, const int slice_id) {
|
||||
return (std::to_string(submap_id.trajectory_id) + "-" + std::to_string(submap_id.submap_index) + "-" + std::to_string(slice_id));
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
Ogre::Vector3 ToOgre(const Eigen::Vector3d& v) {
|
||||
return Ogre::Vector3(v.x(), v.y(), v.z());
|
||||
}
|
||||
|
||||
Ogre::Quaternion ToOgre(const Eigen::Quaterniond& q) {
|
||||
return Ogre::Quaternion(q.w(), q.x(), q.y(), q.z());
|
||||
}
|
||||
|
||||
OgreSlice::OgreSlice(const ::cartographer::mapping::SubmapId& id, int slice_id,
|
||||
Ogre::SceneManager* const scene_manager,
|
||||
Ogre::SceneNode* const submap_node)
|
||||
: id_(id),
|
||||
slice_id_(slice_id),
|
||||
scene_manager_(scene_manager),
|
||||
submap_node_(submap_node),
|
||||
slice_node_(submap_node_->createChildSceneNode()),
|
||||
manual_object_(scene_manager_->createManualObject(
|
||||
kManualObjectPrefix + GetSliceIdentifier(id, slice_id))) {
|
||||
material_ = Ogre::MaterialManager::getSingleton().getByName(kSubmapSourceMaterialName);
|
||||
material_ = material_->clone(
|
||||
kSubmapMaterialPrefix + GetSliceIdentifier(id_, slice_id_), true, "General");
|
||||
material_->setReceiveShadows(false);
|
||||
material_->getTechnique(0)->setLightingEnabled(false);
|
||||
material_->setCullingMode(Ogre::CULL_NONE);
|
||||
material_->setDepthBias(-1.f, 0.f);
|
||||
material_->setDepthWriteEnabled(false);
|
||||
slice_node_->attachObject(manual_object_);
|
||||
}
|
||||
|
||||
OgreSlice::~OgreSlice() {
|
||||
Ogre::MaterialManager::getSingleton().remove(material_->getHandle());
|
||||
if (texture_) {
|
||||
Ogre::TextureManager::getSingleton().remove(texture_->getHandle());
|
||||
texture_.reset();
|
||||
}
|
||||
scene_manager_->destroySceneNode(slice_node_);
|
||||
scene_manager_->destroyManualObject(manual_object_);
|
||||
}
|
||||
|
||||
void OgreSlice::Update(
|
||||
const ::cartographer::io::SubmapTexture& submap_texture) {
|
||||
slice_node_->setPosition(ToOgre(submap_texture.slice_pose.translation()));
|
||||
slice_node_->setOrientation(ToOgre(submap_texture.slice_pose.rotation()));
|
||||
// The call to Ogre's loadRawData below does not work with an RG texture,
|
||||
// therefore we create an RGB one whose blue channel is always 0.
|
||||
std::vector<char> rgb;
|
||||
CHECK_EQ(submap_texture.pixels.intensity.size(),
|
||||
submap_texture.pixels.alpha.size());
|
||||
for (size_t i = 0; i < submap_texture.pixels.intensity.size(); ++i) {
|
||||
rgb.push_back(submap_texture.pixels.intensity[i]);
|
||||
rgb.push_back(submap_texture.pixels.alpha[i]);
|
||||
rgb.push_back(0);
|
||||
}
|
||||
|
||||
manual_object_->clear();
|
||||
const float metric_width = submap_texture.resolution * submap_texture.width;
|
||||
const float metric_height = submap_texture.resolution * submap_texture.height;
|
||||
manual_object_->begin(material_->getName(),
|
||||
Ogre::RenderOperation::OT_TRIANGLE_STRIP);
|
||||
// Bottom left
|
||||
manual_object_->position(-metric_height, 0.0f, 0.0f);
|
||||
manual_object_->textureCoord(0.0f, 1.0f);
|
||||
// Bottom right
|
||||
manual_object_->position(-metric_height, -metric_width, 0.0f);
|
||||
manual_object_->textureCoord(1.0f, 1.0f);
|
||||
// Top left
|
||||
manual_object_->position(0.0f, 0.0f, 0.0f);
|
||||
manual_object_->textureCoord(0.0f, 0.0f);
|
||||
// Top right
|
||||
manual_object_->position(0.0f, -metric_width, 0.0f);
|
||||
manual_object_->textureCoord(1.0f, 0.0f);
|
||||
manual_object_->end();
|
||||
|
||||
Ogre::DataStreamPtr pixel_stream;
|
||||
pixel_stream.reset(new Ogre::MemoryDataStream(rgb.data(), rgb.size()));
|
||||
|
||||
if (texture_) {
|
||||
Ogre::TextureManager::getSingleton().remove(texture_->getHandle());
|
||||
texture_.reset();
|
||||
}
|
||||
const std::string texture_name =
|
||||
kSubmapTexturePrefix + GetSliceIdentifier(id_, slice_id_);
|
||||
texture_ = Ogre::TextureManager::getSingleton().loadRawData(
|
||||
texture_name, Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME,
|
||||
pixel_stream, submap_texture.width, submap_texture.height,
|
||||
Ogre::PF_BYTE_RGB, Ogre::TEX_TYPE_2D, 0);
|
||||
|
||||
Ogre::Pass* const pass = material_->getTechnique(0)->getPass(0);
|
||||
pass->setSceneBlending(Ogre::SBF_ONE, Ogre::SBF_ONE_MINUS_SOURCE_ALPHA);
|
||||
Ogre::TextureUnitState* const texture_unit =
|
||||
pass->getNumTextureUnitStates() > 0 ? pass->getTextureUnitState(0)
|
||||
: pass->createTextureUnitState();
|
||||
|
||||
texture_unit->setTextureName(texture_->getName());
|
||||
texture_unit->setTextureFiltering(Ogre::TFO_NONE);
|
||||
}
|
||||
|
||||
void OgreSlice::SetAlpha(const float alpha) {
|
||||
const Ogre::GpuProgramParametersSharedPtr parameters =
|
||||
material_->getTechnique(0)->getPass(0)->getFragmentProgramParameters();
|
||||
parameters->setNamedConstant("u_alpha", alpha);
|
||||
}
|
||||
|
||||
void OgreSlice::SetVisibility(bool visibility) { visibility_ = visibility; }
|
||||
|
||||
void OgreSlice::UpdateOgreNodeVisibility(bool submap_visibility) {
|
||||
slice_node_->setVisible(submap_visibility && visibility_);
|
||||
}
|
||||
|
||||
} // namespace cartographer_rviz
|
||||
@@ -0,0 +1,339 @@
|
||||
/*
|
||||
* 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 "cartographer_rviz/submaps_display.h"
|
||||
|
||||
#include "OgreResourceGroupManager.h"
|
||||
#include "absl/memory/memory.h"
|
||||
#include "absl/synchronization/mutex.h"
|
||||
|
||||
#include "cartographer/mapping/id.h"
|
||||
#include "cartographer_ros_msgs/msg/submap_list.hpp"
|
||||
#include "cartographer_ros_msgs/srv/submap_query.hpp"
|
||||
#include <geometry_msgs/msg/transform_stamped.hpp>
|
||||
#include <pluginlib/class_list_macros.hpp>
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <rclcpp/version.h>
|
||||
#include <rviz_common/display_context.hpp>
|
||||
#include <rviz_common/frame_manager_iface.hpp>
|
||||
#include <rviz_common/properties/bool_property.hpp>
|
||||
#include <rviz_common/properties/string_property.hpp>
|
||||
#include <rviz_common/message_filter_display.hpp>
|
||||
#include <ament_index_cpp/get_package_share_directory.hpp>
|
||||
|
||||
namespace cartographer_rviz {
|
||||
|
||||
namespace {
|
||||
|
||||
constexpr int kMaxOnGoingRequestsPerTrajectory = 6;
|
||||
constexpr char kMaterialsDirectory[] = "/ogre_media/materials";
|
||||
constexpr char kGlsl120Directory[] = "/glsl120";
|
||||
constexpr char kScriptsDirectory[] = "/scripts";
|
||||
constexpr char kDefaultTrackingFrame[] = "base_link";
|
||||
constexpr char kDefaultSubmapQueryServiceName[] = "/submap_query";
|
||||
|
||||
} // namespace
|
||||
|
||||
SubmapsDisplay::SubmapsDisplay() : rclcpp::Node("submaps_display") {
|
||||
submap_query_service_property_ = new ::rviz_common::properties::StringProperty(
|
||||
"Submap query service", kDefaultSubmapQueryServiceName,
|
||||
"Submap query service to connect to.", this, SLOT(Reset()));
|
||||
tracking_frame_property_ = new ::rviz_common::properties::StringProperty(
|
||||
"Tracking frame", kDefaultTrackingFrame,
|
||||
"Tracking frame, used for fading out submaps.", this);
|
||||
slice_high_resolution_enabled_ = new ::rviz_common::properties::BoolProperty(
|
||||
"High Resolution", true, "Display high resolution slices.", this,
|
||||
SLOT(ResolutionToggled()), this);
|
||||
slice_low_resolution_enabled_ = new ::rviz_common::properties::BoolProperty(
|
||||
"Low Resolution", false, "Display low resolution slices.", this,
|
||||
SLOT(ResolutionToggled()), this);
|
||||
|
||||
callback_group_ = this->create_callback_group(
|
||||
rclcpp::CallbackGroupType::MutuallyExclusive,
|
||||
false);
|
||||
callback_group_executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
|
||||
callback_group_executor_->add_callback_group(callback_group_, this->get_node_base_interface());
|
||||
client_ = this->create_client<::cartographer_ros_msgs::srv::SubmapQuery>(
|
||||
kDefaultSubmapQueryServiceName,
|
||||
#if RCLCPP_VERSION_GTE(17, 0, 0)
|
||||
rclcpp::ServicesQoS(),
|
||||
#else
|
||||
rmw_qos_profile_services_default,
|
||||
#endif
|
||||
callback_group_
|
||||
);
|
||||
trajectories_category_ = new ::rviz_common::properties::Property(
|
||||
"Submaps", QVariant(), "List of all submaps, organized by trajectories.",
|
||||
this);
|
||||
visibility_all_enabled_ = new ::rviz_common::properties::BoolProperty(
|
||||
"All", true,
|
||||
"Whether submaps from all trajectories should be displayed or not.",
|
||||
trajectories_category_, SLOT(AllEnabledToggled()), this);
|
||||
pose_markers_all_enabled_ = new ::rviz_common::properties::BoolProperty(
|
||||
"All Submap Pose Markers", true,
|
||||
"Whether submap pose markers should be displayed or not.",
|
||||
trajectories_category_, SLOT(PoseMarkersEnabledToggled()), this);
|
||||
fade_out_start_distance_in_meters_ =
|
||||
new ::rviz_common::properties::FloatProperty("Fade-out distance", 1.f,
|
||||
"Distance in meters in z-direction beyond "
|
||||
"which submaps will start to fade out.",
|
||||
this);
|
||||
const std::string package_path = ament_index_cpp::get_package_share_directory("cartographer_rviz");
|
||||
Ogre::ResourceGroupManager::getSingleton().addResourceLocation(
|
||||
package_path + kMaterialsDirectory, "FileSystem", "cartographer_rviz");
|
||||
Ogre::ResourceGroupManager::getSingleton().addResourceLocation(
|
||||
package_path + kMaterialsDirectory + kGlsl120Directory, "FileSystem",
|
||||
"cartographer_rviz");
|
||||
Ogre::ResourceGroupManager::getSingleton().addResourceLocation(
|
||||
package_path + kMaterialsDirectory + kScriptsDirectory, "FileSystem",
|
||||
"cartographer_rviz");
|
||||
Ogre::ResourceGroupManager::getSingleton().initialiseAllResourceGroups();
|
||||
|
||||
tf_buffer_ = std::make_unique<tf2_ros::Buffer>(this->get_clock());
|
||||
tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
|
||||
}
|
||||
|
||||
SubmapsDisplay::~SubmapsDisplay() {
|
||||
client_.reset();
|
||||
trajectories_.clear();
|
||||
scene_manager_->destroySceneNode(map_node_);
|
||||
}
|
||||
|
||||
void SubmapsDisplay::Reset() { reset(); }
|
||||
|
||||
void SubmapsDisplay::CreateClient() {
|
||||
client_ = this->create_client<::cartographer_ros_msgs::srv::SubmapQuery>(
|
||||
submap_query_service_property_->getStdString(),
|
||||
#if RCLCPP_VERSION_GTE(17, 0, 0)
|
||||
rclcpp::ServicesQoS(),
|
||||
#else
|
||||
rmw_qos_profile_services_default,
|
||||
#endif
|
||||
callback_group_
|
||||
);
|
||||
}
|
||||
|
||||
void SubmapsDisplay::onInitialize() {
|
||||
MFDClass::onInitialize();
|
||||
map_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode();
|
||||
CreateClient();
|
||||
}
|
||||
|
||||
void SubmapsDisplay::reset() {
|
||||
MFDClass::reset();
|
||||
absl::MutexLock locker(&mutex_);
|
||||
client_.reset();
|
||||
trajectories_.clear();
|
||||
CreateClient();
|
||||
}
|
||||
|
||||
void SubmapsDisplay::processMessage( const ::cartographer_ros_msgs::msg::SubmapList::ConstSharedPtr msg) {
|
||||
absl::MutexLock locker(&mutex_);
|
||||
map_frame_ = absl::make_unique<std::string>(msg->header.frame_id);
|
||||
// In case Cartographer node is relaunched, destroy trajectories from the
|
||||
// previous instance.
|
||||
for (const ::cartographer_ros_msgs::msg::SubmapEntry& submap_entry : msg->submap) {
|
||||
const size_t trajectory_id = submap_entry.trajectory_id;
|
||||
if (trajectories_.count(trajectory_id) == 0) {
|
||||
continue;
|
||||
}
|
||||
const auto& trajectory_submaps = trajectories_[trajectory_id]->submaps;
|
||||
const auto it = trajectory_submaps.find(submap_entry.submap_index);
|
||||
if (it != trajectory_submaps.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;
|
||||
std::set<int> listed_trajectories;
|
||||
for (const ::cartographer_ros_msgs::msg::SubmapEntry& submap_entry : msg->submap) {
|
||||
const SubmapId id{submap_entry.trajectory_id, submap_entry.submap_index};
|
||||
listed_submaps.insert(id);
|
||||
listed_trajectories.insert(submap_entry.trajectory_id);
|
||||
if (trajectories_.count(id.trajectory_id) == 0) {
|
||||
trajectories_.insert(std::make_pair(
|
||||
id.trajectory_id,
|
||||
absl::make_unique<Trajectory>(
|
||||
absl::make_unique<::rviz_common::properties::BoolProperty>(
|
||||
QString("Trajectory %1").arg(id.trajectory_id),
|
||||
visibility_all_enabled_->getBool(),
|
||||
QString(
|
||||
"List of all submaps in Trajectory %1. The checkbox "
|
||||
"controls whether all submaps in this trajectory should "
|
||||
"be displayed or not.")
|
||||
.arg(id.trajectory_id),
|
||||
trajectories_category_),
|
||||
pose_markers_all_enabled_->getBool())));
|
||||
}
|
||||
auto& trajectory_visibility = trajectories_[id.trajectory_id]->visibility;
|
||||
auto& trajectory_submaps = trajectories_[id.trajectory_id]->submaps;
|
||||
auto& pose_markers_visibility =
|
||||
trajectories_[id.trajectory_id]->pose_markers_visibility;
|
||||
if (trajectory_submaps.count(id.submap_index) == 0) {
|
||||
// TODO(ojura): Add RViz properties for adjusting submap pose axes
|
||||
constexpr float kSubmapPoseAxesLength = 0.3f;
|
||||
constexpr float kSubmapPoseAxesRadius = 0.06f;
|
||||
trajectory_submaps.emplace(
|
||||
id.submap_index,
|
||||
absl::make_unique<DrawableSubmap>(
|
||||
id, context_, map_node_, trajectory_visibility.get(),
|
||||
trajectory_visibility->getBool(),
|
||||
pose_markers_visibility->getBool(), kSubmapPoseAxesLength,
|
||||
kSubmapPoseAxesRadius));
|
||||
trajectory_submaps.at(id.submap_index)
|
||||
->SetSliceVisibility(0, slice_high_resolution_enabled_->getBool());
|
||||
trajectory_submaps.at(id.submap_index)
|
||||
->SetSliceVisibility(1, slice_low_resolution_enabled_->getBool());
|
||||
}
|
||||
trajectory_submaps.at(id.submap_index)->Update(msg->header, submap_entry);
|
||||
}
|
||||
// Remove all deleted trajectories not mentioned in the SubmapList.
|
||||
for (auto it = trajectories_.begin(); it != trajectories_.end();) {
|
||||
if (listed_trajectories.count(it->first) == 0) {
|
||||
it = trajectories_.erase(it);
|
||||
} else {
|
||||
++it;
|
||||
}
|
||||
}
|
||||
// Remove all submaps not mentioned in the SubmapList.
|
||||
for (const auto& trajectory_by_id : trajectories_) {
|
||||
const int trajectory_id = trajectory_by_id.first;
|
||||
auto& trajectory_submaps = trajectory_by_id.second->submaps;
|
||||
for (auto it = trajectory_submaps.begin();
|
||||
it != trajectory_submaps.end();) {
|
||||
if (listed_submaps.count(
|
||||
SubmapId{static_cast<int>(trajectory_id), it->first}) == 0) {
|
||||
it = trajectory_submaps.erase(it);
|
||||
} else {
|
||||
++it;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void SubmapsDisplay::update(const float , const float) {
|
||||
absl::MutexLock locker(&mutex_);
|
||||
// Schedule fetching of new submap textures.
|
||||
for (const auto& trajectory_by_id : trajectories_) {
|
||||
int num_ongoing_requests = 0;
|
||||
for (const auto& submap_entry : trajectory_by_id.second->submaps) {
|
||||
if (submap_entry.second->QueryInProgress()) {
|
||||
++num_ongoing_requests;
|
||||
}
|
||||
}
|
||||
for (auto it = trajectory_by_id.second->submaps.rbegin();
|
||||
it != trajectory_by_id.second->submaps.rend() &&
|
||||
num_ongoing_requests < kMaxOnGoingRequestsPerTrajectory;
|
||||
++it) {
|
||||
if (it->second->MaybeFetchTexture(client_, callback_group_executor_)) {
|
||||
++num_ongoing_requests;
|
||||
}
|
||||
}
|
||||
}
|
||||
if (map_frame_ == nullptr) {
|
||||
return;
|
||||
}
|
||||
|
||||
// Update the fading by z distance.
|
||||
const auto klatest = this->get_clock()->now();
|
||||
try {
|
||||
const ::geometry_msgs::msg::TransformStamped transform_stamped =
|
||||
tf_buffer_->lookupTransform(
|
||||
*map_frame_, tracking_frame_property_->getStdString(), tf2::TimePointZero,std::chrono::milliseconds(100));
|
||||
for (auto& trajectory_by_id : trajectories_) {
|
||||
for (auto& submap_entry : trajectory_by_id.second->submaps) {
|
||||
submap_entry.second->SetAlpha(
|
||||
transform_stamped.transform.translation.z,
|
||||
fade_out_start_distance_in_meters_->getFloat());
|
||||
}
|
||||
}
|
||||
} catch (const tf2::TransformException& ex) {
|
||||
RCLCPP_WARN(this->get_logger(), "Could not compute submap fading: %s", ex.what());
|
||||
}
|
||||
// Update the map frame to fixed frame transform.
|
||||
Ogre::Vector3 position;
|
||||
Ogre::Quaternion orientation;
|
||||
if (context_->getFrameManager()->getTransform(*map_frame_, klatest, position,
|
||||
orientation)) {
|
||||
map_node_->setPosition(position);
|
||||
map_node_->setOrientation(orientation);
|
||||
context_->queueRender();
|
||||
}
|
||||
}
|
||||
|
||||
void SubmapsDisplay::AllEnabledToggled() {
|
||||
absl::MutexLock locker(&mutex_);
|
||||
const bool visible = visibility_all_enabled_->getBool();
|
||||
for (auto& trajectory_by_id : trajectories_) {
|
||||
trajectory_by_id.second->visibility->setBool(visible);
|
||||
}
|
||||
}
|
||||
|
||||
void SubmapsDisplay::PoseMarkersEnabledToggled() {
|
||||
absl::MutexLock locker(&mutex_);
|
||||
const bool visible = pose_markers_all_enabled_->getBool();
|
||||
for (auto& trajectory_by_id : trajectories_) {
|
||||
trajectory_by_id.second->pose_markers_visibility->setBool(visible);
|
||||
}
|
||||
}
|
||||
|
||||
void SubmapsDisplay::ResolutionToggled() {
|
||||
absl::MutexLock locker(&mutex_);
|
||||
for (auto& trajectory_by_id : trajectories_) {
|
||||
for (auto& submap_entry : trajectory_by_id.second->submaps) {
|
||||
submap_entry.second->SetSliceVisibility(
|
||||
0, slice_high_resolution_enabled_->getBool());
|
||||
submap_entry.second->SetSliceVisibility(
|
||||
1, slice_low_resolution_enabled_->getBool());
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void Trajectory::AllEnabledToggled() {
|
||||
const bool visible = visibility->getBool();
|
||||
for (auto& submap_entry : submaps) {
|
||||
submap_entry.second->set_visibility(visible);
|
||||
}
|
||||
}
|
||||
|
||||
void Trajectory::PoseMarkersEnabledToggled() {
|
||||
const bool visible = pose_markers_visibility->getBool();
|
||||
for (auto& submap_entry : submaps) {
|
||||
submap_entry.second->set_pose_markers_visibility(visible);
|
||||
}
|
||||
}
|
||||
|
||||
Trajectory::Trajectory(std::unique_ptr<::rviz_common::properties::BoolProperty> property,
|
||||
const bool pose_markers_enabled)
|
||||
: visibility(std::move(property)) {
|
||||
::QObject::connect(visibility.get(), SIGNAL(changed()), this,
|
||||
SLOT(AllEnabledToggled()));
|
||||
// Add toggle for submap pose markers as the first entry of the visibility
|
||||
// property list of this trajectory.
|
||||
pose_markers_visibility = absl::make_unique<::rviz_common::properties::BoolProperty>(
|
||||
QString("Submap Pose Markers"), pose_markers_enabled,
|
||||
QString("Toggles the submap pose markers of this trajectory."),
|
||||
visibility.get());
|
||||
::QObject::connect(pose_markers_visibility.get(), SIGNAL(changed()), this,
|
||||
SLOT(PoseMarkersEnabledToggled()));
|
||||
}
|
||||
|
||||
} // namespace cartographer_rviz
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(cartographer_rviz::SubmapsDisplay, rviz_common::Display)
|
||||
Reference in New Issue
Block a user