diff --git a/mesh_map/include/mesh_map/mesh_map.h b/mesh_map/include/mesh_map/mesh_map.h index bae8915c..5f0a9880 100644 --- a/mesh_map/include/mesh_map/mesh_map.h +++ b/mesh_map/include/mesh_map/mesh_map.h @@ -53,7 +53,7 @@ #include #include #include -#include +#include #include "nanoflann.hpp" #include "nanoflann_mesh_adaptor.h" @@ -401,7 +401,7 @@ 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: @@ -409,8 +409,11 @@ class MeshMap * - 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::SharedPtr save_service; + rclcpp::Service::SharedPtr save_service; // Reconfigurable parameters (see reconfigureCallback method) int min_contour_size; diff --git a/mesh_map/src/mesh_map.cpp b/mesh_map/src/mesh_map.cpp index 2dfcb196..80c456b1 100644 --- a/mesh_map/src/mesh_map.cpp +++ b/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("~/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("~/save_map", [this]( - const std::shared_ptr request, - std::shared_ptr response) + save_service = node->create_service("~/save_map", [this]( + const std::shared_ptr request, + std::shared_ptr 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(*mesh_ptr); kd_tree_ptr = std::make_unique(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)