diff --git a/cartographer_ros/cartographer_ros/occupancy_grid_node_main.cc b/cartographer_ros/cartographer_ros/occupancy_grid_node_main.cc index 8e0d9a1..0a0a4a5 100644 --- a/cartographer_ros/cartographer_ros/occupancy_grid_node_main.cc +++ b/cartographer_ros/cartographer_ros/occupancy_grid_node_main.cc @@ -24,6 +24,7 @@ #include "cartographer/common/mutex.h" #include "cartographer/common/port.h" #include "cartographer/io/image.h" +#include "cartographer/io/submap_painter.h" #include "cartographer/mapping/id.h" #include "cartographer/transform/rigid_transform.h" #include "cartographer_ros/msg_conversion.h" @@ -43,59 +44,10 @@ DEFINE_double(publish_period_sec, 1.0, "OccupancyGrid publishing period."); namespace cartographer_ros { namespace { +using ::cartographer::io::PaintSubmapSlicesResult; +using ::cartographer::io::SubmapSlice; using ::cartographer::mapping::SubmapId; -constexpr cairo_format_t kCairoFormat = CAIRO_FORMAT_ARGB32; - -Eigen::Affine3d ToEigen(const ::cartographer::transform::Rigid3d& rigid3) { - return Eigen::Translation3d(rigid3.translation()) * rigid3.rotation(); -} - -struct SubmapState { - SubmapState() - : surface(::cartographer::io::MakeUniqueCairoSurfacePtr(nullptr)) {} - - // Texture data. - int width; - int height; - int version; - double resolution; - ::cartographer::transform::Rigid3d slice_pose; - ::cartographer::io::UniqueCairoSurfacePtr surface; - // Pixel data used by 'surface'. Must outlive 'surface'. - std::vector cairo_data; - - // Metadata. - ::cartographer::transform::Rigid3d pose; - int metadata_version = -1; -}; - -void CairoDrawEachSubmap( - const double scale, std::map* submaps, cairo_t* cr, - std::function draw_callback) { - cairo_scale(cr, scale, scale); - - for (auto& pair : *submaps) { - auto& submap_state = pair.second; - if (submap_state.surface == nullptr) { - return; - } - const Eigen::Matrix4d homo = - ToEigen(submap_state.pose * submap_state.slice_pose).matrix(); - - cairo_save(cr); - cairo_matrix_t matrix; - cairo_matrix_init(&matrix, homo(1, 0), homo(0, 0), -homo(1, 1), -homo(0, 1), - homo(0, 3), -homo(1, 3)); - cairo_transform(cr, &matrix); - - const double submap_resolution = submap_state.resolution; - cairo_scale(cr, submap_resolution, submap_resolution); - draw_callback(submap_state); - cairo_restore(cr); - } -} - class Node { public: explicit Node(double resolution, double publish_period_sec); @@ -109,7 +61,6 @@ class Node { void DrawAndPublish(const ::ros::WallTimerEvent& timer_event); void PublishOccupancyGrid(const std::string& frame_id, const ros::Time& time, const Eigen::Array2f& origin, - const Eigen::Array2i& size, cairo_surface_t* surface); ::ros::NodeHandle node_handle_; @@ -119,7 +70,7 @@ class Node { ::ros::ServiceClient client_ GUARDED_BY(mutex_); ::ros::Subscriber submap_list_subscriber_ GUARDED_BY(mutex_); ::ros::Publisher occupancy_grid_publisher_ GUARDED_BY(mutex_); - std::map submaps_ GUARDED_BY(mutex_); + std::map submap_slices_ GUARDED_BY(mutex_); ::ros::WallTimer occupancy_grid_publisher_timer_; std::string last_frame_id_; ros::Time last_timestamp_; @@ -154,11 +105,11 @@ void Node::HandleSubmapList( } for (const auto& submap_msg : msg->submap) { const SubmapId id{submap_msg.trajectory_id, submap_msg.submap_index}; - SubmapState& submap_state = submaps_[id]; - submap_state.pose = ToRigid3d(submap_msg.pose); - submap_state.metadata_version = submap_msg.submap_version; - if (submap_state.surface != nullptr && - submap_state.version == submap_msg.submap_version) { + SubmapSlice& submap_slice = submap_slices_[id]; + submap_slice.pose = ToRigid3d(submap_msg.pose); + submap_slice.metadata_version = submap_msg.submap_version; + if (submap_slice.surface != nullptr && + submap_slice.version == submap_msg.submap_version) { continue; } @@ -168,126 +119,86 @@ void Node::HandleSubmapList( continue; } CHECK(!fetched_textures->textures.empty()); - submap_state.version = fetched_textures->version; + submap_slice.version = fetched_textures->version; - // TODO(gaschler): Handle more textures than just the first one. + // We use the first texture only. By convention this is the highest + // resolution texture and that is the one we want to use to construct the + // map for ROS. const auto fetched_texture = fetched_textures->textures.begin(); - submap_state.width = fetched_texture->width; - submap_state.height = fetched_texture->height; - submap_state.slice_pose = fetched_texture->slice_pose; - submap_state.resolution = fetched_texture->resolution; + submap_slice.width = fetched_texture->width; + 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_state.width; + const int expected_stride = 4 * submap_slice.width; + // TODO(jihoonl): Refactor here out to remove kCairoFormat dependency CHECK_EQ(expected_stride, - cairo_format_stride_for_width(kCairoFormat, submap_state.width)); - submap_state.cairo_data.clear(); + cairo_format_stride_for_width( + PaintSubmapSlicesResult::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_state.cairo_data.push_back((alpha << 24) | (intensity << 16) | + submap_slice.cairo_data.push_back((alpha << 24) | (intensity << 16) | (observed << 8) | 0); } - submap_state.surface = ::cartographer::io::MakeUniqueCairoSurfacePtr( + submap_slice.surface = ::cartographer::io::MakeUniqueCairoSurfacePtr( cairo_image_surface_create_for_data( - reinterpret_cast(submap_state.cairo_data.data()), - kCairoFormat, submap_state.width, submap_state.height, - expected_stride)); - CHECK_EQ(cairo_surface_status(submap_state.surface.get()), + reinterpret_cast(submap_slice.cairo_data.data()), + PaintSubmapSlicesResult::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_state.surface.get())); + cairo_surface_status(submap_slice.surface.get())); } last_timestamp_ = msg->header.stamp; last_frame_id_ = msg->header.frame_id; } void Node::DrawAndPublish(const ::ros::WallTimerEvent& unused_timer_event) { - if (submaps_.empty() || last_frame_id_.empty()) { + if (submap_slices_.empty() || last_frame_id_.empty()) { return; } ::cartographer::common::MutexLocker locker(&mutex_); - Eigen::AlignedBox2f bounding_box; - { - auto surface = ::cartographer::io::MakeUniqueCairoSurfacePtr( - cairo_image_surface_create(kCairoFormat, 1, 1)); - auto cr = - ::cartographer::io::MakeUniqueCairoPtr(cairo_create(surface.get())); - const auto update_bounding_box = [&bounding_box, &cr](double x, double y) { - cairo_user_to_device(cr.get(), &x, &y); - bounding_box.extend(Eigen::Vector2f(x, y)); - }; - - CairoDrawEachSubmap( - 1. / resolution_, &submaps_, cr.get(), - [&update_bounding_box, &bounding_box](const SubmapState& submap_state) { - update_bounding_box(0, 0); - update_bounding_box(submap_state.width, 0); - update_bounding_box(0, submap_state.height); - update_bounding_box(submap_state.width, submap_state.height); - }); - } - - const int kPaddingPixel = 5; - const Eigen::Array2i size( - std::ceil(bounding_box.sizes().x()) + 2 * kPaddingPixel, - std::ceil(bounding_box.sizes().y()) + 2 * kPaddingPixel); - const Eigen::Array2f origin(-bounding_box.min().x() + kPaddingPixel, - -bounding_box.min().y() + kPaddingPixel); - - { - auto surface = ::cartographer::io::MakeUniqueCairoSurfacePtr( - cairo_image_surface_create(kCairoFormat, size.x(), size.y())); - auto cr = - ::cartographer::io::MakeUniqueCairoPtr(cairo_create(surface.get())); - cairo_set_source_rgba(cr.get(), 0.5, 0.0, 0.0, 1.); - cairo_paint(cr.get()); - cairo_translate(cr.get(), origin.x(), origin.y()); - CairoDrawEachSubmap(1. / resolution_, &submaps_, cr.get(), - [&cr](const SubmapState& submap_state) { - cairo_set_source_surface( - cr.get(), submap_state.surface.get(), 0., 0.); - cairo_paint(cr.get()); - }); - cairo_surface_flush(surface.get()); - PublishOccupancyGrid(last_frame_id_, last_timestamp_, origin, size, - surface.get()); - } + auto painted_slices = PaintSubmapSlices(&submap_slices_, resolution_); + PublishOccupancyGrid(last_frame_id_, last_timestamp_, painted_slices.origin, + painted_slices.surface.get()); } void Node::PublishOccupancyGrid(const std::string& frame_id, const ros::Time& time, const Eigen::Array2f& origin, - const Eigen::Array2i& size, cairo_surface_t* surface) { nav_msgs::OccupancyGrid occupancy_grid; + const int width = cairo_image_surface_get_width(surface); + const int height = cairo_image_surface_get_height(surface); occupancy_grid.header.stamp = time; occupancy_grid.header.frame_id = frame_id; occupancy_grid.info.map_load_time = time; occupancy_grid.info.resolution = resolution_; - occupancy_grid.info.width = size.x(); - occupancy_grid.info.height = size.y(); + occupancy_grid.info.width = width; + occupancy_grid.info.height = height; occupancy_grid.info.origin.position.x = -origin.x() * resolution_; - occupancy_grid.info.origin.position.y = - (-size.y() + origin.y()) * resolution_; + occupancy_grid.info.origin.position.y = (-height + origin.y()) * resolution_; occupancy_grid.info.origin.position.z = 0.; occupancy_grid.info.origin.orientation.w = 1.; occupancy_grid.info.origin.orientation.x = 0.; occupancy_grid.info.origin.orientation.y = 0.; occupancy_grid.info.origin.orientation.z = 0.; - const uint32_t* pixel_data = - reinterpret_cast(cairo_image_surface_get_data(surface)); - occupancy_grid.data.reserve(size.x() * size.y()); - for (int y = size.y() - 1; y >= 0; --y) { - for (int x = 0; x < size.x(); ++x) { - const uint32_t packed = pixel_data[y * size.x() + x]; + const uint32_t* pixel_data = reinterpret_cast(cairo_image_surface_get_data(surface)); + occupancy_grid.data.reserve(width * height); + for (int y = height - 1; y >= 0; --y) { + for (int x = 0; x < width; ++x) { + const uint32_t packed = pixel_data[y * width + x]; const unsigned char color = packed >> 16; const unsigned char observed = packed >> 8; const int value =