12345qiupeng
/
cartographer_ros
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Write the trajectory on exporting. (#134) 

And some refactorings around writing assets.
master
Holger Rapp 2016-10-20 14:09:05 +02:00 committed by GitHub
parent af78dcbe55
commit 16f192aeb9
4 changed files with 58 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
cartographer_ros/cartographer_ros/CMakeLists.txt
View File

@ -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

48
cartographer_ros/cartographer_ros/assets_writer.cc
View File

@ -21,12 +21,30 @@
#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>&
// 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;
@ -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

6
cartographer_ros/cartographer_ros/assets_writer.h
View File

@ -21,14 +21,14 @@
#include <vector>
#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

16
cartographer_ros/cartographer_ros/node_main.cc
View File

@ -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;
}