Add functional programming example (#16)

* Initial commit of functional programming example

* Code refactoring

* Updated file structure

* Added README and examples

* Added const correctness

* Address some PR comments

* Updates per PR comments

example_srvs/CMakeLists.txt

@@ -0,0 +1,20 @@
+cmake_minimum_required(VERSION 3.8)
+project(example_srvs)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+
+find_package(std_msgs REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+  "srv/GetPath.srv"
+  "srv/SetMap.srv"
+  DEPENDENCIES std_msgs
+)
+
+ament_package()

example_srvs/package.xml

@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>example_srvs</name>
+  <version>0.0.0</version>
+  <description>Services used in the ros_testing_templates examples </description>
+  <maintainer email="[email protected]">Bilal Gill</maintainer>
+  <license>MIT</license>
+
+  <depend>std_msgs</depend>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>rosidl_default_generators</buildtool_depend>
+
+  <exec_depend>rosidl_default_runtime</exec_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

example_srvs/srv/GetPath.srv

@@ -0,0 +1,5 @@
+std_msgs/UInt8MultiArray start
+std_msgs/UInt8MultiArray goal
+---
+std_msgs/Bool success
+std_msgs/UInt8MultiArray path

example_srvs/srv/SetMap.srv

@@ -0,0 +1,3 @@
+std_msgs/UInt8MultiArray map
+---
+std_msgs/Bool success

functional_programming_tests/CMakeLists.txt

@@ -0,0 +1,23 @@
+cmake_minimum_required(VERSION 3.16.3)
+project(functional_programming_tests)
+set(CMAKE_CXX_STANDARD 20)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake REQUIRED)
+find_package(example_srvs REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(std_msgs REQUIRED)
+
+if(BUILD_TESTING)
+  find_package(ament_cmake_gtest REQUIRED)
+  ament_add_gtest(with_functional_programming test/with_functional_programming.cpp TIMEOUT 5)
+  ament_target_dependencies(with_functional_programming rclcpp std_msgs)
+
+  ament_add_gtest(without_functional_programming test/without_functional_programming.cpp TIMEOUT 5)
+  ament_target_dependencies(without_functional_programming rclcpp std_msgs example_srvs)
+endif()
+
+ament_package()

functional_programming_tests/README.md

@@ -0,0 +1,55 @@
+# functional_programming_tests
+
+This directory contains examples of tests that do and do not follow functional programming principles. When working with ROS, it is tempting to follow the established paradigm of encapsulating code and associated data in classes that inherit from or are composed of `rclcpp::Node`.
+
+```
+class MinimalPublisher : public rclcpp::Node
+// ... class definition
+int main(int argc, char * argv[])
+{
+  rclcpp::init(argc, argv);
+  rclcpp::spin(std::make_shared<MinimalPublisher>());
+  rclcpp::shutdown();
+  return 0;
+}
+```
+
+This usually leads to the code being dependent on the ROS ecosystem to function, even when it is not needed. In `without_functional_programming.cpp`, there is class method that requires testing, `generate_global_path`.
+
+```
+class PathGenerator {
+public:
+// ... some code
+    std::optional<Path> generate_global_path(Pos const& start, Pos const& goal);
+// ... more code
+};
+```
+
+To test the method properly, an entire ROS2 pipeline must be spun up.
+
+```
+class TaskPlanningFixture : public testing::Test {
+public:
+	// Adapted from minimal_integration_test
+	TaskPlanningFixture() : node_(std::make_shared<rclcpp::Node>("bad_test_publisher")), executor_(std::make_shared<rclcpp::executors::SingleThreadedExecutor>()) {
+        // Create ROS2 publisher for the costmap
+		costmap_publisher_ = node_->create_publisher<std_msgs::msg::UInt8MultiArray>("test/costmap",1);
+
+        // Publish the costmap every 100 ms
+		timer_ = node_->create_wall_timer(100ms, std::bind(&TaskPlanningFixture::publish_costmap, this));
+	}
+
+	void SetUp() override {
+		executor_->add_node(node_);
+		executor_thread_ = std::thread([this]() { executor_->spin(); });
+	}
+
+    // Cleanup actions that could throw an exception
+	void TearDown() override {
+		executor_->cancel();
+		executor_thread_.join();
+	}
+// ... more code
+```
+
+Writing all this extra code can be time consuming and lead to bugs in testing. In `with_functional_programming.cpp`, the `generate_global_path` method is now a free function, and the `costmap_callback` and `get_costmap` functions aren't needed anymore. This leads to less boilerplate code required to test `generate_global_path`.

functional_programming_tests/package.xml

@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>functional_programming_tests</name>
+  <version>0.0.1</version>
+  <description>Various examples of ROS tests using functional programming</description>
+  <maintainer email="[email protected]">Bilal Gill</maintainer>
+  <license>MIT</license>
+  <author>Bilal Gill</author>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>example_srvs</depend>
+  <depend>rclcpp</depend>
+  <depend>std_msgs</depend>
+
+  <test_depend>ament_cmake_gtest</test_depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+  <test_depend>ros_testing</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

functional_programming_tests/test/with_functional_programming.cpp

@@ -0,0 +1,149 @@
+#include <gtest/gtest.h>
+
+#include <iostream>
+#include <vector>
+#include <cmath>
+#include <optional>
+
+// production_code.h/cc
+// X, Y position
+struct Position {
+    size_t x;
+    size_t y;
+};
+
+template<typename T>
+class Map
+{
+public:
+    Map(std::vector<std::vector<T>> data): data_{data} {};
+    T at(Position const& pos) const {
+        return data_.at(pos.y).at(pos.x);
+    }
+private:
+    std::vector<std::vector<T>> data_;
+};
+
+using Path = std::vector<Position>;
+
+// Operator overload to print path
+std::ostream& operator<<(std::ostream& os, std::vector<Position> const& path) {
+    for (auto const& pose:path) {
+        os << "(" << pose.x << ", " << pose.y << ")\n";
+    }
+    return os;
+}
+
+// Operator overload for position comparison
+bool operator==(Position const& lhs, Position const& rhs) {
+    return lhs.x == rhs.x && lhs.y == rhs.y;
+}
+
+// From the Robot, goal and costmap, generate a trajectory (Deterministic calculation)
+std::optional<Path> generate_global_path(Position const& start, Position const& goal , Map<unsigned char> const& costmap) { // Calculation
+    // Some cool and nifty algorithm
+    // What is the delta in position
+    int const del_x = goal.x - start.x;
+    int const del_y = goal.y - start.y;
+
+    // What direction to move in for each dimension
+    int const del_x_sign = std::copysign(1.0, del_x);
+    int const del_y_sign = std::copysign(1.0, del_y);
+
+    // Push start onto the path
+    Path path;
+    path.push_back(start);
+
+    auto is_occupied = [&costmap](auto const x, auto const y) -> bool {
+        return costmap.at(Position{x, y}) == 1;
+    };
+
+    // Fails if there is any obstacle in the way
+    // Move horizontally
+    for (size_t i = 0; i < (std::abs(del_x)); ++i) {
+        if (is_occupied(path.back().x + del_x_sign, path.back().y)) {        
+            return std::nullopt;
+        }        
+        path.push_back({path.back().x + del_x_sign, path.back().y});
+    }
+    // Move vertically
+    for (size_t i = 0; i < (std::abs(del_y)); i++) {
+        // if (costmap.at(Position{path.back().x, path.back().y + del_y_sign}) == 1) {
+        if (is_occupied(path.back().x, path.back().y + del_y_sign)) {            
+            return std::nullopt;
+        }            
+        path.push_back({path.back().x, path.back().y + del_y_sign});
+    }
+
+    return path;
+}
+
+// test.cc
+
+/**
+ * @brief      Gets the test costmap.
+ *
+ * @return     The test costmap.
+ */
+Map<unsigned char> get_test_costmap() {
+    return {{{0, 0, 0, 0, 0, 0, 0, 0},
+             {0, 0, 0, 1, 0, 0, 0, 0}, 
+             {0, 0, 0, 1, 0, 0, 0, 0}, 
+             {0, 0, 1, 1, 1, 0, 0, 0}, 
+             {0, 0, 1, 0, 1, 1, 0, 0}, 
+             {0, 0, 1, 0, 0, 0, 0, 0}, 
+             {0, 0, 0, 0, 0, 0, 0, 0}, 
+             {0, 0, 0, 0, 0, 0, 0, 0}}};
+}
+
+TEST(generate_path, same_start_and_goal) {
+
+    // GIVEN a costmap and the same start and goal
+    auto const sample_costmap = get_test_costmap();
+
+    Position const start {0, 0};
+    Position const goal {0, 0};
+
+    // WHEN the global path is produced
+    auto const& path = generate_global_path(start, goal, sample_costmap);    
+
+    // THEN the path should have one element, which is the start/goal position
+    std::vector<Position> expected {{0, 0}};
+    EXPECT_EQ(path.value(), expected) << path.value();
+}
+
+TEST(generate_path, no_path) {
+
+    // GIVEN a costmap AND a start and goal position
+    auto const sample_costmap = get_test_costmap();
+
+    Position const start {2, 2};
+    Position const goal {5, 5};
+
+    // WHEN the global path is produced
+    auto const& path = generate_global_path(start, goal, sample_costmap);    
+
+    // THEN the path should not have been generated
+    EXPECT_FALSE(path.has_value()) << path.value();
+}
+
+TEST(generate_path, path_generated) {
+
+    // GIVEN a costmap AND a start and goal position
+    auto const sample_costmap = get_test_costmap();
+
+    Position const start {0, 0};
+    Position const goal {7, 7};
+
+    // WHEN the global path is produced
+    auto const& path = generate_global_path(start, goal, sample_costmap);    
+
+    // THEN the path should have a valid path from start to the goal
+    std::vector<Position> expected {{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}};
+    EXPECT_EQ(path.value(), expected) << path.value();
+}
+
+int main(int argc, char** argv) {
+    ::testing::InitGoogleTest(&argc, argv);
+    return RUN_ALL_TESTS();
+}