From d96a9277e0cdedd2da5200f43aca69d5b2b9c643 Mon Sep 17 00:00:00 2001 From: Ramiro Serra Date: Fri, 16 Aug 2024 11:59:18 -0300 Subject: [PATCH] Add NDT AMCL 3D node. (#422) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit ### Proposed changes Adds a new node for 3D NDT AMCL. Closes #422 Closes #311 Closes #313 #### Type of change - [ ] 🐛 Bugfix (change which fixes an issue) - [x] 🚀 Feature (change which adds functionality) - [ ] 📚 Documentation (change which fixes or extends documentation) ### Checklist _Put an `x` in the boxes that apply. This is simply a reminder of what we will require before merging your code._ - [x] Lint and unit tests (if any) pass locally with my changes - [x] I have added tests that prove my fix is effective or that my feature works - [x] I have added necessary documentation (if appropriate) - [x] All commits have been signed for [DCO](https://developercertificate.org/) --------- Signed-off-by: Ramiro Serra --- beluga/include/beluga/algorithm/amcl_core.hpp | 3 +- .../beluga/algorithm/unscented_transform.hpp | 47 +- beluga/include/beluga/policies/on_motion.hpp | 54 +- .../algorithm/test_unscented_transform.cpp | 63 ++ .../test/beluga/policies/test_on_motion.cpp | 27 +- beluga_amcl/cmake/ament.cmake | 47 ++ beluga_amcl/include/beluga_amcl/amcl_node.hpp | 6 - .../include/beluga_amcl/ndt_amcl_node.hpp | 6 - .../include/beluga_amcl/ndt_amcl_node_3d.hpp | 149 +++++ .../include/beluga_amcl/ros2_common.hpp | 6 + beluga_amcl/src/amcl_node.cpp | 28 +- beluga_amcl/src/ndt_amcl_node.cpp | 31 +- beluga_amcl/src/ndt_amcl_node_3d.cpp | 450 ++++++++++++++ beluga_amcl/src/ros2_common.cpp | 30 +- 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 | 566 ++++++++++++++++++ beluga_amcl/test/test_utils/node_testing.hpp | 103 ++++ .../launch/ndt_replay_3d.launch.xml | 22 + .../utils/ndt_3d_localization_launch.py | 145 +++++ beluga_example/launch/utils/rosbag_launch.py | 11 +- beluga_example/maps/sample_3d_ndt_map.hdf5 | Bin 0 -> 33112 bytes .../params/default_ndt_3d.ros2.yaml | 91 +++ beluga_example/rviz/ndt_amcl_3d.ros2.rviz | 253 ++++++++ beluga_ros/include/beluga_ros/amcl.hpp | 2 +- beluga_ros/include/beluga_ros/messages.hpp | 4 + beluga_ros/include/beluga_ros/tf2_sophus.hpp | 86 +++ beluga_ros/test/test_tf2_sophus.cpp | 27 + 28 files changed, 2173 insertions(+), 94 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_replay_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 create mode 100644 beluga_example/rviz/ndt_amcl_3d.ros2.rviz 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/algorithm/unscented_transform.hpp b/beluga/include/beluga/algorithm/unscented_transform.hpp index ccd0d76d2..c7ebd13a9 100644 --- a/beluga/include/beluga/algorithm/unscented_transform.hpp +++ b/beluga/include/beluga/algorithm/unscented_transform.hpp @@ -15,11 +15,11 @@ #ifndef BELUGA_ALGORITHM_UNSCENTED_TRANSFORM_HPP #define BELUGA_ALGORITHM_UNSCENTED_TRANSFORM_HPP -#include #include #include #include +#include #include #include #include @@ -36,6 +36,23 @@ #include "beluga/eigen_compatibility.hpp" namespace beluga { +namespace detail { + +/// Templated functor to compute weighted mean for arbitrary. Assumes weights and vector's scalar types match. +struct default_weighted_mean_fn { + /// Operator() overload for eigen types. + template + T operator()(const std::vector& samples, const std::vector& weights) const { + return std::transform_reduce( + samples.begin(), samples.end(), weights.begin(), T::Zero().eval(), std::plus<>{}, std::multiplies<>{}); + } +}; + +} // namespace detail + +/// Object for computing a euclidean weighted mean of a vector of elements. +inline constexpr detail::default_weighted_mean_fn default_weighted_mean; + /** * \brief Implements the unscented transform, a mathematical function used to estimate the result of applying a given * a possibly nonlinear transformation to a probability distribution that is characterized with mean and covariance. @@ -45,18 +62,34 @@ namespace beluga { * \tparam MeanDerived Concrete Eigen dense type used for the mean. * \tparam CovarianceDerived Concrete Eigen dense type used for the covariance. * \tparam TransferFn Callable that maps a vector from input space to output space. Possibly non-linear. + * \tparam TransformedT State representing the output space. Deduced from the transfer function. + * \tparam MeanFn Callable that converts a vector of elements in the output space, and weights, to its mean in the + * output space. + * \tparam ResidualFn Callable that computes a residual given two elements in the output space. * \param mean Mean in the input space. * \param covariance Covariance in the input space. - * \param transfer_fn Callable that converts a vector from input space to output space. + * \param transfer_fn Callable that converts a* vector from input space to output space. * \param kappa Parameter used for sigma points selection. A sensible default will be used if not provided. + * \param mean_fn Callable that converts a vector of elements in the output space, and + * weights, to its mean in the output space. A sensible default is provided for the trivial (euclidian) case. + * \param residual_fn Callable that computes a residual given two elements in the output space.. A sensible default is + * provided for the trivial (euclidian) case. * \return A pair containing (mean, estimate) for the output space. */ -template +template < + typename MeanDerived, + typename CovarianceDerived, + typename TransferFn, + typename TransformedT = std::result_of_t, + typename MeanFn = detail::default_weighted_mean_fn, + typename ResidualFn = std::minus> auto unscented_transform( const Eigen::MatrixBase& mean, const Eigen::MatrixBase& covariance, TransferFn&& transfer_fn, - std::optional kappa = std::nullopt) { + std::optional kappa = std::nullopt, + MeanFn mean_fn = default_weighted_mean, + ResidualFn residual_fn = std::minus{}) { using Scalar = typename MeanDerived::Scalar; static_assert( std::is_same_v, @@ -97,15 +130,13 @@ auto unscented_transform( constexpr int kNout = decltype(transfer_fn(mean))::RowsAtCompileTime; static_assert(decltype(transfer_fn(mean))::ColsAtCompileTime == 1, "Output mean should be a column vector"); - const Eigen::Vector out_mean = std::transform_reduce( - transformed_sigma_points.begin(), transformed_sigma_points.end(), weights.begin(), - Eigen::Vector::Zero().eval(), std::plus<>{}, std::multiplies<>{}); + const Eigen::Vector out_mean = mean_fn(transformed_sigma_points, weights); const Eigen::Matrix out_cov = std::transform_reduce( transformed_sigma_points.begin(), transformed_sigma_points.end(), weights.begin(), Eigen::Matrix::Zero().eval(), std::plus<>{}, [&](const auto& sigma_point, const auto weight) { - const Eigen::Vector error = sigma_point - out_mean; + const Eigen::Vector error = residual_fn(sigma_point, out_mean); return (weight * (error * error.transpose())).eval(); }); diff --git a/beluga/include/beluga/policies/on_motion.hpp b/beluga/include/beluga/policies/on_motion.hpp index bbe589642..9d7ad43aa 100644 --- a/beluga/include/beluga/policies/on_motion.hpp +++ b/beluga/include/beluga/policies/on_motion.hpp @@ -15,8 +15,10 @@ #ifndef BELUGA_POLICIES_ON_MOTION_HPP #define BELUGA_POLICIES_ON_MOTION_HPP +#include #include #include +#include /** * \file @@ -60,8 +62,7 @@ struct on_motion_policy_base> { */ constexpr bool operator()(const Sophus::SE2& prev, const Sophus::SE2& current) { const auto delta = prev.inverse() * current; - return std::abs(delta.translation().x()) > min_distance_ || // - std::abs(delta.translation().y()) > min_distance_ || // + return delta.translation().norm() > min_distance_ || // std::abs(delta.so2().log()) > min_angle_; } @@ -70,6 +71,42 @@ 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; + const Eigen::AngleAxis angle_axis{delta.so3().unit_quaternion()}; + return delta.translation().norm() > min_distance_ || // + std::abs(angle_axis.angle()) > 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 +137,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 +154,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/algorithm/test_unscented_transform.cpp b/beluga/test/beluga/algorithm/test_unscented_transform.cpp index e0006670a..50386a846 100644 --- a/beluga/test/beluga/algorithm/test_unscented_transform.cpp +++ b/beluga/test/beluga/algorithm/test_unscented_transform.cpp @@ -18,6 +18,9 @@ #include #include +#include +#include +#include #include "beluga/algorithm/unscented_transform.hpp" @@ -118,6 +121,66 @@ TEST_F(UnscentedTransformTests, WorksWithEigenExpressions) { ASSERT_TRUE(transformed_cov.isApprox(Eigen::Matrix::Identity())); } +double angular_distance(double angle1, double angle2) { + const Sophus::SO2 rot1(angle1); + const Sophus::SO2 rot2(angle2); + + const Sophus::SO2 relative_rotation = rot1.inverse() * rot2; + const double distance = relative_rotation.log(); + + // Ensure the distance is the minimal distance + return std::min(std::abs(distance), 2 * Sophus::Constants::pi() - std::abs(distance)); +} + +TEST_F(UnscentedTransformTests, DifferentMeanFn) { + constexpr double kTolerance = 1e-1; + // This mean in polar coordinates is {1.4, pi}. This is specifically chosen so that we're close to the wrap around + // point. + const Eigen::Vector2d mean{-1.4, 0.}; + const Eigen::Vector2d expected_mean = {1.4, Sophus::Constants::pi()}; + + Eigen::Matrix cov; + cov << 1e-1, 0., // + 0., 1e-1; + + // Demonstrate why using default mean and residual functions can yield bad results, even bad means. + { + const auto [transformed_mean, transformed_cov] = + beluga::unscented_transform(mean, cov, [](const Eigen::Vector2d& in) -> Eigen::Vector2d { + return Eigen::Vector2d{std::hypot(in.x(), in.y()), std::atan2(in.y(), in.x())}; + }); + + const Eigen::Vector2d expected_wrong_mean = {1.4348469228349534, 1.5707963267948966}; + + ASSERT_FALSE(transformed_mean.isApprox(expected_mean, kTolerance)) << transformed_mean.transpose(); + ASSERT_TRUE(transformed_mean.isApprox(expected_wrong_mean, kTolerance)) << transformed_mean.transpose(); + } + { + const auto [transformed_mean, transformed_cov] = beluga::unscented_transform( + mean, cov, + [](const Eigen::Vector2d& in) -> Eigen::Vector2d { + return Eigen::Vector2d{std::hypot(in.x(), in.y()), std::atan2(in.y(), in.x())}; + }, + std::nullopt, + [](const std::vector& samples, const std::vector& weights) -> Eigen::Vector2d { + Eigen::Vector3d aux = Eigen::Vector3d::Zero(); + for (const auto& [s, w] : ranges::views::zip(samples, weights)) { + aux.x() += s.x() * w; + aux.y() += std::sin(s.y()) * w; + aux.z() += std::cos(s.y()) * w; + } + return Eigen::Vector2d{aux.x(), std::atan2(aux.y(), aux.z())}; + }, + [](const Eigen::Vector2d& sample, const Eigen::Vector2d& mean) { + Eigen::Vector2d error; + error.x() = sample.x() - mean.x(); + error.y() = angular_distance(sample.y(), mean.y()); + return error; + }); + ASSERT_TRUE(transformed_mean.isApprox(expected_mean, kTolerance)) << transformed_mean.transpose(); + } +} + TEST_F(UnscentedTransformTests, DifferentKappa) { constexpr double kTolerance = 1e-4; const auto [transformed_mean, transformed_cov] = beluga::unscented_transform( 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..0aa69105c 100644 --- a/beluga_amcl/cmake/ament.cmake +++ b/beluga_amcl/cmake/ament.cmake @@ -139,6 +139,53 @@ 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) + +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 + 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 + rclcpp_components + rclcpp_lifecycle + 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/amcl_node.hpp b/beluga_amcl/include/beluga_amcl/amcl_node.hpp index 9ec3a492a..08d3ec7d1 100644 --- a/beluga_amcl/include/beluga_amcl/amcl_node.hpp +++ b/beluga_amcl/include/beluga_amcl/amcl_node.hpp @@ -74,9 +74,6 @@ class AmclNode : public BaseAMCLNode { auto get_sensor_model(std::string_view, nav_msgs::msg::OccupancyGrid::SharedPtr) const -> beluga_ros::Amcl::sensor_model_variant; - /// Get execution policy given its name. - static auto get_execution_policy(std::string_view) -> beluga_ros::Amcl::execution_policy_variant; - /// Instantiate particle filter given an initial occupancy grid map and the current parametrization. auto make_particle_filter(nav_msgs::msg::OccupancyGrid::SharedPtr) const -> std::unique_ptr; @@ -86,9 +83,6 @@ class AmclNode : public BaseAMCLNode { /// 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::LaserScan::ConstSharedPtr); diff --git a/beluga_amcl/include/beluga_amcl/ndt_amcl_node.hpp b/beluga_amcl/include/beluga_amcl/ndt_amcl_node.hpp index df2369905..573b33aa2 100644 --- a/beluga_amcl/include/beluga_amcl/ndt_amcl_node.hpp +++ b/beluga_amcl/include/beluga_amcl/ndt_amcl_node.hpp @@ -118,9 +118,6 @@ class NdtAmclNode : public BaseAMCLNode { /// 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; @@ -130,9 +127,6 @@ class NdtAmclNode : public BaseAMCLNode { /// 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::LaserScan::ConstSharedPtr); 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..4e61301a9 --- /dev/null +++ b/beluga_amcl/include/beluga_amcl/ndt_amcl_node_3d.hpp @@ -0,0 +1,149 @@ +// 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>>; + +/// State type for the particles. +using StateType = typename beluga::NDTSensorModel::state_type; + +/// Type of a particle-dependent random state generator. +using RandomStateGenerator = std::function; + +/// Partial specialization of the core AMCL pipeline for convinience. +template +using NdtAmcl = beluga::Amcl< + MotionModel, + beluga::NDTSensorModel, + RandomStateGenerator, + beluga::Weight, + std::tuple, + ExecutionPolicy>; + +/// All combinations of supported 3D NDT AMCL variants. +using NdtAmclVariant = std::variant< + NdtAmcl, // + NdtAmcl>; + +/// Supported motion models. +using MotionModelVariant = 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 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 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_; + /// Last known pose estimate, if any. + std::optional> last_known_estimate_; + /// 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/include/beluga_amcl/ros2_common.hpp b/beluga_amcl/include/beluga_amcl/ros2_common.hpp index a6de1abfb..010699039 100644 --- a/beluga_amcl/include/beluga_amcl/ros2_common.hpp +++ b/beluga_amcl/include/beluga_amcl/ros2_common.hpp @@ -15,6 +15,7 @@ #ifndef BELUGA_AMCL_ROS2_COMMON_HPP #define BELUGA_AMCL_ROS2_COMMON_HPP +#include #include #include #include @@ -46,6 +47,8 @@ constexpr std::string_view kStationaryModelName = "stationary"; constexpr std::string_view kNav2DifferentialModelName = "nav2_amcl::DifferentialMotionModel"; /// String identifier for a omnidirectional model name. constexpr std::string_view kNav2OmnidirectionalModelName = "nav2_amcl::OmniMotionModel"; +/// Supported execution policies. +using ExecutionPolicyVariant = std::variant; /// Base AMCL lifecycle node, with some basic common functionalities, such as transform tree utilities, common /// publishers, subscribers, lifecycle related callbacks and configuration points, enabling extension by inheritance. @@ -76,6 +79,9 @@ class BaseAMCLNode : public rclcpp_lifecycle::LifecycleNode { /// Callback for lifecycle transitions from most states to the FINALIZED state. CallbackReturn on_shutdown(const rclcpp_lifecycle::State&) override; + /// Get execution policy from parameters. + auto get_execution_policy() const -> ExecutionPolicyVariant; + /// Callback for the periodic particle updates. void periodic_timer_callback(); diff --git a/beluga_amcl/src/amcl_node.cpp b/beluga_amcl/src/amcl_node.cpp index ac2df08f8..911a3bed5 100644 --- a/beluga_amcl/src/amcl_node.cpp +++ b/beluga_amcl/src/amcl_node.cpp @@ -176,22 +176,6 @@ AmclNode::~AmclNode() { on_shutdown(get_current_state()); } -void AmclNode::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 AmclNode::do_activate(const rclcpp_lifecycle::State&) { { map_sub_ = create_subscription( @@ -331,16 +315,6 @@ auto AmclNode::get_sensor_model(std::string_view name, nav_msgs::msg::OccupancyG throw std::invalid_argument(std::string("Invalid sensor model: ") + std::string(name)); } -auto AmclNode::get_execution_policy(std::string_view name) -> beluga_ros::Amcl::execution_policy_variant { - 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 AmclNode::make_particle_filter(nav_msgs::msg::OccupancyGrid::SharedPtr map) const -> std::unique_ptr { auto params = beluga_ros::AmclParams{}; @@ -363,7 +337,7 @@ auto AmclNode::make_particle_filter(nav_msgs::msg::OccupancyGrid::SharedPtr map) get_motion_model(get_parameter("robot_model_type").as_string()), // get_sensor_model(get_parameter("laser_model_type").as_string(), map), // params, // - get_execution_policy(get_parameter("execution_policy").as_string())); + get_execution_policy()); } void AmclNode::map_callback(nav_msgs::msg::OccupancyGrid::SharedPtr map) { diff --git a/beluga_amcl/src/ndt_amcl_node.cpp b/beluga_amcl/src/ndt_amcl_node.cpp index 891ef7ae3..8b5a6ef91 100644 --- a/beluga_amcl/src/ndt_amcl_node.cpp +++ b/beluga_amcl/src/ndt_amcl_node.cpp @@ -103,22 +103,6 @@ NdtAmclNode::NdtAmclNode(const rclcpp::NodeOptions& options) : BaseAMCLNode{"ndt } } -void NdtAmclNode::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 NdtAmclNode::do_activate(const rclcpp_lifecycle::State&) { RCLCPP_INFO(get_logger(), "Making particle filter"); particle_filter_ = make_particle_filter(); @@ -192,7 +176,7 @@ auto NdtAmclNode::get_initial_estimate() const -> std::optional MotionModelVariant { const auto name = get_parameter("robot_model_type").as_string(); - if (name == kDifferentialModelName || name == kNav2DifferentialModelName) { + if (name == kDifferentialModelName) { auto params = beluga::DifferentialDriveModelParam{}; params.rotation_noise_from_rotation = get_parameter("alpha1").as_double(); params.rotation_noise_from_translation = get_parameter("alpha2").as_double(); @@ -200,7 +184,7 @@ auto NdtAmclNode::get_motion_model() const -> MotionModelVariant { params.translation_noise_from_rotation = get_parameter("alpha4").as_double(); return beluga::DifferentialDriveModel{params}; } - if (name == kOmnidirectionalModelName || name == kNav2OmnidirectionalModelName) { + if (name == kOmnidirectionalModelName) { auto params = beluga::OmnidirectionalDriveModelParam{}; params.rotation_noise_from_rotation = get_parameter("alpha1").as_double(); params.rotation_noise_from_translation = get_parameter("alpha2").as_double(); @@ -228,17 +212,6 @@ beluga::NDTSensorModel NdtAmclNode::get_sensor_model() con params, beluga::io::load_from_hdf5(get_parameter("map_path").as_string())}; } -auto NdtAmclNode::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 NdtAmclNode::make_particle_filter() const -> std::unique_ptr { auto amcl = std::visit( [this](auto motion_model, auto execution_policy) -> NdtAmclVariant { 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..4eda5cdb9 --- /dev/null +++ b/beluga_amcl/src/ndt_amcl_node_3d.cpp @@ -0,0 +1,450 @@ +// 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 "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); + } +} + +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 == kDifferentialModelName) { + 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::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()); + + // We don't support randomly sampling from NDT maps, so we enforce a non-adaptive filter. + assert(params.min_particles == params.max_particles); + 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(); + + std::vector measurement; + measurement.reserve(laser_scan->height * laser_scan->width); + + // Accessing XYZ as suggested here: + // https://docs.ros.org/en/jade/api/sensor_msgs/html/classsensor__msgs_1_1PointCloud2Iterator.html + auto iter_x = sensor_msgs::PointCloud2ConstIterator(*laser_scan, "x"); + auto iter_y = sensor_msgs::PointCloud2ConstIterator(*laser_scan, "y"); + auto iter_z = sensor_msgs::PointCloud2ConstIterator(*laser_scan, "z"); + for (; iter_x != iter_x.end() && iter_y != iter_y.end() && iter_z != iter_z.end(); ++iter_x, ++iter_y, ++iter_z) { + measurement.emplace_back(laser_pose_in_base * Eigen::Vector3d{*iter_x, *iter_y, *iter_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()) { + auto message = geometry_msgs::msg::PoseWithCovarianceStamped{}; + message.header.stamp = laser_scan->header.stamp; + message.header.frame_id = get_parameter("global_frame_id").as_string(); + + static constexpr double kMinimumVariance = + 1e-6; // Make sure we covariance is not singular so that UT doesn't fail. + auto [base_pose_in_map, base_pose_covariance] = new_estimate.value(); + + 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); + } + message.pose = tf2::toMsg(base_pose_in_map, base_pose_covariance); + 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..65e73aa29 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); } { @@ -499,7 +499,31 @@ void BaseAMCLNode::periodic_timer_callback() { }; void BaseAMCLNode::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"); do_autostart_callback(); autostart_timer_->cancel(); } + +auto BaseAMCLNode::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."); +} } // namespace beluga_amcl 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..f2401ee5b --- /dev/null +++ b/beluga_amcl/test/test_ndt_amcl_3d_node.cpp @@ -0,0 +1,566 @@ +// 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 "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")); + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"min_particles", 10}); + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"max_particles", 10}); + } + + 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); + node_ = std::make_shared(); + node_->set_parameter(rclcpp::Parameter("map_path", kTestMapPath)); + node_->set_parameter(rclcpp::Parameter("scan_topic", "point_cloud")); + node_->set_parameter(rclcpp::Parameter{"min_particles", 10}); + node_->set_parameter(rclcpp::Parameter{"max_particles", 10}); + } + + void TearDown() override { rclcpp::shutdown(); } + + std::shared_ptr node_ = nullptr; +}; + +TEST_F(TestLifecycle, FullSpin) { + using lifecycle_msgs::msg::State; + 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(); + 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(); + 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(); + 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(); + 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(); + 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(); + 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") + .append_parameter_override("min_particles", 10) + .append_parameter_override("max_particles", 10)); + 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_EQ(pf.particles().size(), 10UL); }, *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.z", 2.0}); + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"initial_pose.roll", 0.2}); + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"initial_pose.pitch", 0.3}); + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"initial_pose.yaw", 0.1}); + 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.translation().y(), 2.0, 0.01); + + const auto expected_rotation = Sophus::SO3d::rotZ(0.1) * Sophus::SO3d::rotY(0.3) * Sophus::SO3d::rotX(0.2); + ASSERT_TRUE(pose.so3().matrix().isApprox(expected_rotation.matrix(), 1e-6)); +} + +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.0}); + 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.translation().z(), 0., 0.01); + ASSERT_NEAR(pose.so3().unit_quaternion().x(), 0., 0.01); + ASSERT_NEAR(pose.so3().unit_quaternion().y(), 0., 0.01); + ASSERT_NEAR(pose.so3().unit_quaternion().z(), 0., 0.01); + ASSERT_NEAR(pose.so3().unit_quaternion().w(), 1., 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); +} + +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); + } + + 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); + } +} + +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("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") + .append_parameter_override("min_particles", 10) + .append_parameter_override("max_particles", 10); + 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..2724dd892 100644 --- a/beluga_amcl/test/test_utils/node_testing.hpp +++ b/beluga_amcl/test/test_utils/node_testing.hpp @@ -15,6 +15,8 @@ #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 #include @@ -32,6 +34,7 @@ #include #include #include +#include #include #include @@ -51,6 +54,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 +165,34 @@ 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"; + // Modifier to describe what the fields are. + sensor_msgs::PointCloud2Modifier modifier(scan); + + modifier.setPointCloud2Fields( + 3, "x", 1, sensor_msgs::msg::PointField::FLOAT32, "y", 1, sensor_msgs::msg::PointField::FLOAT32, "z", 1, + sensor_msgs::msg::PointField::FLOAT32); + + 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 +215,35 @@ 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{}; + + // Modifier to describe what the fields are. + sensor_msgs::PointCloud2Modifier modifier(scan); + + modifier.setPointCloud2Fields( + 3, "x", 1, sensor_msgs::msg::PointField::FLOAT32, "y", 1, sensor_msgs::msg::PointField::FLOAT32, "z", 1, + sensor_msgs::msg::PointField::FLOAT32); + + 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 +267,37 @@ 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{}; + + // Modifier to describe what the fields are. + sensor_msgs::PointCloud2Modifier modifier(scan); + + modifier.setPointCloud2Fields( + 3, "x", 1, sensor_msgs::msg::PointField::FLOAT32, "y", 1, sensor_msgs::msg::PointField::FLOAT32, "z", 1, + sensor_msgs::msg::PointField::FLOAT32); + + 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 +310,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 +344,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_replay_3d.launch.xml b/beluga_example/launch/ndt_replay_3d.launch.xml new file mode 100644 index 000000000..1c6dc8b40 --- /dev/null +++ b/beluga_example/launch/ndt_replay_3d.launch.xml @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + + + 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..7a048faef --- /dev/null +++ b/beluga_example/launch/utils/ndt_3d_localization_launch.py @@ -0,0 +1,145 @@ +# 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_3d.ros2.yaml'), + description='Parameters file to be used to run the localization node.', + ), + DeclareLaunchArgument( + name='localization_ndt_map', + 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': ['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': ['ndt_amcl']}, + ], + ), + ], + ), + ] + ) + + return LaunchDescription( + [ + SetParameter('use_sim_time', True), + load_composable_nodes if use_composition else load_nodes, + ] + ) diff --git a/beluga_example/launch/utils/rosbag_launch.py b/beluga_example/launch/utils/rosbag_launch.py index f4519d9c9..739a0c06d 100644 --- a/beluga_example/launch/utils/rosbag_launch.py +++ b/beluga_example/launch/utils/rosbag_launch.py @@ -62,6 +62,11 @@ def get_launch_arguments(): default_value='', description='Destination bagfile to create, defaults to timestamped folder', ), + DeclareLaunchArgument( + name='clock', + default_value='True', + description='Publish to /clock. Defaults to True.', + ), ] @@ -73,10 +78,12 @@ def generate_launch_description( record_bag, topics_to_record, bagfile_output, + clock, ): start_paused = get_typed_value(start_paused, bool) record_bag = get_typed_value(record_bag, bool) topics_to_record = get_typed_value(topics_to_record, List[str]) + clock = get_typed_value(clock, bool) bag_play_cmd = [ 'ros2', @@ -85,9 +92,9 @@ def generate_launch_description( rosbag_path, '--rate', playback_rate, - '--clock', ] - + if clock: + bag_play_cmd.append('--clock') if start_paused: bag_play_cmd.append('--start-paused') 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..b406277fc --- /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: differential_drive + # 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_example/rviz/ndt_amcl_3d.ros2.rviz b/beluga_example/rviz/ndt_amcl_3d.ros2.rviz new file mode 100644 index 000000000..4cd67d958 --- /dev/null +++ b/beluga_example/rviz/ndt_amcl_3d.ros2.rviz @@ -0,0 +1,253 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /PointCloud21 + - /MarkerArray1 + - /MarkerArray1/Topic1 + - /MarkerArray2/Topic1 + - /PoseArray1 + - /PoseArray1/Topic1 + Splitter Ratio: 0.4967845678329468 + Tree Height: 796 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: PointCloud2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 0 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.004999999888241291 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /scan/points + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: MarkerArray + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: System Default + History Policy: Keep All + Reliability Policy: Best Effort + Value: /particle_markers + Value: false + - Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Class: rviz_default_plugins/PoseWithCovariance + Color: 255; 25; 0 + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: true + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Name: PoseWithCovariance + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Shape: Arrow + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /pose + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MarkerArray + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: System Default + History Policy: System Default + Reliability Policy: System Default + Value: /particle_markers + Value: true + - Alpha: 1 + Arrow Length: 0.30000001192092896 + Axes Length: 0.30000001192092896 + Axes Radius: 0.009999999776482582 + Class: rviz_default_plugins/PoseArray + Color: 255; 255; 0 + Enabled: true + Head Length: 0.07000000029802322 + Head Radius: 0.029999999329447746 + Name: PoseArray + Shaft Length: 0.23000000417232513 + Shaft Radius: 0.009999999776482582 + Shape: Arrow (Flat) + Topic: + Depth: 5 + Durability Policy: System Default + Filter size: 10 + History Policy: System Default + Reliability Policy: System Default + Value: /particle_cloud + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 36.99085235595703 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 12.622371673583984 + Y: 0.5767303705215454 + Z: -6.465853214263916 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 1.4247963428497314 + Target Frame: + Value: Orbit (rviz) + Yaw: 1.3203938007354736 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1080 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd000000040000000000000156000003a5fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000003a5000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c000002610000000100000100000003a5fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000003a5000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000078000000037fc0100000002fb0000000800540069006d00650100000000000007800000025300fffffffb0000000800540069006d006501000000000000045000000000000000000000051e000003a500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1920 + X: 0 + Y: 1080 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; diff --git a/beluga_ros/include/beluga_ros/messages.hpp b/beluga_ros/include/beluga_ros/messages.hpp index aab53ac22..ec0b2d0d4 100644 --- a/beluga_ros/include/beluga_ros/messages.hpp +++ b/beluga_ros/include/beluga_ros/messages.hpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include #include @@ -31,6 +32,7 @@ #include #include #include +#include #include #include #include @@ -60,6 +62,7 @@ using OccupancyGridConstSharedPtr = OccupancyGrid::ConstSharedPtr; using Point = geometry_msgs::msg::Point; using Pose = geometry_msgs::msg::Pose; using PoseArray = geometry_msgs::msg::PoseArray; +using PoseWithCovariance = geometry_msgs::msg::PoseWithCovariance; using Transform = geometry_msgs::msg::Transform; #elif BELUGA_ROS_VERSION == 1 @@ -74,6 +77,7 @@ using OccupancyGridConstSharedPtr = OccupancyGrid::ConstPtr; using Point = geometry_msgs::Point; using Pose = geometry_msgs::Pose; using PoseArray = geometry_msgs::PoseArray; +using PoseWithCovariance = geometry_msgs::PoseWithCovariance; using Transform = geometry_msgs::Transform; #else diff --git a/beluga_ros/include/beluga_ros/tf2_sophus.hpp b/beluga_ros/include/beluga_ros/tf2_sophus.hpp index 52d5e4c5e..683313ef3 100644 --- a/beluga_ros/include/beluga_ros/tf2_sophus.hpp +++ b/beluga_ros/include/beluga_ros/tf2_sophus.hpp @@ -17,6 +17,10 @@ #include #include +#include +#include +#include +#include #if BELUGA_ROS_VERSION == 2 #include @@ -39,6 +43,28 @@ * \brief Message conversion API overloads for `Sophus` types. */ +namespace detail { +/// Converts a vector representing a tangent in se3 space, and returns a vector representing the same SE3 transform with +/// extrinsic RPY parametrization. +template +inline Eigen::Vector tangential_space_to_xyz_rpy(const Eigen::Vector& tangent) { + // 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 . + const auto se3 = Sophus::SE3::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::Vector euler_angles = se3.so3().matrix().eulerAngles(2, 1, 0); + Eigen::Vector ret; + ret.template head<3>() = se3.translation(); + ret[3] = euler_angles.z(); + ret[4] = euler_angles.y(); + ret[5] = euler_angles.x(); + return ret; +} +} // namespace detail + /// `tf2` namespace extension for message conversion overload resolution. namespace tf2 { @@ -245,6 +271,66 @@ inline Sophus::Matrix3& covarianceRowMajorToEigen(const Array +inline beluga_ros::msg::PoseWithCovariance toMsg( + const Sophus::SE3& in, + const Eigen::Matrix& covariance) { + beluga_ros::msg::PoseWithCovariance out; + tf2::toMsg(in, out.pose); + + // 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 . + + const auto& [base_pose_in_map_xyz_rpy, base_pose_covariance_xyz_rpy] = beluga::unscented_transform( + in.log(), covariance, detail::tangential_space_to_xyz_rpy, std::nullopt, + [](const std::vector>& samples, const std::vector& weights) { + Eigen::Vector out = Eigen::Vector::Zero(); + Eigen::Vector roll_aux = Eigen::Vector::Zero(); + Eigen::Vector pitch_aux = Eigen::Vector::Zero(); + Eigen::Vector yaw_aux = Eigen::Vector::Zero(); + + for (const auto& [s, w] : ranges::views::zip(samples, weights)) { + out.template head<3>() += s.template head<3>() * w; + roll_aux.x() = std::sin(s[3]) * w; + roll_aux.y() = std::cos(s[3]) * w; + pitch_aux.x() = std::sin(s[4]) * w; + pitch_aux.y() = std::cos(s[4]) * w; + yaw_aux.x() = std::sin(s[5]) * w; + yaw_aux.y() = std::cos(s[5]) * w; + } + + out[3] = std::atan2(roll_aux.x(), roll_aux.y()); + out[4] = std::atan2(pitch_aux.x(), pitch_aux.y()); + out[5] = std::atan2(yaw_aux.x(), yaw_aux.y()); + return out; + }, + [](const Eigen::Vector& sample, const Eigen::Vector& mean) { + Eigen::Vector out; + const Sophus::SO3 sample_so3( + Sophus::SO3::rotZ(sample[5]) * Sophus::SO3::rotY(sample[4]) * + Sophus::SO3::rotX(sample[3])); + + const Sophus::SO3 mean_so3( + Sophus::SO3::rotZ(mean[5]) * Sophus::SO3::rotY(mean[4]) * + Sophus::SO3::rotX(mean[3])); + + const Sophus::SO3 delta = mean_so3.inverse() * sample_so3; + const Eigen::AngleAxis angle_axis{delta.unit_quaternion()}; + out.template head<3>() = sample.template head<3>() - mean.template head<3>(); + out.template tail<3>() = angle_axis.axis() * angle_axis.angle(); + return out; + }); + + // ROS covariance elements type is always double, and they're in RowMajor order. + Eigen::Map>(out.covariance.data()) = + base_pose_covariance_xyz_rpy.template cast(); + return out; +} + } // namespace tf2 /// `Sophus` namespace extension for message conversion function overload resolution (ADL enabled). diff --git a/beluga_ros/test/test_tf2_sophus.cpp b/beluga_ros/test/test_tf2_sophus.cpp index 3b256ec0e..3cae85ebb 100644 --- a/beluga_ros/test/test_tf2_sophus.cpp +++ b/beluga_ros/test/test_tf2_sophus.cpp @@ -17,6 +17,7 @@ #include #include +#include "beluga_ros/messages.hpp" #include "beluga_ros/tf2_sophus.hpp" namespace { @@ -71,4 +72,30 @@ TYPED_TEST(PoseConvertTest, ThereAndBackAgain) { ASSERT_EQ(message1, message2); } +template +class PoseWithCovarianceConvertTest : public testing::Test { + public: + using TypeParam = T; +}; + +using Sophus3DTypes = testing::Types; + +TYPED_TEST_SUITE(PoseWithCovarianceConvertTest, Sophus3DTypes, ); + +TYPED_TEST(PoseWithCovarianceConvertTest, Basic) { + using Covariance = Eigen::Matrix; + auto instance = typename TestFixture::TypeParam{}; + const Covariance covariance = (Eigen::Vector::Ones() * 1e-3).asDiagonal(); + const beluga_ros::msg::PoseWithCovariance message = tf2::toMsg(instance, covariance); + ASSERT_DOUBLE_EQ(message.pose.position.x, 0.0); + ASSERT_DOUBLE_EQ(message.pose.position.y, 0.0); + ASSERT_DOUBLE_EQ(message.pose.position.z, 0.0); + ASSERT_DOUBLE_EQ(message.pose.orientation.x, 0.0); + ASSERT_DOUBLE_EQ(message.pose.orientation.y, 0.0); + ASSERT_DOUBLE_EQ(message.pose.orientation.z, 0.0); + ASSERT_DOUBLE_EQ(message.pose.orientation.w, 1.0); + ASSERT_FALSE(message.covariance.empty()); + ASSERT_EQ(message.covariance.size(), 36); +} + } // namespace