8 #include <boost/filesystem.hpp> 9 #include <cost_map_msgs/CostMap.h> 14 #include <opencv2/core/core.hpp> 15 #include <opencv2/imgproc/imgproc.hpp> 16 #include <opencv2/highgui/highgui.hpp> 18 #include <yaml-cpp/yaml.h> 20 #include "../../include/cost_map_ros/converter.hpp" 21 #include "../../include/cost_map_ros/image_bundles.hpp" 22 #include "../../include/cost_map_ros/utilities.hpp" 42 unsigned int number_of_cells_x, number_of_cells_y;
43 std::map<std::string, std::string> layers;
45 YAML::Node config = YAML::LoadFile(filename);
46 for (
const std::string& property : {
47 "frame_id",
"centre_x",
"centre_y",
48 "resolution",
"number_of_cells_x",
"number_of_cells_y"} ) {
49 if (!config[property]) {
50 throw std::logic_error(
"missing required value '" + property +
"'");
53 frame_id = config[
"frame_id"].as<std::string>();
54 resolution = config[
"resolution"].as<
float>();
55 number_of_cells_x = config[
"number_of_cells_x"].as<
unsigned int>();
56 number_of_cells_y = config[
"number_of_cells_y"].as<
unsigned int>();
57 centre << config[
"centre_x"].as<
double>(), config[
"centre_y"].as<double>();
58 if (config[
"layers"]) {
59 for (
unsigned int index = 0; index < config[
"layers"].size(); ++index ) {
60 const YAML::Node& layer = config[
"layers"][index];
61 if (!layer[
"layer_name"]) {
62 throw std::logic_error(
"missing required value 'layer_name'");
64 if (!layer[
"layer_data"]) {
65 throw std::logic_error(
"missing required value 'layer_data'");
67 layers.insert(std::pair<std::string, std::string>(
68 layer[
"layer_name"].as<std::string>(),
69 layer[
"layer_data"].as<std::string>()
73 }
catch(
const YAML::ParserException& e ) {
74 throw std::logic_error(
"failed to parse (bad) yaml '" + filename +
"'");
79 Length length(resolution*number_of_cells_x, resolution*number_of_cells_y);
86 for (
const auto& p : layers ) {
88 boost::filesystem::path parent_path = boost::filesystem::path(filename).parent_path();
89 const std::string& layer_name = p.first;
90 const std::string& image_relative_filename = p.second;
91 boost::filesystem::path image_absolute_filename = parent_path / image_relative_filename;
95 cv::Mat image = cv::imread(image_absolute_filename.string(), cv::IMREAD_UNCHANGED);
98 switch( image.channels() ) {
99 case 3 : encoding =
"bgr8";
break;
100 case 4 : encoding =
"bgra8";
break;
101 default : encoding =
"mono8";
break;
110 ros_image_msg->header.frame_id = frame_id;
144 node[
"number_of_cells_x"] = cost_map.
getSize()(0);
145 node[
"number_of_cells_y"] = cost_map.
getSize()(1);
147 for (
const std::string& layer_name : cost_map.
getLayers()) {
149 layer[
"layer_name"] = layer_name;
150 layer[
"layer_data"] = layer_name +
".png";
151 layers.push_back(layer);
153 node[
"layers"] = layers;
154 std::ofstream ofs(filename, std::ofstream::out);
160 for (
const std::string& layer : cost_map.
getLayers()) {
165 cv::Mat image(cost_map.
getSize().x(), cost_map.
getSize().y(), CV_8UC4);
166 for (
int i = 0; i < cost_map_storage.rows(); ++i) {
167 for (
int j = 0; j < cost_map_storage.cols(); ++j) {
168 cv::Vec4b& rgba = image.at<cv::Vec4b>(i, j);
173 rgba[0] = flipped_value;
174 rgba[1] = flipped_value;
175 rgba[2] = flipped_value;
179 boost::filesystem::path parent_path = boost::filesystem::path(filename).parent_path();
180 boost::filesystem::path image_absolute_filename = parent_path / (layer + std::string(
".png"));
181 cv::imwrite(image_absolute_filename.string(), image);
190 const std::string& image_bundle_location,
191 const std::string& topic_name)
196 std::string yaml_filename;
197 if ( boost::filesystem::exists(image_bundle_location)) {
198 yaml_filename = image_bundle_location;
203 cost_map = std::make_shared<CostMap>();
211 cost_map_msgs::CostMap map_message;
217 const std::string& topic_name,
218 const std::string& yaml_filename)
220 , yaml_filename(yaml_filename)
228 std::lock_guard<std::mutex> guard(
mutex_);
232 ROS_ERROR_STREAM(
"SaveImageBundle : failed to convert cost map msg -> cost map class");
void fromImageBundle(const std::string &filename, cost_map::CostMap &cost_map)
Initialises a adds a single layer from a yaml/image resource pair.
Eigen::Matrix< unsigned char, Eigen::Dynamic, Eigen::Dynamic > Matrix
const unsigned char NO_INFORMATION
void publish(const boost::shared_ptr< M > &message) const
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
const Size & getSize() const
double getResolution() const
ros::Subscriber subscriber_
void setFrameId(const std::string &frameId)
void toMessage(const cost_map::CostMap &cost_map, cost_map_msgs::CostMap &message)
const Matrix & get(const std::string &layer) const
void setGeometry(const Length &length, const double resolution, const Position &position=Position::Zero())
const std::string & getFrameId() const
std::string yaml_filename
SaveImageBundle(const std::string &topic_name, const std::string &yaml_filename="foo.yaml")
Load and publish from an image bundle.
bool addLayerFromROSImage(const sensor_msgs::Image &image, const std::string &layer_name, cost_map::CostMap &cost_map)
bool getPosition(const Index &index, Position &position) const
void _costmapCallback(const cost_map_msgs::CostMap &msg)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
#define ROS_INFO_STREAM(args)
grid_map::Position Position
TFSIMD_FORCE_INLINE tfScalar length(const Quaternion &q)
const std::vector< std::string > & getLayers() const
#define ROS_ERROR_STREAM(args)
LoadImageBundle(const std::string &image_bundle_location, const std::string &topic_name="cost_map")
Load and publish from an image bundle.
std::string resolveResourceName(const std::string &resource_name)
Use rospack to resolve a costmap given a ros package resource name.
void toImageBundle(const std::string &filename, const cost_map::CostMap &cost_map)
Dump a cost map to an image bundle set of files.
sensor_msgs::ImagePtr toImageMsg() const
bool fromMessage(const cost_map_msgs::CostMap &message, cost_map::CostMap &cost_map)