Updated to have error codes in the services

example_srvs/CMakeLists.txt

@@ -12,8 +12,10 @@ find_package(std_msgs REQUIRED)
 find_package(rosidl_default_generators REQUIRED)
 
 rosidl_generate_interfaces(${PROJECT_NAME}
-  "srv/GetPath.srv"
+  "msg/SetMapCodes.msg"
+  "msg/GetPathCodes.msg"
   "srv/SetMap.srv"
+  "srv/GetPath.srv"
   DEPENDENCIES std_msgs
 )
 

example_srvs/msg/GetPathCodes.msg

@@ -0,0 +1,8 @@
+uint8 SUCCESS=0
+
+uint8 EMPTY_OCCUPANCY_MAP=1
+uint8 START_POSITION_INVALID_SIZE=2
+uint8 GOAL_POSITION_INVALID_SIZE=3
+uint8 NO_VALID_PATH=4
+
+uint8 code

example_srvs/msg/SetMapCodes.msg

@@ -0,0 +1,7 @@
+uint8 SUCCESS=0
+
+uint8 EMPTY_REQUEST=1
+uint8 DIMENSION_AND_STRIDE_MISMATCH=2
+uint8 LENGTH_AND_STRIDE_MISMATCH=3
+
+uint8 code

example_srvs/srv/GetPath.srv

@@ -1,5 +1,5 @@
 std_msgs/UInt8MultiArray start
 std_msgs/UInt8MultiArray goal
 ---
-std_msgs/Bool success
+example_srvs/GetPathCodes result
 std_msgs/UInt8MultiArray path

example_srvs/srv/SetMap.srv

@@ -1,3 +1,3 @@
 std_msgs/UInt8MultiArray map
 ---
-std_msgs/Bool success
+example_srvs/SetMapCodes result

functional_programming_tests/include/pathing/pathing.hpp

@@ -6,6 +6,7 @@
 #include <utility>
 #include <vector>
 
+#include <example_srvs/msg/get_path_codes.hpp>
 #include <example_srvs/srv/get_path.hpp>
 #include <tl_expected/expected.hpp>
 
@@ -75,10 +76,12 @@ namespace generate_path {
  * @brief      Types of errors expected in the generate path callback function
  */
 enum class error {
-  EMPTY_OCCUPANCY_MAP,
-  INVALID_START_SIZE,
-  INVALID_GOAL_SIZE,
-  NO_VALID_PATH
+  EMPTY_OCCUPANCY_MAP = example_srvs::msg::GetPathCodes::EMPTY_OCCUPANCY_MAP,
+  INVALID_START_SIZE =
+      example_srvs::msg::GetPathCodes::START_POSITION_INVALID_SIZE,
+  INVALID_GOAL_SIZE =
+      example_srvs::msg::GetPathCodes::GOAL_POSITION_INVALID_SIZE,
+  NO_VALID_PATH = example_srvs::msg::GetPathCodes::NO_VALID_PATH
 };
 
 /**

functional_programming_tests/include/pathing/utilities.hpp

@@ -6,16 +6,35 @@
 
 #include <example_srvs/srv/set_map.hpp>
 #include <std_msgs/msg/u_int8_multi_array.hpp>
+#include <tl_expected/expected.hpp>
 
 namespace pathing::utilities {
+/**
+ * @brief      Types of errors expected in parsing the set map request
+ */
+enum class parsing_set_map_error {
+  EMPTY_REQUEST = 1,
+  DIMENSION_AND_STRIDE_MISMATCH = 2,
+  LENGTH_AND_STRIDE_MISMATCH = 3,
+};
+/**
+ * @brief      Descriptions of the errors
+ */
+std::map<parsing_set_map_error, std::string> const
+    parsing_set_map_error_description = {
+        {parsing_set_map_error::EMPTY_REQUEST, "REQUEST DATA FIELD IS EMPTY!!"},
+        {parsing_set_map_error::DIMENSION_AND_STRIDE_MISMATCH,
+         "OCCUPANCY MAP DIMENSIONS AND STRIDE INCONSISTENT!!"},
+        {parsing_set_map_error::LENGTH_AND_STRIDE_MISMATCH,
+         "OCCUPANCY MAP LENGTH AND STRIDE INCONSISTENT!!"}};
 /**
  * @brief      Converts map from message to occupancy map if possible
  *
  * @param[in]  request containing the map
  *
  * @return     std::optional containing the occupancy map
  */
-std::optional<Map<unsigned char>> parseSetMapRequest(
+tl::expected<Map<unsigned char>, parsing_set_map_error> parseSetMapRequest(
     std::shared_ptr<example_srvs::srv::SetMap::Request> const request);
 
 /**

functional_programming_tests/src/pathing.cpp

@@ -8,6 +8,7 @@
 #include <unordered_map>
 #include <vector>
 
+#include <example_srvs/msg/get_path_codes.hpp>
 #include <example_srvs/srv/get_path.hpp>
 #include <tl_expected/expected.hpp>
 
@@ -120,7 +121,7 @@ tl::expected<example_srvs::srv::GetPath::Response, error> generate_path(
   }
 
   auto response = example_srvs::srv::GetPath::Response{};
-  response.success.data = path.has_value();
+  response.result.code = example_srvs::msg::GetPathCodes::SUCCESS;
   response.path = utilities::createUInt8MultiArrayMessageFromPath(path.value());
 
   return response;

functional_programming_tests/src/pathing_manager.cpp

@@ -3,9 +3,11 @@
 #include "pathing/pathing.hpp"
 #include "pathing/utilities.hpp"
 
+#include <iostream>
 #include <memory>
 #include <string>
 
+#include <example_srvs/msg/get_path_codes.hpp>
 #include <example_srvs/srv/get_path.hpp>
 #include <example_srvs/srv/set_map.hpp>
 #include <tl_expected/expected.hpp>
@@ -17,36 +19,64 @@ namespace pathing {
 Manager::Manager(std::unique_ptr<Manager::MiddlewareHandle> mw,
                  Parameters params)
     : mw_{std::move(mw)}, params_{std::move(params)} {
-  mw_->register_set_map_service(
-      [this](auto const request, auto response) -> void {
-        auto const path = utilities::parseSetMapRequest(request);
-        if (path) this->map_ = path.value();
-        response->success.data = path.has_value();
-      });
+  mw_->register_set_map_service([this](auto const request,
+                                       auto response) -> void {
+    auto const set_map = [this](Map<unsigned char> const map)
+        -> tl::expected<Map<unsigned char>, utilities::parsing_set_map_error> {
+      this->map_ = map;
+      return map;
+    };
+
+    auto const return_successful_response = []([[maybe_unused]] auto const)
+        -> tl::expected<SetMap::Response, utilities::parsing_set_map_error> {
+      auto response = SetMap::Response{};
+      response.result.code = example_srvs::msg::GetPathCodes::SUCCESS;
+      return response;
+    };
+
+    auto const print_error =
+        [this](utilities::parsing_set_map_error const error)
+        -> tl::expected<SetMap::Response, utilities::parsing_set_map_error> {
+      mw_->log_error(utilities::parsing_set_map_error_description.at(error));
+      return tl::make_unexpected(error);
+    };
+
+    auto const return_error_response =
+        [](utilities::parsing_set_map_error const error)
+        -> tl::expected<SetMap::Response, utilities::parsing_set_map_error> {
+      auto response = SetMap::Response{};
+      response.result.code = static_cast<int>(error);
+      return response;
+    };
+
+    *response = utilities::parseSetMapRequest(request)
+                    .and_then(set_map)
+                    .and_then(return_successful_response)
+                    .or_else(print_error)
+                    .or_else(return_error_response)
+                    .value();
+  });
   mw_->register_generate_path_service([this](auto const request,
                                              auto response) {
-    auto const print_error = [this](std::string_view error)
-        -> tl::expected<GetPath::Response, std::string> {
-      mw_->log_error(std::string{error});
-      return tl::make_unexpected("");
+    auto const print_error = [this](auto const error)
+        -> tl::expected<GetPath::Response, pathing::generate_path::error> {
+      mw_->log_error(pathing::generate_path::error_description.at(error));
+      return tl::make_unexpected(error);
     };
 
-    auto const return_empty_response = []([[maybe_unused]] auto const)
-        -> tl::expected<GetPath::Response, std::string> {
+    auto const return_error_response = [](auto const error)
+        -> tl::expected<GetPath::Response, pathing::generate_path::error> {
       auto response = GetPath::Response{};
-      response.success.data = false;
+      response.result.code = static_cast<int>(error);
       response.path = std_msgs::msg::UInt8MultiArray();
       return response;
     };
-    auto const stringify_error = [](auto const error) {
-      return generate_path::error_description.at(error);
-    };
+
     *response =
         generate_path::generate_path(
             request, this->map_, this->params_.robot_size, generate_global_path)
-            .map_error(stringify_error)
             .or_else(print_error)
-            .or_else(return_empty_response)
+            .or_else(return_error_response)
             .value();
   });
 }

functional_programming_tests/src/utilities.cpp

@@ -1,24 +1,27 @@
+#include "pathing/utilities.hpp"
+
 #include "pathing/pathing.hpp"
 
 #include <memory>
 #include <optional>
 
 #include <example_srvs/srv/set_map.hpp>
 #include <std_msgs/msg/u_int8_multi_array.hpp>
+#include <tl_expected/expected.hpp>
 
 namespace pathing::utilities {
-std::optional<Map<unsigned char>> parseSetMapRequest(
+tl::expected<Map<unsigned char>, parsing_set_map_error> parseSetMapRequest(
     std::shared_ptr<example_srvs::srv::SetMap::Request> const request) {
   if (request->map.layout.dim.empty() || request->map.data.empty()) {
-    return std::nullopt;
+    return tl::unexpected(parsing_set_map_error::EMPTY_REQUEST);
   }
   // Check that map layout makes sense
   if ((request->map.layout.dim[0].size * request->map.layout.dim[1].size) !=
       request->map.layout.dim[0].stride) {
-    return std::nullopt;
+    return tl::unexpected(parsing_set_map_error::DIMENSION_AND_STRIDE_MISMATCH);
   }
   if (request->map.layout.dim[0].stride != request->map.data.size()) {
-    return std::nullopt;
+    return tl::unexpected(parsing_set_map_error::LENGTH_AND_STRIDE_MISMATCH);
   }
 
   auto const occupancy_map = request->map;