diff --git a/include/slam_toolbox/serialization.hpp b/include/slam_toolbox/serialization.hpp index 7d1aaaae4..9711a3859 100644 --- 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 index ce821a10d..8a93320da 100644 --- 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 index 5848720d3..8b780f8e3 100644 --- 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 index f38351323..c3a85f0d3 100644 --- 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 index 811117977..398b323e7 100644 --- a/srvs/SerializePoseGraph.srv +++ b/srvs/SerializePoseGraph.srv @@ -1,2 +1,7 @@ string filename ---- \ No newline at end of file +--- +# Result code defintions +uint8 RESULT_SUCCESS=0 +uint8 RESULT_FAILED_TO_WRITE_FILE=255 + +uint8 result \ No newline at end of file