From b6fa20c9d599d1ac18974757425506a860376d62 Mon Sep 17 00:00:00 2001 From: Holger Rapp Date: Mon, 8 May 2017 15:07:14 +0200 Subject: [PATCH] Track Cartographer changes. (#329) Change googlecartographer/cartographer#248 introduced a separation between MapBuilder and TrajectoryOptions. The ROS repositiories require changes to track this modification. --- .../cartographer_ros/assets_writer.cc | 74 +++++++++---------- .../cartographer_ros/assets_writer.h | 17 ++++- .../cartographer_ros/map_builder_bridge.cc | 30 ++++++-- .../cartographer_ros/map_builder_bridge.h | 1 + .../cartographer_ros/node_main.cc | 2 +- .../cartographer_ros/node_options.cc | 3 + .../cartographer_ros/node_options.h | 2 + .../cartographer_ros/occupancy_grid.cc | 15 ++-- .../cartographer_ros/occupancy_grid.h | 8 +- .../cartographer_ros/offline_node_main.cc | 2 +- .../configuration_files/backpack_2d.lua | 2 + .../configuration_files/backpack_3d.lua | 2 + cartographer_ros/configuration_files/pr2.lua | 2 + .../configuration_files/revo_lds.lua | 2 + .../configuration_files/taurob_tracker.lua | 2 + 15 files changed, 103 insertions(+), 61 deletions(-) diff --git a/cartographer_ros/cartographer_ros/assets_writer.cc b/cartographer_ros/cartographer_ros/assets_writer.cc index a92e5e4..60b6f84 100644 --- a/cartographer_ros/cartographer_ros/assets_writer.cc +++ b/cartographer_ros/cartographer_ros/assets_writer.cc @@ -35,12 +35,41 @@ namespace { namespace carto = ::cartographer; +void WriteTrajectory(const std::vector<::cartographer::mapping::TrajectoryNode>& + trajectory_nodes, + const std::string& stem) { + carto::mapping::proto::Trajectory trajectory; + for (const auto& node : trajectory_nodes) { + const auto& data = *node.constant_data; + auto* node_proto = trajectory.add_node(); + node_proto->set_timestamp(carto::common::ToUniversal(data.time)); + *node_proto->mutable_pose() = + carto::transform::ToProto(node.pose * data.tracking_to_pose); + } + + // Write the trajectory. + std::ofstream proto_file(stem + ".pb", + std::ios_base::out | std::ios_base::binary); + CHECK(trajectory.SerializeToOstream(&proto_file)) + << "Could not serialize trajectory."; + proto_file.close(); + CHECK(proto_file) << "Could not write trajectory."; +} + +} // namespace + // Writes an occupancy grid. -void Write2DAssets(const std::vector<::cartographer::mapping::TrajectoryNode>& - trajectory_nodes, - const NodeOptions& options, const std::string& stem) { +void Write2DAssets( + const std::vector<::cartographer::mapping::TrajectoryNode>& + trajectory_nodes, + const string& map_frame, + const ::cartographer::mapping_2d::proto::SubmapsOptions& submaps_options, + const std::string& stem) { + WriteTrajectory(trajectory_nodes, stem); + ::nav_msgs::OccupancyGrid occupancy_grid; - BuildOccupancyGrid(trajectory_nodes, options, &occupancy_grid); + BuildOccupancyGrid2D(trajectory_nodes, map_frame, submaps_options, + &occupancy_grid); WriteOccupancyGridToPgmAndYaml(occupancy_grid, stem); } @@ -49,6 +78,8 @@ void Write2DAssets(const std::vector<::cartographer::mapping::TrajectoryNode>& void Write3DAssets(const std::vector<::cartographer::mapping::TrajectoryNode>& trajectory_nodes, const double voxel_size, const std::string& stem) { + WriteTrajectory(trajectory_nodes, stem); + const auto file_writer_factory = [](const string& filename) { return carto::common::make_unique(filename); }; @@ -86,39 +117,4 @@ void Write3DAssets(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) { - carto::mapping::proto::Trajectory trajectory; - for (const auto& node : trajectory_nodes) { - const auto& data = *node.constant_data; - auto* node_proto = trajectory.add_node(); - node_proto->set_timestamp(carto::common::ToUniversal(data.time)); - *node_proto->mutable_pose() = - carto::transform::ToProto(node.pose * data.tracking_to_pose); - } - - // Write the trajectory. - std::ofstream proto_file(stem + ".pb", - std::ios_base::out | std::ios_base::binary); - 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 9930ebc..5eb8218 100644 --- a/cartographer_ros/cartographer_ros/assets_writer.h +++ b/cartographer_ros/cartographer_ros/assets_writer.h @@ -25,10 +25,19 @@ namespace cartographer_ros { -// Writes all assets derivable from the 'trajectory_nodes' and 'options'. -void WriteAssets(const std::vector<::cartographer::mapping::TrajectoryNode>& - trajectory_nodes, - const NodeOptions& options, const std::string& stem); +// Writes a trajectory proto and an occupancy grid. +void Write2DAssets( + const std::vector<::cartographer::mapping::TrajectoryNode>& + trajectory_nodes, + const string& map_frame, + const ::cartographer::mapping_2d::proto::SubmapsOptions& submaps_options, + const std::string& stem); + +// Writes X-ray images, trajectory proto, 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); } // namespace cartographer_ros diff --git a/cartographer_ros/cartographer_ros/map_builder_bridge.cc b/cartographer_ros/cartographer_ros/map_builder_bridge.cc index 5759472..f58cdfd 100644 --- a/cartographer_ros/cartographer_ros/map_builder_bridge.cc +++ b/cartographer_ros/cartographer_ros/map_builder_bridge.cc @@ -32,8 +32,8 @@ MapBuilderBridge::MapBuilderBridge(const NodeOptions& options, int MapBuilderBridge::AddTrajectory( const std::unordered_set& expected_sensor_ids, const string& tracking_frame) { - const int trajectory_id = - map_builder_.AddTrajectoryBuilder(expected_sensor_ids); + const int trajectory_id = map_builder_.AddTrajectoryBuilder( + expected_sensor_ids, options_.trajectory_builder_options); LOG(INFO) << "Added trajectory with ID '" << trajectory_id << "'."; CHECK_EQ(sensor_bridges_.count(trajectory_id), 0); @@ -60,7 +60,22 @@ void MapBuilderBridge::WriteAssets(const string& stem) { LOG(WARNING) << "No data was collected and no assets will be written."; } else { LOG(INFO) << "Writing assets with stem '" << stem << "'..."; - cartographer_ros::WriteAssets(trajectory_nodes, options_, stem); + if (options_.map_builder_options.use_trajectory_builder_2d()) { + Write2DAssets( + trajectory_nodes, options_.map_frame, + options_.trajectory_builder_options.trajectory_builder_2d_options() + .submaps_options(), + stem); + } + + if (options_.map_builder_options.use_trajectory_builder_3d()) { + Write3DAssets( + trajectory_nodes, + options_.trajectory_builder_options.trajectory_builder_3d_options() + .submaps_options() + .high_resolution(), + stem); + } } } @@ -114,14 +129,19 @@ cartographer_ros_msgs::SubmapList MapBuilderBridge::GetSubmapList() { std::unique_ptr MapBuilderBridge::BuildOccupancyGrid() { + CHECK(options_.map_builder_options.use_trajectory_builder_2d()) + << "Publishing OccupancyGrids for 3D data is not yet supported"; const auto trajectory_nodes = map_builder_.sparse_pose_graph()->GetTrajectoryNodes(); std::unique_ptr occupancy_grid; if (!trajectory_nodes.empty()) { occupancy_grid = cartographer::common::make_unique(); - cartographer_ros::BuildOccupancyGrid(trajectory_nodes, options_, - occupancy_grid.get()); + BuildOccupancyGrid2D( + trajectory_nodes, options_.map_frame, + options_.trajectory_builder_options.trajectory_builder_2d_options() + .submaps_options(), + occupancy_grid.get()); } return occupancy_grid; } diff --git a/cartographer_ros/cartographer_ros/map_builder_bridge.h b/cartographer_ros/cartographer_ros/map_builder_bridge.h index 232fc73..8b9bc3b 100644 --- a/cartographer_ros/cartographer_ros/map_builder_bridge.h +++ b/cartographer_ros/cartographer_ros/map_builder_bridge.h @@ -22,6 +22,7 @@ #include #include "cartographer/mapping/map_builder.h" +#include "cartographer/mapping/proto/trajectory_builder_options.pb.h" #include "cartographer_ros/node_options.h" #include "cartographer_ros/sensor_bridge.h" #include "cartographer_ros/tf_bridge.h" diff --git a/cartographer_ros/cartographer_ros/node_main.cc b/cartographer_ros/cartographer_ros/node_main.cc index cbf524e..bce938d 100644 --- a/cartographer_ros/cartographer_ros/node_main.cc +++ b/cartographer_ros/cartographer_ros/node_main.cc @@ -113,7 +113,7 @@ void Run() { ::ros::Subscriber imu_subscriber; if (options.map_builder_options.use_trajectory_builder_3d() || (options.map_builder_options.use_trajectory_builder_2d() && - options.map_builder_options.trajectory_builder_2d_options() + options.trajectory_builder_options.trajectory_builder_2d_options() .use_imu_data())) { imu_subscriber = node.node_handle()->subscribe( kImuTopic, kInfiniteSubscriberQueueSize, diff --git a/cartographer_ros/cartographer_ros/node_options.cc b/cartographer_ros/cartographer_ros/node_options.cc index 2c7234a..14569a3 100644 --- a/cartographer_ros/cartographer_ros/node_options.cc +++ b/cartographer_ros/cartographer_ros/node_options.cc @@ -27,6 +27,9 @@ NodeOptions CreateNodeOptions( options.map_builder_options = ::cartographer::mapping::CreateMapBuilderOptions( lua_parameter_dictionary->GetDictionary("map_builder").get()); + options.trajectory_builder_options = + ::cartographer::mapping::CreateTrajectoryBuilderOptions( + lua_parameter_dictionary->GetDictionary("trajectory_builder").get()); options.map_frame = lua_parameter_dictionary->GetString("map_frame"); options.tracking_frame = lua_parameter_dictionary->GetString("tracking_frame"); diff --git a/cartographer_ros/cartographer_ros/node_options.h b/cartographer_ros/cartographer_ros/node_options.h index bb6f2ff..bc74ddd 100644 --- a/cartographer_ros/cartographer_ros/node_options.h +++ b/cartographer_ros/cartographer_ros/node_options.h @@ -29,6 +29,8 @@ namespace cartographer_ros { // Top-level options of Cartographer's ROS integration. struct NodeOptions { ::cartographer::mapping::proto::MapBuilderOptions map_builder_options; + ::cartographer::mapping::proto::TrajectoryBuilderOptions + trajectory_builder_options; string map_frame; string tracking_frame; string published_frame; diff --git a/cartographer_ros/cartographer_ros/occupancy_grid.cc b/cartographer_ros/cartographer_ros/occupancy_grid.cc index ecbfefb..5d358a8 100644 --- a/cartographer_ros/cartographer_ros/occupancy_grid.cc +++ b/cartographer_ros/cartographer_ros/occupancy_grid.cc @@ -16,22 +16,21 @@ #include "cartographer_ros/occupancy_grid.h" +#include "cartographer/mapping_2d/map_limits.h" +#include "cartographer/mapping_2d/probability_grid.h" +#include "cartographer/mapping_2d/range_data_inserter.h" #include "cartographer_ros/time_conversion.h" #include "glog/logging.h" namespace cartographer_ros { -void BuildOccupancyGrid( +void BuildOccupancyGrid2D( const std::vector<::cartographer::mapping::TrajectoryNode>& trajectory_nodes, - const NodeOptions& options, + const string& map_frame, + const ::cartographer::mapping_2d::proto::SubmapsOptions& submaps_options, ::nav_msgs::OccupancyGrid* const occupancy_grid) { namespace carto = ::cartographer; - CHECK(options.map_builder_options.use_trajectory_builder_2d()) - << "Publishing OccupancyGrids for 3D data is not yet supported"; - const auto& submaps_options = - options.map_builder_options.trajectory_builder_2d_options() - .submaps_options(); const carto::mapping_2d::MapLimits map_limits = carto::mapping_2d::MapLimits::ComputeMapLimits( submaps_options.resolution(), trajectory_nodes); @@ -47,7 +46,7 @@ void BuildOccupancyGrid( } occupancy_grid->header.stamp = ToRos(trajectory_nodes.back().time()); - occupancy_grid->header.frame_id = options.map_frame; + occupancy_grid->header.frame_id = map_frame; occupancy_grid->info.map_load_time = occupancy_grid->header.stamp; Eigen::Array2i offset; diff --git a/cartographer_ros/cartographer_ros/occupancy_grid.h b/cartographer_ros/cartographer_ros/occupancy_grid.h index 8d9bba4..c6d6f6e 100644 --- a/cartographer_ros/cartographer_ros/occupancy_grid.h +++ b/cartographer_ros/cartographer_ros/occupancy_grid.h @@ -20,15 +20,17 @@ #include #include "cartographer/mapping/trajectory_node.h" -#include "cartographer_ros/node_options.h" +#include "cartographer/mapping_2d/proto/submaps_options.pb.h" #include "nav_msgs/OccupancyGrid.h" namespace cartographer_ros { -void BuildOccupancyGrid( +void BuildOccupancyGrid2D( const std::vector<::cartographer::mapping::TrajectoryNode>& trajectory_nodes, - const NodeOptions& options, ::nav_msgs::OccupancyGrid* occupancy_grid); + const string& map_frame, + const ::cartographer::mapping_2d::proto::SubmapsOptions& submaps_options, + ::nav_msgs::OccupancyGrid* const occupancy_grid); } // namespace cartographer_ros diff --git a/cartographer_ros/cartographer_ros/offline_node_main.cc b/cartographer_ros/cartographer_ros/offline_node_main.cc index 165496e..121f427 100644 --- a/cartographer_ros/cartographer_ros/offline_node_main.cc +++ b/cartographer_ros/cartographer_ros/offline_node_main.cc @@ -140,7 +140,7 @@ void Run(const std::vector& bag_filenames) { // required. if (options.map_builder_options.use_trajectory_builder_3d() || (options.map_builder_options.use_trajectory_builder_2d() && - options.map_builder_options.trajectory_builder_2d_options() + options.trajectory_builder_options.trajectory_builder_2d_options() .use_imu_data())) { check_insert(kImuTopic); } diff --git a/cartographer_ros/configuration_files/backpack_2d.lua b/cartographer_ros/configuration_files/backpack_2d.lua index 13f5ce0..534b85e 100644 --- a/cartographer_ros/configuration_files/backpack_2d.lua +++ b/cartographer_ros/configuration_files/backpack_2d.lua @@ -13,9 +13,11 @@ -- limitations under the License. include "map_builder.lua" +include "trajectory_builder.lua" options = { map_builder = MAP_BUILDER, + trajectory_builder = TRAJECTORY_BUILDER, map_frame = "map", tracking_frame = "base_link", published_frame = "base_link", diff --git a/cartographer_ros/configuration_files/backpack_3d.lua b/cartographer_ros/configuration_files/backpack_3d.lua index 8a2a4bc..15d2941 100644 --- a/cartographer_ros/configuration_files/backpack_3d.lua +++ b/cartographer_ros/configuration_files/backpack_3d.lua @@ -13,9 +13,11 @@ -- limitations under the License. include "map_builder.lua" +include "trajectory_builder.lua" options = { map_builder = MAP_BUILDER, + trajectory_builder = TRAJECTORY_BUILDER, map_frame = "map", tracking_frame = "base_link", published_frame = "base_link", diff --git a/cartographer_ros/configuration_files/pr2.lua b/cartographer_ros/configuration_files/pr2.lua index d8ca9d4..7eec98c 100644 --- a/cartographer_ros/configuration_files/pr2.lua +++ b/cartographer_ros/configuration_files/pr2.lua @@ -13,9 +13,11 @@ -- limitations under the License. include "map_builder.lua" +include "trajectory_builder.lua" options = { map_builder = MAP_BUILDER, + trajectory_builder = TRAJECTORY_BUILDER, map_frame = "map", tracking_frame = "base_footprint", published_frame = "base_footprint", diff --git a/cartographer_ros/configuration_files/revo_lds.lua b/cartographer_ros/configuration_files/revo_lds.lua index 1c8631e..77250d7 100644 --- a/cartographer_ros/configuration_files/revo_lds.lua +++ b/cartographer_ros/configuration_files/revo_lds.lua @@ -13,9 +13,11 @@ -- limitations under the License. include "map_builder.lua" +include "trajectory_builder.lua" options = { map_builder = MAP_BUILDER, + trajectory_builder = TRAJECTORY_BUILDER, map_frame = "map", tracking_frame = "horizontal_laser_link", published_frame = "horizontal_laser_link", diff --git a/cartographer_ros/configuration_files/taurob_tracker.lua b/cartographer_ros/configuration_files/taurob_tracker.lua index ed05895..f7fe1db 100644 --- a/cartographer_ros/configuration_files/taurob_tracker.lua +++ b/cartographer_ros/configuration_files/taurob_tracker.lua @@ -13,9 +13,11 @@ -- limitations under the License. include "map_builder.lua" +include "trajectory_builder.lua" options = { map_builder = MAP_BUILDER, + trajectory_builder = TRAJECTORY_BUILDER, map_frame = "map", tracking_frame = "base_link", published_frame = "odom",