diff --git a/shared/patch/ros2-beluga-amcl.osx.patch b/shared/patch/ros2-beluga-amcl.osx.patch new file mode 100644 index 000000000..b4f62b672 --- /dev/null +++ b/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; + + /// Supported execution policies. +-using ExecutionPolicyVariant = std::variant; ++using ExecutionPolicyVariant = std::variant; + + /// 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; ++using ExecutionPolicyVariant = std::variant; + + /// 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(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(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."); + } diff --git a/shared/patch/ros2-beluga-amcl.win.patch b/shared/patch/ros2-beluga-amcl.win.patch new file mode 100644 index 000000000..abb9a980b --- /dev/null +++ b/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) \ No newline at end of file diff --git a/shared/patch/ros2-beluga-ros.osx.patch b/shared/patch/ros2-beluga-ros.osx.patch new file mode 100644 index 000000000..9ef813be4 --- /dev/null +++ b/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>; + + /// Execution policy variant type for runtime selection support. +- using execution_policy_variant = std::variant; ++ using execution_policy_variant = std::variant; + + /// 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_); + diff --git a/shared/patch/ros2-beluga-ros.win.patch b/shared/patch/ros2-beluga-ros.win.patch new file mode 100644 index 000000000..cb36c9627 --- /dev/null +++ b/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( diff --git a/shared/patch/ros2-beluga.osx.patch b/shared/patch/ros2-beluga.osx.patch new file mode 100644 index 000000000..fbba71b6a --- /dev/null +++ b/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( + "$") + 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 $<$:-fexperimental-library>) ++target_link_options(${PROJECT_NAME} INTERFACE $<$:-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); + +- 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 + 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 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); + + 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 + constexpr auto operator()(Range& range) const -> Range& { +@@ -50,7 +50,7 @@ struct overlay_closure { + const auto converted_mask_value = static_cast>(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, int> = 0, + std::enable_if_t, int> = 0> +- constexpr auto operator()(Range&& range, StateSamplingFunction fn, ExecutionPolicy policy) const -> Range& { +- return (*this)(std::move(policy), std::forward(range), std::move(fn)); ++ constexpr auto operator()(Range&& range, StateSamplingFunction fn, const ExecutionPolicy* policy) ++ const -> Range& { ++ return (*this)(*policy, std::forward(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, 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>, int> = 0, + std::enable_if_t>, 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 , int> = 0> ++ constexpr auto operator()(Range&& range, Input&& input, const ExecutionPolicy* policy) const ++ -> auto& { ++ return (*this)(*policy, range, std::forward(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(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 + constexpr std::size_t floor_and_fibo_hash(double value) { + constexpr auto kFib = []() { +- if constexpr (std::is_same_v) { ++ if constexpr (sizeof(std::size_t) == sizeof(std::uint64_t)) { + return 11400714819323198485LLU; // golden ratio for 64 bits +- } else if constexpr (std::is_same_v) { ++ } 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. diff --git a/shared/patch/ros2-beluga.patch b/shared/patch/ros2-beluga.patch new file mode 100644 index 000000000..779429568 --- /dev/null +++ b/shared/patch/ros2-beluga.patch @@ -0,0 +1,66 @@ +diff --git a/CMakeLists.txt b/CMakeLists.txt +index 5c08a64..b94a7f1 100644 +--- a/CMakeLists.txt ++++ b/CMakeLists.txt +@@ -94,7 +94,7 @@ target_sources(clang_tidy_findable PRIVATE src/clang_tidy_findable.cpp) + target_link_libraries(clang_tidy_findable PRIVATE ${PROJECT_NAME} + beluga_compile_options) + +-option(BUILD_TESTING "Build the testing tree." ON) ++option(BUILD_TESTING "Build the testing tree." OFF) + if(BUILD_TESTING) + message(STATUS "Build testing enabled.") + enable_testing() +diff --git a/include/beluga/policies/on_motion.hpp b/include/beluga/policies/on_motion.hpp +index 9d7ad43..5cb6d95 100644 +--- a/include/beluga/policies/on_motion.hpp ++++ b/include/beluga/policies/on_motion.hpp +@@ -15,7 +15,7 @@ + #ifndef BELUGA_POLICIES_ON_MOTION_HPP + #define BELUGA_POLICIES_ON_MOTION_HPP + +-#include ++#include + #include + #include + #include +@@ -119,12 +119,13 @@ struct on_motion_policy : public on_motion_policy_base { + + /// Return true if motion has been detected. + constexpr bool operator()(const Pose& pose) { +- if (!latest_pose_) { ++ if (!has_latest_pose_) { + latest_pose_ = pose; ++ has_latest_pose_ = true; + return true; + } + +- const bool moved = (*this)(*latest_pose_, pose); ++ const bool moved = (*this)(latest_pose_, pose); + if (moved) { + latest_pose_ = pose; + } +@@ -133,7 +134,9 @@ struct on_motion_policy : public on_motion_policy_base { + } + + private: +- std::optional latest_pose_; ///< The latest pose for motion comparison. ++ // a flag instead of std::optional: GCC 15 fails to instantiate std::optional ++ bool has_latest_pose_{false}; ///< Whether latest_pose_ holds a pose. ++ Pose latest_pose_{}; ///< The latest pose for motion comparison. + }; + + /// Implementation detail for the on_motion_fn object. +diff --git a/include/beluga/sensor/ndt_sensor_model.hpp b/include/beluga/sensor/ndt_sensor_model.hpp +index 21d4d6f..35aff49 100644 +--- a/include/beluga/sensor/ndt_sensor_model.hpp ++++ b/include/beluga/sensor/ndt_sensor_model.hpp +@@ -271,7 +271,7 @@ NDTMapRepresentationT load_from_hdf5(const std::filesystem::path& path_to_hdf5_f + throw std::invalid_argument(ss.str()); + } + +- const H5::H5File file(path_to_hdf5_file, H5F_ACC_RDONLY); ++ const H5::H5File file(path_to_hdf5_file.string(), H5F_ACC_RDONLY); + const H5::DataSet means_dataset = file.openDataSet("means"); + const H5::DataSet covariances_dataset = file.openDataSet("covariances"); + const H5::DataSet resolution_dataset = file.openDataSet("resolution"); \ No newline at end of file diff --git a/shared/vinca.yaml b/shared/vinca.yaml index c8c0c9bfe..08a7c5d53 100644 --- a/shared/vinca.yaml +++ b/shared/vinca.yaml @@ -94,6 +94,8 @@ packages_select_by_deps: - battery_state_broadcaster - behaviortree_cpp - behaviortree_cpp_v3 + - beluga_amcl # requested in https://github.com/RoboStack/ros-jazzy/pull/298; macOS: ros2-beluga*.osx.patch (libc++) + - beluga_ros # requested in https://github.com/RoboStack/ros-jazzy/pull/298 - better_launch - bno055 - bond_core