Pull out FetchSubmapTexture into cartographer_ros. (#433)
parent
fed96d8cee
commit
603439ac05
|
@ -55,4 +55,3 @@ install(TARGETS cartographer_start_trajectory
|
||||||
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||||
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
||||||
)
|
)
|
||||||
|
|
||||||
|
|
|
@ -50,9 +50,6 @@ using carto::transform::Rigid3d;
|
||||||
|
|
||||||
namespace {
|
namespace {
|
||||||
|
|
||||||
constexpr int kInfiniteSubscriberQueueSize = 0;
|
|
||||||
constexpr int kLatestOnlyPublisherQueueSize = 1;
|
|
||||||
|
|
||||||
void ShutdownSubscriber(std::unordered_map<int, ::ros::Subscriber>& subscribers,
|
void ShutdownSubscriber(std::unordered_map<int, ::ros::Subscriber>& subscribers,
|
||||||
int trajectory_id) {
|
int trajectory_id) {
|
||||||
if (subscribers.count(trajectory_id) == 0) {
|
if (subscribers.count(trajectory_id) == 0) {
|
||||||
|
|
|
@ -36,6 +36,9 @@ constexpr char kTrajectoryNodeListTopic[] = "trajectory_node_list";
|
||||||
constexpr char kConstraintListTopic[] = "constraint_list";
|
constexpr char kConstraintListTopic[] = "constraint_list";
|
||||||
constexpr double kConstraintPublishPeriodSec = 0.5;
|
constexpr double kConstraintPublishPeriodSec = 0.5;
|
||||||
|
|
||||||
|
constexpr int kInfiniteSubscriberQueueSize = 0;
|
||||||
|
constexpr int kLatestOnlyPublisherQueueSize = 1;
|
||||||
|
|
||||||
} // namespace cartographer_ros
|
} // namespace cartographer_ros
|
||||||
|
|
||||||
#endif // CARTOGRAPHER_ROS_NODE_CONSTANTS_H_
|
#endif // CARTOGRAPHER_ROS_NODE_CONSTANTS_H_
|
||||||
|
|
|
@ -61,7 +61,6 @@ namespace {
|
||||||
constexpr char kClockTopic[] = "clock";
|
constexpr char kClockTopic[] = "clock";
|
||||||
constexpr char kTfStaticTopic[] = "/tf_static";
|
constexpr char kTfStaticTopic[] = "/tf_static";
|
||||||
constexpr char kTfTopic[] = "tf";
|
constexpr char kTfTopic[] = "tf";
|
||||||
constexpr int kLatestOnlyPublisherQueueSize = 1;
|
|
||||||
|
|
||||||
volatile std::sig_atomic_t sigint_triggered = 0;
|
volatile std::sig_atomic_t sigint_triggered = 0;
|
||||||
|
|
||||||
|
|
|
@ -0,0 +1,58 @@
|
||||||
|
/*
|
||||||
|
* 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_ros/submap.h"
|
||||||
|
|
||||||
|
#include "cartographer/common/make_unique.h"
|
||||||
|
#include "cartographer/common/port.h"
|
||||||
|
#include "cartographer/transform/transform.h"
|
||||||
|
#include "cartographer_ros/msg_conversion.h"
|
||||||
|
#include "cartographer_ros_msgs/SubmapQuery.h"
|
||||||
|
|
||||||
|
namespace cartographer_ros {
|
||||||
|
|
||||||
|
std::unique_ptr<SubmapTexture> FetchSubmapTexture(
|
||||||
|
const ::cartographer::mapping::SubmapId& submap_id,
|
||||||
|
ros::ServiceClient* client) {
|
||||||
|
::cartographer_ros_msgs::SubmapQuery srv;
|
||||||
|
srv.request.trajectory_id = submap_id.trajectory_id;
|
||||||
|
srv.request.submap_index = submap_id.submap_index;
|
||||||
|
if (!client->call(srv)) {
|
||||||
|
return nullptr;
|
||||||
|
}
|
||||||
|
std::string compressed_cells(srv.response.cells.begin(),
|
||||||
|
srv.response.cells.end());
|
||||||
|
std::string cells;
|
||||||
|
::cartographer::common::FastGunzipString(compressed_cells, &cells);
|
||||||
|
const int num_pixels = srv.response.width * srv.response.height;
|
||||||
|
CHECK_EQ(cells.size(), 2 * num_pixels);
|
||||||
|
std::vector<char> intensity;
|
||||||
|
intensity.reserve(num_pixels);
|
||||||
|
std::vector<char> alpha;
|
||||||
|
alpha.reserve(num_pixels);
|
||||||
|
for (int i = 0; i < srv.response.height; ++i) {
|
||||||
|
for (int j = 0; j < srv.response.width; ++j) {
|
||||||
|
intensity.push_back(cells[(i * srv.response.width + j) * 2]);
|
||||||
|
alpha.push_back(cells[(i * srv.response.width + j) * 2 + 1]);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return ::cartographer::common::make_unique<SubmapTexture>(SubmapTexture{
|
||||||
|
srv.response.submap_version, intensity, alpha, srv.response.width,
|
||||||
|
srv.response.height, srv.response.resolution,
|
||||||
|
ToRigid3d(srv.response.slice_pose)});
|
||||||
|
}
|
||||||
|
|
||||||
|
} // namespace cartographer_ros
|
|
@ -0,0 +1,47 @@
|
||||||
|
/*
|
||||||
|
* 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_SUBMAP_H_
|
||||||
|
#define CARTOGRAPHER_ROS_SUBMAP_H_
|
||||||
|
|
||||||
|
#include <memory>
|
||||||
|
#include <vector>
|
||||||
|
|
||||||
|
#include "cartographer/mapping/id.h"
|
||||||
|
#include "cartographer/transform/rigid_transform.h"
|
||||||
|
#include "ros/ros.h"
|
||||||
|
|
||||||
|
namespace cartographer_ros {
|
||||||
|
|
||||||
|
struct SubmapTexture {
|
||||||
|
int version;
|
||||||
|
std::vector<char> intensity;
|
||||||
|
std::vector<char> alpha;
|
||||||
|
int width;
|
||||||
|
int height;
|
||||||
|
double resolution;
|
||||||
|
::cartographer::transform::Rigid3d slice_pose;
|
||||||
|
};
|
||||||
|
|
||||||
|
// Fetch 'submap_id' using the 'client' and returning the response or 'nullptr'
|
||||||
|
// on error.
|
||||||
|
std::unique_ptr<SubmapTexture> FetchSubmapTexture(
|
||||||
|
const ::cartographer::mapping::SubmapId& submap_id,
|
||||||
|
ros::ServiceClient* client);
|
||||||
|
|
||||||
|
} // namespace cartographer_ros
|
||||||
|
|
||||||
|
#endif // CARTOGRAPHER_ROS_SUBMAP_H_
|
|
@ -97,6 +97,8 @@ DrawableSubmap::DrawableSubmap(const ::cartographer::mapping::SubmapId& id,
|
||||||
}
|
}
|
||||||
|
|
||||||
DrawableSubmap::~DrawableSubmap() {
|
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()) {
|
if (QueryInProgress()) {
|
||||||
rpc_request_future_.wait();
|
rpc_request_future_.wait();
|
||||||
}
|
}
|
||||||
|
@ -120,10 +122,9 @@ void DrawableSubmap::Update(
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
::cartographer::common::MutexLocker locker(&mutex_);
|
::cartographer::common::MutexLocker locker(&mutex_);
|
||||||
submap_z_ = metadata.pose.position.z;
|
|
||||||
metadata_version_ = metadata.submap_version;
|
metadata_version_ = metadata.submap_version;
|
||||||
pose_ = ::cartographer_ros::ToRigid3d(metadata.pose);
|
pose_ = ::cartographer_ros::ToRigid3d(metadata.pose);
|
||||||
if (texture_version_ != -1) {
|
if (submap_texture_ != nullptr) {
|
||||||
// We have to update the transform since we are already displaying a texture
|
// We have to update the transform since we are already displaying a texture
|
||||||
// for this submap.
|
// for this submap.
|
||||||
UpdateTransform();
|
UpdateTransform();
|
||||||
|
@ -140,8 +141,10 @@ void DrawableSubmap::Update(
|
||||||
|
|
||||||
bool DrawableSubmap::MaybeFetchTexture(ros::ServiceClient* const client) {
|
bool DrawableSubmap::MaybeFetchTexture(ros::ServiceClient* const client) {
|
||||||
::cartographer::common::MutexLocker locker(&mutex_);
|
::cartographer::common::MutexLocker locker(&mutex_);
|
||||||
// Received metadata version can also be lower - if we restarted Cartographer
|
// Received metadata version can also be lower if we restarted Cartographer.
|
||||||
const bool newer_version_available = texture_version_ != metadata_version_;
|
const bool newer_version_available =
|
||||||
|
submap_texture_ == nullptr ||
|
||||||
|
submap_texture_->version != metadata_version_;
|
||||||
const std::chrono::milliseconds now =
|
const std::chrono::milliseconds now =
|
||||||
std::chrono::duration_cast<std::chrono::milliseconds>(
|
std::chrono::duration_cast<std::chrono::milliseconds>(
|
||||||
std::chrono::system_clock::now().time_since_epoch());
|
std::chrono::system_clock::now().time_since_epoch());
|
||||||
|
@ -153,18 +156,16 @@ bool DrawableSubmap::MaybeFetchTexture(ros::ServiceClient* const client) {
|
||||||
query_in_progress_ = true;
|
query_in_progress_ = true;
|
||||||
last_query_timestamp_ = now;
|
last_query_timestamp_ = now;
|
||||||
rpc_request_future_ = std::async(std::launch::async, [this, client]() {
|
rpc_request_future_ = std::async(std::launch::async, [this, client]() {
|
||||||
::cartographer_ros_msgs::SubmapQuery srv;
|
std::unique_ptr<::cartographer_ros::SubmapTexture> submap_texture =
|
||||||
srv.request.trajectory_id = id_.trajectory_id;
|
::cartographer_ros::FetchSubmapTexture(id_, client);
|
||||||
srv.request.submap_index = id_.submap_index;
|
|
||||||
if (client->call(srv)) {
|
|
||||||
// We emit a signal to update in the right thread, and pass via the
|
|
||||||
// 'response_' member to simplify the signal-slot connection slightly.
|
|
||||||
::cartographer::common::MutexLocker locker(&mutex_);
|
|
||||||
response_ = srv.response;
|
|
||||||
Q_EMIT RequestSucceeded();
|
|
||||||
} else {
|
|
||||||
::cartographer::common::MutexLocker locker(&mutex_);
|
::cartographer::common::MutexLocker locker(&mutex_);
|
||||||
query_in_progress_ = false;
|
query_in_progress_ = false;
|
||||||
|
if (submap_texture != 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_texture_ = std::move(submap_texture);
|
||||||
|
Q_EMIT RequestSucceeded();
|
||||||
}
|
}
|
||||||
});
|
});
|
||||||
return true;
|
return true;
|
||||||
|
@ -176,7 +177,8 @@ bool DrawableSubmap::QueryInProgress() {
|
||||||
}
|
}
|
||||||
|
|
||||||
void DrawableSubmap::SetAlpha(const double current_tracking_z) {
|
void DrawableSubmap::SetAlpha(const double current_tracking_z) {
|
||||||
const double distance_z = std::abs(submap_z_ - current_tracking_z);
|
const double distance_z =
|
||||||
|
std::abs(pose_.translation().z() - current_tracking_z);
|
||||||
const double fade_distance =
|
const double fade_distance =
|
||||||
std::max(distance_z - kFadeOutStartDistanceInMeters, 0.);
|
std::max(distance_z - kFadeOutStartDistanceInMeters, 0.);
|
||||||
const float alpha = static_cast<float>(
|
const float alpha = static_cast<float>(
|
||||||
|
@ -189,29 +191,21 @@ void DrawableSubmap::SetAlpha(const double current_tracking_z) {
|
||||||
|
|
||||||
void DrawableSubmap::UpdateSceneNode() {
|
void DrawableSubmap::UpdateSceneNode() {
|
||||||
::cartographer::common::MutexLocker locker(&mutex_);
|
::cartographer::common::MutexLocker locker(&mutex_);
|
||||||
texture_version_ = response_.submap_version;
|
|
||||||
std::string compressed_cells(response_.cells.begin(), response_.cells.end());
|
|
||||||
std::string cells;
|
|
||||||
::cartographer::common::FastGunzipString(compressed_cells, &cells);
|
|
||||||
slice_pose_ = ::cartographer_ros::ToRigid3d(response_.slice_pose);
|
|
||||||
UpdateTransform();
|
UpdateTransform();
|
||||||
query_in_progress_ = false;
|
|
||||||
// The call to Ogre's loadRawData below does not work with an RG texture,
|
// 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.
|
// therefore we create an RGB one whose blue channel is always 0.
|
||||||
std::vector<char> rgb;
|
std::vector<char> rgb;
|
||||||
for (int i = 0; i < response_.height; ++i) {
|
for (size_t i = 0; i < submap_texture_->intensity.size(); ++i) {
|
||||||
for (int j = 0; j < response_.width; ++j) {
|
rgb.push_back(submap_texture_->intensity[i]);
|
||||||
auto r = cells[(i * response_.width + j) * 2];
|
rgb.push_back(submap_texture_->alpha[i]);
|
||||||
auto g = cells[(i * response_.width + j) * 2 + 1];
|
|
||||||
rgb.push_back(r);
|
|
||||||
rgb.push_back(g);
|
|
||||||
rgb.push_back(0.);
|
rgb.push_back(0.);
|
||||||
}
|
}
|
||||||
}
|
|
||||||
|
|
||||||
manual_object_->clear();
|
manual_object_->clear();
|
||||||
const float metric_width = response_.resolution * response_.width;
|
const float metric_width =
|
||||||
const float metric_height = response_.resolution * response_.height;
|
submap_texture_->resolution * submap_texture_->width;
|
||||||
|
const float metric_height =
|
||||||
|
submap_texture_->resolution * submap_texture_->height;
|
||||||
manual_object_->begin(material_->getName(),
|
manual_object_->begin(material_->getName(),
|
||||||
Ogre::RenderOperation::OT_TRIANGLE_STRIP);
|
Ogre::RenderOperation::OT_TRIANGLE_STRIP);
|
||||||
// Bottom left
|
// Bottom left
|
||||||
|
@ -239,8 +233,8 @@ void DrawableSubmap::UpdateSceneNode() {
|
||||||
kSubmapTexturePrefix + GetSubmapIdentifier(id_);
|
kSubmapTexturePrefix + GetSubmapIdentifier(id_);
|
||||||
texture_ = Ogre::TextureManager::getSingleton().loadRawData(
|
texture_ = Ogre::TextureManager::getSingleton().loadRawData(
|
||||||
texture_name, Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME,
|
texture_name, Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME,
|
||||||
pixel_stream, response_.width, response_.height, Ogre::PF_BYTE_RGB,
|
pixel_stream, submap_texture_->width, submap_texture_->height,
|
||||||
Ogre::TEX_TYPE_2D, 0);
|
Ogre::PF_BYTE_RGB, Ogre::TEX_TYPE_2D, 0);
|
||||||
|
|
||||||
Ogre::Pass* const pass = material_->getTechnique(0)->getPass(0);
|
Ogre::Pass* const pass = material_->getTechnique(0)->getPass(0);
|
||||||
pass->setSceneBlending(Ogre::SBF_ONE, Ogre::SBF_ONE_MINUS_SOURCE_ALPHA);
|
pass->setSceneBlending(Ogre::SBF_ONE, Ogre::SBF_ONE_MINUS_SOURCE_ALPHA);
|
||||||
|
@ -253,7 +247,9 @@ void DrawableSubmap::UpdateSceneNode() {
|
||||||
}
|
}
|
||||||
|
|
||||||
void DrawableSubmap::UpdateTransform() {
|
void DrawableSubmap::UpdateTransform() {
|
||||||
const ::cartographer::transform::Rigid3d pose = pose_ * slice_pose_;
|
CHECK(submap_texture_ != nullptr);
|
||||||
|
const ::cartographer::transform::Rigid3d pose =
|
||||||
|
pose_ * submap_texture_->slice_pose;
|
||||||
scene_node_->setPosition(ToOgre(pose.translation()));
|
scene_node_->setPosition(ToOgre(pose.translation()));
|
||||||
scene_node_->setOrientation(ToOgre(pose.rotation()));
|
scene_node_->setOrientation(ToOgre(pose.rotation()));
|
||||||
}
|
}
|
||||||
|
|
|
@ -18,6 +18,7 @@
|
||||||
#define CARTOGRAPHER_RVIZ_SRC_DRAWABLE_SUBMAP_H_
|
#define CARTOGRAPHER_RVIZ_SRC_DRAWABLE_SUBMAP_H_
|
||||||
|
|
||||||
#include <future>
|
#include <future>
|
||||||
|
#include <memory>
|
||||||
|
|
||||||
#include "Eigen/Core"
|
#include "Eigen/Core"
|
||||||
#include "Eigen/Geometry"
|
#include "Eigen/Geometry"
|
||||||
|
@ -31,6 +32,7 @@
|
||||||
#include "cartographer/common/mutex.h"
|
#include "cartographer/common/mutex.h"
|
||||||
#include "cartographer/mapping/id.h"
|
#include "cartographer/mapping/id.h"
|
||||||
#include "cartographer/transform/rigid_transform.h"
|
#include "cartographer/transform/rigid_transform.h"
|
||||||
|
#include "cartographer_ros/submap.h"
|
||||||
#include "cartographer_ros_msgs/SubmapEntry.h"
|
#include "cartographer_ros_msgs/SubmapEntry.h"
|
||||||
#include "cartographer_ros_msgs/SubmapQuery.h"
|
#include "cartographer_ros_msgs/SubmapQuery.h"
|
||||||
#include "ros/ros.h"
|
#include "ros/ros.h"
|
||||||
|
@ -99,15 +101,13 @@ class DrawableSubmap : public QObject {
|
||||||
Ogre::ManualObject* manual_object_;
|
Ogre::ManualObject* manual_object_;
|
||||||
Ogre::TexturePtr texture_;
|
Ogre::TexturePtr texture_;
|
||||||
Ogre::MaterialPtr material_;
|
Ogre::MaterialPtr material_;
|
||||||
double submap_z_ = 0. GUARDED_BY(mutex_);
|
|
||||||
::cartographer::transform::Rigid3d pose_ GUARDED_BY(mutex_);
|
::cartographer::transform::Rigid3d pose_ GUARDED_BY(mutex_);
|
||||||
::cartographer::transform::Rigid3d slice_pose_ GUARDED_BY(mutex_);
|
|
||||||
std::chrono::milliseconds last_query_timestamp_ GUARDED_BY(mutex_);
|
std::chrono::milliseconds last_query_timestamp_ GUARDED_BY(mutex_);
|
||||||
bool query_in_progress_ = false GUARDED_BY(mutex_);
|
bool query_in_progress_ = false GUARDED_BY(mutex_);
|
||||||
int metadata_version_ = -1 GUARDED_BY(mutex_);
|
int metadata_version_ = -1 GUARDED_BY(mutex_);
|
||||||
int texture_version_ = -1 GUARDED_BY(mutex_);
|
|
||||||
std::future<void> rpc_request_future_;
|
std::future<void> rpc_request_future_;
|
||||||
::cartographer_ros_msgs::SubmapQuery::Response response_ GUARDED_BY(mutex_);
|
std::unique_ptr<::cartographer_ros::SubmapTexture> submap_texture_
|
||||||
|
GUARDED_BY(mutex_);
|
||||||
float current_alpha_ = 0.f;
|
float current_alpha_ = 0.f;
|
||||||
std::unique_ptr<::rviz::BoolProperty> visibility_;
|
std::unique_ptr<::rviz::BoolProperty> visibility_;
|
||||||
};
|
};
|
||||||
|
|
Loading…
Reference in New Issue