Use CreateOccupancyGridMsg() in occupancy_grid_node_main.cc (ros#715)

Follow-up of PR ros#711.
cwecht · Feb 27, 2018 · 7a36712 · 7a36712
1 parent b7d8af8
commit 7a36712
Showing 1 changed file with 3 additions and 43 deletions.
diff --git 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<uint32_t*>(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<nav_msgs::OccupancyGrid> msg_ptr = CreateOccupancyGridMsg(
+      painted_slices, resolution_, last_frame_id_, last_timestamp_);
+  occupancy_grid_publisher_.publish(*msg_ptr);
 }
 
 }  // namespace