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/srv/GetPath.srv

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

example_srvs/srv/SetMap.srv

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

functional_programming_tests/include/pathing/pathing.hpp

@@ -74,10 +74,10 @@ 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 = 1,
+  INVALID_START_SIZE = 2,
+  INVALID_GOAL_SIZE = 3,
+  NO_VALID_PATH = 4
 };
 
 /**

functional_programming_tests/include/pathing/utilities.hpp

@@ -4,18 +4,38 @@
 #include <memory>
 #include <optional>
 
+#include <tl_expected/expected.hpp>
+
 #include <example_srvs/srv/set_map.hpp>
 #include <std_msgs/msg/u_int8_multi_array.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

@@ -115,7 +115,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.code.code = 0;
   response.path = utilities::createUInt8MultiArrayMessageFromPath(path.value());
 
   return response;

functional_programming_tests/src/pathing_manager.cpp

@@ -5,6 +5,7 @@
 
 #include <memory>
 #include <string>
+#include <iostream>
 
 #include <example_srvs/srv/get_path.hpp>
 #include <example_srvs/srv/set_map.hpp>
@@ -18,33 +19,62 @@ Manager::Manager(std::unique_ptr<Manager::MiddlewareHandle> mw)
     : mw_{std::move(mw)} {
   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();
+
+        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 success) 
+        -> tl::expected<SetMap::Response, utilities::parsing_set_map_error> {
+          auto response = SetMap::Response{};
+          response.code.code = 0;
+          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(std::string{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.code.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(std::string{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.code.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);
-    };
+
+    std::cerr << "In path callback, map size is : " << this->map_.get_data().size() << "\n";
+
     *response =
         generate_path::generate_path(request, this->map_, 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/pathing.hpp"
+#include "pathing/utilities.hpp"
 
 #include <memory>
 #include <optional>
 
+#include <tl_expected/expected.hpp>
+
 #include <example_srvs/srv/set_map.hpp>
 #include <std_msgs/msg/u_int8_multi_array.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;

functional_programming_tests/test/with_dependency_injection.cpp

@@ -7,8 +7,10 @@
 
 #include <rclcpp/rclcpp.hpp>
 
-#include <example_srvs/srv/get_path.hpp>
 #include <example_srvs/srv/set_map.hpp>
+#include <example_srvs/srv/get_path.hpp>
+#include <example_srvs/msg/set_map_codes.hpp>
+#include <example_srvs/msg/get_path_codes.hpp>
 #include <gmock/gmock.h>
 #include <gtest/gtest.h>
 #include <std_msgs/msg/u_int8_multi_array.hpp>
@@ -109,7 +111,7 @@ struct PathGenerator {
             std::shared_ptr<example_srvs::srv::SetMap::Request> const request,
             std::shared_ptr<example_srvs::srv::SetMap::Response> response) {
           // Set the map to generate the path from
-          response->success.data = costmap_setter(request->map);
+          response->code.code = costmap_setter(request->map);
         });
     // Register the generate path service
     mw_->register_generate_path_service(  // Trivial comment to enfore params on
@@ -119,21 +121,21 @@ struct PathGenerator {
             std::shared_ptr<example_srvs::srv::GetPath::Response> response) {
           if (map_.get_data().size() == 0) {
             mw_->log_error("MAP IS EMPTY!!");
-            response->success.data = false;
+            response->code.code = example_srvs::msg::GetPathCodes::EMPTY_OCCUPANCY_MAP;
             response->path = std_msgs::msg::UInt8MultiArray();
             return;
           }
           // Check to make sure start and goal fields of the request are of
           // size 2
           if (request->start.data.size() != 2) {
             mw_->log_error("START POSITION MUST CONTAIN TWO ELEMENTS!!");
-            response->success.data = false;
+            response->code.code = example_srvs::msg::GetPathCodes::START_POSITION_INVALID_SIZE;
             response->path = std_msgs::msg::UInt8MultiArray();
             return;
           }
           if (request->goal.data.size() != 2) {
             mw_->log_error("GOAL POSITION MUST CONTAIN TWO ELEMENTS!!");
-            response->success.data = false;
+            response->code.code = example_srvs::msg::GetPathCodes::GOAL_POSITION_INVALID_SIZE;
             response->path = std_msgs::msg::UInt8MultiArray();
             return;
           }
@@ -173,7 +175,7 @@ struct PathGenerator {
             }
           }
 
-          response->success.data = path.has_value();
+          response->code.code = path.has_value() ? example_srvs::msg::GetPathCodes::SUCCESS : example_srvs::msg::GetPathCodes::NO_VALID_PATH;
           response->path = response_path;
         });
   }
@@ -184,18 +186,18 @@ struct PathGenerator {
    * @param costmap The costmap message
    * @return True if the costmap was set successfully, false otherwise
    */
-  bool costmap_setter(
+  unsigned char costmap_setter(
       std_msgs::msg::UInt8MultiArray const& costmap) {  // Action
     // Get the costmap from a ros topic
     // Check that map layout makes sense
     if ((costmap.layout.dim[0].size * costmap.layout.dim[1].size) !=
         costmap.layout.dim[0].stride) {
       mw_->log_error("COSTMAP DIMENSIONS AND STRIDE INCONSISTENT!!");
-      return false;
+      return example_srvs::msg::SetMapCodes::DIMENSION_AND_STRIDE_MISMATCH;
     }
     if (costmap.layout.dim[0].stride != costmap.data.size()) {
       mw_->log_error("COSTMAP LENGTH AND STRIDE INCONSISTENT!!");
-      return false;
+      return example_srvs::msg::SetMapCodes::LENGTH_AND_STRIDE_MISMATCH;
     }
     auto const begin = std::begin(costmap.data);
     // Populate the map
@@ -204,7 +206,7 @@ struct PathGenerator {
           {begin + row * costmap.layout.dim[1].size,
            begin + (row + 1) * costmap.layout.dim[1].size});
     }
-    return true;
+    return example_srvs::msg::SetMapCodes::SUCCESS;
   }
 
   /**
@@ -331,7 +333,7 @@ std::vector<Position> parseGeneratedPath(
  * @brief Create a sample cost map in a request message
  * @return A shared pointer to the request message with the cost map
  */
-std::shared_ptr<example_srvs::srv::SetMap::Request> make_costmap() {
+std::shared_ptr<example_srvs::srv::SetMap::Request> make_occupancy_map() {
   auto const request = std::make_shared<example_srvs::srv::SetMap::Request>();
 
   request->map = std_msgs::msg::UInt8MultiArray();
@@ -394,12 +396,12 @@ TEST(PathGenerator, SetMap) {
   auto const path_generator = PathGenerator{std::move(mw)};
 
   // WHEN the set map service is called
-  auto const request = make_costmap();
+  auto const request = make_occupancy_map();
   auto response = std::make_shared<example_srvs::srv::SetMap::Response>();
   callback(request, response);
 
   // THEN the path generator should successfully set the map
-  EXPECT_TRUE(response->success.data);
+  EXPECT_EQ(response->code.code, example_srvs::msg::SetMapCodes::SUCCESS);
 }
 
 TEST(PathGenerator, NoCostmap) {
@@ -421,7 +423,7 @@ TEST(PathGenerator, NoCostmap) {
   path_callback(path_request, path_response);
 
   // THEN the path generator should fail
-  EXPECT_FALSE(path_response->success.data);
+  EXPECT_EQ(path_response->code.code, example_srvs::msg::GetPathCodes::EMPTY_OCCUPANCY_MAP);
 }
 
 struct PathGeneratorFixture : public testing::Test {
@@ -433,7 +435,7 @@ struct PathGeneratorFixture : public testing::Test {
     // When the map callback is called, set the costmap
     ON_CALL(*mw_, register_set_map_service(testing::_))
         .WillByDefault([&](auto const& map_callback) {
-          auto const map_request = make_costmap();
+          auto const map_request = make_occupancy_map();
           auto map_response =
               std::make_shared<example_srvs::srv::SetMap::Response>();
           map_callback(map_request, map_response);
@@ -459,7 +461,7 @@ TEST_F(PathGeneratorFixture, NoStartNoGoal) {
   path_callback_(path_request, path_response);
 
   // THEN the path generator should fail
-  EXPECT_FALSE(path_response->success.data);
+  EXPECT_EQ(path_response->code.code, example_srvs::msg::GetPathCodes::START_POSITION_INVALID_SIZE);
 }
 
 TEST_F(PathGeneratorFixture, SameStartGoal) {
@@ -474,7 +476,7 @@ TEST_F(PathGeneratorFixture, SameStartGoal) {
   path_callback_(path_request, path_response);
 
   // THEN the path generator should succeed
-  EXPECT_TRUE(path_response->success.data);
+  EXPECT_EQ(path_response->code.code, example_srvs::msg::GetPathCodes::SUCCESS);
   auto const expected = std::vector<Position>{{0, 0}};
   // AND the path should be the same as the start
   EXPECT_EQ(parseGeneratedPath(path_response->path), expected);
@@ -493,7 +495,7 @@ TEST_F(PathGeneratorFixture, NoPath) {
   path_callback_(path_request, path_response);
 
   // THEN the path generator should succeed
-  EXPECT_FALSE(path_response->success.data);
+  EXPECT_EQ(path_response->code.code, example_srvs::msg::GetPathCodes::NO_VALID_PATH);
   auto const expected = std::vector<Position>{};
   // AND the path should be empty
   EXPECT_EQ(parseGeneratedPath(path_response->path), expected);
@@ -511,7 +513,7 @@ TEST_F(PathGeneratorFixture, PathGenerated) {
   path_callback_(path_request, path_response);
 
   // THEN the path generator should succeed
-  EXPECT_TRUE(path_response->success.data);
+  EXPECT_EQ(path_response->code.code, example_srvs::msg::GetPathCodes::SUCCESS);
   auto const expected = std::vector<Position>{
       {0, 0}, {1, 0}, {2, 0}, {3, 0}, {4, 0}, {5, 0}, {6, 0}, {7, 0},
       {7, 1}, {7, 2}, {7, 3}, {7, 4}, {7, 5}, {7, 6}, {7, 7}};