From 5439e9ba3673b680e60591ff0f86853acc966c25 Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Wed, 10 Aug 2016 12:31:19 +0200 Subject: [PATCH] Fix the submap transformation in the RViz plugin. (#13) Removes the SceneManagerListener and instead computes the transform when a new submap list is received. The second part of the transform is also updated when a new texture is received. Also simplifies the drawing code a bit by using a triangle strip. --- cartographer_ros/src/drawable_submap.cc | 123 +++++++++++------------- cartographer_ros/src/drawable_submap.h | 19 ++-- cartographer_ros/src/submaps_display.cc | 19 +--- cartographer_ros/src/submaps_display.h | 24 +---- 4 files changed, 69 insertions(+), 116 deletions(-) diff --git a/cartographer_ros/src/drawable_submap.cc b/cartographer_ros/src/drawable_submap.cc index 6753a9a..a98b46b 100644 --- a/cartographer_ros/src/drawable_submap.cc +++ b/cartographer_ros/src/drawable_submap.cc @@ -22,8 +22,6 @@ #include #include #include -#include -#include #include #include @@ -38,7 +36,6 @@ namespace rviz { namespace { constexpr std::chrono::milliseconds kMinQueryDelayInMs(250); -constexpr char kMapFrame[] = "/map"; constexpr char kSubmapTexturePrefix[] = "SubmapTexture"; constexpr char kManualObjectPrefix[] = "ManualObjectSubmap"; constexpr char kSubmapMaterialPrefix[] = "SubmapMaterial"; @@ -87,10 +84,25 @@ DrawableSubmap::~DrawableSubmap() { } void DrawableSubmap::Update( - const ::cartographer_ros_msgs::SubmapEntry& metadata) { + const ::std_msgs::Header& header, + const ::cartographer_ros_msgs::SubmapEntry& metadata, + ::rviz::FrameManager* const frame_manager) { + Ogre::Vector3 position; + Ogre::Quaternion orientation; + if (!frame_manager->transform(header, metadata.pose, position, orientation)) { + // We don't know where we would display the texture, so we stop here. + return; + } ::cartographer::common::MutexLocker locker(&mutex_); - tf::poseMsgToEigen(metadata.pose, submap_pose_); + position_ = position; + orientation_ = orientation; + submap_z_ = metadata.pose.position.z; metadata_version_ = metadata.submap_version; + if (texture_version_ != -1) { + // We have to update the transform since we are already displaying a texture + // for this submap. + UpdateTransform(); + } } bool DrawableSubmap::MaybeFetchTexture(ros::ServiceClient* const client) { @@ -106,11 +118,6 @@ bool DrawableSubmap::MaybeFetchTexture(ros::ServiceClient* const client) { } query_in_progress_ = true; last_query_timestamp_ = now; - QuerySubmap(client); - return true; -} - -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_; @@ -126,6 +133,7 @@ void DrawableSubmap::QuerySubmap(ros::ServiceClient* const client) { query_in_progress_ = false; } }); + return true; } bool DrawableSubmap::QueryInProgress() { @@ -133,15 +141,26 @@ bool DrawableSubmap::QueryInProgress() { return query_in_progress_; } +void DrawableSubmap::SetAlpha(const double current_tracking_z) { + const double distance_z = std::abs(submap_z_ - current_tracking_z); + const double fade_distance = + std::max(distance_z - kFadeOutStartDistanceInMeters, 0.); + const float alpha = static_cast( + std::max(0., 1. - fade_distance / kFadeOutDistanceInMeters)); + + const Ogre::GpuProgramParametersSharedPtr parameters = + material_->getTechnique(0)->getPass(0)->getFragmentProgramParameters(); + parameters->setNamedConstant("u_alpha", UpdateAlpha(alpha)); +} + 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_); + tf::poseMsgToEigen(response_.slice_pose, slice_pose_); + UpdateTransform(); 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. @@ -159,43 +178,20 @@ void DrawableSubmap::UpdateSceneNode() { manual_object_->clear(); 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); - { - { - // Bottom left - manual_object_->position(-metric_height, 0.0f, 0.0f); - manual_object_->textureCoord(0.0f, 1.0f); - manual_object_->normal(0.0f, 0.0f, 1.0f); - - // Bottom right - manual_object_->position(-metric_height, -metric_width, 0.0f); - manual_object_->textureCoord(1.0f, 1.0f); - manual_object_->normal(0.0f, 0.0f, 1.0f); - - // Top left - manual_object_->position(0.0f, 0.0f, 0.0f); - manual_object_->textureCoord(0.0f, 0.0f); - manual_object_->normal(0.0f, 0.0f, 1.0f); - - // Top left - manual_object_->position(0.0f, 0.0f, 0.0f); - manual_object_->textureCoord(0.0f, 0.0f); - manual_object_->normal(0.0f, 0.0f, 1.0f); - - // Bottom right - manual_object_->position(-metric_height, -metric_width, 0.0f); - manual_object_->textureCoord(1.0f, 1.0f); - manual_object_->normal(0.0f, 0.0f, 1.0f); - - // Top right - manual_object_->position(0.0f, -metric_width, 0.0f); - manual_object_->textureCoord(1.0f, 0.0f); - manual_object_->normal(0.0f, 0.0f, 1.0f); - } - } - + Ogre::RenderOperation::OT_TRIANGLE_STRIP); + // Bottom left + manual_object_->position(-metric_height, 0.0f, 0.0f); + manual_object_->textureCoord(0.0f, 1.0f); + // Bottom right + manual_object_->position(-metric_height, -metric_width, 0.0f); + manual_object_->textureCoord(1.0f, 1.0f); + // Top left + manual_object_->position(0.0f, 0.0f, 0.0f); + manual_object_->textureCoord(0.0f, 0.0f); + // Top right + manual_object_->position(0.0f, -metric_width, 0.0f); + manual_object_->textureCoord(1.0f, 0.0f); manual_object_->end(); Ogre::DataStreamPtr pixel_stream; @@ -222,26 +218,15 @@ void DrawableSubmap::UpdateSceneNode() { texture_unit->setTextureFiltering(Ogre::TFO_NONE); } -void DrawableSubmap::Transform(::rviz::FrameManager* const frame_manager) { - Ogre::Vector3 position; - Ogre::Quaternion orientation; - frame_manager->transform(kMapFrame, ros::Time(0) /* latest */, - transformed_pose_, position, orientation); - scene_node_->setPosition(position); - scene_node_->setOrientation(orientation); -} - -void DrawableSubmap::SetAlpha(const double 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 = - (float)std::max(0., 1. - fade_distance / kFadeOutDistanceInMeters); - - const Ogre::GpuProgramParametersSharedPtr parameters = - material_->getTechnique(0)->getPass(0)->getFragmentProgramParameters(); - parameters->setNamedConstant("u_alpha", UpdateAlpha(alpha)); +void DrawableSubmap::UpdateTransform() { + const Eigen::Quaterniond quaternion(slice_pose_.rotation()); + const Ogre::Quaternion slice_rotation(quaternion.w(), quaternion.x(), + quaternion.y(), quaternion.z()); + const Ogre::Vector3 slice_translation(slice_pose_.translation().x(), + slice_pose_.translation().y(), + slice_pose_.translation().z()); + scene_node_->setPosition(orientation_ * slice_translation + position_); + scene_node_->setOrientation(orientation_ * slice_rotation); } float DrawableSubmap::UpdateAlpha(const float target_alpha) { diff --git a/cartographer_ros/src/drawable_submap.h b/cartographer_ros/src/drawable_submap.h index 2ce8da0..5fb3be0 100644 --- a/cartographer_ros/src/drawable_submap.h +++ b/cartographer_ros/src/drawable_submap.h @@ -19,14 +19,17 @@ #include #include +#include #include #include #include +#include #include #include #include #include #include +#include #include #include @@ -51,7 +54,9 @@ class DrawableSubmap : public QObject { // 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); + void Update(const ::std_msgs::Header& header, + const ::cartographer_ros_msgs::SubmapEntry& metadata, + ::rviz::FrameManager* frame_manager); // If an update is needed, it will send an RPC using 'client' to request the // new data for the submap and returns true. @@ -60,10 +65,6 @@ class DrawableSubmap : public QObject { // Returns whether an RPC is in progress. bool QueryInProgress(); - // Transforms the scene node for this submap before being rendered onto a - // texture. - void Transform(::rviz::FrameManager* frame_manager); - // Sets the alpha of the submap taking into account its slice height and the // 'current_tracking_z'. void SetAlpha(double current_tracking_z); @@ -77,7 +78,7 @@ class DrawableSubmap : public QObject { void UpdateSceneNode(); private: - void QuerySubmap(ros::ServiceClient* client); + void UpdateTransform(); float UpdateAlpha(float target_alpha); const int submap_id_; @@ -89,8 +90,10 @@ class DrawableSubmap : public QObject { Ogre::ManualObject* manual_object_; Ogre::TexturePtr texture_; Ogre::MaterialPtr material_; - Eigen::Affine3d submap_pose_ GUARDED_BY(mutex_); - geometry_msgs::Pose transformed_pose_ GUARDED_BY(mutex_); + double submap_z_ = 0. GUARDED_BY(mutex_); + Ogre::Vector3 position_ GUARDED_BY(mutex_); + Ogre::Quaternion orientation_ GUARDED_BY(mutex_); + Eigen::Affine3d slice_pose_ GUARDED_BY(mutex_); std::chrono::milliseconds last_query_timestamp_ GUARDED_BY(mutex_); bool query_in_progress_ = false GUARDED_BY(mutex_); int metadata_version_ = -1 GUARDED_BY(mutex_); diff --git a/cartographer_ros/src/submaps_display.cc b/cartographer_ros/src/submaps_display.cc index 3ca858a..d72ed69 100644 --- a/cartographer_ros/src/submaps_display.cc +++ b/cartographer_ros/src/submaps_display.cc @@ -17,7 +17,6 @@ #include "submaps_display.h" #include -#include #include #include #include @@ -28,7 +27,6 @@ #include #include #include -#include #include #include "node_constants.h" @@ -47,9 +45,7 @@ constexpr char kDefaultTrackingFrame[] = "base_link"; } // namespace -SubmapsDisplay::SubmapsDisplay() - : scene_manager_listener_([this]() { UpdateTransforms(); }), - tf_listener_(tf_buffer_) { +SubmapsDisplay::SubmapsDisplay() : tf_listener_(tf_buffer_) { submap_query_service_property_ = new ::rviz::StringProperty( "Submap query service", QString("/cartographer/") + kSubmapQueryServiceName, @@ -84,7 +80,6 @@ void SubmapsDisplay::CreateClient() { void SubmapsDisplay::onInitialize() { MFDClass::onInitialize(); - scene_manager_->addListener(&scene_manager_listener_); CreateClient(); } @@ -113,7 +108,8 @@ void SubmapsDisplay::processMessage( ::cartographer::common::make_unique( submap_id, trajectory_id, context_->getSceneManager())); } - trajectory[submap_id]->Update(submap_entries[submap_id]); + trajectory[submap_id]->Update(msg->header, submap_entries[submap_id], + context_->getFrameManager()); } } } @@ -154,15 +150,6 @@ void SubmapsDisplay::update(const float wall_dt, const float ros_dt) { } } -void SubmapsDisplay::UpdateTransforms() { - ::cartographer::common::MutexLocker locker(&mutex_); - for (auto& trajectory : trajectories_) { - for (auto& submap : trajectory) { - submap->Transform(context_->getFrameManager()); - } - } -} - } // namespace rviz } // namespace cartographer_ros diff --git a/cartographer_ros/src/submaps_display.h b/cartographer_ros/src/submaps_display.h index c7fb1bc..8a8ad97 100644 --- a/cartographer_ros/src/submaps_display.h +++ b/cartographer_ros/src/submaps_display.h @@ -17,18 +17,11 @@ #ifndef CARTOGRAPHER_ROS_GOOGLE_CARTOGRAPHER_SRC_SUBMAPS_DISPLAY_H_ #define CARTOGRAPHER_ROS_GOOGLE_CARTOGRAPHER_SRC_SUBMAPS_DISPLAY_H_ -#include -#include -#include -#include -#include #include #include #include -#include -#include #include -#include +#include #include #include @@ -60,18 +53,6 @@ class SubmapsDisplay void Reset(); private: - class SceneManagerListener : public Ogre::SceneManager::Listener { - public: - SceneManagerListener(std::function callback) - : callback_(callback) {} - void preUpdateSceneGraph(Ogre::SceneManager* source, Ogre::Camera* camera) { - callback_(); - } - - private: - std::function callback_; - }; - void CreateClient(); void onInitialize() override; @@ -80,9 +61,6 @@ class SubmapsDisplay const ::cartographer_ros_msgs::SubmapList::ConstPtr& msg) override; void update(float wall_dt, float ros_dt) override; - void UpdateTransforms(); - - SceneManagerListener scene_manager_listener_; ::tf2_ros::Buffer tf_buffer_; ::tf2_ros::TransformListener tf_listener_; ros::ServiceClient client_;