More functional tools for parameters (TAMS-Group#17)

Signed-off-by: Tyler Weaver <[email protected]>

.github/workflows/ci.yaml

@@ -21,8 +21,9 @@ jobs:
             ROS_REPO: testing
             TARGET_CMAKE_ARGS: -DENABLE_COVERAGE=ON
             CCOV: true
-          - NAME: "rolling-main"
-            ROS_REPO: main
+          # TODO: re-enable testing on rolling-main once ros2_control issue is fixed
+          # - NAME: "rolling-main"
+          #   ROS_REPO: main
           - NAME: "address-leak-ub-sanitizers"
             TARGET_CMAKE_ARGS: >
               -DCMAKE_BUILD_TYPE=Debug

include/bio_ik/ik_evolution_1.hpp

@@ -26,6 +26,8 @@
 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 // POSSIBILITY OF SUCH DAMAGE.
 
+#pragma once
+
 #include <memory>
 #include <optional>
 #include <set>
@@ -38,6 +40,6 @@ namespace bio_ik {
 std::optional<std::unique_ptr<IKSolver>> makeEvolution1Solver(
     const IKParams& params);
 
-std::set<std::string> getEvolution1ModeSet();
+const auto getEvolution1Modes = []() { return std::set<std::string>{"bio1"}; };
 
 }  // namespace bio_ik

include/bio_ik/ik_evolution_2.hpp

@@ -26,6 +26,8 @@
 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 // POSSIBILITY OF SUCH DAMAGE.
 
+#pragma once
+
 #include <memory>
 #include <optional>
 #include <set>
@@ -38,6 +40,8 @@ namespace bio_ik {
 std::optional<std::unique_ptr<IKSolver>> makeEvolution2Solver(
     const IKParams& params);
 
-std::set<std::string> getEvolution2ModeSet();
+const auto getEvolution2Modes = []() {
+  return std::set<std::string>{"bio2", "bio2_memetic", "bio2_memetic_l"};
+};
 
 }  // namespace bio_ik

include/bio_ik/ik_gradient.hpp

@@ -26,6 +26,8 @@
 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 // POSSIBILITY OF SUCH DAMAGE.
 
+#pragma once
+
 #include <Eigen/Core>          // For NumTraits
 #include <bio_ik/ik_base.hpp>  // for IKSolver
 #include <bio_ik/problem.hpp>  // for Problem, Problem::GoalInfo
@@ -106,6 +108,13 @@ struct IKJacobian : IKJacobianBase<IKSolver> {
 
 std::optional<std::unique_ptr<IKSolver>> makeGradientDecentSolver(
     const IKParams& params);
-std::set<std::string> getGradientDecentModeSet();
+
+const auto getGradientDecentModes = []() {
+  return std::set<std::string>{
+      "gd",     "gd_2",   "gd_4",  "gd_8",   "gd_r",   "gd_r_2",
+      "gd_r_4", "gd_r_8", "gd_c",  "gd_c_2", "gd_c_4", "gd_c_8",
+      "jac",    "jac_2",  "jac_4", "jac_8",
+  };
+};
 
 }  // namespace bio_ik

include/bio_ik/ik_test.hpp

@@ -26,6 +26,8 @@
 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 // POSSIBILITY OF SUCH DAMAGE.
 
+#pragma once
+
 #include <memory>
 #include <optional>
 #include <set>
@@ -36,6 +38,7 @@
 namespace bio_ik {
 
 std::optional<std::unique_ptr<IKSolver>> makeTestSolver(const IKParams& params);
-std::set<std::string> getTestModeSet();
+
+const auto getTestModes = []() { return std::set<std::string>{"test"}; };
 
 }  // namespace bio_ik

include/bio_ik/parameters.hpp

@@ -36,14 +36,14 @@
 #include <rclcpp/rclcpp.hpp>
 #include <string>
 
-#include "bio_ik/status_or.hpp"
+#include "bio_ik/util/result.hpp"
 
 namespace bio_ik {
 
 /**
  * @brief      Parameters settable via ros.
  */
-struct RosParameters {
+struct [[nodiscard]] RosParameters {
   // Plugin parameters
   bool enable_profiler = false;
 
@@ -65,9 +65,10 @@ struct RosParameters {
 
   inline operator std::string() const {
     return fmt::format(
-        "[RosParameters: enable_profiler={}, mode={}, enable_counter={}, "
-        "random_seed={}, dpos={}, drot={}, dtwist={}, skip_wipeout={}, "
-        "population_size={}, elite_count={}, enable_linear_fitness={}]",
+        "[bio_ik::RosParameters:\n  enable_profiler={},\n  mode={},\n  "
+        "enable_counter={},\n  random_seed={},\n  dpos={},\n  drot={},\n  "
+        "dtwist={},\n  skip_wipeout={},\n  population_size={},\n  "
+        "elite_count={},\n  enable_linear_fitness={},\n]",
         enable_profiler, mode, enable_counter, random_seed, dpos, drot, dtwist,
         skip_wipeout, population_size, elite_count, enable_linear_fitness);
   }
@@ -78,9 +79,9 @@ struct RosParameters {
  *
  * @param[in]  ros_params  The ros parameters struct
  *
- * @return     error string if invalid, ok if valid
+ * @return     The ros parameters on success, error status otherwise
  */
-Status validate(const RosParameters& ros_params);
+[[nodiscard]] Result<RosParameters> validate(const RosParameters& ros_params);
 
 /**
  * @brief      Gets the ros parameters
@@ -89,6 +90,7 @@ Status validate(const RosParameters& ros_params);
  *
  * @return     The ros parameters on success, error status otherwise
  */
-StatusOr<RosParameters> get_ros_parameters(const rclcpp::Node::SharedPtr& node);
+[[nodiscard]] Result<RosParameters> get_ros_parameters(
+    const rclcpp::Node::SharedPtr& node);
 
 }  // namespace bio_ik

include/bio_ik/util/curried.hpp

@@ -0,0 +1,75 @@
+// Copyright (c) 2022, Tyler Weaver
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//    * Redistributions of source code must retain the above copyright
+//      notice, this list of conditions and the following disclaimer.
+//
+//    * Redistributions in binary form must reproduce the above copyright
+//      notice, this list of conditions and the following disclaimer in the
+//      documentation and/or other materials provided with the distribution.
+//
+//    * Neither the name of the Universität Hamburg nor the names of its
+//      contributors may be used to endorse or promote products derived from
+//      this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+#include <tuple>
+
+/**
+ * @brief      Curried template copied from
+ *             https://gitlab.com/manning-fpcpp-book/code-examples/-/blob/master/chapter-11/make-curried/main.cpp
+ *
+ * @tparam     Function      The function to curry
+ * @tparam     CapturedArgs  The args to capture
+ */
+template <typename Function, typename... CapturedArgs>
+class Curried {
+ private:
+  using CapturedArgsTuple = std::tuple<std::decay_t<CapturedArgs>...>;
+
+  template <typename... Args>
+  static auto capture_by_copy(Args&&... args) {
+    return std::tuple<std::decay_t<Args>...>(std::forward<Args>(args)...);
+  }
+
+ public:
+  Curried(Function function, CapturedArgs... args)
+      : m_function(function), m_captured(capture_by_copy(std::move(args)...)) {}
+
+  Curried(Function function, std::tuple<CapturedArgs...> args)
+      : m_function(function), m_captured(std::move(args)) {}
+
+  template <typename... NewArgs>
+  auto operator()(NewArgs&&... args) const {
+    auto new_args = capture_by_copy(std::forward<NewArgs>(args)...);
+
+    auto all_args = std::tuple_cat(m_captured, std::move(new_args));
+
+    if constexpr (std::is_invocable_v<Function, CapturedArgs..., NewArgs...>) {
+      return std::apply(m_function, all_args);
+
+    } else {
+      return Curried<Function, CapturedArgs..., NewArgs...>(m_function,
+                                                            all_args);
+    }
+  }
+
+ private:
+  Function m_function;
+  std::tuple<CapturedArgs...> m_captured;
+};

include/bio_ik/status_or.hpp → include/bio_ik/util/no_discard.hpp

@@ -28,33 +28,21 @@
 
 #pragma once
 
-#include <string>
-#include <tuple>
-
-#include "_external/expected.hpp"
-
-namespace bio_ik {
-
-enum class Error { OK, ERROR };
-
-struct Status {
-  Error value = Error::OK;
-  std::string what = "";
-
-  Status() = default;
-  Status(const std::string& error_string)
-      : value(Error::ERROR), what(error_string) {}
-
-  inline operator bool() const noexcept { return value == Error::OK; }
-  inline bool operator==(const Status& other) {
-    return std::tie(value, what) == std::tie(other.value, other.what);
+/**
+ * @brief      Template for creating lambdas with the nodiscard attribute
+ *
+ * @tparam     F     The lambda
+ *
+ * @example    auto f = NoDiscard([]{ return Error{ErrorCode::UNKNOWN,
+ * "yikes!"}; });
+ */
+template <typename F>
+struct NoDiscard {
+  F f_;
+  NoDiscard(F const& f) : f_(f) {}
+  template <typename... T>
+  [[nodiscard]] constexpr auto operator()(T&&... t) noexcept(
+      noexcept(f(std::forward<T>(t)...))) {
+    return f_(std::forward<T>(t)...);
   }
 };
-auto OKStatus = [] { return Status(); };
-
-using tl::expected;
-using tl::unexpected;
-template <typename T>
-using StatusOr = expected<T, Status>;
-
-}  // namespace bio_ik

include/bio_ik/util/parameter_loader.hpp

@@ -0,0 +1,92 @@
+// Copyright (c) 2022, Tyler Weaver
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//    * Redistributions of source code must retain the above copyright
+//      notice, this list of conditions and the following disclaimer.
+//
+//    * Redistributions in binary form must reproduce the above copyright
+//      notice, this list of conditions and the following disclaimer in the
+//      documentation and/or other materials provided with the distribution.
+//
+//    * Neither the name of the Universität Hamburg nor the names of its
+//      contributors may be used to endorse or promote products derived from
+//      this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+#include <rcl_interfaces/msg/parameter_descriptor.hpp>
+#include <rcl_interfaces/msg/parameter_type.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <string>
+
+/**
+ * @brief      Callable that declares and gets parameters using rclcpp.
+ */
+class ParameterLoader {
+  rclcpp::Node::SharedPtr node_;
+
+  [[nodiscard]] rcl_interfaces::msg::ParameterDescriptor make_descriptor(
+      const std::string& description, const std::string& constraints) const {
+    rcl_interfaces::msg::ParameterDescriptor msg;
+    msg.description = description;
+    msg.additional_constraints = constraints;
+    return msg;
+  }
+
+  template <typename T>
+  [[nodiscard]] Result<T> declare_parameter(
+      const std::string& name, const T& default_value,
+      const std::string& description = "",
+      const std::string& constraints = "") const {
+    try {
+      return node_
+          ->declare_parameter(name, rclcpp::ParameterValue{default_value},
+                              make_descriptor(description, constraints))
+          .get<T>();
+    } catch (const std::exception& ex) {
+      return Exception(ex.what());
+    }
+  }
+
+  template <typename T>
+  [[nodiscard]] Result<T> get_parameter(const std::string& name) const {
+    try {
+      return node_->get_parameter(name).get_value<T>();
+    } catch (const std::exception& ex) {
+      return Exception(ex.what());
+    }
+  }
+
+ public:
+  ParameterLoader(const rclcpp::Node::SharedPtr& node) : node_{node} {}
+
+  template <typename T>
+  [[nodiscard]] Result<T> operator()(
+      const std::string& name, const T& default_value,
+      const std::string& description = "",
+      const std::string& constraints = "") const {
+    if (!node_->has_parameter(name)) {
+      if (const auto result = declare_parameter<T>(name, default_value,
+                                                   description, constraints);
+          !result) {
+        return make_unexpected(result.error());
+      }
+    }
+
+    return get_parameter<T>(name);
+  }
+};