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

Pull out WritePgm and WriteYaml. (#603)

master
Holger Rapp 2017-11-17 09:36:34 +01:00 committed by Wally B. Feed
parent 10eba093cc
commit 50be3d91a4
3 changed files with 91 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

49
cartographer_ros/cartographer_ros/ros_map.cc Normal file
View File

@ -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

41
cartographer_ros/cartographer_ros/ros_map.h Normal file
View File

@ -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 <string>
#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_ */

33
cartographer_ros/cartographer_ros/ros_map_writing_points_processor.cc
View File

@ -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&