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

Track Cartographer changes. (#329) 

Change googlecartographer/cartographer#248 introduced a separation
between MapBuilder and TrajectoryOptions. The ROS repositiories require
changes to track this modification.
master
Holger Rapp 2017-05-08 15:07:14 +02:00 committed by GitHub
parent e4de18cc65
commit b6fa20c9d5
15 changed files with 103 additions and 61 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

74
cartographer_ros/cartographer_ros/assets_writer.cc
View File

@ -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<carto::io::StreamFileWriter>(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

17
cartographer_ros/cartographer_ros/assets_writer.h
View File

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

30
cartographer_ros/cartographer_ros/map_builder_bridge.cc
View File

@ -32,8 +32,8 @@ MapBuilderBridge::MapBuilderBridge(const NodeOptions& options,
int MapBuilderBridge::AddTrajectory(
const std::unordered_set<string>& 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<nav_msgs::OccupancyGrid>
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<nav_msgs::OccupancyGrid> occupancy_grid;
if (!trajectory_nodes.empty()) {
occupancy_grid =
cartographer::common::make_unique<nav_msgs::OccupancyGrid>();
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;
}

1
cartographer_ros/cartographer_ros/map_builder_bridge.h
View File

@ -22,6 +22,7 @@
#include <unordered_set>
#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"

2
cartographer_ros/cartographer_ros/node_main.cc
View File

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

3
cartographer_ros/cartographer_ros/node_options.cc
View File

@ -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");

2
cartographer_ros/cartographer_ros/node_options.h
View File

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

15
cartographer_ros/cartographer_ros/occupancy_grid.cc
View File

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

8
cartographer_ros/cartographer_ros/occupancy_grid.h
View File

@ -20,15 +20,17 @@
#include <vector>
#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

2
cartographer_ros/cartographer_ros/offline_node_main.cc
View File

@ -140,7 +140,7 @@ void Run(const std::vector<string>& 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);
}

2
cartographer_ros/configuration_files/backpack_2d.lua
View File

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

2
cartographer_ros/configuration_files/backpack_3d.lua
View File

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

2
cartographer_ros/configuration_files/pr2.lua
View File

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

2
cartographer_ros/configuration_files/revo_lds.lua
View File

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

2
cartographer_ros/configuration_files/taurob_tracker.lua
View File

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