FetchSubmapTextures returns multiple textures. (#519)

This changes the Ros api that multiple textures will be returned,
which are forwarded from the Ros service call.
Adapts all usages to follow this new api.
master
gaschler 2017-10-09 04:19:48 -07:00 committed by GitHub
parent d5e6647206
commit b2a2ac4912
5 changed files with 45 additions and 32 deletions

View File

@ -162,13 +162,18 @@ void Node::HandleSubmapList(
continue; continue;
} }
auto fetched_texture = ::cartographer_ros::FetchSubmapTexture(id, &client_); auto fetched_textures =
if (fetched_texture == nullptr) { ::cartographer_ros::FetchSubmapTextures(id, &client_);
if (fetched_textures == nullptr) {
continue; continue;
} }
CHECK(!fetched_textures->textures.empty());
submap_state.version = fetched_textures->version;
// TODO(gaschler): Handle more textures than just the first one.
const auto fetched_texture = fetched_textures->textures.begin();
submap_state.width = fetched_texture->width; submap_state.width = fetched_texture->width;
submap_state.height = fetched_texture->height; submap_state.height = fetched_texture->height;
submap_state.version = fetched_texture->version;
submap_state.slice_pose = fetched_texture->slice_pose; submap_state.slice_pose = fetched_texture->slice_pose;
submap_state.resolution = fetched_texture->resolution; submap_state.resolution = fetched_texture->resolution;

View File

@ -24,7 +24,7 @@
namespace cartographer_ros { namespace cartographer_ros {
std::unique_ptr<SubmapTexture> FetchSubmapTexture( std::unique_ptr<SubmapTextures> FetchSubmapTextures(
const ::cartographer::mapping::SubmapId& submap_id, const ::cartographer::mapping::SubmapId& submap_id,
ros::ServiceClient* client) { ros::ServiceClient* client) {
::cartographer_ros_msgs::SubmapQuery srv; ::cartographer_ros_msgs::SubmapQuery srv;
@ -34,8 +34,9 @@ std::unique_ptr<SubmapTexture> FetchSubmapTexture(
return nullptr; return nullptr;
} }
CHECK(!srv.response.textures.empty()); CHECK(!srv.response.textures.empty());
// TODO(gaschler): Forward all the textures. auto response = ::cartographer::common::make_unique<SubmapTextures>();
const auto& texture = srv.response.textures[0]; response->version = srv.response.submap_version;
for (const auto& texture : srv.response.textures) {
std::string compressed_cells(texture.cells.begin(), texture.cells.end()); std::string compressed_cells(texture.cells.begin(), texture.cells.end());
std::string cells; std::string cells;
::cartographer::common::FastGunzipString(compressed_cells, &cells); ::cartographer::common::FastGunzipString(compressed_cells, &cells);
@ -51,9 +52,11 @@ std::unique_ptr<SubmapTexture> FetchSubmapTexture(
alpha.push_back(cells[(i * texture.width + j) * 2 + 1]); alpha.push_back(cells[(i * texture.width + j) * 2 + 1]);
} }
} }
return ::cartographer::common::make_unique<SubmapTexture>(SubmapTexture{ response->textures.emplace_back(
srv.response.submap_version, intensity, alpha, texture.width, SubmapTexture{intensity, alpha, texture.width, texture.height,
texture.height, texture.resolution, ToRigid3d(texture.slice_pose)}); texture.resolution, ToRigid3d(texture.slice_pose)});
}
return response;
} }
} // namespace cartographer_ros } // namespace cartographer_ros

View File

@ -27,7 +27,6 @@
namespace cartographer_ros { namespace cartographer_ros {
struct SubmapTexture { struct SubmapTexture {
int version;
std::vector<char> intensity; std::vector<char> intensity;
std::vector<char> alpha; std::vector<char> alpha;
int width; int width;
@ -36,9 +35,14 @@ struct SubmapTexture {
::cartographer::transform::Rigid3d slice_pose; ::cartographer::transform::Rigid3d slice_pose;
}; };
struct SubmapTextures {
int version;
std::vector<SubmapTexture> textures;
};
// Fetch 'submap_id' using the 'client' and returning the response or 'nullptr' // Fetch 'submap_id' using the 'client' and returning the response or 'nullptr'
// on error. // on error.
std::unique_ptr<SubmapTexture> FetchSubmapTexture( std::unique_ptr<SubmapTextures> FetchSubmapTextures(
const ::cartographer::mapping::SubmapId& submap_id, const ::cartographer::mapping::SubmapId& submap_id,
ros::ServiceClient* client); ros::ServiceClient* client);

View File

@ -117,8 +117,8 @@ 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 = const bool newer_version_available =
submap_texture_ == nullptr || submap_textures_ == nullptr ||
submap_texture_->version != metadata_version_; submap_textures_->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());
@ -130,15 +130,15 @@ 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]() {
std::unique_ptr<::cartographer_ros::SubmapTexture> submap_texture = std::unique_ptr<::cartographer_ros::SubmapTextures> submap_textures =
::cartographer_ros::FetchSubmapTexture(id_, client); ::cartographer_ros::FetchSubmapTextures(id_, client);
::cartographer::common::MutexLocker locker(&mutex_); ::cartographer::common::MutexLocker locker(&mutex_);
query_in_progress_ = false; query_in_progress_ = false;
if (submap_texture != nullptr) { if (submap_textures != nullptr) {
// We emit a signal to update in the right thread, and pass via the // We emit a signal to update in the right thread, and pass via the
// 'submap_texture_' member to simplify the signal-slot connection // 'submap_texture_' member to simplify the signal-slot connection
// slightly. // slightly.
submap_texture_ = std::move(submap_texture); submap_textures_ = std::move(submap_textures);
Q_EMIT RequestSucceeded(); Q_EMIT RequestSucceeded();
} }
}); });
@ -168,7 +168,8 @@ void DrawableSubmap::SetAlpha(const double current_tracking_z) {
void DrawableSubmap::UpdateSceneNode() { void DrawableSubmap::UpdateSceneNode() {
::cartographer::common::MutexLocker locker(&mutex_); ::cartographer::common::MutexLocker locker(&mutex_);
ogre_submap_.Update(*submap_texture_); // TODO(gaschler): Add UI feature to show all textures.
ogre_submap_.Update(submap_textures_->textures[0]);
display_context_->queueRender(); display_context_->queueRender();
} }

View File

@ -101,7 +101,7 @@ class DrawableSubmap : public QObject {
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_);
std::future<void> rpc_request_future_; std::future<void> rpc_request_future_;
std::unique_ptr<::cartographer_ros::SubmapTexture> submap_texture_ std::unique_ptr<::cartographer_ros::SubmapTextures> submap_textures_
GUARDED_BY(mutex_); 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_;