Add NDT AMCL 3D node.

Signed-off-by: Ramiro Serra <[email protected]>

beluga/include/beluga/algorithm/amcl_core.hpp

@@ -26,6 +26,7 @@
 #include <range/v3/range/concepts.hpp>
 #include <range/v3/view/any_view.hpp>
 #include <range/v3/view/take_exactly.hpp>
+#include "beluga/policies/on_motion.hpp"
 
 namespace beluga {
 
@@ -115,7 +116,7 @@ class Amcl {
         execution_policy_{std::move(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)},
+        update_policy_{beluga::policies::on_motion<state_type>(params_.update_min_d, params_.update_min_a)},
         resample_policy_{beluga::policies::every_n(params_.resample_interval)},
         random_state_generator_(std::move(random_state_generator)) {
     if (params_.selective_resampling) {

beluga/include/beluga/policies/on_motion.hpp

@@ -17,6 +17,7 @@
 
 #include <beluga/policies/policy.hpp>
 #include <sophus/se2.hpp>
+#include <sophus/se3.hpp>
 
 /**
  * \file
@@ -70,6 +71,44 @@ struct on_motion_policy_base<Sophus::SE2<Scalar>> {
   Scalar min_angle_{0.0};     ///< The minimum rotation angle (in radians) to trigger the action.
 };
 
+/// Specialization for on_motion_policy_base in SE3 space.
+/**
+ * \tparam Scalar The scalar type for SE3 elements.
+ */
+template <class Scalar>
+struct on_motion_policy_base<Sophus::SE3<Scalar>> {
+ public:
+  /// Constructor.
+  /**
+   * \param min_distance The minimum translation distance to trigger the action.
+   * \param min_angle The minimum rotation angle (in radians) to trigger the action.
+   */
+  constexpr on_motion_policy_base(Scalar min_distance, Scalar min_angle)
+      : min_distance_(min_distance), min_angle_(min_angle) {}
+
+  /// Return true if motion has been detected.
+  /**
+   * \param prev The previous SE3 pose to check for motion.
+   * \param current The current SE3 pose to check for motion.
+   * \return True if the action should be triggered based on motion, false otherwise.
+   *
+   * Checks the motion based on the provided SE3 pose, and triggers the action if the
+   * motion surpasses the specified distance and angle thresholds.
+   */
+  constexpr bool operator()(const Sophus::SE3<Scalar>& prev, const Sophus::SE3<Scalar>& current) {
+    const auto delta = prev.inverse() * current;
+    return std::abs(delta.translation().x()) > min_distance_ ||  //
+           std::abs(delta.translation().y()) > min_distance_ ||  //
+           std::abs(delta.translation().z()) > min_distance_ ||  //
+           std::abs(delta.angleX()) > min_angle_ || std::abs(delta.angleY()) > min_angle_ ||
+           std::abs(delta.angleZ()) > min_angle_;
+  }
+
+ private:
+  Scalar min_distance_{0.0};  ///< The minimum translation distance to trigger the action.
+  Scalar min_angle_{0.0};     ///< The minimum rotation angle (in radians) to trigger the action.
+};
+
 /// Base implementation for the on_motion_policy algorithm.
 /**
  * \tparam Pose The pose type to check for motion.
@@ -100,14 +139,14 @@ struct on_motion_policy : public on_motion_policy_base<Pose> {
 };
 
 /// Implementation detail for the on_motion_fn object.
+template <class StateType = Sophus::SE2d>
 struct on_motion_fn {
-  /// Overload that creates the policy closure in SE2 space.
+  /// Overload that creates the policy closure in the state space.
   /**
    * This policy triggers an action based on motion.
    */
-  template <class Scalar>
-  constexpr auto operator()(Scalar min_distance, Scalar min_angle) const {
-    return beluga::make_policy(on_motion_policy<Sophus::SE2<Scalar>>{min_distance, min_angle});
+  constexpr auto operator()(typename StateType::Scalar min_distance, typename StateType::Scalar min_angle) const {
+    return beluga::make_policy(on_motion_policy<StateType>{min_distance, min_angle});
   }
 };
 
@@ -117,8 +156,11 @@ struct on_motion_fn {
 /**
  * This policy is designed to be used for scenarios where an action needs to be performed
  * based on the detected motion, considering specified distance and angle thresholds.
+
+ * \tparam StateType The state type used to check for motion.
  */
-inline constexpr detail::on_motion_fn on_motion;
+template <typename StateType = Sophus::SE2d>
+inline constexpr detail::on_motion_fn<StateType> on_motion;
 
 }  // namespace beluga::policies
 

beluga/test/beluga/policies/test_on_motion.cpp

@@ -21,8 +21,8 @@
 
 namespace {
 
-TEST(OnMotionPolicy, TriggerOnMotion) {
-  auto policy = beluga::policies::on_motion(0.1, 0.05);
+TEST(OnMotionPolicy, TriggerOnMotion2D) {
+  auto policy = beluga::policies::on_motion<Sophus::SE2d>(0.1, 0.05);
   const Sophus::SE2d pose1(Sophus::SO2d(0.2), Eigen::Vector2d(1.0, 2.0));
   const Sophus::SE2d pose2(Sophus::SO2d(0.25), Eigen::Vector2d(1.2, 2.2));
 
@@ -31,13 +31,32 @@ TEST(OnMotionPolicy, TriggerOnMotion) {
   ASSERT_TRUE(policy(pose2));   // Second pose triggers the policy
 }
 
-TEST(OnMotionPolicy, NoTriggerWithoutMotion) {
-  auto policy = beluga::policies::on_motion(0.1, 0.05);
+TEST(OnMotionPolicy, NoTriggerWithoutMotion2D) {
+  auto policy = beluga::policies::on_motion<Sophus::SE2d>(0.1, 0.05);
   const Sophus::SE2d pose1(Sophus::SO2d(0.1), Eigen::Vector2d(1.0, 2.0));
   const Sophus::SE2d pose2(Sophus::SO2d(0.1), Eigen::Vector2d(1.05, 2.05));
 
   ASSERT_TRUE(policy(pose1));   // First pose triggers the policy
   ASSERT_FALSE(policy(pose2));  // Small motion should not trigger the policy
 }
 
+TEST(OnMotionPolicy, TriggerOnMotion3D) {
+  auto policy = beluga::policies::on_motion<Sophus::SE3d>(0.1, 0.05);
+  const Sophus::SE3d pose1(Sophus::SO3d::rotX(0.2), Eigen::Vector3d(1.0, 2.0, 0.));
+  const Sophus::SE3d pose2(Sophus::SO3d::rotX(0.25), Eigen::Vector3d(1.2, 2.2, 0.));
+
+  ASSERT_TRUE(policy(pose1));   // First pose triggers the policy
+  ASSERT_FALSE(policy(pose1));  // Same pose should not trigger again
+  ASSERT_TRUE(policy(pose2));   // Second pose triggers the policy
+}
+
+TEST(OnMotionPolicy, NoTriggerWithoutMotion3D) {
+  auto policy = beluga::policies::on_motion<Sophus::SE3d>(0.1, 0.05);
+  const Sophus::SE3d pose1(Sophus::SO3d::rotX(0.1), Eigen::Vector3d(1.0, 2.0, 0.));
+  const Sophus::SE3d pose2(Sophus::SO3d::rotX(0.1), Eigen::Vector3d(1.05, 2.05, 0.));
+
+  ASSERT_TRUE(policy(pose1));   // First pose triggers the policy
+  ASSERT_FALSE(policy(pose2));  // Small motion should not trigger the policy
+}
+
 }  // namespace

beluga_amcl/cmake/ament.cmake

@@ -16,6 +16,8 @@ find_package(ament_cmake REQUIRED)
 find_package(beluga REQUIRED)
 find_package(beluga_ros REQUIRED)
 find_package(bondcpp REQUIRED)
+find_package(pcl_ros REQUIRED)
+find_package(pcl_conversions REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rclcpp_components REQUIRED)
 find_package(rclcpp_lifecycle REQUIRED)
@@ -75,6 +77,7 @@ ament_export_dependencies(
   beluga
   beluga_ros
   bondcpp
+  pcl_conversions
   rclcpp
   rclcpp_components
   rclcpp_lifecycle
@@ -109,6 +112,7 @@ ament_target_dependencies(
   PUBLIC beluga
          beluga_ros
          bondcpp
+         pcl_conversions
          rclcpp
          rclcpp_components
          rclcpp_lifecycle
@@ -139,6 +143,59 @@ install(TARGETS ndt_amcl_node DESTINATION lib/${PROJECT_NAME})
 
 install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME})
 
+add_library(ndt_amcl_node_3d_component SHARED)
+target_sources(ndt_amcl_node_3d_component PRIVATE src/ndt_amcl_node_3d.cpp
+                                                  src/ros2_common.cpp)
+target_include_directories(
+  ndt_amcl_node_3d_component
+  PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+         $<INSTALL_INTERFACE:include/${PROJECT_NAME}>)
+
+target_compile_features(ndt_amcl_node_3d_component PUBLIC cxx_std_17)
+
+target_link_libraries(ndt_amcl_node_3d_component PUBLIC beluga_amcl_ros2_common)
+
+ament_target_dependencies(
+  ndt_amcl_node_3d_component
+  PUBLIC beluga
+         beluga_ros
+         bondcpp
+         pcl_ros
+         pcl_conversions
+         rclcpp
+         rclcpp_components
+         rclcpp_lifecycle
+         std_srvs)
+
+rclcpp_components_register_node(
+  ndt_amcl_node_3d_component
+  PLUGIN "beluga_amcl::NdtAmclNode3D"
+  EXECUTABLE ndt_amcl_node_3d)
+
+install(
+  TARGETS ndt_amcl_node_3d_component
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin)
+
+install(TARGETS ndt_amcl_node_3d DESTINATION lib/${PROJECT_NAME})
+
+install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME})
+
+ament_export_dependencies(
+  beluga
+  beluga_ros
+  bondcpp
+  rclcpp
+  pcl_ros
+  pcl_conversions
+  rclcpp_components
+  rclcpp_lifecycle
+  pcl_ros
+  pcl_conversions
+  std_srvs)
+ament_export_include_directories("include/${PROJECT_NAME}")
+
 if(BUILD_TESTING)
   enable_testing()
   add_subdirectory(test)

beluga_amcl/include/beluga_amcl/ndt_amcl_node_3d.hpp

@@ -0,0 +1,155 @@
+// Copyright 2022-2024 Ekumen, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef BELUGA_AMCL_NDT_AMCL_NODE_3D_HPP
+#define BELUGA_AMCL_NDT_AMCL_NODE_3D_HPP
+
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/message_filter.h>
+#include <tf2_ros/transform_broadcaster.h>
+#include <tf2_ros/transform_listener.h>
+
+#include <beluga/algorithm/amcl_core.hpp>
+#include <beluga/beluga.hpp>
+
+#include <execution>
+#include <functional>
+#include <optional>
+#include <sophus/types.hpp>
+#include <tuple>
+#include <variant>
+
+#include <geometry_msgs/msg/pose_array.hpp>
+#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
+#include <rclcpp_lifecycle/lifecycle_node.hpp>
+#include <sensor_msgs/msg/laser_scan.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+#include <std_srvs/srv/empty.hpp>
+
+#include <beluga_ros/laser_scan.hpp>
+#include "beluga_amcl/ros2_common.hpp"
+
+#include <message_filters/subscriber.h>
+
+/**
+ * \file
+ * \brief ROS 2 integration of the 3D NDT-AMCL algorithm.
+ */
+
+namespace beluga_amcl {
+
+/// Underlying map representation for the NDT sensor model.
+using NDTMapRepresentation =
+    beluga::SparseValueGrid3<std::unordered_map<Eigen::Vector3i, beluga::NDTCell3d, beluga::detail::CellHasher<3>>>;
+
+/// Type of a particle-dependent random state generator.
+using RandomStateGenerator = std::function<typename beluga::NDTSensorModel<NDTMapRepresentation>::state_type()>;
+
+/// Partial specialization of the core AMCL pipeline for convinience.
+template <class MotionModel, class ExecutionPolicy>
+using NdtAmcl = beluga::Amcl<
+    MotionModel,
+    beluga::NDTSensorModel<NDTMapRepresentation>,
+    RandomStateGenerator,
+    beluga::Weight,
+    std::tuple<typename beluga::NDTSensorModel<NDTMapRepresentation>::state_type, beluga::Weight>,
+    ExecutionPolicy>;
+
+/// All combinations of supported 3D NDT AMCL variants.
+using NdtAmclVariant = std::variant<
+    NdtAmcl<beluga::DifferentialDriveModel3d, std::execution::parallel_policy>,  //
+    NdtAmcl<beluga::DifferentialDriveModel3d, std::execution::sequenced_policy>>;
+
+/// Supported motion models.
+using MotionModelVariant = std::variant<beluga::DifferentialDriveModel3d>;
+
+/// Supported execution policies.
+using ExecutionPolicyVariant = std::variant<std::execution::sequenced_policy, std::execution::parallel_policy>;
+
+/// 3D NDT AMCL as a ROS 2 composable lifecycle node.
+class NdtAmclNode3D : public BaseAMCLNode {
+ public:
+  /// Constructor.
+  explicit NdtAmclNode3D(const rclcpp::NodeOptions& options = rclcpp::NodeOptions{});
+
+ protected:
+  /// Callback for lifecycle transitions from the INACTIVE state to the ACTIVE state.
+  void do_activate(const rclcpp_lifecycle::State&) override;
+
+  /// Callback for lifecycle transitions from the ACTIVE state to the INACTIVE state.
+  void do_deactivate(const rclcpp_lifecycle::State&) override;
+
+  /// Callback for lifecycle transitions from the INACTIVE state to the UNCONFIGURED state.
+  void do_cleanup(const rclcpp_lifecycle::State&) override;
+
+  /// Get initial pose estimate from parameters if set.
+  auto get_initial_estimate() const -> std::optional<std::pair<Sophus::SE3d, Sophus::Matrix6d>>;
+
+  /// Get motion model as per current parametrization.
+  auto get_motion_model() const -> MotionModelVariant;
+
+  /// Get execution policy given its name.
+  auto get_execution_policy() const -> ExecutionPolicyVariant;
+
+  /// Get sensor model as per current parametrization.
+  beluga::NDTSensorModel<NDTMapRepresentation> get_sensor_model() const;
+
+  /// Instantiate particle filter given an initial occupancy grid map and the current parametrization.
+  auto make_particle_filter() const -> std::unique_ptr<NdtAmclVariant>;
+
+  /// Callback for periodic particle cloud updates.
+  void do_periodic_timer_callback() override;
+
+  /// Callback for node to configure and activate itself.
+  void do_autostart_callback() override;
+
+  /// Callback for laser scan updates.
+  void laser_callback(sensor_msgs::msg::PointCloud2::ConstSharedPtr);
+
+  /// Callback for pose (re) initialization.
+  void do_initial_pose_callback(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr message) override;
+
+  /// Initialize particles from an estimated pose and covariance.
+  /**
+   * If an exception occurs during the initialization, an error message is logged, and the initialization
+   * process is also aborted, returning false. If the initialization is successful, the TF broadcast is
+   * enabled.
+   *
+   * \param estimate A pair representing the estimated pose and covariance for initialization.
+   * \return True if the initialization is successful, false otherwise.
+   */
+  bool initialize_from_estimate(const std::pair<Sophus::SE3d, Sophus::Matrix6d>& estimate);
+
+  /// Laser scan updates subscription.
+  std::unique_ptr<message_filters::Subscriber<sensor_msgs::msg::PointCloud2, rclcpp_lifecycle::LifecycleNode>>
+      laser_scan_sub_;
+
+  /// Transform synchronization filter for laser scan updates.
+  std::unique_ptr<tf2_ros::MessageFilter<sensor_msgs::msg::PointCloud2>> laser_scan_filter_;
+  /// Connection for laser scan updates filter and callback.
+  message_filters::Connection laser_scan_connection_;
+
+  /// Particle filter instance.
+  std::unique_ptr<NdtAmclVariant> particle_filter_ = nullptr;
+  /// Last known pose estimate, if any.
+  std::optional<std::pair<Sophus::SE3d, Sophus::Matrix6d>> last_known_estimate_ = std::nullopt;
+  /// Last known map to odom correction estimate, if any.
+  std::optional<Sophus::SE3d> last_known_odom_transform_in_map_;
+  /// Whether to broadcast transforms or not.
+  bool enable_tf_broadcast_{false};
+};
+
+}  // namespace beluga_amcl
+
+#endif  // BELUGA_AMCL_NDT_AMCL_NODE_3D_HPP