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

Also write a YAML file for the final map. (#81) 

This allows the saved map to be served by map_server.
master
Wolfgang Hess 2016-10-05 15:30:25 +02:00 committed by GitHub
parent 6fd405c78d
commit 965d70fbee
5 changed files with 132 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
cartographer_ros/CMakeLists.txt
View File

@ -81,6 +81,8 @@ link_directories(${Boost_LIBRARY_DIRS})
add_executable(cartographer_node
src/cartographer_node_main.cc
src/map_writer.cc
src/map_writer.h
src/msg_conversion.cc
src/msg_conversion.h
src/node_options.cc
@ -100,7 +102,8 @@ target_link_libraries(cartographer_node
${CARTOGRAPHER_LIBRARIES}
${PCL_LIBRARIES}
${catkin_LIBRARIES}
gflags # TODO(whess): CMake-ify?
gflags
yaml-cpp
)
add_dependencies(cartographer_node
${catkin_EXPORTED_TARGETS}

1
cartographer_ros/package.xml
View File

@ -56,6 +56,7 @@
<depend>pcl_conversions</depend>
<depend>eigen_conversions</depend>
<depend>message_runtime</depend>
<depend>yaml-cpp</depend>
<test_depend>rosunit</test_depend>

31
cartographer_ros/src/cartographer_node_main.cc
View File

@ -15,7 +15,6 @@
*/
#include <chrono>
#include <fstream>
#include <map>
#include <queue>
#include <string>
@ -63,6 +62,7 @@
#include "tf2_ros/transform_broadcaster.h"
#include "tf2_ros/transform_listener.h"
#include "map_writer.h"
#include "msg_conversion.h"
#include "node_options.h"
#include "occupancy_grid.h"
@ -479,30 +479,11 @@ bool Node::HandleFinishTrajectory(
const auto trajectory_nodes =
map_builder_.sparse_pose_graph()->GetTrajectoryNodes();
if (!trajectory_nodes.empty()) {
std::string filename = request.stem + ".pgm";
LOG(INFO) << "Saving final map to '" << filename << "'...";
::nav_msgs::OccupancyGrid grid;
BuildOccupancyGrid(trajectory_nodes, options_, &grid);
std::ofstream output_file(filename, std::ios::out | std::ios::binary);
const std::string header =
"P5\n# Cartographer map; " + std::to_string(grid.info.resolution) +
" m/pixel\n" + std::to_string(grid.info.width) + " " +
std::to_string(grid.info.height) + "\n255\n";
output_file.write(header.data(), header.size());
for (size_t y = 0; y < grid.info.height; ++y) {
for (size_t x = 0; x < grid.info.width; ++x) {
size_t i = x + (grid.info.height - y - 1) * grid.info.width;
if (grid.data[i] >= 0 && grid.data[i] <= 100) {
output_file.put((100 - grid.data[i]) * 254 / 100);
} else {
constexpr uint8_t kUnknownValue = 205;
output_file.put(kUnknownValue);
}
}
}
output_file.close();
CHECK(output_file) << "Writing '" << filename << "' failed.";
// TODO(whess): Also write a yaml file similar to map_saver?
::nav_msgs::OccupancyGrid occupancy_grid;
BuildOccupancyGrid(trajectory_nodes, options_, &occupancy_grid);
WriteOccupancyGridToPgmAndYaml(occupancy_grid, request.stem);
} else {
LOG(WARNING) << "Map is empty and will not be saved.";
}
}
return true;

89
cartographer_ros/src/map_writer.cc Normal file
View File

@ -0,0 +1,89 @@
/*
* Copyright 2016 The Cartographer Authors
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#include "map_writer.h"
#include <fstream>
#include "glog/logging.h"
#include "yaml-cpp/yaml.h"
namespace cartographer_ros {
namespace {
void WriteOccupancyGridToPgm(const ::nav_msgs::OccupancyGrid& grid,
const std::string& filename) {
LOG(INFO) << "Saving map to '" << filename << "'...";
std::ofstream pgm_file(filename, std::ios::out | std::ios::binary);
const std::string header = "P5\n# Cartographer map; " +
std::to_string(grid.info.resolution) +
" m/pixel\n" + std::to_string(grid.info.width) +
" " + std::to_string(grid.info.height) + "\n255\n";
pgm_file.write(header.data(), header.size());
for (size_t y = 0; y < grid.info.height; ++y) {
for (size_t x = 0; x < grid.info.width; ++x) {
const size_t i = x + (grid.info.height - y - 1) * grid.info.width;
if (grid.data[i] >= 0 && grid.data[i] <= 100) {
pgm_file.put((100 - grid.data[i]) * 255 / 100);
} else {
// We choose a value between the free and occupied threshold.
constexpr uint8_t kUnknownValue = 128;
pgm_file.put(kUnknownValue);
}
}
}
pgm_file.close();
CHECK(pgm_file) << "Writing '" << filename << "' failed.";
}
void WriteOccupancyGridInfoToYaml(const ::nav_msgs::OccupancyGrid& grid,
const std::string& map_filename,
const std::string& yaml_filename) {
LOG(INFO) << "Saving map info to '" << yaml_filename << "'...";
std::ofstream yaml_file(yaml_filename, std::ios::out | std::ios::binary);
{
YAML::Emitter out(yaml_file);
out << YAML::BeginMap;
// TODO(whess): Use basename only?
out << YAML::Key << "image" << YAML::Value << map_filename;
out << YAML::Key << "resolution" << YAML::Value << grid.info.resolution;
// According to map_server documentation "many parts of the system currently
// ignore yaw" so it is good we use a zero value.
constexpr double kYawButMaybeIgnored = 0.;
out << YAML::Key << "origin" << YAML::Value << YAML::Flow << YAML::BeginSeq
<< grid.info.origin.position.x << grid.info.origin.position.y
<< kYawButMaybeIgnored << YAML::EndSeq;
out << YAML::Key << "occupied_thresh" << YAML::Value << 0.51;
out << YAML::Key << "free_thresh" << YAML::Value << 0.49;
out << YAML::Key << "negate" << YAML::Value << 0;
out << YAML::EndMap;
CHECK(out.good()) << out.GetLastError();
}
yaml_file.close();
CHECK(yaml_file) << "Writing '" << yaml_filename << "' failed.";
}
} // namespace
void WriteOccupancyGridToPgmAndYaml(
const ::nav_msgs::OccupancyGrid& occupancy_grid, const std::string& stem) {
const std::string pgm_filename = stem + ".pgm";
WriteOccupancyGridToPgm(occupancy_grid, pgm_filename);
WriteOccupancyGridInfoToYaml(occupancy_grid, pgm_filename, stem + ".yaml");
}
} // namespace cartographer_ros

32
cartographer_ros/src/map_writer.h Normal file
View File

@ -0,0 +1,32 @@
/*
* Copyright 2016 The Cartographer Authors
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#ifndef CARTOGRAPHER_ROS_MAP_WRITER_H_
#define CARTOGRAPHER_ROS_MAP_WRITER_H_
#include <string>
#include "nav_msgs/OccupancyGrid.h"
namespace cartographer_ros {
// Writes the given 'occupancy_grid' as 'stem'.pgm and 'stem'.yaml.
void WriteOccupancyGridToPgmAndYaml(
const ::nav_msgs::OccupancyGrid& occupancy_grid, const std::string& stem);
} // namespace cartographer_ros
#endif // CARTOGRAPHER_ROS_MAP_WRITER_H_