Skip to content

Commit

Permalink
Change_map service to map_server [Rebase/Noetic] (ros-planning#1029)
Browse files Browse the repository at this point in the history 
* Refactored map loading from constructor to three methods
* Added change_map service using LoadMap.srv
  • Loading branch information
@DLu @MatthijsBurgh
DLu authored and MatthijsBurgh committed Aug 10, 2021
1 parent e1d5152 commit d58fb04
Show file tree
Hide file tree
Showing 8 changed files with 270 additions and 120 deletions.
2 changes: 2 additions & 0 deletions map_server/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -93,12 +93,14 @@ if(CATKIN_ENABLE_TESTING)
${SDL_IMAGE_LIBRARIES}
)

find_package(roslib REQUIRED)
add_executable(rtest test/rtest.cpp test/test_constants.cpp)
add_dependencies(rtest ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
add_dependencies(tests rtest)
target_link_libraries( rtest
${GTEST_LIBRARIES}
${catkin_LIBRARIES}
${roslib_LIBRARIES}
)

# This has to be done after we've already built targets, or catkin variables get borked
Expand Down
1 change: 1 addition & 0 deletions map_server/package.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,6 +26,7 @@
<depend>tf2</depend>
<depend>yaml-cpp</depend>

<test_depend>roslib</test_depend>
<test_depend>rospy</test_depend>
<test_depend>rostest</test_depend>
<test_depend>rosunit</test_depend>
Expand Down
301 changes: 181 additions & 120 deletions map_server/src/main.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,6 +44,7 @@
#include "ros/console.h"
#include "map_server/image_loader.h"
#include "nav_msgs/MapMetaData.h"
#include "nav_msgs/LoadMap.h"
#include "yaml-cpp/yaml.h"

#ifdef HAVE_YAMLCPP_GT_0_5_0
Expand All @@ -67,158 +68,218 @@ class MapServer
int negate;
double occ_th, free_th;
MapMode mode = TRINARY;
std::string frame_id;
ros::NodeHandle private_nh("~");
private_nh.param("frame_id", frame_id, std::string("map"));
private_nh.param("frame_id", frame_id_, std::string("map"));

//When called this service returns a copy of the current map
get_map_service_ = nh_.advertiseService("static_map", &MapServer::mapCallback, this);

//Change the currently published map
change_map_srv_ = nh_.advertiseService("change_map", &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);

deprecated_ = (res != 0);
if (!deprecated_) {
//mapfname = fname + ".pgm";
//std::ifstream fin((fname + ".yaml").c_str());
std::ifstream fin(fname.c_str());
if (fin.fail()) {
ROS_ERROR("Map_server could not open %s.", fname.c_str());
exit(-1);
}
#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.");
exit(-1);
}
try {
doc["negate"] >> negate;
} catch (YAML::InvalidScalar &) {
ROS_ERROR("The map does not contain a negate tag or it is invalid.");
if (!loadMapFromYaml(fname))
{
exit(-1);
}
try {
doc["occupied_thresh"] >> occ_th;
} catch (YAML::InvalidScalar &) {
ROS_ERROR("The map does not contain an occupied_thresh tag or it is invalid.");
exit(-1);
}
try {
doc["free_thresh"] >> free_th;
} catch (YAML::InvalidScalar &) {
ROS_ERROR("The map does not contain a free_thresh tag or it is invalid.");
} else {
if (!loadMapFromParams(fname, res))
{
exit(-1);
}
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());
exit(-1);
}
} 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.");
exit(-1);
}
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.");
exit(-1);
}
private:
ros::NodeHandle nh_;
ros::Publisher map_pub_;
ros::Publisher metadata_pub_;
ros::ServiceServer get_map_service_;
ros::ServiceServer change_map_srv_;
bool deprecated_;
std::string frame_id_;

boost::filesystem::path mapfpath(mapfname);
if (!mapfpath.is_absolute())
{
boost::filesystem::path dir(fname);
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.");
exit(-1);
}
} else {
private_nh.param("negate", negate, 0);
private_nh.param("occupied_thresh", occ_th, 0.65);
private_nh.param("free_thresh", free_th, 0.196);
mapfname = fname;
origin[0] = origin[1] = origin[2] = 0.0;
}
/** Callback invoked when someone requests our service */
bool 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;
}

ROS_INFO("Loading map from image \"%s\"", mapfname.c_str());
try
/** Callback invoked when someone requests to change the map */
bool changeMapCallback(nav_msgs::LoadMap::Request &request,
nav_msgs::LoadMap::Response &response )
{
if (loadMapFromYaml(request.map_url))
{
map_server::loadMapFromFile(&map_resp_,mapfname.c_str(),res,negate,occ_th,free_th, origin, mode);
response.result = response.RESULT_SUCCESS;
ROS_INFO("Changed map to %s", request.map_url.c_str());
}
catch (std::runtime_error e)
else
{
ROS_ERROR("%s", e.what());
exit(-1);
response.result = response.RESULT_UNDEFINED_FAILURE;
}
return true;
}

/** Load a map given all the values needed to understand it
*/
bool 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 image \"%s\"", map_file_name.c_str());
try {
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_DEBUG("Waiting for valid time (make sure use_sime_time is false or a clock server (e.g., gazebo) is running)");
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.frame_id = frame_id_;
map_resp_.map.header.stamp = ros::Time::now();
ROS_DEBUG("Got time");
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;

get_map_service_ = nh_.advertiseService("static_map", &MapServer::mapCallback, this);
//pub = nh_.advertise<nav_msgs::MapMetaData>("map_metadata", 1,

// Latched publisher for metadata
metadata_pub_= nh_.advertise<nav_msgs::MapMetaData>("map_metadata", 1, true);
//Publish latched topics
metadata_pub_.publish( meta_data_message_ );

// Latched publisher for data
map_pub_ = nh_.advertise<nav_msgs::OccupancyGrid>("map", 1, true);
map_pub_.publish( map_resp_.map );
return true;
}

private:
ros::NodeHandle nh_;
ros::Publisher map_pub_;
ros::Publisher metadata_pub_;
ros::ServiceServer get_map_service_;
bool deprecated_;
/** Load a map using the deprecated method
*/
bool 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);
}

/** Callback invoked when someone requests our service */
bool mapCallback(nav_msgs::GetMap::Request &req,
nav_msgs::GetMap::Response &res )
/** Load a map given a path to a yaml file
*/
bool loadMapFromYaml(std::string path_to_yaml)
{
// request is empty; we ignore it
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;

// = operator is overloaded to make deep copy (tricky!)
res = map_resp_;
ROS_INFO("Sending map");
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;
}

return true;
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);
}

/** The map data is cached here, to be sent out to service callers
Expand Down
Loading

0 comments on commit d58fb04

Please sign in to comment.