diff --git a/cartographer_ros/cartographer_ros/CMakeLists.txt b/cartographer_ros/cartographer_ros/CMakeLists.txt index acd15b8..61bdc21 100644 --- a/cartographer_ros/cartographer_ros/CMakeLists.txt +++ b/cartographer_ros/cartographer_ros/CMakeLists.txt @@ -55,3 +55,14 @@ install(TARGETS cartographer_start_trajectory LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) + +google_binary(cartographer_occupancy_grid_node + SRCS + occupancy_grid_node_main.cc +) + +install(TARGETS cartographer_occupancy_grid_node + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) diff --git a/cartographer_ros/cartographer_ros/occupancy_grid_node_main.cc b/cartographer_ros/cartographer_ros/occupancy_grid_node_main.cc new file mode 100644 index 0000000..f2fa815 --- /dev/null +++ b/cartographer_ros/cartographer_ros/occupancy_grid_node_main.cc @@ -0,0 +1,318 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include +#include + +#include "Eigen/Core" +#include "Eigen/Geometry" +#include "cairo/cairo.h" +#include "cartographer/common/mutex.h" +#include "cartographer/common/port.h" +#include "cartographer/mapping/id.h" +#include "cartographer/transform/rigid_transform.h" +#include "cartographer_ros/msg_conversion.h" +#include "cartographer_ros/node_constants.h" +#include "cartographer_ros/ros_log_sink.h" +#include "cartographer_ros/submap.h" +#include "cartographer_ros_msgs/SubmapList.h" +#include "cartographer_ros_msgs/SubmapQuery.h" +#include "gflags/gflags.h" +#include "nav_msgs/OccupancyGrid.h" +#include "ros/ros.h" + +DEFINE_double(resolution, 0.05, + "Resolution of a grid cell in the published occupancy grid."); + +namespace cartographer_ros { +namespace { + +using ::cartographer::mapping::SubmapId; + +constexpr cairo_format_t kCairoFormat = CAIRO_FORMAT_ARGB32; + +// std::unique_ptr for Cairo surfaces. The surface is destroyed when the +// std::unique_ptr is reset or destroyed. +using UniqueCairoSurfacePtr = + std::unique_ptr; + +UniqueCairoSurfacePtr MakeUniqueCairoSurfacePtr(cairo_surface_t* surface) { + return UniqueCairoSurfacePtr(surface, cairo_surface_destroy); +} + +// std::unique_ptr for Cairo contexts. +using UniqueCairoPtr = std::unique_ptr; + +UniqueCairoPtr MakeUniqueCairoPtr(cairo_t* surface) { + return UniqueCairoPtr(surface, cairo_destroy); +} + +Eigen::Affine3d ToEigen(const ::cartographer::transform::Rigid3d& rigid3) { + return Eigen::Translation3d(rigid3.translation()) * rigid3.rotation(); +} + +struct SubmapState { + SubmapState() : surface(MakeUniqueCairoSurfacePtr(nullptr)) {} + + // Texture data. + int width; + int height; + int version; + double resolution; + ::cartographer::transform::Rigid3d slice_pose; + UniqueCairoSurfacePtr surface; + // Pixel data used by 'surface'. Must outlive 'surface'. + std::vector cairo_data; + + // Metadata. + ::cartographer::transform::Rigid3d pose; + int metadata_version = -1; +}; + +void CairoDrawEachSubmap( + const double scale, std::map* submaps, cairo_t* cr, + std::function 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); + ~Node() {} + + Node(const Node&) = delete; + Node& operator=(const Node&) = delete; + + private: + void HandleSubmapList(const cartographer_ros_msgs::SubmapList::ConstPtr& msg); + void DrawAndPublish(const string& frame_id, const ros::Time& time); + void PublishOccupancyGrid(const string& frame_id, const ros::Time& time, + const Eigen::Array2f& origin, + const Eigen::Array2i& size, + cairo_surface_t* surface); + + ::ros::NodeHandle node_handle_; + const double resolution_; + + ::cartographer::common::Mutex mutex_; + ::ros::ServiceClient client_ GUARDED_BY(mutex_); + ::ros::Subscriber submap_list_subscriber_ GUARDED_BY(mutex_); + ::ros::Publisher occupancy_grid_publisher_ GUARDED_BY(mutex_); + std::map submaps_ GUARDED_BY(mutex_); +}; + +Node::Node(const double resolution) + : resolution_(resolution), + client_(node_handle_.serviceClient<::cartographer_ros_msgs::SubmapQuery>( + kSubmapQueryServiceName)), + submap_list_subscriber_(node_handle_.subscribe( + kSubmapListTopic, kLatestOnlyPublisherQueueSize, + boost::function( + [this](const cartographer_ros_msgs::SubmapList::ConstPtr& msg) { + HandleSubmapList(msg); + }))), + occupancy_grid_publisher_( + node_handle_.advertise<::nav_msgs::OccupancyGrid>( + kOccupancyGridTopic, kLatestOnlyPublisherQueueSize, + true /* latched */)) + +{} + +void Node::HandleSubmapList( + const cartographer_ros_msgs::SubmapList::ConstPtr& msg) { + ::cartographer::common::MutexLocker locker(&mutex_); + + // We do not do any work if nobody listens. + if (occupancy_grid_publisher_.getNumSubscribers() == 0) { + return; + } + 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) { + continue; + } + + auto fetched_texture = ::cartographer_ros::FetchSubmapTexture(id, &client_); + if (fetched_texture == nullptr) { + continue; + } + submap_state.width = fetched_texture->width; + submap_state.height = fetched_texture->height; + submap_state.version = fetched_texture->version; + submap_state.slice_pose = fetched_texture->slice_pose; + submap_state.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; + CHECK_EQ(expected_stride, + cairo_format_stride_for_width(kCairoFormat, submap_state.width)); + submap_state.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) | + (observed << 8) | 0); + } + + submap_state.surface = + MakeUniqueCairoSurfacePtr(cairo_image_surface_create_for_data( + reinterpret_cast(submap_state.cairo_data.data()), + kCairoFormat, submap_state.width, submap_state.height, + expected_stride)); + CHECK_EQ(cairo_surface_status(submap_state.surface.get()), + CAIRO_STATUS_SUCCESS) + << cairo_status_to_string( + cairo_surface_status(submap_state.surface.get())); + } + DrawAndPublish(msg->header.frame_id, msg->header.stamp); +} + +void Node::DrawAndPublish(const string& frame_id, const ros::Time& time) { + if (submaps_.empty()) { + return; + } + + Eigen::AlignedBox2f bounding_box; + { + auto surface = MakeUniqueCairoSurfacePtr( + cairo_image_surface_create(kCairoFormat, 1, 1)); + auto cr = 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 = MakeUniqueCairoSurfacePtr( + cairo_image_surface_create(kCairoFormat, size.x(), size.y())); + auto cr = 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(frame_id, time, origin, size, surface.get()); + } +} + +void Node::PublishOccupancyGrid(const string& frame_id, const ros::Time& time, + const Eigen::Array2f& origin, + const Eigen::Array2i& size, + cairo_surface_t* surface) { + nav_msgs::OccupancyGrid occupancy_grid; + 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.origin.position.x = -origin.x() * resolution_; + occupancy_grid.info.origin.position.y = + (-size.y() + 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* pixel_data = + reinterpret_cast(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 packed = pixel_data[y * size.x() + 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); +} + +} // namespace +} // namespace cartographer_ros + +int main(int argc, char** argv) { + google::InitGoogleLogging(argv[0]); + google::ParseCommandLineFlags(&argc, &argv, true); + + ::ros::init(argc, argv, "cartographer_occupancy_grid_node"); + ::ros::start(); + + cartographer_ros::ScopedRosLogSink ros_log_sink; + ::cartographer_ros::Node node(FLAGS_resolution); + + ::ros::spin(); + ::ros::shutdown(); +}