diff --git a/cartographer_ros/src/drawable_submap.cc b/cartographer_ros/src/drawable_submap.cc index 61f75d7..6753a9a 100644 --- a/cartographer_ros/src/drawable_submap.cc +++ b/cartographer_ros/src/drawable_submap.cc @@ -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::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 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 = diff --git a/cartographer_ros/src/drawable_submap.h b/cartographer_ros/src/drawable_submap.h index 1ebf77d..2ce8da0 100644 --- a/cartographer_ros/src/drawable_submap.h +++ b/cartographer_ros/src/drawable_submap.h @@ -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 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 diff --git a/cartographer_ros/src/submaps_display.cc b/cartographer_ros/src/submaps_display.cc index b8653bb..3ca858a 100644 --- a/cartographer_ros/src/submaps_display.cc +++ b/cartographer_ros/src/submaps_display.cc @@ -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( - 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( + 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() {