diff --git a/cartographer_ros/cartographer_ros/ros_map.cc b/cartographer_ros/cartographer_ros/ros_map.cc new file mode 100644 index 0000000..6dfc92f --- /dev/null +++ b/cartographer_ros/cartographer_ros/ros_map.cc @@ -0,0 +1,49 @@ +/* + * Copyright 2017 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 "cartographer_ros/ros_map.h" + +namespace cartographer_ros { + +void WritePgm(const ::cartographer::io::Image& image, const double resolution, + ::cartographer::io::FileWriter* file_writer) { + const std::string header = "P5\n# Cartographer map; " + + std::to_string(resolution) + " m/pixel\n" + + std::to_string(image.width()) + " " + + std::to_string(image.height()) + "\n255\n"; + file_writer->Write(header.data(), header.size()); + for (int y = 0; y < image.height(); ++y) { + for (int x = 0; x < image.width(); ++x) { + const char color = image.GetPixel(x, y)[0]; + file_writer->Write(&color, 1); + } + } +} + +void WriteYaml(const double resolution, const Eigen::Vector2d& origin, + const std::string& pgm_filename, + ::cartographer::io::FileWriter* file_writer) { + // Magic constants taken directly from ros map_saver code: + // https://github.com/ros-planning/navigation/blob/ac41d2480c4cf1602daf39a6e9629142731d92b0/map_server/src/map_saver.cpp#L114 + const std::string output = + "image: " + pgm_filename + "\n" + + "resolution: " + std::to_string(resolution) + "\n" + "origin: [" + + std::to_string(origin.x()) + ", " + std::to_string(origin.y()) + + ", 0.]\nnegate: 0\noccupied_thresh: 0.65\nfree_thresh: 0.196\n"; + file_writer->Write(output.data(), output.size()); +} + +} // namespace cartographer_ros diff --git a/cartographer_ros/cartographer_ros/ros_map.h b/cartographer_ros/cartographer_ros/ros_map.h new file mode 100644 index 0000000..64ad6d6 --- /dev/null +++ b/cartographer_ros/cartographer_ros/ros_map.h @@ -0,0 +1,41 @@ +/* + * Copyright 2017 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_ROS_MAP_H_ +#define CARTOGRAPHER_ROS_ROS_MAP_H_ + +#include + +#include "Eigen/Core" +#include "cartographer/io/file_writer.h" +#include "cartographer/io/image.h" +#include "cartographer/mapping_2d/map_limits.h" + +namespace cartographer_ros { + +// Write 'image' as a pgm into 'file_writer'. The resolution is used in the +// comment only' +void WritePgm(const ::cartographer::io::Image& image, const double resolution, + ::cartographer::io::FileWriter* file_writer); + +// Write the corresponding yaml into 'file_writer'. +void WriteYaml(const double resolution, const Eigen::Vector2d& origin, + const std::string& pgm_filename, + ::cartographer::io::FileWriter* file_writer); + +} // namespace cartographer_ros + +#endif /* end of include guard: CARTOGRAPHER_ROS_ROS_MAP_H_ */ diff --git a/cartographer_ros/cartographer_ros/ros_map_writing_points_processor.cc b/cartographer_ros/cartographer_ros/ros_map_writing_points_processor.cc index 72f6e41..eca6c61 100644 --- a/cartographer_ros/cartographer_ros/ros_map_writing_points_processor.cc +++ b/cartographer_ros/cartographer_ros/ros_map_writing_points_processor.cc @@ -19,41 +19,10 @@ #include "cartographer/common/make_unique.h" #include "cartographer/io/image.h" #include "cartographer/io/probability_grid_points_processor.h" +#include "cartographer_ros/ros_map.h" namespace cartographer_ros { -namespace { - -void WritePgm(const ::cartographer::io::Image& image, const double resolution, - ::cartographer::io::FileWriter* file_writer) { - const std::string header = "P5\n# Cartographer map; " + - std::to_string(resolution) + " m/pixel\n" + - std::to_string(image.width()) + " " + - std::to_string(image.height()) + "\n255\n"; - file_writer->Write(header.data(), header.size()); - for (int y = 0; y < image.height(); ++y) { - for (int x = 0; x < image.width(); ++x) { - const char color = image.GetPixel(x, y)[0]; - file_writer->Write(&color, 1); - } - } -} - -void WriteYaml(const double resolution, const Eigen::Vector2d& origin, - const std::string& pgm_filename, - ::cartographer::io::FileWriter* file_writer) { - // Magic constants taken directly from ros map_saver code: - // https://github.com/ros-planning/navigation/blob/ac41d2480c4cf1602daf39a6e9629142731d92b0/map_server/src/map_saver.cpp#L114 - const std::string output = - "image: " + pgm_filename + "\n" + - "resolution: " + std::to_string(resolution) + "\n" + "origin: [" + - std::to_string(origin.x()) + ", " + std::to_string(origin.y()) + - ", 0.]\nnegate: 0\noccupied_thresh: 0.65\nfree_thresh: 0.196\n"; - file_writer->Write(output.data(), output.size()); -} - -} // namespace - RosMapWritingPointsProcessor::RosMapWritingPointsProcessor( const double resolution, const ::cartographer::mapping_2d::proto::RangeDataInserterOptions&