Simplify the Rviz submap texture update logic. (#12)
Wolfgang Hess
GitHub
parent
d2b583ddf7
commit
d8a4c07464
|
|
@ -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,50 +106,26 @@ 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]() {
|
||||
::cartographer_ros_msgs::SubmapQuery srv;
|
||||
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));
|
||||
Q_EMIT RequestSucceeded();
|
||||
} else {
|
||||
OnRequestFailure();
|
||||
}
|
||||
});
|
||||
}
|
||||
|
||||
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;
|
||||
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_;
|
||||
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_);
|
||||
query_in_progress_ = false;
|
||||
}
|
||||
});
|
||||
}
|
||||
|
||||
bool DrawableSubmap::QueryInProgress() {
|
||||
|
|
@ -158,13 +134,22 @@ bool DrawableSubmap::QueryInProgress() {
|
|||
}
|
||||
|
||||
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 =
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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(
|
||||
::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;
|
||||
}
|
||||
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()));
|
||||
}
|
||||
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() {
|
||||
|
|
|
|||
Loading…
Reference in New Issue