Migrate collision checker and parameters to library (#38)

* Migrate collision checker and parameters to library

functional_programming_tests/include/pathing/collision_checking.hpp

@@ -0,0 +1,47 @@
+#include "pathing/pathing.hpp"
+
+template <typename T>
+struct CollisionChecker {
+  /**
+   * @brief Checks if the robot is colliding with an obstacle or out of bounds
+   * @param robot_size The size of the robot in pixels
+   */
+  CollisionChecker(int robot_size) : robot_size_{robot_size} {}
+
+  /**
+   * @brief Checks if the robot is colliding with an obstacle or out of bounds
+   * @param map The map to check for collision
+   * @param x position of the center of robot
+   * @param y position of the center of robot
+   * TODO: This check doesn't make any physical sense. The robot should be
+   *       checked radially from the center of the robot. This is a square
+   *       starting from the corner of the robot as the origin.
+   * @return True if the robot is colliding with an obstacle or out of bounds
+   */
+  bool operator()(Map<T> const& map, auto const x, auto const y) const {
+    // Get the dimensions of the map for bounds checking
+    auto const [dim_x, dim_y] = map.dim();
+
+    // Check if the robot is out of bounds or colliding with an obstacle
+    for (int dx = 0; dx < robot_size_; ++dx) {
+      for (int dy = 0; dy < robot_size_; ++dy) {
+        // Get the position of the robot to check for collision
+        int const px = x + dx;
+        int const py = y + dy;
+
+        // Check if the robot is out of bounds
+        if (px < 0 || px >= dim_x || py < 0 || py >= dim_y) {
+          return true;
+        }
+        // Check if the robot is colliding with an obstacle
+        if (map.at(Position{x + dx, y + dy}) == 255) {
+          return true;
+        }
+      }
+    }
+    return false;
+  }
+
+ private:
+  int robot_size_;
+};

functional_programming_tests/include/pathing/pathing.hpp

@@ -54,20 +54,21 @@ std::ostream& operator<<(std::ostream& os, std::vector<Position> const& path);
 bool operator==(Position const& lhs, Position const& rhs);
 
 using PathingGenerator = std::function<std::optional<Path>(
-    Position const&, Position const&, Map<unsigned char> const&)>;
+    Position const&, Position const&, Map<unsigned char> const&, int)>;
 
 /**
  * @brief      Generates a path
  *
  * @param      start position
  * @param      goal position
  * @param      occupancy_map to path through
+ * @param      robot_size in pixels
  *
  * @return     std::optional containing the Path
  */
 std::optional<Path> generate_global_path(
     Position const& start, Position const& goal,
-    Map<unsigned char> const& occupancy_map);
+    Map<unsigned char> const& occupancy_map, int robot_size);
 
 namespace generate_path {
 /**
@@ -97,13 +98,15 @@ std::map<error, std::string> const error_description = {
  *
  * @param[in]  request contains the start and goal positions
  * @param[in]  occupancy_map to path through
+ * @param[in]  robot_size in pixels
  * @param[in]  path_generator algorithm to use for pathing
  *
  * @return     The path if successful, otherwise an error
  */
 tl::expected<example_srvs::srv::GetPath::Response, error> generate_path(
     std::shared_ptr<example_srvs::srv::GetPath::Request> const request,
-    Map<unsigned char> const& occupancy_map, PathingGenerator path_generator);
+    Map<unsigned char> const& occupancy_map, int robot_size,
+    PathingGenerator path_generator);
 
 }  // namespace generate_path
 

functional_programming_tests/include/pathing/pathing_manager.hpp

@@ -14,6 +14,21 @@ using GetPath = example_srvs::srv::GetPath;
 namespace pathing {
 
 struct Manager {
+  /**
+   * @brief Parameters for the path manager
+   */
+  struct Parameters {
+    /**
+     * @brief The size of the robot in pixels
+     * @note This is a toy description of robot size as a square
+     *       centered at the robot's upper left hand corner
+     */
+    int robot_size;
+  };
+
+  /**
+   * @brief Interface for interacting with ros services
+   */
   struct MiddlewareHandle {
     // Define map service callback type
     using SetMapCallback = std::function<void(
@@ -60,15 +75,21 @@ struct Manager {
   /**
    * @brief Manages occupancy_map and path generation for the pathing node
    * @param mw The middleware handle for interacting with services
+   * @param params for manager configuration
    */
-  Manager(std::unique_ptr<MiddlewareHandle> mw);
+  Manager(std::unique_ptr<MiddlewareHandle> mw, Parameters params);
 
  private:
   /**
    * @brief Middleware handle for interacting with services
    */
   std::unique_ptr<MiddlewareHandle> mw_;
 
+  /**
+   * @brief Parameters for the path manager
+   */
+  Parameters params_;
+
   /**
    * @brief Occupancy map to path through
    */

functional_programming_tests/src/pathing.cpp

@@ -27,7 +27,7 @@ bool operator==(Position const& lhs, Position const& rhs) {
 
 std::optional<Path> generate_global_path(
     Position const& start, Position const& goal,
-    Map<unsigned char> const& occupancy_map) {  // Calculation
+    Map<unsigned char> const& occupancy_map, int robot_size) {
   // Pre-checks
   if (occupancy_map.empty()) {
     return std::nullopt;
@@ -82,9 +82,13 @@ std::optional<Path> generate_global_path(
 
   // Fails if there is any obstacle in the way
   // Move horizontally
-  repeat(std::abs(del_x), [&move, &del_x_sign]() { move(del_x_sign, 0); });
+  // x limit is reduced by half of the robot size to prevent the
+  // algorithm from checking collision outside of the map
+  auto const x_limit = std::abs(del_x) - std::floor(robot_size / 2);
+  repeat(x_limit, [&move, &del_x_sign]() { move(del_x_sign, 0); });
   // Move vertically
-  repeat(std::abs(del_y), [&move, &del_y_sign]() { move(0, del_y_sign); });
+  auto const y_limit = std::abs(del_y) - std::floor(robot_size / 2);
+  repeat(y_limit, [&move, &del_y_sign]() { move(0, del_y_sign); });
 
   return path;
 }
@@ -93,7 +97,8 @@ namespace generate_path {
 
 tl::expected<example_srvs::srv::GetPath::Response, error> generate_path(
     std::shared_ptr<example_srvs::srv::GetPath::Request> const request,
-    Map<unsigned char> const& occupancy_map, PathingGenerator path_generator) {
+    Map<unsigned char> const& occupancy_map, int robot_size,
+    PathingGenerator path_generator) {
   if (occupancy_map.get_data().size() == 0) {
     return tl::unexpected(error::EMPTY_OCCUPANCY_MAP);
   }
@@ -109,7 +114,7 @@ tl::expected<example_srvs::srv::GetPath::Response, error> generate_path(
   auto const goal = Position{request->goal.data[0], request->goal.data[1]};
 
   // Generate the path using the path generator function that was input
-  auto const path = path_generator(start, goal, occupancy_map);
+  auto const path = path_generator(start, goal, occupancy_map, robot_size);
   if (!path.has_value()) {
     return tl::unexpected(error::NO_VALID_PATH);
   }

functional_programming_tests/src/pathing_manager.cpp

@@ -14,8 +14,9 @@ using SetMap = example_srvs::srv::SetMap;
 using GetPath = example_srvs::srv::GetPath;
 
 namespace pathing {
-Manager::Manager(std::unique_ptr<Manager::MiddlewareHandle> mw)
-    : mw_{std::move(mw)} {
+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);
@@ -41,7 +42,8 @@ Manager::Manager(std::unique_ptr<Manager::MiddlewareHandle> mw)
       return generate_path::error_description.at(error);
     };
     *response =
-        generate_path::generate_path(request, this->map_, generate_global_path)
+        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)

functional_programming_tests/src/pathing_node.cpp

@@ -56,7 +56,7 @@ int main(int argc, char** argv) {
 
   rclcpp::init(argc, argv);
   auto const node = std::make_shared<rclcpp::Node>("PathGenerator");
-  pathing::Manager pm{std::make_unique<RosMiddleware>(node)};
+  pathing::Manager pm{std::make_unique<RosMiddleware>(node), {1}};
 
   rclcpp::spin(node);
   rclcpp::shutdown();