diff --git a/cartographer_ros/cartographer_ros/CMakeLists.txt b/cartographer_ros/cartographer_ros/CMakeLists.txt index 7c17bf9..acd15b8 100644 --- a/cartographer_ros/cartographer_ros/CMakeLists.txt +++ b/cartographer_ros/cartographer_ros/CMakeLists.txt @@ -55,4 +55,3 @@ install(TARGETS cartographer_start_trajectory LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) - diff --git a/cartographer_ros/cartographer_ros/node.cc b/cartographer_ros/cartographer_ros/node.cc index 1fa7d70..5a25b54 100644 --- a/cartographer_ros/cartographer_ros/node.cc +++ b/cartographer_ros/cartographer_ros/node.cc @@ -50,9 +50,6 @@ using carto::transform::Rigid3d; namespace { -constexpr int kInfiniteSubscriberQueueSize = 0; -constexpr int kLatestOnlyPublisherQueueSize = 1; - void ShutdownSubscriber(std::unordered_map& subscribers, int trajectory_id) { if (subscribers.count(trajectory_id) == 0) { diff --git a/cartographer_ros/cartographer_ros/node_constants.h b/cartographer_ros/cartographer_ros/node_constants.h index 5a3bb45..d3b72fd 100644 --- a/cartographer_ros/cartographer_ros/node_constants.h +++ b/cartographer_ros/cartographer_ros/node_constants.h @@ -36,6 +36,9 @@ constexpr char kTrajectoryNodeListTopic[] = "trajectory_node_list"; constexpr char kConstraintListTopic[] = "constraint_list"; constexpr double kConstraintPublishPeriodSec = 0.5; +constexpr int kInfiniteSubscriberQueueSize = 0; +constexpr int kLatestOnlyPublisherQueueSize = 1; + } // namespace cartographer_ros #endif // CARTOGRAPHER_ROS_NODE_CONSTANTS_H_ diff --git a/cartographer_ros/cartographer_ros/offline_node_main.cc b/cartographer_ros/cartographer_ros/offline_node_main.cc index af41cc2..148a078 100644 --- a/cartographer_ros/cartographer_ros/offline_node_main.cc +++ b/cartographer_ros/cartographer_ros/offline_node_main.cc @@ -61,7 +61,6 @@ namespace { constexpr char kClockTopic[] = "clock"; constexpr char kTfStaticTopic[] = "/tf_static"; constexpr char kTfTopic[] = "tf"; -constexpr int kLatestOnlyPublisherQueueSize = 1; volatile std::sig_atomic_t sigint_triggered = 0; diff --git a/cartographer_ros/cartographer_ros/submap.cc b/cartographer_ros/cartographer_ros/submap.cc new file mode 100644 index 0000000..ff79a3d --- /dev/null +++ b/cartographer_ros/cartographer_ros/submap.cc @@ -0,0 +1,58 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer_ros/submap.h" + +#include "cartographer/common/make_unique.h" +#include "cartographer/common/port.h" +#include "cartographer/transform/transform.h" +#include "cartographer_ros/msg_conversion.h" +#include "cartographer_ros_msgs/SubmapQuery.h" + +namespace cartographer_ros { + +std::unique_ptr FetchSubmapTexture( + const ::cartographer::mapping::SubmapId& submap_id, + ros::ServiceClient* client) { + ::cartographer_ros_msgs::SubmapQuery srv; + srv.request.trajectory_id = submap_id.trajectory_id; + srv.request.submap_index = submap_id.submap_index; + if (!client->call(srv)) { + return nullptr; + } + std::string compressed_cells(srv.response.cells.begin(), + srv.response.cells.end()); + std::string cells; + ::cartographer::common::FastGunzipString(compressed_cells, &cells); + const int num_pixels = srv.response.width * srv.response.height; + CHECK_EQ(cells.size(), 2 * num_pixels); + std::vector intensity; + intensity.reserve(num_pixels); + std::vector alpha; + alpha.reserve(num_pixels); + for (int i = 0; i < srv.response.height; ++i) { + for (int j = 0; j < srv.response.width; ++j) { + intensity.push_back(cells[(i * srv.response.width + j) * 2]); + alpha.push_back(cells[(i * srv.response.width + j) * 2 + 1]); + } + } + return ::cartographer::common::make_unique(SubmapTexture{ + srv.response.submap_version, intensity, alpha, srv.response.width, + srv.response.height, srv.response.resolution, + ToRigid3d(srv.response.slice_pose)}); +} + +} // namespace cartographer_ros diff --git a/cartographer_ros/cartographer_ros/submap.h b/cartographer_ros/cartographer_ros/submap.h new file mode 100644 index 0000000..883b158 --- /dev/null +++ b/cartographer_ros/cartographer_ros/submap.h @@ -0,0 +1,47 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_ROS_SUBMAP_H_ +#define CARTOGRAPHER_ROS_SUBMAP_H_ + +#include +#include + +#include "cartographer/mapping/id.h" +#include "cartographer/transform/rigid_transform.h" +#include "ros/ros.h" + +namespace cartographer_ros { + +struct SubmapTexture { + int version; + std::vector intensity; + std::vector alpha; + int width; + int height; + double resolution; + ::cartographer::transform::Rigid3d slice_pose; +}; + +// Fetch 'submap_id' using the 'client' and returning the response or 'nullptr' +// on error. +std::unique_ptr FetchSubmapTexture( + const ::cartographer::mapping::SubmapId& submap_id, + ros::ServiceClient* client); + +} // namespace cartographer_ros + +#endif // CARTOGRAPHER_ROS_SUBMAP_H_ diff --git a/cartographer_rviz/cartographer_rviz/drawable_submap.cc b/cartographer_rviz/cartographer_rviz/drawable_submap.cc index 9a3af1f..8f2b56f 100644 --- a/cartographer_rviz/cartographer_rviz/drawable_submap.cc +++ b/cartographer_rviz/cartographer_rviz/drawable_submap.cc @@ -97,6 +97,8 @@ DrawableSubmap::DrawableSubmap(const ::cartographer::mapping::SubmapId& id, } DrawableSubmap::~DrawableSubmap() { + // 'query_in_progress_' must be true until the Q_EMIT has happened. Qt then + // makes sure that 'RequestSucceeded' is not called after our destruction. if (QueryInProgress()) { rpc_request_future_.wait(); } @@ -120,10 +122,9 @@ void DrawableSubmap::Update( return; } ::cartographer::common::MutexLocker locker(&mutex_); - submap_z_ = metadata.pose.position.z; metadata_version_ = metadata.submap_version; pose_ = ::cartographer_ros::ToRigid3d(metadata.pose); - if (texture_version_ != -1) { + if (submap_texture_ != nullptr) { // We have to update the transform since we are already displaying a texture // for this submap. UpdateTransform(); @@ -140,8 +141,10 @@ void DrawableSubmap::Update( 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 = texture_version_ != metadata_version_; + // Received metadata version can also be lower if we restarted Cartographer. + const bool newer_version_available = + submap_texture_ == nullptr || + submap_texture_->version != metadata_version_; const std::chrono::milliseconds now = std::chrono::duration_cast( std::chrono::system_clock::now().time_since_epoch()); @@ -153,18 +156,16 @@ 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]() { - ::cartographer_ros_msgs::SubmapQuery srv; - srv.request.trajectory_id = id_.trajectory_id; - srv.request.submap_index = id_.submap_index; - if (client->call(srv)) { + std::unique_ptr<::cartographer_ros::SubmapTexture> submap_texture = + ::cartographer_ros::FetchSubmapTexture(id_, client); + ::cartographer::common::MutexLocker locker(&mutex_); + query_in_progress_ = false; + if (submap_texture != nullptr) { // 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; + // 'submap_texture_' member to simplify the signal-slot connection + // slightly. + submap_texture_ = std::move(submap_texture); Q_EMIT RequestSucceeded(); - } else { - ::cartographer::common::MutexLocker locker(&mutex_); - query_in_progress_ = false; } }); return true; @@ -176,7 +177,8 @@ bool DrawableSubmap::QueryInProgress() { } void DrawableSubmap::SetAlpha(const double current_tracking_z) { - const double distance_z = std::abs(submap_z_ - current_tracking_z); + const double distance_z = + std::abs(pose_.translation().z() - current_tracking_z); const double fade_distance = std::max(distance_z - kFadeOutStartDistanceInMeters, 0.); const float alpha = static_cast( @@ -189,29 +191,21 @@ void DrawableSubmap::SetAlpha(const double current_tracking_z) { 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); - slice_pose_ = ::cartographer_ros::ToRigid3d(response_.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. std::vector rgb; - 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.); - } + for (size_t i = 0; i < submap_texture_->intensity.size(); ++i) { + rgb.push_back(submap_texture_->intensity[i]); + rgb.push_back(submap_texture_->alpha[i]); + rgb.push_back(0.); } manual_object_->clear(); - const float metric_width = response_.resolution * response_.width; - const float metric_height = response_.resolution * response_.height; + const float metric_width = + submap_texture_->resolution * submap_texture_->width; + const float metric_height = + submap_texture_->resolution * submap_texture_->height; manual_object_->begin(material_->getName(), Ogre::RenderOperation::OT_TRIANGLE_STRIP); // Bottom left @@ -239,8 +233,8 @@ void DrawableSubmap::UpdateSceneNode() { kSubmapTexturePrefix + GetSubmapIdentifier(id_); texture_ = Ogre::TextureManager::getSingleton().loadRawData( texture_name, Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, - pixel_stream, response_.width, response_.height, Ogre::PF_BYTE_RGB, - Ogre::TEX_TYPE_2D, 0); + pixel_stream, submap_texture_->width, submap_texture_->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); @@ -253,7 +247,9 @@ void DrawableSubmap::UpdateSceneNode() { } void DrawableSubmap::UpdateTransform() { - const ::cartographer::transform::Rigid3d pose = pose_ * slice_pose_; + CHECK(submap_texture_ != nullptr); + const ::cartographer::transform::Rigid3d pose = + pose_ * submap_texture_->slice_pose; scene_node_->setPosition(ToOgre(pose.translation())); scene_node_->setOrientation(ToOgre(pose.rotation())); } diff --git a/cartographer_rviz/cartographer_rviz/drawable_submap.h b/cartographer_rviz/cartographer_rviz/drawable_submap.h index 1bb665a..5c42a04 100644 --- a/cartographer_rviz/cartographer_rviz/drawable_submap.h +++ b/cartographer_rviz/cartographer_rviz/drawable_submap.h @@ -18,6 +18,7 @@ #define CARTOGRAPHER_RVIZ_SRC_DRAWABLE_SUBMAP_H_ #include +#include #include "Eigen/Core" #include "Eigen/Geometry" @@ -31,6 +32,7 @@ #include "cartographer/common/mutex.h" #include "cartographer/mapping/id.h" #include "cartographer/transform/rigid_transform.h" +#include "cartographer_ros/submap.h" #include "cartographer_ros_msgs/SubmapEntry.h" #include "cartographer_ros_msgs/SubmapQuery.h" #include "ros/ros.h" @@ -99,15 +101,13 @@ class DrawableSubmap : public QObject { Ogre::ManualObject* manual_object_; Ogre::TexturePtr texture_; Ogre::MaterialPtr material_; - double submap_z_ = 0. GUARDED_BY(mutex_); ::cartographer::transform::Rigid3d pose_ GUARDED_BY(mutex_); - ::cartographer::transform::Rigid3d 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_); - int texture_version_ = -1 GUARDED_BY(mutex_); std::future rpc_request_future_; - ::cartographer_ros_msgs::SubmapQuery::Response response_ GUARDED_BY(mutex_); + std::unique_ptr<::cartographer_ros::SubmapTexture> submap_texture_ + GUARDED_BY(mutex_); float current_alpha_ = 0.f; std::unique_ptr<::rviz::BoolProperty> visibility_; };