added std_srvs Trigger instead of Empty

mesh_map/include/mesh_map/mesh_map.h

@@ -53,7 +53,7 @@
 #include <std_msgs/msg/color_rgba.hpp>
 #include <tf2_ros/buffer.h>
 #include <visualization_msgs/msg/marker.hpp>
-#include <std_srvs/srv/empty.hpp>
+#include <std_srvs/srv/trigger.hpp>
 
 #include "nanoflann.hpp"
 #include "nanoflann_mesh_adaptor.h"
@@ -401,16 +401,19 @@ class MeshMap
   mesh_map::AbstractLayer::Ptr layer(const std::string& layer_name);
 
   /**
-   * @brief calls 'saveLayer' on every active layer. Every layer itself writes its costs 
+   * @brief calls 'writeLayer' on every active layer. Every layer itself writes its costs 
    *        to the working file / part to a dataset named after the layer-name.
    *  
    * Example:
    *  - Working file: "my_map.h5"
    *  - Working mesh part: "my_mesh_part"
    * 
    * A BorderLayer of name 'border' would write the costs to "my_map.h5/my_mesh_part/channels/border"  
+   * 
+   * @return Trigger response. If success is false, the message field is 
+   *      beeing filled with all the failed layer names seperated by comma 
    */
-  void saveLayers();
+  std_srvs::srv::Trigger::Response writeLayers();
 
   //! This is an abstract interface to load mesh information from somewhere
   //! The default case is loading from a HDF5 file
@@ -462,7 +465,7 @@ class MeshMap
   //! dynamic params callback handle
   rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr config_callback;
 
-  rclcpp::Service<std_srvs::srv::Empty>::SharedPtr save_service;
+  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr save_service;
 
   // Reconfigurable parameters (see reconfigureCallback method)
   int min_contour_size;

mesh_map/src/mesh_map.cpp

@@ -148,11 +148,11 @@ MeshMap::MeshMap(tf2_ros::Buffer& tf, const rclcpp::Node::SharedPtr& node)
   vector_field_pub = node->create_publisher<visualization_msgs::msg::Marker>("~/vector_field", rclcpp::QoS(1).transient_local());
   config_callback = node->add_on_set_parameters_callback(std::bind(&MeshMap::reconfigureCallback, this, std::placeholders::_1));
 
-  save_service = node->create_service<std_srvs::srv::Empty>("~/save_map", [this](
-    const std::shared_ptr<std_srvs::srv::Empty::Request> request,
-    std::shared_ptr<std_srvs::srv::Empty::Response>      response)
+  save_service = node->create_service<std_srvs::srv::Trigger>("~/save_map", [this](
+    const std::shared_ptr<std_srvs::srv::Trigger::Request> request,
+    std::shared_ptr<std_srvs::srv::Trigger::Response>      response)
   {
-    saveLayers();
+    *response = writeLayers();
   });
 }
 
@@ -253,7 +253,7 @@ bool MeshMap::readMap()
     RCLCPP_INFO_STREAM(node->get_logger(), "The mesh has been loaded successfully with " 
       << mesh_ptr->numVertices() << " vertices and " << mesh_ptr->numFaces() << " faces and "
       << mesh_ptr->numEdges() << " edges.");
-
+    // build a tree for fast lookups
     adaptor_ptr = std::make_unique<NanoFlannMeshAdaptor>(*mesh_ptr);
     kd_tree_ptr = std::make_unique<KDTree>(3,*adaptor_ptr, nanoflann::KDTreeSingleIndexAdaptorParams(10));
     kd_tree_ptr->buildIndex();
@@ -1162,17 +1162,33 @@ mesh_map::AbstractLayer::Ptr MeshMap::layer(const std::string& layer_name)
     })->second;
 }
 
-void MeshMap::saveLayers()
+std_srvs::srv::Trigger::Response MeshMap::writeLayers()
 {
+  std::stringstream ss;
+  bool write_failure = false;
+
   for (auto& layer : loaded_layers)
   {
     auto& layer_plugin = layer.second;
     const auto& layer_name = layer.first;
 
     RCLCPP_INFO_STREAM(node->get_logger(), "Writing '" << layer_name << "' to file.");
-    layer_plugin->writeLayer();
-    RCLCPP_INFO_STREAM(node->get_logger(), "Finished writing '" << layer_name << "' to file.");
+    if(layer_plugin->writeLayer())
+    {
+      RCLCPP_INFO_STREAM(node->get_logger(), "Finished writing '" << layer_name << "' to file.");
+    } else {
+      // this is not the first failure. add a comma in between 
+      if(write_failure){ss << ",";}
+      ss << layer_name;
+      write_failure = true;
+      RCLCPP_ERROR_STREAM(node->get_logger(), "Error while writing '" << layer_name << "' to file.");      
+    }
   }
+
+  std_srvs::srv::Trigger::Response res;
+  res.success = !write_failure;
+  res.message = ss.str();
+  return res;
 }
 
 bool MeshMap::barycentricCoords(const Vector& p, const lvr2::FaceHandle& triangle, float& u, float& v, float& w)