309 lines
8.0 KiB
C++
309 lines
8.0 KiB
C++
|
|
#include "map_server/map_server.h"
|
|
|
|
|
|
map_server::MapServer::MapServer(const std::string& fname, double res)
|
|
{
|
|
ros::NodeHandle private_nh("~");
|
|
this->initialize(private_nh, fname, res);
|
|
}
|
|
|
|
map_server::MapServer::MapServer(ros::NodeHandle nh, std::string name_sp, const std::string& fname, double res)
|
|
{
|
|
nh_ = ros::NodeHandle(name_sp);
|
|
this->initialize(nh, fname, res);
|
|
}
|
|
|
|
void map_server::MapServer::initialize(ros::NodeHandle nh, const std::string& fname, double res)
|
|
{
|
|
std::string mapfname = "";
|
|
double origin[3];
|
|
int negate;
|
|
double occ_th, free_th;
|
|
MapMode mode = TRINARY;
|
|
nh.param("frame_id", frame_id_, std::string("map"));
|
|
nh.param("type", type_, 8);
|
|
|
|
// When called this service returns a copy of the current map
|
|
get_map_service_ = nh_.advertiseService("static_map", &map_server::MapServer::mapCallback, this);
|
|
|
|
// Change the currently published map
|
|
change_map_srv_ = nh_.advertiseService("change_map", &map_server::MapServer::changeMapCallback, this);
|
|
|
|
// Latched publisher for metadata
|
|
metadata_pub_ = nh_.advertise<nav_msgs::MapMetaData>("map_metadata", 1, true);
|
|
|
|
// Latched publisher for data
|
|
map_pub_ = nh_.advertise<nav_msgs::OccupancyGrid>("map", 1, true);
|
|
if (type_ != 8) map_convert_ = nh_.advertise<nav_msgs::OccupancyGrid>("map_convert", 1, true);
|
|
|
|
deprecated_ = (res != 0);
|
|
if (!deprecated_)
|
|
{
|
|
if (!loadMapFromYaml(fname))
|
|
{
|
|
exit(-1);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if (!loadMapFromParams(fname, res))
|
|
{
|
|
exit(-1);
|
|
}
|
|
}
|
|
}
|
|
|
|
bool map_server::MapServer::mapCallback(nav_msgs::GetMap::Request& req, nav_msgs::GetMap::Response& res)
|
|
{
|
|
// request is empty; we ignore it
|
|
|
|
// = operator is overloaded to make deep copy (tricky!)
|
|
res = map_resp_;
|
|
ROS_INFO("Sending map");
|
|
|
|
return true;
|
|
}
|
|
|
|
bool map_server::MapServer::changeMapCallback(nav_msgs::LoadMap::Request& request,
|
|
nav_msgs::LoadMap::Response& response)
|
|
{
|
|
if (loadMapFromYaml(request.map_url))
|
|
{
|
|
response.result = response.RESULT_SUCCESS;
|
|
ROS_INFO("Changed map to %s", request.map_url.c_str());
|
|
}
|
|
else
|
|
{
|
|
response.result = response.RESULT_UNDEFINED_FAILURE;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
|
|
bool map_server::MapServer::loadMapFromValues(std::string map_file_name, double resolution,
|
|
int negate, double occ_th, double free_th,
|
|
double origin[3], MapMode mode)
|
|
{
|
|
ROS_INFO("Loading map from images \"%s\"", map_file_name.c_str());
|
|
try
|
|
{
|
|
if (type_ == 32)
|
|
map_server::loadMapFromFile32Bit(&map_resp_, map_file_name.c_str(),
|
|
resolution, negate, occ_th, free_th,
|
|
origin, mode);
|
|
else if (type_ == 16)
|
|
map_server::loadMapFromFile16Bit(&map_resp_, map_file_name.c_str(),
|
|
resolution, negate, occ_th, free_th,
|
|
origin, mode);
|
|
else
|
|
map_server::loadMapFromFile(&map_resp_, map_file_name.c_str(),
|
|
resolution, negate, occ_th, free_th,
|
|
origin, mode);
|
|
}
|
|
catch (std::runtime_error& e)
|
|
{
|
|
ROS_ERROR("%s", e.what());
|
|
return false;
|
|
}
|
|
|
|
// To make sure get a consistent time in simulation
|
|
ros::Time::waitForValid();
|
|
map_resp_.map.info.map_load_time = ros::Time::now();
|
|
map_resp_.map.header.frame_id = frame_id_;
|
|
map_resp_.map.header.stamp = ros::Time::now();
|
|
ROS_INFO("Read a %d X %d map @ %.3lf m/cell",
|
|
map_resp_.map.info.width,
|
|
map_resp_.map.info.height,
|
|
map_resp_.map.info.resolution);
|
|
meta_data_message_ = map_resp_.map.info;
|
|
|
|
// Publish latched topics
|
|
metadata_pub_.publish(meta_data_message_);
|
|
map_pub_.publish(map_resp_.map);
|
|
|
|
if (type_ == 16)
|
|
{
|
|
double occ_th = 0.65;
|
|
double free_th = 0.196;
|
|
nav_msgs::OccupancyGrid convert;
|
|
convert.header = map_resp_.map.header;
|
|
convert.info = map_resp_.map.info;
|
|
convert.info.width = map_resp_.map.info.width / 2;
|
|
convert.data.resize(convert.info.width * convert.info.height);
|
|
unsigned int color_sum, color;
|
|
double color_avg;
|
|
unsigned char value;
|
|
int size_y = map_resp_.map.info.height;
|
|
int size_x = map_resp_.map.info.width;
|
|
|
|
for (int j = 0; j < map_resp_.map.info.height; j++)
|
|
{
|
|
for (int i = 0; i < map_resp_.map.info.width; i += 2)
|
|
{
|
|
unsigned char low = map_resp_.map.data[MAP_IDX(size_x, i, size_y - j - 1)];
|
|
unsigned char high = map_resp_.map.data[MAP_IDX(size_x, i + 1, size_y - j - 1)];
|
|
color = (int)((low & 0x00FF) | (high << 8));
|
|
unsigned char red = (unsigned char)(color & 0x00FF0000) >> 16;
|
|
unsigned char green = (unsigned char)(color & 0x0000FF00) >> 8;
|
|
unsigned char blue = (unsigned char)(color & 0x000000FF);
|
|
color_sum = red + green + blue;
|
|
|
|
color_avg = color_sum / 2.0;
|
|
|
|
double occ = (255 - color_avg) / 255.0;
|
|
if (occ > occ_th)
|
|
value = +100;
|
|
else if (occ < free_th)
|
|
value = 0;
|
|
else
|
|
{
|
|
double ratio = (occ - free_th) / (occ_th - free_th);
|
|
value = 1 + 98 * ratio;
|
|
}
|
|
convert.data[MAP_IDX(size_x / 2, i / 2, size_y - j - 1)] = value;
|
|
}
|
|
}
|
|
map_convert_.publish(convert);
|
|
}
|
|
return true;
|
|
}
|
|
|
|
|
|
bool map_server::MapServer::loadMapFromParams(std::string map_file_name, double resolution)
|
|
{
|
|
ros::NodeHandle private_nh("~");
|
|
int negate;
|
|
double occ_th;
|
|
double free_th;
|
|
double origin[3];
|
|
private_nh.param("negate", negate, 0);
|
|
private_nh.param("occupied_thresh", occ_th, 0.65);
|
|
private_nh.param("free_thresh", free_th, 0.196);
|
|
origin[0] = origin[1] = origin[2] = 0.0;
|
|
return loadMapFromValues(map_file_name, resolution, negate, occ_th, free_th, origin, TRINARY);
|
|
}
|
|
|
|
|
|
bool map_server::MapServer::loadMapFromYaml(std::string path_to_yaml)
|
|
{
|
|
std::string mapfname;
|
|
MapMode mode;
|
|
double res;
|
|
int negate;
|
|
double occ_th;
|
|
double free_th;
|
|
double origin[3];
|
|
std::ifstream fin(path_to_yaml.c_str());
|
|
if (fin.fail())
|
|
{
|
|
ROS_ERROR("Map_server could not open %s.", path_to_yaml.c_str());
|
|
return false;
|
|
}
|
|
#ifdef HAVE_YAMLCPP_GT_0_5_0
|
|
// The document loading process changed in yaml-cpp 0.5.
|
|
YAML::Node doc = YAML::Load(fin);
|
|
#else
|
|
YAML::Parser parser(fin);
|
|
YAML::Node doc;
|
|
parser.GetNextDocument(doc);
|
|
#endif
|
|
try
|
|
{
|
|
doc["resolution"] >> res;
|
|
}
|
|
catch (YAML::InvalidScalar&)
|
|
{
|
|
ROS_ERROR("The map does not contain a resolution tag or it is invalid.");
|
|
return false;
|
|
}
|
|
try
|
|
{
|
|
doc["negate"] >> negate;
|
|
}
|
|
catch (YAML::InvalidScalar&)
|
|
{
|
|
ROS_ERROR("The map does not contain a negate tag or it is invalid.");
|
|
return false;
|
|
}
|
|
try
|
|
{
|
|
doc["occupied_thresh"] >> occ_th;
|
|
}
|
|
catch (YAML::InvalidScalar&)
|
|
{
|
|
ROS_ERROR("The map does not contain an occupied_thresh tag or it is invalid.");
|
|
return false;
|
|
}
|
|
try
|
|
{
|
|
doc["free_thresh"] >> free_th;
|
|
}
|
|
catch (YAML::InvalidScalar&)
|
|
{
|
|
ROS_ERROR("The map does not contain a free_thresh tag or it is invalid.");
|
|
return false;
|
|
}
|
|
try
|
|
{
|
|
std::string modeS = "";
|
|
doc["mode"] >> modeS;
|
|
|
|
if (modeS == "trinary")
|
|
mode = TRINARY;
|
|
else if (modeS == "scale")
|
|
mode = SCALE;
|
|
else if (modeS == "raw")
|
|
mode = RAW;
|
|
else
|
|
{
|
|
ROS_ERROR("Invalid mode tag \"%s\".", modeS.c_str());
|
|
return false;
|
|
}
|
|
}
|
|
catch (YAML::Exception&)
|
|
{
|
|
ROS_DEBUG("The map does not contain a mode tag or it is invalid... assuming Trinary");
|
|
mode = TRINARY;
|
|
}
|
|
try
|
|
{
|
|
doc["origin"][0] >> origin[0];
|
|
doc["origin"][1] >> origin[1];
|
|
doc["origin"][2] >> origin[2];
|
|
}
|
|
catch (YAML::InvalidScalar&)
|
|
{
|
|
ROS_ERROR("The map does not contain an origin tag or it is invalid.");
|
|
return false;
|
|
}
|
|
try
|
|
{
|
|
doc["image"] >> mapfname;
|
|
// TODO: make this path-handling more robust
|
|
if (mapfname.size() == 0)
|
|
{
|
|
ROS_ERROR("The image tag cannot be an empty string.");
|
|
return false;
|
|
}
|
|
|
|
boost::filesystem::path mapfpath(mapfname);
|
|
if (!mapfpath.is_absolute())
|
|
{
|
|
boost::filesystem::path dir(path_to_yaml);
|
|
dir = dir.parent_path();
|
|
mapfpath = dir / mapfpath;
|
|
mapfname = mapfpath.string();
|
|
}
|
|
}
|
|
catch (YAML::InvalidScalar&)
|
|
{
|
|
ROS_ERROR("The map does not contain an image tag or it is invalid.");
|
|
return false;
|
|
}
|
|
return loadMapFromValues(mapfname, res, negate, occ_th, free_th, origin, mode);
|
|
}
|
|
|
|
|
|
|