diff --git a/cartographer_ros/cartographer_ros/occupancy_grid_node_main.cc b/cartographer_ros/cartographer_ros/occupancy_grid_node_main.cc index c8eef6a..447282a 100644 --- a/cartographer_ros/cartographer_ros/occupancy_grid_node_main.cc +++ b/cartographer_ros/cartographer_ros/occupancy_grid_node_main.cc @@ -137,33 +137,11 @@ void Node::HandleSubmapList( submap_slice.height = fetched_texture->height; submap_slice.slice_pose = fetched_texture->slice_pose; submap_slice.resolution = fetched_texture->resolution; - - // 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 * submap_slice.width; - CHECK_EQ(expected_stride, - cairo_format_stride_for_width(::cartographer::io::kCairoFormat, - submap_slice.width)); submap_slice.cairo_data.clear(); - for (size_t i = 0; i < fetched_texture->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 = fetched_texture->intensity.at(i); - const uint8_t alpha = fetched_texture->alpha.at(i); - const uint8_t observed = (intensity == 0 && alpha == 0) ? 0 : 255; - submap_slice.cairo_data.push_back((alpha << 24) | (intensity << 16) | - (observed << 8) | 0); - } - - submap_slice.surface = ::cartographer::io::MakeUniqueCairoSurfacePtr( - cairo_image_surface_create_for_data( - reinterpret_cast(submap_slice.cairo_data.data()), - ::cartographer::io::kCairoFormat, submap_slice.width, - submap_slice.height, expected_stride)); - CHECK_EQ(cairo_surface_status(submap_slice.surface.get()), - CAIRO_STATUS_SUCCESS) - << cairo_status_to_string( - cairo_surface_status(submap_slice.surface.get())); + submap_slice.surface = + 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/submap.cc b/cartographer_ros/cartographer_ros/submap.cc index eff5877..6bfee75 100644 --- a/cartographer_ros/cartographer_ros/submap.cc +++ b/cartographer_ros/cartographer_ros/submap.cc @@ -24,6 +24,55 @@ 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( const ::cartographer::mapping::SubmapId& submap_id, ros::ServiceClient* client) { @@ -37,24 +86,12 @@ std::unique_ptr FetchSubmapTextures( auto response = ::cartographer::common::make_unique(); response->version = srv.response.submap_version; for (const auto& texture : srv.response.textures) { - std::string compressed_cells(texture.cells.begin(), texture.cells.end()); - std::string cells; - ::cartographer::common::FastGunzipString(compressed_cells, &cells); - const int num_pixels = texture.width * texture.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 < texture.height; ++i) { - for (int j = 0; j < texture.width; ++j) { - intensity.push_back(cells[(i * texture.width + j) * 2]); - alpha.push_back(cells[(i * texture.width + j) * 2 + 1]); - } - } - response->textures.emplace_back( - SubmapTexture{intensity, alpha, texture.width, texture.height, - texture.resolution, ToRigid3d(texture.slice_pose)}); + const std::string compressed_cells(texture.cells.begin(), + texture.cells.end()); + response->textures.emplace_back(SubmapTexture{ + UnpackTextureData(compressed_cells, texture.width, texture.height), + texture.width, texture.height, texture.resolution, + ToRigid3d(texture.slice_pose)}); } return response; } diff --git a/cartographer_ros/cartographer_ros/submap.h b/cartographer_ros/cartographer_ros/submap.h index 13fe291..d73d89e 100644 --- a/cartographer_ros/cartographer_ros/submap.h +++ b/cartographer_ros/cartographer_ros/submap.h @@ -18,8 +18,10 @@ #define CARTOGRAPHER_ROS_SUBMAP_H_ #include +#include #include +#include "cartographer/io/image.h" #include "cartographer/mapping/id.h" #include "cartographer/transform/rigid_transform.h" #include "ros/ros.h" @@ -27,8 +29,11 @@ namespace cartographer_ros { struct SubmapTexture { - std::vector intensity; - std::vector alpha; + struct Pixels { + std::vector intensity; + std::vector alpha; + }; + Pixels pixels; int width; int height; double resolution; @@ -46,6 +51,16 @@ std::unique_ptr 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/ogre_slice.cc b/cartographer_rviz/cartographer_rviz/ogre_slice.cc index bf1aa33..35a9d2e 100644 --- a/cartographer_rviz/cartographer_rviz/ogre_slice.cc +++ b/cartographer_rviz/cartographer_rviz/ogre_slice.cc @@ -88,10 +88,11 @@ void OgreSlice::Update( // 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; - CHECK_EQ(submap_texture.intensity.size(), submap_texture.alpha.size()); - 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]); + CHECK_EQ(submap_texture.pixels.intensity.size(), + submap_texture.pixels.alpha.size()); + for (size_t i = 0; i < submap_texture.pixels.intensity.size(); ++i) { + rgb.push_back(submap_texture.pixels.intensity[i]); + rgb.push_back(submap_texture.pixels.alpha[i]); rgb.push_back(0); }