diff --git a/cartographer_ros/cartographer_ros/assets_writer.cc b/cartographer_ros/cartographer_ros/assets_writer.cc index d91e02d..05cc123 100644 --- a/cartographer_ros/cartographer_ros/assets_writer.cc +++ b/cartographer_ros/cartographer_ros/assets_writer.cc @@ -31,34 +31,6 @@ namespace cartographer_ros { -namespace { - -namespace carto = ::cartographer; - -void WriteTrajectory(const std::vector<::cartographer::mapping::TrajectoryNode>& - trajectory_nodes, - const std::string& stem) { - carto::mapping::proto::Trajectory trajectory; - // TODO(whess): Add multi-trajectory support. - 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>& @@ -66,8 +38,6 @@ void Write2DAssets( 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; BuildOccupancyGrid2D(trajectory_nodes, map_frame, submaps_options, &occupancy_grid); @@ -79,8 +49,7 @@ void Write2DAssets( void Write3DAssets(const std::vector<::cartographer::mapping::TrajectoryNode>& trajectory_nodes, const double voxel_size, const std::string& stem) { - WriteTrajectory(trajectory_nodes, stem); - + namespace carto = ::cartographer; const auto file_writer_factory = [](const string& filename) { return carto::common::make_unique(filename); }; diff --git a/cartographer_ros/cartographer_ros/assets_writer_main.cc b/cartographer_ros/cartographer_ros/assets_writer_main.cc index 317a1b8..e8af902 100644 --- a/cartographer_ros/cartographer_ros/assets_writer_main.cc +++ b/cartographer_ros/cartographer_ros/assets_writer_main.cc @@ -26,6 +26,7 @@ #include "cartographer/io/file_writer.h" #include "cartographer/io/points_processor.h" #include "cartographer/io/points_processor_pipeline_builder.h" +#include "cartographer/mapping/proto/sparse_pose_graph.pb.h" #include "cartographer/sensor/point_cloud.h" #include "cartographer/sensor/range_data.h" #include "cartographer/transform/transform_interpolation_buffer.h" @@ -55,8 +56,8 @@ DEFINE_string( "URDF file that contains static links for your sensor configuration."); DEFINE_string(bag_filename, "", "Bag to process."); DEFINE_string( - trajectory_filename, "", - "Proto containing the trajectory written by /finish_trajectory service."); + pose_graph_filename, "", + "Proto containing the pose graph written by /write_assets service."); DEFINE_bool(use_bag_transforms, true, "Whether to read and use the transforms from the bag."); @@ -134,7 +135,7 @@ void HandleMessage( pipeline.back()->Process(std::move(batch)); } -void Run(const string& trajectory_filename, const string& bag_filename, +void Run(const string& pose_graph_filename, const string& bag_filename, const string& configuration_directory, const string& configuration_basename, const string& urdf_filename) { auto file_resolver = @@ -145,9 +146,15 @@ void Run(const string& trajectory_filename, const string& bag_filename, carto::common::LuaParameterDictionary lua_parameter_dictionary( code, std::move(file_resolver)); - std::ifstream stream(trajectory_filename.c_str()); - carto::mapping::proto::Trajectory trajectory_proto; - CHECK(trajectory_proto.ParseFromIstream(&stream)); + std::ifstream stream(pose_graph_filename.c_str()); + carto::mapping::proto::SparsePoseGraph pose_graph_proto; + CHECK(pose_graph_proto.ParseFromIstream(&stream)); + CHECK_EQ(pose_graph_proto.trajectory_size(), 1) + << "Only pose graphs containing a single trajectory are supported."; + const carto::mapping::proto::Trajectory& trajectory_proto = + pose_graph_proto.trajectory(0); + CHECK_GT(trajectory_proto.node_size(), 0) + << "Trajectory does not contain any nodes."; const auto file_writer_factory = [](const string& filename) { return carto::common::make_unique(filename); @@ -222,10 +229,10 @@ int main(int argc, char** argv) { CHECK(!FLAGS_configuration_basename.empty()) << "-configuration_basename is missing."; CHECK(!FLAGS_bag_filename.empty()) << "-bag_filename is missing."; - CHECK(!FLAGS_trajectory_filename.empty()) - << "-trajectory_filename is missing."; + CHECK(!FLAGS_pose_graph_filename.empty()) + << "-pose_graph_filename is missing."; - ::cartographer_ros::Run(FLAGS_trajectory_filename, FLAGS_bag_filename, + ::cartographer_ros::Run(FLAGS_pose_graph_filename, FLAGS_bag_filename, FLAGS_configuration_directory, FLAGS_configuration_basename, FLAGS_urdf_filename); } diff --git a/cartographer_ros/cartographer_ros/map_builder_bridge.cc b/cartographer_ros/cartographer_ros/map_builder_bridge.cc index 88719dc..be46037 100644 --- a/cartographer_ros/cartographer_ros/map_builder_bridge.cc +++ b/cartographer_ros/cartographer_ros/map_builder_bridge.cc @@ -59,22 +59,29 @@ void MapBuilderBridge::FinishTrajectory(const int trajectory_id) { sensor_bridges_.erase(trajectory_id); } +void MapBuilderBridge::SerializeState(const std::string& stem) { + std::ofstream proto_file(stem + ".pb", + std::ios_base::out | std::ios_base::binary); + CHECK(map_builder_.sparse_pose_graph()->ToProto().SerializeToOstream( + &proto_file)) + << "Could not serialize pose graph."; + proto_file.close(); + CHECK(proto_file) << "Could not write pose graph."; +} + void MapBuilderBridge::WriteAssets(const string& stem) { - std::vector<::cartographer::mapping::TrajectoryNode> trajectory_nodes; - for (const auto& single_trajectory : - map_builder_.sparse_pose_graph()->GetTrajectoryNodes()) { - trajectory_nodes.insert(trajectory_nodes.end(), single_trajectory.begin(), - single_trajectory.end()); - } - if (trajectory_nodes.empty()) { + const auto trajectory_nodes = + map_builder_.sparse_pose_graph()->GetTrajectoryNodes(); + // TODO(yutakaoka): Add multi-trajectory support. + CHECK_EQ(trajectory_options_.count(0), 1); + CHECK_EQ(trajectory_nodes.size(), 1); + if (trajectory_nodes[0].empty()) { LOG(WARNING) << "No data was collected and no assets will be written."; } else { LOG(INFO) << "Writing assets with stem '" << stem << "'..."; - // TODO(yutakaoka): Add multi-trajectory support. - CHECK_EQ(trajectory_options_.count(0), 1); if (node_options_.map_builder_options.use_trajectory_builder_2d()) { Write2DAssets( - trajectory_nodes, node_options_.map_frame, + trajectory_nodes[0], node_options_.map_frame, trajectory_options_[0] .trajectory_builder_options.trajectory_builder_2d_options() .submaps_options(), @@ -83,7 +90,7 @@ void MapBuilderBridge::WriteAssets(const string& stem) { if (node_options_.map_builder_options.use_trajectory_builder_3d()) { Write3DAssets( - trajectory_nodes, + trajectory_nodes[0], trajectory_options_[0] .trajectory_builder_options.trajectory_builder_3d_options() .submaps_options() diff --git a/cartographer_ros/cartographer_ros/map_builder_bridge.h b/cartographer_ros/cartographer_ros/map_builder_bridge.h index cc7d019..2c8f83a 100644 --- a/cartographer_ros/cartographer_ros/map_builder_bridge.h +++ b/cartographer_ros/cartographer_ros/map_builder_bridge.h @@ -52,6 +52,7 @@ class MapBuilderBridge { int AddTrajectory(const std::unordered_set& expected_sensor_ids, const TrajectoryOptions& trajectory_options); void FinishTrajectory(int trajectory_id); + void SerializeState(const string& stem); void WriteAssets(const string& stem); bool HandleSubmapQuery( diff --git a/cartographer_ros/cartographer_ros/node.cc b/cartographer_ros/cartographer_ros/node.cc index 5f8244c..967c07c 100644 --- a/cartographer_ros/cartographer_ros/node.cc +++ b/cartographer_ros/cartographer_ros/node.cc @@ -484,6 +484,7 @@ bool Node::HandleWriteAssets( ::cartographer_ros_msgs::WriteAssets::Request& request, ::cartographer_ros_msgs::WriteAssets::Response& response) { carto::common::MutexLocker lock(&mutex_); + map_builder_bridge_.SerializeState(request.stem); map_builder_bridge_.WriteAssets(request.stem); return true; } diff --git a/cartographer_ros/cartographer_ros/offline_node_main.cc b/cartographer_ros/cartographer_ros/offline_node_main.cc index dfbf508..bfd32dd 100644 --- a/cartographer_ros/cartographer_ros/offline_node_main.cc +++ b/cartographer_ros/cartographer_ros/offline_node_main.cc @@ -262,6 +262,7 @@ void Run(const std::vector& bag_filenames) { node.map_builder_bridge()->FinishTrajectory(trajectory_id); } + node.map_builder_bridge()->SerializeState(bag_filenames.front()); node.map_builder_bridge()->WriteAssets(bag_filenames.front()); } diff --git a/cartographer_ros/launch/assets_writer_backpack_2d.launch b/cartographer_ros/launch/assets_writer_backpack_2d.launch index 4f0e605..8432f2a 100644 --- a/cartographer_ros/launch/assets_writer_backpack_2d.launch +++ b/cartographer_ros/launch/assets_writer_backpack_2d.launch @@ -21,7 +21,7 @@ -configuration_basename assets_writer_backpack_2d.lua -urdf_filename $(find cartographer_ros)/urdf/backpack_2d.urdf -bag_filename $(arg bag_filenames) - -trajectory_filename $(arg trajectory_filename)" + -pose_graph_filename $(arg pose_graph_filename)" output="screen"> diff --git a/cartographer_ros/launch/assets_writer_backpack_3d.launch b/cartographer_ros/launch/assets_writer_backpack_3d.launch index 0472219..c350f39 100644 --- a/cartographer_ros/launch/assets_writer_backpack_3d.launch +++ b/cartographer_ros/launch/assets_writer_backpack_3d.launch @@ -21,7 +21,7 @@ -configuration_basename assets_writer_backpack_3d.lua -urdf_filename $(find cartographer_ros)/urdf/backpack_3d.urdf -bag_filename $(arg bag_filenames) - -trajectory_filename $(arg trajectory_filename)" + -pose_graph_filename $(arg pose_graph_filename)" output="screen">