diff --git a/map_server/CMakeLists.txt b/map_server/CMakeLists.txt
index b608544389..37a67cc360 100644
--- a/map_server/CMakeLists.txt
+++ b/map_server/CMakeLists.txt
@@ -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
diff --git a/map_server/package.xml b/map_server/package.xml
index 36950ee40e..ecb93b61b7 100644
--- a/map_server/package.xml
+++ b/map_server/package.xml
@@ -26,6 +26,7 @@
tf2
yaml-cpp
+ roslib
rospy
rostest
rosunit
diff --git a/map_server/src/main.cpp b/map_server/src/main.cpp
index d401ff7b30..043fb3588c 100644
--- a/map_server/src/main.cpp
+++ b/map_server/src/main.cpp
@@ -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
@@ -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("map_metadata", 1, true);
+
+ // Latched publisher for data
+ map_pub_ = nh_.advertise("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("map_metadata", 1,
-
- // Latched publisher for metadata
- metadata_pub_= nh_.advertise("map_metadata", 1, true);
+ //Publish latched topics
metadata_pub_.publish( meta_data_message_ );
-
- // Latched publisher for data
- map_pub_ = nh_.advertise("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
diff --git a/map_server/test/rtest.cpp b/map_server/test/rtest.cpp
index cbb5173d82..5178952357 100644
--- a/map_server/test/rtest.cpp
+++ b/map_server/test/rtest.cpp
@@ -32,9 +32,11 @@
#include
#include
#include
+#include
#include
#include
#include
+#include
#include "test_constants.h"
@@ -135,6 +137,56 @@ TEST_F(MapClientTest, subscribe_topic_metadata)
ASSERT_EQ(map_metadata_->height, g_valid_image_height);
}
+/* Change the map, retrieve the map via topic, and compare to ground truth */
+TEST_F(MapClientTest, change_map)
+{
+ ros::Subscriber sub = n_->subscribe("map", 1, boost::bind(&MapClientTest::mapCallback, this, _1));
+
+ // Try a few times, because the server may not be up yet.
+ for (int i = 20; i > 0 && !got_map_; i--)
+ {
+ ros::spinOnce();
+ ros::Duration d = ros::Duration().fromSec(0.25);
+ d.sleep();
+ }
+ ASSERT_TRUE(got_map_);
+
+ // Now change the map
+ got_map_ = false;
+ nav_msgs::LoadMap::Request req;
+ nav_msgs::LoadMap::Response resp;
+ req.map_url = ros::package::getPath("map_server") + "/test/testmap2.yaml";
+ ASSERT_TRUE(ros::service::waitForService("change_map", 5000));
+ ASSERT_TRUE(ros::service::call("change_map", req, resp));
+ ASSERT_EQ(0u, resp.result);
+ for (int i = 20; i > 0 && !got_map_; i--)
+ {
+ ros::spinOnce();
+ ros::Duration d = ros::Duration().fromSec(0.25);
+ d.sleep();
+ }
+
+ ASSERT_FLOAT_EQ(tmap2::g_valid_image_res, map_->info.resolution);
+ ASSERT_EQ(tmap2::g_valid_image_width, map_->info.width);
+ ASSERT_EQ(tmap2::g_valid_image_height, map_->info.height);
+ ASSERT_STREQ("map", map_->header.frame_id.c_str());
+ for(unsigned int i=0; i < map_->info.width * map_->info.height; i++)
+ ASSERT_EQ(tmap2::g_valid_image_content[i], map_->data[i]) << "idx:" << i;
+
+ //Put the old map back so the next test isn't broken
+ got_map_ = false;
+ req.map_url = ros::package::getPath("map_server") + "/test/testmap.yaml";
+ ASSERT_TRUE(ros::service::call("change_map", req, resp));
+ ASSERT_EQ(0u, resp.result);
+ for (int i = 20; i > 0 && !got_map_; i--)
+ {
+ ros::spinOnce();
+ ros::Duration d = ros::Duration().fromSec(0.25);
+ d.sleep();
+ }
+ ASSERT_TRUE(got_map_);
+}
+
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
diff --git a/map_server/test/test_constants.cpp b/map_server/test/test_constants.cpp
index d2b3f242ea..4521521129 100644
--- a/map_server/test/test_constants.cpp
+++ b/map_server/test/test_constants.cpp
@@ -61,3 +61,24 @@ const char* g_spectrum_png_file = "test/spectrum.png";
const float g_valid_image_res = 0.1;
+// Constants for testmap2
+namespace tmap2
+{
+ const char g_valid_image_content[] = {
+ 100,100,100,100,100,100,100,100,100,100,100,100,
+ 100,0,0,0,0,0,0,0,0,0,0,100,
+ 100,0,100,100,100,100,100,100,100,100,0,100,
+ 100,0,100,0,0,0,0,0,0,100,0,100,
+ 100,0,0,0,0,0,0,0,0,0,0,100,
+ 100,0,0,0,0,0,0,0,0,0,0,100,
+ 100,0,0,0,0,0,0,0,0,0,0,100,
+ 100,0,0,0,0,0,0,0,0,0,0,100,
+ 100,0,0,0,0,0,0,0,0,0,0,100,
+ 100,0,100,0,0,0,0,0,0,100,0,100,
+ 100,0,0,0,0,0,0,0,0,0,0,100,
+ 100,100,100,100,100,100,100,100,100,100,100,100,
+ };
+ const float g_valid_image_res = 0.1;
+ const unsigned int g_valid_image_width = 12;
+ const unsigned int g_valid_image_height = 12;
+}
diff --git a/map_server/test/test_constants.h b/map_server/test/test_constants.h
index 90776d2752..c23a578cfe 100644
--- a/map_server/test/test_constants.h
+++ b/map_server/test/test_constants.h
@@ -41,4 +41,11 @@ extern const char* g_valid_bmp_file;
extern const float g_valid_image_res;
extern const char* g_spectrum_png_file;
+namespace tmap2 {
+ extern const char g_valid_image_content[];
+ extern const float g_valid_image_res;
+ extern const unsigned int g_valid_image_width;
+ extern const unsigned int g_valid_image_height;
+}
+
#endif
diff --git a/map_server/test/testmap2.png b/map_server/test/testmap2.png
new file mode 100644
index 0000000000..8ca910629d
Binary files /dev/null and b/map_server/test/testmap2.png differ
diff --git a/map_server/test/testmap2.yaml b/map_server/test/testmap2.yaml
new file mode 100644
index 0000000000..150a04a958
--- /dev/null
+++ b/map_server/test/testmap2.yaml
@@ -0,0 +1,6 @@
+image: testmap2.png
+resolution: 0.1
+origin: [-2.0, -3.0, -1.0]
+negate: 1
+occupied_thresh: 0.5
+free_thresh: 0.2