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

Follow googlecartographer/cartographer#599 (#561) 

Use submap_painter. Merge this after googlecartographer/cartographer#599.
master
Jihoon Lee 2017-11-10 13:27:27 +01:00 committed by Wally B. Feed
parent ae6aefaf97
commit d0697e6847
1 changed files with 43 additions and 132 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

175
cartographer_ros/cartographer_ros/occupancy_grid_node_main.cc
View File

@ -24,6 +24,7 @@
#include "cartographer/common/mutex.h" #include "cartographer/common/mutex.h"
#include "cartographer/common/port.h" #include "cartographer/common/port.h"
#include "cartographer/io/image.h" #include "cartographer/io/image.h"
#include "cartographer/io/submap_painter.h"
#include "cartographer/mapping/id.h" #include "cartographer/mapping/id.h"
#include "cartographer/transform/rigid_transform.h" #include "cartographer/transform/rigid_transform.h"
#include "cartographer_ros/msg_conversion.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 cartographer_ros {
namespace { namespace {
using ::cartographer::io::PaintSubmapSlicesResult;
using ::cartographer::io::SubmapSlice;
using ::cartographer::mapping::SubmapId; 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<uint32_t> cairo_data;
// Metadata.
::cartographer::transform::Rigid3d pose;
int metadata_version = -1;
};
void CairoDrawEachSubmap(
const double scale, std::map<SubmapId, SubmapState>* submaps, cairo_t* cr,
std::function<void(const SubmapState&)> 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 { class Node {
public: public:
explicit Node(double resolution, double publish_period_sec); explicit Node(double resolution, double publish_period_sec);
@ -109,7 +61,6 @@ class Node {
void DrawAndPublish(const ::ros::WallTimerEvent& timer_event); void DrawAndPublish(const ::ros::WallTimerEvent& timer_event);
void PublishOccupancyGrid(const std::string& frame_id, const ros::Time& time, void PublishOccupancyGrid(const std::string& frame_id, const ros::Time& time,
const Eigen::Array2f& origin, const Eigen::Array2f& origin,
const Eigen::Array2i& size,
cairo_surface_t* surface); cairo_surface_t* surface);
::ros::NodeHandle node_handle_; ::ros::NodeHandle node_handle_;
@ -119,7 +70,7 @@ class Node {
::ros::ServiceClient client_ GUARDED_BY(mutex_); ::ros::ServiceClient client_ GUARDED_BY(mutex_);
::ros::Subscriber submap_list_subscriber_ GUARDED_BY(mutex_); ::ros::Subscriber submap_list_subscriber_ GUARDED_BY(mutex_);
::ros::Publisher occupancy_grid_publisher_ GUARDED_BY(mutex_); ::ros::Publisher occupancy_grid_publisher_ GUARDED_BY(mutex_);
std::map<SubmapId, SubmapState> submaps_ GUARDED_BY(mutex_); std::map<SubmapId, SubmapSlice> submap_slices_ GUARDED_BY(mutex_);
::ros::WallTimer occupancy_grid_publisher_timer_; ::ros::WallTimer occupancy_grid_publisher_timer_;
std::string last_frame_id_; std::string last_frame_id_;
ros::Time last_timestamp_; ros::Time last_timestamp_;
@ -154,11 +105,11 @@ void Node::HandleSubmapList(
} }
for (const auto& submap_msg : msg->submap) { for (const auto& submap_msg : msg->submap) {
const SubmapId id{submap_msg.trajectory_id, submap_msg.submap_index}; const SubmapId id{submap_msg.trajectory_id, submap_msg.submap_index};
SubmapState& submap_state = submaps_[id]; SubmapSlice& submap_slice = submap_slices_[id];
submap_state.pose = ToRigid3d(submap_msg.pose); submap_slice.pose = ToRigid3d(submap_msg.pose);
submap_state.metadata_version = submap_msg.submap_version; submap_slice.metadata_version = submap_msg.submap_version;
if (submap_state.surface != nullptr && if (submap_slice.surface != nullptr &&
submap_state.version == submap_msg.submap_version) { submap_slice.version == submap_msg.submap_version) {
continue; continue;
} }
@ -168,126 +119,86 @@ void Node::HandleSubmapList(
continue; continue;
} }
CHECK(!fetched_textures->textures.empty()); 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(); const auto fetched_texture = fetched_textures->textures.begin();
submap_state.width = fetched_texture->width; submap_slice.width = fetched_texture->width;
submap_state.height = fetched_texture->height; submap_slice.height = fetched_texture->height;
submap_state.slice_pose = fetched_texture->slice_pose; submap_slice.slice_pose = fetched_texture->slice_pose;
submap_state.resolution = fetched_texture->resolution; submap_slice.resolution = fetched_texture->resolution;
// Properly dealing with a non-common stride would make this code much more // Properly dealing with a non-common stride would make this code much more
// complicated. Let's check that it is not needed. // 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, CHECK_EQ(expected_stride,
cairo_format_stride_for_width(kCairoFormat, submap_state.width)); cairo_format_stride_for_width(
submap_state.cairo_data.clear(); PaintSubmapSlicesResult::kCairoFormat, submap_slice.width));
submap_slice.cairo_data.clear();
for (size_t i = 0; i < fetched_texture->intensity.size(); ++i) { for (size_t i = 0; i < fetched_texture->intensity.size(); ++i) {
// We use the red channel to track intensity information. The green // We use the red channel to track intensity information. The green
// channel we use to track if a cell was ever observed. // channel we use to track if a cell was ever observed.
const uint8_t intensity = fetched_texture->intensity.at(i); const uint8_t intensity = fetched_texture->intensity.at(i);
const uint8_t alpha = fetched_texture->alpha.at(i); const uint8_t alpha = fetched_texture->alpha.at(i);
const uint8_t observed = (intensity == 0 && alpha == 0) ? 0 : 255; 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); (observed << 8) | 0);
} }
submap_state.surface = ::cartographer::io::MakeUniqueCairoSurfacePtr( submap_slice.surface = ::cartographer::io::MakeUniqueCairoSurfacePtr(
cairo_image_surface_create_for_data( cairo_image_surface_create_for_data(
reinterpret_cast<unsigned char*>(submap_state.cairo_data.data()), reinterpret_cast<unsigned char*>(submap_slice.cairo_data.data()),
kCairoFormat, submap_state.width, submap_state.height, PaintSubmapSlicesResult::kCairoFormat, submap_slice.width,
expected_stride)); submap_slice.height, expected_stride));
CHECK_EQ(cairo_surface_status(submap_state.surface.get()), CHECK_EQ(cairo_surface_status(submap_slice.surface.get()),
CAIRO_STATUS_SUCCESS) CAIRO_STATUS_SUCCESS)
<< cairo_status_to_string( << cairo_status_to_string(
cairo_surface_status(submap_state.surface.get())); cairo_surface_status(submap_slice.surface.get()));
} }
last_timestamp_ = msg->header.stamp; last_timestamp_ = msg->header.stamp;
last_frame_id_ = msg->header.frame_id; last_frame_id_ = msg->header.frame_id;
} }
void Node::DrawAndPublish(const ::ros::WallTimerEvent& unused_timer_event) { 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; return;
} }
::cartographer::common::MutexLocker locker(&mutex_); ::cartographer::common::MutexLocker locker(&mutex_);
Eigen::AlignedBox2f bounding_box; auto painted_slices = PaintSubmapSlices(&submap_slices_, resolution_);
{ PublishOccupancyGrid(last_frame_id_, last_timestamp_, painted_slices.origin,
auto surface = ::cartographer::io::MakeUniqueCairoSurfacePtr( painted_slices.surface.get());
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());
}
} }
void Node::PublishOccupancyGrid(const std::string& frame_id, void Node::PublishOccupancyGrid(const std::string& frame_id,
const ros::Time& time, const ros::Time& time,
const Eigen::Array2f& origin, const Eigen::Array2f& origin,
const Eigen::Array2i& size,
cairo_surface_t* surface) { cairo_surface_t* surface) {
nav_msgs::OccupancyGrid occupancy_grid; 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.stamp = time;
occupancy_grid.header.frame_id = frame_id; occupancy_grid.header.frame_id = frame_id;
occupancy_grid.info.map_load_time = time; occupancy_grid.info.map_load_time = time;
occupancy_grid.info.resolution = resolution_; occupancy_grid.info.resolution = resolution_;
occupancy_grid.info.width = size.x(); occupancy_grid.info.width = width;
occupancy_grid.info.height = size.y(); occupancy_grid.info.height = height;
occupancy_grid.info.origin.position.x = -origin.x() * resolution_; occupancy_grid.info.origin.position.x = -origin.x() * resolution_;
occupancy_grid.info.origin.position.y = occupancy_grid.info.origin.position.y = (-height + origin.y()) * resolution_;
(-size.y() + origin.y()) * resolution_;
occupancy_grid.info.origin.position.z = 0.; occupancy_grid.info.origin.position.z = 0.;
occupancy_grid.info.origin.orientation.w = 1.; occupancy_grid.info.origin.orientation.w = 1.;
occupancy_grid.info.origin.orientation.x = 0.; occupancy_grid.info.origin.orientation.x = 0.;
occupancy_grid.info.origin.orientation.y = 0.; occupancy_grid.info.origin.orientation.y = 0.;
occupancy_grid.info.origin.orientation.z = 0.; occupancy_grid.info.origin.orientation.z = 0.;
const uint32_t* pixel_data = const uint32_t* pixel_data = reinterpret_cast<uint32_t*>(cairo_image_surface_get_data(surface));
reinterpret_cast<uint32_t*>(cairo_image_surface_get_data(surface)); occupancy_grid.data.reserve(width * height);
occupancy_grid.data.reserve(size.x() * size.y()); for (int y = height - 1; y >= 0; --y) {
for (int y = size.y() - 1; y >= 0; --y) { for (int x = 0; x < width; ++x) {
for (int x = 0; x < size.x(); ++x) { const uint32_t packed = pixel_data[y * width + x];
const uint32_t packed = pixel_data[y * size.x() + x];
const unsigned char color = packed >> 16; const unsigned char color = packed >> 16;
const unsigned char observed = packed >> 8; const unsigned char observed = packed >> 8;
const int value = const int value =