From 037cf3c8cac170ce7c7934929549e244be32d610 Mon Sep 17 00:00:00 2001 From: Ramiro Serra Date: Thu, 8 Aug 2024 15:15:19 -0300 Subject: [PATCH] Add NDT AMCL 3D node. Signed-off-by: Ramiro Serra --- beluga/include/beluga/algorithm/amcl_core.hpp | 3 +- beluga/include/beluga/policies/on_motion.hpp | 52 +- .../test/beluga/policies/test_on_motion.cpp | 27 +- beluga_amcl/cmake/ament.cmake | 57 ++ .../include/beluga_amcl/ndt_amcl_node_3d.hpp | 155 +++++ beluga_amcl/package.xml | 3 + beluga_amcl/src/ndt_amcl_node_3d.cpp | 503 ++++++++++++++++ beluga_amcl/src/ros2_common.cpp | 6 +- beluga_amcl/test/cmake/ament.cmake | 10 + .../test/test_data/sample_3d_ndt_map.hdf5 | Bin 0 -> 33112 bytes beluga_amcl/test/test_ndt_amcl_3d_node.cpp | 564 ++++++++++++++++++ beluga_amcl/test/test_utils/node_testing.hpp | 80 +++ .../launch/ndt_simulation_3d.launch.xml | 16 + .../utils/ndt_3d_localization_launch.py | 146 +++++ beluga_example/maps/sample_3d_ndt_map.hdf5 | Bin 0 -> 33112 bytes .../params/default_ndt_3d.ros2.yaml | 91 +++ beluga_ros/include/beluga_ros/amcl.hpp | 2 +- 17 files changed, 1701 insertions(+), 14 deletions(-) create mode 100644 beluga_amcl/include/beluga_amcl/ndt_amcl_node_3d.hpp create mode 100644 beluga_amcl/src/ndt_amcl_node_3d.cpp create mode 100644 beluga_amcl/test/test_data/sample_3d_ndt_map.hdf5 create mode 100644 beluga_amcl/test/test_ndt_amcl_3d_node.cpp create mode 100644 beluga_example/launch/ndt_simulation_3d.launch.xml create mode 100644 beluga_example/launch/utils/ndt_3d_localization_launch.py create mode 100644 beluga_example/maps/sample_3d_ndt_map.hdf5 create mode 100644 beluga_example/params/default_ndt_3d.ros2.yaml diff --git a/beluga/include/beluga/algorithm/amcl_core.hpp b/beluga/include/beluga/algorithm/amcl_core.hpp index c55efcccd..72eaa7436 100644 --- a/beluga/include/beluga/algorithm/amcl_core.hpp +++ b/beluga/include/beluga/algorithm/amcl_core.hpp @@ -26,6 +26,7 @@ #include #include #include +#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(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) { diff --git a/beluga/include/beluga/policies/on_motion.hpp b/beluga/include/beluga/policies/on_motion.hpp index bbe589642..6e1409d5f 100644 --- a/beluga/include/beluga/policies/on_motion.hpp +++ b/beluga/include/beluga/policies/on_motion.hpp @@ -17,6 +17,7 @@ #include #include +#include /** * \file @@ -70,6 +71,44 @@ struct on_motion_policy_base> { 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 +struct on_motion_policy_base> { + 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& prev, const Sophus::SE3& 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 { }; /// Implementation detail for the on_motion_fn object. +template 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 - constexpr auto operator()(Scalar min_distance, Scalar min_angle) const { - return beluga::make_policy(on_motion_policy>{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{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 +inline constexpr detail::on_motion_fn on_motion; } // namespace beluga::policies diff --git a/beluga/test/beluga/policies/test_on_motion.cpp b/beluga/test/beluga/policies/test_on_motion.cpp index 7c259e7ae..cc13fae6a 100644 --- a/beluga/test/beluga/policies/test_on_motion.cpp +++ b/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(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,8 +31,8 @@ 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(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)); @@ -40,4 +40,23 @@ TEST(OnMotionPolicy, NoTriggerWithoutMotion) { ASSERT_FALSE(policy(pose2)); // Small motion should not trigger the policy } +TEST(OnMotionPolicy, TriggerOnMotion3D) { + auto policy = beluga::policies::on_motion(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(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 diff --git a/beluga_amcl/cmake/ament.cmake b/beluga_amcl/cmake/ament.cmake index 78e5c7afb..98acad4f8 100644 --- a/beluga_amcl/cmake/ament.cmake +++ b/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 $ + $) + +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) diff --git a/beluga_amcl/include/beluga_amcl/ndt_amcl_node_3d.hpp b/beluga_amcl/include/beluga_amcl/ndt_amcl_node_3d.hpp new file mode 100644 index 000000000..e251362c3 --- /dev/null +++ b/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 +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include "beluga_amcl/ros2_common.hpp" + +#include + +/** + * \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>>; + +/// Type of a particle-dependent random state generator. +using RandomStateGenerator = std::function::state_type()>; + +/// Partial specialization of the core AMCL pipeline for convinience. +template +using NdtAmcl = beluga::Amcl< + MotionModel, + beluga::NDTSensorModel, + RandomStateGenerator, + beluga::Weight, + std::tuple::state_type, beluga::Weight>, + ExecutionPolicy>; + +/// All combinations of supported 3D NDT AMCL variants. +using NdtAmclVariant = std::variant< + NdtAmcl, // + NdtAmcl>; + +/// Supported motion models. +using MotionModelVariant = std::variant; + +/// Supported execution policies. +using ExecutionPolicyVariant = std::variant; + +/// 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>; + + /// 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 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; + + /// 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& estimate); + + /// Laser scan updates subscription. + std::unique_ptr> + laser_scan_sub_; + + /// Transform synchronization filter for laser scan updates. + std::unique_ptr> laser_scan_filter_; + /// Connection for laser scan updates filter and callback. + message_filters::Connection laser_scan_connection_; + + /// Particle filter instance. + std::unique_ptr particle_filter_ = nullptr; + /// Last known pose estimate, if any. + std::optional> last_known_estimate_ = std::nullopt; + /// Last known map to odom correction estimate, if any. + std::optional 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 diff --git a/beluga_amcl/package.xml b/beluga_amcl/package.xml index 6526d9e00..07d471b3c 100644 --- a/beluga_amcl/package.xml +++ b/beluga_amcl/package.xml @@ -21,6 +21,7 @@ message_filters std_srvs + diagnostic_updater dynamic_reconfigure nodelet @@ -29,6 +30,8 @@ rclcpp rclcpp_components rclcpp_lifecycle + pcl_conversions + pcl_ros rostest ament_cmake_gmock diff --git a/beluga_amcl/src/ndt_amcl_node_3d.cpp b/beluga_amcl/src/ndt_amcl_node_3d.cpp new file mode 100644 index 000000000..58a8992d8 --- /dev/null +++ b/beluga_amcl/src/ndt_amcl_node_3d.cpp @@ -0,0 +1,503 @@ +// Copyright 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. + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include "beluga_amcl/ndt_amcl_node_3d.hpp" +#include "beluga_amcl/ros2_common.hpp" + +namespace beluga_amcl { + +NdtAmclNode3D::NdtAmclNode3D(const rclcpp::NodeOptions& options) : BaseAMCLNode{"ndt_amcl", "", options} { + RCLCPP_INFO(get_logger(), "Creating"); + + { + auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); + descriptor.description = "Initial pose roll axis mean."; + this->declare_parameter("initial_pose.roll", 0.0, descriptor); + } + + { + auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); + descriptor.description = "Initial pose pitch axis mean."; + this->declare_parameter("initial_pose.pitch", 0.0, descriptor); + } + + { + auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); + descriptor.description = "Initial pose z axis mean."; + this->declare_parameter("initial_pose.z", 0.0, descriptor); + } + + { + auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); + descriptor.description = "Initial pose z axis covariance."; + this->declare_parameter("initial_pose.covariance_z", 1e-6, descriptor); + } + + { + auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); + descriptor.description = "Initial pose roll axis covariance."; + this->declare_parameter("initial_pose.covariance_roll", 1e-6, descriptor); + } + + { + auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); + descriptor.description = "Initial pose pitch axis covariance."; + this->declare_parameter("initial_pose.covariance_pitch", 1e-6, descriptor); + } + + { + auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); + descriptor.description = "Likelihood for measurements that lie inside cells that are not present in the map."; + descriptor.floating_point_range.resize(1); + descriptor.floating_point_range[0].from_value = 0; + descriptor.floating_point_range[0].to_value = 1; + descriptor.floating_point_range[0].step = 0; + declare_parameter("minimum_likelihood", rclcpp::ParameterValue(0.01), descriptor); + } + + { + auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); + descriptor.description = "Scaling parameter d1 in literature, used for scaling 3D likelihoods."; + descriptor.floating_point_range.resize(1); + descriptor.floating_point_range[0].from_value = 0; + descriptor.floating_point_range[0].to_value = 1000; + descriptor.floating_point_range[0].step = 0; + declare_parameter("d1", rclcpp::ParameterValue(1.0), descriptor); + } + + { + auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); + descriptor.description = "Scaling parameter d2 in literature, used for scaling 3D likelihoods."; + descriptor.floating_point_range.resize(1); + descriptor.floating_point_range[0].from_value = 0; + descriptor.floating_point_range[0].to_value = 1000; + descriptor.floating_point_range[0].step = 0; + declare_parameter("d2", rclcpp::ParameterValue(0.6), descriptor); + } + + if (get_parameter("autostart").as_bool()) { + auto autostart_delay = std::chrono::duration(get_parameter("autostart_delay").as_double()); + autostart_timer_ = create_wall_timer(autostart_delay, std::bind(&NdtAmclNode3D::autostart_callback, this)); + } +} + +void NdtAmclNode3D::do_autostart_callback() { + using lifecycle_msgs::msg::State; + auto current_state = configure(); + if (current_state.id() != State::PRIMARY_STATE_INACTIVE) { + RCLCPP_WARN(get_logger(), "Failed to auto configure, shutting down"); + shutdown(); + } + RCLCPP_WARN(get_logger(), "Auto configured successfully"); + current_state = activate(); + if (current_state.id() != State::PRIMARY_STATE_ACTIVE) { + RCLCPP_WARN(get_logger(), "Failed to auto activate, shutting down"); + shutdown(); + } + RCLCPP_INFO(get_logger(), "Auto activated successfully"); +} + +void NdtAmclNode3D::do_activate(const rclcpp_lifecycle::State&) { + RCLCPP_INFO(get_logger(), "Making particle filter"); + particle_filter_ = make_particle_filter(); + { + using LaserScanSubscriber = + message_filters::Subscriber; + laser_scan_sub_ = std::make_unique( + shared_from_this(), get_parameter("scan_topic").as_string(), rmw_qos_profile_sensor_data, + common_subscription_options_); + + // Message filter that caches laser scan readings until it is possible to transform + // from laser frame to odom frame and update the particle filter. + laser_scan_filter_ = std::make_unique>( + *laser_scan_sub_, *tf_buffer_, get_parameter("odom_frame_id").as_string(), 10, get_node_logging_interface(), + get_node_clock_interface(), tf2::durationFromSec(get_parameter("transform_tolerance").as_double())); + + laser_scan_connection_ = + laser_scan_filter_->registerCallback(std::bind(&NdtAmclNode3D::laser_callback, this, std::placeholders::_1)); + RCLCPP_INFO(get_logger(), "Subscribed to scan_topic: %s", laser_scan_sub_->getTopic().c_str()); + + const auto initial_estimate = get_initial_estimate(); + if (initial_estimate.has_value()) { + last_known_estimate_ = initial_estimate; + last_known_odom_transform_in_map_.reset(); + initialize_from_estimate(initial_estimate.value()); + } + } +} + +void NdtAmclNode3D::do_deactivate(const rclcpp_lifecycle::State&) { + RCLCPP_INFO(get_logger(), "Cleaning up"); + particle_cloud_pub_.reset(); + laser_scan_connection_.disconnect(); + laser_scan_filter_.reset(); + laser_scan_sub_.reset(); +} + +void NdtAmclNode3D::do_cleanup(const rclcpp_lifecycle::State&) { + RCLCPP_INFO(get_logger(), "Cleaning up"); + particle_cloud_pub_.reset(); + pose_pub_.reset(); + particle_filter_.reset(); + enable_tf_broadcast_ = false; +} + +auto NdtAmclNode3D::get_initial_estimate() const -> std::optional> { + if (!get_parameter("set_initial_pose").as_bool()) { + return std::nullopt; + } + + const auto pose = Sophus::SE3d{ + Sophus::SO3d::rotZ(get_parameter("initial_pose.yaw").as_double()) * + Sophus::SO3d::rotY(get_parameter("initial_pose.pitch").as_double()) * + Sophus::SO3d::rotX(get_parameter("initial_pose.roll").as_double()), + Eigen::Vector3d{ + get_parameter("initial_pose.x").as_double(), + get_parameter("initial_pose.y").as_double(), + get_parameter("initial_pose.z").as_double(), + }}; + + Sophus::Matrix6d covariance = Sophus::Matrix6d::Zero(); + + // TODO(serraramiro1): We are ignoring correlations here. + // Is it realistic for a user to initialize a filter with correlations? + covariance.coeffRef(0, 0) = get_parameter("initial_pose.covariance_x").as_double(); + covariance.coeffRef(1, 1) = get_parameter("initial_pose.covariance_y").as_double(); + covariance.coeffRef(2, 2) = get_parameter("initial_pose.covariance_z").as_double(); + covariance.coeffRef(3, 3) = get_parameter("initial_pose.covariance_roll").as_double(); + covariance.coeffRef(4, 4) = get_parameter("initial_pose.covariance_pitch").as_double(); + covariance.coeffRef(5, 5) = get_parameter("initial_pose.covariance_yaw").as_double(); + + return std::make_pair(pose, covariance); +} + +auto NdtAmclNode3D::get_motion_model() const -> MotionModelVariant { + const auto name = get_parameter("robot_model_type").as_string(); + if (name == kDifferentialModelName || name == kNav2DifferentialModelName) { + auto params = beluga::DifferentialDriveModelParam{}; + params.rotation_noise_from_rotation = get_parameter("alpha1").as_double(); + params.rotation_noise_from_translation = get_parameter("alpha2").as_double(); + params.translation_noise_from_translation = get_parameter("alpha3").as_double(); + params.translation_noise_from_rotation = get_parameter("alpha4").as_double(); + return beluga::DifferentialDriveModel{params}; + } + throw std::invalid_argument(std::string("Invalid motion model: ") + name); +} + +beluga::NDTSensorModel NdtAmclNode3D::get_sensor_model() const { + auto params = beluga::NDTModelParam3d{}; + params.minimum_likelihood = get_parameter("minimum_likelihood").as_double(); + params.d1 = get_parameter("d1").as_double(); + params.d2 = get_parameter("d2").as_double(); + const auto map_path = get_parameter("map_path").as_string(); + RCLCPP_INFO(get_logger(), "Loading map from %s.", map_path.c_str()); + + return beluga::NDTSensorModel{ + params, beluga::io::load_from_hdf5(get_parameter("map_path").as_string())}; +} + +auto NdtAmclNode3D::get_execution_policy() const -> ExecutionPolicyVariant { + const auto name = get_parameter("execution_policy").as_string(); + if (name == "seq") { + return std::execution::seq; + } + if (name == "par") { + return std::execution::par; + } + throw std::invalid_argument("Execution policy must be seq or par."); +}; + +auto NdtAmclNode3D::make_particle_filter() const -> std::unique_ptr { + auto amcl = std::visit( + [this](auto motion_model, auto execution_policy) -> NdtAmclVariant { + auto params = beluga::AmclParams{}; + params.update_min_d = get_parameter("update_min_d").as_double(); + params.update_min_a = get_parameter("update_min_a").as_double(); + params.resample_interval = static_cast(get_parameter("resample_interval").as_int()); + params.selective_resampling = get_parameter("selective_resampling").as_bool(); + params.min_particles = static_cast(get_parameter("min_particles").as_int()); + params.max_particles = static_cast(get_parameter("max_particles").as_int()); + params.alpha_slow = get_parameter("recovery_alpha_slow").as_double(); + params.alpha_fast = get_parameter("recovery_alpha_fast").as_double(); + params.kld_epsilon = get_parameter("pf_err").as_double(); + params.kld_z = get_parameter("pf_z").as_double(); + + auto hasher = beluga::spatial_hash( + get_parameter("spatial_resolution_x").as_double(), get_parameter("spatial_resolution_theta").as_double()); + + RandomStateGenerator random_state_maker = []() { return Sophus::SE3d{}; }; + + return beluga::Amcl( + std::move(motion_model), + get_sensor_model(), // + std::move(random_state_maker), // + std::move(hasher), // + params, // + execution_policy); + }, + get_motion_model(), get_execution_policy()); + return std::make_unique(std::move(amcl)); +} + +void NdtAmclNode3D::do_initial_pose_callback(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr message) { + auto pose = Sophus::SE3d{}; + tf2::convert(message->pose.pose, pose); + const Eigen::Map covariance(message->pose.covariance.data()); + last_known_estimate_ = std::make_pair(pose, covariance); + last_known_odom_transform_in_map_.reset(); + initialize_from_estimate(last_known_estimate_.value()); +} + +void NdtAmclNode3D::do_periodic_timer_callback() { + if (!particle_filter_) { + return; + } + + if (particle_cloud_pub_->get_subscription_count() == 0) { + return; + } + std::visit( + [this](const auto& particle_filter) { + auto message = beluga_ros::msg::PoseArray{}; + beluga_ros::assign_particle_cloud(particle_filter.particles(), message); + beluga_ros::stamp_message(get_parameter("global_frame_id").as_string(), now(), message); + particle_cloud_pub_->publish(message); + }, + *particle_filter_); +} + +// TODO(alon): Wouldn't it be better in the callback of each message to simply receive +// it and define another timer or thread to do the work of calculation and publication? +void NdtAmclNode3D::laser_callback(sensor_msgs::msg::PointCloud2::ConstSharedPtr laser_scan) { + if (!particle_filter_ || !last_known_estimate_.has_value()) { + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), 2000, "Ignoring laser data because the particle filter has not been initialized"); + return; + } + + auto base_pose_in_odom = Sophus::SE3d{}; + auto laser_pose_in_base = Sophus::SE3d{}; + try { + // Use the lookupTransform overload with no timeout since we're not using a dedicated + // tf thread. The message filter we are using avoids the need for it. + tf2::convert( + tf_buffer_ + ->lookupTransform( + get_parameter("odom_frame_id").as_string(), get_parameter("base_frame_id").as_string(), + tf2_ros::fromMsg(laser_scan->header.stamp)) + .transform, + base_pose_in_odom); + tf2::convert( + tf_buffer_ + ->lookupTransform( + get_parameter("base_frame_id").as_string(), laser_scan->header.frame_id, + tf2_ros::fromMsg(laser_scan->header.stamp)) + .transform, + laser_pose_in_base); + } catch (const tf2::TransformException& error) { + RCLCPP_ERROR(get_logger(), "Couldn't find needed transform, : %s", error.what()); + return; + } + + const auto update_start_time = std::chrono::high_resolution_clock::now(); + + pcl::PointCloud pcl; + std::vector measurement; + pcl::fromROSMsg(*laser_scan, pcl); + measurement.reserve(pcl.points.size()); + + // TODO (serraramiro1): Can this be more efficient? + for (const auto& point : pcl.points) { + measurement.emplace_back(laser_pose_in_base * Eigen::Vector3d{point.x, point.y, point.z}); + } + + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 2000, "Processing %ld points.", measurement.size()); + const auto new_estimate = std::visit( + [base_pose_in_odom, measurement = measurement](auto& particle_filter) { + return particle_filter.update( + base_pose_in_odom, // + std::move(measurement)); + }, + *particle_filter_); + + const auto update_stop_time = std::chrono::high_resolution_clock::now(); + const auto update_duration = update_stop_time - update_start_time; + + if (new_estimate.has_value()) { + const auto& [base_pose_in_map, _] = new_estimate.value(); + last_known_odom_transform_in_map_ = base_pose_in_map * base_pose_in_odom.inverse(); + last_known_estimate_ = new_estimate; + + const auto num_particles = + std::visit([](const auto& particle_filter) { return particle_filter.particles().size(); }, *particle_filter_); + + RCLCPP_INFO( + get_logger(), "Particle filter update iteration stats: %ld particles - %.3fms", num_particles, + std::chrono::duration(update_duration).count()); + } + + if (!last_known_estimate_.has_value()) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 2000, "Estimate not available for publishing"); + return; + } + + // Transforms are always published to keep them current. + if (enable_tf_broadcast_ && get_parameter("tf_broadcast").as_bool()) { + if (last_known_odom_transform_in_map_.has_value()) { + auto message = geometry_msgs::msg::TransformStamped{}; + // Sending a transform that is valid into the future so that odom can be used. + const auto expiration_stamp = tf2_ros::fromMsg(laser_scan->header.stamp) + + tf2::durationFromSec(get_parameter("transform_tolerance").as_double()); + message.header.stamp = tf2_ros::toMsg(expiration_stamp); + message.header.frame_id = get_parameter("global_frame_id").as_string(); + message.child_frame_id = get_parameter("odom_frame_id").as_string(); + message.transform = tf2::toMsg(last_known_odom_transform_in_map_.value()); + tf_broadcaster_->sendTransform(message); + } + } + + // New pose messages are only published on updates to the filter. + if (new_estimate.has_value()) { + // ROS covariances use extrinsic RPY parametrization, we need to convert to it from our tangent space + // representation. We use an unscented transform to apply such transform as it's non linear. + // See https://www.ros.org/reps/rep-0103.html#covariance-representation . + static const auto kTangentialSpaceToXyzRpy = + [](const Eigen::Vector& tangent) -> Eigen::Vector { + const auto se3 = Sophus::SE3d::exp(tangent); + // Eigen's eulerAngles uses intrinsic rotations, but XYZ extrinsic rotation is the same as ZYX intrinsic rotation. + // See https://pages.github.berkeley.edu/EECS-106/fa21-site/assets/discussions/D1_Rotations_soln.pdf + // This gives (extrinsic) yaw, pitch, roll in that order. + const Eigen::Vector3d euler_angles = se3.so3().matrix().eulerAngles(2, 1, 0); + Eigen::Vector ret; + ret.head<3>() = se3.translation(); + ret[3] = euler_angles.z(); + ret[4] = euler_angles.y(); + ret[5] = euler_angles.x(); + return ret; + }; + auto message = geometry_msgs::msg::PoseWithCovarianceStamped{}; + message.header.stamp = laser_scan->header.stamp; + message.header.frame_id = get_parameter("global_frame_id").as_string(); + auto [base_pose_in_map, base_pose_covariance] = new_estimate.value(); + tf2::toMsg(base_pose_in_map, message.pose.pose); + + static constexpr double kMinimumVariance = + 1e-6; // Make sure we covariance is not singular so that UT doesn't fail. + for (auto index = Eigen::Index{0}; index <= base_pose_covariance.cols(); ++index) { + base_pose_covariance.coeffRef(index, index) = std::max(base_pose_covariance(index, index), kMinimumVariance); + } + const auto& [base_pose_in_map_xyz_rpy, base_pose_covariance_xyz_rpy] = + beluga::unscented_transform(base_pose_in_map.log(), base_pose_covariance, kTangentialSpaceToXyzRpy); + + std::copy( + base_pose_covariance_xyz_rpy.data(), base_pose_covariance_xyz_rpy.data() + base_pose_covariance_xyz_rpy.size(), + message.pose.covariance.data()); + pose_pub_->publish(message); + } +} + +bool NdtAmclNode3D::initialize_from_estimate(const std::pair& estimate) { + RCLCPP_INFO(get_logger(), "Initializing particles from estimated pose and covariance"); + + if (particle_filter_ == nullptr) { + RCLCPP_ERROR(get_logger(), "Could not initialize particles: The particle filter has not been initialized"); + return false; + } + + try { + std::visit( + [estimate](auto& particle_filter) { particle_filter.initialize(estimate.first, estimate.second); }, + *particle_filter_); + } catch (const std::runtime_error& error) { + RCLCPP_ERROR(get_logger(), "Could not initialize particles: %s", error.what()); + return false; + } + + enable_tf_broadcast_ = true; + + const auto num_particles = + std::visit([](const auto& particle_filter) { return particle_filter.particles().size(); }, *particle_filter_); + + const auto& pose = estimate.first; + RCLCPP_INFO( + get_logger(), "Particle filter initialized with %ld particles about initial pose x=%g, y=%g, z=%g", num_particles, + pose.translation().x(), pose.translation().y(), pose.translation().z()); + + return true; +} +} // namespace beluga_amcl + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(beluga_amcl::NdtAmclNode3D) diff --git a/beluga_amcl/src/ros2_common.cpp b/beluga_amcl/src/ros2_common.cpp index 20c2df261..1b865b4aa 100644 --- a/beluga_amcl/src/ros2_common.cpp +++ b/beluga_amcl/src/ros2_common.cpp @@ -334,19 +334,19 @@ BaseAMCLNode::BaseAMCLNode( { auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); descriptor.description = "Initial pose x axis covariance."; - this->declare_parameter("initial_pose.covariance_x", 0.0, descriptor); + this->declare_parameter("initial_pose.covariance_x", 1e-6, descriptor); } { auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); descriptor.description = "Initial pose y axis covariance."; - this->declare_parameter("initial_pose.covariance_y", 0.0, descriptor); + this->declare_parameter("initial_pose.covariance_y", 1e-6, descriptor); } { auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); descriptor.description = "Initial pose yaw covariance."; - this->declare_parameter("initial_pose.covariance_yaw", 0.0, descriptor); + this->declare_parameter("initial_pose.covariance_yaw", 1e-6, descriptor); } { diff --git a/beluga_amcl/test/cmake/ament.cmake b/beluga_amcl/test/cmake/ament.cmake index cb2bb926d..fac96d3e5 100644 --- a/beluga_amcl/test/cmake/ament.cmake +++ b/beluga_amcl/test/cmake/ament.cmake @@ -37,3 +37,13 @@ target_include_directories(test_ndt_amcl_node PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/test_utils) target_compile_options(test_ndt_amcl_node PRIVATE -Wno-deprecated-copy) target_link_libraries(test_ndt_amcl_node ndt_amcl_node_component) + +ament_add_gmock( + test_ndt_amcl_3d_node + test_ndt_amcl_3d_node.cpp + WORKING_DIRECTORY + ${CMAKE_CURRENT_BINARY_DIR}) +target_include_directories(test_ndt_amcl_3d_node + PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/test_utils) +target_compile_options(test_ndt_amcl_3d_node PRIVATE -Wno-deprecated-copy) +target_link_libraries(test_ndt_amcl_3d_node ndt_amcl_node_3d_component) diff --git a/beluga_amcl/test/test_data/sample_3d_ndt_map.hdf5 b/beluga_amcl/test/test_data/sample_3d_ndt_map.hdf5 new file mode 100644 index 0000000000000000000000000000000000000000..ed299db32ed6c8b86fc7f15a0c269843ecd464c8 GIT binary patch literal 33112 zcmeIb2UHYW+qT)FfPeu(!GHn9h!_BY?ylOk3j>NdU>0*$6fsAF0xBX#lBg((Sx^zW zyNWJz#+)(doFj&*yX@9;-go`8X3bi&zWHa|XSLUJJ$2VpyXt`29OcY!-J(@#+sd{o ztdb>FB~$O+FK3$npSf;bTDDZl zN%MR&O{b}vl=xNEGdKS7|5s0-bxXbzZn&rEuKi3rYWg@cscP`hF=N7H89Q`f2sn7$ zIhRy9&rxcXJiVlH+@koe%ak;IrWCg*KGpJ91|u=v z^cgVKbmspIx_0Q;qL{pulBUboFyHw9Vms!~d+Falt^8Nod^oG@e;xW)IO}R!a8$_woPycr$&F{5$m5{yu@fPvGwp`1=I@K7qea;O`Uo z`vm^CeFFc3??=v*`}NfRU-f?Ezn{micZS7^|9WHgpT9pgzg_uX_5UxwU$gl22JrX$ zwPGb*em$PgzW@H<8u4%af7t%-zOTqKy&pDj??0c<-}e>fC-awopTOTI@b?M)eFA@< zz~3kE_X+%e;|YZAZA(&P`-+JhdNN?+hQ-;lZ^s(o;LQ44$=L2r#4$l5S{xlIf?~JV zOwg2D3>@sYC7ry}?jnarlovLp)nh^1pi0b{l2^5GFz8Ai>*{Prb{yy{E-Y4(1sB}% zWj9})CBVVep7n_B<@MSQg1uqJou0nnmMSHQ^TOdeIEdqT*(XjDb_o7@F#c#dvFJU5 zfrGm~Z&T*%aDJsP)01PsA&a+=Z?X0a9L(sjg^Wu*&UQ|pB9c~@4dAg38N~YM5f%|U+RNmrlGqmAC{K<|D7$@w1H8n{B66f!~DO`gB)%QV@zoCWI+e##yY=814{NH0n( z0#~vtBQG&G%LeGdl25mj&(1}9IB34FO)E2)cNqgT;8piMn z-n;q?==SCSsolS;00#rlELSoa&sCbZatCyv{$(yH*k8i{2aE5@R5HQSc}=%=UTW}| zK9kVi&los3uSPn_SU8Og)z@e2UGK19z0)HJQ^A6TgFklh%A6kqB!f$dQt%x+R{ zppzaBn(r(8b5lXPsg)j#IklIBYi%`(%XHqiMVWJKWv2GR;}9(vIerfr;X6WrgB9%8 zD47fGD{DGe_tk^rd@@P7gPjd<@Wg>-O2(nsMqQ52ISp7aaXq0O`!H~@%YBJFV^T%iM(`5)5AgYecfmpt$rVI zMh{MJpGhn&BlU1FX#TJJIy}Ct;py=m47fF56B%?z65wEsH#y3lljdE}bv`mq4;EHf zN0^0{3>RM9m z&v_hKGo!nk{6L+~~BiC<|0XmL1$w7gbD4+qWno-{bET5G zbXcz+Y&Tg0R%pMMaCRjHIM}lM9`eei2^r*{Y?R@OPf@ZJk3@E2gj5MQRXb~TUm3uV38iYw|gTQ zJ!CNh2Vcbg`W)PKD^Okkymv0R|62^(xP80^4x0D8VrM+l@2jK-r623ajjN>vIJj|J zs&cLDC94=aL;pO`{o+P)zv6r?94x=gf(`YtU=Lia$y#phDuB+%1=7IfyZ{Hsm+Gv{ zIq~tBHegHx9T@WPJh|AbtN;hS=Wiy>@-s>6{*i3R

5m-(){icKjeM92_@d6*(KR zhPV&u$_^=QA%ZTsDUAA9mL3jjh3)Lv;c4V}!{*w_`I7|@_j;jIt{M)G3=v4ij83HV z;NFalHjo9UH=N2`Z%SA=I5vJRu}fP?7R7}a7Pas@qdAIOSEiCm0q z#ST1QT>zbQ8_D}qV-*Km6;vVK8w|v;$wKBtP)`vIewIZx)~l_BgYD&CORHrMWS*^y zV%Epp-3z97Ok`iSJ?IMu@l5yJIYfjpJX2f$N&qLjxD)F}*Axe1-ppY8j&voS4J(r2 zQD=1E;g6|gQU4t}IEXoYS!==++|brJ6k`Bw)~#U+cUuc^FrbD9V_UW-xkHAr_jmNv zgO2L)WN($qesBVEcdV%$&-pN*|Edm5wHXBr9CX}ug$*sSj@<7ZtG(0cvIe|)CWLJ1(tO#xXK*@PYXeTE1gZ#SE)=ulR1a8cYA z_U4rPgx73kf(M)zz+I10NY$(52psHLwgcDDu97%9&YaJZKGfi~%B*R@ersn+?BG@`GlFhn%L4bqNfu3yh zlF{UDxfjg0lJ(T!$cl-~x(EJhIEXo}noUI2YKf-mw~hjc9_z7I3kR=6IIcO8SqrXKReM$p?3H$4Fl?LJJ3P=hq_>BF7M?IhjJo zW-~-kqhCQT`NoQHFz|>I>2frS+0~{w^J!j+ANZrje&*KYTRw0Q*J{9pGt-SFnWInJ z}!lu|s>z72#lTnZD%0;!>k%UIa96T#+B9ZA5k?jjsasC|Gv={uhUI4&lBZ|nr{HNS_nZRRDw zLF^|}J%O#_9xWWc6{fJ*tr&Jk+88|?oO(8vIG=7s0*5>0yHBgmf|2LO68qCsgo7(x zBiWf(Z<0Q5J_)rptkQxm9>4T$?#d8(Qp9E9KbxPrW@@2|NqLKHysZp#!72g7oJsuEJUBSuei8^u=m~)|J6*9|j zCBu6>Gk|gZS~F*^uGYgrWE}5Prt9%ehN;Z%T(D{7kIcZ1JN)1vGW+`^kfOmqgz_b7 z3SiCrG*WeoP6G##S@=Abw2W~hZ>K)cfdOA%5Z8UR4R8>daUc7VLcd3b%rjTDpvNv- zVi3RU;UF@nEo(6+8%AiE`lq$v%=Lb3nboO!IM{33SmpN#PaVS5bwd(zz*lSTXy4>5 zR~*EgZ35J2>nO{=VfgWqb4#BbXrHhAhVL*e5{1BlFk=-qyB z@Re64$xO*0eU3UXW$iaGVA8pnjQ^b%ii4fPF0vob^(AAr7l_^?duqVOZn5l>F9&sS z5OW9B&Lj>q0yMj3eA9#UXn%ICm#+W^zfNew3~r!jvm3Nv^LM?}f`{D)lb%B_Y2hH| zcr1@$y)^f=4YTIx!5{VA$)&J5dN_zoVxlJr8WpK6yZyNqj6M`dUi|biz(Hh|tL=$# zjL1$}*i#SQZdj7E3Cz&JL1gOI9ZrfZ_GG&?U!((zz7(=i6C3N`AToIMcsNeu>AyP< zgfmulzHkukty{^=-a1qmTqROr(UmW3QfLJc4j$u+DIbRe#_iX+j4IHAIM1i`00SKC zKmQ`z{mgb^DU?-TC_h#MBIBQRKNk+ddkQC;P#{>3hc$e(5xJ z;@G|VxE6T^5Sg*dCi=la__#42*x&~Zg%<~O3egL$c=MOb9R4YU9DcEcwF!+Bk~^(e zgZFC9WTtf9)C>;dobHC2Z1tTU!sOY38qhAR5|jQQDi;p!4B19vZJpSbjVlRHZq^n+ zVU+{Z;Ndha9IP6ULA(=ENr$;9!kD7h0=QxRJZ9y$FAN+!p(3G2)*8J|hkovx@YOsJ^^TZi>x2Zgx{aIlN94>RW4R(9OQJhAZe4-M#F zY#(tPC>r1(_Va3`J8AA;imkOHh*fx$BNsJs3>@5d?kYL);?VT>|w?^r?{=`=z!5*h+q;JL@emz`?U` zPO`|Ia|qP?wm9hv!hIV1`oTfCVcAB~#BQ&l@r$Es@a7s9rpM{;l<)JQC@hJ)94Mv;;Oj*>Rdz4cpfpVouD&n{&m4%b&4#GD)R(@CS*PyF^> z>!AVBTlH#f9vs|remk*hYb2FzJ~8jIO0(dI<`tL*{58eF>Z8Y!lR2Bnoa>df7FiPo z@Zia&%!>0H7&zGO#BSEV|50N3qI$mi>0twC-+MI~cg9x-2XXEAay2>snPUX2=_5rD zy}Zz-3>;i#{p;(!opZP8&Isq#ATow7<27*bRjZb4=$Bb!eefOq=E4X)=rLv*+viG@ z00-@Ne$Xx%c7v3;!xJVe-T>|ou^{DYwKBj#%$?D+1Gib*kWAX=&4M0Iw@HuFO%w-T z&J{?_{;_O7hfsEg+Y|w`KU|&^j=v(n!8`JQo7wfU2HRc#gz;InHV?GDA3%Ook#ukn z*SbE>jvKKuLUVT2Y8I?~-Himhm0;oE8g2(E=+&1yJUg8EnY@<)pSk#w@;fJL;2`Ea zt#wlS?crT!!h>`*=u#AZ zipyibofm`HBieBWIJo4&S*G8{W{lUJs`|%18EViY-JLBlf0GUlp0W_NxA&YSRZAAI z&%;%EutL^u`RnXHIyi{^L=W|2yJWd&J3OqxfDM~%CzboQ)xg2KpZuAPwKlPwsopGZMbd0RR6lUI2!$DkYV8sh$=$QoO%E0w% z5WQ~QZG7P%yk(n3qU_~=NPD$#@LauR%=vjmtdnNC_?FDmfKF{zk-3rUHE^p17-ZEf}gf`FgV-i1Qv#-Jyqr@Zc-1?5(Q(7`tiRG$5}z z&qmC?t%rliT-!dM9qy_XRBcBIU^~lV-1^Ef1P;zRae_>Y8OX+;&(gda6Xgfuyzwr{ zesB2u;l1yJy^6LgCtE)*TX?%R_Dwm`^SU}r&mAC1N-~Ll3IPT^l%Uvd)JO+c|^J} zd1fm;Sg^b@sTnmy4+oJ+7!^bIe=0Pba42Q~i@o$_RuBs{97M)CH;HH&Zy|YthXK5B z<_626E{xvKgcTjv!$D+%=m>JPcYuZ*7_R}*d${ANFC4VvH(Qg{o{qq^EqP$ zF!slJ?aa+B4RCO&eH*sNYc*TBLV@60ZH*eNTD&!ht7K(>gSZykCytH2K1e*){}-)y}#$IT_``J}H^afTb*VvA2U- zD-OnwbRpN%DiHP(Wt$hIir~+M1~&I+TLBIR`!6AHcJ3!<6Q&8{7nRb2U8jvCUA7m~ z!a-cC^pk6B|FY4-sEB$B%f#&@_bgh8aByeSeQfO0rEEDbOYvrH3la1keU|vAYX}_d z(>jFsg&EmT=YJT2xFs6!Iv3288u3B{2XU<@$+uX~+N;^Atq$nHEeGStHfmEo{%>W0npEZuH z$bD6qc6Hoi21M`X%=HF1sO#E>Jbhc5y^>p?(Fb+s17%apI;hy4?@`z*`ZsnX@O>Xy72OJ+aqv(z=0` z>AK;y0HRkgAwv%b%e7iWMr?0Ec3Irg66rwwb#ZI6joe0N;jcdM*&2yg0Mmh2V0(< zPaMJqvfpB_`sG#MYygp|C0@|M!H?Pg%tgl-a_GYNe2t%<7Hsk{mt@<{HNZifvw31A zl29~79NMj`2wn_JVS5i6z{0^Pk%vgJ$|74X_K|SNEl~hJI|@wo<{4Tz_;t-Vc1hX6 z7I!8sH$Vl|Lw!oj$pup@VZz1DHH#Gs*515!(2m>5Mrl{X%rf!^?S@tyC z0KSe~%?w$Ur-y?j1W)j%qy3uIEej(mR`kFv27%TcM=U? z?(=-sus?)>gWKNQG2GrH;yZmTqrP5A11`|FWIUPT8aSxj`|~7X@o)m;KkB{?%&YKC zJEm?s4IF&*dJy?s5Klf{Siq!|%o0JDxev7MoVFR@;PHZv#O>iu5_R0joGwK~u;!Fa z%*}oy1UP6_ScAP{TuP2)57q}Co1_EFoUmg19=oqNi2Zo=Od~VGm-^Y=E+>NMbxba$ zhJ%xnqe#AAF3G(YpsqP(w+2L}EeZF5gVL7@Y;f(Rq?xvgDESxHgPr;&5a)vl8aRk^ ze4aaUbM9J;gS5r-!KRMSwf)C-XW-yL`8_Ri6*DyY+|dU#;6^LGHZ*>{;-LGweyr`b zU8KUgM&$G34hAqRaR=Ej%hLb{F=OKC0CMA9Fk7v?rvTn@xXZR#ZpFaCj$nHu!Fr8IZA+o?9ENg^(-B!Uh)guZ1iLUm^4=Yn!~g(102M)>g3A57W4_# zrVe;7fSFZ0lZKD|^>A>N?OKvMtP=C&yqoa;!z>**;F%j~?x-`s!TLkfnHzzNNso|J z!8f>t2Aq(#njF1fQwIlet#adEvGaFuOy5@X81UKW-R%1)8x{_ZRN0czrxudU<@5Bh z-y;PO=dHiHKnDj`kDS5mua-d;9S#tdI>hL}g*26`kvX@k2p*g7Xu#eM z`NTc3ycQ1PTC=hiu=)L~8Qv5-$$+PaZzN@ox6{BuyRJjn4aZB8q6yEm9V+%@z|dCf zN&d}Kii7SSIpx>1O`XzoGt?V&;LymX#8=%?3kU1PW@x|LW|H9pGPFUlmKv~X^$Db# z?xzk8Vn4UK3@7oCcl9cF!h&76d~J?CT!4cw$4n&I2@6Swj`Q^@{XRX2^U8mZ5#ZqU zFL6ZEp$sWE{++Np-bDa64f7?TS6CJf9xPd$T|0IX+5B~w(CJMLJ@`rO#Vlx&u7-n{ zJAL;W@|i5rxJKGCAbQ)IZSjSJ`r(@h(>IyCD7{?ph}889!kKzn0^hvMK~iwx!Kq7E@BeghY` z*MT^%*vSw*931tcD|>e?CBnmg!j>HoMk6*Y~oWbm{xF#-Ta9c;9zL>6UOmvCvtIK9cJ5?&Ia&` zb`!ZCpfbQgT&pnYAiL2k$1wMKRRN3|6ij9_m9%hB->EkFI%5%OW5;NiOND;mYuzR? zxA-z2IEXnvrp;$g?d+`Sba0LibbsbgItCs$z(HiT#3qpEuZ|0?4^7MiyG1T1^$SCC z;UF?x^%$mC{3gxU)fsy5!3AG3_PcjJ96UeFldMRJVNSGi(&cUZ>I*LI9>BPH@&X)O z8`^?NX|b17&Z?YWXHZiu*vmGG+$mdC2M2Mj1p(1a*=KQiQ;x6q0|(z9$((OBM1X?< z=>r*?n@8D3cODBq9l~|sldi)_pO`{59K@XN^M;X%y%??Kih)`%DQPv^WnZS^;3KbY zOoOtGNzSNkc_Y8?VL+VsF|L*`9CRJKgZN!&PsVnRQh!e@(1Jn3gP9Q9Y9bsYqt}x4 zkt4~3Pm~#ux{3i)>Rn*=I+tSMAhTmK@hHqBQ?4c%8s{Wvz}AgRvG2~^(ZfMpJH>7< zp%!zQ8v}PUAbN*eN2%f9`!2o6^8;In=WdSKwz9t-M5e;}YdSbMs9_KpaC&V==y<>cNYQU9224=gkNdpIwskm(jd79Wq$SKJRAbOE48>r#nguPoxP-ZkKRj-q9 zc5{>-RQ9;lRs$a-b9K}*vREBLhSMBF!~1*npkcsfrbNG30S;~&<*WRBc=Ov&&HQKO z)!>h}l^EYCb#!nLbMe}Fsj0SQay0|!KFWca-7C=m2X&48mER*Z>R(1%CbY92WDbei z4K7s-aPWBwRWhk1R_GTL9#ezX`76lj70+~V(0nap$02@A7wyjlGft$FB=)-i2TxwT zuk87G;SSB6C3$M_NdL9u@X!h(9DKUywUT)}`LbWZvt?@V%fk#}`*Vd34w|pkq=H&~ zd~bOjIHK-4a%5+K9uBs(IH1h=xcjO8;Q7%SFu7$k;jMWQ4ptd>O!=H!;&v+6t<_gG zc)))Pnd|x47Y>@Q)ookLyxy})`+;Fcw-D`)00SJnTm7_htqR6rI>%!x^1ubxx)X<* zrx`eS+Ifv~t!3?IYub%|s|F*+Zy@!nrmEqf`C7qKyz=sH52ihz|TZcLTZ6&Yy*YyFX1*GIP5f*Ov=2 zs=?=uYl&0YIK@HpwMNXzP=}pf;0v}|zmDvXI=INxS z)`5kCt>-jVGVPLY>5DJjuLjFbSV0=Ea!?#JU+boeg|64KrFr1x5%QnK$Z5~O!7Z2N zk0<%_;-*!iX4}GX8t``GY_h8!!@xoQT8ffUUm9qrcqqsK9&>gk4=oEBIN0Y(E#=;O zxN-WcBW%8uo^@!ZNp179K2NN*WYWqIcb(YD63EhBIDs2 zs)2(w)+H(Xxt%><=bUjs4`0Kl8;2RH`zkeZT`ksGO z=<~p4FQ>*@wp{ANj@^Ry@3~DT#tMpChDt-E=|F3gReU*HUc@Ao{h-Fe9HnpCkJ6`q`sO)G#?+Tj`?DX2yf6pr_kJMsPv1NT z?Qm0H$(r{gUmr`^5B6f-5Bl^^-#kY-Zt5$?jlVLev6TJLKXa5mT#^sUr2nYHO*ML0 z$SB86drduKGufxk^v&n``s)SNd}N zcOQF{?_HUR+I+4|b6o@eeWGvxN~K4!`1gi9uxYD9kb z|B+GAfAmc?mHwQI+MH4Pri==GdHvppX{OSrfBMS4$&75PWs zoJarneEFWtwMx$Z&d9a751MEG?%_P?S6_MVa*buK6`C?C`j5V;#`973EoV@hGfLl- zQT?N@WR0ddGR=9p`aK8rKl_ya%A*}_suh~&$UQ7&uhc|&Kgi%*6?&5VGG9-wl|@Z` zIcv^`;srC)t>t&*3yR-UOTqXOlZvVUCLRIB7=uFZM# zeW7o@FLQ0qD}C%+&M5a{nu*$+QTnEg(lcd#C4QZ&^#74jQPWKHOnK}}$>A9Kc$sQ* zUZE+E8VebvZ^|ouxraUlO?lK<=qoQ%A2rtRzG;rqrz+K2`4uk7FF9|n<@oO!`~2OP zv#9ZG&3V*V=Dv#ls+DJJnu!_<8KrNYDIYh_0kM!lZN4sgzcccA<{H;A*YrPX98>1} zqaQBUSik%7EOV`*e`+e%lKcvne$6-6DtVb}?9E)Om3ue->Z8U& zM(LYp%ExhE=!011+I(H~%o)^J${d;I3~0`v#!}{>FTc#Siki-qY0jgEg$!zQUg^t@ zK1JX7r&h`9ci%K08I}A(KO8Sp9yQkQzIi`XZptrnpUU-Z`4ukxI@esQ?&UFNx$ZpYq|TUFXz$6Gg0oxR3q}cZ_1dT5BlckW3F*s z)R6_=sJ#z+!m4%t|`aMU^nlnld2QeRg^BnpoqmpZ7VGjLwMmZ+;Df&kF zxT%(_-?e!rGUj}^T$}T#m4!XXwK=c!Ww%FP`jt1=$eU~AQJbH$xi)8%zA1xei|ZnT z8p~Xp=is0{64jIw`IM(LZcC1+IftI0p^%UsJx{`BQM`sVvG*XE4UH)YKCiwtTkbB+C@ z#=h_}*XE4UH_wrq<_wNwDRX50o+D??8S}GJW}>k!{nZbbY0jg@Qm#w?%vAcce2q8Kl9tD%WrxGL6ycpT<^55q-F;u0n)^b^v^*r$ zQ7@)n+rO2z4>>87Du11(bo?f*h+8am8slhq+TfeiF>{?1lpV=wmwuJXEL$tR8Az$? zw9nFmQ>l_Feii-Eq)1vOB})e_ZqPLYUq~*&>!e0+ZTS7$zet_(no46&br#;fD3bEG zveFv$G;V8^uTtw8&83|AEokW)Mbhnct)z>V>2zAHBI(isUh=qK#e8P6O|= zQn%MpMycUV%2jwJje9V|IO>WWUu?=}Y2xQ-?)2oX!u&_yq*ZJbmop-od-46N)Njjl zF1x*krnwhM1Ez;@c~jHri%LaO)U83BvDhtI+xwl=EIx<}A7jH8Z~sv0yo}JUYYK%O zVMWp`O6la|(Ol>E{NT zQm#nKYMDmA9=c8Y27i^R?2e zeSuEhxq+s67fHRloTo86Z_?>Io=Da|OYpiPo%QVE%|+uE70Pu8Pb%>XC*7g zS=>cqnsm|iq!eGupGFK?B@M24SXyOBpr2h?s z9kWW>cB~9<*CAc{#&}D`8aVNpUsg+#+RQLclJifWrAk9qYPs-L<@g*AgOpQjG~KyP z{`(6nS4fRR8`52Ij{K(FtfP-tLI}XWnzGo^ZmG#ezY5X^wwW$ee|%j{%`_y-pxw3XS1ay9;x(T&6ZO8!KqS{xH9}x zV|!^-1FgiwSKwbwrP93o7^6PFEdOwZpS0C=I5*VBo=*&EE~(de(Ot3S`45xEN#_rz z(ovnA`FdrBNZ!Irdaj$3_@Y!-X-1W^)N{2X&yHoJD(5Zv+O~UjZ&l| z18w;&b-s%?B0VL2P(LoZMydP-x+c;-T_9bZJ}^JE29aD}Y@+F1TjodVeWc9Gx9EpH zo<`>#tkkr1Y2K^vB4a^&J7aSHN#ZBFP5HjXYZ#wZ&!-3OHZxvOMHt70l;RuiyOBTQ zi8rU6lux5KcQxu=hI4+;OY@bJos8FCa8&=qmOplr&Ci~=f>!qM6sCl@=f9i0f>sp zR$AcePrD>Ub9aUwl|l;^(MdPwbGa){NX>^j@S9TSbJ6?PNj09Z;^Ts;TuGOmlK;3g z+9zc-S$A{3bawp>%FGSn?i4PS4*sy_^DIj~p>qT`v3zv?IY%Gy)tN+2bFine?naRw3(w<5u#=71QzUwI?PxBx??L0% zQdayHi;0|nL3!>$b`3FUP&&6R!i(GPFH+~#C%IaS+_^gYj?-JBn)AEVi|Z0^%{$$j z%5kUr(KYfPyveweM{W{is z$my}1>!Copv-fdn;YK2oE50;v{TXRS!ffutvQ~7*{xj0y>KrXR=T9xC9F^Y0ETU!l z*P;U&ot6T(+wd`|l=?4RFEM%|j&^84`}Wx_1&YZu_(ECwgp4DV-qHeoW|WS8INdRSzy^+9(?DXU#86 zElD>v=t+}DSo5d-uhW5_5~-p6S*fYzSD|0mWa?7=tkf=WBKImYk=nYRm1g?$bl->7 zlxciKI^1P3{c(3Q?PI@9DrZ$yY`J_LEj@I%G~{VA?XVz@x_xM3EPq|3g+t=#4|{K; z<4B42n!AGD?Aes-W|vPbuP>#M#F?wtQlhmzGN|R9TJ(O;YGSKCyXZtgq(#oN3_q6V z(d3NLw*@?=#w2I3>OIn9Z$N{)~3pdP;ib!qWWB z_o!Q&BT}Q(cpCHI5q15RBz@^_AwJ&znvPG7ljhiV<9x<_pot#QQjoJVopa$1Z5FUw zns_~#mYnv8o~p%32@PX}+M{35*-tpBZu^$pqccTRa2+O9^RGcus=cCZ`?fZ2nHI{m z8t{%LHy&x^V@uG0!B=RUdt3T2yS7-V&j~uEM<#7P%7NSU@F;CkVn3bNiY|n?ZJuN*g|3+w$SB@WD^{m7-p2?M!9QdJ|PfKZI z8qo-2X`Kuv&!-o`ufGOnv#MgCMq z1v;tpXc~FCGQZ8aJe}+{myTFap1*%<35^xJ#l(}9_;p><>0r$|&fcvOA3I?+ZS{6E zeY~g&|FF?2I_Xdp9pdT0*PQ%==0>*_({@zgndC#XomWq;UUC^eZn`xe82!ocWvM-X zf08x7@k&3=?S=zyGufI?YhHmKiLK1f?pmDRoRUc6>o?+t;-R+h@e4iT4nx%20{G?SuIZMbt;?tI6WDk)fFO;f}=ytij5 zqvyEc;_!JMeAVrfjHS3pu8M;P-y?8}amfrTstGzxPL;F#BsO?W- zR5vfa*U?p!y;4IAYwO7eWs*8G@EJNkZ(AK$rhasI@IrL=Ti!Uqqs=Fe5zPwPf>;s*^cklL#& zi07Mk;CGffE3IK?bBnFo^4q_kmTK;6M&FI=z$d;sBl!)Dq;aQu@i%z+_s07;G3G`O z{;OrQ`jjv3vXkDc9Hx;5dr;2PhXPhBuhdf&A*SKYTSf3xgN z=|rj>O`F||w_Orr9P-{xY%BEOy{Aq#eip*GMeBR=Ul)up=HF^SU#7O>J)0$QGoRiP z613gNH`(rw)Eh`9`p1~$_B3Qv>v?F zLqqS|cA>SF1@W5Flc>j~2>L9j6`!1tNxRsz5<7kE!Rt3Ir{|k)=W2B8#kZ@PNCOWH zrH0nMd1tF6x}beDWi_37|8@6i#pb<5eoYUab3Q^3(gED~>%I8x>5(3rqZeDgEK`H`>V zXk@t-{H_Vsd|}#tdg<{5nI|QwL}~F^qcFZ=kJHk;9--VpDU`ouaayW-#+fD#oXEHP zwoW>g&|D1mna0OXkC7gY>&aF4F+qM`5hKN)u%SawPvB3w6X|kV12Ncs0>AIAS_;hx z=Il$4=NCqLOHZ0sqEo+(<2%Lp8o!idMXu*GzMyW5aYBPIu0;Q-{HFH9jGF1?>9D8} zKK}F~u3Wcn;{HM7`7_siXy6Q1)P;=aN0(Vmzi)3W>U_rV%}*_-Z(3`(NYxmANd9ul zH0(ehj2X>88@Pgo1U#g9JLg&K;5@CR&5@Nq|D=$i2h_&S|WO4XZI6C0hK&!@Yb zm5$qRoJT<{Z>V@yI{U$%POrC+f3|a-G`o3#c+HsZ@rpEfvFmemU6qF0H1xuiiZV z`0nM#F=MB3R*j}B6ray;Jep5W}|2j=SS7|V)^PJYiOM=?Zp%P zeEv#!8ojw^7uRD@4F95a2F*J%gg$j#zz?psgO=zKMfq&fR`8glUsK7P*ki;)4 zc18-)xzS1&lKGjvGNen5+lbrdtmIEOjgdZWmH+-$y%heedaiUQ#(@s1pUMX{43x&5 z@f7!aCGm!8J*0$MwYmNslK6~Toux%n%Fy5~DSTAQJfpf)1F^JI693U*opE_yUC#Y< z3V&%{6gRa)JF!>O)qI4-25$0@N;F1H;b(91q&Wd?#Vcere`#qin(=uy*U)b@KkH2? zTF}Oh-foi2y9F$#`!gGems+pp!@Qzth;?7`DP$Qxls`|OKlT*6>Q?Z55+2hjUy^C| z=;eI#@z(sK2LrfgE-UyUZ!P)N9T^%hc?HjWFU~LDyp+B>xRFnLc|w}*Ttyu7ER*+K zaz@&A$CZ0nViTW}C)Z=^&>urL@^*8RB%92(VyRMF__$)cbS=HDcw_t)e%1at#*n18 zV&s9%e6g@O+>MoOMLX>#e&&Nz?r>OT`ZZ${@1u34qqemby;IloqwM?9rtQPHOVif# z@4A$x_Fi_h*@jHMZSYb$YPN^i^8R{$@|b8kYV$0zV4ghx+D$6B_YfC{Zsz-1Sn-#8 zl@Y@aZsz;DS@S*Z!nqU8H}iGm--AgrJn8Y2&HUjv#pQ1$7)x(H%;M?CLy~mniD2_G zi(fzGuoS+fHg|PT7XR|i5oymZH|lMj%@-@4B$X8`#6w=$ykS19q<&^8PQ97MH}NJ?trOcw$%fhdnU0;M5q-+hwr{g|wN->M$gZsT zSe~P=9bueqzl~J($>yu?tZtmq$BqVl%Hq3k9L?Fhv=dLsnd7MwIi}18a?dWCU!)1+ zF8_3(H&xmE;bZlwV-qX!m7LjHt_kG~1tg?tHh-{HYq~VliGH@s<`1<>qK7Zqh}GoG z>^I5u!$!hwmiMzGI-M44U7ODNn#E^}SLlF@Dx$Z1FYOmzr>H|tPB%+1Q;o$D0m9n+_C_jhLT35|;LZCyR--sCKP N;wTHg`;b`re*hU1Xea;x literal 0 HcmV?d00001 diff --git a/beluga_amcl/test/test_ndt_amcl_3d_node.cpp b/beluga_amcl/test/test_ndt_amcl_3d_node.cpp new file mode 100644 index 000000000..3099e2f57 --- /dev/null +++ b/beluga_amcl/test/test_ndt_amcl_3d_node.cpp @@ -0,0 +1,564 @@ +// Copyright 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. + +#include + +#include +#include +#include +#include +#include + +#include + +#include +#include + +#include "beluga_amcl/ndt_amcl_node_3d.hpp" +#include "test_utils/node_testing.hpp" + +namespace { + +const std::string kTestMapPath = "./test_data/sample_3d_ndt_map.hdf5"; + +using namespace std::chrono_literals; +using beluga_amcl::testing::spin_for; +using beluga_amcl::testing::spin_until; + +/// Test class that provides convenient public accessors. +class AmclNodeUnderTest : public beluga_amcl::NdtAmclNode3D { + public: + /// Get particle filter pointer. + const auto& particle_filter() { return particle_filter_; } + + /// Return true if the particle filter has been initialized. + bool is_initialized() const { return particle_filter_ != nullptr; } + + /// Return the last known estimate. Throws if there is no estimate. + const auto& estimate() { return last_known_estimate_.value(); } +}; + +/// Base node fixture class with common utilities. +template +class BaseNodeFixture : public T { + public: + void SetUp() override { + rclcpp::init(0, nullptr); + ndt_amcl_node_ = std::make_shared(); + tester_node_ = std::make_shared(); + tester_node_->create_transform_buffer(); + ndt_amcl_node_->set_parameter(rclcpp::Parameter("map_path", kTestMapPath)); + ndt_amcl_node_->set_parameter(rclcpp::Parameter("scan_topic", "point_cloud")); + } + + void TearDown() override { rclcpp::shutdown(); } + + bool wait_for_initialization() { + return spin_until([this] { return ndt_amcl_node_->is_initialized(); }, 200ms, ndt_amcl_node_, tester_node_); + } + + bool wait_for_pose_estimate() { + tester_node_->latest_pose().reset(); + tester_node_->create_pose_subscriber(); + return spin_until([this] { return tester_node_->latest_pose().has_value(); }, 1000ms, ndt_amcl_node_, tester_node_); + } + + bool wait_for_transform(const std::string& target, const std::string& source) { + return spin_until( + [&, this] { return tester_node_->can_transform(target, source); }, 1000ms, ndt_amcl_node_, tester_node_); + } + + bool wait_for_particle_cloud() { + tester_node_->create_particle_cloud_subscriber(); + return spin_until( + [this] { return tester_node_->latest_particle_cloud().has_value(); }, 1000ms, ndt_amcl_node_, tester_node_); + } + + protected: + std::shared_ptr ndt_amcl_node_; + std::shared_ptr tester_node_; +}; + +class TestLifecycle : public ::testing::Test { + public: + void SetUp() override { rclcpp::init(0, nullptr); } + + void TearDown() override { rclcpp::shutdown(); } +}; + +TEST_F(TestLifecycle, FullSpin) { + using lifecycle_msgs::msg::State; + auto node = std::make_shared(); + node->set_parameter(rclcpp::Parameter("map_path", kTestMapPath)); + node->set_parameter(rclcpp::Parameter("scan_topic", "point_cloud")); + ASSERT_EQ(node->get_current_state().id(), State::PRIMARY_STATE_UNCONFIGURED); + spin_for(40ms, node); + ASSERT_EQ(node->configure().id(), State::PRIMARY_STATE_INACTIVE); + spin_for(40ms, node); + ASSERT_EQ(node->activate().id(), State::PRIMARY_STATE_ACTIVE); + spin_for(40ms, node); + ASSERT_EQ(node->deactivate().id(), State::PRIMARY_STATE_INACTIVE); + spin_for(40ms, node); + ASSERT_EQ(node->cleanup().id(), State::PRIMARY_STATE_UNCONFIGURED); + spin_for(40ms, node); + ASSERT_EQ(node->shutdown().id(), State::PRIMARY_STATE_FINALIZED); +} + +TEST_F(TestLifecycle, ShutdownWhenActive) { + using lifecycle_msgs::msg::State; + auto node = std::make_shared(); + node->set_parameter(rclcpp::Parameter("map_path", kTestMapPath)); + node->set_parameter(rclcpp::Parameter("scan_topic", "point_cloud")); + ASSERT_EQ(node->get_current_state().id(), State::PRIMARY_STATE_UNCONFIGURED); + spin_for(10ms, node); + ASSERT_EQ(node->configure().id(), State::PRIMARY_STATE_INACTIVE); + spin_for(10ms, node); + ASSERT_EQ(node->activate().id(), State::PRIMARY_STATE_ACTIVE); + spin_for(10ms, node); + ASSERT_EQ(node->shutdown().id(), State::PRIMARY_STATE_FINALIZED); +} + +TEST_F(TestLifecycle, ShutdownWhenInactive) { + using lifecycle_msgs::msg::State; + auto node = std::make_shared(); + node->set_parameter(rclcpp::Parameter("map_path", kTestMapPath)); + node->set_parameter(rclcpp::Parameter("scan_topic", "point_cloud")); + ASSERT_EQ(node->get_current_state().id(), State::PRIMARY_STATE_UNCONFIGURED); + spin_for(10ms, node); + ASSERT_EQ(node->configure().id(), State::PRIMARY_STATE_INACTIVE); + spin_for(10ms, node); + ASSERT_EQ(node->shutdown().id(), State::PRIMARY_STATE_FINALIZED); +} + +TEST_F(TestLifecycle, ShutdownWhenUnconfigured) { + using lifecycle_msgs::msg::State; + auto node = std::make_shared(); + node->set_parameter(rclcpp::Parameter("map_path", kTestMapPath)); + node->set_parameter(rclcpp::Parameter("scan_topic", "point_cloud")); + ASSERT_EQ(node->get_current_state().id(), State::PRIMARY_STATE_UNCONFIGURED); + spin_for(10ms, node); + ASSERT_EQ(node->shutdown().id(), State::PRIMARY_STATE_FINALIZED); +} + +TEST_F(TestLifecycle, DestroyWhenActive) { + using lifecycle_msgs::msg::State; + auto node = std::make_shared(); + node->set_parameter(rclcpp::Parameter("map_path", kTestMapPath)); + node->set_parameter(rclcpp::Parameter("scan_topic", "point_cloud")); + ASSERT_EQ(node->get_current_state().id(), State::PRIMARY_STATE_UNCONFIGURED); + spin_for(10ms, node); + ASSERT_EQ(node->configure().id(), State::PRIMARY_STATE_INACTIVE); + spin_for(10ms, node); + ASSERT_EQ(node->activate().id(), State::PRIMARY_STATE_ACTIVE); +} + +TEST_F(TestLifecycle, DestroyWhenInactive) { + using lifecycle_msgs::msg::State; + auto node = std::make_shared(); + node->set_parameter(rclcpp::Parameter("map_path", kTestMapPath)); + node->set_parameter(rclcpp::Parameter("scan_topic", "point_cloud")); + ASSERT_EQ(node->get_current_state().id(), State::PRIMARY_STATE_UNCONFIGURED); + spin_for(10ms, node); + ASSERT_EQ(node->configure().id(), State::PRIMARY_STATE_INACTIVE); +} + +TEST_F(TestLifecycle, DestroyWhenUnconfigured) { + using lifecycle_msgs::msg::State; + auto node = std::make_shared(); + node->set_parameter(rclcpp::Parameter("map_path", kTestMapPath)); + node->set_parameter(rclcpp::Parameter("scan_topic", "point_cloud")); + ASSERT_EQ(node->get_current_state().id(), State::PRIMARY_STATE_UNCONFIGURED); +} + +TEST_F(TestLifecycle, AutoStart) { + using lifecycle_msgs::msg::State; + auto node = std::make_shared(rclcpp::NodeOptions{} + .append_parameter_override("autostart", true) + .append_parameter_override("map_path", kTestMapPath) + .append_parameter_override("scan_topic", "point_cloud")); + spin_until([&] { return node->get_current_state().id() == State::PRIMARY_STATE_ACTIVE; }, 100ms, node); + ASSERT_EQ(node->get_current_state().id(), State::PRIMARY_STATE_ACTIVE); +} + +class TestInitializationWithModel : public BaseNodeFixture<::testing::TestWithParam> {}; + +INSTANTIATE_TEST_SUITE_P(Models, TestInitializationWithModel, testing::Values("differential_drive")); + +TEST_P(TestInitializationWithModel, ParticleCount) { + const auto* const motion_model = GetParam(); + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"set_initial_pose", true}); + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"robot_model_type", motion_model}); + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"scan_topic", "point_cloud"}); + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"min_particles", 10}); + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"max_particles", 10}); + ndt_amcl_node_->configure(); + ndt_amcl_node_->activate(); + + ASSERT_TRUE(wait_for_initialization()); + + std::visit( + [](const auto& pf) { + ASSERT_GE(pf.particles().size(), 10UL); + ASSERT_LE(pf.particles().size(), 30UL); + }, + *ndt_amcl_node_->particle_filter()); +} + +class TestNode : public BaseNodeFixture<::testing::Test> {}; + +TEST_F(TestNode, SetInitialPose) { + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"set_initial_pose", true}); + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"initial_pose.x", 34.0}); + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"initial_pose.y", 2.0}); + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"initial_pose.yaw", 0.3}); + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"initial_pose.covariance_x", 0.001}); + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"initial_pose.covariance_y", 0.001}); + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"initial_pose.covariance_yaw", 0.001}); + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"initial_pose.covariance_xy", 0.0}); + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"initial_pose.covariance_xyaw", 0.0}); + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"initial_pose.covariance_yyaw", 0.0}); + ndt_amcl_node_->configure(); + ndt_amcl_node_->activate(); + ASSERT_TRUE(wait_for_initialization()); + const auto [pose, _] = ndt_amcl_node_->estimate(); + ASSERT_NEAR(pose.translation().x(), 34.0, 0.01); + ASSERT_NEAR(pose.translation().y(), 2.0, 0.01); + // ASSERT_NEAR(pose.so2().log(), 0.3, 0.01); +} + +TEST_F(TestNode, BroadcastWhenInitialPoseSet) { + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"set_initial_pose", true}); + ndt_amcl_node_->configure(); + ndt_amcl_node_->activate(); + ASSERT_TRUE(wait_for_initialization()); + ASSERT_FALSE(tester_node_->can_transform("map", "odom")); + tester_node_->publish_3d_laser_scan(); + ASSERT_TRUE(wait_for_pose_estimate()); + ASSERT_TRUE(wait_for_transform("map", "odom")); +} + +TEST_F(TestNode, NoBroadcastWhenNoInitialPose) { + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"set_initial_pose", false}); + ndt_amcl_node_->configure(); + ndt_amcl_node_->activate(); + ASSERT_TRUE(wait_for_initialization()); + ASSERT_FALSE(tester_node_->can_transform("map", "odom")); + ASSERT_FALSE(wait_for_pose_estimate()); + ASSERT_FALSE(tester_node_->can_transform("map", "odom")); +} + +TEST_F(TestNode, NoBroadcastWhenInitialPoseInvalid) { + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"set_initial_pose", true}); + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"initial_pose.covariance_x", 0.0}); + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"initial_pose.covariance_y", -0.5}); + ndt_amcl_node_->configure(); + ndt_amcl_node_->activate(); + ASSERT_TRUE(wait_for_initialization()); + ASSERT_FALSE(tester_node_->can_transform("map", "odom")); + tester_node_->publish_3d_laser_scan(); + ASSERT_FALSE(wait_for_pose_estimate()); + ASSERT_FALSE(tester_node_->can_transform("map", "odom")); +} + +TEST_F(TestNode, KeepCurrentEstimate) { + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"set_initial_pose", true}); + { + // Set initial pose values to simulate an estimate that has converged. + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"initial_pose.x", 34.0}); + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"initial_pose.y", 2.0}); + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"initial_pose.yaw", 0.3}); + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"initial_pose.covariance_x", 0.001}); + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"initial_pose.covariance_y", 0.001}); + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"initial_pose.covariance_yaw", 0.001}); + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"initial_pose.covariance_xy", 0.0}); + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"initial_pose.covariance_xyaw", 0.0}); + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"initial_pose.covariance_yyaw", 0.0}); + } + + ndt_amcl_node_->configure(); + ndt_amcl_node_->activate(); + + { + // Initializing with the first map and initial pose values. + ASSERT_TRUE(wait_for_initialization()); + const auto [pose, _] = ndt_amcl_node_->estimate(); + ASSERT_NEAR(pose.translation().x(), 34.0, 0.01); + ASSERT_NEAR(pose.translation().y(), 2.0, 0.01); + // ASSERT_NEAR(pose.so2().log(), 0.3, 0.01); + } + + tester_node_->publish_3d_laser_scan(); + ASSERT_TRUE(wait_for_pose_estimate()); + const auto [estimate, _] = ndt_amcl_node_->estimate(); + + { + // Set new initial pose values that will be ignored. + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"initial_pose.x", 1.0}); + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"initial_pose.y", 29.0}); + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"initial_pose.yaw", -0.4}); + } + + spin_for(50ms, tester_node_, ndt_amcl_node_); + + { + // Initializing with the second map but keeping the old estimate. + // Ignoring the new initial pose values. + const auto [pose, _] = ndt_amcl_node_->estimate(); + ASSERT_NEAR(pose.translation().x(), estimate.translation().x(), 0.01); + ASSERT_NEAR(pose.translation().y(), estimate.translation().y(), 0.01); + // ASSERT_NEAR(pose.so2().log(), estimate.so2().log(), 0.01); + } +} + +TEST_F(TestNode, KeepCurrentEstimateAfterCleanup) { + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"set_initial_pose", true}); + + { + // Set initial pose values to simulate an estimate that has converged. + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"initial_pose.x", 34.0}); + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"initial_pose.y", 2.0}); + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"initial_pose.yaw", 0.3}); + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"initial_pose.covariance_x", 0.001}); + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"initial_pose.covariance_y", 0.001}); + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"initial_pose.covariance_yaw", 0.001}); + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"initial_pose.covariance_xy", 0.0}); + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"initial_pose.covariance_xyaw", 0.0}); + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"initial_pose.covariance_yyaw", 0.0}); + } + + ndt_amcl_node_->configure(); + ndt_amcl_node_->activate(); + ASSERT_TRUE(wait_for_initialization()); + tester_node_->publish_3d_laser_scan(); + ASSERT_TRUE(wait_for_pose_estimate()); + ndt_amcl_node_->deactivate(); + ndt_amcl_node_->cleanup(); + + { + // Set new initial pose values that will be ignored. + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"initial_pose.x", 12.0}); + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"initial_pose.y", 1.0}); + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"initial_pose.yaw", 0.7}); + } + + ndt_amcl_node_->configure(); + ndt_amcl_node_->activate(); + ASSERT_NE(ndt_amcl_node_->particle_filter().get(), nullptr); + ASSERT_TRUE(wait_for_initialization()); + const auto [pose, _] = ndt_amcl_node_->estimate(); + ASSERT_NEAR(pose.translation().x(), 12.0, 0.01); + ASSERT_NEAR(pose.translation().y(), 1.0, 0.01); + // ASSERT_NEAR(pose.so2().log(), 0.7, 0.01); +} + +TEST_F(TestNode, InvalidMotionModel) { + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"robot_model_type", "non_existing_model"}); + ndt_amcl_node_->configure(); + ndt_amcl_node_->activate(); + ASSERT_FALSE(wait_for_initialization()); +} +TEST_F(TestNode, InvalidExecutionPolicy) { + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"execution_policy", "non_existing_policy"}); + ndt_amcl_node_->configure(); + ndt_amcl_node_->activate(); + ASSERT_FALSE(wait_for_initialization()); +} + +TEST_F(TestNode, SequentialExecutionPolicy) { + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"execution_policy", "seq"}); + ndt_amcl_node_->configure(); + ndt_amcl_node_->activate(); + ASSERT_TRUE(wait_for_initialization()); +} + +TEST_F(TestNode, ParallelExecutionPolicy) { + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"execution_policy", "par"}); + ndt_amcl_node_->configure(); + ndt_amcl_node_->activate(); + ASSERT_TRUE(wait_for_initialization()); +} + +TEST_F(TestNode, InitialPoseBeforeInitialize) { + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"set_initial_pose", false}); + ndt_amcl_node_->configure(); + ndt_amcl_node_->activate(); + ASSERT_FALSE(wait_for_pose_estimate()); + tester_node_->publish_default_initial_pose(); + spin_for(10ms, ndt_amcl_node_); // ensure orderly processing + tester_node_->publish_3d_laser_scan(); + ASSERT_TRUE(wait_for_pose_estimate()); +} + +TEST_F(TestNode, InitialPoseAfterInitialize) { + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"set_initial_pose", false}); + ndt_amcl_node_->configure(); + ndt_amcl_node_->activate(); + ASSERT_TRUE(wait_for_initialization()); + ASSERT_FALSE(tester_node_->can_transform("map", "odom")); + tester_node_->publish_default_initial_pose(); + spin_for(10ms, ndt_amcl_node_); // ensure orderly processing + tester_node_->publish_3d_laser_scan(); + ASSERT_TRUE(wait_for_pose_estimate()); + ASSERT_TRUE(wait_for_transform("map", "odom")); +} + +TEST_F(TestNode, InitialPoseWithWrongFrame) { + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"set_initial_pose", false}); + ndt_amcl_node_->configure(); + ndt_amcl_node_->activate(); + ASSERT_TRUE(wait_for_initialization()); + ASSERT_FALSE(tester_node_->can_transform("map", "odom")); + tester_node_->publish_initial_pose_with_wrong_frame(); + spin_for(10ms, ndt_amcl_node_); // ensure orderly processing + tester_node_->publish_3d_laser_scan(); + ASSERT_FALSE(wait_for_pose_estimate()); + ASSERT_FALSE(tester_node_->can_transform("map", "odom")); +} + +TEST_F(TestNode, CanUpdatePoseEstimate) { + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"set_initial_pose", false}); + ndt_amcl_node_->configure(); + ndt_amcl_node_->activate(); + ASSERT_TRUE(wait_for_initialization()); + ASSERT_FALSE(tester_node_->can_transform("map", "odom")); + tester_node_->publish_default_initial_pose(); + spin_for(10ms, ndt_amcl_node_); // ensure orderly processing + tester_node_->publish_3d_laser_scan(); + ASSERT_TRUE(wait_for_pose_estimate()); + ASSERT_TRUE(wait_for_transform("map", "odom")); + + { + const auto [pose, _] = ndt_amcl_node_->estimate(); + ASSERT_NEAR(pose.translation().x(), 0.0, 0.01); + ASSERT_NEAR(pose.translation().y(), 0.0, 0.01); + // ASSERT_NEAR(pose.so2().log(), 0., 0.01); + } + + tester_node_->publish_initial_pose(1., 1.); + spin_for(10ms, ndt_amcl_node_); // ensure orderly processing + tester_node_->publish_3d_laser_scan(); + ASSERT_TRUE(wait_for_pose_estimate()); + ASSERT_TRUE(wait_for_transform("map", "odom")); + + { + const auto [pose, _] = ndt_amcl_node_->estimate(); + ASSERT_NEAR(pose.translation().x(), 1.0, 0.01); + ASSERT_NEAR(pose.translation().y(), 1.0, 0.01); + // ASSERT_NEAR(pose.so2().log(), 0., 0.01); + } +} + +TEST_F(TestNode, IsPublishingParticleCloud) { + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"set_initial_pose", true}); + ndt_amcl_node_->configure(); + ndt_amcl_node_->activate(); + ASSERT_TRUE(wait_for_initialization()); + tester_node_->create_particle_cloud_subscriber(); + ASSERT_TRUE(wait_for_particle_cloud()); +} + +TEST_F(TestNode, LaserScanWithNoOdomToBase) { + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"set_initial_pose", true}); + ndt_amcl_node_->configure(); + ndt_amcl_node_->activate(); + ASSERT_TRUE(wait_for_initialization()); + tester_node_->publish_laser_scan_with_no_odom_to_base(); + ASSERT_FALSE(wait_for_pose_estimate()); +} + +TEST_F(TestNode, TransformValue) { + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"set_initial_pose", true}); + { + // Set initial pose values to simulate an estimate that has converged. + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"initial_pose.x", 1.0}); + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"initial_pose.y", 2.0}); + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"initial_pose.z", 3.0}); + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"initial_pose.roll", 0.}); + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"initial_pose.pitch", 0.}); + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"initial_pose.yaw", Sophus::Constants::pi() / 3}); + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"initial_pose.covariance_x", 0.0}); + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"initial_pose.covariance_y", 0.0}); + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"initial_pose.covariance_yaw", 0.0}); + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"initial_pose.covariance_xy", 0.0}); + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"initial_pose.covariance_xyaw", 0.0}); + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"initial_pose.covariance_yyaw", 0.0}); + } + + ndt_amcl_node_->configure(); + ndt_amcl_node_->activate(); + ASSERT_TRUE(wait_for_initialization()); + tester_node_->publish_3d_laser_scan_with_odom_to_base(Sophus::SE3d{ + Sophus::SO3d::rotZ(Sophus::Constants::pi() / 3.), + Sophus::Vector3d{3., 4., 5.}, + }); + ASSERT_TRUE(wait_for_pose_estimate()); + + const auto [pose, _] = ndt_amcl_node_->estimate(); + ASSERT_NEAR(pose.translation().x(), 1.0, 0.01); + ASSERT_NEAR(pose.translation().y(), 2.0, 0.01); + // This is 0 cause we flatten the states as part of the motion model update process. + ASSERT_NEAR(pose.translation().z(), 0.0, 0.01); + ASSERT_NEAR(pose.so3().angleX(), 0., 0.01); + ASSERT_NEAR(pose.so3().angleY(), 0., 0.01); + ASSERT_NEAR(pose.so3().angleZ(), Sophus::Constants::pi() / 3, 0.01); + + ASSERT_TRUE(wait_for_transform("map", "odom")); + const auto transform = tester_node_->lookup_transform_3d("map", "odom"); + EXPECT_NEAR(transform.translation().x(), -2.00, 0.01); + EXPECT_NEAR(transform.translation().y(), -2.00, 0.01); + EXPECT_NEAR(transform.translation().z(), -5.00, 0.01); + ASSERT_NEAR(pose.so3().angleX(), 0., 0.01); + ASSERT_NEAR(pose.so3().angleY(), 0., 0.01); + EXPECT_NEAR(transform.so3().angleZ(), 0.0, 0.01); +} + +class TestParameterValue : public ::testing::TestWithParam {}; + +INSTANTIATE_TEST_SUITE_P( + InvalidParameterValues, + TestParameterValue, + testing::Values( + rclcpp::Parameter("min_particles", -1), + rclcpp::Parameter("max_particles", -1), + rclcpp::Parameter("recovery_alpha_slow", -1.0), + rclcpp::Parameter("recovery_alpha_slow", 2.0), + rclcpp::Parameter("recovery_alpha_fast", -1.0), + rclcpp::Parameter("recovery_alpha_fast", 2.0), + rclcpp::Parameter("pf_err", -1.0), + rclcpp::Parameter("pf_err", 2.0), + rclcpp::Parameter("spatial_resolution_x", -1.0), + rclcpp::Parameter("spatial_resolution_y", -1.0), + rclcpp::Parameter("spatial_resolution_theta", -1.0), + rclcpp::Parameter("spatial_resolution_theta", 7.0), + rclcpp::Parameter("resample_interval", 0), + rclcpp::Parameter("transform_tolerance", -1.0), + rclcpp::Parameter("alpha1", -1.0), + rclcpp::Parameter("alpha2", -1.0), + rclcpp::Parameter("alpha3", -1.0), + rclcpp::Parameter("alpha4", -1.0), + rclcpp::Parameter("alpha5", -1.0), + rclcpp::Parameter("update_min_a", -1.0), + rclcpp::Parameter("update_min_a", 7.0), + rclcpp::Parameter("update_min_d", -1.0), + rclcpp::Parameter("laser_max_range", -1.0), + rclcpp::Parameter("laser_min_range", -1.0), + rclcpp::Parameter("max_beams", 1))); + +TEST_P(TestParameterValue, InvalidValue) { + const auto options = + rclcpp::NodeOptions{}.parameter_overrides({GetParam()}).append_parameter_override("scan_topic", "point_cloud"); + ASSERT_ANY_THROW(std::make_shared(options)); +} + +} // namespace diff --git a/beluga_amcl/test/test_utils/node_testing.hpp b/beluga_amcl/test/test_utils/node_testing.hpp index 8b6a22521..4255b720a 100644 --- a/beluga_amcl/test/test_utils/node_testing.hpp +++ b/beluga_amcl/test/test_utils/node_testing.hpp @@ -15,6 +15,7 @@ #ifndef BELUGA_AMCL_TEST_TEST_UTILS_NODE_TESTING_HPP // NOLINT(llvm-header-guard,-warnings-as-errors) #define BELUGA_AMCL_TEST_TEST_UTILS_NODE_TESTING_HPP // NOLINT(llvm-header-guard,-warnings-as-errors) +#include #include #include @@ -32,6 +33,7 @@ #include #include #include +#include #include #include @@ -51,6 +53,9 @@ class TesterNode : public rclcpp::Node { laser_scan_publisher_ = create_publisher("scan", rclcpp::SystemDefaultsQoS()); + point_cloud_publisher_ = + create_publisher("point_cloud", rclcpp::SystemDefaultsQoS()); + global_localization_client_ = create_client("reinitialize_global_localization"); nomotion_update_client_ = create_client("request_nomotion_update"); @@ -159,6 +164,28 @@ class TesterNode : public rclcpp::Node { tf_broadcaster_->sendTransform(transform_laser); } + void publish_3d_laser_scan() { + const auto timestamp = now(); + + auto scan = sensor_msgs::msg::PointCloud2{}; + scan.header.stamp = timestamp; + scan.header.frame_id = "laser"; + + auto transform_base = geometry_msgs::msg::TransformStamped{}; + transform_base.header.stamp = timestamp; + transform_base.header.frame_id = "odom"; + transform_base.child_frame_id = "base_footprint"; + + auto transform_laser = geometry_msgs::msg::TransformStamped{}; + transform_laser.header.stamp = timestamp; + transform_laser.header.frame_id = "base_footprint"; + transform_laser.child_frame_id = "laser"; + + point_cloud_publisher_->publish(scan); + tf_broadcaster_->sendTransform(transform_base); + tf_broadcaster_->sendTransform(transform_laser); + } + void publish_laser_scan_with_no_odom_to_base() { const auto timestamp = now(); @@ -181,6 +208,27 @@ class TesterNode : public rclcpp::Node { tf_broadcaster_->sendTransform(transform_laser); } + void publish_3d_laser_scan_with_no_odom_to_base() { + const auto timestamp = now(); + auto scan = sensor_msgs::msg::PointCloud2{}; + scan.header.stamp = timestamp; + scan.header.frame_id = "laser"; + + auto transform_base = geometry_msgs::msg::TransformStamped{}; + transform_base.header.stamp = timestamp; + transform_base.header.frame_id = "odom"; + transform_base.child_frame_id = "unexpected_base"; + + auto transform_laser = geometry_msgs::msg::TransformStamped{}; + transform_laser.header.stamp = timestamp; + transform_laser.header.frame_id = "unexpected_base"; + transform_laser.child_frame_id = "laser"; + + point_cloud_publisher_->publish(scan); + tf_broadcaster_->sendTransform(transform_base); + tf_broadcaster_->sendTransform(transform_laser); + } + void publish_laser_scan_with_odom_to_base(const Sophus::SE2d& transform) { const auto timestamp = now(); @@ -204,6 +252,29 @@ class TesterNode : public rclcpp::Node { tf_broadcaster_->sendTransform(transform_laser); } + void publish_3d_laser_scan_with_odom_to_base(const Sophus::SE3d& transform) { + const auto timestamp = now(); + + auto scan = sensor_msgs::msg::PointCloud2{}; + scan.header.stamp = timestamp; + scan.header.frame_id = "laser"; + + auto transform_base = geometry_msgs::msg::TransformStamped{}; + transform_base.header.stamp = timestamp; + transform_base.header.frame_id = "odom"; + transform_base.child_frame_id = "base_footprint"; + transform_base.transform = tf2::toMsg(transform); + + auto transform_laser = geometry_msgs::msg::TransformStamped{}; + transform_laser.header.stamp = timestamp; + transform_laser.header.frame_id = "base_footprint"; + transform_laser.child_frame_id = "laser"; + + point_cloud_publisher_->publish(scan); + tf_broadcaster_->sendTransform(transform_base); + tf_broadcaster_->sendTransform(transform_laser); + } + bool can_transform(const std::string& target, const std::string& source) const { return tf_buffer_ && tf_buffer_->canTransform(target, source, tf2::TimePointZero); } @@ -216,6 +287,14 @@ class TesterNode : public rclcpp::Node { return transform; } + auto lookup_transform_3d(const std::string& target, const std::string& source) const { + auto transform = Sophus::SE3d{}; + if (tf_buffer_) { + tf2::convert(tf_buffer_->lookupTransform(target, source, tf2::TimePointZero).transform, transform); + } + return transform; + } + template bool wait_for_global_localization_service(const std::chrono::duration& timeout) const { return global_localization_client_->wait_for_service(timeout); @@ -242,6 +321,7 @@ class TesterNode : public rclcpp::Node { PublisherPtr map_publisher_; PublisherPtr initial_pose_publisher_; PublisherPtr laser_scan_publisher_; + PublisherPtr point_cloud_publisher_; template using SubscriberPtr = std::shared_ptr>; diff --git a/beluga_example/launch/ndt_simulation_3d.launch.xml b/beluga_example/launch/ndt_simulation_3d.launch.xml new file mode 100644 index 000000000..c8a998228 --- /dev/null +++ b/beluga_example/launch/ndt_simulation_3d.launch.xml @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/beluga_example/launch/utils/ndt_3d_localization_launch.py b/beluga_example/launch/utils/ndt_3d_localization_launch.py new file mode 100644 index 000000000..863c45f04 --- /dev/null +++ b/beluga_example/launch/utils/ndt_3d_localization_launch.py @@ -0,0 +1,146 @@ +# Copyright 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. + +from pathlib import Path + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.actions import GroupAction +from launch_ros.actions import ComposableNodeContainer +from launch_ros.actions import Node +from launch_ros.descriptions.composable_node import ComposableNode +from launch.utilities.type_utils import get_typed_value + +from launch_ros.actions import SetParameter + +from beluga_example.launch_utils import with_launch_arguments +from beluga_example.launch_utils import ( + get_node_with_arguments_declared_from_params_file, +) + + +def get_launch_arguments(): + example_dir_path = Path(get_package_share_directory('beluga_example')) + return [ + DeclareLaunchArgument( + name='localization_plugin', + default_value='beluga_amcl::NdtAmclNode', + description='Localization node plugin to use if composition is enabled. ', + ), + DeclareLaunchArgument( + name='localization_prefix', + default_value='', + description='Set of commands/arguments to precede the node command (e.g. "timem --").', + ), + DeclareLaunchArgument( + name='localization_params_file', + default_value=str(example_dir_path / 'params' / 'default_ndt.ros2.yaml'), + description='Parameters file to be used to run the localization node.', + ), + DeclareLaunchArgument( + name='localization_ndt_map', + default_value=str(example_dir_path / 'maps' / 'turtlebot3_world.hdf5'), + description='Map HDF5 file used by the localization node.', + ), + DeclareLaunchArgument( + name='use_composition', + default_value='False', + description='Whether to use composed node bringup.', + ), + DeclareLaunchArgument( + name='use_sim_time', + default_value='False', + description='Whether to use simulation clock.', + ), + ] + + +@with_launch_arguments(get_launch_arguments()) +def generate_launch_description( + localization_plugin, + localization_prefix, + localization_params_file, + localization_ndt_map, + use_composition, + use_sim_time, +): + use_composition = get_typed_value(use_composition, bool) + use_sim_time = get_typed_value(use_sim_time, bool) + + load_nodes = GroupAction( + actions=[ + *get_node_with_arguments_declared_from_params_file( + package='beluga_amcl', + executable='ndt_amcl_node_3d', + namespace='', + name='ndt_amcl', + output='screen', + arguments=['--ros-args', '--log-level', 'info'], + prefix=localization_prefix, + params_file=localization_params_file, + extra_params={'map_path': localization_ndt_map}, + ), + Node( + package='nav2_lifecycle_manager', + executable='lifecycle_manager', + namespace='', + name='lifecycle_manager_localization', + output='screen', + arguments=['--ros-args', '--log-level', 'info'], + sigterm_timeout='20', + sigkill_timeout='20', + parameters=[ + {'autostart': True}, + {'node_names': ['map_server', 'ndt_amcl']}, + ], + ), + ] + ) + + load_composable_nodes = GroupAction( + actions=[ + ComposableNodeContainer( + package='rclcpp_components', + executable='component_container', + namespace='', + name='amcl_component_container', + composable_node_descriptions=[ + ComposableNode( + package='beluga_amcl', + plugin=localization_plugin, + name='ndt_amcl', + parameters=[localization_params_file], + ), + ComposableNode( + package='nav2_lifecycle_manager', + plugin='nav2_lifecycle_manager::LifecycleManager', + name='lifecycle_manager_localization', + parameters=[ + {'autostart': True}, + {'node_names': ['map_server', 'ndt_amcl']}, + ], + ), + ], + ), + ] + ) + + return LaunchDescription( + [ + SetParameter('use_sim_time', True), + load_composable_nodes if use_composition else load_nodes, + ] + ) diff --git a/beluga_example/maps/sample_3d_ndt_map.hdf5 b/beluga_example/maps/sample_3d_ndt_map.hdf5 new file mode 100644 index 0000000000000000000000000000000000000000..ed299db32ed6c8b86fc7f15a0c269843ecd464c8 GIT binary patch literal 33112 zcmeIb2UHYW+qT)FfPeu(!GHn9h!_BY?ylOk3j>NdU>0*$6fsAF0xBX#lBg((Sx^zW zyNWJz#+)(doFj&*yX@9;-go`8X3bi&zWHa|XSLUJJ$2VpyXt`29OcY!-J(@#+sd{o ztdb>FB~$O+FK3$npSf;bTDDZl zN%MR&O{b}vl=xNEGdKS7|5s0-bxXbzZn&rEuKi3rYWg@cscP`hF=N7H89Q`f2sn7$ zIhRy9&rxcXJiVlH+@koe%ak;IrWCg*KGpJ91|u=v z^cgVKbmspIx_0Q;qL{pulBUboFyHw9Vms!~d+Falt^8Nod^oG@e;xW)IO}R!a8$_woPycr$&F{5$m5{yu@fPvGwp`1=I@K7qea;O`Uo z`vm^CeFFc3??=v*`}NfRU-f?Ezn{micZS7^|9WHgpT9pgzg_uX_5UxwU$gl22JrX$ zwPGb*em$PgzW@H<8u4%af7t%-zOTqKy&pDj??0c<-}e>fC-awopTOTI@b?M)eFA@< zz~3kE_X+%e;|YZAZA(&P`-+JhdNN?+hQ-;lZ^s(o;LQ44$=L2r#4$l5S{xlIf?~JV zOwg2D3>@sYC7ry}?jnarlovLp)nh^1pi0b{l2^5GFz8Ai>*{Prb{yy{E-Y4(1sB}% zWj9})CBVVep7n_B<@MSQg1uqJou0nnmMSHQ^TOdeIEdqT*(XjDb_o7@F#c#dvFJU5 zfrGm~Z&T*%aDJsP)01PsA&a+=Z?X0a9L(sjg^Wu*&UQ|pB9c~@4dAg38N~YM5f%|U+RNmrlGqmAC{K<|D7$@w1H8n{B66f!~DO`gB)%QV@zoCWI+e##yY=814{NH0n( z0#~vtBQG&G%LeGdl25mj&(1}9IB34FO)E2)cNqgT;8piMn z-n;q?==SCSsolS;00#rlELSoa&sCbZatCyv{$(yH*k8i{2aE5@R5HQSc}=%=UTW}| zK9kVi&los3uSPn_SU8Og)z@e2UGK19z0)HJQ^A6TgFklh%A6kqB!f$dQt%x+R{ zppzaBn(r(8b5lXPsg)j#IklIBYi%`(%XHqiMVWJKWv2GR;}9(vIerfr;X6WrgB9%8 zD47fGD{DGe_tk^rd@@P7gPjd<@Wg>-O2(nsMqQ52ISp7aaXq0O`!H~@%YBJFV^T%iM(`5)5AgYecfmpt$rVI zMh{MJpGhn&BlU1FX#TJJIy}Ct;py=m47fF56B%?z65wEsH#y3lljdE}bv`mq4;EHf zN0^0{3>RM9m z&v_hKGo!nk{6L+~~BiC<|0XmL1$w7gbD4+qWno-{bET5G zbXcz+Y&Tg0R%pMMaCRjHIM}lM9`eei2^r*{Y?R@OPf@ZJk3@E2gj5MQRXb~TUm3uV38iYw|gTQ zJ!CNh2Vcbg`W)PKD^Okkymv0R|62^(xP80^4x0D8VrM+l@2jK-r623ajjN>vIJj|J zs&cLDC94=aL;pO`{o+P)zv6r?94x=gf(`YtU=Lia$y#phDuB+%1=7IfyZ{Hsm+Gv{ zIq~tBHegHx9T@WPJh|AbtN;hS=Wiy>@-s>6{*i3R

5m-(){icKjeM92_@d6*(KR zhPV&u$_^=QA%ZTsDUAA9mL3jjh3)Lv;c4V}!{*w_`I7|@_j;jIt{M)G3=v4ij83HV z;NFalHjo9UH=N2`Z%SA=I5vJRu}fP?7R7}a7Pas@qdAIOSEiCm0q z#ST1QT>zbQ8_D}qV-*Km6;vVK8w|v;$wKBtP)`vIewIZx)~l_BgYD&CORHrMWS*^y zV%Epp-3z97Ok`iSJ?IMu@l5yJIYfjpJX2f$N&qLjxD)F}*Axe1-ppY8j&voS4J(r2 zQD=1E;g6|gQU4t}IEXoYS!==++|brJ6k`Bw)~#U+cUuc^FrbD9V_UW-xkHAr_jmNv zgO2L)WN($qesBVEcdV%$&-pN*|Edm5wHXBr9CX}ug$*sSj@<7ZtG(0cvIe|)CWLJ1(tO#xXK*@PYXeTE1gZ#SE)=ulR1a8cYA z_U4rPgx73kf(M)zz+I10NY$(52psHLwgcDDu97%9&YaJZKGfi~%B*R@ersn+?BG@`GlFhn%L4bqNfu3yh zlF{UDxfjg0lJ(T!$cl-~x(EJhIEXo}noUI2YKf-mw~hjc9_z7I3kR=6IIcO8SqrXKReM$p?3H$4Fl?LJJ3P=hq_>BF7M?IhjJo zW-~-kqhCQT`NoQHFz|>I>2frS+0~{w^J!j+ANZrje&*KYTRw0Q*J{9pGt-SFnWInJ z}!lu|s>z72#lTnZD%0;!>k%UIa96T#+B9ZA5k?jjsasC|Gv={uhUI4&lBZ|nr{HNS_nZRRDw zLF^|}J%O#_9xWWc6{fJ*tr&Jk+88|?oO(8vIG=7s0*5>0yHBgmf|2LO68qCsgo7(x zBiWf(Z<0Q5J_)rptkQxm9>4T$?#d8(Qp9E9KbxPrW@@2|NqLKHysZp#!72g7oJsuEJUBSuei8^u=m~)|J6*9|j zCBu6>Gk|gZS~F*^uGYgrWE}5Prt9%ehN;Z%T(D{7kIcZ1JN)1vGW+`^kfOmqgz_b7 z3SiCrG*WeoP6G##S@=Abw2W~hZ>K)cfdOA%5Z8UR4R8>daUc7VLcd3b%rjTDpvNv- zVi3RU;UF@nEo(6+8%AiE`lq$v%=Lb3nboO!IM{33SmpN#PaVS5bwd(zz*lSTXy4>5 zR~*EgZ35J2>nO{=VfgWqb4#BbXrHhAhVL*e5{1BlFk=-qyB z@Re64$xO*0eU3UXW$iaGVA8pnjQ^b%ii4fPF0vob^(AAr7l_^?duqVOZn5l>F9&sS z5OW9B&Lj>q0yMj3eA9#UXn%ICm#+W^zfNew3~r!jvm3Nv^LM?}f`{D)lb%B_Y2hH| zcr1@$y)^f=4YTIx!5{VA$)&J5dN_zoVxlJr8WpK6yZyNqj6M`dUi|biz(Hh|tL=$# zjL1$}*i#SQZdj7E3Cz&JL1gOI9ZrfZ_GG&?U!((zz7(=i6C3N`AToIMcsNeu>AyP< zgfmulzHkukty{^=-a1qmTqROr(UmW3QfLJc4j$u+DIbRe#_iX+j4IHAIM1i`00SKC zKmQ`z{mgb^DU?-TC_h#MBIBQRKNk+ddkQC;P#{>3hc$e(5xJ z;@G|VxE6T^5Sg*dCi=la__#42*x&~Zg%<~O3egL$c=MOb9R4YU9DcEcwF!+Bk~^(e zgZFC9WTtf9)C>;dobHC2Z1tTU!sOY38qhAR5|jQQDi;p!4B19vZJpSbjVlRHZq^n+ zVU+{Z;Ndha9IP6ULA(=ENr$;9!kD7h0=QxRJZ9y$FAN+!p(3G2)*8J|hkovx@YOsJ^^TZi>x2Zgx{aIlN94>RW4R(9OQJhAZe4-M#F zY#(tPC>r1(_Va3`J8AA;imkOHh*fx$BNsJs3>@5d?kYL);?VT>|w?^r?{=`=z!5*h+q;JL@emz`?U` zPO`|Ia|qP?wm9hv!hIV1`oTfCVcAB~#BQ&l@r$Es@a7s9rpM{;l<)JQC@hJ)94Mv;;Oj*>Rdz4cpfpVouD&n{&m4%b&4#GD)R(@CS*PyF^> z>!AVBTlH#f9vs|remk*hYb2FzJ~8jIO0(dI<`tL*{58eF>Z8Y!lR2Bnoa>df7FiPo z@Zia&%!>0H7&zGO#BSEV|50N3qI$mi>0twC-+MI~cg9x-2XXEAay2>snPUX2=_5rD zy}Zz-3>;i#{p;(!opZP8&Isq#ATow7<27*bRjZb4=$Bb!eefOq=E4X)=rLv*+viG@ z00-@Ne$Xx%c7v3;!xJVe-T>|ou^{DYwKBj#%$?D+1Gib*kWAX=&4M0Iw@HuFO%w-T z&J{?_{;_O7hfsEg+Y|w`KU|&^j=v(n!8`JQo7wfU2HRc#gz;InHV?GDA3%Ook#ukn z*SbE>jvKKuLUVT2Y8I?~-Himhm0;oE8g2(E=+&1yJUg8EnY@<)pSk#w@;fJL;2`Ea zt#wlS?crT!!h>`*=u#AZ zipyibofm`HBieBWIJo4&S*G8{W{lUJs`|%18EViY-JLBlf0GUlp0W_NxA&YSRZAAI z&%;%EutL^u`RnXHIyi{^L=W|2yJWd&J3OqxfDM~%CzboQ)xg2KpZuAPwKlPwsopGZMbd0RR6lUI2!$DkYV8sh$=$QoO%E0w% z5WQ~QZG7P%yk(n3qU_~=NPD$#@LauR%=vjmtdnNC_?FDmfKF{zk-3rUHE^p17-ZEf}gf`FgV-i1Qv#-Jyqr@Zc-1?5(Q(7`tiRG$5}z z&qmC?t%rliT-!dM9qy_XRBcBIU^~lV-1^Ef1P;zRae_>Y8OX+;&(gda6Xgfuyzwr{ zesB2u;l1yJy^6LgCtE)*TX?%R_Dwm`^SU}r&mAC1N-~Ll3IPT^l%Uvd)JO+c|^J} zd1fm;Sg^b@sTnmy4+oJ+7!^bIe=0Pba42Q~i@o$_RuBs{97M)CH;HH&Zy|YthXK5B z<_626E{xvKgcTjv!$D+%=m>JPcYuZ*7_R}*d${ANFC4VvH(Qg{o{qq^EqP$ zF!slJ?aa+B4RCO&eH*sNYc*TBLV@60ZH*eNTD&!ht7K(>gSZykCytH2K1e*){}-)y}#$IT_``J}H^afTb*VvA2U- zD-OnwbRpN%DiHP(Wt$hIir~+M1~&I+TLBIR`!6AHcJ3!<6Q&8{7nRb2U8jvCUA7m~ z!a-cC^pk6B|FY4-sEB$B%f#&@_bgh8aByeSeQfO0rEEDbOYvrH3la1keU|vAYX}_d z(>jFsg&EmT=YJT2xFs6!Iv3288u3B{2XU<@$+uX~+N;^Atq$nHEeGStHfmEo{%>W0npEZuH z$bD6qc6Hoi21M`X%=HF1sO#E>Jbhc5y^>p?(Fb+s17%apI;hy4?@`z*`ZsnX@O>Xy72OJ+aqv(z=0` z>AK;y0HRkgAwv%b%e7iWMr?0Ec3Irg66rwwb#ZI6joe0N;jcdM*&2yg0Mmh2V0(< zPaMJqvfpB_`sG#MYygp|C0@|M!H?Pg%tgl-a_GYNe2t%<7Hsk{mt@<{HNZifvw31A zl29~79NMj`2wn_JVS5i6z{0^Pk%vgJ$|74X_K|SNEl~hJI|@wo<{4Tz_;t-Vc1hX6 z7I!8sH$Vl|Lw!oj$pup@VZz1DHH#Gs*515!(2m>5Mrl{X%rf!^?S@tyC z0KSe~%?w$Ur-y?j1W)j%qy3uIEej(mR`kFv27%TcM=U? z?(=-sus?)>gWKNQG2GrH;yZmTqrP5A11`|FWIUPT8aSxj`|~7X@o)m;KkB{?%&YKC zJEm?s4IF&*dJy?s5Klf{Siq!|%o0JDxev7MoVFR@;PHZv#O>iu5_R0joGwK~u;!Fa z%*}oy1UP6_ScAP{TuP2)57q}Co1_EFoUmg19=oqNi2Zo=Od~VGm-^Y=E+>NMbxba$ zhJ%xnqe#AAF3G(YpsqP(w+2L}EeZF5gVL7@Y;f(Rq?xvgDESxHgPr;&5a)vl8aRk^ ze4aaUbM9J;gS5r-!KRMSwf)C-XW-yL`8_Ri6*DyY+|dU#;6^LGHZ*>{;-LGweyr`b zU8KUgM&$G34hAqRaR=Ej%hLb{F=OKC0CMA9Fk7v?rvTn@xXZR#ZpFaCj$nHu!Fr8IZA+o?9ENg^(-B!Uh)guZ1iLUm^4=Yn!~g(102M)>g3A57W4_# zrVe;7fSFZ0lZKD|^>A>N?OKvMtP=C&yqoa;!z>**;F%j~?x-`s!TLkfnHzzNNso|J z!8f>t2Aq(#njF1fQwIlet#adEvGaFuOy5@X81UKW-R%1)8x{_ZRN0czrxudU<@5Bh z-y;PO=dHiHKnDj`kDS5mua-d;9S#tdI>hL}g*26`kvX@k2p*g7Xu#eM z`NTc3ycQ1PTC=hiu=)L~8Qv5-$$+PaZzN@ox6{BuyRJjn4aZB8q6yEm9V+%@z|dCf zN&d}Kii7SSIpx>1O`XzoGt?V&;LymX#8=%?3kU1PW@x|LW|H9pGPFUlmKv~X^$Db# z?xzk8Vn4UK3@7oCcl9cF!h&76d~J?CT!4cw$4n&I2@6Swj`Q^@{XRX2^U8mZ5#ZqU zFL6ZEp$sWE{++Np-bDa64f7?TS6CJf9xPd$T|0IX+5B~w(CJMLJ@`rO#Vlx&u7-n{ zJAL;W@|i5rxJKGCAbQ)IZSjSJ`r(@h(>IyCD7{?ph}889!kKzn0^hvMK~iwx!Kq7E@BeghY` z*MT^%*vSw*931tcD|>e?CBnmg!j>HoMk6*Y~oWbm{xF#-Ta9c;9zL>6UOmvCvtIK9cJ5?&Ia&` zb`!ZCpfbQgT&pnYAiL2k$1wMKRRN3|6ij9_m9%hB->EkFI%5%OW5;NiOND;mYuzR? zxA-z2IEXnvrp;$g?d+`Sba0LibbsbgItCs$z(HiT#3qpEuZ|0?4^7MiyG1T1^$SCC z;UF?x^%$mC{3gxU)fsy5!3AG3_PcjJ96UeFldMRJVNSGi(&cUZ>I*LI9>BPH@&X)O z8`^?NX|b17&Z?YWXHZiu*vmGG+$mdC2M2Mj1p(1a*=KQiQ;x6q0|(z9$((OBM1X?< z=>r*?n@8D3cODBq9l~|sldi)_pO`{59K@XN^M;X%y%??Kih)`%DQPv^WnZS^;3KbY zOoOtGNzSNkc_Y8?VL+VsF|L*`9CRJKgZN!&PsVnRQh!e@(1Jn3gP9Q9Y9bsYqt}x4 zkt4~3Pm~#ux{3i)>Rn*=I+tSMAhTmK@hHqBQ?4c%8s{Wvz}AgRvG2~^(ZfMpJH>7< zp%!zQ8v}PUAbN*eN2%f9`!2o6^8;In=WdSKwz9t-M5e;}YdSbMs9_KpaC&V==y<>cNYQU9224=gkNdpIwskm(jd79Wq$SKJRAbOE48>r#nguPoxP-ZkKRj-q9 zc5{>-RQ9;lRs$a-b9K}*vREBLhSMBF!~1*npkcsfrbNG30S;~&<*WRBc=Ov&&HQKO z)!>h}l^EYCb#!nLbMe}Fsj0SQay0|!KFWca-7C=m2X&48mER*Z>R(1%CbY92WDbei z4K7s-aPWBwRWhk1R_GTL9#ezX`76lj70+~V(0nap$02@A7wyjlGft$FB=)-i2TxwT zuk87G;SSB6C3$M_NdL9u@X!h(9DKUywUT)}`LbWZvt?@V%fk#}`*Vd34w|pkq=H&~ zd~bOjIHK-4a%5+K9uBs(IH1h=xcjO8;Q7%SFu7$k;jMWQ4ptd>O!=H!;&v+6t<_gG zc)))Pnd|x47Y>@Q)ookLyxy})`+;Fcw-D`)00SJnTm7_htqR6rI>%!x^1ubxx)X<* zrx`eS+Ifv~t!3?IYub%|s|F*+Zy@!nrmEqf`C7qKyz=sH52ihz|TZcLTZ6&Yy*YyFX1*GIP5f*Ov=2 zs=?=uYl&0YIK@HpwMNXzP=}pf;0v}|zmDvXI=INxS z)`5kCt>-jVGVPLY>5DJjuLjFbSV0=Ea!?#JU+boeg|64KrFr1x5%QnK$Z5~O!7Z2N zk0<%_;-*!iX4}GX8t``GY_h8!!@xoQT8ffUUm9qrcqqsK9&>gk4=oEBIN0Y(E#=;O zxN-WcBW%8uo^@!ZNp179K2NN*WYWqIcb(YD63EhBIDs2 zs)2(w)+H(Xxt%><=bUjs4`0Kl8;2RH`zkeZT`ksGO z=<~p4FQ>*@wp{ANj@^Ry@3~DT#tMpChDt-E=|F3gReU*HUc@Ao{h-Fe9HnpCkJ6`q`sO)G#?+Tj`?DX2yf6pr_kJMsPv1NT z?Qm0H$(r{gUmr`^5B6f-5Bl^^-#kY-Zt5$?jlVLev6TJLKXa5mT#^sUr2nYHO*ML0 z$SB86drduKGufxk^v&n``s)SNd}N zcOQF{?_HUR+I+4|b6o@eeWGvxN~K4!`1gi9uxYD9kb z|B+GAfAmc?mHwQI+MH4Pri==GdHvppX{OSrfBMS4$&75PWs zoJarneEFWtwMx$Z&d9a751MEG?%_P?S6_MVa*buK6`C?C`j5V;#`973EoV@hGfLl- zQT?N@WR0ddGR=9p`aK8rKl_ya%A*}_suh~&$UQ7&uhc|&Kgi%*6?&5VGG9-wl|@Z` zIcv^`;srC)t>t&*3yR-UOTqXOlZvVUCLRIB7=uFZM# zeW7o@FLQ0qD}C%+&M5a{nu*$+QTnEg(lcd#C4QZ&^#74jQPWKHOnK}}$>A9Kc$sQ* zUZE+E8VebvZ^|ouxraUlO?lK<=qoQ%A2rtRzG;rqrz+K2`4uk7FF9|n<@oO!`~2OP zv#9ZG&3V*V=Dv#ls+DJJnu!_<8KrNYDIYh_0kM!lZN4sgzcccA<{H;A*YrPX98>1} zqaQBUSik%7EOV`*e`+e%lKcvne$6-6DtVb}?9E)Om3ue->Z8U& zM(LYp%ExhE=!011+I(H~%o)^J${d;I3~0`v#!}{>FTc#Siki-qY0jgEg$!zQUg^t@ zK1JX7r&h`9ci%K08I}A(KO8Sp9yQkQzIi`XZptrnpUU-Z`4ukxI@esQ?&UFNx$ZpYq|TUFXz$6Gg0oxR3q}cZ_1dT5BlckW3F*s z)R6_=sJ#z+!m4%t|`aMU^nlnld2QeRg^BnpoqmpZ7VGjLwMmZ+;Df&kF zxT%(_-?e!rGUj}^T$}T#m4!XXwK=c!Ww%FP`jt1=$eU~AQJbH$xi)8%zA1xei|ZnT z8p~Xp=is0{64jIw`IM(LZcC1+IftI0p^%UsJx{`BQM`sVvG*XE4UH)YKCiwtTkbB+C@ z#=h_}*XE4UH_wrq<_wNwDRX50o+D??8S}GJW}>k!{nZbbY0jg@Qm#w?%vAcce2q8Kl9tD%WrxGL6ycpT<^55q-F;u0n)^b^v^*r$ zQ7@)n+rO2z4>>87Du11(bo?f*h+8am8slhq+TfeiF>{?1lpV=wmwuJXEL$tR8Az$? zw9nFmQ>l_Feii-Eq)1vOB})e_ZqPLYUq~*&>!e0+ZTS7$zet_(no46&br#;fD3bEG zveFv$G;V8^uTtw8&83|AEokW)Mbhnct)z>V>2zAHBI(isUh=qK#e8P6O|= zQn%MpMycUV%2jwJje9V|IO>WWUu?=}Y2xQ-?)2oX!u&_yq*ZJbmop-od-46N)Njjl zF1x*krnwhM1Ez;@c~jHri%LaO)U83BvDhtI+xwl=EIx<}A7jH8Z~sv0yo}JUYYK%O zVMWp`O6la|(Ol>E{NT zQm#nKYMDmA9=c8Y27i^R?2e zeSuEhxq+s67fHRloTo86Z_?>Io=Da|OYpiPo%QVE%|+uE70Pu8Pb%>XC*7g zS=>cqnsm|iq!eGupGFK?B@M24SXyOBpr2h?s z9kWW>cB~9<*CAc{#&}D`8aVNpUsg+#+RQLclJifWrAk9qYPs-L<@g*AgOpQjG~KyP z{`(6nS4fRR8`52Ij{K(FtfP-tLI}XWnzGo^ZmG#ezY5X^wwW$ee|%j{%`_y-pxw3XS1ay9;x(T&6ZO8!KqS{xH9}x zV|!^-1FgiwSKwbwrP93o7^6PFEdOwZpS0C=I5*VBo=*&EE~(de(Ot3S`45xEN#_rz z(ovnA`FdrBNZ!Irdaj$3_@Y!-X-1W^)N{2X&yHoJD(5Zv+O~UjZ&l| z18w;&b-s%?B0VL2P(LoZMydP-x+c;-T_9bZJ}^JE29aD}Y@+F1TjodVeWc9Gx9EpH zo<`>#tkkr1Y2K^vB4a^&J7aSHN#ZBFP5HjXYZ#wZ&!-3OHZxvOMHt70l;RuiyOBTQ zi8rU6lux5KcQxu=hI4+;OY@bJos8FCa8&=qmOplr&Ci~=f>!qM6sCl@=f9i0f>sp zR$AcePrD>Ub9aUwl|l;^(MdPwbGa){NX>^j@S9TSbJ6?PNj09Z;^Ts;TuGOmlK;3g z+9zc-S$A{3bawp>%FGSn?i4PS4*sy_^DIj~p>qT`v3zv?IY%Gy)tN+2bFine?naRw3(w<5u#=71QzUwI?PxBx??L0% zQdayHi;0|nL3!>$b`3FUP&&6R!i(GPFH+~#C%IaS+_^gYj?-JBn)AEVi|Z0^%{$$j z%5kUr(KYfPyveweM{W{is z$my}1>!Copv-fdn;YK2oE50;v{TXRS!ffutvQ~7*{xj0y>KrXR=T9xC9F^Y0ETU!l z*P;U&ot6T(+wd`|l=?4RFEM%|j&^84`}Wx_1&YZu_(ECwgp4DV-qHeoW|WS8INdRSzy^+9(?DXU#86 zElD>v=t+}DSo5d-uhW5_5~-p6S*fYzSD|0mWa?7=tkf=WBKImYk=nYRm1g?$bl->7 zlxciKI^1P3{c(3Q?PI@9DrZ$yY`J_LEj@I%G~{VA?XVz@x_xM3EPq|3g+t=#4|{K; z<4B42n!AGD?Aes-W|vPbuP>#M#F?wtQlhmzGN|R9TJ(O;YGSKCyXZtgq(#oN3_q6V z(d3NLw*@?=#w2I3>OIn9Z$N{)~3pdP;ib!qWWB z_o!Q&BT}Q(cpCHI5q15RBz@^_AwJ&znvPG7ljhiV<9x<_pot#QQjoJVopa$1Z5FUw zns_~#mYnv8o~p%32@PX}+M{35*-tpBZu^$pqccTRa2+O9^RGcus=cCZ`?fZ2nHI{m z8t{%LHy&x^V@uG0!B=RUdt3T2yS7-V&j~uEM<#7P%7NSU@F;CkVn3bNiY|n?ZJuN*g|3+w$SB@WD^{m7-p2?M!9QdJ|PfKZI z8qo-2X`Kuv&!-o`ufGOnv#MgCMq z1v;tpXc~FCGQZ8aJe}+{myTFap1*%<35^xJ#l(}9_;p><>0r$|&fcvOA3I?+ZS{6E zeY~g&|FF?2I_Xdp9pdT0*PQ%==0>*_({@zgndC#XomWq;UUC^eZn`xe82!ocWvM-X zf08x7@k&3=?S=zyGufI?YhHmKiLK1f?pmDRoRUc6>o?+t;-R+h@e4iT4nx%20{G?SuIZMbt;?tI6WDk)fFO;f}=ytij5 zqvyEc;_!JMeAVrfjHS3pu8M;P-y?8}amfrTstGzxPL;F#BsO?W- zR5vfa*U?p!y;4IAYwO7eWs*8G@EJNkZ(AK$rhasI@IrL=Ti!Uqqs=Fe5zPwPf>;s*^cklL#& zi07Mk;CGffE3IK?bBnFo^4q_kmTK;6M&FI=z$d;sBl!)Dq;aQu@i%z+_s07;G3G`O z{;OrQ`jjv3vXkDc9Hx;5dr;2PhXPhBuhdf&A*SKYTSf3xgN z=|rj>O`F||w_Orr9P-{xY%BEOy{Aq#eip*GMeBR=Ul)up=HF^SU#7O>J)0$QGoRiP z613gNH`(rw)Eh`9`p1~$_B3Qv>v?F zLqqS|cA>SF1@W5Flc>j~2>L9j6`!1tNxRsz5<7kE!Rt3Ir{|k)=W2B8#kZ@PNCOWH zrH0nMd1tF6x}beDWi_37|8@6i#pb<5eoYUab3Q^3(gED~>%I8x>5(3rqZeDgEK`H`>V zXk@t-{H_Vsd|}#tdg<{5nI|QwL}~F^qcFZ=kJHk;9--VpDU`ouaayW-#+fD#oXEHP zwoW>g&|D1mna0OXkC7gY>&aF4F+qM`5hKN)u%SawPvB3w6X|kV12Ncs0>AIAS_;hx z=Il$4=NCqLOHZ0sqEo+(<2%Lp8o!idMXu*GzMyW5aYBPIu0;Q-{HFH9jGF1?>9D8} zKK}F~u3Wcn;{HM7`7_siXy6Q1)P;=aN0(Vmzi)3W>U_rV%}*_-Z(3`(NYxmANd9ul zH0(ehj2X>88@Pgo1U#g9JLg&K;5@CR&5@Nq|D=$i2h_&S|WO4XZI6C0hK&!@Yb zm5$qRoJT<{Z>V@yI{U$%POrC+f3|a-G`o3#c+HsZ@rpEfvFmemU6qF0H1xuiiZV z`0nM#F=MB3R*j}B6ray;Jep5W}|2j=SS7|V)^PJYiOM=?Zp%P zeEv#!8ojw^7uRD@4F95a2F*J%gg$j#zz?psgO=zKMfq&fR`8glUsK7P*ki;)4 zc18-)xzS1&lKGjvGNen5+lbrdtmIEOjgdZWmH+-$y%heedaiUQ#(@s1pUMX{43x&5 z@f7!aCGm!8J*0$MwYmNslK6~Toux%n%Fy5~DSTAQJfpf)1F^JI693U*opE_yUC#Y< z3V&%{6gRa)JF!>O)qI4-25$0@N;F1H;b(91q&Wd?#Vcere`#qin(=uy*U)b@KkH2? zTF}Oh-foi2y9F$#`!gGems+pp!@Qzth;?7`DP$Qxls`|OKlT*6>Q?Z55+2hjUy^C| z=;eI#@z(sK2LrfgE-UyUZ!P)N9T^%hc?HjWFU~LDyp+B>xRFnLc|w}*Ttyu7ER*+K zaz@&A$CZ0nViTW}C)Z=^&>urL@^*8RB%92(VyRMF__$)cbS=HDcw_t)e%1at#*n18 zV&s9%e6g@O+>MoOMLX>#e&&Nz?r>OT`ZZ${@1u34qqemby;IloqwM?9rtQPHOVif# z@4A$x_Fi_h*@jHMZSYb$YPN^i^8R{$@|b8kYV$0zV4ghx+D$6B_YfC{Zsz-1Sn-#8 zl@Y@aZsz;DS@S*Z!nqU8H}iGm--AgrJn8Y2&HUjv#pQ1$7)x(H%;M?CLy~mniD2_G zi(fzGuoS+fHg|PT7XR|i5oymZH|lMj%@-@4B$X8`#6w=$ykS19q<&^8PQ97MH}NJ?trOcw$%fhdnU0;M5q-+hwr{g|wN->M$gZsT zSe~P=9bueqzl~J($>yu?tZtmq$BqVl%Hq3k9L?Fhv=dLsnd7MwIi}18a?dWCU!)1+ zF8_3(H&xmE;bZlwV-qX!m7LjHt_kG~1tg?tHh-{HYq~VliGH@s<`1<>qK7Zqh}GoG z>^I5u!$!hwmiMzGI-M44U7ODNn#E^}SLlF@Dx$Z1FYOmzr>H|tPB%+1Q;o$D0m9n+_C_jhLT35|;LZCyR--sCKP N;wTHg`;b`re*hU1Xea;x literal 0 HcmV?d00001 diff --git a/beluga_example/params/default_ndt_3d.ros2.yaml b/beluga_example/params/default_ndt_3d.ros2.yaml new file mode 100644 index 000000000..19f7ec852 --- /dev/null +++ b/beluga_example/params/default_ndt_3d.ros2.yaml @@ -0,0 +1,91 @@ +ndt_amcl: + ros__parameters: + # Odometry motion model type. + robot_model_type: nav2_amcl::DifferentialMotionModel + # Expected process noise in odometry’s rotation estimate from rotation. + alpha1: 0.05 + # Expected process noise in odometry’s rotation estimate from translation. + alpha2: 0.002 + # Expected process noise in odometry’s translation estimate from translation. + alpha3: 0.05 + # Expected process noise in odometry’s translation estimate from rotation. + alpha4: 0.002 + # Expected process noise in odometry's strafe estimate from translation. + alpha5: 0.002 + # The name of the coordinate frame published by the localization system. + global_frame_id: map + # The name of the coordinate frame published by the odometry system. + odom_frame_id: odom + # The name of the coordinate frame of the robot base. + base_frame_id: base_link + # The name of the topic where the map is published by the map server. + map_topic: map + # The name of the topic where scans are being published. + scan_topic: scan/points + # The name of the topic where an initial pose can be published. + # The particle filter will be reset using the provided pose with covariance. + initial_pose_topic: initialpose + # Maximum number of particles that will be used. + max_particles: 1000 + # Minimum number of particles that will be used. + min_particles: 1000 + # Error allowed by KLD criteria. + pf_err: 0.05 + # KLD criteria parameter. + # Upper standard normal quantile for the probability that the error in the + # estimated distribution is less than pf_err. + pf_z: 3.0 + # Fast exponential filter constant, used to filter the average particles weights. + # Random particles are added if the fast filter result is larger than the slow filter result + # allowing the particle filter to recover from a bad approximation. + recovery_alpha_fast: 0.1 + # Slow exponential filter constant, used to filter the average particles weights. + # Random particles are added if the fast filter result is larger than the slow filter result + # allowing the particle filter to recover from a bad approximation. + recovery_alpha_slow: 0.001 + # Resample will happen after the amount of updates specified here happen. + resample_interval: 1 + # Minimum angle difference from last resample for resampling to happen again. + update_min_a: 0.1 + # Maximum angle difference from last resample for resampling to happen again. + update_min_d: 0.2 + # Maximum range of the laser. + laser_max_range: 100.0 + # Maximum number of beams to use in the sensor model. + max_beams: 5000000 + # Whether to broadcast map to odom transform or not. + tf_broadcast: true + # Transform tolerance allowed. + transform_tolerance: 1.0 + # Execution policy used to apply the motion update and importance weight steps. + # Valid options: "seq", "par". + execution_policy: par + # Whether to set initial pose based on parameters. + # When enabled, particles will be initialized with the specified pose coordinates and covariance. + set_initial_pose: true + # Initial pose x coordinate. + initial_pose.x: 0.0 + # Initial pose y coordinate. + initial_pose.y: 2.0 + # Initial pose z coordinate. + initial_pose.z: 0.0 + # Initial pose yaw coordinate. + initial_pose.yaw: 0.0 + # Initial pose xx covariance. + initial_pose.covariance_x: 0.25 + # Initial pose yy covariance. + initial_pose.covariance_y: 0.25 + # Initial pose zz covariance. + initial_pose.covariance_z: 0.25 + # Initial pose rollroll covariance. + initial_pose.covariance_roll: 0.0685 + # Initial pose pitchpitch covariance. + initial_pose.covariance_pitch: 0.0685 + # Initial pose yawyaw covariance. + initial_pose.covariance_yaw: 0.0685 + # Scaling parameter for NDT cells amplitude. + d1: 1.0 + # Scaling parameter for NDT cells covariance. + d2: 0.5 + # Minimum score NDT measurement cells. + minimum_likelihood: 0.01 diff --git a/beluga_ros/include/beluga_ros/amcl.hpp b/beluga_ros/include/beluga_ros/amcl.hpp index 4d0db910c..470bdb556 100644 --- a/beluga_ros/include/beluga_ros/amcl.hpp +++ b/beluga_ros/include/beluga_ros/amcl.hpp @@ -125,7 +125,7 @@ class Amcl { execution_policy_{std::move(execution_policy)}, spatial_hasher_{params_.spatial_resolution_x, params_.spatial_resolution_y, params_.spatial_resolution_theta}, 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(params_.update_min_d, params_.update_min_a)}, resample_policy_{beluga::policies::every_n(params_.resample_interval)} { if (params_.selective_resampling) { resample_policy_ = resample_policy_ && beluga::policies::on_effective_size_drop;