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

Simplify the Rviz submap texture update logic. (#12)

master
Wolfgang Hess 2016-08-05 14:23:30 +02:00 committed by GitHub
parent d2b583ddf7
commit d8a4c07464
3 changed files with 88 additions and 118 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

97
cartographer_ros/src/drawable_submap.cc
View File

@ -64,11 +64,7 @@ DrawableSubmap::DrawableSubmap(const int submap_id, const int trajectory_id,
kManualObjectPrefix + GetSubmapIdentifier(trajectory_id, submap_id))),
submap_id_(submap_id),
trajectory_id_(trajectory_id),
last_query_timestamp_(0),
query_in_progress_(false),
version_(0),
texture_count_(0),
current_alpha_(0) {
last_query_timestamp_(0) {
material_ = Ogre::MaterialManager::getSingleton().getByName(
kSubmapSourceMaterialName);
material_ = material_->clone(kSubmapMaterialPrefix +
@ -77,7 +73,7 @@ DrawableSubmap::DrawableSubmap(const int submap_id, const int trajectory_id,
material_->getTechnique(0)->setLightingEnabled(false);
material_->setDepthWriteEnabled(false);
scene_node_->attachObject(manual_object_);
connect(this, SIGNAL(RequestSucceeded()), this, SLOT(OnRequestSuccess()));
connect(this, SIGNAL(RequestSucceeded()), this, SLOT(UpdateSceneNode()));
}
DrawableSubmap::~DrawableSubmap() {
@ -90,12 +86,16 @@ DrawableSubmap::~DrawableSubmap() {
scene_manager_->destroyManualObject(manual_object_);
}
bool DrawableSubmap::Update(
const ::cartographer_ros_msgs::SubmapEntry& metadata,
ros::ServiceClient* const client) {
void DrawableSubmap::Update(
const ::cartographer_ros_msgs::SubmapEntry& metadata) {
::cartographer::common::MutexLocker locker(&mutex_);
tf::poseMsgToEigen(metadata.pose, submap_pose_);
const bool newer_version_available = version_ < metadata.submap_version;
metadata_version_ = metadata.submap_version;
}
bool DrawableSubmap::MaybeFetchTexture(ros::ServiceClient* const client) {
::cartographer::common::MutexLocker locker(&mutex_);
const bool newer_version_available = texture_version_ < metadata_version_;
const std::chrono::milliseconds now =
std::chrono::duration_cast<std::chrono::milliseconds>(
std::chrono::system_clock::now().time_since_epoch());
@ -106,65 +106,50 @@ bool DrawableSubmap::Update(
}
query_in_progress_ = true;
last_query_timestamp_ = now;
last_query_slice_height_ = metadata.pose.position.z;
QuerySubmap(submap_id_, trajectory_id_, client);
QuerySubmap(client);
return true;
}
void DrawableSubmap::QuerySubmap(const int submap_id, const int trajectory_id,
ros::ServiceClient* const client) {
rpc_request_future_ = std::async(
std::launch::async, [this, submap_id, trajectory_id, client]() {
void DrawableSubmap::QuerySubmap(ros::ServiceClient* const client) {
rpc_request_future_ = std::async(std::launch::async, [this, client]() {
::cartographer_ros_msgs::SubmapQuery srv;
srv.request.submap_id = submap_id;
srv.request.trajectory_id = trajectory_id;
srv.request.submap_id = submap_id_;
srv.request.trajectory_id = trajectory_id_;
if (client->call(srv)) {
response_.reset(
new ::cartographer_ros_msgs::SubmapQuery::Response(srv.response));
// 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 {
OnRequestFailure();
::cartographer::common::MutexLocker locker(&mutex_);
query_in_progress_ = false;
}
});
}
void DrawableSubmap::OnRequestSuccess() {
::cartographer::common::MutexLocker locker(&mutex_);
version_ = response_->submap_version;
resolution_ = response_->resolution;
width_ = response_->width;
height_ = response_->height;
slice_height_ = last_query_slice_height_;
std::string compressed_cells(response_->cells.begin(),
response_->cells.end());
cells_.clear();
::cartographer::common::FastGunzipString(compressed_cells, &cells_);
Eigen::Affine3d slice_pose;
tf::poseMsgToEigen(response_->slice_pose, slice_pose);
tf::poseEigenToMsg(submap_pose_ * slice_pose, transformed_pose_);
response_.reset();
query_in_progress_ = false;
UpdateSceneNode();
}
void DrawableSubmap::OnRequestFailure() {
::cartographer::common::MutexLocker locker(&mutex_);
query_in_progress_ = false;
}
bool DrawableSubmap::QueryInProgress() {
::cartographer::common::MutexLocker locker(&mutex_);
return query_in_progress_;
}
void DrawableSubmap::UpdateSceneNode() {
::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);
Eigen::Affine3d slice_pose;
tf::poseMsgToEigen(response_.slice_pose, slice_pose);
tf::poseEigenToMsg(submap_pose_ * slice_pose, transformed_pose_);
query_in_progress_ = false;
// 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;
for (int i = 0; i < height_; ++i) {
for (int j = 0; j < width_; ++j) {
auto r = cells_[(i * width_ + j) * 2];
auto g = cells_[(i * width_ + j) * 2 + 1];
for (int i = 0; i < response_.height; ++i) {
for (int j = 0; j < response_.width; ++j) {
auto r = cells[(i * response_.width + j) * 2];
auto g = cells[(i * response_.width + j) * 2 + 1];
rgb.push_back(r);
rgb.push_back(g);
rgb.push_back(0.);
@ -172,8 +157,8 @@ void DrawableSubmap::UpdateSceneNode() {
}
manual_object_->clear();
const float metric_width = resolution_ * width_;
const float metric_height = resolution_ * height_;
const float metric_width = response_.resolution * response_.width;
const float metric_height = response_.resolution * response_.height;
manual_object_->begin(material_->getName(),
Ogre::RenderOperation::OT_TRIANGLE_LIST);
@ -221,12 +206,11 @@ void DrawableSubmap::UpdateSceneNode() {
texture_.setNull();
}
const std::string texture_name =
kSubmapTexturePrefix + GetSubmapIdentifier(trajectory_id_, submap_id_) +
std::to_string(texture_count_);
kSubmapTexturePrefix + GetSubmapIdentifier(trajectory_id_, submap_id_);
texture_ = Ogre::TextureManager::getSingleton().loadRawData(
texture_name, Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME,
pixel_stream, width_, height_, Ogre::PF_BYTE_RGB, Ogre::TEX_TYPE_2D, 0);
++texture_count_;
pixel_stream, response_.width, response_.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);
@ -248,7 +232,8 @@ void DrawableSubmap::Transform(::rviz::FrameManager* const frame_manager) {
}
void DrawableSubmap::SetAlpha(const double current_tracking_z) {
const double distance_z = std::abs(slice_height_ - current_tracking_z);
const double distance_z =
std::abs(submap_pose_.translation().z() - current_tracking_z);
const double fade_distance =
std::max(distance_z - kFadeOutStartDistanceInMeters, 0.);
const float alpha =

36
cartographer_ros/src/drawable_submap.h
View File

@ -49,11 +49,13 @@ class DrawableSubmap : public QObject {
DrawableSubmap(const DrawableSubmap&) = delete;
DrawableSubmap& operator=(const DrawableSubmap&) = delete;
// 'submap_entry' contains metadata which is used to find out whether this
// 'DrawableSubmap' should update itself. If an update is needed, it will send
// an RPC using 'client' to request the new data for the submap.
bool Update(const ::cartographer_ros_msgs::SubmapEntry& submap_entry,
ros::ServiceClient* client);
// Updates the 'metadata' for this submap. If necessary, the next call to
// MaybeFetchTexture() will fetch a new submap texture.
void Update(const ::cartographer_ros_msgs::SubmapEntry& metadata);
// If an update is needed, it will send an RPC using 'client' to request the
// new data for the submap and returns true.
bool MaybeFetchTexture(ros::ServiceClient* client);
// Returns whether an RPC is in progress.
bool QueryInProgress();
@ -72,13 +74,10 @@ class DrawableSubmap : public QObject {
private Q_SLOTS:
// Callback when an rpc request succeeded.
void OnRequestSuccess();
void UpdateSceneNode();
private:
void QuerySubmap(int submap_id, int trajectory_id,
ros::ServiceClient* client);
void OnRequestFailure();
void UpdateSceneNode();
void QuerySubmap(ros::ServiceClient* client);
float UpdateAlpha(float target_alpha);
const int submap_id_;
@ -93,19 +92,12 @@ class DrawableSubmap : public QObject {
Eigen::Affine3d submap_pose_ GUARDED_BY(mutex_);
geometry_msgs::Pose transformed_pose_ GUARDED_BY(mutex_);
std::chrono::milliseconds last_query_timestamp_ GUARDED_BY(mutex_);
bool query_in_progress_ GUARDED_BY(mutex_);
float resolution_ GUARDED_BY(mutex_);
int width_ GUARDED_BY(mutex_);
int height_ GUARDED_BY(mutex_);
int version_ GUARDED_BY(mutex_);
double slice_height_ GUARDED_BY(mutex_);
double last_query_slice_height_ GUARDED_BY(mutex_);
bool query_in_progress_ = false GUARDED_BY(mutex_);
int metadata_version_ = -1 GUARDED_BY(mutex_);
int texture_version_ = -1 GUARDED_BY(mutex_);
std::future<void> rpc_request_future_;
std::string cells_ GUARDED_BY(mutex_);
std::unique_ptr<::cartographer_ros_msgs::SubmapQuery::Response> response_
GUARDED_BY(mutex_);
int texture_count_;
float current_alpha_;
::cartographer_ros_msgs::SubmapQuery::Response response_ GUARDED_BY(mutex_);
float current_alpha_ = 0.f;
};
} // namespace rviz

57
cartographer_ros/src/submaps_display.cc
View File

@ -38,7 +38,7 @@ namespace rviz {
namespace {
constexpr int kMaxOnGoingRequests = 6;
constexpr int kMaxOnGoingRequestsPerTrajectory = 6;
constexpr char kMaterialsDirectory[] = "/ogre_media/materials";
constexpr char kGlsl120Directory[] = "/glsl120";
constexpr char kScriptsDirectory[] = "/scripts";
@ -104,47 +104,22 @@ void SubmapsDisplay::processMessage(
if (trajectory_id >= trajectories_.size()) {
trajectories_.emplace_back();
}
auto& trajectory = trajectories_[trajectory_id];
const std::vector<::cartographer_ros_msgs::SubmapEntry>& submap_entries =
msg->trajectory[trajectory_id].submap;
if (submap_entries.empty()) {
return;
}
for (int submap_id = trajectories_[trajectory_id].size();
submap_id < submap_entries.size(); ++submap_id) {
trajectories_[trajectory_id].push_back(
for (int submap_id = 0; submap_id < submap_entries.size(); ++submap_id) {
if (submap_id >= trajectory.size()) {
trajectory.push_back(
::cartographer::common::make_unique<DrawableSubmap>(
submap_id, trajectory_id, context_->getSceneManager()));
}
}
int num_ongoing_requests = 0;
for (const auto& trajectory : trajectories_) {
for (const auto& submap : trajectory) {
if (submap->QueryInProgress()) {
++num_ongoing_requests;
if (num_ongoing_requests == kMaxOnGoingRequests) {
return;
}
}
}
}
for (int trajectory_id = 0; trajectory_id < msg->trajectory.size();
++trajectory_id) {
const std::vector<::cartographer_ros_msgs::SubmapEntry>& submap_entries =
msg->trajectory[trajectory_id].submap;
for (int submap_id = submap_entries.size() - 1; submap_id >= 0;
--submap_id) {
if (trajectories_[trajectory_id][submap_id]->Update(
submap_entries[submap_id], &client_)) {
++num_ongoing_requests;
if (num_ongoing_requests == kMaxOnGoingRequests) {
return;
}
}
trajectory[submap_id]->Update(submap_entries[submap_id]);
}
}
}
void SubmapsDisplay::update(const float wall_dt, const float ros_dt) {
// Update the fading by z distance.
try {
const ::geometry_msgs::TransformStamped transform_stamped =
tf_buffer_.lookupTransform(map_frame_property_->getStdString(),
@ -159,6 +134,24 @@ void SubmapsDisplay::update(const float wall_dt, const float ros_dt) {
} catch (const tf2::TransformException& ex) {
ROS_WARN("Could not compute submap fading: %s", ex.what());
}
// Schedule fetching of new submap textures.
for (const auto& trajectory : trajectories_) {
int num_ongoing_requests = 0;
for (const auto& submap : trajectory) {
if (submap->QueryInProgress()) {
++num_ongoing_requests;
}
}
for (int submap_id = trajectory.size() - 1;
submap_id >= 0 &&
num_ongoing_requests < kMaxOnGoingRequestsPerTrajectory;
--submap_id) {
if (trajectory[submap_id]->MaybeFetchTexture(&client_)) {
++num_ongoing_requests;
}
}
}
}
void SubmapsDisplay::UpdateTransforms() {