12345qiupeng
/
cartographer_ros
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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

Follow-up of PR #711.
master
Michael Grupp 2018-02-27 22:15:39 +01:00 committed by Christoph Schütte
parent b7d8af834e
commit 7a36712405
1 changed files with 3 additions and 43 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

46
cartographer_ros/cartographer_ros/occupancy_grid_node_main.cc
View File

@ -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