diff --git a/slam_toolbox/include/slam_toolbox/serialization.hpp b/slam_toolbox/include/slam_toolbox/serialization.hpp index 7a109f70a..731fe24c2 100644 --- a/slam_toolbox/include/slam_toolbox/serialization.hpp +++ b/slam_toolbox/include/slam_toolbox/serialization.hpp @@ -35,7 +35,7 @@ inline bool fileExists(const std::string& name) return (stat (name.c_str(), &buffer) == 0); } -inline void write(const std::string& filename, +inline bool write(const std::string& filename, karto::Mapper& mapper, karto::Dataset& dataset) { @@ -43,10 +43,12 @@ inline void write(const std::string& filename, { mapper.SaveToFile(filename + std::string(".posegraph")); dataset.SaveToFile(filename + std::string(".data")); + return true; } catch (boost::archive::archive_exception e) { ROS_ERROR("Failed to write file: Exception %s", e.what()); + return false; } } diff --git a/slam_toolbox/src/map_saver.cpp b/slam_toolbox/src/map_saver.cpp index e43cbe669..ea941d421 100644 --- a/slam_toolbox/src/map_saver.cpp +++ b/slam_toolbox/src/map_saver.cpp @@ -47,6 +47,7 @@ bool MapSaver::saveMapCallback( { ROS_WARN("Map Saver: Cannot save map, no map yet received on topic %s.", map_name_.c_str()); + resp.success = false; return false; } @@ -62,6 +63,7 @@ bool MapSaver::saveMapCallback( int rc = system("rosrun map_server map_saver"); } ros::Duration(1.0).sleep(); + resp.success = true; return true; } diff --git a/slam_toolbox/src/slam_toolbox_common.cpp b/slam_toolbox/src/slam_toolbox_common.cpp index a778fbb26..ef708774d 100644 --- a/slam_toolbox/src/slam_toolbox_common.cpp +++ b/slam_toolbox/src/slam_toolbox_common.cpp @@ -584,6 +584,7 @@ bool SlamToolbox::serializePoseGraphCallback( slam_toolbox_msgs::SerializePoseGraph::Response &resp) /*****************************************************************************/ { + bool success = false; std::string filename = req.filename; // if we're inside the snap, we need to write to commonly accessible space @@ -593,7 +594,8 @@ bool SlamToolbox::serializePoseGraphCallback( } boost::mutex::scoped_lock lock(smapper_mutex_); - serialization::write(filename, *smapper_->getMapper(), *dataset_); + success = serialization::write(filename, *smapper_->getMapper(), *dataset_); + resp.success = success; return true; } @@ -706,6 +708,7 @@ bool SlamToolbox::deserializePoseGraphCallback( { ROS_ERROR("Deserialization called without valid processor type set. " "Undefined behavior!"); + resp.success = false; return false; } @@ -714,6 +717,7 @@ bool SlamToolbox::deserializePoseGraphCallback( if (filename.empty()) { ROS_WARN("No map file given!"); + resp.success = false; return true; } @@ -730,6 +734,7 @@ bool SlamToolbox::deserializePoseGraphCallback( { ROS_ERROR("DeserializePoseGraph: Failed to read " "file: %s.", filename.c_str()); + resp.success = false; return true; } ROS_DEBUG("DeserializePoseGraph: Successfully read file."); @@ -755,9 +760,11 @@ bool SlamToolbox::deserializePoseGraphCallback( req.initial_pose.y, req.initial_pose.theta); break; default: + resp.success = false; ROS_FATAL("Deserialization called without valid processor type set."); } + resp.success = true; return true; } diff --git a/slam_toolbox_msgs/srv/DeserializePoseGraph.srv b/slam_toolbox_msgs/srv/DeserializePoseGraph.srv index 480fea926..aa9517dc2 100644 --- a/slam_toolbox_msgs/srv/DeserializePoseGraph.srv +++ b/slam_toolbox_msgs/srv/DeserializePoseGraph.srv @@ -9,4 +9,5 @@ int8 LOCALIZE_AT_POSE = 3 string filename int8 match_type geometry_msgs/Pose2D initial_pose ---- \ No newline at end of file +--- +bool success \ No newline at end of file diff --git a/slam_toolbox_msgs/srv/SaveMap.srv b/slam_toolbox_msgs/srv/SaveMap.srv index f38351323..6068bc60a 100644 --- a/slam_toolbox_msgs/srv/SaveMap.srv +++ b/slam_toolbox_msgs/srv/SaveMap.srv @@ -1,2 +1,3 @@ std_msgs/String name --- +bool success diff --git a/slam_toolbox_msgs/srv/SerializePoseGraph.srv b/slam_toolbox_msgs/srv/SerializePoseGraph.srv index 811117977..f87f11da3 100644 --- a/slam_toolbox_msgs/srv/SerializePoseGraph.srv +++ b/slam_toolbox_msgs/srv/SerializePoseGraph.srv @@ -1,2 +1,3 @@ string filename ---- \ No newline at end of file +--- +bool success \ No newline at end of file