Added Service response structure for save map and serialize Posegraph (…

…SteveMacenski#421) * Added Service response structure for save map and serialize Posegraph * Updated Segmentation_fault to Undefined_failure condition * Updated result of serialize service * Updated code according to the project's style guide Co-authored-by: SumontreeP <[email protected]>
malban · Jul 16, 2021 · fd353d7 · fd353d7
1 parent 05dfd3b
commit fd353d7
Show file tree

Hide file tree

Showing 5 changed files with 32 additions and 4 deletions.
diff --git a/include/slam_toolbox/serialization.hpp b/include/slam_toolbox/serialization.hpp
@@ -34,7 +34,7 @@ inline bool fileExists(const std::string & name)
   return stat(name.c_str(), &buffer) == 0;
 }
 
-inline void write(
+inline bool write(
   const std::string & filename,
   karto::Mapper & mapper,
   karto::Dataset & dataset,
@@ -43,9 +43,11 @@ inline void write(
   try {
     mapper.SaveToFile(filename + std::string(".posegraph"));
     dataset.SaveToFile(filename + std::string(".data"));
+    return true;
   } catch (boost::archive::archive_exception e) {
     RCLCPP_ERROR(node->get_logger(),
       "Failed to write file: Exception %s", e.what());
+    return false;
   }
 }
 

diff --git a/src/map_saver.cpp b/src/map_saver.cpp
@@ -53,6 +53,7 @@ bool MapSaver::saveMapCallback(
     RCLCPP_WARN(node_->get_logger(),
       "Map Saver: Cannot save map, no map yet received on topic %s.",
       map_name_.c_str());
+    response->result = response->RESULT_NO_MAP_RECEIEVD;
     return false;
   }
 
@@ -61,10 +62,20 @@ bool MapSaver::saveMapCallback(
     RCLCPP_INFO(node_->get_logger(),
       "SlamToolbox: Saving map as %s.", name.c_str());
     int rc = system(("ros2 run nav2_map_server map_saver_cli -f " + name  + " --ros-args -p map_subscribe_transient_local:=true").c_str());
+    if (rc == 0) {
+      response->result = response->RESULT_SUCCESS;
+    } else {
+      response->result = response->RESULT_UNDEFINED_FAILURE;
+    }
   } else {
     RCLCPP_INFO(node_->get_logger(),
       "SlamToolbox: Saving map in current directory.");
     int rc = system("ros2 run nav2_map_server map_saver_cli --ros-args -p map_subscribe_transient_local:=true");
+    if (rc == 0) {
+      response->result = response->RESULT_SUCCESS;
+    } else {
+      response->result = response->RESULT_UNDEFINED_FAILURE;
+    }
   }
 
   rclcpp::sleep_for(std::chrono::seconds(1));

diff --git a/src/slam_toolbox_common.cpp b/src/slam_toolbox_common.cpp
@@ -638,8 +638,12 @@ bool SlamToolbox::serializePoseGraphCallback(
   }
 
   boost::mutex::scoped_lock lock(smapper_mutex_);
-  serialization::write(filename, *smapper_->getMapper(),
-    *dataset_, shared_from_this());
+  if (serialization::write(filename, *smapper_->getMapper(), *dataset_, shared_from_this())) {
+    resp->result = resp->RESULT_SUCCESS;
+  } else {
+    resp->result = resp->RESULT_FAILED_TO_WRITE_FILE;
+  }
+
   return true;
 }
 

diff --git a/srvs/SaveMap.srv b/srvs/SaveMap.srv
@@ -1,2 +1,8 @@
 std_msgs/String name
 ---
+# Result code defintions
+uint8 RESULT_SUCCESS=0
+uint8 RESULT_NO_MAP_RECEIEVD=1
+uint8 RESULT_UNDEFINED_FAILURE=255
+
+uint8 result
diff --git a/srvs/SerializePoseGraph.srv b/srvs/SerializePoseGraph.srv
@@ -1,2 +1,7 @@
 string filename
----
+---
+# Result code defintions
+uint8 RESULT_SUCCESS=0
+uint8 RESULT_FAILED_TO_WRITE_FILE=255
+
+uint8 result