diff --git a/beluga_amcl/cmake/ament.cmake b/beluga_amcl/cmake/ament.cmake index 9cb65aad1..0583b7c92 100644 --- a/beluga_amcl/cmake/ament.cmake +++ b/beluga_amcl/cmake/ament.cmake @@ -65,6 +65,49 @@ install(TARGETS amcl_node DESTINATION lib/${PROJECT_NAME}) install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME}) +add_library(ndt_amcl_node_component SHARED) +target_sources(ndt_amcl_node_component PRIVATE src/ndt_amcl_node.cpp) +target_include_directories( + ndt_amcl_node_component + PUBLIC $ + $) +target_compile_features(ndt_amcl_node_component PUBLIC cxx_std_17) +ament_target_dependencies( + ndt_amcl_node_component + PUBLIC beluga + beluga_ros + bondcpp + nav2_msgs + rclcpp + rclcpp_components + rclcpp_lifecycle + std_srvs) +rclcpp_components_register_node( + ndt_amcl_node_component + PLUGIN "beluga_amcl::NdtAmclNode" + EXECUTABLE ndt_amcl_node) + +ament_export_dependencies( + beluga + beluga_ros + bondcpp + nav2_msgs + rclcpp + rclcpp_components + rclcpp_lifecycle + std_srvs) +ament_export_include_directories("include/${PROJECT_NAME}") + +install( + TARGETS ndt_amcl_node_component + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin) + +install(TARGETS ndt_amcl_node DESTINATION lib/${PROJECT_NAME}) + +install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME}) + if(BUILD_TESTING) enable_testing() add_subdirectory(test) diff --git a/beluga_amcl/include/beluga_amcl/ndt_amcl_node.hpp b/beluga_amcl/include/beluga_amcl/ndt_amcl_node.hpp new file mode 100644 index 000000000..b182ff1c0 --- /dev/null +++ b/beluga_amcl/include/beluga_amcl/ndt_amcl_node.hpp @@ -0,0 +1,193 @@ +// 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_HPP +#define BELUGA_AMCL_NDT_AMCL_NODE_HPP + +#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 + +/** + * \file + * \brief ROS 2 integration of the 2D NDT-AMCL algorithm. + */ + +namespace beluga_amcl { + +/// Underlying map representation for the NDT sensor model. +using NDTMapRepresentation = + beluga::SparseValueGrid>>; + +/// Type of a particle-dependent random state generator. +using RandomStateGenerator = std::function::state_type()>( + const beluga::TupleVector::state_type, beluga::Weight>>)>; + +/// Partial specialization of the core AMCL pipeline for convinience. +template +using NdtAmcl = beluga::Amcl< + MotionModel, + beluga::NDTSensorModel, + RandomStateGenerator, + beluga::Weight, + std::tuple::state_type, beluga::Weight>, + ExecutionPolicy>; + +/// All combinations of supported NDT AMCL variants. +using NdtAmclVariant = std::variant< + NdtAmcl, // + NdtAmcl, // + NdtAmcl, // + NdtAmcl, // + NdtAmcl, // + NdtAmcl // + >; + +/// Supported motion models. +using MotionModelVariant = + std::variant; + +/// Supported execution policies. +using ExecutionPolicyVariant = std::variant; + +/// 2D NDT AMCL as a ROS 2 composable lifecycle node. +class NdtAmclNode : public rclcpp_lifecycle::LifecycleNode { + public: + using rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + + /// Constructor. + explicit NdtAmclNode(const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); + ~NdtAmclNode() override; + + protected: + /// Callback for lifecycle transitions from the UNCONFIGURED state to the INACTIVE state. + CallbackReturn on_configure(const rclcpp_lifecycle::State&) override; + + /// Callback for lifecycle transitions from the INACTIVE state to the ACTIVE state. + CallbackReturn on_activate(const rclcpp_lifecycle::State&) override; + + /// Callback for lifecycle transitions from the ACTIVE state to the INACTIVE state. + CallbackReturn on_deactivate(const rclcpp_lifecycle::State&) override; + + /// Callback for lifecycle transitions from the INACTIVE state to the UNCONFIGURED state. + CallbackReturn on_cleanup(const rclcpp_lifecycle::State&) override; + + /// Callback for lifecycle transitions from most states to the FINALIZED state. + CallbackReturn on_shutdown(const rclcpp_lifecycle::State&) override; + + /// Get initial pose estimate from parameters if set. + auto get_initial_estimate() const -> std::optional>; + + /// Get motion model as per current parametrization. + auto get_motion_model() const -> MotionModelVariant; + + /// Get execution policy given its name. + auto get_execution_policy() const -> ExecutionPolicyVariant; + + /// Get sensor model as per current parametrization. + beluga::NDTSensorModel get_sensor_model() const; + + /// Instantiate particle filter given an initial occupancy grid map and the current parametrization. + auto make_particle_filter() const -> std::unique_ptr; + + /// Callback for periodic particle cloud updates. + void timer_callback(); + + /// Callback for laser scan updates. + void laser_callback(sensor_msgs::msg::LaserScan::ConstSharedPtr); + + /// Callback for pose (re)initialization. + void initial_pose_callback(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr); + + /// 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); + + /// Node bond with the lifecycle manager. + std::unique_ptr bond_; + /// Timer for periodic particle cloud updates. + rclcpp::TimerBase::SharedPtr timer_; + + /// Particle cloud publisher. + rclcpp_lifecycle::LifecyclePublisher::SharedPtr particle_cloud_pub_; + /// Estimated pose publisher. + rclcpp_lifecycle::LifecyclePublisher::SharedPtr pose_pub_; + + /// Pose (re)initialization subscription. + rclcpp::Subscription::SharedPtr initial_pose_sub_; + /// Laser scan updates subscription. + std::unique_ptr> + laser_scan_sub_; + + /// Transforms buffer. + std::unique_ptr tf_buffer_; + /// Transforms broadcaster. + std::unique_ptr tf_broadcaster_; + /// Transforms listener. + std::unique_ptr tf_listener_; + /// Transform synchronization filter for laser scan updates. + std::unique_ptr> laser_scan_filter_; + /// Connection for laser scan updates filter and callback. + message_filters::Connection laser_scan_connection_; + + /// Particle filter instance. + std::unique_ptr particle_filter_ = nullptr; + /// Last known pose estimate, if any. + std::optional> last_known_estimate_ = std::nullopt; + /// Whether to broadcast transforms or not. + bool enable_tf_broadcast_{false}; +}; + +} // namespace beluga_amcl + +#endif // BELUGA_AMCL_NDT_AMCL_NODE_HPP diff --git a/beluga_amcl/include/beluga_amcl/ros2_common.hpp b/beluga_amcl/include/beluga_amcl/ros2_common.hpp new file mode 100644 index 000000000..c8e4717b0 --- /dev/null +++ b/beluga_amcl/include/beluga_amcl/ros2_common.hpp @@ -0,0 +1,473 @@ +// 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. + +#ifndef BELUGA_AMCL_ROS2_COMMON_HPP +#define BELUGA_AMCL_ROS2_COMMON_HPP + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +namespace beluga_amcl { + +/// String identifier for a diff drive model. +constexpr std::string_view kDifferentialModelName = "differential_drive"; +/// String identifier for a omnidirectional drive model. +constexpr std::string_view kOmnidirectionalModelName = "omnidirectional_drive"; +/// String identifier for a stationary model. +constexpr std::string_view kStationaryModelName = "stationary"; +/// String identifier for a diff drive model. +constexpr std::string_view kNav2DifferentialModelName = "nav2_amcl::DifferentialMotionModel"; +/// String identifier for a omnidirectional model name. +constexpr std::string_view kNav2OmnidirectionalModelName = "nav2_amcl::OmniMotionModel"; + +/// Declares common AMCL-related parameters to the node. +inline void declare_common_params(rclcpp_lifecycle::LifecycleNode& node) { + { + auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); + descriptor.description = "The name of the coordinate frame published by the localization system."; + node.declare_parameter("global_frame_id", rclcpp::ParameterValue("map"), descriptor); + } + + { + auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); + descriptor.description = "The name of the coordinate frame to use for odometry."; + node.declare_parameter("odom_frame_id", rclcpp::ParameterValue("odom"), descriptor); + } + + { + auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); + descriptor.description = "The name of the coordinate frame to use for the robot base."; + node.declare_parameter("base_frame_id", rclcpp::ParameterValue("base_footprint"), descriptor); + } + + { + auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); + descriptor.description = "Path to load the map from an hdf5 file."; + node.declare_parameter("map_path", rclcpp::ParameterValue("map_path"), descriptor); + } + + { + auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); + descriptor.description = "Topic to subscribe to in order to receive the initial pose of the robot."; + node.declare_parameter("initial_pose_topic", rclcpp::ParameterValue("initialpose"), descriptor); + } + + { + auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); + descriptor.description = "Topic to subscribe to in order to receive the laser scan for localization."; + node.declare_parameter("scan_topic", rclcpp::ParameterValue("scan"), descriptor); + } + + { + auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); + descriptor.description = "Minimum allowed number of particles."; + descriptor.integer_range.resize(1); + descriptor.integer_range[0].from_value = 0; + descriptor.integer_range[0].to_value = std::numeric_limits::max(); + descriptor.integer_range[0].step = 1; + node.declare_parameter("min_particles", rclcpp::ParameterValue(500), descriptor); + } + + { + auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); + descriptor.description = "Maximum allowed number of particles."; + descriptor.integer_range.resize(1); + descriptor.integer_range[0].from_value = 0; + descriptor.integer_range[0].to_value = std::numeric_limits::max(); + descriptor.integer_range[0].step = 1; + node.declare_parameter("max_particles", rclcpp::ParameterValue(2000), descriptor); + } + + { + auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); + descriptor.description = + "Exponential decay rate for the slow average weight filter, used in deciding when to recover " + "by adding random poses."; + 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; + node.declare_parameter("recovery_alpha_slow", rclcpp::ParameterValue(0.0), descriptor); + } + + { + auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); + descriptor.description = + "Exponential decay rate for the fast average weight filter, used in deciding when to recover " + "by adding random poses."; + 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; + node.declare_parameter("recovery_alpha_fast", rclcpp::ParameterValue(0.0), descriptor); + } + + { + auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); + descriptor.description = + "Maximum particle filter population error between the true distribution " + "and the estimated distribution. It is used in KLD resampling to limit the " + "allowed number of particles to the minimum necessary."; + 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; + node.declare_parameter("pf_err", rclcpp::ParameterValue(0.05), descriptor); + } + + { + auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); + descriptor.description = + "Upper standard normal quantile for P, where P is the probability " + "that the error in the estimated distribution will be less than pf_err " + "in KLD resampling."; + node.declare_parameter("pf_z", rclcpp::ParameterValue(0.99), descriptor); + } + + { + auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); + descriptor.description = + "Resolution in meters for the X axis used to divide the space in buckets for KLD resampling."; + descriptor.floating_point_range.resize(1); + descriptor.floating_point_range[0].from_value = 0; + descriptor.floating_point_range[0].to_value = std::numeric_limits::max(); + descriptor.floating_point_range[0].step = 0; + node.declare_parameter("spatial_resolution_x", rclcpp::ParameterValue(0.5), descriptor); + } + + { + auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); + descriptor.description = + "Resolution in meters for the Y axis used to divide the space in buckets for KLD resampling."; + descriptor.floating_point_range.resize(1); + descriptor.floating_point_range[0].from_value = 0; + descriptor.floating_point_range[0].to_value = std::numeric_limits::max(); + descriptor.floating_point_range[0].step = 0; + node.declare_parameter("spatial_resolution_y", rclcpp::ParameterValue(0.5), descriptor); + } + + { + auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); + descriptor.description = + "Resolution in radians for the theta axis to divide the space in buckets for KLD resampling."; + descriptor.floating_point_range.resize(1); + descriptor.floating_point_range[0].from_value = 0; + descriptor.floating_point_range[0].to_value = 2 * Sophus::Constants::pi(); + descriptor.floating_point_range[0].step = 0; + node.declare_parameter( + "spatial_resolution_theta", rclcpp::ParameterValue(10 * Sophus::Constants::pi() / 180), descriptor); + } + + { + auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); + descriptor.description = "Number of filter updates required before resampling. "; + descriptor.integer_range.resize(1); + descriptor.integer_range[0].from_value = 1; + descriptor.integer_range[0].to_value = std::numeric_limits::max(); + descriptor.integer_range[0].step = 1; + node.declare_parameter("resample_interval", rclcpp::ParameterValue(1), descriptor); + } + + { + auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); + descriptor.description = + "When set to true, will reduce the resampling rate when not needed and help " + "avoid particle deprivation. The resampling will only happen if the effective " + "number of particles (N_eff = 1/(sum(k_i^2))) is lower than half the current " + "number of particles."; + descriptor.read_only = true; + node.declare_parameter("selective_resampling", false, descriptor); + } + + { + auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); + descriptor.description = + "Set this to false to prevent amcl from publishing the transform " + "between the global frame and the odometry frame."; + node.declare_parameter("tf_broadcast", rclcpp::ParameterValue(true), descriptor); + } + + { + auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); + descriptor.description = + "Time with which to post-date the transform that is published, " + "to indicate that this transform is valid into the future"; + descriptor.floating_point_range.resize(1); + descriptor.floating_point_range[0].from_value = 0; + descriptor.floating_point_range[0].to_value = std::numeric_limits::max(); + descriptor.floating_point_range[0].step = 0; + node.declare_parameter("transform_tolerance", rclcpp::ParameterValue(1.0), descriptor); + } + + { + auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); + descriptor.description = "Which motion model to use [differential_drive, omnidirectional_drive, stationary]."; + node.declare_parameter("robot_model_type", rclcpp::ParameterValue(std::string(kDifferentialModelName)), descriptor); + } + + { + auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); + descriptor.description = "Rotation noise from rotation for the differential drive model."; + descriptor.floating_point_range.resize(1); + descriptor.floating_point_range[0].from_value = 0; + descriptor.floating_point_range[0].to_value = std::numeric_limits::max(); + descriptor.floating_point_range[0].step = 0; + node.declare_parameter("alpha1", rclcpp::ParameterValue(0.2), descriptor); + } + + { + auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); + descriptor.description = "Rotation noise from translation for the differential drive model."; + descriptor.floating_point_range.resize(1); + descriptor.floating_point_range[0].from_value = 0; + descriptor.floating_point_range[0].to_value = std::numeric_limits::max(); + descriptor.floating_point_range[0].step = 0; + node.declare_parameter("alpha2", rclcpp::ParameterValue(0.2), descriptor); + } + + { + auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); + descriptor.description = "Translation noise from translation for the differential drive model."; + descriptor.floating_point_range.resize(1); + descriptor.floating_point_range[0].from_value = 0; + descriptor.floating_point_range[0].to_value = std::numeric_limits::max(); + descriptor.floating_point_range[0].step = 0; + node.declare_parameter("alpha3", rclcpp::ParameterValue(0.2), descriptor); + } + + { + auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); + descriptor.description = "Translation noise from rotation for the differential drive model."; + descriptor.floating_point_range.resize(1); + descriptor.floating_point_range[0].from_value = 0; + descriptor.floating_point_range[0].to_value = std::numeric_limits::max(); + descriptor.floating_point_range[0].step = 0; + node.declare_parameter("alpha4", rclcpp::ParameterValue(0.2), descriptor); + } + + { + auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); + descriptor.description = "Strafe noise from translation for the omnidirectional drive model."; + descriptor.floating_point_range.resize(1); + descriptor.floating_point_range[0].from_value = 0; + descriptor.floating_point_range[0].to_value = std::numeric_limits::max(); + descriptor.floating_point_range[0].step = 0; + node.declare_parameter("alpha5", rclcpp::ParameterValue(0.2), descriptor); + } + + { + auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); + descriptor.description = "Rotational movement required before performing a filter update."; + descriptor.floating_point_range.resize(1); + descriptor.floating_point_range[0].from_value = 0; + descriptor.floating_point_range[0].to_value = 2 * Sophus::Constants::pi(); + descriptor.floating_point_range[0].step = 0; + node.declare_parameter("update_min_a", rclcpp::ParameterValue(0.2), descriptor); + } + + { + auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); + descriptor.description = "Translational movement required before performing a filter update."; + descriptor.floating_point_range.resize(1); + descriptor.floating_point_range[0].from_value = 0; + descriptor.floating_point_range[0].to_value = std::numeric_limits::max(); + descriptor.floating_point_range[0].step = 0; + node.declare_parameter("update_min_d", rclcpp::ParameterValue(0.25), descriptor); + } + + { + auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); + descriptor.description = "Maximum scan range to be considered."; + descriptor.floating_point_range.resize(1); + descriptor.floating_point_range[0].from_value = 0; + descriptor.floating_point_range[0].to_value = std::numeric_limits::max(); + descriptor.floating_point_range[0].step = 0; + node.declare_parameter("laser_max_range", rclcpp::ParameterValue(100.0), descriptor); + } + + { + auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); + descriptor.description = "Minimum scan range to be considered."; + descriptor.floating_point_range.resize(1); + descriptor.floating_point_range[0].from_value = 0; + descriptor.floating_point_range[0].to_value = std::numeric_limits::max(); + descriptor.floating_point_range[0].step = 0; + node.declare_parameter("laser_min_range", rclcpp::ParameterValue(0.0), descriptor); + } + + { + auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); + descriptor.description = "How many evenly-spaced beams in each scan will be used when updating the filter."; + descriptor.integer_range.resize(1); + descriptor.integer_range[0].from_value = 2; + descriptor.integer_range[0].to_value = std::numeric_limits::max(); + descriptor.integer_range[0].step = 1; + node.declare_parameter("max_beams", rclcpp::ParameterValue(60), descriptor); + } + + { + auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); + descriptor.description = "Set the initial pose from the initial_pose parameters."; + node.declare_parameter("set_initial_pose", false, descriptor); + } + + { + auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); + descriptor.description = "Initial pose x axis coordinate."; + node.declare_parameter("initial_pose.x", 0.0, descriptor); + } + + { + auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); + descriptor.description = "Initial pose y axis coordinate."; + node.declare_parameter("initial_pose.y", 0.0, descriptor); + } + + { + auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); + descriptor.description = "Initial pose yaw rotation."; + node.declare_parameter("initial_pose.yaw", 0.0, descriptor); + } + + { + auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); + descriptor.description = "Initial pose x axis covariance."; + node.declare_parameter("initial_pose.covariance_x", 0.0, descriptor); + } + + { + auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); + descriptor.description = "Initial pose y axis covariance."; + node.declare_parameter("initial_pose.covariance_y", 0.0, descriptor); + } + + { + auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); + descriptor.description = "Initial pose yaw covariance."; + node.declare_parameter("initial_pose.covariance_yaw", 0.0, descriptor); + } + + { + auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); + descriptor.description = "Initial pose xy covariance."; + node.declare_parameter("initial_pose.covariance_xy", 0.0, descriptor); + } + + { + auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); + descriptor.description = "Initial pose xyaw covariance."; + node.declare_parameter("initial_pose.covariance_xyaw", 0.0, descriptor); + } + + { + auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); + descriptor.description = "Initial pose yyaw covariance."; + node.declare_parameter("initial_pose.covariance_yyaw", 0.0, descriptor); + } + + { + auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); + descriptor.description = "Execution policy used to process particles [seq, par]."; + node.declare_parameter("execution_policy", "seq", descriptor); + } +} + +/// Helper struct to hold a SE2 state and its weight. +struct RepresentativeData { + /// SE2 state. + Sophus::SE2d state; + /// Weight. + double weight{0.}; +}; + +/// Hash implementation for SE2 transforms. +struct RepresentativeBinHash { + /// Computes a hash out of a SE2 transform. + std::size_t operator()(const Sophus::SE2d& s) const noexcept { + std::size_t h1 = std::hash{}(s.translation().x()); + std::size_t h2 = std::hash{}(s.translation().y()); + std::size_t h3 = std::hash{}(s.so2().log()); + return h1 ^ (h2 << 1) ^ (h3 << 2); + } +}; + +/// Equal comparison functor for SE2 transforms. +struct RepresentativeBinEqual { + /// Actual hash implementation. + bool operator()(const Sophus::SE2d& lhs, const Sophus::SE2d& rhs) const noexcept { + // good enough, since copies of the same candidate are expected to be identical copies + return lhs.translation().x() == rhs.translation().x() && // + lhs.translation().y() == rhs.translation().y() && // + lhs.so2().log() == rhs.so2().log(); + } +}; + +// Particle weights from the filter may or may not be representative of the +// true distribution. If we resampled, they are not, and there will be multiple copies +// of the most likely candidates, all with unit weight. In this case the number of copies +// is a proxy for the prob density at each candidate. If we did not resample before updating +// the estimation and publishing this message (which can happen if the resample interval +// is set to something other than 1), then all particles are expected to be different +// and their weights are proportional to the prob density at each candidate. +// +// Only the combination of both the state distribution and the candidate weights together +// provide information about the probability density at each candidate. +// To handle both cases, we group repeated candidates and compute the accumulated weight. +template +nav2_msgs::msg::ParticleCloud make_representative_particle_cloud( + const ParticleFilter& particle_filter, + std::string_view global_frame, + rclcpp::Time now) { + std::unordered_map + representatives_map; + representatives_map.reserve(particle_filter.particles().size()); + + double max_weight = 1e-5; // never risk dividing by zero + + for (const auto& [state, weight] : particle_filter.particles()) { + auto& representative = representatives_map[state]; // if the element does not exist, create it + representative.state = state; + representative.weight += weight; + if (representative.weight > max_weight) { + max_weight = representative.weight; + } + } + + auto message = nav2_msgs::msg::ParticleCloud{}; + message.header.stamp = now; + message.header.frame_id = global_frame; + message.particles.reserve(particle_filter.particles().size()); + + for (const auto& [key, representative] : representatives_map) { + auto& particle = message.particles.emplace_back(); + tf2::toMsg(representative.state, particle.pose); + particle.weight = representative.weight / max_weight; + } + + return message; +} + +} // namespace beluga_amcl +#endif diff --git a/beluga_amcl/src/amcl_node.cpp b/beluga_amcl/src/amcl_node.cpp index b1dcfd5ea..4a2af980a 100644 --- a/beluga_amcl/src/amcl_node.cpp +++ b/beluga_amcl/src/amcl_node.cpp @@ -13,6 +13,7 @@ // limitations under the License. #include +#include #include #include @@ -40,274 +41,20 @@ namespace beluga_amcl { namespace { -constexpr std::string_view kDifferentialModelName = "differential_drive"; -constexpr std::string_view kOmnidirectionalModelName = "omnidirectional_drive"; -constexpr std::string_view kStationaryModelName = "stationary"; - constexpr std::string_view kLikelihoodFieldModelName = "likelihood_field"; constexpr std::string_view kBeamSensorModelName = "beam"; -constexpr std::string_view kNav2DifferentialModelName = "nav2_amcl::DifferentialMotionModel"; -constexpr std::string_view kNav2OmnidirectionalModelName = "nav2_amcl::OmniMotionModel"; - } // namespace AmclNode::AmclNode(const rclcpp::NodeOptions& options) : rclcpp_lifecycle::LifecycleNode{"amcl", "", options} { RCLCPP_INFO(get_logger(), "Creating"); - - { - auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); - descriptor.description = "The name of the coordinate frame published by the localization system."; - declare_parameter("global_frame_id", rclcpp::ParameterValue("map"), descriptor); - } - - { - auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); - descriptor.description = "The name of the coordinate frame to use for odometry."; - declare_parameter("odom_frame_id", rclcpp::ParameterValue("odom"), descriptor); - } - - { - auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); - descriptor.description = "The name of the coordinate frame to use for the robot base."; - declare_parameter("base_frame_id", rclcpp::ParameterValue("base_footprint"), descriptor); - } - + declare_common_params(*this); { auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); descriptor.description = "Topic to subscribe to in order to receive the map to localize on."; declare_parameter("map_topic", rclcpp::ParameterValue("map"), descriptor); } - { - auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); - descriptor.description = "Topic to subscribe to in order to receive the initial pose of the robot."; - declare_parameter("initial_pose_topic", rclcpp::ParameterValue("initialpose"), descriptor); - } - - { - auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); - descriptor.description = "Topic to subscribe to in order to receive the laser scan for localization."; - declare_parameter("scan_topic", rclcpp::ParameterValue("scan"), descriptor); - } - - { - auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); - descriptor.description = "Minimum allowed number of particles."; - descriptor.integer_range.resize(1); - descriptor.integer_range[0].from_value = 0; - descriptor.integer_range[0].to_value = std::numeric_limits::max(); - descriptor.integer_range[0].step = 1; - declare_parameter("min_particles", rclcpp::ParameterValue(500), descriptor); - } - - { - auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); - descriptor.description = "Maximum allowed number of particles."; - descriptor.integer_range.resize(1); - descriptor.integer_range[0].from_value = 0; - descriptor.integer_range[0].to_value = std::numeric_limits::max(); - descriptor.integer_range[0].step = 1; - declare_parameter("max_particles", rclcpp::ParameterValue(2000), descriptor); - } - - { - auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); - descriptor.description = - "Exponential decay rate for the slow average weight filter, used in deciding when to recover " - "by adding random poses."; - 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("recovery_alpha_slow", rclcpp::ParameterValue(0.0), descriptor); - } - - { - auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); - descriptor.description = - "Exponential decay rate for the fast average weight filter, used in deciding when to recover " - "by adding random poses."; - 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("recovery_alpha_fast", rclcpp::ParameterValue(0.0), descriptor); - } - - { - auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); - descriptor.description = - "Maximum particle filter population error between the true distribution " - "and the estimated distribution. It is used in KLD resampling to limit the " - "allowed number of particles to the minimum necessary."; - 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("pf_err", rclcpp::ParameterValue(0.05), descriptor); - } - - { - auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); - descriptor.description = - "Upper standard normal quantile for P, where P is the probability " - "that the error in the estimated distribution will be less than pf_err " - "in KLD resampling."; - declare_parameter("pf_z", rclcpp::ParameterValue(0.99), descriptor); - } - - { - auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); - descriptor.description = - "Resolution in meters for the X axis used to divide the space in buckets for KLD resampling."; - descriptor.floating_point_range.resize(1); - descriptor.floating_point_range[0].from_value = 0; - descriptor.floating_point_range[0].to_value = std::numeric_limits::max(); - descriptor.floating_point_range[0].step = 0; - declare_parameter("spatial_resolution_x", rclcpp::ParameterValue(0.5), descriptor); - } - - { - auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); - descriptor.description = - "Resolution in meters for the Y axis used to divide the space in buckets for KLD resampling."; - descriptor.floating_point_range.resize(1); - descriptor.floating_point_range[0].from_value = 0; - descriptor.floating_point_range[0].to_value = std::numeric_limits::max(); - descriptor.floating_point_range[0].step = 0; - declare_parameter("spatial_resolution_y", rclcpp::ParameterValue(0.5), descriptor); - } - - { - auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); - descriptor.description = - "Resolution in radians for the theta axis to divide the space in buckets for KLD resampling."; - descriptor.floating_point_range.resize(1); - descriptor.floating_point_range[0].from_value = 0; - descriptor.floating_point_range[0].to_value = 2 * Sophus::Constants::pi(); - descriptor.floating_point_range[0].step = 0; - declare_parameter( - "spatial_resolution_theta", rclcpp::ParameterValue(10 * Sophus::Constants::pi() / 180), descriptor); - } - - { - auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); - descriptor.description = "Number of filter updates required before resampling. "; - descriptor.integer_range.resize(1); - descriptor.integer_range[0].from_value = 1; - descriptor.integer_range[0].to_value = std::numeric_limits::max(); - descriptor.integer_range[0].step = 1; - declare_parameter("resample_interval", rclcpp::ParameterValue(1), descriptor); - } - - { - auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); - descriptor.description = - "When set to true, will reduce the resampling rate when not needed and help " - "avoid particle deprivation. The resampling will only happen if the effective " - "number of particles (N_eff = 1/(sum(k_i^2))) is lower than half the current " - "number of particles."; - descriptor.read_only = true; - declare_parameter("selective_resampling", false, descriptor); - } - - { - auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); - descriptor.description = - "Set this to false to prevent amcl from publishing the transform " - "between the global frame and the odometry frame."; - declare_parameter("tf_broadcast", rclcpp::ParameterValue(true), descriptor); - } - - { - auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); - descriptor.description = - "Time with which to post-date the transform that is published, " - "to indicate that this transform is valid into the future"; - descriptor.floating_point_range.resize(1); - descriptor.floating_point_range[0].from_value = 0; - descriptor.floating_point_range[0].to_value = std::numeric_limits::max(); - descriptor.floating_point_range[0].step = 0; - declare_parameter("transform_tolerance", rclcpp::ParameterValue(1.0), descriptor); - } - - { - auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); - descriptor.description = "Which motion model to use [differential_drive, omnidirectional_drive, stationary]."; - declare_parameter("robot_model_type", rclcpp::ParameterValue(std::string(kDifferentialModelName)), descriptor); - } - - { - auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); - descriptor.description = "Rotation noise from rotation for the differential drive model."; - descriptor.floating_point_range.resize(1); - descriptor.floating_point_range[0].from_value = 0; - descriptor.floating_point_range[0].to_value = std::numeric_limits::max(); - descriptor.floating_point_range[0].step = 0; - declare_parameter("alpha1", rclcpp::ParameterValue(0.2), descriptor); - } - - { - auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); - descriptor.description = "Rotation noise from translation for the differential drive model."; - descriptor.floating_point_range.resize(1); - descriptor.floating_point_range[0].from_value = 0; - descriptor.floating_point_range[0].to_value = std::numeric_limits::max(); - descriptor.floating_point_range[0].step = 0; - declare_parameter("alpha2", rclcpp::ParameterValue(0.2), descriptor); - } - - { - auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); - descriptor.description = "Translation noise from translation for the differential drive model."; - descriptor.floating_point_range.resize(1); - descriptor.floating_point_range[0].from_value = 0; - descriptor.floating_point_range[0].to_value = std::numeric_limits::max(); - descriptor.floating_point_range[0].step = 0; - declare_parameter("alpha3", rclcpp::ParameterValue(0.2), descriptor); - } - - { - auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); - descriptor.description = "Translation noise from rotation for the differential drive model."; - descriptor.floating_point_range.resize(1); - descriptor.floating_point_range[0].from_value = 0; - descriptor.floating_point_range[0].to_value = std::numeric_limits::max(); - descriptor.floating_point_range[0].step = 0; - declare_parameter("alpha4", rclcpp::ParameterValue(0.2), descriptor); - } - - { - auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); - descriptor.description = "Strafe noise from translation for the omnidirectional drive model."; - descriptor.floating_point_range.resize(1); - descriptor.floating_point_range[0].from_value = 0; - descriptor.floating_point_range[0].to_value = std::numeric_limits::max(); - descriptor.floating_point_range[0].step = 0; - declare_parameter("alpha5", rclcpp::ParameterValue(0.2), descriptor); - } - - { - auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); - descriptor.description = "Rotational movement required before performing a filter update."; - descriptor.floating_point_range.resize(1); - descriptor.floating_point_range[0].from_value = 0; - descriptor.floating_point_range[0].to_value = 2 * Sophus::Constants::pi(); - descriptor.floating_point_range[0].step = 0; - declare_parameter("update_min_a", rclcpp::ParameterValue(0.2), descriptor); - } - - { - auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); - descriptor.description = "Translational movement required before performing a filter update."; - descriptor.floating_point_range.resize(1); - descriptor.floating_point_range[0].from_value = 0; - descriptor.floating_point_range[0].to_value = std::numeric_limits::max(); - descriptor.floating_point_range[0].step = 0; - declare_parameter("update_min_d", rclcpp::ParameterValue(0.25), descriptor); - } - { auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); descriptor.description = "Which observation model to use [beam, likelihood_field]."; @@ -323,37 +70,6 @@ AmclNode::AmclNode(const rclcpp::NodeOptions& options) : rclcpp_lifecycle::Lifec descriptor.floating_point_range[0].step = 0; declare_parameter("laser_likelihood_max_dist", rclcpp::ParameterValue(2.0), descriptor); } - - { - auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); - descriptor.description = "Maximum scan range to be considered."; - descriptor.floating_point_range.resize(1); - descriptor.floating_point_range[0].from_value = 0; - descriptor.floating_point_range[0].to_value = std::numeric_limits::max(); - descriptor.floating_point_range[0].step = 0; - declare_parameter("laser_max_range", rclcpp::ParameterValue(100.0), descriptor); - } - - { - auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); - descriptor.description = "Minimum scan range to be considered."; - descriptor.floating_point_range.resize(1); - descriptor.floating_point_range[0].from_value = 0; - descriptor.floating_point_range[0].to_value = std::numeric_limits::max(); - descriptor.floating_point_range[0].step = 0; - declare_parameter("laser_min_range", rclcpp::ParameterValue(0.0), descriptor); - } - - { - auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); - descriptor.description = "How many evenly-spaced beams in each scan will be used when updating the filter."; - descriptor.integer_range.resize(1); - descriptor.integer_range[0].from_value = 2; - descriptor.integer_range[0].to_value = std::numeric_limits::max(); - descriptor.integer_range[0].step = 1; - declare_parameter("max_beams", rclcpp::ParameterValue(60), descriptor); - } - { auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); descriptor.description = "Mixture weight for the probability of hitting an obstacle."; @@ -427,72 +143,6 @@ AmclNode::AmclNode(const rclcpp::NodeOptions& options) : rclcpp_lifecycle::Lifec "and ignore subsequent ones."; declare_parameter("first_map_only", false, descriptor); } - - { - auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); - descriptor.description = "Set the initial pose from the initial_pose parameters."; - declare_parameter("set_initial_pose", false, descriptor); - } - - { - auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); - descriptor.description = "Initial pose x axis coordinate."; - declare_parameter("initial_pose.x", 0.0, descriptor); - } - - { - auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); - descriptor.description = "Initial pose y axis coordinate."; - declare_parameter("initial_pose.y", 0.0, descriptor); - } - - { - auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); - descriptor.description = "Initial pose yaw rotation."; - declare_parameter("initial_pose.yaw", 0.0, descriptor); - } - - { - auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); - descriptor.description = "Initial pose x axis covariance."; - declare_parameter("initial_pose.covariance_x", 0.0, descriptor); - } - - { - auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); - descriptor.description = "Initial pose y axis covariance."; - declare_parameter("initial_pose.covariance_y", 0.0, descriptor); - } - - { - auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); - descriptor.description = "Initial pose yaw covariance."; - declare_parameter("initial_pose.covariance_yaw", 0.0, descriptor); - } - - { - auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); - descriptor.description = "Initial pose xy covariance."; - declare_parameter("initial_pose.covariance_xy", 0.0, descriptor); - } - - { - auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); - descriptor.description = "Initial pose xyaw covariance."; - declare_parameter("initial_pose.covariance_xyaw", 0.0, descriptor); - } - - { - auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); - descriptor.description = "Initial pose yyaw covariance."; - declare_parameter("initial_pose.covariance_yyaw", 0.0, descriptor); - } - - { - auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); - descriptor.description = "Execution policy used to process particles [seq, par]."; - declare_parameter("execution_policy", "seq", descriptor); - } } AmclNode::~AmclNode() { @@ -804,67 +454,8 @@ void AmclNode::timer_callback() { return; } - // Particle weights from the filter may or may not be representative of the - // true distribution. If we resampled, they are not, and there will be multiple copies - // of the most likely candidates, all with unit weight. In this case the number of copies - // is a proxy for the prob density at each candidate. If we did not resample before updating - // the estimation and publishing this message (which can happen if the resample interval - // is set to something other than 1), then all particles are expected to be different - // and their weights are proportional to the prob density at each candidate. - // - // Only the combination of both the state distribution and the candidate weights together - // provide information about the probability density at each candidate. - // To handle both cases, we group repeated candidates and compute the accumulated weight - - struct RepresentativeData { - Sophus::SE2d state; - double weight{0.}; - }; - - struct RepresentativeBinHash { - std::size_t operator()(const Sophus::SE2d& s) const noexcept { - std::size_t h1 = std::hash{}(s.translation().x()); - std::size_t h2 = std::hash{}(s.translation().y()); - std::size_t h3 = std::hash{}(s.so2().log()); - return h1 ^ (h2 << 1) ^ (h3 << 2); - } - }; - - struct RepresentativeBinEqual { - bool operator()(const Sophus::SE2d& lhs, const Sophus::SE2d& rhs) const noexcept { - // good enough, since copies of the same candidate are expected to be identical copies - return lhs.translation().x() == rhs.translation().x() && // - lhs.translation().y() == rhs.translation().y() && // - lhs.so2().log() == rhs.so2().log(); - } - }; - - std::unordered_map - representatives_map; - representatives_map.reserve(particle_filter_->particles().size()); - double max_weight = 1e-5; // never risk dividing by zero - - for (const auto& [state, weight] : particle_filter_->particles()) { - auto& representative = representatives_map[state]; // if the element does not exist, create it - representative.state = state; - representative.weight += weight; - if (representative.weight > max_weight) { - max_weight = representative.weight; - } - } - - auto message = nav2_msgs::msg::ParticleCloud{}; - message.header.stamp = now(); - message.header.frame_id = get_parameter("global_frame_id").as_string(); - message.particles.reserve(particle_filter_->particles().size()); - - for (const auto& [key, representative] : representatives_map) { - auto& particle = message.particles.emplace_back(); - tf2::toMsg(representative.state, particle.pose); - particle.weight = representative.weight / max_weight; - } - - particle_cloud_pub_->publish(message); + particle_cloud_pub_->publish( + make_representative_particle_cloud(*particle_filter_, get_parameter("global_frame_id").as_string(), now())); } void AmclNode::laser_callback(sensor_msgs::msg::LaserScan::ConstSharedPtr laser_scan) { diff --git a/beluga_amcl/src/ndt_amcl_node.cpp b/beluga_amcl/src/ndt_amcl_node.cpp new file mode 100644 index 000000000..523d50de4 --- /dev/null +++ b/beluga_amcl/src/ndt_amcl_node.cpp @@ -0,0 +1,491 @@ +// 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 + +namespace beluga_amcl { + +NdtAmclNode::NdtAmclNode(const rclcpp::NodeOptions& options) + : rclcpp_lifecycle::LifecycleNode{"ndt_amcl", "", options} { + RCLCPP_INFO(get_logger(), "Creating"); + + declare_common_params(*this); + { + 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.0), descriptor); + } + + { + auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); + descriptor.description = "Scaling parameter d1 in literature, used for scaling 2D 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 2D 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(1.0), descriptor); + } +} + +NdtAmclNode::~NdtAmclNode() { + RCLCPP_INFO(get_logger(), "Destroying"); + // In case this lifecycle node wasn't properly shut down, do it here + on_shutdown(get_current_state()); +} + +NdtAmclNode::CallbackReturn NdtAmclNode::on_configure(const rclcpp_lifecycle::State&) { + RCLCPP_INFO(get_logger(), "Configuring"); + particle_cloud_pub_ = create_publisher("particle_cloud", rclcpp::SensorDataQoS()); + pose_pub_ = create_publisher("pose", rclcpp::SystemDefaultsQoS()); + return CallbackReturn::SUCCESS; +} + +NdtAmclNode::CallbackReturn NdtAmclNode::on_activate(const rclcpp_lifecycle::State&) { + RCLCPP_INFO(get_logger(), "Activating"); + particle_filter_ = make_particle_filter(); + particle_cloud_pub_->on_activate(); + pose_pub_->on_activate(); + + { + bond_ = std::make_unique("bond", get_name(), shared_from_this()); + bond_->setHeartbeatPeriod(0.10); + bond_->setHeartbeatTimeout(4.0); + bond_->start(); + RCLCPP_INFO(get_logger(), "Created bond (%s) to lifecycle manager", get_name()); + } + + // Accessing the particle filter is not thread safe. + // This ensures that different callbacks are not called concurrently. + auto common_callback_group = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + auto common_subscription_options = rclcpp::SubscriptionOptions{}; + common_subscription_options.callback_group = common_callback_group; + + { + using namespace std::chrono_literals; + // TODO(alon): create a parameter for the timer rate? + timer_ = create_wall_timer(200ms, std::bind(&NdtAmclNode::timer_callback, this), common_callback_group); + } + + { + initial_pose_sub_ = create_subscription( + get_parameter("initial_pose_topic").as_string(), rclcpp::SystemDefaultsQoS(), + std::bind(&NdtAmclNode::initial_pose_callback, this, std::placeholders::_1), common_subscription_options); + RCLCPP_INFO(get_logger(), "Subscribed to initial_pose_topic: %s", initial_pose_sub_->get_topic_name()); + } + + { + tf_buffer_ = std::make_unique(get_clock()); + tf_buffer_->setCreateTimerInterface( + std::make_shared(get_node_base_interface(), get_node_timers_interface())); + tf_broadcaster_ = std::make_unique(shared_from_this()); + tf_listener_ = std::make_unique( + *tf_buffer_, this, + false); // avoid using dedicated tf thread + + 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(&NdtAmclNode::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; + initialize_from_estimate(initial_estimate.value()); + } + } + return CallbackReturn::SUCCESS; +} + +NdtAmclNode::CallbackReturn NdtAmclNode::on_deactivate(const rclcpp_lifecycle::State&) { + RCLCPP_INFO(get_logger(), "Deactivating"); + particle_cloud_pub_->on_deactivate(); + pose_pub_->on_deactivate(); + initial_pose_sub_.reset(); + laser_scan_connection_.disconnect(); + laser_scan_filter_.reset(); + laser_scan_sub_.reset(); + tf_listener_.reset(); + tf_broadcaster_.reset(); + tf_buffer_.reset(); + bond_.reset(); + return CallbackReturn::SUCCESS; +} + +NdtAmclNode::CallbackReturn NdtAmclNode::on_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; + return CallbackReturn::SUCCESS; +} + +NdtAmclNode::CallbackReturn NdtAmclNode::on_shutdown(const rclcpp_lifecycle::State& state) { + RCLCPP_INFO(get_logger(), "Shutting down"); + if (state.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { + on_deactivate(state); + on_cleanup(state); + } + if (state.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { + on_cleanup(state); + } + return CallbackReturn::SUCCESS; +} + +auto NdtAmclNode::get_initial_estimate() const -> std::optional> { + if (!get_parameter("set_initial_pose").as_bool()) { + return std::nullopt; + } + + const auto pose = Sophus::SE2d{ + Sophus::SO2d{get_parameter("initial_pose.yaw").as_double()}, + Eigen::Vector2d{ + get_parameter("initial_pose.x").as_double(), + get_parameter("initial_pose.y").as_double(), + }, + }; + + Eigen::Matrix3d covariance; + 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_yaw").as_double(); + covariance.coeffRef(0, 1) = get_parameter("initial_pose.covariance_xy").as_double(); + covariance.coeffRef(1, 0) = covariance.coeffRef(0, 1); + covariance.coeffRef(0, 2) = get_parameter("initial_pose.covariance_xyaw").as_double(); + covariance.coeffRef(2, 0) = covariance.coeffRef(0, 2); + covariance.coeffRef(1, 2) = get_parameter("initial_pose.covariance_yyaw").as_double(); + covariance.coeffRef(2, 1) = covariance.coeffRef(1, 2); + + return std::make_pair(pose, covariance); +} + +auto NdtAmclNode::get_motion_model() const -> MotionModelVariant { + const auto name = get_parameter("robot_model_type").as_string(); + if (name == kDifferentialModelName || name == kNav2DifferentialModelName) { + auto params = beluga::DifferentialDriveModelParam{}; + params.rotation_noise_from_rotation = get_parameter("alpha1").as_double(); + params.rotation_noise_from_translation = get_parameter("alpha2").as_double(); + params.translation_noise_from_translation = get_parameter("alpha3").as_double(); + params.translation_noise_from_rotation = get_parameter("alpha4").as_double(); + return beluga::DifferentialDriveModel{params}; + } + if (name == kOmnidirectionalModelName || name == kNav2OmnidirectionalModelName) { + auto params = beluga::OmnidirectionalDriveModelParam{}; + 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(); + params.strafe_noise_from_translation = get_parameter("alpha5").as_double(); + return beluga::OmnidirectionalDriveModel{params}; + } + if (name == kStationaryModelName) { + return beluga::StationaryModel{}; + } + + throw std::invalid_argument(std::string("Invalid motion model: ") + name); +} + +beluga::NDTSensorModel NdtAmclNode::get_sensor_model() const { + auto params = beluga::NDTModelParam{}; + 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_ERROR(get_logger(), "Loading map from %s.", map_path.c_str()); + + return beluga::NDTSensorModel{ + params, beluga::io::load_from_hdf5_2d(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 { + 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_y").as_double(), + get_parameter("spatial_resolution_theta").as_double()); + + RandomStateGenerator random_state_maker = [](const auto& particles) { + static thread_local auto generator = std::mt19937{std::random_device()()}; + const auto estimate = beluga::estimate(beluga::views::states(particles), beluga::views::weights(particles)); + return [estimate]() { + return beluga::MultivariateNormalDistribution{estimate.first, estimate.second}(generator); + }; + }; + + 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 NdtAmclNode::timer_callback() { + if (!particle_filter_) { + return; + } + + if (particle_cloud_pub_->get_subscription_count() == 0) { + return; + } + std::visit( + [this](const auto& particle_filter) { + particle_cloud_pub_->publish( + make_representative_particle_cloud(particle_filter, get_parameter("global_frame_id").as_string(), now())); + }, + *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 NdtAmclNode::laser_callback(sensor_msgs::msg::LaserScan::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::SE2d{}; + 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); + } catch (const tf2::TransformException& error) { + RCLCPP_ERROR(get_logger(), "Could not transform from odom to base: %s", error.what()); + return; + } + + auto laser_pose_in_base = Sophus::SE3d{}; + try { + 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(), "Could not transform from base to laser: %s", error.what()); + return; + } + + const auto update_start_time = std::chrono::high_resolution_clock::now(); + + // TODO(nahuel, Ramiro): Remove this once we update the measurement type. + auto scan = beluga_ros::LaserScan{ + laser_scan, laser_pose_in_base, static_cast(get_parameter("max_beams").as_int()), + get_parameter("laser_min_range").as_double(), get_parameter("laser_max_range").as_double()}; + auto measurement = scan.points_in_cartesian_coordinates() | // + ranges::views::transform([&scan](const auto& p) { + return Eigen::Vector2d((scan.origin() * Sophus::Vector3d{p.x(), p.y(), 0}).head<2>()); + }) | + ranges::to; + const auto new_estimate = std::visit( + [base_pose_in_odom, measurement = std::move(measurement)](auto& particle_filter) { + return particle_filter.update( + base_pose_in_odom, // + std::move(measurement)); + }, + *particle_filter_); + RCLCPP_INFO(get_logger(), "No-motion update requested"); + + 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()) { + 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 %ld points - %.3fms", num_particles, + laser_scan->ranges.size(), 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; + } + + const auto& [base_pose_in_map, base_pose_covariance] = last_known_estimate_.value(); + + // 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(); + tf2::toMsg(base_pose_in_map, message.pose.pose); + tf2::covarianceEigenToRowMajor(base_pose_covariance, message.pose.covariance); + pose_pub_->publish(message); + } + + // Transforms are always published to keep them current. + if (enable_tf_broadcast_ && get_parameter("tf_broadcast").as_bool()) { + const auto odom_transform_in_map = base_pose_in_map * base_pose_in_odom.inverse(); + 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(odom_transform_in_map); + tf_broadcaster_->sendTransform(message); + } +} + +void NdtAmclNode::initial_pose_callback(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr message) { + const auto global_frame_id = get_parameter("global_frame_id").as_string(); + if (message->header.frame_id != global_frame_id) { + RCLCPP_WARN( + get_logger(), "Ignoring initial pose in frame \"%s\"; it must be in the global frame \"%s\"", + message->header.frame_id.c_str(), global_frame_id.c_str()); + return; + } + + auto pose = Sophus::SE2d{}; + tf2::convert(message->pose.pose, pose); + + auto covariance = Eigen::Matrix3d{}; + tf2::covarianceRowMajorToEigen(message->pose.covariance, covariance); + + last_known_estimate_ = std::make_pair(pose, covariance); + initialize_from_estimate(last_known_estimate_.value()); +} + +bool NdtAmclNode::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, yaw=%g", + num_particles, pose.translation().x(), pose.translation().y(), pose.so2().log()); + + return true; +} +} // namespace beluga_amcl + +RCLCPP_COMPONENTS_REGISTER_NODE(beluga_amcl::NdtAmclNode) diff --git a/beluga_amcl/test/cmake/ament.cmake b/beluga_amcl/test/cmake/ament.cmake index ebd04a5cd..2a1fa1da2 100644 --- a/beluga_amcl/test/cmake/ament.cmake +++ b/beluga_amcl/test/cmake/ament.cmake @@ -14,6 +14,20 @@ find_package(ament_cmake_gmock REQUIRED) +file(COPY test_data DESTINATION ${CMAKE_CURRENT_BINARY_DIR}) + ament_add_gmock(test_amcl_node test_amcl_node.cpp) target_compile_options(test_amcl_node PRIVATE -Wno-deprecated-copy) target_link_libraries(test_amcl_node amcl_node_component) +target_include_directories(test_amcl_node + PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/test_utils) + +ament_add_gmock( + test_ndt_amcl_node + test_ndt_amcl_node.cpp + WORKING_DIRECTORY + ${CMAKE_CURRENT_BINARY_DIR}) +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) diff --git a/beluga_amcl/test/test_amcl_node.cpp b/beluga_amcl/test/test_amcl_node.cpp index 65a2844e4..e091dbf79 100644 --- a/beluga_amcl/test/test_amcl_node.cpp +++ b/beluga_amcl/test/test_amcl_node.cpp @@ -13,61 +13,21 @@ // limitations under the License. #include -#include + +#include "test_utils/node_testing.hpp" #include #include +#include + #include namespace { using namespace std::chrono_literals; - -/// Spin a group of nodes until a condition is met. -/** - * \param predicate The stop condition. - * \param timeout Maximum time to spin. - * \param nodes The nodes to spin. - * \return True if the condition was met. False if it timed out. - */ -template -bool spin_until( - Predicate&& predicate, - const std::chrono::duration& timeout, - const std::shared_ptr&... nodes) { - rclcpp::executors::SingleThreadedExecutor executor; - (executor.add_node(nodes->get_node_base_interface()), ...); - - const auto deadline = std::chrono::high_resolution_clock::now() + timeout; - while (rclcpp::ok() && !predicate() && std::chrono::high_resolution_clock::now() < deadline) { - executor.spin_once(deadline - std::chrono::high_resolution_clock::now()); // wait for it - executor.spin_some(deadline - std::chrono::high_resolution_clock::now()); // flush it all out - } - return predicate(); // last minute check -} - -/// Spin a group of nodes until a condition is met with a default timeout. -/** - * \param predicate The stop condition. - * \param nodes The nodes to spin. - * \return True if the condition was met. False if it timed out. - */ -template -bool spin_until(Predicate&& predicate, const std::shared_ptr&... nodes) { - return spin_until(std::forward(predicate), 10s, nodes...); -} - -/// Spin a group of nodes for a given duration of time. -/** - * \param predicate The stop condition. - * \param duration Time to spin. - */ -template -void spin_for(const std::chrono::duration& duration, const std::shared_ptr&... nodes) { - const auto duration_is_over = []() { return false; }; - spin_until(duration_is_over, duration, nodes...); -} +using beluga_amcl::testing::spin_for; +using beluga_amcl::testing::spin_until; /// Test class that provides convenient public accessors. class AmclNodeUnderTest : public beluga_amcl::AmclNode { @@ -82,214 +42,6 @@ class AmclNodeUnderTest : public beluga_amcl::AmclNode { const auto& estimate() { return last_known_estimate_.value(); } }; -// Tester node that can publish default messages and test ROS interactions. -class TesterNode : public rclcpp::Node { - public: - TesterNode() : rclcpp::Node{"tester_node", "", rclcpp::NodeOptions()} { - map_publisher_ = create_publisher("map", rclcpp::SystemDefaultsQoS()); - - initial_pose_publisher_ = - create_publisher("initialpose", rclcpp::SystemDefaultsQoS()); - - laser_scan_publisher_ = create_publisher("scan", rclcpp::SystemDefaultsQoS()); - - global_localization_client_ = create_client("reinitialize_global_localization"); - - nomotion_update_client_ = create_client("request_nomotion_update"); - } - - void create_transform_buffer() { - // NOTE(nahuel): This cannot be called in the constructor as it uses shared_from_this(). - // That's why we provide an initialization method. - tf_buffer_ = std::make_unique(get_clock()); - tf_buffer_->setCreateTimerInterface( - std::make_shared(get_node_base_interface(), get_node_timers_interface())); - tf_broadcaster_ = std::make_unique(shared_from_this()); - tf_listener_ = std::make_unique( - *tf_buffer_, this, - false); // avoid using dedicated tf thread - } - - void create_pose_subscriber() { - pose_subscriber_ = create_subscription( - "pose", rclcpp::SystemDefaultsQoS(), std::bind(&TesterNode::pose_callback, this, std::placeholders::_1)); - } - - void pose_callback(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr message) { latest_pose_ = *message; } - - auto& latest_pose() { return latest_pose_; } - - void create_particle_cloud_subscriber() { - particle_cloud_subscriber_ = create_subscription( - "particle_cloud", rclcpp::SystemDefaultsQoS(), - std::bind(&TesterNode::particle_cloud_callback, this, std::placeholders::_1)); - } - - void particle_cloud_callback(nav2_msgs::msg::ParticleCloud::SharedPtr message) { latest_particle_cloud_ = *message; } - - const auto& latest_particle_cloud() const { return latest_particle_cloud_; } - - static auto make_dummy_map() { - auto map = nav_msgs::msg::OccupancyGrid{}; - map.header.frame_id = "map"; - map.info.resolution = 1.0; - map.info.width = 2; - map.info.height = 2; - map.data = std::vector{0, 0, 0, 0}; - return map; - } - - void publish_map() { map_publisher_->publish(make_dummy_map()); } - - void publish_map_with_wrong_frame() { - auto map = make_dummy_map(); - map.header.frame_id = "non_existing_frame"; - map_publisher_->publish(map); - } - - void publish_default_initial_pose() { - auto pose = geometry_msgs::msg::PoseWithCovarianceStamped{}; - pose.header.frame_id = "map"; - initial_pose_publisher_->publish(pose); - } - - void publish_initial_pose(double x, double y) { - auto pose = geometry_msgs::msg::PoseWithCovarianceStamped{}; - pose.header.frame_id = "map"; - pose.pose.pose.position.x = x; - pose.pose.pose.position.y = y; - initial_pose_publisher_->publish(pose); - } - - void publish_initial_pose_with_wrong_frame() { - auto pose = geometry_msgs::msg::PoseWithCovarianceStamped{}; - pose.header.frame_id = "non_existing_frame"; - initial_pose_publisher_->publish(pose); - } - - void publish_laser_scan() { - const auto timestamp = now(); - - auto scan = sensor_msgs::msg::LaserScan{}; - scan.header.stamp = timestamp; - scan.header.frame_id = "laser"; - - auto transform_base = geometry_msgs::msg::TransformStamped{}; - transform_base.header.stamp = timestamp; - transform_base.header.frame_id = "odom"; - transform_base.child_frame_id = "base_footprint"; - - auto transform_laser = geometry_msgs::msg::TransformStamped{}; - transform_laser.header.stamp = timestamp; - transform_laser.header.frame_id = "base_footprint"; - transform_laser.child_frame_id = "laser"; - - laser_scan_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(); - - auto scan = sensor_msgs::msg::LaserScan{}; - 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"; - - laser_scan_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(); - - auto scan = sensor_msgs::msg::LaserScan{}; - 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"; - - laser_scan_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); - } - - auto lookup_transform(const std::string& target, const std::string& source) const { - auto transform = Sophus::SE2d{}; - 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); - } - - auto async_request_global_localization() { - auto request = std::make_shared(); - return global_localization_client_->async_send_request(request); - } - - auto prune_pending_global_localization_requests() { global_localization_client_->prune_pending_requests(); } - - auto async_nomotion_update_request() { - auto request = std::make_shared(); - return nomotion_update_client_->async_send_request(request); - } - - auto prune_pending_nomotion_update_request() { nomotion_update_client_->prune_pending_requests(); } - - private: - template - using PublisherPtr = std::shared_ptr>; - - PublisherPtr map_publisher_; - PublisherPtr initial_pose_publisher_; - PublisherPtr laser_scan_publisher_; - - template - using SubscriberPtr = std::shared_ptr>; - - SubscriberPtr pose_subscriber_; - SubscriberPtr particle_cloud_subscriber_; - - std::optional latest_pose_; - std::optional latest_particle_cloud_; - - std::unique_ptr tf_buffer_; - std::unique_ptr tf_broadcaster_; - std::unique_ptr tf_listener_; - - std::shared_ptr> global_localization_client_; - std::shared_ptr> nomotion_update_client_; -}; - /// Base node fixture class with common utilities. template class BaseNodeFixture : public T { @@ -297,7 +49,7 @@ class BaseNodeFixture : public T { void SetUp() override { rclcpp::init(0, nullptr); amcl_node_ = std::make_shared(); - tester_node_ = std::make_shared(); + tester_node_ = std::make_shared(); tester_node_->create_transform_buffer(); } @@ -349,7 +101,7 @@ class BaseNodeFixture : public T { protected: std::shared_ptr amcl_node_; - std::shared_ptr tester_node_; + std::shared_ptr tester_node_; }; class TestLifecycle : public BaseNodeFixture<::testing::Test> {}; diff --git a/beluga_amcl/test/test_data/turtlebot3_world.hdf5 b/beluga_amcl/test/test_data/turtlebot3_world.hdf5 new file mode 100644 index 000000000..de69054d7 Binary files /dev/null and b/beluga_amcl/test/test_data/turtlebot3_world.hdf5 differ diff --git a/beluga_amcl/test/test_ndt_amcl_node.cpp b/beluga_amcl/test/test_ndt_amcl_node.cpp new file mode 100644 index 000000000..f7f39f284 --- /dev/null +++ b/beluga_amcl/test/test_ndt_amcl_node.cpp @@ -0,0 +1,524 @@ +// 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 "test_utils/node_testing.hpp" + +#include + +#include +#include + +#include + +#include + +#include + +namespace { + +const std::string kTestMapPath = "./test_data/turtlebot3_world.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::NdtAmclNode { + 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)); + } + + 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_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 BaseNodeFixture<::testing::Test> {}; + +TEST_F(TestLifecycle, FullSpin) { + using lifecycle_msgs::msg::State; + auto node = std::make_shared(); + node->set_parameter(rclcpp::Parameter("map_path", kTestMapPath)); + 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->deactivate().id(), State::PRIMARY_STATE_INACTIVE); + spin_for(10ms, node); + ASSERT_EQ(node->cleanup().id(), State::PRIMARY_STATE_UNCONFIGURED); + spin_for(10ms, node); + ASSERT_EQ(node->shutdown().id(), State::PRIMARY_STATE_FINALIZED); +} + +TEST_F(TestLifecycle, ShutdownWhenActive) { + using lifecycle_msgs::msg::State; + auto node = std::make_shared(); + node->set_parameter(rclcpp::Parameter("map_path", kTestMapPath)); + ASSERT_EQ(node->get_current_state().id(), State::PRIMARY_STATE_UNCONFIGURED); + spin_for(10ms, node); + ASSERT_EQ(node->configure().id(), State::PRIMARY_STATE_INACTIVE); + spin_for(10ms, node); + ASSERT_EQ(node->activate().id(), State::PRIMARY_STATE_ACTIVE); + spin_for(10ms, node); + ASSERT_EQ(node->shutdown().id(), State::PRIMARY_STATE_FINALIZED); +} + +TEST_F(TestLifecycle, ShutdownWhenInactive) { + using lifecycle_msgs::msg::State; + auto node = std::make_shared(); + node->set_parameter(rclcpp::Parameter("map_path", kTestMapPath)); + ASSERT_EQ(node->get_current_state().id(), State::PRIMARY_STATE_UNCONFIGURED); + spin_for(10ms, node); + ASSERT_EQ(node->configure().id(), State::PRIMARY_STATE_INACTIVE); + spin_for(10ms, node); + ASSERT_EQ(node->shutdown().id(), State::PRIMARY_STATE_FINALIZED); +} + +TEST_F(TestLifecycle, ShutdownWhenUnconfigured) { + using lifecycle_msgs::msg::State; + auto node = std::make_shared(); + node->set_parameter(rclcpp::Parameter("map_path", kTestMapPath)); + ASSERT_EQ(node->get_current_state().id(), State::PRIMARY_STATE_UNCONFIGURED); + spin_for(10ms, node); + ASSERT_EQ(node->shutdown().id(), State::PRIMARY_STATE_FINALIZED); +} + +TEST_F(TestLifecycle, DestroyWhenActive) { + using lifecycle_msgs::msg::State; + auto node = std::make_shared(); + node->set_parameter(rclcpp::Parameter("map_path", kTestMapPath)); + ASSERT_EQ(node->get_current_state().id(), State::PRIMARY_STATE_UNCONFIGURED); + spin_for(10ms, node); + ASSERT_EQ(node->configure().id(), State::PRIMARY_STATE_INACTIVE); + spin_for(10ms, node); + ASSERT_EQ(node->activate().id(), State::PRIMARY_STATE_ACTIVE); +} + +TEST_F(TestLifecycle, DestroyWhenInactive) { + using lifecycle_msgs::msg::State; + auto node = std::make_shared(); + node->set_parameter(rclcpp::Parameter("map_path", kTestMapPath)); + ASSERT_EQ(node->get_current_state().id(), State::PRIMARY_STATE_UNCONFIGURED); + spin_for(10ms, node); + ASSERT_EQ(node->configure().id(), State::PRIMARY_STATE_INACTIVE); +} + +TEST_F(TestLifecycle, DestroyWhenUnconfigured) { + using lifecycle_msgs::msg::State; + auto node = std::make_shared(); + node->set_parameter(rclcpp::Parameter("map_path", kTestMapPath)); + ASSERT_EQ(node->get_current_state().id(), State::PRIMARY_STATE_UNCONFIGURED); +} + +class TestInitializationWithModel : public BaseNodeFixture<::testing::TestWithParam> {}; + +INSTANTIATE_TEST_SUITE_P( + Models, + TestInitializationWithModel, + testing::Values("differential_drive", "omnidirectional_drive", "stationary")); + +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{"min_particles", 10}); + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"max_particles", 30}); + + ndt_amcl_node_->configure(); + ndt_amcl_node_->activate(); + + ASSERT_TRUE(wait_for_initialization()); + + std::visit( + [](const auto& pf) { + ASSERT_GE(pf.particles().size(), 10UL); + ASSERT_LE(pf.particles().size(), 30UL); + }, + *ndt_amcl_node_->particle_filter()); +} + +class TestNode : public BaseNodeFixture<::testing::Test> {}; + +TEST_F(TestNode, SetInitialPose) { + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"set_initial_pose", true}); + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"initial_pose.x", 34.0}); + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"initial_pose.y", 2.0}); + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"initial_pose.yaw", 0.3}); + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"initial_pose.covariance_x", 0.001}); + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"initial_pose.covariance_y", 0.001}); + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"initial_pose.covariance_yaw", 0.001}); + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"initial_pose.covariance_xy", 0.0}); + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"initial_pose.covariance_xyaw", 0.0}); + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"initial_pose.covariance_yyaw", 0.0}); + ndt_amcl_node_->configure(); + ndt_amcl_node_->activate(); + ASSERT_TRUE(wait_for_initialization()); + const auto [pose, _] = ndt_amcl_node_->estimate(); + ASSERT_NEAR(pose.translation().x(), 34.0, 0.01); + ASSERT_NEAR(pose.translation().y(), 2.0, 0.01); + ASSERT_NEAR(pose.so2().log(), 0.3, 0.01); +} + +TEST_F(TestNode, BroadcastWhenInitialPoseSet) { + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"set_initial_pose", true}); + ndt_amcl_node_->configure(); + ndt_amcl_node_->activate(); + ASSERT_TRUE(wait_for_initialization()); + ASSERT_FALSE(tester_node_->can_transform("map", "odom")); + tester_node_->publish_laser_scan(); + ASSERT_TRUE(wait_for_pose_estimate()); + ASSERT_TRUE(tester_node_->can_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")); + tester_node_->publish_laser_scan(); + 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.0}); + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"initial_pose.covariance_xy", -50.0}); + ndt_amcl_node_->configure(); + ndt_amcl_node_->activate(); + ASSERT_TRUE(wait_for_initialization()); + ASSERT_FALSE(tester_node_->can_transform("map", "odom")); + tester_node_->publish_laser_scan(); + ASSERT_FALSE(wait_for_pose_estimate()); + ASSERT_FALSE(tester_node_->can_transform("map", "odom")); +} + +TEST_F(TestNode, KeepCurrentEstimate) { + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"set_initial_pose", true}); + { + // Set initial pose values to simulate an estimate that has converged. + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"initial_pose.x", 34.0}); + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"initial_pose.y", 2.0}); + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"initial_pose.yaw", 0.3}); + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"initial_pose.covariance_x", 0.001}); + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"initial_pose.covariance_y", 0.001}); + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"initial_pose.covariance_yaw", 0.001}); + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"initial_pose.covariance_xy", 0.0}); + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"initial_pose.covariance_xyaw", 0.0}); + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"initial_pose.covariance_yyaw", 0.0}); + } + + ndt_amcl_node_->configure(); + ndt_amcl_node_->activate(); + + { + // Initializing with the first map and initial pose values. + ASSERT_TRUE(wait_for_initialization()); + const auto [pose, _] = ndt_amcl_node_->estimate(); + ASSERT_NEAR(pose.translation().x(), 34.0, 0.01); + ASSERT_NEAR(pose.translation().y(), 2.0, 0.01); + ASSERT_NEAR(pose.so2().log(), 0.3, 0.01); + } + + tester_node_->publish_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_laser_scan(); + ASSERT_TRUE(wait_for_pose_estimate()); + ndt_amcl_node_->deactivate(); + ndt_amcl_node_->cleanup(); + + { + // Set new initial pose values that will be ignored. + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"initial_pose.x", 12.0}); + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"initial_pose.y", 1.0}); + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"initial_pose.yaw", 0.7}); + } + + ndt_amcl_node_->configure(); + ndt_amcl_node_->activate(); + ASSERT_NE(ndt_amcl_node_->particle_filter().get(), nullptr); + ASSERT_TRUE(wait_for_initialization()); + const auto [pose, _] = ndt_amcl_node_->estimate(); + ASSERT_NEAR(pose.translation().x(), 12.0, 0.01); + ASSERT_NEAR(pose.translation().y(), 1.0, 0.01); + ASSERT_NEAR(pose.so2().log(), 0.7, 0.01); +} + +TEST_F(TestNode, InvalidMotionModel) { + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"robot_model_type", "non_existing_model"}); + ndt_amcl_node_->configure(); + ndt_amcl_node_->activate(); + ASSERT_FALSE(wait_for_initialization()); +} +TEST_F(TestNode, InvalidExecutionPolicy) { + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"execution_policy", "non_existing_policy"}); + ndt_amcl_node_->configure(); + ndt_amcl_node_->activate(); + ASSERT_FALSE(wait_for_initialization()); +} + +TEST_F(TestNode, SequentialExecutionPolicy) { + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"execution_policy", "seq"}); + ndt_amcl_node_->configure(); + ndt_amcl_node_->activate(); + ASSERT_TRUE(wait_for_initialization()); +} + +TEST_F(TestNode, ParallelExecutionPolicy) { + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"execution_policy", "par"}); + ndt_amcl_node_->configure(); + ndt_amcl_node_->activate(); + ASSERT_TRUE(wait_for_initialization()); +} + +TEST_F(TestNode, InitialPoseBeforeInitialize) { + ndt_amcl_node_->set_parameter(rclcpp::Parameter{"set_initial_pose", false}); + ndt_amcl_node_->configure(); + ndt_amcl_node_->activate(); + ASSERT_FALSE(wait_for_pose_estimate()); + tester_node_->publish_default_initial_pose(); + tester_node_->publish_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(); + tester_node_->publish_laser_scan(); + ASSERT_TRUE(wait_for_pose_estimate()); + ASSERT_TRUE(tester_node_->can_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(); + tester_node_->publish_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(); + tester_node_->publish_laser_scan(); + ASSERT_TRUE(wait_for_pose_estimate()); + ASSERT_TRUE(tester_node_->can_transform("map", "odom")); + + { + const auto [pose, _] = ndt_amcl_node_->estimate(); + ASSERT_NEAR(pose.translation().x(), 0.0, 0.01); + ASSERT_NEAR(pose.translation().y(), 0.0, 0.01); + ASSERT_NEAR(pose.so2().log(), 0., 0.01); + } + + tester_node_->publish_initial_pose(1., 1.); + tester_node_->publish_laser_scan(); + ASSERT_TRUE(wait_for_pose_estimate()); + ASSERT_TRUE(tester_node_->can_transform("map", "odom")); + + { + const auto [pose, _] = ndt_amcl_node_->estimate(); + ASSERT_NEAR(pose.translation().x(), 1.0, 0.01); + ASSERT_NEAR(pose.translation().y(), 1.0, 0.01); + ASSERT_NEAR(pose.so2().log(), 0., 0.01); + } +} + +TEST_F(TestNode, IsPublishingParticleCloud) { + ndt_amcl_node_->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.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_laser_scan_with_odom_to_base(Sophus::SE2d{ + Sophus::SO2d{Sophus::Constants::pi() / 3}, + Sophus::Vector2d{3., 4.}, + }); + 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); + ASSERT_NEAR(pose.so2().log(), Sophus::Constants::pi() / 3, 0.01); + + const auto transform = tester_node_->lookup_transform("map", "odom"); + EXPECT_NEAR(transform.translation().x(), -2.00, 0.01); + EXPECT_NEAR(transform.translation().y(), -2.00, 0.01); + EXPECT_NEAR(transform.so2().log(), 0.0, 0.01); +} + +class TestParameterValue : public ::testing::TestWithParam {}; + +INSTANTIATE_TEST_SUITE_P( + InvalidParameterValues, + TestParameterValue, + testing::Values( + rclcpp::Parameter("min_particles", -1), + rclcpp::Parameter("max_particles", -1), + rclcpp::Parameter("recovery_alpha_slow", -1.0), + rclcpp::Parameter("recovery_alpha_slow", 2.0), + rclcpp::Parameter("recovery_alpha_fast", -1.0), + rclcpp::Parameter("recovery_alpha_fast", 2.0), + rclcpp::Parameter("pf_err", -1.0), + rclcpp::Parameter("pf_err", 2.0), + rclcpp::Parameter("spatial_resolution_x", -1.0), + rclcpp::Parameter("spatial_resolution_y", -1.0), + rclcpp::Parameter("spatial_resolution_theta", -1.0), + rclcpp::Parameter("spatial_resolution_theta", 7.0), + rclcpp::Parameter("resample_interval", 0), + rclcpp::Parameter("transform_tolerance", -1.0), + rclcpp::Parameter("alpha1", -1.0), + rclcpp::Parameter("alpha2", -1.0), + rclcpp::Parameter("alpha3", -1.0), + rclcpp::Parameter("alpha4", -1.0), + rclcpp::Parameter("alpha5", -1.0), + rclcpp::Parameter("update_min_a", -1.0), + rclcpp::Parameter("update_min_a", 7.0), + rclcpp::Parameter("update_min_d", -1.0), + rclcpp::Parameter("laser_max_range", -1.0), + rclcpp::Parameter("laser_min_range", -1.0), + rclcpp::Parameter("max_beams", 1))); + +TEST_P(TestParameterValue, InvalidValue) { + const auto options = rclcpp::NodeOptions().parameter_overrides({GetParam()}); + 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 new file mode 100644 index 000000000..ba31b754c --- /dev/null +++ b/beluga_amcl/test/test_utils/node_testing.hpp @@ -0,0 +1,296 @@ +// 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. + +#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 +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include + +namespace beluga_amcl::testing { + +using namespace std::chrono_literals; + +// Tester node that can publish default messages and test ROS interactions. +class TesterNode : public rclcpp::Node { + public: + TesterNode() : rclcpp::Node{"tester_node", "", rclcpp::NodeOptions()} { + map_publisher_ = create_publisher("map", rclcpp::SystemDefaultsQoS()); + + initial_pose_publisher_ = + create_publisher("initialpose", rclcpp::SystemDefaultsQoS()); + + laser_scan_publisher_ = create_publisher("scan", rclcpp::SystemDefaultsQoS()); + + global_localization_client_ = create_client("reinitialize_global_localization"); + + nomotion_update_client_ = create_client("request_nomotion_update"); + } + + void create_transform_buffer() { + // NOTE(nahuel): This cannot be called in the constructor as it uses shared_from_this(). + // That's why we provide an initialization method. + tf_buffer_ = std::make_unique(get_clock()); + tf_buffer_->setCreateTimerInterface( + std::make_shared(get_node_base_interface(), get_node_timers_interface())); + tf_broadcaster_ = std::make_unique(shared_from_this()); + tf_listener_ = std::make_unique( + *tf_buffer_, this, + false); // avoid using dedicated tf thread + } + + void create_pose_subscriber() { + pose_subscriber_ = create_subscription( + "pose", rclcpp::SystemDefaultsQoS(), std::bind(&TesterNode::pose_callback, this, std::placeholders::_1)); + } + + void pose_callback(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr message) { latest_pose_ = *message; } + + auto& latest_pose() { return latest_pose_; } + + void create_particle_cloud_subscriber() { + particle_cloud_subscriber_ = create_subscription( + "particle_cloud", rclcpp::SystemDefaultsQoS(), + std::bind(&TesterNode::particle_cloud_callback, this, std::placeholders::_1)); + } + + void particle_cloud_callback(nav2_msgs::msg::ParticleCloud::SharedPtr message) { latest_particle_cloud_ = *message; } + + const auto& latest_particle_cloud() const { return latest_particle_cloud_; } + + static auto make_dummy_map() { + auto map = nav_msgs::msg::OccupancyGrid{}; + map.header.frame_id = "map"; + map.info.resolution = 1.0; + map.info.width = 2; + map.info.height = 2; + map.data = std::vector{0, 0, 0, 0}; + return map; + } + + void publish_map() { map_publisher_->publish(make_dummy_map()); } + + void publish_map_with_wrong_frame() { + auto map = make_dummy_map(); + map.header.frame_id = "non_existing_frame"; + map_publisher_->publish(map); + } + + void publish_default_initial_pose() { + auto pose = geometry_msgs::msg::PoseWithCovarianceStamped{}; + pose.header.frame_id = "map"; + initial_pose_publisher_->publish(pose); + } + + void publish_initial_pose(double x, double y) { + auto pose = geometry_msgs::msg::PoseWithCovarianceStamped{}; + pose.header.frame_id = "map"; + pose.pose.pose.position.x = x; + pose.pose.pose.position.y = y; + initial_pose_publisher_->publish(pose); + } + + void publish_initial_pose_with_wrong_frame() { + auto pose = geometry_msgs::msg::PoseWithCovarianceStamped{}; + pose.header.frame_id = "non_existing_frame"; + initial_pose_publisher_->publish(pose); + } + + void publish_laser_scan() { + const auto timestamp = now(); + + auto scan = sensor_msgs::msg::LaserScan{}; + scan.header.stamp = timestamp; + scan.header.frame_id = "laser"; + + auto transform_base = geometry_msgs::msg::TransformStamped{}; + transform_base.header.stamp = timestamp; + transform_base.header.frame_id = "odom"; + transform_base.child_frame_id = "base_footprint"; + + auto transform_laser = geometry_msgs::msg::TransformStamped{}; + transform_laser.header.stamp = timestamp; + transform_laser.header.frame_id = "base_footprint"; + transform_laser.child_frame_id = "laser"; + + laser_scan_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(); + + auto scan = sensor_msgs::msg::LaserScan{}; + 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"; + + laser_scan_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(); + + auto scan = sensor_msgs::msg::LaserScan{}; + 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"; + + laser_scan_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); + } + + auto lookup_transform(const std::string& target, const std::string& source) const { + auto transform = Sophus::SE2d{}; + 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); + } + + auto async_request_global_localization() { + auto request = std::make_shared(); + return global_localization_client_->async_send_request(request); + } + + auto prune_pending_global_localization_requests() { global_localization_client_->prune_pending_requests(); } + + auto async_nomotion_update_request() { + auto request = std::make_shared(); + return nomotion_update_client_->async_send_request(request); + } + + auto prune_pending_nomotion_update_request() { nomotion_update_client_->prune_pending_requests(); } + + private: + template + using PublisherPtr = std::shared_ptr>; + + PublisherPtr map_publisher_; + PublisherPtr initial_pose_publisher_; + PublisherPtr laser_scan_publisher_; + + template + using SubscriberPtr = std::shared_ptr>; + + SubscriberPtr pose_subscriber_; + SubscriberPtr particle_cloud_subscriber_; + + std::optional latest_pose_; + std::optional latest_particle_cloud_; + + std::unique_ptr tf_buffer_; + std::unique_ptr tf_broadcaster_; + std::unique_ptr tf_listener_; + + std::shared_ptr> global_localization_client_; + std::shared_ptr> nomotion_update_client_; +}; + +/// Spin a group of nodes until a condition is met. +/** + * \param predicate The stop condition. + * \param timeout Maximum time to spin. + * \param nodes The nodes to spin. + * \return True if the condition was met. False if it timed out. + */ +template +inline bool spin_until( + Predicate&& predicate, + const std::chrono::duration& timeout, + const std::shared_ptr&... nodes) { + rclcpp::executors::SingleThreadedExecutor executor; + (executor.add_node(nodes->get_node_base_interface()), ...); + + const auto deadline = std::chrono::high_resolution_clock::now() + timeout; + while (rclcpp::ok() && !predicate() && std::chrono::high_resolution_clock::now() < deadline) { + executor.spin_once(deadline - std::chrono::high_resolution_clock::now()); // wait for it + executor.spin_some(deadline - std::chrono::high_resolution_clock::now()); // flush it all out + } + return predicate(); // last minute check +} + +/// Spin a group of nodes until a condition is met with a default timeout. +/** + * \param predicate The stop condition. + * \param nodes The nodes to spin. + * \return True if the condition was met. False if it timed out. + */ +template +inline bool spin_until(Predicate&& predicate, const std::shared_ptr&... nodes) { + return spin_until(std::forward(predicate), 10s, nodes...); +} + +/// Spin a group of nodes for a given duration of time. +/** + * \param predicate The stop condition. + * \param duration Time to spin. + */ +template +inline void spin_for(const std::chrono::duration& duration, const std::shared_ptr&... nodes) { + const auto duration_is_over = []() { return false; }; + spin_until(duration_is_over, duration, nodes...); +} + +} // namespace beluga_amcl::testing +#endif