From 16f192aeb94893fabb55a630af51f02ea5b85651 Mon Sep 17 00:00:00 2001 From: Holger Rapp Date: Thu, 20 Oct 2016 14:09:05 +0200 Subject: [PATCH] Write the trajectory on exporting. (#134) And some refactorings around writing assets. --- .../cartographer_ros/CMakeLists.txt | 6 ++- .../cartographer_ros/assets_writer.cc | 52 +++++++++++++++++-- .../cartographer_ros/assets_writer.h | 6 +-- .../cartographer_ros/node_main.cc | 16 +----- 4 files changed, 58 insertions(+), 22 deletions(-) diff --git a/cartographer_ros/cartographer_ros/CMakeLists.txt b/cartographer_ros/cartographer_ros/CMakeLists.txt index a84ef2d..418180f 100644 --- a/cartographer_ros/cartographer_ros/CMakeLists.txt +++ b/cartographer_ros/cartographer_ros/CMakeLists.txt @@ -1,9 +1,14 @@ google_library(assets_writer USES_CARTOGRAPHER + USES_ROS SRCS assets_writer.cc HDRS assets_writer.h + DEPENDS + map_writer + node_options + occupancy_grid ) google_library(map_writer @@ -114,7 +119,6 @@ google_binary(cartographer_node node_main.cc DEPENDS assets_writer - map_writer msg_conversion node_options occupancy_grid diff --git a/cartographer_ros/cartographer_ros/assets_writer.cc b/cartographer_ros/cartographer_ros/assets_writer.cc index d0807f9..13f295a 100644 --- a/cartographer_ros/cartographer_ros/assets_writer.cc +++ b/cartographer_ros/cartographer_ros/assets_writer.cc @@ -21,14 +21,32 @@ #include "cartographer/io/ply_writing_points_processor.h" #include "cartographer/io/points_processor.h" #include "cartographer/io/xray_points_processor.h" +#include "cartographer/mapping_2d/proto/laser_fan_inserter_options.pb.h" +#include "cartographer/proto/trajectory.pb.h" +#include "cartographer_ros/map_writer.h" +#include "cartographer_ros/occupancy_grid.h" +#include "nav_msgs/OccupancyGrid.h" namespace cartographer_ros { +namespace { + namespace carto = ::cartographer; -void WriteAssets(const std::vector<::cartographer::mapping::TrajectoryNode>& - trajectory_nodes, - const double voxel_size, const std::string& stem) { +// Writes an occupany grid. +void Write2DAssets(const std::vector<::cartographer::mapping::TrajectoryNode>& + trajectory_nodes, + const NodeOptions& options, const std::string& stem) { + ::nav_msgs::OccupancyGrid occupancy_grid; + BuildOccupancyGrid(trajectory_nodes, options, &occupancy_grid); + WriteOccupancyGridToPgmAndYaml(occupancy_grid, stem); +} + +// Writes X-ray images and PLY files from the 'trajectory_nodes'. The filenames +// will all start with 'stem'. +void Write3DAssets(const std::vector<::cartographer::mapping::TrajectoryNode>& + trajectory_nodes, + const double voxel_size, const std::string& stem) { carto::io::NullPointsProcessor null_points_processor; carto::io::XRayPointsProcessor xy_xray_points_processor( voxel_size, carto::transform::Rigid3f::Rotation( @@ -63,4 +81,32 @@ void WriteAssets(const std::vector<::cartographer::mapping::TrajectoryNode>& ply_writing_points_processor.Flush(); } +} // namespace + +void WriteAssets(const std::vector<::cartographer::mapping::TrajectoryNode>& + trajectory_nodes, + const NodeOptions& options, const std::string& stem) { + // Write the trajectory. + std::ofstream proto_file(stem + ".pb", + std::ios_base::out | std::ios_base::binary); + const carto::proto::Trajectory trajectory = + carto::mapping::ToProto(trajectory_nodes); + CHECK(trajectory.SerializeToOstream(&proto_file)) + << "Could not serialize trajectory."; + proto_file.close(); + CHECK(proto_file) << "Could not write trajectory."; + + if (options.map_builder_options.use_trajectory_builder_2d()) { + Write2DAssets(trajectory_nodes, options, stem); + } + + if (options.map_builder_options.use_trajectory_builder_3d()) { + Write3DAssets(trajectory_nodes, + options.map_builder_options.trajectory_builder_3d_options() + .submaps_options() + .high_resolution(), + stem); + } +} + } // namespace cartographer_ros diff --git a/cartographer_ros/cartographer_ros/assets_writer.h b/cartographer_ros/cartographer_ros/assets_writer.h index 92bc0b1..9930ebc 100644 --- a/cartographer_ros/cartographer_ros/assets_writer.h +++ b/cartographer_ros/cartographer_ros/assets_writer.h @@ -21,14 +21,14 @@ #include #include "cartographer/mapping/trajectory_node.h" +#include "cartographer_ros/node_options.h" namespace cartographer_ros { -// Writes X-ray images and PLY files from the 'trajectory_nodes'. The filenames -// will all start with 'stem'. +// Writes all assets derivable from the 'trajectory_nodes' and 'options'. void WriteAssets(const std::vector<::cartographer::mapping::TrajectoryNode>& trajectory_nodes, - double voxel_size, const std::string& stem); + const NodeOptions& options, const std::string& stem); } // namespace cartographer_ros diff --git a/cartographer_ros/cartographer_ros/node_main.cc b/cartographer_ros/cartographer_ros/node_main.cc index 10a6612..805fce1 100644 --- a/cartographer_ros/cartographer_ros/node_main.cc +++ b/cartographer_ros/cartographer_ros/node_main.cc @@ -47,7 +47,6 @@ #include "cartographer/transform/rigid_transform.h" #include "cartographer/transform/transform.h" #include "cartographer_ros/assets_writer.h" -#include "cartographer_ros/map_writer.h" #include "cartographer_ros/msg_conversion.h" #include "cartographer_ros/node_options.h" #include "cartographer_ros/occupancy_grid.h" @@ -357,20 +356,7 @@ bool Node::HandleFinishTrajectory( LOG(WARNING) << "Map is empty and will not be saved."; return true; } - - if (options_.map_builder_options.use_trajectory_builder_2d()) { - ::nav_msgs::OccupancyGrid occupancy_grid; - BuildOccupancyGrid(trajectory_nodes, options_, &occupancy_grid); - WriteOccupancyGridToPgmAndYaml(occupancy_grid, request.stem); - } - - if (options_.map_builder_options.use_trajectory_builder_3d()) { - WriteAssets(trajectory_nodes, - options_.map_builder_options.trajectory_builder_3d_options() - .submaps_options() - .high_resolution(), - request.stem); - } + WriteAssets(trajectory_nodes, options_, request.stem); return true; }