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

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
cartographer_ros/cartographer_ros/occupancy_grid_node_main.cc
View File

@ -162,13 +162,18 @@ void Node::HandleSubmapList(
continue;
}
auto fetched_texture = ::cartographer_ros::FetchSubmapTexture(id, &client_);
if (fetched_texture == nullptr) {
auto fetched_textures =
::cartographer_ros::FetchSubmapTextures(id, &client_);
if (fetched_textures == nullptr) {
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.height = fetched_texture->height;
submap_state.version = fetched_texture->version;
submap_state.slice_pose = fetched_texture->slice_pose;
submap_state.resolution = fetched_texture->resolution;

15
cartographer_ros/cartographer_ros/submap.cc
View File

@ -24,7 +24,7 @@
namespace cartographer_ros {
std::unique_ptr<SubmapTexture> FetchSubmapTexture(
std::unique_ptr<SubmapTextures> FetchSubmapTextures(
const ::cartographer::mapping::SubmapId& submap_id,
ros::ServiceClient* client) {
::cartographer_ros_msgs::SubmapQuery srv;
@ -34,8 +34,9 @@ std::unique_ptr<SubmapTexture> FetchSubmapTexture(
return nullptr;
}
CHECK(!srv.response.textures.empty());
// TODO(gaschler): Forward all the textures.
const auto& texture = srv.response.textures[0];
auto response = ::cartographer::common::make_unique<SubmapTextures>();
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 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]);
}
}
return ::cartographer::common::make_unique<SubmapTexture>(SubmapTexture{
srv.response.submap_version, intensity, alpha, texture.width,
texture.height, texture.resolution, ToRigid3d(texture.slice_pose)});
response->textures.emplace_back(
SubmapTexture{intensity, alpha, texture.width, texture.height,
texture.resolution, ToRigid3d(texture.slice_pose)});
}
return response;
}
} // namespace cartographer_ros

8
cartographer_ros/cartographer_ros/submap.h
View File

@ -27,7 +27,6 @@
namespace cartographer_ros {
struct SubmapTexture {
int version;
std::vector<char> intensity;
std::vector<char> alpha;
int width;
@ -36,9 +35,14 @@ struct SubmapTexture {
::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'
// on error.
std::unique_ptr<SubmapTexture> FetchSubmapTexture(
std::unique_ptr<SubmapTextures> FetchSubmapTextures(
const ::cartographer::mapping::SubmapId& submap_id,
ros::ServiceClient* client);

15
cartographer_rviz/cartographer_rviz/drawable_submap.cc
View File

@ -117,8 +117,8 @@ bool DrawableSubmap::MaybeFetchTexture(ros::ServiceClient* const client) {
::cartographer::common::MutexLocker locker(&mutex_);
// Received metadata version can also be lower if we restarted Cartographer.
const bool newer_version_available =
submap_texture_ == nullptr ||
submap_texture_->version != metadata_version_;
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());
@ -130,15 +130,15 @@ bool DrawableSubmap::MaybeFetchTexture(ros::ServiceClient* const client) {
query_in_progress_ = true;
last_query_timestamp_ = now;
rpc_request_future_ = std::async(std::launch::async, [this, client]() {
std::unique_ptr<::cartographer_ros::SubmapTexture> submap_texture =
::cartographer_ros::FetchSubmapTexture(id_, client);
std::unique_ptr<::cartographer_ros::SubmapTextures> submap_textures =
::cartographer_ros::FetchSubmapTextures(id_, client);
::cartographer::common::MutexLocker locker(&mutex_);
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
// 'submap_texture_' member to simplify the signal-slot connection
// slightly.
submap_texture_ = std::move(submap_texture);
submap_textures_ = std::move(submap_textures);
Q_EMIT RequestSucceeded();
}
});
@ -168,7 +168,8 @@ void DrawableSubmap::SetAlpha(const double current_tracking_z) {
void DrawableSubmap::UpdateSceneNode() {
::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();
}

2
cartographer_rviz/cartographer_rviz/drawable_submap.h
View File

@ -101,7 +101,7 @@ class DrawableSubmap : public QObject {
bool query_in_progress_ = false GUARDED_BY(mutex_);
int metadata_version_ = -1 GUARDED_BY(mutex_);
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_);
float current_alpha_ = 0.f;
std::unique_ptr<::rviz::BoolProperty> visibility_;