diff --git a/cartographer_ros/cartographer_ros/CMakeLists.txt b/cartographer_ros/cartographer_ros/CMakeLists.txt index 5b9bc91..bd567bd 100644 --- a/cartographer_ros/cartographer_ros/CMakeLists.txt +++ b/cartographer_ros/cartographer_ros/CMakeLists.txt @@ -91,6 +91,17 @@ install(TARGETS cartographer_pbstream_to_ros_map RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) +google_binary(cartographer_pbstream_map_publisher + SRCS + pbstream_map_publisher_main.cc +) + +install(TARGETS cartographer_pbstream_map_publisher + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + # TODO(cschuet): Add support for shared library case. if (${BUILD_GRPC}) google_binary(cartographer_grpc_node diff --git a/cartographer_ros/cartographer_ros/msg_conversion.cc b/cartographer_ros/cartographer_ros/msg_conversion.cc index 07c9501..22b183a 100644 --- a/cartographer_ros/cartographer_ros/msg_conversion.cc +++ b/cartographer_ros/cartographer_ros/msg_conversion.cc @@ -21,6 +21,7 @@ #include "cartographer/common/math.h" #include "cartographer/common/port.h" #include "cartographer/common/time.h" +#include "cartographer/io/submap_painter.h" #include "cartographer/transform/proto/transform.pb.h" #include "cartographer/transform/transform.h" #include "cartographer_ros/time_conversion.h" @@ -30,6 +31,7 @@ #include "geometry_msgs/TransformStamped.h" #include "geometry_msgs/Vector3.h" #include "glog/logging.h" +#include "nav_msgs/OccupancyGrid.h" #include "ros/ros.h" #include "ros/serialization.h" #include "sensor_msgs/Imu.h" @@ -284,4 +286,53 @@ cartographer::transform::Rigid3d ComputeLocalFrameFromLatLong( return cartographer::transform::Rigid3d(rotation * -translation, rotation); } +std::unique_ptr CreateOccupancyGridMsg( + const cartographer::io::PaintSubmapSlicesResult& painted_slices, + const double resolution, const std::string& frame_id, + const ros::Time& time) { + auto occupancy_grid = + ::cartographer::common::make_unique(); + + const int width = cairo_image_surface_get_width(painted_slices.surface.get()); + const int height = + cairo_image_surface_get_height(painted_slices.surface.get()); + const ros::Time now = ros::Time::now(); + + 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 = + -painted_slices.origin.x() * resolution; + occupancy_grid->info.origin.position.y = + (-height + painted_slices.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( + cairo_image_surface_get_data(painted_slices.surface.get())); + 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); + } + } + + return occupancy_grid; +} + } // namespace cartographer_ros diff --git a/cartographer_ros/cartographer_ros/msg_conversion.h b/cartographer_ros/cartographer_ros/msg_conversion.h index c66ddef..3941d1e 100644 --- a/cartographer_ros/cartographer_ros/msg_conversion.h +++ b/cartographer_ros/cartographer_ros/msg_conversion.h @@ -19,11 +19,13 @@ #include "cartographer/common/port.h" #include "cartographer/common/time.h" +#include "cartographer/io/submap_painter.h" #include "cartographer/sensor/point_cloud.h" #include "cartographer/transform/rigid_transform.h" #include "geometry_msgs/Pose.h" #include "geometry_msgs/Transform.h" #include "geometry_msgs/TransformStamped.h" +#include "nav_msgs/OccupancyGrid.h" #include "pcl/point_cloud.h" #include "pcl/point_types.h" #include "pcl_conversions/pcl_conversions.h" @@ -79,6 +81,13 @@ Eigen::Vector3d LatLongAltToEcef(double latitude, double longitude, cartographer::transform::Rigid3d ComputeLocalFrameFromLatLong(double latitude, double longitude); +// Points to an occupancy grid message at a specific resolution from painted +// submap slices obtained via ::cartographer::io::PaintSubmapSlices(...). +std::unique_ptr CreateOccupancyGridMsg( + const cartographer::io::PaintSubmapSlicesResult& painted_slices, + const double resolution, const std::string& frame_id, + const ros::Time& time); + } // namespace cartographer_ros #endif // CARTOGRAPHER_ROS_MSG_CONVERSION_H_ diff --git a/cartographer_ros/cartographer_ros/pbstream_map_publisher_main.cc b/cartographer_ros/cartographer_ros/pbstream_map_publisher_main.cc new file mode 100644 index 0000000..0104e0d --- /dev/null +++ b/cartographer_ros/cartographer_ros/pbstream_map_publisher_main.cc @@ -0,0 +1,115 @@ +/* + * Copyright 2018 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. + */ + +// Publishes a frozen nav_msgs/OccupancyGrid map from serialized submaps. + +#include +#include + +#include "cartographer/io/proto_stream.h" +#include "cartographer/io/submap_painter.h" +#include "cartographer/mapping/proto/pose_graph.pb.h" +#include "cartographer/mapping/proto/serialization.pb.h" +#include "cartographer/mapping/proto/submap.pb.h" +#include "cartographer/mapping/submaps.h" +#include "cartographer/mapping_2d/probability_grid.h" +#include "cartographer/mapping_2d/submaps.h" +#include "cartographer/mapping_3d/submaps.h" +#include "cartographer_ros/msg_conversion.h" +#include "cartographer_ros/node_constants.h" +#include "cartographer_ros/ros_log_sink.h" +#include "cartographer_ros/ros_map.h" +#include "cartographer_ros/submap.h" +#include "gflags/gflags.h" +#include "glog/logging.h" +#include "nav_msgs/OccupancyGrid.h" +#include "ros/ros.h" + +DEFINE_string(pbstream_filename, "", + "Filename of a pbstream to draw a map from."); +DEFINE_string(map_topic, "map", "Name of the published map topic."); +DEFINE_string(map_frame_id, "map", "Frame ID of the published map."); +DEFINE_double(resolution, 0.05, "Resolution of a grid cell in the drawn map."); + +namespace cartographer_ros { +namespace { + +void Run(const std::string& pbstream_filename, const std::string& map_topic, + const std::string& map_frame_id, const double resolution) { + ::cartographer::io::ProtoStreamReader reader(pbstream_filename); + + ::cartographer::mapping::proto::PoseGraph pose_graph; + CHECK(reader.ReadProto(&pose_graph)); + + LOG(INFO) << "Loading submap slices from serialized data."; + std::map<::cartographer::mapping::SubmapId, ::cartographer::io::SubmapSlice> + submap_slices; + for (;;) { + ::cartographer::mapping::proto::SerializedData proto; + if (!reader.ReadProto(&proto)) { + break; + } + if (proto.has_submap()) { + const auto& submap = proto.submap(); + const ::cartographer::mapping::SubmapId id{ + submap.submap_id().trajectory_id(), + submap.submap_id().submap_index()}; + const ::cartographer::transform::Rigid3d global_submap_pose = + ::cartographer::transform::ToRigid3( + pose_graph.trajectory(id.trajectory_id) + .submap(id.submap_index) + .pose()); + FillSubmapSlice(global_submap_pose, submap, &submap_slices[id]); + } + } + CHECK(reader.eof()); + + ::ros::NodeHandle node_handle(""); + ::ros::Publisher pub = node_handle.advertise( + map_topic, kLatestOnlyPublisherQueueSize, true /*latched */); + + LOG(INFO) << "Generating combined map image from submap slices."; + const auto painted_slices = + ::cartographer::io::PaintSubmapSlices(submap_slices, resolution); + std::unique_ptr msg_ptr = CreateOccupancyGridMsg( + painted_slices, resolution, FLAGS_map_frame_id, ros::Time::now()); + + LOG(INFO) << "Publishing occupancy grid topic " << map_topic + << " (frame_id: " << map_frame_id + << ", resolution:" << std::to_string(resolution) << ")."; + pub.publish(*msg_ptr); + ::ros::spin(); + ::ros::shutdown(); +} + +} // namespace +} // namespace cartographer_ros + +int main(int argc, char** argv) { + FLAGS_alsologtostderr = true; + google::InitGoogleLogging(argv[0]); + google::ParseCommandLineFlags(&argc, &argv, true); + + CHECK(!FLAGS_pbstream_filename.empty()) << "-pbstream_filename is missing."; + + ::ros::init(argc, argv, "cartographer_pbstream_map_publisher"); + ::ros::start(); + + cartographer_ros::ScopedRosLogSink ros_log_sink; + + ::cartographer_ros::Run(FLAGS_pbstream_filename, FLAGS_map_topic, + FLAGS_map_frame_id, FLAGS_resolution); +}