diff --git a/cartographer_ros/cartographer_ros/occupancy_grid_node_main.cc b/cartographer_ros/cartographer_ros/occupancy_grid_node_main.cc index 447282a..8efb281 100644 --- a/cartographer_ros/cartographer_ros/occupancy_grid_node_main.cc +++ b/cartographer_ros/cartographer_ros/occupancy_grid_node_main.cc @@ -138,10 +138,10 @@ void Node::HandleSubmapList( submap_slice.slice_pose = fetched_texture->slice_pose; submap_slice.resolution = fetched_texture->resolution; submap_slice.cairo_data.clear(); - submap_slice.surface = - DrawTexture(fetched_texture->pixels.intensity, - fetched_texture->pixels.alpha, fetched_texture->width, - fetched_texture->height, &submap_slice.cairo_data); + submap_slice.surface = ::cartographer::io::DrawTexture( + fetched_texture->pixels.intensity, fetched_texture->pixels.alpha, + fetched_texture->width, fetched_texture->height, + &submap_slice.cairo_data); } // Delete all submaps that didn't appear in the message. diff --git a/cartographer_ros/cartographer_ros/pbstream_to_ros_map_main.cc b/cartographer_ros/cartographer_ros/pbstream_to_ros_map_main.cc index e29873c..be0ca42 100644 --- a/cartographer_ros/cartographer_ros/pbstream_to_ros_map_main.cc +++ b/cartographer_ros/cartographer_ros/pbstream_to_ros_map_main.cc @@ -39,36 +39,6 @@ DEFINE_double(resolution, 0.05, "Resolution of a grid cell in the drawn map."); namespace cartographer_ros { namespace { -void FillSubmapSlice( - const ::cartographer::transform::Rigid3d& global_submap_pose, - const ::cartographer::mapping::proto::Submap& proto, - ::cartographer::io::SubmapSlice* const submap_slice) { - ::cartographer::mapping::proto::SubmapQuery::Response response; - ::cartographer::transform::Rigid3d local_pose; - if (proto.has_submap_3d()) { - ::cartographer::mapping_3d::Submap submap(proto.submap_3d()); - local_pose = submap.local_pose(); - submap.ToResponseProto(global_submap_pose, &response); - } else { - ::cartographer::mapping_2d::Submap submap(proto.submap_2d()); - local_pose = submap.local_pose(); - submap.ToResponseProto(global_submap_pose, &response); - } - submap_slice->pose = global_submap_pose; - - auto& texture_proto = response.textures(0); - const SubmapTexture::Pixels pixels = UnpackTextureData( - texture_proto.cells(), texture_proto.width(), texture_proto.height()); - submap_slice->width = texture_proto.width(); - submap_slice->height = texture_proto.height(); - submap_slice->resolution = texture_proto.resolution(); - submap_slice->slice_pose = - ::cartographer::transform::ToRigid3(texture_proto.slice_pose()); - submap_slice->surface = - DrawTexture(pixels.intensity, pixels.alpha, texture_proto.width(), - texture_proto.height(), &submap_slice->cairo_data); -} - void Run(const std::string& pbstream_filename, const std::string& map_filestem, const double resolution) { ::cartographer::io::ProtoStreamReader reader(pbstream_filename); diff --git a/cartographer_ros/cartographer_ros/submap.cc b/cartographer_ros/cartographer_ros/submap.cc index 6bfee75..f62e9a3 100644 --- a/cartographer_ros/cartographer_ros/submap.cc +++ b/cartographer_ros/cartographer_ros/submap.cc @@ -24,56 +24,7 @@ namespace cartographer_ros { -SubmapTexture::Pixels UnpackTextureData(const std::string& compressed_cells, - const int width, const int height) { - SubmapTexture::Pixels pixels; - std::string cells; - ::cartographer::common::FastGunzipString(compressed_cells, &cells); - const int num_pixels = width * height; - CHECK_EQ(cells.size(), 2 * num_pixels); - pixels.intensity.reserve(num_pixels); - pixels.alpha.reserve(num_pixels); - for (int i = 0; i < height; ++i) { - for (int j = 0; j < width; ++j) { - pixels.intensity.push_back(cells[(i * width + j) * 2]); - pixels.alpha.push_back(cells[(i * width + j) * 2 + 1]); - } - } - return pixels; -} - -::cartographer::io::UniqueCairoSurfacePtr DrawTexture( - const std::vector& intensity, const std::vector& alpha, - const int width, const int height, - std::vector* const cairo_data) { - CHECK(cairo_data->empty()); - - // Properly dealing with a non-common stride would make this code much more - // complicated. Let's check that it is not needed. - const int expected_stride = 4 * width; - CHECK_EQ(expected_stride, cairo_format_stride_for_width( - ::cartographer::io::kCairoFormat, width)); - for (size_t i = 0; i < intensity.size(); ++i) { - // We use the red channel to track intensity information. The green - // channel we use to track if a cell was ever observed. - const uint8_t intensity_value = intensity.at(i); - const uint8_t alpha_value = alpha.at(i); - const uint8_t observed = - (intensity_value == 0 && alpha_value == 0) ? 0 : 255; - cairo_data->push_back((alpha_value << 24) | (intensity_value << 16) | - (observed << 8) | 0); - } - - auto surface = ::cartographer::io::MakeUniqueCairoSurfacePtr( - cairo_image_surface_create_for_data( - reinterpret_cast(cairo_data->data()), - ::cartographer::io::kCairoFormat, width, height, expected_stride)); - CHECK_EQ(cairo_surface_status(surface.get()), CAIRO_STATUS_SUCCESS) - << cairo_status_to_string(cairo_surface_status(surface.get())); - return surface; -} - -std::unique_ptr FetchSubmapTextures( +std::unique_ptr<::cartographer::io::SubmapTextures> FetchSubmapTextures( const ::cartographer::mapping::SubmapId& submap_id, ros::ServiceClient* client) { ::cartographer_ros_msgs::SubmapQuery srv; @@ -83,13 +34,15 @@ std::unique_ptr FetchSubmapTextures( return nullptr; } CHECK(!srv.response.textures.empty()); - auto response = ::cartographer::common::make_unique(); + auto response = + ::cartographer::common::make_unique<::cartographer::io::SubmapTextures>(); response->version = srv.response.submap_version; for (const auto& texture : srv.response.textures) { const std::string compressed_cells(texture.cells.begin(), texture.cells.end()); - response->textures.emplace_back(SubmapTexture{ - UnpackTextureData(compressed_cells, texture.width, texture.height), + response->textures.emplace_back(::cartographer::io::SubmapTexture{ + ::cartographer::io::UnpackTextureData(compressed_cells, texture.width, + texture.height), texture.width, texture.height, texture.resolution, ToRigid3d(texture.slice_pose)}); } diff --git a/cartographer_ros/cartographer_ros/submap.h b/cartographer_ros/cartographer_ros/submap.h index d73d89e..8091f8b 100644 --- a/cartographer_ros/cartographer_ros/submap.h +++ b/cartographer_ros/cartographer_ros/submap.h @@ -22,45 +22,19 @@ #include #include "cartographer/io/image.h" +#include "cartographer/io/submap_painter.h" #include "cartographer/mapping/id.h" #include "cartographer/transform/rigid_transform.h" #include "ros/ros.h" namespace cartographer_ros { -struct SubmapTexture { - struct Pixels { - std::vector intensity; - std::vector alpha; - }; - Pixels pixels; - int width; - int height; - double resolution; - ::cartographer::transform::Rigid3d slice_pose; -}; - -struct SubmapTextures { - int version; - std::vector textures; -}; - // Fetch 'submap_id' using the 'client' and returning the response or 'nullptr' // on error. -std::unique_ptr FetchSubmapTextures( +std::unique_ptr<::cartographer::io::SubmapTextures> FetchSubmapTextures( const ::cartographer::mapping::SubmapId& submap_id, ros::ServiceClient* client); -// Unpacks cell data as provided by the backend into 'intensity' and 'alpha'. -SubmapTexture::Pixels UnpackTextureData(const std::string& compressed_cells, - int width, int height); - -// Draw a texture into a cairo surface. 'cairo_data' will store the pixel data -// for the surface and must therefore outlive the use of the surface. -::cartographer::io::UniqueCairoSurfacePtr DrawTexture( - const std::vector& intensity, const std::vector& alpha, - int width, int height, std::vector* cairo_data); - } // 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 b5ea213..1cb5cc8 100644 --- a/cartographer_rviz/cartographer_rviz/drawable_submap.cc +++ b/cartographer_rviz/cartographer_rviz/drawable_submap.cc @@ -134,7 +134,7 @@ 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]() { - std::unique_ptr<::cartographer_ros::SubmapTextures> submap_textures = + std::unique_ptr<::cartographer::io::SubmapTextures> submap_textures = ::cartographer_ros::FetchSubmapTextures(id_, client); ::cartographer::common::MutexLocker locker(&mutex_); query_in_progress_ = false; diff --git a/cartographer_rviz/cartographer_rviz/drawable_submap.h b/cartographer_rviz/cartographer_rviz/drawable_submap.h index 5320b74..60a6421 100644 --- a/cartographer_rviz/cartographer_rviz/drawable_submap.h +++ b/cartographer_rviz/cartographer_rviz/drawable_submap.h @@ -25,6 +25,7 @@ #include "OgreSceneManager.h" #include "OgreSceneNode.h" #include "cartographer/common/mutex.h" +#include "cartographer/io/submap_painter.h" #include "cartographer/mapping/id.h" #include "cartographer/transform/rigid_transform.h" #include "cartographer_ros/submap.h" @@ -105,7 +106,7 @@ class DrawableSubmap : public QObject { bool query_in_progress_ = false GUARDED_BY(mutex_); int metadata_version_ = -1 GUARDED_BY(mutex_); std::future rpc_request_future_; - std::unique_ptr<::cartographer_ros::SubmapTextures> submap_textures_ + std::unique_ptr<::cartographer::io::SubmapTextures> submap_textures_ GUARDED_BY(mutex_); float current_alpha_ = 0.f; std::unique_ptr<::rviz::BoolProperty> visibility_; diff --git a/cartographer_rviz/cartographer_rviz/ogre_slice.cc b/cartographer_rviz/cartographer_rviz/ogre_slice.cc index 35a9d2e..6f1c9f1 100644 --- a/cartographer_rviz/cartographer_rviz/ogre_slice.cc +++ b/cartographer_rviz/cartographer_rviz/ogre_slice.cc @@ -82,7 +82,7 @@ OgreSlice::~OgreSlice() { } void OgreSlice::Update( - const ::cartographer_ros::SubmapTexture& submap_texture) { + const ::cartographer::io::SubmapTexture& submap_texture) { slice_node_->setPosition(ToOgre(submap_texture.slice_pose.translation())); slice_node_->setOrientation(ToOgre(submap_texture.slice_pose.rotation())); // The call to Ogre's loadRawData below does not work with an RG texture, diff --git a/cartographer_rviz/cartographer_rviz/ogre_slice.h b/cartographer_rviz/cartographer_rviz/ogre_slice.h index 97605b4..95e8238 100644 --- a/cartographer_rviz/cartographer_rviz/ogre_slice.h +++ b/cartographer_rviz/cartographer_rviz/ogre_slice.h @@ -26,8 +26,8 @@ #include "OgreSceneNode.h" #include "OgreTexture.h" #include "OgreVector3.h" +#include "cartographer/io/submap_painter.h" #include "cartographer/mapping/id.h" -#include "cartographer_ros/submap.h" namespace cartographer_rviz { @@ -50,7 +50,7 @@ class OgreSlice { // Updates the texture and pose of the submap using new data from // 'submap_texture'. - void Update(const ::cartographer_ros::SubmapTexture& submap_texture); + void Update(const ::cartographer::io::SubmapTexture& submap_texture); // Changes the opacity of the submap to 'alpha'. void SetAlpha(float alpha);