Refactor to class - node - action server

map_server/CMakeLists.txt

@@ -5,7 +5,9 @@ find_package(catkin REQUIRED
         COMPONENTS
             roscpp
             tf
+            actionlib
             nav_msgs
+            move_base_msgs
         )
 
 find_package(Boost REQUIRED COMPONENTS system)
@@ -21,16 +23,24 @@ catkin_package(
         include
     LIBRARIES
         image_loader
+        map_generator
     CATKIN_DEPENDS
         roscpp
         tf
         nav_msgs
+        move_base_msgs
+        actionlib
 )
 
 include_directories( include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS} )
 add_library(image_loader src/image_loader.cpp)
 target_link_libraries(image_loader SDL SDL_image ${Boost_LIBRARIES})
 
+add_library(map_generator src/map_generator.cpp)
+target_link_libraries(map_generator
+    ${catkin_LIBRARIES}
+    )
+
 add_executable(map_server src/main.cpp)
 target_link_libraries(map_server
     image_loader
@@ -41,6 +51,14 @@ target_link_libraries(map_server
 add_executable(map_server-map_saver src/map_saver.cpp)
 set_target_properties(map_server-map_saver PROPERTIES OUTPUT_NAME map_saver)
 target_link_libraries(map_server-map_saver
+    map_generator
+    ${catkin_LIBRARIES}
+    )
+
+add_executable(map_server-service_based_map_saver src/service_based_map_saver.cpp)
+set_target_properties(map_server-service_based_map_saver PROPERTIES OUTPUT_NAME service_based_map_saver)
+target_link_libraries(map_server-service_based_map_saver
+    map_generator
     ${catkin_LIBRARIES}
     )
 

map_server/README.md

@@ -0,0 +1,52 @@
+# Usage
+
+## Backward compatible usage
+
+Run a map_saver node that subscribes to the /map topic, saves the map and exits.
+```
+rosrun map_server map_saver -f <map_name>
+```
+
+## Service based usage
+
+Run a service_based_map_saver node that is an action server for map saving.
+The action will subscribe to the /map topic,
+write the available map to a file with the name given in the action goal,
+and unsubscribe from the /map topic.
+```
+rosrun map_server service_based_map_saver
+```
+Call the action, e.g. by publishing to the goal topic:
+```
+rostopic pub /service_based_map_saver/goal move_base_msgs/SaveMapActionGoal "header:
+  seq: 0
+  stamp:
+    secs: 0
+    nsecs: 0
+  frame_id: ''
+goal_id:
+  stamp:
+    secs: 0
+    nsecs: 0
+  id: '1'
+goal:
+  map_name: 'test'"
+```
+Once a map is available, the map will be saved. Trigger by writing a map to the /map topic:
+```
+rostopic pub /map nav_msgs/OccupancyGrid "header:
+ seq: 0
+ stamp:
+   secs: 0
+   nsecs: 0
+ frame_id: ''
+info:
+ map_load_time: {secs: 0, nsecs: 0}
+ resolution: 0.0
+ width: 0
+ height: 0
+ origin:
+   position: {x: 0.0, y: 0.0, z: 0.0}
+   orientation: {x: 0.0, y: 0.0, z: 0.0, w: 0.0}
+data: [0]"
+```

map_server/include/map_server/map_generator.h

@@ -0,0 +1,63 @@
+/*
+ * Copyright (c) 2017 Intermodalics bvba
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the Willow Garage, Inc. nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+#ifndef MAP_GENERATOR_H
+#define MAP_GENERATOR_H
+
+/*
+ * Author: Dominick Vanthienen
+ */
+
+#include "ros/ros.h"
+#include "nav_msgs/GetMap.h"
+#include <actionlib/server/simple_action_server.h>
+#include <move_base_msgs/SaveMapAction.h>
+
+/**
+ * @brief Map generation node.
+ */
+class MapGenerator
+{
+ public:
+   MapGenerator(const std::string& default_map_name,
+                const std::string& action_name);
+   MapGenerator(const std::string& mapname);
+
+   void mapCallback(const nav_msgs::OccupancyGridConstPtr& map);
+   void saveMapActionCallback(const move_base_msgs::SaveMapGoalConstPtr &goal);
+   bool saved_map() {return saved_map_;}
+ private:
+   std::string mapname_;
+   std::string action_name_;
+   ros::Subscriber map_sub_;
+   bool saved_map_;
+   ros::NodeHandle n_;
+   actionlib::SimpleActionServer<move_base_msgs::SaveMapAction> as_;
+};
+
+#endif

map_server/package.xml

@@ -16,18 +16,22 @@
     <buildtool_depend version_gte="0.5.68">catkin</buildtool_depend>
 
     <build_depend>nav_msgs</build_depend>
+    <build_depend>move_base_msgs</build_depend>
     <build_depend>roscpp</build_depend>
     <build_depend>rostest</build_depend>
     <build_depend>sdl-image</build_depend>
     <build_depend>tf</build_depend>
     <build_depend>yaml-cpp</build_depend>
+    <build_depend>actionlib</build_depend>
 
     <run_depend>nav_msgs</run_depend>
+    <run_depend>move_base_msgs</run_depend>
     <run_depend>roscpp</run_depend>
     <run_depend>rostest</run_depend>
     <run_depend>sdl-image</run_depend>
     <run_depend>tf</run_depend>
     <run_depend>yaml-cpp</run_depend>
+    <run_depend>actionlib</run_depend>
 
     <test_depend>rospy</test_depend>
 </package>