Follow googlecartographer/cartographer#599 (#561)
Use submap_painter. Merge this after googlecartographer/cartographer#599.master
Jihoon Lee
Wally B. Feed
parent
ae6aefaf97
commit
d0697e6847
|
|
@ -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<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 {
|
||||
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<SubmapId, SubmapState> submaps_ GUARDED_BY(mutex_);
|
||||
std::map<SubmapId, SubmapSlice> 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<unsigned char*>(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<unsigned char*>(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<uint32_t*>(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<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 =
|
||||
|
|
|
|||
Loading…
Reference in New Issue