From 7a3671240561f623493019b913d6c8938528ca1b Mon Sep 17 00:00:00 2001 From: Michael Grupp Date: Tue, 27 Feb 2018 22:15:39 +0100 Subject: [PATCH] Use CreateOccupancyGridMsg() in occupancy_grid_node_main.cc (#715) Follow-up of PR #711. --- .../occupancy_grid_node_main.cc | 46 ++----------------- 1 file changed, 3 insertions(+), 43 deletions(-) diff --git a/cartographer_ros/cartographer_ros/occupancy_grid_node_main.cc b/cartographer_ros/cartographer_ros/occupancy_grid_node_main.cc index 8efb281..8385fad 100644 --- a/cartographer_ros/cartographer_ros/occupancy_grid_node_main.cc +++ b/cartographer_ros/cartographer_ros/occupancy_grid_node_main.cc @@ -160,49 +160,9 @@ void Node::DrawAndPublish(const ::ros::WallTimerEvent& unused_timer_event) { ::cartographer::common::MutexLocker locker(&mutex_); 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, - 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 = width; - occupancy_grid.info.height = height; - occupancy_grid.info.origin.position.x = -origin.x() * 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(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 = - observed == 0 - ? -1 - : ::cartographer::common::RoundToInt((1. - color / 255.) * 100.); - CHECK_LE(-1, value); - CHECK_GE(100, value); - occupancy_grid.data.push_back(value); - } - } - occupancy_grid_publisher_.publish(occupancy_grid); + std::unique_ptr msg_ptr = CreateOccupancyGridMsg( + painted_slices, resolution_, last_frame_id_, last_timestamp_); + occupancy_grid_publisher_.publish(*msg_ptr); } } // namespace