cartographer_ros: ros_map_writing_points

Go to the documentation of this file.
00001 /*
00002  * Copyright 2016 The Cartographer Authors
00003  *
00004  * Licensed under the Apache License, Version 2.0 (the "License");
00005  * you may not use this file except in compliance with the License.
00006  * You may obtain a copy of the License at
00007  *
00008  *      http://www.apache.org/licenses/LICENSE-2.0
00009  *
00010  * Unless required by applicable law or agreed to in writing, software
00011  * distributed under the License is distributed on an "AS IS" BASIS,
00012  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00013  * See the License for the specific language governing permissions and
00014  * limitations under the License.
00015  */
00016 
00017 #include "cartographer_ros/ros_map_writing_points_processor.h"
00018 
00019 #include "absl/memory/memory.h"
00020 #include "cartographer/io/image.h"
00021 #include "cartographer/io/probability_grid_points_processor.h"
00022 #include "cartographer_ros/ros_map.h"
00023 
00024 namespace cartographer_ros {
00025 
00026 RosMapWritingPointsProcessor::RosMapWritingPointsProcessor(
00027     const double resolution,
00028     const ::cartographer::mapping::proto::
00029         ProbabilityGridRangeDataInserterOptions2D& range_data_inserter_options,
00030     ::cartographer::io::FileWriterFactory file_writer_factory,
00031     const std::string& filestem,
00032     ::cartographer::io::PointsProcessor* const next)
00033     : filestem_(filestem),
00034       next_(next),
00035       file_writer_factory_(file_writer_factory),
00036       range_data_inserter_(range_data_inserter_options),
00037       probability_grid_(::cartographer::io::CreateProbabilityGrid(
00038           resolution, &conversion_tables_)) {}
00039 
00040 std::unique_ptr<RosMapWritingPointsProcessor>
00041 RosMapWritingPointsProcessor::FromDictionary(
00042     ::cartographer::io::FileWriterFactory file_writer_factory,
00043     ::cartographer::common::LuaParameterDictionary* const dictionary,
00044     ::cartographer::io::PointsProcessor* const next) {
00045   return absl::make_unique<RosMapWritingPointsProcessor>(
00046       dictionary->GetDouble("resolution"),
00047       ::cartographer::mapping::CreateProbabilityGridRangeDataInserterOptions2D(
00048           dictionary->GetDictionary("range_data_inserter").get()),
00049       file_writer_factory, dictionary->GetString("filestem"), next);
00050 }
00051 
00052 void RosMapWritingPointsProcessor::Process(
00053     std::unique_ptr<::cartographer::io::PointsBatch> batch) {
00054   range_data_inserter_.Insert({batch->origin, batch->points, {}},
00055                               &probability_grid_);
00056   next_->Process(std::move(batch));
00057 }
00058 
00059 ::cartographer::io::PointsProcessor::FlushResult
00060 RosMapWritingPointsProcessor::Flush() {
00061   Eigen::Array2i offset;
00062   std::unique_ptr<::cartographer::io::Image> image =
00063       ::cartographer::io::DrawProbabilityGrid(probability_grid_, &offset);
00064   if (image != nullptr) {
00065     auto pgm_writer = file_writer_factory_(filestem_ + ".pgm");
00066     const std::string pgm_filename = pgm_writer->GetFilename();
00067     const auto& limits = probability_grid_.limits();
00068     image->Rotate90DegreesClockwise();
00069 
00070     WritePgm(*image, limits.resolution(), pgm_writer.get());
00071     CHECK(pgm_writer->Close());
00072 
00073     const Eigen::Vector2d origin(
00074         limits.max().x() - (offset.y() + image->width()) * limits.resolution(),
00075         limits.max().y() -
00076             (offset.x() + image->height()) * limits.resolution());
00077     auto yaml_writer = file_writer_factory_(filestem_ + ".yaml");
00078     WriteYaml(limits.resolution(), origin, pgm_filename, yaml_writer.get());
00079     CHECK(yaml_writer->Close());
00080   }
00081 
00082   switch (next_->Flush()) {
00083     case FlushResult::kRestartStream:
00084       LOG(FATAL) << "ROS map writing must be configured to occur after any "
00085                     "stages that require multiple passes.";
00086 
00087     case FlushResult::kFinished:
00088       return FlushResult::kFinished;
00089   }
00090   LOG(FATAL);
00091   // The following unreachable return statement is needed to avoid a GCC bug
00092   // described at https://gcc.gnu.org/bugzilla/show_bug.cgi?id=81508
00093   return FlushResult::kFinished;
00094 }
00095 
00096 }  // namespace cartographer_ros