diff --git a/cartographer_ros/cartographer_ros/CMakeLists.txt b/cartographer_ros/cartographer_ros/CMakeLists.txt index 04c0090..496fa4b 100644 --- a/cartographer_ros/cartographer_ros/CMakeLists.txt +++ b/cartographer_ros/cartographer_ros/CMakeLists.txt @@ -15,6 +15,8 @@ google_binary(cartographer_assets_writer SRCS assets_writer_main.cc + ros_map_writing_points_processor.h + ros_map_writing_points_processor.cc ) install(TARGETS cartographer_assets_writer diff --git a/cartographer_ros/cartographer_ros/assets_writer_main.cc b/cartographer_ros/cartographer_ros/assets_writer_main.cc index bc3cc1b..180780b 100644 --- a/cartographer_ros/cartographer_ros/assets_writer_main.cc +++ b/cartographer_ros/cartographer_ros/assets_writer_main.cc @@ -32,6 +32,7 @@ #include "cartographer/sensor/range_data.h" #include "cartographer/transform/transform_interpolation_buffer.h" #include "cartographer_ros/msg_conversion.h" +#include "cartographer_ros/ros_map_writing_points_processor.h" #include "cartographer_ros/split_string.h" #include "cartographer_ros/time_conversion.h" #include "cartographer_ros/urdf_reader.h" @@ -154,6 +155,16 @@ void Run(const string& pose_graph_filename, carto::io::PointsProcessorPipelineBuilder builder; carto::io::RegisterBuiltInPointsProcessors(all_trajectories, file_writer_factory, &builder); + builder.Register( + RosMapWritingPointsProcessor::kConfigurationFileActionName, + [file_writer_factory]( + ::cartographer::common::LuaParameterDictionary* const dictionary, + ::cartographer::io::PointsProcessor* const next) + -> std::unique_ptr<::cartographer::io::PointsProcessor> { + return RosMapWritingPointsProcessor::FromDictionary(file_writer_factory, + dictionary, next); + }); + std::vector> pipeline = builder.CreatePipeline( lua_parameter_dictionary.GetDictionary("pipeline").get()); diff --git a/cartographer_ros/cartographer_ros/ros_map_writing_points_processor.cc b/cartographer_ros/cartographer_ros/ros_map_writing_points_processor.cc new file mode 100644 index 0000000..4f416c0 --- /dev/null +++ b/cartographer_ros/cartographer_ros/ros_map_writing_points_processor.cc @@ -0,0 +1,124 @@ +/* + * 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 "cartographer_ros/ros_map_writing_points_processor.h" + +#include "cartographer/common/make_unique.h" +#include "cartographer/io/image.h" +#include "cartographer/io/probability_grid_points_processor.h" + +namespace cartographer_ros { + +namespace { + +void WritePgm(const ::cartographer::io::Image& image, const double resolution, + ::cartographer::io::FileWriter* file_writer) { + // Flipping the image into ROS coordinate frame. + const std::string header = "P5\n# Cartographer map; " + + std::to_string(resolution) + " m/pixel\n" + + std::to_string(image.height()) + " " + + std::to_string(image.width()) + "\n255\n"; + file_writer->Write(header.data(), header.size()); + for (int x = 0; x < image.width(); ++x) { + for (int y = image.height() - 1; y >= 0; --y) { + const char color = image.GetPixel(x, y)[0]; + file_writer->Write(&color, 1); + } + } +} + +void WriteYaml(const ::cartographer::io::Image& image, + const ::cartographer::mapping_2d::MapLimits& limits, + const string& pgm_filename, const Eigen::Array2i& offset, + ::cartographer::io::FileWriter* file_writer) { + const double resolution = limits.resolution(); + const double x_offset = + limits.max().x() - (offset.y() + image.height()) * resolution; + const double y_offset = + limits.max().y() - (offset.x() + image.width()) * resolution; + // 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(x_offset) + ", " + std::to_string(y_offset) + + ", 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& + range_data_inserter_options, + ::cartographer::io::FileWriterFactory file_writer_factory, + const string& filestem, ::cartographer::io::PointsProcessor* const next) + : filestem_(filestem), + next_(next), + file_writer_factory_(file_writer_factory), + range_data_inserter_(range_data_inserter_options), + probability_grid_(::cartographer::io::CreateProbabilityGrid(resolution)) { +} + +std::unique_ptr +RosMapWritingPointsProcessor::FromDictionary( + ::cartographer::io::FileWriterFactory file_writer_factory, + ::cartographer::common::LuaParameterDictionary* const dictionary, + ::cartographer::io::PointsProcessor* const next) { + return ::cartographer::common::make_unique( + dictionary->GetDouble("resolution"), + ::cartographer::mapping_2d::CreateRangeDataInserterOptions( + dictionary->GetDictionary("range_data_inserter").get()), + file_writer_factory, dictionary->GetString("filestem"), next); +} + +void RosMapWritingPointsProcessor::Process( + std::unique_ptr<::cartographer::io::PointsBatch> batch) { + range_data_inserter_.Insert({batch->origin, batch->points, {}}, + &probability_grid_); + next_->Process(std::move(batch)); +} + +::cartographer::io::PointsProcessor::FlushResult +RosMapWritingPointsProcessor::Flush() { + Eigen::Array2i offset; + std::unique_ptr<::cartographer::io::Image> image = + ::cartographer::io::DrawProbabilityGrid(probability_grid_, &offset); + if (image != nullptr) { + auto pgm_writer = file_writer_factory_(filestem_ + ".pgm"); + const std::string pgm_filename = pgm_writer->GetFilename(); + WritePgm(*image, probability_grid_.limits().resolution(), pgm_writer.get()); + CHECK(pgm_writer->Close()); + + auto yaml_writer = file_writer_factory_(filestem_ + ".yaml"); + WriteYaml(*image, probability_grid_.limits(), pgm_filename, offset, + yaml_writer.get()); + CHECK(yaml_writer->Close()); + } + + switch (next_->Flush()) { + case FlushResult::kRestartStream: + LOG(FATAL) << "ROS map writing must be configured to occur after any " + "stages that require multiple passes."; + + case FlushResult::kFinished: + return FlushResult::kFinished; + } + LOG(FATAL); +} + +} // namespace cartographer_ros diff --git a/cartographer_ros/cartographer_ros/ros_map_writing_points_processor.h b/cartographer_ros/cartographer_ros/ros_map_writing_points_processor.h new file mode 100644 index 0000000..1dabc0c --- /dev/null +++ b/cartographer_ros/cartographer_ros/ros_map_writing_points_processor.h @@ -0,0 +1,64 @@ +/* + * 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_ROS_MAP_WRITING_POINTS_PROCESSOR_H_ +#define CARTOGRAPHER_ROS_ROS_MAP_WRITING_POINTS_PROCESSOR_H_ + +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/io/file_writer.h" +#include "cartographer/io/points_processor.h" +#include "cartographer/mapping_2d/proto/range_data_inserter_options.pb.h" +#include "cartographer/mapping_2d/range_data_inserter.h" + +namespace cartographer_ros { + +// Very similar to Cartographer's ProbabilityGridPointsProcessor, but writes +// out a PGM and YAML suitable for ROS map server to consume. +class RosMapWritingPointsProcessor + : public ::cartographer::io::PointsProcessor { + public: + constexpr static const char* kConfigurationFileActionName = "write_ros_map"; + RosMapWritingPointsProcessor( + double resolution, + const ::cartographer::mapping_2d::proto::RangeDataInserterOptions& + range_data_inserter_options, + ::cartographer::io::FileWriterFactory file_writer_factory, + const string& filestem, PointsProcessor* next); + RosMapWritingPointsProcessor(const RosMapWritingPointsProcessor&) = delete; + RosMapWritingPointsProcessor& operator=(const RosMapWritingPointsProcessor&) = + delete; + + static std::unique_ptr FromDictionary( + ::cartographer::io::FileWriterFactory file_writer_factory, + ::cartographer::common::LuaParameterDictionary* dictionary, + PointsProcessor* next); + + ~RosMapWritingPointsProcessor() override {} + + void Process(std::unique_ptr<::cartographer::io::PointsBatch> batch) override; + FlushResult Flush() override; + + private: + const string filestem_; + PointsProcessor* const next_; + ::cartographer::io::FileWriterFactory file_writer_factory_; + ::cartographer::mapping_2d::RangeDataInserter range_data_inserter_; + ::cartographer::mapping_2d::ProbabilityGrid probability_grid_; +}; + +} // namespace cartographer_ros + +#endif // CARTOGRAPHER_ROS_ROS_MAP_WRITING_POINTS_PROCESSOR_H_