Change_map service to map_server [Rebase/Noetic]

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

map_server/package.xml

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

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