Add beluga_amcl and beluga_ros

shared/patch/ros2-beluga-amcl.osx.patch

@@ -0,0 +1,69 @@
+diff --git a/include/beluga_amcl/ndt_amcl_node.hpp b/include/beluga_amcl/ndt_amcl_node.hpp
+index d309a2c..744e75b 100644
+--- a/include/beluga_amcl/ndt_amcl_node.hpp
++++ b/include/beluga_amcl/ndt_amcl_node.hpp
+@@ -98,7 +98,7 @@ using MotionModelVariant =
+     std::variant<beluga::DifferentialDriveModel2d, beluga::StationaryModel, beluga::OmnidirectionalDriveModel>;
+
+ /// Supported execution policies.
+-using ExecutionPolicyVariant = std::variant<std::execution::sequenced_policy, std::execution::parallel_policy>;
++using ExecutionPolicyVariant = std::variant<const std::execution::sequenced_policy*, const std::execution::parallel_policy*>;
+
+ /// 2D NDT AMCL as a ROS 2 composable lifecycle node.
+ class NdtAmclNode : public BaseAMCLNode {
+diff --git a/include/beluga_amcl/ros2_common.hpp b/include/beluga_amcl/ros2_common.hpp
+index d749d83..01f2186 100644
+--- a/include/beluga_amcl/ros2_common.hpp
++++ b/include/beluga_amcl/ros2_common.hpp
+@@ -52,7 +52,7 @@ constexpr std::string_view kNav2DifferentialModelName = "nav2_amcl::Differential
+ /// String identifier for a omnidirectional model name.
+ constexpr std::string_view kNav2OmnidirectionalModelName = "nav2_amcl::OmniMotionModel";
+ /// Supported execution policies.
+-using ExecutionPolicyVariant = std::variant<std::execution::sequenced_policy, std::execution::parallel_policy>;
++using ExecutionPolicyVariant = std::variant<const std::execution::sequenced_policy*, const std::execution::parallel_policy*>;
+
+ /// Base AMCL lifecycle node, with some basic common functionalities, such as transform tree utilities, common
+ /// publishers, subscribers, lifecycle related callbacks and configuration points, enabling extension by inheritance.
+diff --git a/src/ndt_amcl_node.cpp b/src/ndt_amcl_node.cpp
+index bead7e7..f577d10 100644
+--- a/src/ndt_amcl_node.cpp
++++ b/src/ndt_amcl_node.cpp
+@@ -259,7 +259,7 @@ auto NdtAmclNode::make_particle_filter() const -> std::unique_ptr<NdtAmclVariant
+             std::move(random_state_maker),  //
+             std::move(hasher),              //
+             params,                         //
+-            execution_policy);
++            *execution_policy);
+       },
+       get_motion_model(), get_execution_policy());
+   return std::make_unique<NdtAmclVariant>(std::move(amcl));
+diff --git a/src/ndt_amcl_node_3d.cpp b/src/ndt_amcl_node_3d.cpp
+index c14efec..a2ef77c 100644
+--- a/src/ndt_amcl_node_3d.cpp
++++ b/src/ndt_amcl_node_3d.cpp
+@@ -314,7 +314,7 @@ auto NdtAmclNode3D::make_particle_filter() const -> std::unique_ptr<NdtAmclVaria
+             std::move(random_state_maker),  //
+             std::move(hasher),              //
+             params,                         //
+-            execution_policy);
++            *execution_policy);
+       },
+       get_motion_model(), get_execution_policy());
+   return std::make_unique<NdtAmclVariant>(std::move(amcl));
+diff --git a/src/ros2_common.cpp b/src/ros2_common.cpp
+index 0a0cd20..fa4d181 100644
+--- a/src/ros2_common.cpp
++++ b/src/ros2_common.cpp
+@@ -559,10 +559,10 @@ void BaseAMCLNode::autostart_callback() {
+ auto BaseAMCLNode::get_execution_policy() const -> ExecutionPolicyVariant {
+   const auto name = get_parameter("execution_policy").as_string();
+   if (name == "seq") {
+-    return std::execution::seq;
++    return &std::execution::seq;
+   }
+   if (name == "par") {
+-    return std::execution::par;
++    return &std::execution::par;
+   }
+   throw std::invalid_argument("Execution policy must be seq or par.");
+ }

shared/patch/ros2-beluga-amcl.win.patch

@@ -0,0 +1,12 @@
+diff --git a/beluga_amcl/CMakeLists.txt b/beluga_amcl/CMakeLists.txt
+index 85d042e..dafb64f 100644
+--- a/beluga_amcl/CMakeLists.txt
++++ b/beluga_amcl/CMakeLists.txt
+@@ -71,6 +71,7 @@ if(NOT PARANOID)
+     endif()
+   endforeach()
+ endif()
++set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON)
+
+ add_library(beluga_amcl_ros2_common SHARED)
+ target_sources(beluga_amcl_ros2_common PRIVATE src/ros2_common.cpp)

shared/patch/ros2-beluga-ros.osx.patch

@@ -0,0 +1,39 @@
+diff --git a/include/beluga_ros/amcl.hpp b/include/beluga_ros/amcl.hpp
+index 46c612c..872b9fb 100644
+--- a/include/beluga_ros/amcl.hpp
++++ b/include/beluga_ros/amcl.hpp
+@@ -117,7 +117,7 @@ class Amcl {
+       beluga::BeamSensorModel<beluga_ros::OccupancyGrid>>;
+
+   /// Execution policy variant type for runtime selection support.
+-  using execution_policy_variant = std::variant<std::execution::sequenced_policy, std::execution::parallel_policy>;
++  using execution_policy_variant = std::variant<const std::execution::sequenced_policy*, const std::execution::parallel_policy*>;
+
+   /// Constructor.
+   /**
+diff --git a/src/amcl.cpp b/src/amcl.cpp
+index c38360e..b2eee7f 100644
+--- a/src/amcl.cpp
++++ b/src/amcl.cpp
+@@ -30,7 +30,7 @@ Amcl::Amcl(
+     motion_model_variant motion_model,
+     sensor_model_variant sensor_model,
+     const AmclParams& params = AmclParams(),
+-    execution_policy_variant execution_policy = std::execution::seq)
++    execution_policy_variant execution_policy = &std::execution::seq)
+     : params_{params},
+       map_distribution_{map},
+       motion_model_{std::move(motion_model)},
+@@ -95,9 +95,9 @@ auto Amcl::update(
+   std::visit(
+       [&, this](auto& policy, auto& motion_model, auto& sensor_model) {
+         particles_ |=
+-            beluga::actions::propagate(policy, motion_model(control_action_window_ << base_pose_in_odom)) |  //
+-            beluga::actions::reweight(policy, sensor_model(std::move(measurement))) |                        //
+-            beluga::actions::normalize(policy);
++            beluga::actions::propagate(*policy, motion_model(control_action_window_ << base_pose_in_odom)) |  //
++            beluga::actions::reweight(*policy, sensor_model(std::move(measurement))) |                        //
++            beluga::actions::normalize(*policy);
+       },
+       execution_policy_, motion_model_, sensor_model_);
+

shared/patch/ros2-beluga-ros.win.patch

@@ -0,0 +1,12 @@
+diff --git a/beluga_ros/CMakeLists.txt b/beluga_ros/CMakeLists.txt
+index 212972e..8395c83 100644
+--- a/beluga_ros/CMakeLists.txt
++++ b/beluga_ros/CMakeLists.txt
+@@ -73,6 +73,7 @@ if(NOT PARANOID)
+   endforeach()
+ endif()
+
++set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON)
+ add_library(beluga_ros)
+ target_sources(beluga_ros PRIVATE src/amcl.cpp src/ndt_ellipsoid.cpp)
+ target_include_directories(

shared/patch/ros2-beluga.osx.patch

@@ -0,0 +1,195 @@
+diff --git a/CMakeLists.txt b/CMakeLists.txt
+index b94a7f1..cacf183 100644
+--- a/CMakeLists.txt
++++ b/CMakeLists.txt
+@@ -72,6 +72,9 @@ target_include_directories(
+             "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
+ target_link_libraries(${PROJECT_NAME} INTERFACE ${_deps})
+ target_compile_features(${PROJECT_NAME} INTERFACE cxx_std_17)
++# libc++ only has the execution policies with -fexperimental-library
++target_compile_options(${PROJECT_NAME} INTERFACE $<$<CXX_COMPILER_ID:AppleClang,Clang>:-fexperimental-library>)
++target_link_options(${PROJECT_NAME} INTERFACE $<$<CXX_COMPILER_ID:AppleClang,Clang>:-fexperimental-library>)
+ target_compile_definitions(${PROJECT_NAME} INTERFACE EIGEN_NO_DEBUG
+                                                      SOPHUS_USE_BASIC_LOGGING)
+
+diff --git a/include/beluga/actions/normalize.hpp b/include/beluga/actions/normalize.hpp
+index 14c54a5..81f33ca 100644
+--- a/include/beluga/actions/normalize.hpp
++++ b/include/beluga/actions/normalize.hpp
+@@ -42,13 +42,13 @@ struct normalize_closure {
+  public:
+   static_assert(std::is_execution_policy_v<ExecutionPolicy>);
+
+-  constexpr normalize_closure() noexcept : policy_{std::execution::seq} {}
++  constexpr normalize_closure() noexcept : policy_{&std::execution::seq} {}
+
+-  constexpr explicit normalize_closure(ExecutionPolicy policy) : policy_{std::move(policy)} {}
++  constexpr explicit normalize_closure(const ExecutionPolicy& policy) : policy_{&policy} {}
+
+-  constexpr explicit normalize_closure(double factor) noexcept : policy_{std::execution::seq}, factor_{factor} {}
++  constexpr explicit normalize_closure(double factor) noexcept : policy_{&std::execution::seq}, factor_{factor} {}
+
+-  constexpr normalize_closure(ExecutionPolicy policy, double factor) : policy_{std::move(policy)}, factor_{factor} {}
++  constexpr normalize_closure(const ExecutionPolicy& policy, double factor) : policy_{&policy}, factor_{factor} {}
+
+   template <class Range>
+   constexpr auto operator()(Range& range) const -> Range& {
+@@ -75,7 +75,7 @@ struct normalize_closure {
+     }
+
+     std::transform(
+-        policy_,          //
++        *policy_,    //
+         weights.begin(),  //
+         weights.end(),    //
+         weights.begin(),  //
+@@ -85,7 +85,7 @@ struct normalize_closure {
+   }
+
+  private:
+-  ExecutionPolicy policy_{};
++  const ExecutionPolicy* policy_;
+   std::optional<double> factor_;
+ };
+
+diff --git a/include/beluga/actions/overlay.hpp b/include/beluga/actions/overlay.hpp
+index 5bc20c8..e9469d8 100644
+--- a/include/beluga/actions/overlay.hpp
++++ b/include/beluga/actions/overlay.hpp
+@@ -38,10 +38,10 @@ struct overlay_closure {
+   static_assert(ranges::range<MaskRange>);
+
+   constexpr overlay_closure(MaskRange mask_range, MaskValue mask_value)
+-      : policy_{std::execution::seq}, mask_range_{std::move(mask_range)}, mask_value_{std::move(mask_value)} {}
++      : policy_{&std::execution::seq}, mask_range_{std::move(mask_range)}, mask_value_{std::move(mask_value)} {}
+
+-  constexpr explicit overlay_closure(ExecutionPolicy policy, MaskRange mask_range, MaskValue mask_value)
+-      : policy_{std::move(policy)}, mask_range_{std::move(mask_range)}, mask_value_{std::move(mask_value)} {}
++  constexpr explicit overlay_closure(const ExecutionPolicy& policy, MaskRange mask_range, MaskValue mask_value)
++      : policy_{&policy}, mask_range_{std::move(mask_range)}, mask_value_{std::move(mask_value)} {}
+
+   template <class Range>
+   constexpr auto operator()(Range& range) const -> Range& {
+@@ -50,7 +50,7 @@ struct overlay_closure {
+     const auto converted_mask_value = static_cast<ranges::range_value_t<Range>>(mask_value_);
+
+     std::transform(
+-        policy_,                   //
++        *policy_,             //
+         std::begin(common_range),  //
+         std::end(common_range),    //
+         std::begin(mask_range_),   //
+@@ -63,7 +63,7 @@ struct overlay_closure {
+   }
+
+  private:
+-  ExecutionPolicy policy_{};
++  const ExecutionPolicy* policy_;
+   MaskRange mask_range_{};
+   MaskValue mask_value_{};
+ };
+diff --git a/include/beluga/actions/propagate.hpp b/include/beluga/actions/propagate.hpp
+index 7009447..fddf76a 100644
+--- a/include/beluga/actions/propagate.hpp
++++ b/include/beluga/actions/propagate.hpp
+@@ -85,8 +85,9 @@ struct propagate_base_fn {
+       class ExecutionPolicy,
+       std::enable_if_t<ranges::range<Range>, int> = 0,
+       std::enable_if_t<std::is_execution_policy_v<ExecutionPolicy>, int> = 0>
+-  constexpr auto operator()(Range&& range, StateSamplingFunction fn, ExecutionPolicy policy) const -> Range& {
+-    return (*this)(std::move(policy), std::forward<Range>(range), std::move(fn));
++  constexpr auto operator()(Range&& range, StateSamplingFunction fn, const ExecutionPolicy* policy)
++      const -> Range& {
++    return (*this)(*policy, std::forward<Range>(range), std::move(fn));
+   }
+
+   /// Overload that returns a view closure to compose with other views.
+@@ -94,8 +95,8 @@ struct propagate_base_fn {
+       class ExecutionPolicy,        //
+       class StateSamplingFunction,  //
+       std::enable_if_t<std::is_execution_policy_v<ExecutionPolicy>, int> = 0>
+-  constexpr auto operator()(ExecutionPolicy policy, StateSamplingFunction fn) const {
+-    return ranges::make_action_closure(ranges::bind_back(propagate_base_fn{}, std::move(fn), std::move(policy)));
++  constexpr auto operator()(const ExecutionPolicy& policy, StateSamplingFunction fn) const {
++    return ranges::make_action_closure(ranges::bind_back(propagate_base_fn{}, std::move(fn), &policy));
+   }
+ };
+
+diff --git a/include/beluga/actions/reweight.hpp b/include/beluga/actions/reweight.hpp
+index 6bda73b..3428d0a 100644
+--- a/include/beluga/actions/reweight.hpp
++++ b/include/beluga/actions/reweight.hpp
+@@ -136,8 +136,15 @@ struct reweight_fn {
+       class Input,
+       std::enable_if_t<std::is_execution_policy_v<std::decay_t<ExecutionPolicy>>, int> = 0,
+       std::enable_if_t<!ranges::range<std::decay_t<ExecutionPolicy>>, int> = 0>
+-  constexpr auto operator()(ExecutionPolicy policy, Input input) const {
+-    return ranges::make_action_closure(ranges::bind_back(reweight_fn{}, std::move(input), std::move(policy)));
++  constexpr auto operator()(const ExecutionPolicy& policy, Input input) const {
++    return ranges::make_action_closure(ranges::bind_back(reweight_fn{}, std::move(input), &policy));
++  }
++
++  /// Overload for the policy a view closure keeps by reference.
++  template <class Range, class Input, class ExecutionPolicy, std::enable_if_t<ranges::range<Range>, int> = 0>
++  constexpr auto operator()(Range&& range, Input&& input, const ExecutionPolicy* policy) const
++      -> auto& {
++    return (*this)(*policy, range, std::forward<Input>(input));
+   }
+
+   /// Overload that returns a view closure to compose with other views using the default execution policy.
+diff --git a/include/beluga/algorithm/amcl_core.hpp b/include/beluga/algorithm/amcl_core.hpp
+index 63f6ac1..6cba805 100644
+--- a/include/beluga/algorithm/amcl_core.hpp
++++ b/include/beluga/algorithm/amcl_core.hpp
+@@ -108,11 +108,11 @@ class Amcl {
+       RandomStateGenerator random_state_generator,
+       spatial_hasher_type spatial_hasher,
+       const AmclParams& params = AmclParams{},
+-      ExecutionPolicy execution_policy = std::execution::seq)
++      const ExecutionPolicy& execution_policy = std::execution::seq)
+       : params_{params},
+         motion_model_{std::move(motion_model)},
+         sensor_model_{std::move(sensor_model)},
+-        execution_policy_{std::move(execution_policy)},
++        execution_policy_{&execution_policy},
+         spatial_hasher_{std::move(spatial_hasher)},
+         random_probability_estimator_{params_.alpha_slow, params_.alpha_fast},
+         update_policy_{beluga::policies::on_motion<state_type>(params_.update_min_d, params_.update_min_a)},
+@@ -172,9 +172,9 @@ class Amcl {
+     }
+
+     particles_ |= beluga::actions::propagate(
+-                      execution_policy_, motion_model_(control_action_window_ << std::move(control_action))) |  //
+-                  beluga::actions::reweight(execution_policy_, sensor_model_(std::move(measurement))) |         //
+-                  beluga::actions::normalize(execution_policy_);
++                      *execution_policy_, motion_model_(control_action_window_ << std::move(control_action))) |  //
++                  beluga::actions::reweight(*execution_policy_, sensor_model_(std::move(measurement))) |         //
++                  beluga::actions::normalize(*execution_policy_);
+
+     const double random_state_probability = random_probability_estimator_(particles_);
+
+@@ -218,7 +218,7 @@ class Amcl {
+
+   MotionModel motion_model_;
+   SensorModel sensor_model_;
+-  ExecutionPolicy execution_policy_;
++  const ExecutionPolicy* execution_policy_;
+
+   spatial_hasher_type spatial_hasher_;
+   beluga::ThrunRecoveryProbabilityEstimator random_probability_estimator_;
+diff --git a/include/beluga/algorithm/spatial_hash.hpp b/include/beluga/algorithm/spatial_hash.hpp
+index 9c6c6ba..a3d0a8a 100644
+--- a/include/beluga/algorithm/spatial_hash.hpp
++++ b/include/beluga/algorithm/spatial_hash.hpp
+@@ -44,9 +44,9 @@ namespace detail {
+ template <std::size_t N, std::size_t I>
+ constexpr std::size_t floor_and_fibo_hash(double value) {
+   constexpr auto kFib = []() {
+-    if constexpr (std::is_same_v<std::size_t, std::uint64_t>) {
++    if constexpr (sizeof(std::size_t) == sizeof(std::uint64_t)) {
+       return 11400714819323198485LLU;  // golden ratio for 64 bits
+-    } else if constexpr (std::is_same_v<std::size_t, std::uint32_t>) {
++    } else if constexpr (sizeof(std::size_t) == sizeof(std::uint32_t)) {
+       return 2654435769U;  // golden ratio for 32 bits
+     } else {
+       // Write false in a sufficiently complex way so as to confuse the compiler.