Also write a YAML file for the final map. (#81)
This allows the saved map to be served by map_server.master
parent
6fd405c78d
commit
965d70fbee
|
@ -81,6 +81,8 @@ link_directories(${Boost_LIBRARY_DIRS})
|
||||||
|
|
||||||
add_executable(cartographer_node
|
add_executable(cartographer_node
|
||||||
src/cartographer_node_main.cc
|
src/cartographer_node_main.cc
|
||||||
|
src/map_writer.cc
|
||||||
|
src/map_writer.h
|
||||||
src/msg_conversion.cc
|
src/msg_conversion.cc
|
||||||
src/msg_conversion.h
|
src/msg_conversion.h
|
||||||
src/node_options.cc
|
src/node_options.cc
|
||||||
|
@ -100,7 +102,8 @@ target_link_libraries(cartographer_node
|
||||||
${CARTOGRAPHER_LIBRARIES}
|
${CARTOGRAPHER_LIBRARIES}
|
||||||
${PCL_LIBRARIES}
|
${PCL_LIBRARIES}
|
||||||
${catkin_LIBRARIES}
|
${catkin_LIBRARIES}
|
||||||
gflags # TODO(whess): CMake-ify?
|
gflags
|
||||||
|
yaml-cpp
|
||||||
)
|
)
|
||||||
add_dependencies(cartographer_node
|
add_dependencies(cartographer_node
|
||||||
${catkin_EXPORTED_TARGETS}
|
${catkin_EXPORTED_TARGETS}
|
||||||
|
|
|
@ -56,6 +56,7 @@
|
||||||
<depend>pcl_conversions</depend>
|
<depend>pcl_conversions</depend>
|
||||||
<depend>eigen_conversions</depend>
|
<depend>eigen_conversions</depend>
|
||||||
<depend>message_runtime</depend>
|
<depend>message_runtime</depend>
|
||||||
|
<depend>yaml-cpp</depend>
|
||||||
|
|
||||||
<test_depend>rosunit</test_depend>
|
<test_depend>rosunit</test_depend>
|
||||||
|
|
||||||
|
|
|
@ -15,7 +15,6 @@
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#include <chrono>
|
#include <chrono>
|
||||||
#include <fstream>
|
|
||||||
#include <map>
|
#include <map>
|
||||||
#include <queue>
|
#include <queue>
|
||||||
#include <string>
|
#include <string>
|
||||||
|
@ -63,6 +62,7 @@
|
||||||
#include "tf2_ros/transform_broadcaster.h"
|
#include "tf2_ros/transform_broadcaster.h"
|
||||||
#include "tf2_ros/transform_listener.h"
|
#include "tf2_ros/transform_listener.h"
|
||||||
|
|
||||||
|
#include "map_writer.h"
|
||||||
#include "msg_conversion.h"
|
#include "msg_conversion.h"
|
||||||
#include "node_options.h"
|
#include "node_options.h"
|
||||||
#include "occupancy_grid.h"
|
#include "occupancy_grid.h"
|
||||||
|
@ -479,30 +479,11 @@ bool Node::HandleFinishTrajectory(
|
||||||
const auto trajectory_nodes =
|
const auto trajectory_nodes =
|
||||||
map_builder_.sparse_pose_graph()->GetTrajectoryNodes();
|
map_builder_.sparse_pose_graph()->GetTrajectoryNodes();
|
||||||
if (!trajectory_nodes.empty()) {
|
if (!trajectory_nodes.empty()) {
|
||||||
std::string filename = request.stem + ".pgm";
|
::nav_msgs::OccupancyGrid occupancy_grid;
|
||||||
LOG(INFO) << "Saving final map to '" << filename << "'...";
|
BuildOccupancyGrid(trajectory_nodes, options_, &occupancy_grid);
|
||||||
::nav_msgs::OccupancyGrid grid;
|
WriteOccupancyGridToPgmAndYaml(occupancy_grid, request.stem);
|
||||||
BuildOccupancyGrid(trajectory_nodes, options_, &grid);
|
} else {
|
||||||
std::ofstream output_file(filename, std::ios::out | std::ios::binary);
|
LOG(WARNING) << "Map is empty and will not be saved.";
|
||||||
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?
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
return true;
|
return true;
|
||||||
|
|
|
@ -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
|
|
@ -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_
|
Loading…
Reference in New Issue