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

Track googlecartographer/cartographer#333. (#373)

master
Holger Rapp 2017-06-12 18:06:29 +02:00 committed by Wolfgang Hess
parent 5ffe91f19c
commit 8140946a34
3 changed files with 27 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
cartographer_ros/cartographer_ros/color.cc
View File

@ -27,7 +27,7 @@ namespace {
constexpr float kInitialHue = 0.69f;
constexpr float kSaturation = 0.85f;
constexpr float kValue = 0.77f;
constexpr float kGoldenRatioConjugate = (std::sqrt(5) - 1) / 2.f;
constexpr float kGoldenRatioConjugate = (std::sqrt(5.f) - 1.f) / 2.f;
::cartographer_ros::ColorRgb HsvToRgb(const float h, const float s,
const float v) {

36
cartographer_ros/cartographer_ros/map_builder_bridge.cc
View File

@ -71,18 +71,18 @@ void MapBuilderBridge::SerializeState(const std::string& stem) {
}
void MapBuilderBridge::WriteAssets(const string& stem) {
const auto trajectory_nodes =
const auto all_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()) {
CHECK_EQ(all_trajectory_nodes.size(), 1);
if (all_trajectory_nodes[0].empty()) {
LOG(WARNING) << "No data was collected and no assets will be written.";
} else {
LOG(INFO) << "Writing assets with stem '" << stem << "'...";
if (node_options_.map_builder_options.use_trajectory_builder_2d()) {
Write2DAssets(
trajectory_nodes[0], node_options_.map_frame,
all_trajectory_nodes[0], node_options_.map_frame,
trajectory_options_[0]
.trajectory_builder_options.trajectory_builder_2d_options()
.submaps_options(),
@ -91,7 +91,7 @@ void MapBuilderBridge::WriteAssets(const string& stem) {
if (node_options_.map_builder_options.use_trajectory_builder_3d()) {
Write3DAssets(
trajectory_nodes[0],
all_trajectory_nodes[0],
trajectory_options_[0]
.trajectory_builder_options.trajectory_builder_3d_options()
.submaps_options()
@ -140,7 +140,7 @@ cartographer_ros_msgs::SubmapList MapBuilderBridge::GetSubmapList() {
for (size_t submap_index = 0; submap_index != submap_transforms.size();
++submap_index) {
cartographer_ros_msgs::SubmapEntry submap_entry;
submap_entry.submap_version = submaps->Get(submap_index)->num_range_data;
submap_entry.submap_version = submaps->Get(submap_index)->num_range_data();
submap_entry.pose = ToGeometryMsgPose(submap_transforms[submap_index]);
trajectory_submap_list.submap.push_back(submap_entry);
}
@ -153,19 +153,20 @@ std::unique_ptr<nav_msgs::OccupancyGrid>
MapBuilderBridge::BuildOccupancyGrid() {
CHECK(node_options_.map_builder_options.use_trajectory_builder_2d())
<< "Publishing OccupancyGrids for 3D data is not yet supported";
std::vector<::cartographer::mapping::TrajectoryNode> trajectory_nodes;
for (const auto& single_trajectory :
std::vector<::cartographer::mapping::TrajectoryNode> flat_trajectory_nodes;
for (const auto& single_trajectory_nodes :
map_builder_.sparse_pose_graph()->GetTrajectoryNodes()) {
trajectory_nodes.insert(trajectory_nodes.end(), single_trajectory.begin(),
single_trajectory.end());
flat_trajectory_nodes.insert(flat_trajectory_nodes.end(),
single_trajectory_nodes.begin(),
single_trajectory_nodes.end());
}
std::unique_ptr<nav_msgs::OccupancyGrid> occupancy_grid;
if (!trajectory_nodes.empty()) {
if (!flat_trajectory_nodes.empty()) {
occupancy_grid =
cartographer::common::make_unique<nav_msgs::OccupancyGrid>();
CHECK_EQ(trajectory_options_.count(0), 1);
BuildOccupancyGrid2D(
trajectory_nodes, node_options_.map_frame,
flat_trajectory_nodes, node_options_.map_frame,
trajectory_options_[0]
.trajectory_builder_options.trajectory_builder_2d_options()
.submaps_options(),
@ -204,13 +205,13 @@ MapBuilderBridge::GetTrajectoryStates() {
visualization_msgs::MarkerArray MapBuilderBridge::GetTrajectoryNodesList() {
visualization_msgs::MarkerArray trajectory_nodes_list;
const auto trajectory_nodes =
const auto all_trajectory_nodes =
map_builder_.sparse_pose_graph()->GetTrajectoryNodes();
int marker_id = 0;
for (int trajectory_id = 0;
trajectory_id < static_cast<int>(trajectory_nodes.size());
trajectory_id < static_cast<int>(all_trajectory_nodes.size());
++trajectory_id) {
const auto& single_trajectory = trajectory_nodes[trajectory_id];
const auto& single_trajectory_nodes = all_trajectory_nodes[trajectory_id];
visualization_msgs::Marker marker;
marker.id = marker_id++;
marker.type = visualization_msgs::Marker::LINE_STRIP;
@ -223,7 +224,10 @@ visualization_msgs::MarkerArray MapBuilderBridge::GetTrajectoryNodesList() {
marker.color.a = 1.0;
marker.scale.x = kTrajectoryLineStripMarkerScale;
marker.pose.orientation.w = 1.0;
for (const auto& node : single_trajectory) {
for (const auto& node : single_trajectory_nodes) {
if (node.trimmed()) {
continue;
}
const ::geometry_msgs::Point node_point = ToGeometryMsgPoint(
(node.pose * node.constant_data->tracking_to_pose).translation());
marker.points.push_back(node_point);

6
cartographer_ros/cartographer_ros/occupancy_grid.cc
View File

@ -29,6 +29,9 @@ Eigen::AlignedBox2f ComputeMapBoundingBox(
trajectory_nodes) {
Eigen::AlignedBox2f bounding_box(Eigen::Vector2f::Zero());
for (const auto& node : trajectory_nodes) {
if (node.trimmed()) {
continue;
}
const auto& data = *node.constant_data;
::cartographer::sensor::RangeData range_data;
if (!data.range_data_3d.returns.empty()) {
@ -68,6 +71,9 @@ void BuildOccupancyGrid2D(
carto::mapping_2d::RangeDataInserter range_data_inserter(
submaps_options.range_data_inserter_options());
for (const auto& node : trajectory_nodes) {
if (node.trimmed()) {
continue;
}
CHECK(node.constant_data->range_data_3d.returns.empty()); // No 3D yet.
range_data_inserter.Insert(
carto::sensor::TransformRangeData(node.constant_data->range_data_2d,