diff --git a/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp b/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp index e82ed5f6..669a2d2d 100644 --- a/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp +++ b/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp @@ -476,13 +476,13 @@ SerestController::apply_corner_guard( bool SerestController::should_turn_in_place( bool allow_reverse, double e_theta, double e_theta_goal, - double dist_to_end, double turn_in_place_thr) const + double dist_to_end, [[maybe_unused]] double turn_in_place_thr) const { // Mantenemos compatibilidad con la firma, pero ignoramos turn_in_place_thr // y usamos dos umbrales internos sin exponer parámetros. const double PI = 3.14159265358979323846; const double thr_enter = 60.0 * PI / 180.0; // entra a girar si |e_theta| > 60° - const double thr_exit = 35.0 * PI / 180.0; // sale de girar si |e_theta| < 35° + // const double thr_exit = 35.0 * PI / 180.0; // sale de girar si |e_theta| < 35° // No permitimos “atajo” marcha atrás en esta decisión: si no permites reverse, // el criterio es más estricto. diff --git a/localizers/easynav_costmap_localizer/include/easynav_costmap_localizer/AMCLLocalizer.hpp b/localizers/easynav_costmap_localizer/include/easynav_costmap_localizer/AMCLLocalizer.hpp index 5bba34d8..19b81a81 100644 --- a/localizers/easynav_costmap_localizer/include/easynav_costmap_localizer/AMCLLocalizer.hpp +++ b/localizers/easynav_costmap_localizer/include/easynav_costmap_localizer/AMCLLocalizer.hpp @@ -37,7 +37,7 @@ #include "nav_msgs/msg/odometry.hpp" #include "tf2/LinearMath/Transform.hpp" -#include "tf2_ros/transform_broadcaster.h" +#include "tf2_ros/transform_broadcaster.hpp" #include "easynav_core/LocalizerMethodBase.hpp" diff --git a/localizers/easynav_gps_localizer/include/easynav_gps_localizer/GpsLocalizer.hpp b/localizers/easynav_gps_localizer/include/easynav_gps_localizer/GpsLocalizer.hpp index 3bb42903..a6ece830 100644 --- a/localizers/easynav_gps_localizer/include/easynav_gps_localizer/GpsLocalizer.hpp +++ b/localizers/easynav_gps_localizer/include/easynav_gps_localizer/GpsLocalizer.hpp @@ -29,7 +29,7 @@ #include "sensor_msgs/msg/nav_sat_fix.hpp" #include "easynav_core/LocalizerMethodBase.hpp" #include "geometry_msgs/msg/transform_stamped.hpp" -#include "tf2_ros/static_transform_broadcaster.h" +#include "tf2_ros/static_transform_broadcaster.hpp" #include "sensor_msgs/msg/imu.hpp" #include "tf2/LinearMath/Quaternion.hpp" #include "tf2/LinearMath/Matrix3x3.hpp" diff --git a/localizers/easynav_gps_localizer/src/easynav_gps_localizer/GpsLocalizer.cpp b/localizers/easynav_gps_localizer/src/easynav_gps_localizer/GpsLocalizer.cpp index 6435d4c4..a50a7161 100644 --- a/localizers/easynav_gps_localizer/src/easynav_gps_localizer/GpsLocalizer.cpp +++ b/localizers/easynav_gps_localizer/src/easynav_gps_localizer/GpsLocalizer.cpp @@ -88,7 +88,7 @@ void GpsLocalizer::imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg) } -void GpsLocalizer::update_rt(NavState & nav_state) +void GpsLocalizer::update_rt([[maybe_unused]] NavState & nav_state) { } diff --git a/localizers/easynav_navmap_localizer/include/easynav_navmap_localizer/AMCLLocalizer.hpp b/localizers/easynav_navmap_localizer/include/easynav_navmap_localizer/AMCLLocalizer.hpp index 23ec92ef..fbfd32a9 100644 --- a/localizers/easynav_navmap_localizer/include/easynav_navmap_localizer/AMCLLocalizer.hpp +++ b/localizers/easynav_navmap_localizer/include/easynav_navmap_localizer/AMCLLocalizer.hpp @@ -37,7 +37,7 @@ #include "nav_msgs/msg/odometry.hpp" #include "tf2/LinearMath/Transform.hpp" -#include "tf2_ros/transform_broadcaster.h" +#include "tf2_ros/transform_broadcaster.hpp" #include "navmap_core/NavMap.hpp" diff --git a/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer copy_v1.cpp b/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer copy_v1.cpp deleted file mode 100644 index 42e769fa..00000000 --- a/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer copy_v1.cpp +++ /dev/null @@ -1,1325 +0,0 @@ -// Copyright 2025 Intelligent Robotics Lab -// -// This file is part of the project Easy Navigation (EasyNav in short) -// licensed under the GNU General Public License v3.0. -// See for details. -// -// Easy Navigation program is free software: you can redistribute it and/or modify -// it under the terms of the GNU General Public License as published by -// the Free Software Foundation, either version 3 of the License, or -// (at your option) any later version. -// -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. -// -// You should have received a copy of the GNU General Public License -// along with this program. If not, see . - -/// \file -/// \brief Implementation of the AMCLLocalizer class using Costmap2D. - -#include -#include -#include // <-- NUEVO -#include // <-- NUEVO - -#include "bonxai/bonxai.hpp" -#include "bonxai/probabilistic_map.hpp" - -#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" -#include "tf2/LinearMath/Vector3.hpp" - -#include "easynav_common/RTTFBuffer.hpp" -#include "easynav_common/types/Perceptions.hpp" -#include "easynav_common/types/PointPerception.hpp" -#include "easynav_common/types/IMUPerception.hpp" - -#include "navmap_core/NavMap.hpp" - -#include "easynav_navmap_localizer/AMCLLocalizer.hpp" -#include "easynav_localizer/LocalizerNode.hpp" - -namespace easynav -{ -namespace navmap -{ - -static constexpr unsigned char NO_INFORMATION = 255; -static constexpr unsigned char LETHAL_OBSTACLE = 254; -static constexpr unsigned char INSCRIBED_INFLATED_OBSTACLE = 253; -static constexpr unsigned char MAX_NON_OBSTACLE = 252; -static constexpr unsigned char FREE_SPACE = 0; - -tf2::Vector3 computeMean( - const std::vector & particles, - std::size_t start, std::size_t count) -{ - tf2::Vector3 weighted_sum(0.0, 0.0, 0.0); - double total_weight = 0.0; - - if (start >= particles.size() || count == 0) {return weighted_sum;} - - std::size_t end = std::min(start + count, particles.size()); - - for (std::size_t i = start; i < end; ++i) { - const Particle & p = particles[i]; - const tf2::Vector3 & origin = p.pose.getOrigin(); - if (!std::isfinite(origin.x()) || !std::isfinite(origin.y()) || !std::isfinite(origin.z())) { - continue; // descarta contribuciones inválidas - } - weighted_sum += origin * p.weight; - total_weight += p.weight; - } - - if (total_weight > 0.0) { - return weighted_sum / total_weight; - } else { - return tf2::Vector3(0.0, 0.0, 0.0); - } -} - -tf2::Matrix3x3 -computeCovariance( - const std::vector & particles, - std::size_t start, std::size_t count, - const tf2::Vector3 & mean) -{ - double total_weight = 0.0; - double cov[3][3] = {{0.0}}; - - if (start >= particles.size() || count == 0) {return tf2::Matrix3x3();} - - std::size_t end = std::min(start + count, particles.size()); - - for (std::size_t i = start; i < end; ++i) { - const Particle & p = particles[i]; - tf2::Vector3 delta = p.pose.getOrigin() - mean; - - double w = p.weight; - total_weight += w; - - cov[0][0] += w * delta.x() * delta.x(); - cov[0][1] += w * delta.x() * delta.y(); - cov[0][2] += w * delta.x() * delta.z(); - cov[1][1] += w * delta.y() * delta.y(); - cov[1][2] += w * delta.y() * delta.z(); - cov[2][2] += w * delta.z() * delta.z(); - } - - if (total_weight > 0.0) { - cov[0][0] /= total_weight; - cov[0][1] /= total_weight; - cov[0][2] /= total_weight; - cov[1][1] /= total_weight; - cov[1][2] /= total_weight; - cov[2][2] /= total_weight; - - // La matriz es simétrica - cov[1][0] = cov[0][1]; - cov[2][0] = cov[0][2]; - cov[2][1] = cov[1][2]; - } - - return tf2::Matrix3x3( - cov[0][0], cov[0][1], cov[0][2], - cov[1][0], cov[1][1], cov[1][2], - cov[2][0], cov[2][1], cov[2][2]); -} - -double -extractYaw(const tf2::Transform & transform) -{ - double roll, pitch, yaw; - tf2::Matrix3x3(transform.getRotation()).getRPY(roll, pitch, yaw); - return yaw; -} - -double -computeYawVariance( - const std::vector & particles, - std::size_t start, std::size_t count) -{ - double sum_cos = 0.0; - double sum_sin = 0.0; - double total_weight = 0.0; - - if (start >= particles.size() || count == 0) { - return 0.0; - } - - std::size_t end = std::min(start + count, particles.size()); - - for (std::size_t i = start; i < end; ++i) { - const Particle & p = particles[i]; - double yaw = extractYaw(p.pose); - double w = p.weight; - - sum_cos += w * std::cos(yaw); - sum_sin += w * std::sin(yaw); - total_weight += w; - } - - if (total_weight == 0.0) { - return 0.0; - } - - double mean_cos = sum_cos / total_weight; - double mean_sin = sum_sin / total_weight; - - double R = std::sqrt(mean_cos * mean_cos + mean_sin * mean_sin); - if (!std::isfinite(R)) {R = 0.0;} - R = std::clamp(R, 1e-12, 1.0); - double variance = -2.0 * std::log(R); - if (!std::isfinite(variance) || variance < 0.0) {variance = 0.0;} - return variance; -} - -using std::placeholders::_1; -using namespace std::chrono_literals; - - -AMCLLocalizer::AMCLLocalizer() -{ - NavState::register_printer( - [](const nav_msgs::msg::Odometry & odom) { - std::ostringstream ret; - double x = odom.pose.pose.position.x; - double y = odom.pose.pose.position.y; - - tf2::Quaternion q( - odom.pose.pose.orientation.x, - odom.pose.pose.orientation.y, - odom.pose.pose.orientation.z, - odom.pose.pose.orientation.w); - - double roll, pitch, yaw; - tf2::Matrix3x3(q).getRPY(roll, pitch, yaw); - - ret << "Odometry with pose: (x: " << x << ", y: " << y << ", yaw: " << yaw << ")"; - return ret.str(); - }); -} - -AMCLLocalizer::~AMCLLocalizer() -{ -} - -std::expected -AMCLLocalizer::on_initialize() -{ - auto node = get_node(); - const auto & plugin_name = get_plugin_name(); - - int num_particles; - double x_init, y_init, yaw_init, std_dev_xy, std_dev_yaw; - std::string perception_model; - - node->declare_parameter(plugin_name + ".num_particles", 100); - node->declare_parameter(plugin_name + ".initial_pose.x", 0.0); - node->declare_parameter(plugin_name + ".initial_pose.y", 0.0); - node->declare_parameter(plugin_name + ".initial_pose.yaw", 0.0); - node->declare_parameter(plugin_name + ".initial_pose.std_dev_xy", 0.5); - node->declare_parameter(plugin_name + ".initial_pose.std_dev_yaw", 0.5); - node->declare_parameter(plugin_name + ".reseed_freq", 1.0); - node->declare_parameter(plugin_name + ".noise_translation", 0.01); - node->declare_parameter(plugin_name + ".noise_rotation", 0.01); - node->declare_parameter(plugin_name + ".noise_translation_to_rotation", 0.01); - node->declare_parameter(plugin_name + ".min_noise_xy", 0.05); - node->declare_parameter(plugin_name + ".min_noise_yaw", 0.05); - node->declare_parameter(plugin_name + ".compute_odom_from_tf", false); - node->declare_parameter(plugin_name + ".perception_model", "occupancy"); - - node->get_parameter(plugin_name + ".num_particles", num_particles); - node->get_parameter(plugin_name + ".initial_pose.x", x_init); - node->get_parameter(plugin_name + ".initial_pose.y", y_init); - node->get_parameter(plugin_name + ".initial_pose.yaw", yaw_init); - node->get_parameter(plugin_name + ".initial_pose.std_dev_xy", std_dev_xy); - node->get_parameter(plugin_name + ".initial_pose.std_dev_yaw", std_dev_yaw); - node->get_parameter(plugin_name + ".noise_translation", noise_translation_); - node->get_parameter(plugin_name + ".noise_rotation", noise_rotation_); - node->get_parameter(plugin_name + ".noise_translation_to_rotation", - noise_translation_to_rotation_); - node->get_parameter(plugin_name + ".min_noise_xy", min_noise_xy_); - node->get_parameter(plugin_name + ".min_noise_yaw", min_noise_yaw_); - node->get_parameter(plugin_name + ".compute_odom_from_tf", compute_odom_from_tf_); - node->get_parameter(plugin_name + ".perception_model", perception_model); - - // if (perception_model == "octomap") { - // percepcion_model_ = PerceptionModel::OCT; - // } else { - // percepcion_model_ = PerceptionModel::OCC; - // } - - double reseed_freq; - node->get_parameter(plugin_name + ".reseed_freq", reseed_freq); - reseed_time_ = 1.0 / reseed_freq; - - RCLCPP_INFO(node->get_logger(), "Initialized AMCL pose with %d particles", num_particles); - RCLCPP_INFO(node->get_logger(), "at position (%lf, %lf, %lf) std_dev [%lf, %lf]", - x_init, y_init, yaw_init, std_dev_xy, std_dev_yaw); - - std::normal_distribution noise_x(x_init, std_dev_xy); - std::normal_distribution noise_y(y_init, std_dev_xy); - std::normal_distribution noise_yaw(yaw_init, std_dev_yaw); - - particles_.resize(num_particles); - for (auto & p : particles_) { - tf2::Quaternion q; - q.setRPY(0.0, 0.0, noise_yaw(rng_)); - p.pose.setRotation(q); - p.pose.setOrigin(tf2::Vector3(noise_x(rng_), noise_y(rng_), 0.0)); - - p.hits = 0; - p.possible_hits = 0; - p.weight = 1.0 / num_particles; - } - - tf_broadcaster_ = std::make_unique(get_node()); - - auto node_typed = std::dynamic_pointer_cast(get_node()); - auto rt_cbg = node_typed->get_real_time_cbg(); - rclcpp::SubscriptionOptions options; - options.callback_group = rt_cbg; - - if (!compute_odom_from_tf_) { - odom_sub_ = get_node()->create_subscription( - "odom", rclcpp::SensorDataQoS().reliable(), - std::bind(&AMCLLocalizer::odom_callback, this, _1), options); - } - - particles_pub_ = get_node()->create_publisher( - node->get_fully_qualified_name() + std::string("/") + plugin_name + "/particles", 10); - estimate_pub_ = get_node()->create_publisher( - node->get_fully_qualified_name() + std::string("/") + plugin_name + "/pose", 10); - - last_reseed_ = get_node()->now(); - - get_node()->get_logger().set_level(rclcpp::Logger::Level::Debug); - - return {}; -} - -void printTransform(const tf2::Transform & tf) -{ - const tf2::Vector3 & origin = tf.getOrigin(); - const tf2::Quaternion & rot = tf.getRotation(); - - std::cerr << "Translation: [" - << origin.x() << ", " - << origin.y() << ", " - << origin.z() << "]\t"; - - std::cerr << "Rotation (quaternion): [" - << rot.x() << ", " - << rot.y() << ", " - << rot.z() << ", " - << rot.w() << "]\n"; -} - -void -AMCLLocalizer::update_rt(NavState & nav_state) -{ - predict(nav_state); - - nav_state.set("robot_pose", get_pose()); -} - -void -AMCLLocalizer::update(NavState & nav_state) -{ - correct(nav_state); - - if ((get_node()->now() - last_reseed_).seconds() > reseed_time_) { - reseed(); - last_reseed_ = get_node()->now(); - } - - nav_state.set("robot_pose", get_pose()); - - publishParticles(); -} - -void -AMCLLocalizer::odom_callback(nav_msgs::msg::Odometry::UniquePtr msg) -{ - if (compute_odom_from_tf_) {return;} - - tf2::fromMsg(msg->pose.pose, odom_); - - if (!initialized_odom_) { - last_odom_ = odom_; - initialized_odom_ = true; - } -} - -void -AMCLLocalizer::update_odom_from_tf() -{ - geometry_msgs::msg::TransformStamped tf_msg; - try { - tf_msg = RTTFBuffer::getInstance()->lookupTransform( - "odom", "base_footprint", tf2::TimePointZero, tf2::durationFromSec(0.0)); - } catch (const tf2::TransformException & ex) { - RCLCPP_WARN(get_node()->get_logger(), "AMCLLocalizer::update: TF failed: %s", ex.what()); - return; - } - - tf2::Transform tf_odom; - tf2::fromMsg(tf_msg.transform, tf_odom); - - last_odom_ = odom_; - odom_ = tf_odom; - - initialized_odom_ = true; -} - - -std::optional -get_latest_imu_quat(const NavState & nav_state) -{ - if (!nav_state.has("imu")) { - return std::nullopt; - } - // Obtiene la colección de percepciones IMU - const auto imus = nav_state.get("imu"); - if (imus.empty() || !imus.back()) { - return std::nullopt; - } - - // Selecciono la última (puedes ordenar por stamp si lo prefieres) - const auto & imu_msg = imus.back()->data; - - tf2::Quaternion q; - q.setX(imu_msg.orientation.x); - q.setY(imu_msg.orientation.y); - q.setZ(imu_msg.orientation.z); - q.setW(imu_msg.orientation.w); - - // Normaliza por robustez numérica - if (q.length2() < 1e-12) { - return std::nullopt; - } - q.normalize(); - return q; -} - -static inline tf2::Vector3 project_onto_plane(const tf2::Vector3 & v, const tf2::Vector3 & n_unit) -{ - return v - n_unit * v.dot(n_unit); -} - -static inline tf2::Quaternion frame_from_normal_and_yaw( - const tf2::Vector3 & n_world_unit, double yaw_world) -{ - tf2::Vector3 x_tgt(std::cos(yaw_world), std::sin(yaw_world), 0.0); - tf2::Vector3 x_tan = project_onto_plane(x_tgt, n_world_unit); - - double xn = x_tan.length(); - if (xn < 1e-9) { - // fallback estable - x_tan = project_onto_plane(tf2::Vector3(1.0, 0.0, 0.0), n_world_unit); - xn = x_tan.length(); - if (xn < 1e-9) { - tf2::Quaternion q; q.setRPY(0, 0, yaw_world); - return q; - } - } - x_tan /= xn; - - tf2::Vector3 z_axis = n_world_unit; - tf2::Vector3 y_axis = z_axis.cross(x_tan); - double yn = y_axis.length(); - if (yn < 1e-9) { - tf2::Quaternion q; q.setRPY(0, 0, yaw_world); - return q; - } - y_axis /= yn; - - tf2::Matrix3x3 R( - x_tan.x(), y_axis.x(), z_axis.x(), - x_tan.y(), y_axis.y(), z_axis.y(), - x_tan.z(), y_axis.z(), z_axis.z()); - tf2::Quaternion q; R.getRotation(q); - return q; -} - -static inline tf2::Vector3 to_tf(const Eigen::Vector3f & v) -{ - return {static_cast(v.x()), static_cast(v.y()), static_cast(v.z())}; -} - -void -AMCLLocalizer::predict(NavState & nav_state) -{ - if (!initialized_odom_) { - if (compute_odom_from_tf_) { - update_odom_from_tf(); - } - return; - } - if (compute_odom_from_tf_) { - update_odom_from_tf(); - } - - tf2::Transform delta = last_odom_.inverseTimes(odom_); - - const bool have_navmap = nav_state.has("map.navmap"); - if (have_navmap) { - navmap_ = nav_state.get<::navmap::NavMap>("map.navmap"); - } - - // IMU disponible - const auto imu_q_opt = get_latest_imu_quat(nav_state); - - // Escalado de ruido - tf2::Vector3 t = delta.getOrigin(); - double dx = t.x(), dy = t.y(), dz = t.z(); - double trans_len = std::sqrt(dx * dx + dy * dy + dz * dz); - - double roll, pitch, yaw; - tf2::Matrix3x3(delta.getRotation()).getRPY(roll, pitch, yaw); - double rot_len = std::abs(yaw); - - std::random_device rd; - std::mt19937 gen(rd()); - - for (auto & p : particles_) { - // 1) Odometría con ruido (igual que ya hacías) - std::normal_distribution noise_dx(0.0, std::abs(dx) * noise_translation_); - std::normal_distribution noise_dy(0.0, std::abs(dy) * noise_translation_); - std::normal_distribution noise_dz(0.0, std::abs(dz) * noise_translation_); - std::normal_distribution noise_yaw( - 0.0, rot_len * noise_rotation_ + trans_len * noise_translation_to_rotation_); - - tf2::Vector3 noisy_translation( - dx + noise_dx(gen), - dy + noise_dy(gen), - dz + noise_dz(gen)); - - double noisy_yaw = yaw + noise_yaw(gen); - tf2::Quaternion noisy_q; noisy_q.setRPY(0.0, 0.0, noisy_yaw); - tf2::Transform noisy_delta(noisy_q, noisy_translation); - - p.pose = p.pose * noisy_delta; - - // 2) Ajuste con NavMap: Z desde superficie y orientación según IMU o normal - if (have_navmap) { - // Localiza navcel cercano usando pista - ::navmap::NavMap::LocateOpts opts; - if (p.last_cid != std::numeric_limits::max()) { - opts.hint_cid = p.last_cid; - opts.hint_surface = p.last_surface; - } - opts.planar_eps = 1e-4f; - opts.height_eps = 0.50f; - opts.use_downward_ray = true; - - tf2::Vector3 Pw = p.pose.getOrigin(); - - std::size_t sidx = 0; - ::navmap::NavCelId cid = std::numeric_limits::max(); - Eigen::Vector3f bary, hit_eig; - - const bool ok = navmap_.locate_navcel( - Eigen::Vector3f(static_cast(Pw.x()), - static_cast(Pw.y()), - static_cast(Pw.z() + 0.5)), - sidx, cid, bary, &hit_eig, opts); - - if (ok) { - // Z desde superficie - tf2::Vector3 hit = to_tf(hit_eig); - p.pose.setOrigin(tf2::Vector3(Pw.x(), Pw.y(), hit.z())); - - if (imu_q_opt.has_value()) { - // 2.a) Orientación desde IMU - p.pose.setRotation(*imu_q_opt); - } else { - // 2.b) Sin IMU: alinear con normal y conservar yaw odométrico - const auto & cel = navmap_.navcels[cid]; - tf2::Vector3 n_world = to_tf(cel.normal); - double nlen = n_world.length(); - if (nlen > 1e-9) { - n_world /= nlen; - double r, pp, y; - tf2::Matrix3x3(p.pose.getRotation()).getRPY(r, pp, y); - tf2::Quaternion q_align = frame_from_normal_and_yaw(n_world, y); - p.pose.setRotation(q_align); - } - } - - // Guarda pista para walking - p.last_cid = cid; - p.last_surface = sidx; - } else { - p.pose.setOrigin(tf2::Vector3(Pw.x(), Pw.y(), Pw.z())); - if (imu_q_opt.has_value()) { - p.pose.setRotation(*imu_q_opt); - } - } - } - } - - last_odom_ = odom_; - - // Publicación como ya tienes - pose_ = getEstimatedPose(); - tf2::Transform map2bf = pose_; - tf2::Transform map2odom = map2bf * odom_.inverse(); - publishTF(map2odom); - publishEstimatedPose(map2bf); -} - - -static inline double sqr(double x) {return x * x;} - -using ProgressCallback = std::function; // [0,1] - - -#include -#include -#include -#include - -// ===================== INFLATE MAP (brushfire) ===================== -std::shared_ptr -inflate_map( - const std::shared_ptr & src, - double sigma, - double p_min, - ProgressCallback progress /*= nullptr*/) -{ - using namespace Bonxai; - - if (!src || sigma <= 0.0) { - return nullptr; - } - - // 1) Clonar configuración y obtener tamaño de vóxel - const auto & src_grid = src->grid(); - - const double res = src_grid.voxelSize(); - - auto dst = std::make_shared(res); - dst->setOptions(src->options()); - - // 2) Semillas: voxeles ocupados del origen - std::vector seeds; - src->getOccupiedVoxels(seeds); - if (seeds.empty()) { - return dst; // nada que inflar - } - - // 3) Parámetros de inflado - const int R = static_cast(std::ceil(3.0 * sigma / res)); - if (R <= 0) { - auto acc = dst->grid().createAccessor(); - for (const auto & c : seeds) { - auto * cell = acc.value(c, /*create=*/true); - cell->probability_log = dst->options().clamp_max_log; - cell->update_id = 0; - } - return dst; - } - - const double inv_2sig2 = 1.0 / (2.0 * sigma * sigma); - std::vector lut_logod(R + 1); - for (int d = 0; d <= R; ++d) { - const double dist_m = d * res; - const double p = std::exp(-(dist_m * dist_m) * inv_2sig2); - lut_logod[d] = (p >= p_min) ? - ProbabilisticMap::logods(static_cast(p)) : - std::numeric_limits::min(); - } - - // 4) Brushfire por buckets - std::vector> buckets(R + 1); - buckets[0] = seeds; - - struct KeyHash - { - size_t operator()(const CoordT & c) const noexcept - { - return (static_cast(c.x) * 73856093u) ^ - (static_cast(c.y) * 19349663u) ^ - (static_cast(c.z) * 83492791u); - } - }; - struct KeyEq - { - bool operator()(const CoordT & a, const CoordT & b) const noexcept - { - return a.x == b.x && a.y == b.y && a.z == b.z; - } - }; - std::unordered_set visited; - visited.reserve(seeds.size() * 4); - for (const auto & c : seeds) { - visited.insert(c); - } - - auto acc = dst->grid().createAccessor(); - const auto clamp_min = dst->options().clamp_min_log; - const auto clamp_max = dst->options().clamp_max_log; - - auto write_max = [&](const CoordT & c, int32_t v) { - if (v == std::numeric_limits::min()) {return;} - auto * cell = acc.value(c, /*create=*/true); - int32_t proposed = std::max(cell->probability_log, v); - if (proposed < clamp_min) {proposed = clamp_min;} - if (proposed > clamp_max) {proposed = clamp_max;} - cell->probability_log = proposed; - cell->update_id = 0; - }; - - write_max(CoordT{0, 0, 0}, std::numeric_limits::min()); // no-op - for (const auto & c : seeds) { - write_max(c, lut_logod[0]); - } - - static const CoordT N6[6] = { - {1, 0, 0}, {-1, 0, 0}, - {0, 1, 0}, {0, -1, 0}, - {0, 0, 1}, {0, 0, -1} - }; - - std::cout << "Inflating probabilistic map (brushfire): seeds=" << seeds.size() - << " sigma=" << sigma << "m, R=" << R << ", res=" << res << "m\n"; - - size_t expanded = 0; - for (int d = 0; d < R; ++d) { - auto & bucket = buckets[d]; - - if (progress) { - float frac = static_cast(d) / static_cast(R); - progress(frac); - } - - if (R >= 10 && (d % std::max(1, R / 10) == 0)) { - double pct = 100.0 * static_cast(d) / static_cast(R); - std::cout << " d=" << d << "/" << R << " (" << pct << "%)\n"; - } - - const int nd = d + 1; - for (const CoordT & c0 : bucket) { - for (const CoordT & dn : N6) { - CoordT c{c0.x + dn.x, c0.y + dn.y, c0.z + dn.z}; - if (visited.find(c) != visited.end()) {continue;} - visited.insert(c); - buckets[nd].push_back(c); - write_max(c, lut_logod[nd]); - } - ++expanded; - } - } - - if (progress) {progress(1.0f);} - - std::cout << "Inflation complete. Expanded " << expanded - << " nodes within R=" << R << ".\n"; - - return dst; -} - -// ===================== RAYCAST DDA (sin alloc) ===================== -namespace -{ - -struct CoordHash -{ - size_t operator()(const Bonxai::CoordT & c) const noexcept - { - return (size_t)c.x * 73856093u ^ (size_t)c.y * 19349663u ^ (size_t)c.z * 83492791u; - } -}; -struct CoordEq -{ - bool operator()(const Bonxai::CoordT & a, const Bonxai::CoordT & b) const noexcept - { - return a.x == b.x && a.y == b.y && a.z == b.z; - } -}; - -// Devuelve true si hay ocupado ANTES del final (dejando tail voxels sin comprobar) -inline bool ray_occluded_dda( - const Bonxai::ProbabilisticMap & occ_map, - const Bonxai::VoxelGrid::ConstAccessor & acc, - const Bonxai::CoordT & a, const Bonxai::CoordT & b, - int tail_skip_voxels = 1) -{ - using Bonxai::CoordT; - using CellT = Bonxai::ProbabilisticMap::CellT; - - int x = a.x, y = a.y, z = a.z; - const int xe = b.x, ye = b.y, ze = b.z; - - const int dx = std::abs(xe - x); - const int dy = std::abs(ye - y); - const int dz = std::abs(ze - z); - - const int sx = (xe >= x) ? 1 : -1; - const int sy = (ye >= y) ? 1 : -1; - const int sz = (ze >= z) ? 1 : -1; - - int nsteps = std::max({dx, dy, dz}); - if (nsteps <= 0) {return false;} - - // Errores acumulados (enteros); inicializados a la mitad del máximo para simetría - int ex = (dx >= dy && dx >= dz) ? dx / 2 : (dy >= dz ? dy / 2 : dz / 2); - int ey = ex, ez = ex; - - for (int step = 0; step < nsteps; ++step) { - // deja sin comprobar los últimos 'tail_skip_voxels' y el final - if (step < nsteps - std::max(0, tail_skip_voxels) - 1) { - const CellT * cell = acc.value(CoordT{x, y, z}); - if (cell && cell->probability_log > occ_map.options().occupancy_threshold_log) { - return true; - } - } - - if (dx >= dy && dx >= dz) { - x += sx; - ey += dy; ez += dz; - if (ey >= dx) {y += sy; ey -= dx;} - if (ez >= dx) {z += sz; ez -= dx;} - } else if (dy >= dx && dy >= dz) { - y += sy; - ex += dx; ez += dz; - if (ex >= dy) {x += sx; ex -= dy;} - if (ez >= dy) {z += sz; ez -= dy;} - } else { - z += sz; - ex += dx; ey += dy; - if (ex >= dz) {x += sx; ex -= dz;} - if (ey >= dz) {y += sy; ey -= dz;} - } - } - - return false; -} - -} // namespace - -void -AMCLLocalizer::correct(NavState & nav_state) -{ - auto t0 = get_node()->now(); - if (!nav_state.has("points")) { - RCLCPP_WARN(get_node()->get_logger(), "There is yet no points perceptions"); - return; - } - - const auto perceptions = nav_state.get("points"); - - if (!nav_state.has("map.bonxai")) { - RCLCPP_WARN(get_node()->get_logger(), "There is yet no a bonxai map"); - return; - } - - - double stdev_inflation = 1.5; // metros; expón como parámetro si quieres - double p_min = 0.1; // metros; expón como parámetro si quieres - - if (!nav_state.has("map.bonxai.inflated")) { - auto start = get_node()->now(); - RCLCPP_INFO(get_node()->get_logger(), "Creating inflation map"); - - const auto original_map = nav_state.get_ptr("map.bonxai"); - - - auto logger = get_node()->get_logger(); - auto clock = get_node()->get_clock(); // SharedPtr - - auto progress_cb = [logger, clock](float f) { - RCLCPP_INFO_THROTTLE( - logger, *clock, 2000, - "Inflating... %.1f%%", static_cast(f) * 100.0); - }; - - std::shared_ptr inflated_map = - inflate_map(original_map, stdev_inflation, p_min, progress_cb); - - if (inflated_map == nullptr) { - RCLCPP_ERROR(get_node()->get_logger(), "Error creating bonxai inflated map"); - return; - } - - nav_state.set("map.bonxai.inflated", inflated_map); - - auto end = get_node()->now(); - RCLCPP_INFO(get_node()->get_logger(), "Created in %lf seconds", (end - start).seconds()); - } - - const auto original_map = nav_state.get_ptr("map.bonxai"); - const auto map = nav_state.get_ptr("map.bonxai.inflated"); - - auto t1 = get_node()->now(); - - // === Downsample adaptado al tamaño de vóxel del mapa inflado + límite duro de puntos - const double vox = map->grid().voxelSize(); - const double ds_vox = std::max(0.5, 2.0 * vox); // 2x voxel ~ agresivo y barato - auto view = PointPerceptionsOpsView(perceptions) - .downsample(ds_vox) - .fuse(get_tf_prefix() + "base_footprint"); - auto filtered = view->as_points(); - - static constexpr size_t MAX_PTS = 1500; - if (filtered.size() > MAX_PTS) { - pcl::PointCloud slim; - // Copiamos metadatos razonables - slim.header = filtered.header; - slim.is_dense = filtered.is_dense; - slim.points.reserve(MAX_PTS); - - const size_t stride = std::max(1, filtered.size() / MAX_PTS); - for (size_t i = 0; i < filtered.size() && slim.points.size() < MAX_PTS; i += stride) { - slim.points.push_back(filtered.points[i]); - } - - slim.width = static_cast(slim.points.size()); - slim.height = 1; - - filtered.swap(slim); // ahora sí: ambos son pcl::PointCloud - } - - auto t2 = get_node()->now(); - - if (filtered.empty()) { - RCLCPP_WARN(get_node()->get_logger(), "No points to correct"); - return; - } - - auto t3 = get_node()->now(); - - // === Parámetros de gating y tolerancia - constexpr float P_RAY_MIN = 0.2f; // Sólo raytrazar si el endpoint parece consistente - - - const double res_vox = original_map->grid().voxelSize(); - const double d_allow = p_min * std::sqrt(-2.0 * std::log(stdev_inflation)); - const int TAIL_SKIP = std::max(1, static_cast(std::ceil(d_allow / res_vox))); - - for (auto & particle : particles_) { - float hits = 0.0f; - float possible_hits = 0.0f; - - // Accessors por partícula (lectura): baratos y evitan recreate por consulta - auto acc_occ = original_map->grid().createConstAccessor(); - auto acc_inf = map->grid().createConstAccessor(); - - // Origen del rayo (una vez por partícula) - const tf2::Vector3 origin_w = particle.pose.getOrigin(); - const Bonxai::CoordT key_origin = - original_map->grid().posToCoord({origin_w.x(), origin_w.y(), origin_w.z()}); - - // Agrupa endpoints por vóxel final para no repetir trabajo - std::unordered_map end_counts; - end_counts.reserve(filtered.size()); - - for (const auto & pt : filtered) { - const tf2::Vector3 pw = particle.pose * tf2::Vector3(pt.x, pt.y, pt.z); - if (!std::isfinite(pw.x())) {continue;} - const Bonxai::CoordT key_end = - original_map->grid().posToCoord({pw.x(), pw.y(), pw.z()}); - ++end_counts[key_end]; - } - - for (const auto & kv : end_counts) { - const Bonxai::CoordT & key_end = kv.first; - const int mult = kv.second; - - possible_hits += static_cast(mult); - - // Probabilidad del endpoint (rápido, sin crear accessors) - float p_end = 0.0f; - if (const auto * cell_end = acc_inf.value(key_end)) { - // Si tienes un helper estático ProbabilisticMap::prob(int32_t), úsalo: - p_end = Bonxai::ProbabilisticMap::prob(cell_end->probability_log); - // Si no existiera, descomenta: - // p_end = map->queryProbability(key_end); - } - - if (p_end < P_RAY_MIN) { - hits += p_end * mult; // contribución baja -> no raytrazamos - continue; - } - - // Pinta bien -> confirmar ausencia de oclusión en mapa original - const bool occluded = ray_occluded_dda(*original_map, acc_occ, key_origin, key_end, - TAIL_SKIP); - hits += (occluded ? 0.0f : p_end) * mult; - } - - particle.hits += hits; - particle.possible_hits += possible_hits; - } - - auto t4 = get_node()->now(); - - // === Pesos (potencia tau) + normalización - const double tau = 0.7; // < 1 afila la distribución (más contraste) - - for (auto & particle : particles_) { - if (particle.possible_hits > 0) { - double mean_p = static_cast(particle.hits) / - static_cast(particle.possible_hits); - mean_p = std::clamp(mean_p, 1e-6, 1.0); // evitar 0 exacto - particle.weight = std::pow(mean_p, tau); - } else { - particle.weight = 1e-6; - } - } - - double total_weight = 0.0; - for (const auto & p : particles_) { - total_weight += p.weight; - } - if (total_weight > 0.0) { - for (auto & p : particles_) { - p.weight /= total_weight; - } - } - - auto t5 = get_node()->now(); - - std::cerr << "Prepare maps: " << std::fixed << std::setprecision(8) << (t1 - t0).seconds() << - std::endl; - std::cerr << "Prepare perception: " << std::fixed << std::setprecision(8) << - (t2 - t1).seconds() << std::endl; - std::cerr << "Core: " << std::fixed << std::setprecision(8) << (t4 - t3).seconds() << std::endl; - std::cerr << "Final: " << std::fixed << std::setprecision(8) << (t5 - t4).seconds() << std::endl; -} - -void -AMCLLocalizer::reseed() -{ - // 1) Comprobaciones tempranas - if (particles_.empty()) {return;} - if (particles_[0].possible_hits == 0) {return;} - - const std::size_t N = particles_.size(); - const std::size_t N_top = std::max(1, N / 2); - - // 2) Ordenar por peso (desc) - std::sort(particles_.begin(), particles_.end(), - [](const Particle & a, const Particle & b) { - return a.weight > b.weight; - }); - - std::cerr << - "=================================================================================\n"; - for (std::size_t i = 0; i < particles_.size(); i++) { - std::cerr << "[" << i << "] " << particles_[i].hits << "/" << particles_[i].possible_hits - << " " << static_cast(particles_[i].last_cid) << std::endl; - } - - // 3) Estadísticos top-N - tf2::Vector3 mean = computeMean(particles_, 0, N_top); - tf2::Matrix3x3 cov = computeCovariance(particles_, 0, N_top, mean); - - double a = cov[0][0]; - double b = cov[0][1]; - double c = cov[1][1]; - - auto safe_cov_2x2 = [](double a_in, double b_in, double c_in) { - const double eps = 1e-9; - double a2 = std::isfinite(a_in) ? a_in : 0.0; - double b2 = std::isfinite(b_in) ? b_in : 0.0; - double c2 = std::isfinite(c_in) ? c_in : 0.0; - - if (a2 < eps) {a2 = eps;} - if (c2 < eps) {c2 = eps;} - - double det = a2 * c2 - b2 * b2; - if (det < 0.0) { - double max_b = std::sqrt(a2 * c2) - eps; - if (max_b < 0.0) {max_b = 0.0;} - if (b2 > max_b) {b2 = max_b;} - if (b2 < -max_b) {b2 = -max_b;} - } - return std::tuple(a2, b2, c2); - }; - - auto [a2, b2, c2] = safe_cov_2x2(a, b, c); - - double l00 = std::sqrt(std::max(a2, 0.0)); - double l10 = (l00 > 0.0) ? (b2 / l00) : 0.0; - double t = c2 - l10 * l10; - if (t < 0.0) {t = 0.0;} - double l11 = std::sqrt(t); - - double yaw_variance = computeYawVariance(particles_, 0, N_top); - if (!std::isfinite(yaw_variance) || yaw_variance < 0.0) {yaw_variance = 0.0;} - double yaw_stddev = std::sqrt(yaw_variance); - if (!std::isfinite(yaw_stddev) || yaw_stddev < min_noise_yaw_) {yaw_stddev = min_noise_yaw_;} - - std::normal_distribution yaw_noise(0.0, yaw_stddev); - - // Distribuciones auxiliares - std::normal_distribution n01(0.0, 1.0); - std::normal_distribution index_dist( - 0.0, std::max(1.0, 0.05 * static_cast(N))); - - // Muestreador de offset XY - auto sample_xy_offset = [&]() -> std::pair { - double z0 = n01(rng_), z1 = n01(rng_); - double dx = l00 * z0; - double dy = l10 * z0 + l11 * z1; - - if (!std::isfinite(dx) || !std::isfinite(dy)) { - std::normal_distribution nIso(0.0, min_noise_xy_); - return {nIso(rng_), nIso(rng_)}; - } - - double r = std::hypot(dx, dy); - if (!(r >= min_noise_xy_)) { - if (r < 1e-12) { - std::normal_distribution nIso(0.0, min_noise_xy_); - dx = nIso(rng_); dy = nIso(rng_); - } else { - double s = (min_noise_xy_ / r); - dx *= s; dy *= s; - } - } - return {dx, dy}; - }; - - // 4) Reseed de la mitad inferior - for (std::size_t i = N_top; i < N; ++i) { - int selected_idx; - int guard = 0; - do { - double idx_sample = std::round(index_dist(rng_)); - selected_idx = static_cast(idx_sample); - if (selected_idx < 0) {selected_idx = -selected_idx;} - if (selected_idx >= static_cast(N_top)) {selected_idx = static_cast(N_top) - 1;} - if (selected_idx < 0) {selected_idx = 0;} - if (++guard > 16) {selected_idx = 0; break;} - } while (selected_idx < 0 || static_cast(selected_idx) >= N_top); - - const auto & ref_particle = particles_[static_cast(selected_idx)]; - tf2::Vector3 origin = ref_particle.pose.getOrigin(); - - auto [dx_off, dy_off] = sample_xy_offset(); - - double r_ref, p_ref, y_ref; - tf2::Matrix3x3(ref_particle.pose.getRotation()).getRPY(r_ref, p_ref, y_ref); - - double noisy_yaw = y_ref + yaw_noise(rng_); - if (!std::isfinite(noisy_yaw)) {noisy_yaw = y_ref;} - - tf2::Quaternion q; - q.setRPY(0.0, 0.0, noisy_yaw); - if (q.length2() < 1e-20 || !std::isfinite(q.x()) || !std::isfinite(q.y()) || - !std::isfinite(q.z()) || !std::isfinite(q.w())) - { - q.setRPY(0.0, 0.0, y_ref); - } - - tf2::Vector3 new_origin(origin.x() + dx_off, origin.y() + dy_off, origin.z()); - if (!std::isfinite(new_origin.x()) || !std::isfinite(new_origin.y()) || - !std::isfinite(new_origin.z())) - { - new_origin = tf2::Vector3(origin.x(), origin.y(), origin.z()); - } - - particles_[i].pose.setOrigin(new_origin); - particles_[i].pose.setRotation(q); - particles_[i].weight = ref_particle.weight; - } - - // 5) Reset contadores + normalización de pesos - for (auto & particle : particles_) { - particle.hits = 0; - particle.possible_hits = 0; - } - - double total_weight = 0.0; - for (const auto & p : particles_) { - if (std::isfinite(p.weight)) {total_weight += p.weight;} - } - - if (total_weight > 0.0 && std::isfinite(total_weight)) { - for (auto & p : particles_) { - p.weight /= total_weight; - if (!std::isfinite(p.weight) || p.weight < 0.0) {p.weight = 0.0;} - } - } else { - double w = 1.0 / static_cast(particles_.size()); - for (auto & p : particles_) { - p.weight = w; - } - } -} - - -static inline bool is_finite_tf(const tf2::Transform & T) -{ - const auto & o = T.getOrigin(); - const auto & q = T.getRotation(); - return std::isfinite(o.x()) && std::isfinite(o.y()) && std::isfinite(o.z()) && - std::isfinite(q.x()) && std::isfinite(q.y()) && std::isfinite(q.z()) && - std::isfinite(q.w()); -} - -void -AMCLLocalizer::publishTF(const tf2::Transform & map2bf) -{ - if (!is_finite_tf(map2bf)) { - RCLCPP_ERROR(get_node()->get_logger(), "publishTF: transform contiene NaN/inf; no se publica"); - return; - } - - geometry_msgs::msg::TransformStamped tf_msg; - tf_msg.header.stamp = get_node()->now(); - tf_msg.header.frame_id = get_tf_prefix() + "map"; - tf_msg.child_frame_id = get_tf_prefix() + "odom"; - tf_msg.transform = tf2::toMsg(map2bf); - - RTTFBuffer::getInstance()->setTransform(tf_msg, "easynav", false); - tf_broadcaster_->sendTransform(tf_msg); -} - -void -AMCLLocalizer::publishParticles() -{ - geometry_msgs::msg::PoseArray array_msg; - array_msg.header.stamp = get_node()->now(); - array_msg.header.frame_id = get_tf_prefix() + "map"; - - array_msg.poses.reserve(particles_.size()); - for (const auto & p : particles_) { - geometry_msgs::msg::Pose pose_msg; - pose_msg.position.x = p.pose.getOrigin().x(); - pose_msg.position.y = p.pose.getOrigin().y(); - pose_msg.position.z = p.pose.getOrigin().z(); - pose_msg.orientation.x = p.pose.getRotation().x(); - pose_msg.orientation.y = p.pose.getRotation().y(); - pose_msg.orientation.z = p.pose.getRotation().z(); - pose_msg.orientation.w = p.pose.getRotation().w(); - array_msg.poses.push_back(pose_msg); - } - - particles_pub_->publish(array_msg); -} - -tf2::Transform -AMCLLocalizer::getEstimatedPose() const -{ - if (particles_.empty()) { - return tf2::Transform::getIdentity(); - } - - const std::size_t N = particles_.size(); - const std::size_t N_top = N / 2; - - std::vector sorted_particles = particles_; - std::sort(sorted_particles.begin(), sorted_particles.end(), - [](const Particle & a, const Particle & b) { - return a.weight > b.weight; - }); - - tf2::Vector3 mean = computeMean(sorted_particles, 0, N_top); - if (!std::isfinite(mean.x()) || !std::isfinite(mean.y()) || !std::isfinite(mean.z())) { - mean = tf2::Vector3(0.0, 0.0, 0.0); - } - double roll, pitch, yaw; - tf2::Matrix3x3(sorted_particles[0].pose.getRotation()).getRPY(roll, pitch, yaw); - tf2::Quaternion q; - q.setRPY(0.0, 0.0, yaw); - - tf2::Transform est; - est.setOrigin(mean); - est.setRotation(q); - return est; -} - -void -AMCLLocalizer::publishEstimatedPose(const tf2::Transform & est_pose) -{ - if (particles_.empty()) {return;} - - const std::size_t N = particles_.size(); - const std::size_t N_top = N / 2; - - tf2::Vector3 mean = est_pose.getOrigin(); - tf2::Matrix3x3 cov = computeCovariance(particles_, 0, N_top, mean); - double yaw_variance = computeYawVariance(particles_, 0, N_top); - - geometry_msgs::msg::PoseWithCovarianceStamped msg; - msg.header.stamp = get_node()->now(); - msg.header.frame_id = get_tf_prefix() + "map"; - - msg.pose.pose.position.x = mean.x(); - msg.pose.pose.position.y = mean.y(); - msg.pose.pose.position.z = mean.z(); - msg.pose.pose.orientation = tf2::toMsg(est_pose.getRotation()); - - for (int r = 0; r < 3; ++r) { - for (int c = 0; c < 3; ++c) { - msg.pose.covariance[6 * r + c] = cov[r][c]; - } - } - msg.pose.covariance[6 * 5 + 5] = yaw_variance; - - estimate_pub_->publish(msg); -} - -nav_msgs::msg::Odometry -AMCLLocalizer::get_pose() -{ - nav_msgs::msg::Odometry odom_msg; - - odom_msg.header.stamp = get_node()->now(); - odom_msg.header.frame_id = get_tf_prefix() + "map"; - odom_msg.child_frame_id = get_tf_prefix() + "base_footprint"; - - pose_ = getEstimatedPose(); - tf2::Transform est_pose = pose_; - - odom_msg.pose.pose.position.x = est_pose.getOrigin().x(); - odom_msg.pose.pose.position.y = est_pose.getOrigin().y(); - odom_msg.pose.pose.position.z = est_pose.getOrigin().z(); - odom_msg.pose.pose.orientation = tf2::toMsg(est_pose.getRotation()); - - if (!particles_.empty()) { - const std::size_t N = particles_.size(); - const std::size_t N_top = N / 2; - - std::vector sorted_particles = particles_; - std::sort(sorted_particles.begin(), sorted_particles.end(), - [](const Particle & a, const Particle & b) { - return a.weight > b.weight; - }); - - tf2::Vector3 mean = computeMean(sorted_particles, 0, N_top); - tf2::Matrix3x3 cov = computeCovariance(sorted_particles, 0, N_top, mean); - double yaw_var = computeYawVariance(sorted_particles, 0, N_top); - - for (int r = 0; r < 3; ++r) { - for (int c = 0; c < 3; ++c) { - odom_msg.pose.covariance[6 * r + c] = cov[r][c]; - } - } - - odom_msg.pose.covariance[6 * 5 + 5] = yaw_var; - } - - odom_msg.twist.twist.linear.x = 0.0; - odom_msg.twist.twist.linear.y = 0.0; - odom_msg.twist.twist.linear.z = 0.0; - odom_msg.twist.twist.angular.x = 0.0; - odom_msg.twist.twist.angular.y = 0.0; - odom_msg.twist.twist.angular.z = 0.0; - - return odom_msg; -} - -} // namespace navmap - -} // namespace easynav - -#include -PLUGINLIB_EXPORT_CLASS(easynav::navmap::AMCLLocalizer, easynav::LocalizerMethodBase) diff --git a/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer copy_v4_bad.cpp b/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer copy_v4_bad.cpp deleted file mode 100644 index 82065930..00000000 --- a/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer copy_v4_bad.cpp +++ /dev/null @@ -1,1180 +0,0 @@ -// Copyright 2025 Intelligent Robotics Lab -// -// This file is part of the project Easy Navigation (EasyNav in short) -// licensed under the GNU General Public License v3.0. -// See for details. -// -// Easy Navigation program is free software: you can redistribute it and/or modify -// it under the terms of the GNU General Public License as published by -// the Free Software Foundation, either version 3 of the License, or -// (at your option) any later version. -// -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. -// -// You should have received a copy of the GNU General Public License -// along with this program. If not, see . - -/// \file -/// \brief Implementation of the AMCLLocalizer with probabilistic inflation + raycast check. - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "bonxai/bonxai.hpp" -#include "bonxai/probabilistic_map.hpp" - -#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" -#include "tf2/LinearMath/Vector3.hpp" - -#include "easynav_common/RTTFBuffer.hpp" -#include "easynav_common/types/Perceptions.hpp" -#include "easynav_common/types/PointPerception.hpp" -#include "easynav_common/types/IMUPerception.hpp" - -#include "navmap_core/NavMap.hpp" - -#include "easynav_navmap_localizer/AMCLLocalizer.hpp" -#include "easynav_localizer/LocalizerNode.hpp" - -namespace easynav -{ -namespace navmap -{ - -static inline double sqr(double x) {return x * x;} - -tf2::Vector3 computeMean( - const std::vector & particles, - std::size_t start, std::size_t count) -{ - tf2::Vector3 weighted_sum(0.0, 0.0, 0.0); - double total_weight = 0.0; - - if (start >= particles.size() || count == 0) {return weighted_sum;} - std::size_t end = std::min(start + count, particles.size()); - - for (std::size_t i = start; i < end; ++i) { - const Particle & p = particles[i]; - const tf2::Vector3 & origin = p.pose.getOrigin(); - if (!std::isfinite(origin.x()) || !std::isfinite(origin.y()) || !std::isfinite(origin.z())) { - continue; - } - weighted_sum += origin * p.weight; - total_weight += p.weight; - } - return (total_weight > 0.0) ? (weighted_sum / total_weight) : tf2::Vector3(0.0, 0.0, 0.0); -} - -tf2::Matrix3x3 -computeCovariance( - const std::vector & particles, - std::size_t start, std::size_t count, - const tf2::Vector3 & mean) -{ - double total_weight = 0.0; - double cov[3][3] = {{0.0}}; - - if (start >= particles.size() || count == 0) {return tf2::Matrix3x3();} - std::size_t end = std::min(start + count, particles.size()); - - for (std::size_t i = start; i < end; ++i) { - const Particle & p = particles[i]; - tf2::Vector3 d = p.pose.getOrigin() - mean; - - double w = p.weight; - total_weight += w; - - cov[0][0] += w * d.x() * d.x(); - cov[0][1] += w * d.x() * d.y(); - cov[0][2] += w * d.x() * d.z(); - cov[1][1] += w * d.y() * d.y(); - cov[1][2] += w * d.y() * d.z(); - cov[2][2] += w * d.z() * d.z(); - } - - if (total_weight > 0.0) { - cov[0][0] /= total_weight; - cov[0][1] /= total_weight; - cov[0][2] /= total_weight; - cov[1][1] /= total_weight; - cov[1][2] /= total_weight; - cov[2][2] /= total_weight; - cov[1][0] = cov[0][1]; - cov[2][0] = cov[0][2]; - cov[2][1] = cov[1][2]; - } - - return tf2::Matrix3x3( - cov[0][0], cov[0][1], cov[0][2], - cov[1][0], cov[1][1], cov[1][2], - cov[2][0], cov[2][1], cov[2][2]); -} - -double extractYaw(const tf2::Transform & transform) -{ - double r, p, y; - tf2::Matrix3x3(transform.getRotation()).getRPY(r, p, y); - return y; -} - -double computeYawVariance( - const std::vector & particles, - std::size_t start, std::size_t count) -{ - double sum_cos = 0.0, sum_sin = 0.0, total_weight = 0.0; - if (start >= particles.size() || count == 0) {return 0.0;} - - std::size_t end = std::min(start + count, particles.size()); - for (std::size_t i = start; i < end; ++i) { - const Particle & p = particles[i]; - double y = extractYaw(p.pose); - double w = p.weight; - sum_cos += w * std::cos(y); - sum_sin += w * std::sin(y); - total_weight += w; - } - - if (total_weight == 0.0) {return 0.0;} - double mc = sum_cos / total_weight; - double ms = sum_sin / total_weight; - double R = std::sqrt(mc * mc + ms * ms); - R = std::clamp((std::isfinite(R) ? R : 0.0), 1e-12, 1.0); - double var = -2.0 * std::log(R); - return (std::isfinite(var) && var >= 0.0) ? var : 0.0; -} - -using std::placeholders::_1; -using namespace std::chrono_literals; - -// ------------------- helpers ------------------- -static inline tf2::Vector3 project_onto_plane(const tf2::Vector3 & v, const tf2::Vector3 & n_unit) -{ - return v - n_unit * v.dot(n_unit); -} - -static inline tf2::Quaternion frame_from_normal_and_yaw( - const tf2::Vector3 & n_world_unit, double yaw_world) -{ - tf2::Vector3 x_tgt(std::cos(yaw_world), std::sin(yaw_world), 0.0); - tf2::Vector3 x_tan = project_onto_plane(x_tgt, n_world_unit); - double xn = x_tan.length(); - if (xn < 1e-9) { - tf2::Quaternion q; q.setRPY(0, 0, yaw_world); - return q; - } - x_tan /= xn; - - tf2::Vector3 z_axis = n_world_unit; - tf2::Vector3 y_axis = z_axis.cross(x_tan); - double yn = y_axis.length(); - if (yn < 1e-9) { - tf2::Quaternion q; q.setRPY(0, 0, yaw_world); - return q; - } - y_axis /= yn; - - tf2::Matrix3x3 R( - x_tan.x(), y_axis.x(), z_axis.x(), - x_tan.y(), y_axis.y(), z_axis.y(), - x_tan.z(), y_axis.z(), z_axis.z()); - tf2::Quaternion q; R.getRotation(q); - return q; -} - -static inline tf2::Vector3 to_tf(const Eigen::Vector3f & v) -{ - return {static_cast(v.x()), static_cast(v.y()), static_cast(v.z())}; -} - -static inline bool is_finite_tf(const tf2::Transform & T) -{ - const auto & o = T.getOrigin(); - const auto & q = T.getRotation(); - return std::isfinite(o.x()) && std::isfinite(o.y()) && std::isfinite(o.z()) && - std::isfinite(q.x()) && std::isfinite(q.y()) && std::isfinite(q.z()) && - std::isfinite(q.w()); -} - -// ------------------- ctor/dtor ------------------- -AMCLLocalizer::AMCLLocalizer() -{ - NavState::register_printer( - [](const nav_msgs::msg::Odometry & odom) { - std::ostringstream ret; - double x = odom.pose.pose.position.x; - double y = odom.pose.pose.position.y; - - tf2::Quaternion q( - odom.pose.pose.orientation.x, - odom.pose.pose.orientation.y, - odom.pose.pose.orientation.z, - odom.pose.pose.orientation.w); - - double roll, pitch, yaw; - tf2::Matrix3x3(q).getRPY(roll, pitch, yaw); - - ret << "Odometry with pose: (x: " << x << ", y: " << y << ", yaw: " << yaw << ")"; - return ret.str(); - }); -} - -AMCLLocalizer::~AMCLLocalizer() = default; - -// ------------------- on_initialize ------------------- -std::expected -AMCLLocalizer::on_initialize() -{ - auto node = get_node(); - const auto & plugin_name = get_plugin_name(); - - int num_particles; - double x_init, y_init, yaw_init, std_dev_xy, std_dev_yaw; - - node->declare_parameter(plugin_name + ".num_particles", 100); - node->declare_parameter(plugin_name + ".initial_pose.x", 0.0); - node->declare_parameter(plugin_name + ".initial_pose.y", 0.0); - node->declare_parameter(plugin_name + ".initial_pose.yaw", 0.0); - node->declare_parameter(plugin_name + ".initial_pose.std_dev_xy", 0.5); - node->declare_parameter(plugin_name + ".initial_pose.std_dev_yaw", 0.5); - node->declare_parameter(plugin_name + ".reseed_freq", 1.0); - node->declare_parameter(plugin_name + ".noise_translation", 0.01); - node->declare_parameter(plugin_name + ".noise_rotation", 0.01); - node->declare_parameter(plugin_name + ".noise_translation_to_rotation", 0.01); - node->declare_parameter(plugin_name + ".min_noise_xy", 0.05); - node->declare_parameter(plugin_name + ".min_noise_yaw", 0.05); - node->declare_parameter(plugin_name + ".compute_odom_from_tf", false); - node->declare_parameter(plugin_name + ".inflation_stddev", 0.05); - node->declare_parameter(plugin_name + ".inflation_prob_min", 0.01); - node->declare_parameter(plugin_name + ".correct_max_points", 1500); - node->declare_parameter(plugin_name + ".weights_tau", 0.7); - node->declare_parameter(plugin_name + ".top_keep_fraction", 0.2); - - node->get_parameter(plugin_name + ".num_particles", num_particles); - node->get_parameter(plugin_name + ".initial_pose.x", x_init); - node->get_parameter(plugin_name + ".initial_pose.y", y_init); - node->get_parameter(plugin_name + ".initial_pose.yaw", yaw_init); - node->get_parameter(plugin_name + ".initial_pose.std_dev_xy", std_dev_xy); - node->get_parameter(plugin_name + ".initial_pose.std_dev_yaw", std_dev_yaw); - node->get_parameter(plugin_name + ".noise_translation", noise_translation_); - node->get_parameter(plugin_name + ".noise_rotation", noise_rotation_); - node->get_parameter(plugin_name + ".noise_translation_to_rotation", - noise_translation_to_rotation_); - node->get_parameter(plugin_name + ".min_noise_xy", min_noise_xy_); - node->get_parameter(plugin_name + ".min_noise_yaw", min_noise_yaw_); - node->get_parameter(plugin_name + ".compute_odom_from_tf", compute_odom_from_tf_); - node->get_parameter(plugin_name + ".inflation_stddev", inflation_stddev_); - node->get_parameter(plugin_name + ".inflation_prob_min", inflation_prob_min_); - int tmp_max_pts = 1500; - node->get_parameter(plugin_name + ".correct_max_points", tmp_max_pts); - correct_max_points_ = (tmp_max_pts > - 0) ? static_cast(tmp_max_pts) : std::size_t{1500}; - node->get_parameter(plugin_name + ".weights_tau", weights_tau_); - node->get_parameter(plugin_name + ".top_keep_fraction", top_keep_fraction_); - - if (!std::isfinite(inflation_stddev_) || inflation_stddev_ <= 0.0) {inflation_stddev_ = 1.5;} - if (!std::isfinite(inflation_prob_min_) || inflation_prob_min_ <= 0.0) { - inflation_prob_min_ = 0.01; - } - if (!std::isfinite(weights_tau_) || weights_tau_ <= 0.0) {weights_tau_ = 0.7;} - if (!std::isfinite(top_keep_fraction_) || top_keep_fraction_ <= 0.0) {top_keep_fraction_ = 0.2;} - if (top_keep_fraction_ > 1.0) {top_keep_fraction_ = 1.0;} - - double reseed_freq; - node->get_parameter(plugin_name + ".reseed_freq", reseed_freq); - reseed_time_ = 1.0 / reseed_freq; - - RCLCPP_INFO(node->get_logger(), "Initialized AMCL with %d particles", num_particles); - RCLCPP_INFO(node->get_logger(), "init pose (%.3f, %.3f, %.3f) std_dev [%.3f, %.3f]", - x_init, y_init, yaw_init, std_dev_xy, std_dev_yaw); - RCLCPP_INFO(node->get_logger(), "inflation_stddev=%.3f, inflation_prob_min=%.3f, " - "correct_max_points=%zu, weights_tau=%.3f, top_keep_fraction=%.3f", - inflation_stddev_, inflation_prob_min_, correct_max_points_, weights_tau_, top_keep_fraction_); - - std::normal_distribution noise_x(x_init, std_dev_xy); - std::normal_distribution noise_y(y_init, std_dev_xy); - std::normal_distribution noise_yaw(yaw_init, std_dev_yaw); - - particles_.resize(num_particles); - for (auto & p : particles_) { - tf2::Quaternion q; q.setRPY(0.0, 0.0, noise_yaw(rng_)); - p.pose.setRotation(q); - p.pose.setOrigin(tf2::Vector3(noise_x(rng_), noise_y(rng_), 0.0)); - p.hits = 0; - p.possible_hits = 0; - p.weight = 1.0 / num_particles; - } - - tf_broadcaster_ = std::make_unique(get_node()); - - auto node_typed = std::dynamic_pointer_cast(get_node()); - auto rt_cbg = node_typed->get_real_time_cbg(); - rclcpp::SubscriptionOptions options; options.callback_group = rt_cbg; - - if (!compute_odom_from_tf_) { - odom_sub_ = get_node()->create_subscription( - "odom", rclcpp::SensorDataQoS().reliable(), - std::bind(&AMCLLocalizer::odom_callback, this, _1), options); - } - - const auto fq = node->get_fully_qualified_name() + std::string("/") + plugin_name; - particles_pub_ = get_node()->create_publisher(fq + "/particles", - 10); - estimate_pub_ = get_node()->create_publisher(fq + - "/pose", 10); - - last_reseed_ = get_node()->now(); - get_node()->get_logger().set_level(rclcpp::Logger::Level::Debug); - return {}; -} - -// ------------------- odom/tf ------------------- -void -AMCLLocalizer::odom_callback(nav_msgs::msg::Odometry::UniquePtr msg) -{ - if (compute_odom_from_tf_) {return;} - tf2::fromMsg(msg->pose.pose, odom_); - if (!initialized_odom_) {last_odom_ = odom_; initialized_odom_ = true;} -} - -void -AMCLLocalizer::update_odom_from_tf() -{ - geometry_msgs::msg::TransformStamped tf_msg; - try { - tf_msg = RTTFBuffer::getInstance()->lookupTransform( - "odom", "base_footprint", tf2::TimePointZero, tf2::durationFromSec(0.0)); - } catch (const tf2::TransformException & ex) { - RCLCPP_WARN(get_node()->get_logger(), "AMCLLocalizer::update: TF failed: %s", ex.what()); - return; - } - - tf2::Transform tf_odom; tf2::fromMsg(tf_msg.transform, tf_odom); - last_odom_ = odom_; - odom_ = tf_odom; - initialized_odom_ = true; -} - -// ------------------- imu ------------------- -std::optional -get_latest_imu_quat(const NavState & nav_state) -{ - if (!nav_state.has("imu")) {return std::nullopt;} - const auto imus = nav_state.get("imu"); - if (imus.empty() || !imus.back()) {return std::nullopt;} - - const auto & imu_msg = imus.back()->data; - tf2::Quaternion q; - q.setX(imu_msg.orientation.x); - q.setY(imu_msg.orientation.y); - q.setZ(imu_msg.orientation.z); - q.setW(imu_msg.orientation.w); - if (q.length2() < 1e-12) {return std::nullopt;} - q.normalize(); - return q; -} - -// ------------------- predict ------------------- -void -AMCLLocalizer::predict(NavState & nav_state) -{ - if (!initialized_odom_) { - if (compute_odom_from_tf_) {update_odom_from_tf();} - return; - } - if (compute_odom_from_tf_) {update_odom_from_tf();} - - tf2::Transform delta = last_odom_.inverseTimes(odom_); - - const bool have_navmap = nav_state.has("map.navmap"); - if (have_navmap) { - navmap_ = nav_state.get<::navmap::NavMap>("map.navmap"); - } - - const auto imu_q_opt = get_latest_imu_quat(nav_state); - - tf2::Vector3 t = delta.getOrigin(); - double dx = t.x(), dy = t.y(), dz = t.z(); - double trans_len = std::sqrt(dx * dx + dy * dy + dz * dz); - - double r, p, y; - tf2::Matrix3x3(delta.getRotation()).getRPY(r, p, y); - double rot_len = std::abs(y); - - std::random_device rd; std::mt19937 gen(rd()); - - for (auto & ptl : particles_) { - std::normal_distribution noise_dx(0.0, std::abs(dx) * noise_translation_); - std::normal_distribution noise_dy(0.0, std::abs(dy) * noise_translation_); - std::normal_distribution noise_dz(0.0, std::abs(dz) * noise_translation_); - std::normal_distribution noise_yaw( - 0.0, rot_len * noise_rotation_ + trans_len * noise_translation_to_rotation_); - - tf2::Vector3 noisy_translation(dx + noise_dx(gen), dy + noise_dy(gen), dz + noise_dz(gen)); - double noisy_yaw = y + noise_yaw(gen); - tf2::Quaternion noisy_q; noisy_q.setRPY(0.0, 0.0, noisy_yaw); - tf2::Transform noisy_delta(noisy_q, noisy_translation); - - ptl.pose = ptl.pose * noisy_delta; - - if (have_navmap) { - ::navmap::NavMap::LocateOpts opts; - if (ptl.last_cid != std::numeric_limits::max()) { - opts.hint_cid = ptl.last_cid; - opts.hint_surface = ptl.last_surface; - } - opts.planar_eps = 1e-4f; - opts.height_eps = 0.50f; - opts.use_downward_ray = true; - - tf2::Vector3 Pw = ptl.pose.getOrigin(); - - std::size_t sidx = 0; - ::navmap::NavCelId cid = std::numeric_limits::max(); - Eigen::Vector3f bary, hit_eig; - - const bool ok = navmap_.locate_navcel( - Eigen::Vector3f(static_cast(Pw.x()), - static_cast(Pw.y()), - static_cast(Pw.z() + 0.5f)), - sidx, cid, bary, &hit_eig, opts); - - if (ok) { - tf2::Vector3 hit = to_tf(hit_eig); - ptl.pose.setOrigin(tf2::Vector3(Pw.x(), Pw.y(), hit.z())); - - if (imu_q_opt.has_value()) { - ptl.pose.setRotation(*imu_q_opt); - } else { - const auto & cel = navmap_.navcels[cid]; - tf2::Vector3 n_world = to_tf(cel.normal); - double nlen = n_world.length(); - if (nlen > 1e-9) { - n_world /= nlen; - double rr, pp, yy; - tf2::Matrix3x3(ptl.pose.getRotation()).getRPY(rr, pp, yy); - tf2::Quaternion q_align = frame_from_normal_and_yaw(n_world, yy); - ptl.pose.setRotation(q_align); - } - } - ptl.last_cid = cid; - ptl.last_surface = sidx; - } else { - ptl.pose.setOrigin(tf2::Vector3(Pw.x(), Pw.y(), Pw.z())); - if (imu_q_opt.has_value()) { - ptl.pose.setRotation(*imu_q_opt); - } - } - } - } - - last_odom_ = odom_; - pose_ = getEstimatedPose(); - tf2::Transform map2bf = pose_; - tf2::Transform map2odom = map2bf * odom_.inverse(); - publishTF(map2odom); - publishEstimatedPose(map2bf); -} - -// ------------------- inflate map (brushfire) ------------------- -using ProgressCallback = std::function; - -std::shared_ptr -inflate_map( - const std::shared_ptr & src, - double sigma, - double p_min, - ProgressCallback progress = nullptr) -{ - using namespace Bonxai; - - if (!src || sigma <= 0.0) {return nullptr;} - - const auto & src_grid = src->grid(); - const double res = src_grid.voxelSize(); - - auto dst = std::make_shared(res); - dst->setOptions(src->options()); - - std::vector seeds; - src->getOccupiedVoxels(seeds); - if (seeds.empty()) {return dst;} - - const int R = static_cast(std::ceil(3.0 * sigma / res)); - if (R <= 0) { - auto acc = dst->grid().createAccessor(); - for (const auto & c : seeds) { - auto * cell = acc.value(c, /*create=*/true); - cell->probability_log = dst->options().clamp_max_log; - cell->update_id = 0; - } - return dst; - } - - const double inv_2sig2 = 1.0 / (2.0 * sigma * sigma); - std::vector lut_logod(R + 1); - for (int d = 0; d <= R; ++d) { - const double dist_m = d * res; - const double p = std::exp(-(dist_m * dist_m) * inv_2sig2); - lut_logod[d] = (p >= p_min) ? - ProbabilisticMap::logods(static_cast(p)) : - std::numeric_limits::min(); - } - - std::vector> buckets(R + 1); - buckets[0] = seeds; - - struct KeyHash - { - size_t operator()(const CoordT & c) const noexcept - { - return (size_t)c.x * 73856093u ^ (size_t)c.y * 19349663u ^ (size_t)c.z * 83492791u; - } - }; - struct KeyEq - { - bool operator()(const CoordT & a, const CoordT & b) const noexcept - { - return a.x == b.x && a.y == b.y && a.z == b.z; - } - }; - std::unordered_set visited; - visited.reserve(seeds.size() * 4); - for (const auto & c : seeds) { - visited.insert(c); - } - - auto acc = dst->grid().createAccessor(); - const auto clamp_min = dst->options().clamp_min_log; - const auto clamp_max = dst->options().clamp_max_log; - - auto write_max = [&](const CoordT & c, int32_t v) { - if (v == std::numeric_limits::min()) {return;} - auto * cell = acc.value(c, /*create=*/true); - int32_t proposed = std::max(cell->probability_log, v); - if (proposed < clamp_min) {proposed = clamp_min;} - if (proposed > clamp_max) {proposed = clamp_max;} - cell->probability_log = proposed; - cell->update_id = 0; - }; - - for (const auto & c : seeds) { - write_max(c, lut_logod[0]); - } - - static const CoordT N6[6] = { - {1, 0, 0}, {-1, 0, 0}, - {0, 1, 0}, {0, -1, 0}, - {0, 0, 1}, {0, 0, -1} - }; - - std::cout << "Inflating probabilistic map: seeds=" << seeds.size() - << " sigma=" << sigma << "m, R=" << R << ", res=" << res << "m\n"; - - size_t expanded = 0; - for (int d = 0; d < R; ++d) { - auto & bucket = buckets[d]; - - if (progress) {progress(static_cast(d) / static_cast(R));} - - if (R >= 10 && (d % std::max(1, R / 10) == 0)) { - double pct = 100.0 * static_cast(d) / static_cast(R); - std::cout << " d=" << d << "/" << R << " (" << pct << "%)\n"; - } - - const int nd = d + 1; - for (const CoordT & c0 : bucket) { - for (const CoordT & dn : N6) { - CoordT c{c0.x + dn.x, c0.y + dn.y, c0.z + dn.z}; - if (visited.find(c) != visited.end()) {continue;} - visited.insert(c); - buckets[nd].push_back(c); - write_max(c, lut_logod[nd]); - } - ++expanded; - } - } - - if (progress) {progress(1.0f);} - std::cout << "Inflation complete. Expanded " << expanded << " nodes within R=" << R << ".\n"; - return dst; -} - -// ------------------- raycast DDA ------------------- -namespace -{ -struct CoordHash -{ - size_t operator()(const Bonxai::CoordT & c) const noexcept - { - return (size_t)c.x * 73856093u ^ (size_t)c.y * 19349663u ^ (size_t)c.z * 83492791u; - } -}; -struct CoordEq -{ - bool operator()(const Bonxai::CoordT & a, const Bonxai::CoordT & b) const noexcept - { - return a.x == b.x && a.y == b.y && a.z == b.z; - } -}; - -inline bool ray_occluded_dda( - const Bonxai::ProbabilisticMap & occ_map, - const Bonxai::VoxelGrid::ConstAccessor & acc, - const Bonxai::CoordT & a, const Bonxai::CoordT & b, - int tail_skip_voxels = 1) -{ - using Bonxai::CoordT; - using CellT = Bonxai::ProbabilisticMap::CellT; - - int x = a.x, y = a.y, z = a.z; - const int xe = b.x, ye = b.y, ze = b.z; - - const int dx = std::abs(xe - x); - const int dy = std::abs(ye - y); - const int dz = std::abs(ze - z); - - const int sx = (xe >= x) ? 1 : -1; - const int sy = (ye >= y) ? 1 : -1; - const int sz = (ze >= z) ? 1 : -1; - - int nsteps = std::max({dx, dy, dz}); - if (nsteps <= 0) {return false;} - - int ex = (dx >= dy && dx >= dz) ? dx / 2 : (dy >= dz ? dy / 2 : dz / 2); - int ey = ex, ez = ex; - - for (int step = 0; step < nsteps; ++step) { - if (step < nsteps - std::max(0, tail_skip_voxels) - 1) { - const CellT * cell = acc.value(CoordT{x, y, z}); - if (cell && cell->probability_log > occ_map.options().occupancy_threshold_log) { - return true; - } - } - - if (dx >= dy && dx >= dz) { - x += sx; - ey += dy; ez += dz; - if (ey >= dx) {y += sy; ey -= dx;} - if (ez >= dx) {z += sz; ez -= dx;} - } else if (dy >= dx && dy >= dz) { - y += sy; - ex += dx; ez += dz; - if (ex >= dy) {x += sx; ex -= dy;} - if (ez >= dy) {z += sz; ez -= dy;} - } else { - z += sz; - ex += dx; ey += dy; - if (ex >= dz) {x += sx; ex -= dz;} - if (ey >= dz) {y += sy; ey -= dz;} - } - } - return false; -} -} // unnamed namespace - -// ------------------- update/publish ------------------- -void -AMCLLocalizer::update_rt(NavState & nav_state) -{ - predict(nav_state); - nav_state.set("robot_pose", get_pose()); -} - -void -AMCLLocalizer::update(NavState & nav_state) -{ - correct(nav_state); - - if ((get_node()->now() - last_reseed_).seconds() > reseed_time_) { - reseed(); - last_reseed_ = get_node()->now(); - } - - nav_state.set("robot_pose", get_pose()); - publishParticles(); -} - -// ------------------- correct ------------------- -void -AMCLLocalizer::correct(NavState & nav_state) -{ - auto t0 = get_node()->now(); - if (!nav_state.has("points")) { - RCLCPP_WARN(get_node()->get_logger(), "There is yet no points perceptions"); - return; - } - const auto perceptions = nav_state.get("points"); - - if (!nav_state.has("map.bonxai")) { - RCLCPP_WARN(get_node()->get_logger(), "There is yet no a bonxai map"); - return; - } - - if (!nav_state.has("map.bonxai.inflated")) { - auto start = get_node()->now(); - RCLCPP_INFO(get_node()->get_logger(), "Creating inflation map"); - const auto original_map = nav_state.get_ptr("map.bonxai"); - - auto logger = get_node()->get_logger(); - auto clock = get_node()->get_clock(); - - auto progress_cb = [logger, clock](float f) { - RCLCPP_INFO_THROTTLE(logger, *clock, 2000, "Inflating... %.1f%%", double(f) * 100.0); - }; - - std::shared_ptr inflated_map = - inflate_map(original_map, inflation_stddev_, inflation_prob_min_, progress_cb); - - if (!inflated_map) { - RCLCPP_ERROR(get_node()->get_logger(), "Error creating bonxai inflated map"); - return; - } - nav_state.set("map.bonxai.inflated", inflated_map); - - auto end = get_node()->now(); - RCLCPP_INFO(get_node()->get_logger(), "Created in %.3f seconds", (end - start).seconds()); - } - - const auto original_map = nav_state.get_ptr("map.bonxai"); - const auto map = nav_state.get_ptr("map.bonxai.inflated"); - auto t1 = get_node()->now(); - - const double vox = map->grid().voxelSize(); - const double ds_vox = std::max(0.5, 2.0 * vox); - auto view = PointPerceptionsOpsView(perceptions) - .downsample(ds_vox) - .fuse(get_tf_prefix() + "base_footprint"); - auto filtered = view->as_points(); - - if (filtered.size() > correct_max_points_) { - pcl::PointCloud slim; - slim.header = filtered.header; - slim.is_dense = filtered.is_dense; - slim.points.reserve(correct_max_points_); - - const size_t stride = std::max(1, filtered.size() / correct_max_points_); - for (size_t i = 0; i < filtered.size() && slim.points.size() < correct_max_points_; - i += stride) - { - slim.points.push_back(filtered.points[i]); - } - slim.width = static_cast(slim.points.size()); - slim.height = 1; - filtered.swap(slim); - } - - auto t2 = get_node()->now(); - if (filtered.empty()) { - RCLCPP_WARN(get_node()->get_logger(), "No points to correct"); - return; - } - auto t3 = get_node()->now(); - - // Tail-skip derived from Gaussian cutoff: distance where prob == inflation_prob_min_ - const double res_vox = original_map->grid().voxelSize(); - const double d_tail = inflation_stddev_ * std::sqrt(-2.0 * std::log(std::max(1e-6, - inflation_prob_min_))); - const int TAIL_SKIP = std::max(1, static_cast(std::ceil(d_tail / res_vox))); - - const float P_RAY_MIN = static_cast(inflation_prob_min_); - - auto acc_occ = original_map->grid().createConstAccessor(); - auto acc_inf = map->grid().createConstAccessor(); - - for (auto & particle : particles_) { - - bool debug = particle == particles_.front(); - - float hits = 0.0f, possible_hits = 0.0f; - - const tf2::Vector3 origin_w = particle.pose.getOrigin(); - const Bonxai::CoordT key_origin = - original_map->grid().posToCoord({origin_w.x(), origin_w.y(), origin_w.z()}); - - std::unordered_map end_counts; - end_counts.reserve(filtered.size()); - - for (const auto & pt : filtered) { - const tf2::Vector3 pw = particle.pose * tf2::Vector3(pt.x, pt.y, pt.z); - if (!std::isfinite(pw.x())) {continue;} - const Bonxai::CoordT key_end = original_map->grid().posToCoord({pw.x(), pw.y(), pw.z()}); - ++end_counts[key_end]; - } - - for (const auto & kv : end_counts) { - const Bonxai::CoordT & key_end = kv.first; - const int mult = kv.second; - possible_hits += static_cast(mult); - - float p_end = 0.0f; - if (const auto * cell_end = acc_inf.value(key_end)) { - p_end = Bonxai::ProbabilisticMap::prob(cell_end->probability_log); - } - - if (p_end < P_RAY_MIN) { - hits += p_end * mult; - continue; - } - - const bool occluded = ray_occluded_dda(*original_map, acc_occ, key_origin, key_end, - TAIL_SKIP); - hits += (occluded ? 0.0f : p_end) * mult; - } - - particle.hits += hits; - particle.possible_hits += possible_hits; - } - - auto t4 = get_node()->now(); - - for (auto & particle : particles_) { - if (particle.possible_hits > 0) { - double mean_p = static_cast(particle.hits) / - static_cast(particle.possible_hits); - mean_p = std::clamp(mean_p, 1e-6, 1.0); - particle.weight = std::pow(mean_p, weights_tau_); - } else { - particle.weight = 1e-6; - } - } - - double total_weight = 0.0; - for (const auto & p : particles_) { - total_weight += p.weight; - } - if (total_weight > 0.0) { - for (auto & p : particles_) { - p.weight /= total_weight; - } - } - - auto t5 = get_node()->now(); - - std::cerr << "Prepare maps: " << std::fixed << std::setprecision(6) << (t1 - t0).seconds() << - "\n"; - std::cerr << "Prepare perception: " << std::fixed << std::setprecision(6) << - (t2 - t1).seconds() << "\n"; - std::cerr << "Core: " << std::fixed << std::setprecision(6) << (t4 - t3).seconds() << "\n"; - std::cerr << "Final: " << std::fixed << std::setprecision(6) << (t5 - t4).seconds() << "\n"; -} - -// ------------------- reseed ------------------- -void -AMCLLocalizer::reseed() -{ - if (particles_.empty()) {return;} - if (particles_[0].possible_hits == 0) {return;} - - const std::size_t N = particles_.size(); - auto top_count = [&](std::size_t n) -> std::size_t { - const double f = std::clamp(top_keep_fraction_, 0.0, 1.0); - std::size_t k = static_cast(std::floor(static_cast(n) * f)); - return std::max(1, std::min(k, n)); - }; - const std::size_t N_top = top_count(N); - - std::sort(particles_.begin(), particles_.end(), - [](const Particle & a, const Particle & b) {return a.weight > b.weight;}); - - std::cerr << - "=================================================================================\n"; - for (std::size_t i = 0; i < particles_.size(); i++) { - std::cerr << "[" << i << "] " << particles_[i].hits << "/" << particles_[i].possible_hits - << " " << static_cast(particles_[i].last_cid) << std::endl; - } - - - tf2::Vector3 mean = computeMean(particles_, 0, N_top); - tf2::Matrix3x3 cov = computeCovariance(particles_, 0, N_top, mean); - - double a = cov[0][0], b = cov[0][1], c = cov[1][1]; - - auto safe_cov_2x2 = [](double a_in, double b_in, double c_in) { - const double eps = 1e-9; - double a2 = std::isfinite(a_in) ? a_in : 0.0; - double b2 = std::isfinite(b_in) ? b_in : 0.0; - double c2 = std::isfinite(c_in) ? c_in : 0.0; - if (a2 < eps) {a2 = eps;} - if (c2 < eps) {c2 = eps;} - double det = a2 * c2 - b2 * b2; - if (det < 0.0) { - double max_b = std::sqrt(a2 * c2) - eps; - max_b = std::max(0.0, max_b); - if (b2 > max_b) {b2 = max_b;} - if (b2 < -max_b) {b2 = -max_b;} - } - return std::tuple(a2, b2, c2); - }; - - auto [a2, b2, c2] = safe_cov_2x2(a, b, c); - double l00 = std::sqrt(std::max(a2, 0.0)); - double l10 = (l00 > 0.0) ? (b2 / l00) : 0.0; - double t = c2 - l10 * l10; if (t < 0.0) {t = 0.0;} - double l11 = std::sqrt(t); - - double yaw_variance = computeYawVariance(particles_, 0, N_top); - if (!std::isfinite(yaw_variance) || yaw_variance < 0.0) {yaw_variance = 0.0;} - double yaw_stddev = std::sqrt(yaw_variance); - if (!std::isfinite(yaw_stddev) || yaw_stddev < min_noise_yaw_) {yaw_stddev = min_noise_yaw_;} - std::normal_distribution yaw_noise(0.0, yaw_stddev); - - std::normal_distribution n01(0.0, 1.0); - std::normal_distribution index_dist(0.0, std::max(1.0, 0.05 * static_cast(N))); - - auto sample_xy_offset = [&]() -> std::pair { - double z0 = n01(rng_), z1 = n01(rng_); - double dx = l00 * z0; - double dy = l10 * z0 + l11 * z1; - - if (!std::isfinite(dx) || !std::isfinite(dy)) { - std::normal_distribution nIso(0.0, min_noise_xy_); - return {nIso(rng_), nIso(rng_)}; - } - - double r = std::hypot(dx, dy); - if (!(r >= min_noise_xy_)) { - if (r < 1e-12) { - std::normal_distribution nIso(0.0, min_noise_xy_); - dx = nIso(rng_); dy = nIso(rng_); - } else { - double s = (min_noise_xy_ / r); - dx *= s; dy *= s; - } - } - return {dx, dy}; - }; - - for (std::size_t i = N_top; i < N; ++i) { - int selected_idx; - int guard = 0; - do { - double idx_sample = std::round(index_dist(rng_)); - selected_idx = static_cast(idx_sample); - if (selected_idx < 0) {selected_idx = -selected_idx;} - if (selected_idx >= static_cast(N_top)) {selected_idx = static_cast(N_top) - 1;} - if (selected_idx < 0) {selected_idx = 0;} - if (++guard > 16) {selected_idx = 0; break;} - } while (selected_idx < 0 || static_cast(selected_idx) >= N_top); - - const auto & ref = particles_[static_cast(selected_idx)]; - tf2::Vector3 origin = ref.pose.getOrigin(); - auto [dx_off, dy_off] = sample_xy_offset(); - - double rr, pp, yy; - tf2::Matrix3x3(ref.pose.getRotation()).getRPY(rr, pp, yy); - double noisy_yaw = yy + yaw_noise(rng_); - if (!std::isfinite(noisy_yaw)) {noisy_yaw = yy;} - - tf2::Quaternion q; q.setRPY(0.0, 0.0, noisy_yaw); - if (q.length2() < 1e-20 || !std::isfinite(q.x()) || !std::isfinite(q.y()) || - !std::isfinite(q.z()) || !std::isfinite(q.w())) - { - q.setRPY(0.0, 0.0, yy); - } - - tf2::Vector3 new_origin(origin.x() + dx_off, origin.y() + dy_off, origin.z()); - if (!std::isfinite(new_origin.x()) || !std::isfinite(new_origin.y()) || - !std::isfinite(new_origin.z())) - { - new_origin = tf2::Vector3(origin.x(), origin.y(), origin.z()); - } - - particles_[i].pose.setOrigin(new_origin); - particles_[i].pose.setRotation(q); - particles_[i].weight = ref.weight; - } - - for (auto & p : particles_) { - p.hits = 0; p.possible_hits = 0; - } - - double total_weight = 0.0; - for (const auto & p : particles_) { - if (std::isfinite(p.weight)) { - total_weight += p.weight; - } - } - - if (total_weight > 0.0 && std::isfinite(total_weight)) { - for (auto & p : particles_) { - p.weight /= total_weight; - if (!std::isfinite(p.weight) || p.weight < 0.0) {p.weight = 0.0;} - } - } else { - double w = 1.0 / static_cast(particles_.size()); - for (auto & p : particles_) { - p.weight = w; - } - } -} - -// ------------------- publish & pose ------------------- -void -AMCLLocalizer::publishTF(const tf2::Transform & map2bf) -{ - if (!is_finite_tf(map2bf)) { - RCLCPP_ERROR(get_node()->get_logger(), "publishTF: transform has NaN/inf; skipping"); - return; - } - - geometry_msgs::msg::TransformStamped tf_msg; - tf_msg.header.stamp = get_node()->now(); - tf_msg.header.frame_id = get_tf_prefix() + "map"; - tf_msg.child_frame_id = get_tf_prefix() + "odom"; - tf_msg.transform = tf2::toMsg(map2bf); - - RTTFBuffer::getInstance()->setTransform(tf_msg, "easynav", false); - tf_broadcaster_->sendTransform(tf_msg); -} - -void -AMCLLocalizer::publishParticles() -{ - geometry_msgs::msg::PoseArray array_msg; - array_msg.header.stamp = get_node()->now(); - array_msg.header.frame_id = get_tf_prefix() + "map"; - - array_msg.poses.reserve(particles_.size()); - for (const auto & p : particles_) { - geometry_msgs::msg::Pose pose_msg; - pose_msg.position.x = p.pose.getOrigin().x(); - pose_msg.position.y = p.pose.getOrigin().y(); - pose_msg.position.z = p.pose.getOrigin().z(); - pose_msg.orientation = tf2::toMsg(p.pose.getRotation()); - array_msg.poses.push_back(pose_msg); - } - particles_pub_->publish(array_msg); -} - -tf2::Transform -AMCLLocalizer::getEstimatedPose() const -{ - if (particles_.empty()) {return tf2::Transform::getIdentity();} - - const std::size_t N = particles_.size(); - auto top_count = [&](std::size_t n) -> std::size_t { - const double f = std::clamp(top_keep_fraction_, 0.0, 1.0); - std::size_t k = static_cast(std::floor(static_cast(n) * f)); - return std::max(1, std::min(k, n)); - }; - const std::size_t N_top = top_count(N); - - std::vector sorted = particles_; - std::sort(sorted.begin(), sorted.end(), - [](const Particle & a, const Particle & b) {return a.weight > b.weight;}); - - tf2::Vector3 mean = computeMean(sorted, 0, N_top); - if (!std::isfinite(mean.x()) || !std::isfinite(mean.y()) || !std::isfinite(mean.z())) { - mean = tf2::Vector3(0.0, 0.0, 0.0); - } - double r, p, y; - tf2::Matrix3x3(sorted[0].pose.getRotation()).getRPY(r, p, y); - tf2::Quaternion q; q.setRPY(0.0, 0.0, y); - - tf2::Transform est; est.setOrigin(mean); est.setRotation(q); - return est; -} - -void -AMCLLocalizer::publishEstimatedPose(const tf2::Transform & est_pose) -{ - if (particles_.empty()) {return;} - - const std::size_t N = particles_.size(); - auto top_count = [&](std::size_t n) -> std::size_t { - const double f = std::clamp(top_keep_fraction_, 0.0, 1.0); - std::size_t k = static_cast(std::floor(static_cast(n) * f)); - return std::max(1, std::min(k, n)); - }; - const std::size_t N_top = top_count(N); - - tf2::Vector3 mean = est_pose.getOrigin(); - tf2::Matrix3x3 cov = computeCovariance(particles_, 0, N_top, mean); - double yaw_variance = computeYawVariance(particles_, 0, N_top); - - geometry_msgs::msg::PoseWithCovarianceStamped msg; - msg.header.stamp = get_node()->now(); - msg.header.frame_id = get_tf_prefix() + "map"; - - msg.pose.pose.position.x = mean.x(); - msg.pose.pose.position.y = mean.y(); - msg.pose.pose.position.z = mean.z(); - msg.pose.pose.orientation = tf2::toMsg(est_pose.getRotation()); - - for (int r = 0; r < 3; ++r) { - for (int c = 0; c < 3; ++c) { - msg.pose.covariance[6 * r + c] = cov[r][c]; - } - } - msg.pose.covariance[6 * 5 + 5] = yaw_variance; - - estimate_pub_->publish(msg); -} - -nav_msgs::msg::Odometry -AMCLLocalizer::get_pose() -{ - nav_msgs::msg::Odometry odom_msg; - - odom_msg.header.stamp = get_node()->now(); - odom_msg.header.frame_id = get_tf_prefix() + "map"; - odom_msg.child_frame_id = get_tf_prefix() + "base_footprint"; - - pose_ = getEstimatedPose(); - tf2::Transform est_pose = pose_; - - odom_msg.pose.pose.position.x = est_pose.getOrigin().x(); - odom_msg.pose.pose.position.y = est_pose.getOrigin().y(); - odom_msg.pose.pose.position.z = est_pose.getOrigin().z(); - odom_msg.pose.pose.orientation = tf2::toMsg(est_pose.getRotation()); - - if (!particles_.empty()) { - const std::size_t N = particles_.size(); - auto top_count = [&](std::size_t n) -> std::size_t { - const double f = std::clamp(top_keep_fraction_, 0.0, 1.0); - std::size_t k = static_cast(std::floor(static_cast(n) * f)); - return std::max(1, std::min(k, n)); - }; - const std::size_t N_top = top_count(N); - - std::vector sorted = particles_; - std::sort(sorted.begin(), sorted.end(), - [](const Particle & a, const Particle & b) {return a.weight > b.weight;}); - - tf2::Vector3 mean = computeMean(sorted, 0, N_top); - tf2::Matrix3x3 cov = computeCovariance(sorted, 0, N_top, mean); - double yaw_var = computeYawVariance(sorted, 0, N_top); - - for (int r = 0; r < 3; ++r) { - for (int c = 0; c < 3; ++c) { - odom_msg.pose.covariance[6 * r + c] = cov[r][c]; - } - } - odom_msg.pose.covariance[6 * 5 + 5] = yaw_var; - } - - odom_msg.twist.twist.linear.x = 0.0; - odom_msg.twist.twist.linear.y = 0.0; - odom_msg.twist.twist.linear.z = 0.0; - odom_msg.twist.twist.angular.x = 0.0; - odom_msg.twist.twist.angular.y = 0.0; - odom_msg.twist.twist.angular.z = 0.0; - - return odom_msg; -} - -} // namespace navmap -} // namespace easynav - -#include -PLUGINLIB_EXPORT_CLASS(easynav::navmap::AMCLLocalizer, easynav::LocalizerMethodBase) diff --git a/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp b/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp index 7a869791..dda296e3 100644 --- a/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp @@ -60,9 +60,6 @@ static constexpr unsigned char INSCRIBED_INFLATED_OBSTACLE = 253; static constexpr unsigned char MAX_NON_OBSTACLE = 252; static constexpr unsigned char FREE_SPACE = 0; -// ---------- math helpers ---------- -static inline double sqr(double x) {return x * x;} - // ---------- stats over particles ---------- tf2::Vector3 computeMean( const std::vector & particles, std::size_t start, @@ -640,7 +637,7 @@ static ScoreAgg score_particle_sensor_cloud( const Bonxai::ProbabilisticMap & occ_map, const Bonxai::ProbabilisticMap & inf_map, int tail_skip, - bool debug = false) + [[maybe_unused]] bool debug = false) { using Bonxai::CoordT; ScoreAgg s; @@ -749,7 +746,7 @@ void AMCLLocalizer::correct(NavState & nav_state) try { T_bf_sensor_cache[b.frame_id] = lookup_bf_to_sensor(b.frame_id); - const tf2::Transform & t = T_bf_sensor_cache[b.frame_id]; + // const tf2::Transform & t = T_bf_sensor_cache[b.frame_id]; } catch (const tf2::TransformException & ex) { RCLCPP_WARN(get_node()->get_logger(), "TF bf->%s failed: %s", b.frame_id.c_str(), diff --git a/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer_copy_v0.cpp b/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer_copy_v0.cpp deleted file mode 100644 index 58716bae..00000000 --- a/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer_copy_v0.cpp +++ /dev/null @@ -1,1247 +0,0 @@ -// Copyright 2025 Intelligent Robotics Lab -// -// This file is part of the project Easy Navigation (EasyNav in short) -// licensed under the GNU General Public License v3.0. -// See for details. -// -// Easy Navigation program is free software: you can redistribute it and/or modify -// it under the terms of the GNU General Public License as published by -// the Free Software Foundation, either version 3 of the License, or -// (at your option) any later version. -// -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. -// -// You should have received a copy of the GNU General Public License -// along with this program. If not, see . - -/// \file -/// \brief Implementation of the AMCLLocalizer class using Costmap2D. - -#include -#include - -#include "bonxai/bonxai.hpp" -#include "bonxai/probabilistic_map.hpp" - -#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" -#include "tf2/LinearMath/Vector3.hpp" - -#include "easynav_common/RTTFBuffer.hpp" -#include "easynav_common/types/Perceptions.hpp" -#include "easynav_common/types/PointPerception.hpp" -#include "easynav_common/types/IMUPerception.hpp" - -#include "navmap_core/NavMap.hpp" - -#include "easynav_navmap_localizer/AMCLLocalizer.hpp" -#include "easynav_localizer/LocalizerNode.hpp" - -namespace easynav -{ -namespace navmap -{ - -static constexpr unsigned char NO_INFORMATION = 255; -static constexpr unsigned char LETHAL_OBSTACLE = 254; -static constexpr unsigned char INSCRIBED_INFLATED_OBSTACLE = 253; -static constexpr unsigned char MAX_NON_OBSTACLE = 252; -static constexpr unsigned char FREE_SPACE = 0; - -tf2::Vector3 computeMean( - const std::vector & particles, - std::size_t start, std::size_t count) -{ - tf2::Vector3 weighted_sum(0.0, 0.0, 0.0); - double total_weight = 0.0; - - if (start >= particles.size() || count == 0) {return weighted_sum;} - - std::size_t end = std::min(start + count, particles.size()); - - for (std::size_t i = start; i < end; ++i) { - const Particle & p = particles[i]; - const tf2::Vector3 & origin = p.pose.getOrigin(); - if (!std::isfinite(origin.x()) || !std::isfinite(origin.y()) || !std::isfinite(origin.z())) { - continue; // descarta contribuciones inválidas - } - weighted_sum += origin * p.weight; - total_weight += p.weight; - } - - if (total_weight > 0.0) { - return weighted_sum / total_weight; - } else { - return tf2::Vector3(0.0, 0.0, 0.0); - } -} - -tf2::Matrix3x3 -computeCovariance( - const std::vector & particles, - std::size_t start, std::size_t count, - const tf2::Vector3 & mean) -{ - double total_weight = 0.0; - double cov[3][3] = {{0.0}}; - - if (start >= particles.size() || count == 0) {return tf2::Matrix3x3();} - - std::size_t end = std::min(start + count, particles.size()); - - for (std::size_t i = start; i < end; ++i) { - const Particle & p = particles[i]; - tf2::Vector3 delta = p.pose.getOrigin() - mean; - - double w = p.weight; - total_weight += w; - - cov[0][0] += w * delta.x() * delta.x(); - cov[0][1] += w * delta.x() * delta.y(); - cov[0][2] += w * delta.x() * delta.z(); - cov[1][1] += w * delta.y() * delta.y(); - cov[1][2] += w * delta.y() * delta.z(); - cov[2][2] += w * delta.z() * delta.z(); - } - - if (total_weight > 0.0) { - cov[0][0] /= total_weight; - cov[0][1] /= total_weight; - cov[0][2] /= total_weight; - cov[1][1] /= total_weight; - cov[1][2] /= total_weight; - cov[2][2] /= total_weight; - - // La matriz es simétrica - cov[1][0] = cov[0][1]; - cov[2][0] = cov[0][2]; - cov[2][1] = cov[1][2]; - } - - return tf2::Matrix3x3( - cov[0][0], cov[0][1], cov[0][2], - cov[1][0], cov[1][1], cov[1][2], - cov[2][0], cov[2][1], cov[2][2]); -} - -double -extractYaw(const tf2::Transform & transform) -{ - double roll, pitch, yaw; - tf2::Matrix3x3(transform.getRotation()).getRPY(roll, pitch, yaw); - return yaw; -} - -double -computeYawVariance( - const std::vector & particles, - std::size_t start, std::size_t count) -{ - double sum_cos = 0.0; - double sum_sin = 0.0; - double total_weight = 0.0; - - if (start >= particles.size() || count == 0) { - return 0.0; - } - - std::size_t end = std::min(start + count, particles.size()); - - for (std::size_t i = start; i < end; ++i) { - const Particle & p = particles[i]; - double yaw = extractYaw(p.pose); - double w = p.weight; - - sum_cos += w * std::cos(yaw); - sum_sin += w * std::sin(yaw); - total_weight += w; - } - - if (total_weight == 0.0) { - return 0.0; - } - - double mean_cos = sum_cos / total_weight; - double mean_sin = sum_sin / total_weight; - - double R = std::sqrt(mean_cos * mean_cos + mean_sin * mean_sin); - if (!std::isfinite(R)) {R = 0.0;} - R = std::clamp(R, 1e-12, 1.0); - double variance = -2.0 * std::log(R); - if (!std::isfinite(variance) || variance < 0.0) {variance = 0.0;} - return variance; -} - -using std::placeholders::_1; -using namespace std::chrono_literals; - - -AMCLLocalizer::AMCLLocalizer() -{ - NavState::register_printer( - [](const nav_msgs::msg::Odometry & odom) { - std::ostringstream ret; - double x = odom.pose.pose.position.x; - double y = odom.pose.pose.position.y; - - tf2::Quaternion q( - odom.pose.pose.orientation.x, - odom.pose.pose.orientation.y, - odom.pose.pose.orientation.z, - odom.pose.pose.orientation.w); - - double roll, pitch, yaw; - tf2::Matrix3x3(q).getRPY(roll, pitch, yaw); - - ret << "Odometry with pose: (x: " << x << ", y: " << y << ", yaw: " << yaw << ")"; - return ret.str(); - }); -} - -AMCLLocalizer::~AMCLLocalizer() -{ -} - -std::expected -AMCLLocalizer::on_initialize() -{ - auto node = get_node(); - const auto & plugin_name = get_plugin_name(); - - int num_particles; - double x_init, y_init, yaw_init, std_dev_xy, std_dev_yaw; - std::string perception_model; - - node->declare_parameter(plugin_name + ".num_particles", 100); - node->declare_parameter(plugin_name + ".initial_pose.x", 0.0); - node->declare_parameter(plugin_name + ".initial_pose.y", 0.0); - node->declare_parameter(plugin_name + ".initial_pose.yaw", 0.0); - node->declare_parameter(plugin_name + ".initial_pose.std_dev_xy", 0.5); - node->declare_parameter(plugin_name + ".initial_pose.std_dev_yaw", 0.5); - node->declare_parameter(plugin_name + ".reseed_freq", 1.0); - node->declare_parameter(plugin_name + ".noise_translation", 0.01); - node->declare_parameter(plugin_name + ".noise_rotation", 0.01); - node->declare_parameter(plugin_name + ".noise_translation_to_rotation", 0.01); - node->declare_parameter(plugin_name + ".min_noise_xy", 0.05); - node->declare_parameter(plugin_name + ".min_noise_yaw", 0.05); - node->declare_parameter(plugin_name + ".compute_odom_from_tf", false); - node->declare_parameter(plugin_name + ".perception_model", "occupancy"); - - node->get_parameter(plugin_name + ".num_particles", num_particles); - node->get_parameter(plugin_name + ".initial_pose.x", x_init); - node->get_parameter(plugin_name + ".initial_pose.y", y_init); - node->get_parameter(plugin_name + ".initial_pose.yaw", yaw_init); - node->get_parameter(plugin_name + ".initial_pose.std_dev_xy", std_dev_xy); - node->get_parameter(plugin_name + ".initial_pose.std_dev_yaw", std_dev_yaw); - node->get_parameter(plugin_name + ".noise_translation", noise_translation_); - node->get_parameter(plugin_name + ".noise_rotation", noise_rotation_); - node->get_parameter(plugin_name + ".noise_translation_to_rotation", - noise_translation_to_rotation_); - node->get_parameter(plugin_name + ".min_noise_xy", min_noise_xy_); - node->get_parameter(plugin_name + ".min_noise_yaw", min_noise_yaw_); - node->get_parameter(plugin_name + ".compute_odom_from_tf", compute_odom_from_tf_); - node->get_parameter(plugin_name + ".perception_model", perception_model); - - // if (perception_model == "octomap") { - // percepcion_model_ = PerceptionModel::OCT; - // } else { - // percepcion_model_ = PerceptionModel::OCC; - // } - - double reseed_freq; - node->get_parameter(plugin_name + ".reseed_freq", reseed_freq); - reseed_time_ = 1.0 / reseed_freq; - - RCLCPP_INFO(node->get_logger(), "Initialized AMCL pose with %d particles", num_particles); - RCLCPP_INFO(node->get_logger(), "at position (%lf, %lf, %lf) std_dev [%lf, %lf]", - x_init, y_init, yaw_init, std_dev_xy, std_dev_yaw); - - std::normal_distribution noise_x(x_init, std_dev_xy); - std::normal_distribution noise_y(y_init, std_dev_xy); - std::normal_distribution noise_yaw(yaw_init, std_dev_yaw); - - particles_.resize(num_particles); - for (auto & p : particles_) { - tf2::Quaternion q; - q.setRPY(0.0, 0.0, noise_yaw(rng_)); - p.pose.setRotation(q); - p.pose.setOrigin(tf2::Vector3(noise_x(rng_), noise_y(rng_), 0.0)); - - p.hits = 0; - p.possible_hits = 0; - p.weight = 1.0 / num_particles; - } - - tf_broadcaster_ = std::make_unique(get_node()); - - auto node_typed = std::dynamic_pointer_cast(get_node()); - auto rt_cbg = node_typed->get_real_time_cbg(); - rclcpp::SubscriptionOptions options; - options.callback_group = rt_cbg; - - if (!compute_odom_from_tf_) { - odom_sub_ = get_node()->create_subscription( - "odom", rclcpp::SensorDataQoS().reliable(), - std::bind(&AMCLLocalizer::odom_callback, this, _1), options); - } - - particles_pub_ = get_node()->create_publisher( - node->get_fully_qualified_name() + std::string("/") + plugin_name + "/particles", 10); - estimate_pub_ = get_node()->create_publisher( - node->get_fully_qualified_name() + std::string("/") + plugin_name + "/pose", 10); - - last_reseed_ = get_node()->now(); - - get_node()->get_logger().set_level(rclcpp::Logger::Level::Debug); - - return {}; -} - -void printTransform(const tf2::Transform & tf) -{ - const tf2::Vector3 & origin = tf.getOrigin(); - const tf2::Quaternion & rot = tf.getRotation(); - - std::cerr << "Translation: [" - << origin.x() << ", " - << origin.y() << ", " - << origin.z() << "]\t"; - - std::cerr << "Rotation (quaternion): [" - << rot.x() << ", " - << rot.y() << ", " - << rot.z() << ", " - << rot.w() << "]\n"; -} - -void -AMCLLocalizer::update_rt(NavState & nav_state) -{ - predict(nav_state); - - nav_state.set("robot_pose", get_pose()); -} - -void -AMCLLocalizer::update(NavState & nav_state) -{ - correct(nav_state); - - if ((get_node()->now() - last_reseed_).seconds() > reseed_time_) { - reseed(); - last_reseed_ = get_node()->now(); - } - - nav_state.set("robot_pose", get_pose()); - - publishParticles(); -} - -void -AMCLLocalizer::odom_callback(nav_msgs::msg::Odometry::UniquePtr msg) -{ - if (compute_odom_from_tf_) {return;} - - tf2::fromMsg(msg->pose.pose, odom_); - - if (!initialized_odom_) { - last_odom_ = odom_; - initialized_odom_ = true; - } -} - -void -AMCLLocalizer::update_odom_from_tf() -{ - geometry_msgs::msg::TransformStamped tf_msg; - try { - tf_msg = RTTFBuffer::getInstance()->lookupTransform( - "odom", "base_footprint", tf2::TimePointZero, tf2::durationFromSec(0.0)); - } catch (const tf2::TransformException & ex) { - RCLCPP_WARN(get_node()->get_logger(), "AMCLLocalizer::update: TF failed: %s", ex.what()); - return; - } - - tf2::Transform tf_odom; - tf2::fromMsg(tf_msg.transform, tf_odom); - - last_odom_ = odom_; - odom_ = tf_odom; - - initialized_odom_ = true; -} - - -std::optional -get_latest_imu_quat(const NavState & nav_state) -{ - if (!nav_state.has("imu")) { - return std::nullopt; - } - // Obtiene la colección de percepciones IMU - const auto imus = nav_state.get("imu"); - if (imus.empty() || !imus.back()) { - return std::nullopt; - } - - // Selecciono la última (puedes ordenar por stamp si lo prefieres) - const auto & imu_msg = imus.back()->data; - - tf2::Quaternion q; - q.setX(imu_msg.orientation.x); - q.setY(imu_msg.orientation.y); - q.setZ(imu_msg.orientation.z); - q.setW(imu_msg.orientation.w); - - // Normaliza por robustez numérica - if (q.length2() < 1e-12) { - return std::nullopt; - } - q.normalize(); - return q; -} - -static inline tf2::Vector3 project_onto_plane(const tf2::Vector3 & v, const tf2::Vector3 & n_unit) -{ - return v - n_unit * v.dot(n_unit); -} - -static inline tf2::Quaternion frame_from_normal_and_yaw( - const tf2::Vector3 & n_world_unit, double yaw_world) -{ - tf2::Vector3 x_tgt(std::cos(yaw_world), std::sin(yaw_world), 0.0); - tf2::Vector3 x_tan = project_onto_plane(x_tgt, n_world_unit); - - double xn = x_tan.length(); - if (xn < 1e-9) { - // fallback estable - x_tan = project_onto_plane(tf2::Vector3(1.0, 0.0, 0.0), n_world_unit); - xn = x_tan.length(); - if (xn < 1e-9) { - tf2::Quaternion q; q.setRPY(0, 0, yaw_world); - return q; - } - } - x_tan /= xn; - - tf2::Vector3 z_axis = n_world_unit; - tf2::Vector3 y_axis = z_axis.cross(x_tan); - double yn = y_axis.length(); - if (yn < 1e-9) { - tf2::Quaternion q; q.setRPY(0, 0, yaw_world); - return q; - } - y_axis /= yn; - - tf2::Matrix3x3 R( - x_tan.x(), y_axis.x(), z_axis.x(), - x_tan.y(), y_axis.y(), z_axis.y(), - x_tan.z(), y_axis.z(), z_axis.z()); - tf2::Quaternion q; R.getRotation(q); - return q; -} - -static inline tf2::Vector3 to_tf(const Eigen::Vector3f & v) -{ - return {static_cast(v.x()), static_cast(v.y()), static_cast(v.z())}; -} - -void -AMCLLocalizer::predict(NavState & nav_state) -{ - if (!initialized_odom_) { - if (compute_odom_from_tf_) { - update_odom_from_tf(); - } - return; - } - if (compute_odom_from_tf_) { - update_odom_from_tf(); - } - - tf2::Transform delta = last_odom_.inverseTimes(odom_); - - const bool have_navmap = nav_state.has("map.navmap"); - if (have_navmap) { - navmap_ = nav_state.get<::navmap::NavMap>("map.navmap"); - } - - // IMU disponible - const auto imu_q_opt = get_latest_imu_quat(nav_state); - - // Escalado de ruido - tf2::Vector3 t = delta.getOrigin(); - double dx = t.x(), dy = t.y(), dz = t.z(); - double trans_len = std::sqrt(dx * dx + dy * dy + dz * dz); - - double roll, pitch, yaw; - tf2::Matrix3x3(delta.getRotation()).getRPY(roll, pitch, yaw); - double rot_len = std::abs(yaw); - - std::random_device rd; - std::mt19937 gen(rd()); - - for (auto & p : particles_) { - // 1) Odometría con ruido (igual que ya hacías) - std::normal_distribution noise_dx(0.0, std::abs(dx) * noise_translation_); - std::normal_distribution noise_dy(0.0, std::abs(dy) * noise_translation_); - std::normal_distribution noise_dz(0.0, std::abs(dz) * noise_translation_); - std::normal_distribution noise_yaw( - 0.0, rot_len * noise_rotation_ + trans_len * noise_translation_to_rotation_); - - tf2::Vector3 noisy_translation( - dx + noise_dx(gen), - dy + noise_dy(gen), - dz + noise_dz(gen)); - - double noisy_yaw = yaw + noise_yaw(gen); - tf2::Quaternion noisy_q; noisy_q.setRPY(0.0, 0.0, noisy_yaw); - tf2::Transform noisy_delta(noisy_q, noisy_translation); - - p.pose = p.pose * noisy_delta; - - // 2) Ajuste con NavMap: Z desde superficie y orientación según IMU o normal - if (have_navmap) { - // Localiza navcel cercano usando pista - ::navmap::NavMap::LocateOpts opts; - if (p.last_cid != std::numeric_limits::max()) { - opts.hint_cid = p.last_cid; - opts.hint_surface = p.last_surface; - } - opts.planar_eps = 1e-4f; - opts.height_eps = 0.50f; - opts.use_downward_ray = true; - - tf2::Vector3 Pw = p.pose.getOrigin(); - - std::size_t sidx = 0; - ::navmap::NavCelId cid = std::numeric_limits::max(); - Eigen::Vector3f bary, hit_eig; - - const bool ok = navmap_.locate_navcel( - Eigen::Vector3f(static_cast(Pw.x()), - static_cast(Pw.y()), - static_cast(Pw.z() + 4.0)), - sidx, cid, bary, &hit_eig, opts); - - if (ok) { - // std::cerr << "found navcel for (" << Pw.x() << ", " << Pw.y() << ", " << Pw.z() << ") at " << hit_eig.z() << std::endl; - - // Z desde superficie - tf2::Vector3 hit = to_tf(hit_eig); - p.pose.setOrigin(tf2::Vector3(Pw.x(), Pw.y(), hit.z())); - - if (imu_q_opt.has_value()) { - // 2.a) Orientación desde IMU - p.pose.setRotation(*imu_q_opt); - } else { - // 2.b) Sin IMU: alinear con normal y conservar yaw odométrico - const auto & cel = navmap_.navcels[cid]; - tf2::Vector3 n_world = to_tf(cel.normal); - double nlen = n_world.length(); - if (nlen > 1e-9) { - n_world /= nlen; - double r, pp, y; - tf2::Matrix3x3(p.pose.getRotation()).getRPY(r, pp, y); - tf2::Quaternion q_align = frame_from_normal_and_yaw(n_world, y); - p.pose.setRotation(q_align); - } - } - - // Guarda pista para walking - p.last_cid = cid; - p.last_surface = sidx; - } else { - p.pose.setOrigin(tf2::Vector3(Pw.x(), Pw.y(), Pw.z())); - if (imu_q_opt.has_value()) { - p.pose.setRotation(*imu_q_opt); - } - // std::cerr << "not found navcel for (" << Pw.x() << ", " << Pw.y() << ", " << Pw.z() << ")" << std::endl; - } - } - } - - last_odom_ = odom_; - - // Publicación como ya tienes - pose_ = getEstimatedPose(); - tf2::Transform map2bf = pose_; - tf2::Transform map2odom = map2bf * odom_.inverse(); - publishTF(map2odom); - publishEstimatedPose(map2bf); -} - - -static inline double sqr(double x) {return x * x;} - -using ProgressCallback = std::function; // [0,1] - - -#include -#include -#include -#include - -std::shared_ptr -inflate_map( - const std::shared_ptr & src, - double sigma, - double p_min, - ProgressCallback progress /*= nullptr*/) -{ - using namespace Bonxai; - - if (!src || sigma <= 0.0) { - return nullptr; - } - - // 1) Clonar configuración y obtener tamaño de vóxel - const auto & src_grid = src->grid(); - - // Si tu grid expone voxelSize(), úsalo; de lo contrario puedes calcularlo con coordToPos({0,0,0}) y {1,0,0}. - const double res = src_grid.voxelSize(); - - auto dst = std::make_shared(res); - dst->setOptions(src->options()); - - // 2) Semillas: voxeles ocupados del origen - std::vector seeds; - src->getOccupiedVoxels(seeds); // celdas con probability_log > occupancy_threshold_log - if (seeds.empty()) { - return dst; // nada que inflar - } - - // 3) Parámetros de inflado: radio en vóxeles y LUT de probabilidad por distancia entera - const int R = static_cast(std::ceil(3.0 * sigma / res)); - if (R <= 0) { - // sigma muy pequeño: solo marcar ocupados - auto acc = dst->grid().createAccessor(); - for (const auto & c : seeds) { - auto * cell = acc.value(c, /*create=*/true); - cell->probability_log = dst->options().clamp_max_log; - cell->update_id = 0; - } - return dst; - } - - const double inv_2sig2 = 1.0 / (2.0 * sigma * sigma); - std::vector lut_logod(R + 1); - for (int d = 0; d <= R; ++d) { - const double dist_m = d * res; - const double p = std::exp(-(dist_m * dist_m) * inv_2sig2); - lut_logod[d] = (p >= p_min) ? - ProbabilisticMap::logods(static_cast(p)) : - std::numeric_limits::min(); // sentinel: no escribir - } - - // 4) Estructuras del brushfire - // Buckets por distancia entera [0..R] - std::vector> buckets(R + 1); - buckets[0] = seeds; - - // Conjunto de visitados (para no reinsertar). Hash ligero para CoordT{x,y,z} - struct KeyHash - { - size_t operator()(const CoordT & c) const noexcept - { - // hashes primos distintos para mezclar - return (static_cast(c.x) * 73856093u) ^ - (static_cast(c.y) * 19349663u) ^ - (static_cast(c.z) * 83492791u); - } - }; - struct KeyEq - { - bool operator()(const CoordT & a, const CoordT & b) const noexcept - { - return a.x == b.x && a.y == b.y && a.z == b.z; - } - }; - std::unordered_set visited; - visited.reserve(seeds.size() * 4); - for (const auto & c : seeds) { - visited.insert(c); - } - - // Acceso a celdas del destino y clamps - auto acc = dst->grid().createAccessor(); - const auto clamp_min = dst->options().clamp_min_log; - const auto clamp_max = dst->options().clamp_max_log; - - auto write_max = [&](const CoordT & c, int32_t v) { - if (v == std::numeric_limits::min()) {return;} // por p < p_min - auto * cell = acc.value(c, /*create=*/true); - int32_t proposed = std::max(cell->probability_log, v); - if (proposed < clamp_min) {proposed = clamp_min;} - if (proposed > clamp_max) {proposed = clamp_max;} - cell->probability_log = proposed; - cell->update_id = 0; - }; - - // Marca semillas (d = 0) - write_max(CoordT{0, 0, 0}, std::numeric_limits::min()); // no-op para forzar instanciación de lambda - for (const auto & c : seeds) { - write_max(c, lut_logod[0]); - } - - // Vecindad 6 (rápida). Puedes cambiar a 26 si quieres una isometría mejor. - static const CoordT N6[6] = { - {1, 0, 0}, {-1, 0, 0}, - {0, 1, 0}, {0, -1, 0}, - {0, 0, 1}, {0, 0, -1} - }; - - // 5) Progreso y trazas - std::cout << "Inflating probabilistic map (brushfire): seeds=" << seeds.size() - << " sigma=" << sigma << "m, R=" << R << ", res=" << res << "m\n"; - - // 6) Expansión por capas de distancia - size_t expanded = 0; - for (int d = 0; d < R; ++d) { - auto & bucket = buckets[d]; - - // Progreso por nivel (si hay callback) - if (progress) { - float frac = static_cast(d) / static_cast(R); - progress(frac); - } - - // Avance informativo por consola cada ~10 % de los niveles - if (R >= 10 && (d % std::max(1, R / 10) == 0)) { - double pct = 100.0 * static_cast(d) / static_cast(R); - std::cout << " d=" << d << "/" << R << " (" << pct << "%)\n"; - } - - const int nd = d + 1; - for (const CoordT & c0 : bucket) { - for (const CoordT & dn : N6) { - CoordT c{c0.x + dn.x, c0.y + dn.y, c0.z + dn.z}; - if (visited.find(c) != visited.end()) {continue;} - visited.insert(c); - buckets[nd].push_back(c); - write_max(c, lut_logod[nd]); - } - ++expanded; - } - } - - // Fin de progreso (100 %) - if (progress) {progress(1.0f);} - - std::cout << "Inflation complete. Expanded " << expanded - << " nodes within R=" << R << ".\n"; - - return dst; -} - -inline bool is_ray_occluded( - const Bonxai::ProbabilisticMap & map, - const Bonxai::CoordT & key_origin, - const Bonxai::CoordT & key_end, - int tail_skip_voxels = 1) -{ - using CellT = Bonxai::ProbabilisticMap::CellT; - auto acc = map.grid().createConstAccessor(); - - // 1) Trazamos el rayo en coords discretas con la API real (vector) - std::vector ray; - Bonxai::ComputeRay(key_origin, key_end, ray); - - if (ray.empty()) {return false;} - - // 2) No miramos los últimos 'tail_skip_voxels' ni el propio voxel final - // (tolerancia a error de rango). Aseguramos límites válidos. - const int total = static_cast(ray.size()); - const int last_to_check = std::max(0, total - std::max(0, tail_skip_voxels) - 1); - - for (int i = 0; i < last_to_check; ++i) { - const Bonxai::CoordT & c = ray[i]; - const CellT * cell = acc.value(c); // ConstAccessor::value(const CoordT&) const - if (cell && cell->probability_log > map.options().occupancy_threshold_log) { - return true; // oclusión previa - } - } - return false; // sin impactos previos -} - - -void -AMCLLocalizer::correct(NavState & nav_state) -{ - auto t0 = get_node()->now(); - if (!nav_state.has("points")) { - RCLCPP_WARN(get_node()->get_logger(), "There is yet no points perceptions"); - return; - } - - const auto perceptions = nav_state.get("points"); - - if (!nav_state.has("map.bonxai")) { - RCLCPP_WARN(get_node()->get_logger(), "There is yet no a bonxai map"); - return; - } - - - if (!nav_state.has("map.bonxai.inflated")) { - auto start = get_node()->now(); - RCLCPP_INFO(get_node()->get_logger(), "Creating inflation map"); - - const auto original_map = nav_state.get_ptr("map.bonxai"); - - double stdev_inflation = 1.5; // metros; puedes exponerlo como parámetro ROS 2 - - auto logger = get_node()->get_logger(); - auto clock = get_node()->get_clock(); // SharedPtr - - auto progress_cb = [logger, clock](float f) { - // f en [0,1] - // En Humble/Foxy suele ser: RCLCPP_INFO_THROTTLE(logger, *clock, 2000, ...) - RCLCPP_INFO_THROTTLE( - logger, *clock, 2000, - "Inflating... %.1f%%", static_cast(f) * 100.0); - }; - - - std::shared_ptr inflated_map = - inflate_map(original_map, stdev_inflation, /*p_min=*/0.01, progress_cb); - - if (inflated_map == nullptr) { - RCLCPP_ERROR(get_node()->get_logger(), "Error creating bonxai inflated map"); - return; - } - - - nav_state.set("map.bonxai.inflated", inflated_map); - - auto end = get_node()->now(); - RCLCPP_INFO(get_node()->get_logger(), "Created in %lf seconds", (end - start).seconds()); - - } - - - const auto original_map = nav_state.get_ptr("map.bonxai"); - const auto map = nav_state.get_ptr("map.bonxai.inflated"); - - auto t1 = get_node()->now(); - - const auto & filtered = PointPerceptionsOpsView(perceptions) - .downsample(2.0) - .fuse(get_tf_prefix() + "base_footprint") - ->as_points(); - - auto t2 = get_node()->now(); - - if (filtered.empty()) { - RCLCPP_WARN(get_node()->get_logger(), "No points to correct"); - return; - } - - - auto t3 = get_node()->now(); - - size_t sidx = 0; - for (auto & particle : particles_) { - float hits = 0; - float possible_hits = 0; - for (const auto & pt : filtered) { - tf2::Vector3 pt_world = particle.pose * tf2::Vector3(pt.x, pt.y, pt.z); - - if (std::isnan(pt_world.x())) {continue;} - - possible_hits += 1.0f; - - const Bonxai::CoordT key_origin = original_map->grid().posToCoord( - {particle.pose.getOrigin().x(), particle.pose.getOrigin().y(), - particle.pose.getOrigin().z()}); - const Bonxai::CoordT key_end = original_map->grid().posToCoord( - {pt_world.x(), pt_world.y(), pt_world.z()}); - - const bool occluded = is_ray_occluded(*original_map, key_origin, key_end, - /*tail_skip_voxels=*/1); - - float p = 0.0f; - if (!occluded) { - p = map->queryProbability(key_end); - } else { - p = 0.0f; // rayo bloqueado antes del punto - } - - hits += p; - } - - particle.hits += hits; - particle.possible_hits += possible_hits; - } - auto t4 = get_node()->now(); - - const double tau = 0.7; // < 1 afila la distribución (más contraste) - - for (auto & particle : particles_) { - if (particle.possible_hits > 0) { - double mean_p = static_cast(particle.hits) / - static_cast(particle.possible_hits); - mean_p = std::clamp(mean_p, 1e-6, 1.0); // evitar 0 exacto - particle.weight = std::pow(mean_p, tau); - } else { - particle.weight = 1e-6; - } - } - -// Normalización - double total_weight = 0.0; - for (const auto & p : particles_) { - total_weight += p.weight; - } - if (total_weight > 0.0) { - for (auto & p : particles_) { - p.weight /= total_weight; - } - } - - auto t5 = get_node()->now(); - - std::cerr << "Prepare maps: " << std::fixed << std::setprecision(8) << (t1 - t0).seconds() << - std::endl; - std::cerr << "Prepare perception: " << std::fixed << std::setprecision(8) << - (t2 - t1).seconds() << std::endl; - std::cerr << "Core: " << std::fixed << std::setprecision(8) << (t4 - t3).seconds() << std::endl; - std::cerr << "Final: " << std::fixed << std::setprecision(8) << (t5 - t4).seconds() << std::endl; - - -} - -void -AMCLLocalizer::reseed() -{ - // 1) Comprobaciones tempranas - if (particles_.empty()) {return;} - if (particles_[0].possible_hits == 0) {return;} - - const std::size_t N = particles_.size(); - const std::size_t N_top = std::max(1, N / 2); - - // 2) Ordenar por peso (desc) - std::sort(particles_.begin(), particles_.end(), - [](const Particle & a, const Particle & b) { - return a.weight > b.weight; - }); - - std::cerr << - "=================================================================================\n"; - for (std::size_t i = 0; i < particles_.size(); i++) { - std::cerr << "[" << i << "] " << particles_[i].hits << "/" << particles_[i].possible_hits - << " " << static_cast(particles_[i].last_cid) << std::endl; - } - - - // 3) Estadísticos top-N - tf2::Vector3 mean = computeMean(particles_, 0, N_top); - tf2::Matrix3x3 cov = computeCovariance(particles_, 0, N_top, mean); - - double a = cov[0][0]; - double b = cov[0][1]; - double c = cov[1][1]; - - auto safe_cov_2x2 = [](double a_in, double b_in, double c_in) { - const double eps = 1e-9; - double a2 = std::isfinite(a_in) ? a_in : 0.0; - double b2 = std::isfinite(b_in) ? b_in : 0.0; - double c2 = std::isfinite(c_in) ? c_in : 0.0; - - if (a2 < eps) {a2 = eps;} - if (c2 < eps) {c2 = eps;} - - double det = a2 * c2 - b2 * b2; - if (det < 0.0) { - double max_b = std::sqrt(a2 * c2) - eps; - if (max_b < 0.0) {max_b = 0.0;} - if (b2 > max_b) {b2 = max_b;} - if (b2 < -max_b) {b2 = -max_b;} - } - return std::tuple(a2, b2, c2); - }; - - auto [a2, b2, c2] = safe_cov_2x2(a, b, c); - - double l00 = std::sqrt(std::max(a2, 0.0)); - double l10 = (l00 > 0.0) ? (b2 / l00) : 0.0; - double t = c2 - l10 * l10; - if (t < 0.0) {t = 0.0;} - double l11 = std::sqrt(t); - - double yaw_variance = computeYawVariance(particles_, 0, N_top); - if (!std::isfinite(yaw_variance) || yaw_variance < 0.0) {yaw_variance = 0.0;} - double yaw_stddev = std::sqrt(yaw_variance); - if (!std::isfinite(yaw_stddev) || yaw_stddev < min_noise_yaw_) {yaw_stddev = min_noise_yaw_;} - - std::normal_distribution yaw_noise(0.0, yaw_stddev); - - // Distribuciones auxiliares - std::normal_distribution n01(0.0, 1.0); - std::normal_distribution index_dist( - 0.0, std::max(1.0, 0.05 * static_cast(N))); - - // Muestreador de offset XY: usa rng_ capturado (sin parámetros) - auto sample_xy_offset = [&]() -> std::pair { - double z0 = n01(rng_), z1 = n01(rng_); - double dx = l00 * z0; - double dy = l10 * z0 + l11 * z1; - - if (!std::isfinite(dx) || !std::isfinite(dy)) { - std::normal_distribution nIso(0.0, min_noise_xy_); - return {nIso(rng_), nIso(rng_)}; - } - - double r = std::hypot(dx, dy); - if (!(r >= min_noise_xy_)) { - if (r < 1e-12) { - std::normal_distribution nIso(0.0, min_noise_xy_); - dx = nIso(rng_); - dy = nIso(rng_); - } else { - double s = (min_noise_xy_ / r); - dx *= s; dy *= s; - } - } - return {dx, dy}; - }; - - // 4) Reseed de la mitad inferior - for (std::size_t i = N_top; i < N; ++i) { - int selected_idx; - int guard = 0; - do { - double idx_sample = std::round(index_dist(rng_)); - selected_idx = static_cast(idx_sample); - if (selected_idx < 0) {selected_idx = -selected_idx;} - if (selected_idx >= static_cast(N_top)) {selected_idx = static_cast(N_top) - 1;} - if (selected_idx < 0) {selected_idx = 0;} - if (++guard > 16) {selected_idx = 0; break;} - } while (selected_idx < 0 || static_cast(selected_idx) >= N_top); - - const auto & ref_particle = particles_[static_cast(selected_idx)]; - tf2::Vector3 origin = ref_particle.pose.getOrigin(); - - auto [dx_off, dy_off] = sample_xy_offset(); - - double r_ref, p_ref, y_ref; - tf2::Matrix3x3(ref_particle.pose.getRotation()).getRPY(r_ref, p_ref, y_ref); - - double noisy_yaw = y_ref + yaw_noise(rng_); - if (!std::isfinite(noisy_yaw)) {noisy_yaw = y_ref;} - - tf2::Quaternion q; - q.setRPY(0.0, 0.0, noisy_yaw); - if (q.length2() < 1e-20 || !std::isfinite(q.x()) || !std::isfinite(q.y()) || - !std::isfinite(q.z()) || !std::isfinite(q.w())) - { - q.setRPY(0.0, 0.0, y_ref); - } - - tf2::Vector3 new_origin(origin.x() + dx_off, origin.y() + dy_off, 0.0); - if (!std::isfinite(new_origin.x()) || !std::isfinite(new_origin.y()) || - !std::isfinite(new_origin.z())) - { - new_origin = tf2::Vector3(origin.x(), origin.y(), 0.0); - } - - particles_[i].pose.setOrigin(new_origin); - particles_[i].pose.setRotation(q); - particles_[i].weight = ref_particle.weight; - } - - // 5) Reset contadores + normalización de pesos - for (auto & particle : particles_) { - particle.hits = 0; - particle.possible_hits = 0; - } - - double total_weight = 0.0; - for (const auto & p : particles_) { - if (std::isfinite(p.weight)) {total_weight += p.weight;} - } - - if (total_weight > 0.0 && std::isfinite(total_weight)) { - for (auto & p : particles_) { - p.weight /= total_weight; - if (!std::isfinite(p.weight) || p.weight < 0.0) {p.weight = 0.0;} - } - } else { - double w = 1.0 / static_cast(particles_.size()); - for (auto & p : particles_) { - p.weight = w; - } - } -} - - -static inline bool is_finite_tf(const tf2::Transform & T) -{ - const auto & o = T.getOrigin(); - const auto & q = T.getRotation(); - return std::isfinite(o.x()) && std::isfinite(o.y()) && std::isfinite(o.z()) && - std::isfinite(q.x()) && std::isfinite(q.y()) && std::isfinite(q.z()) && - std::isfinite(q.w()); -} - -void -AMCLLocalizer::publishTF(const tf2::Transform & map2bf) -{ - if (!is_finite_tf(map2bf)) { - RCLCPP_ERROR(get_node()->get_logger(), "publishTF: transform contiene NaN/inf; no se publica"); - return; - } - - geometry_msgs::msg::TransformStamped tf_msg; - tf_msg.header.stamp = get_node()->now(); - tf_msg.header.frame_id = get_tf_prefix() + "map"; - tf_msg.child_frame_id = get_tf_prefix() + "odom"; - tf_msg.transform = tf2::toMsg(map2bf); - - RTTFBuffer::getInstance()->setTransform(tf_msg, "easynav", false); - tf_broadcaster_->sendTransform(tf_msg); -} - -void -AMCLLocalizer::publishParticles() -{ - geometry_msgs::msg::PoseArray array_msg; - array_msg.header.stamp = get_node()->now(); - array_msg.header.frame_id = get_tf_prefix() + "map"; - - array_msg.poses.reserve(particles_.size()); - for (const auto & p : particles_) { - geometry_msgs::msg::Pose pose_msg; - pose_msg.position.x = p.pose.getOrigin().x(); - pose_msg.position.y = p.pose.getOrigin().y(); - pose_msg.position.z = p.pose.getOrigin().z(); - pose_msg.orientation.x = p.pose.getRotation().x(); - pose_msg.orientation.y = p.pose.getRotation().y(); - pose_msg.orientation.z = p.pose.getRotation().z(); - pose_msg.orientation.w = p.pose.getRotation().w(); - array_msg.poses.push_back(pose_msg); - } - - particles_pub_->publish(array_msg); -} - -tf2::Transform -AMCLLocalizer::getEstimatedPose() const -{ - if (particles_.empty()) { - return tf2::Transform::getIdentity(); - } - - const std::size_t N = particles_.size(); - const std::size_t N_top = N / 2; - - std::vector sorted_particles = particles_; - std::sort(sorted_particles.begin(), sorted_particles.end(), - [](const Particle & a, const Particle & b) { - return a.weight > b.weight; - }); - - tf2::Vector3 mean = computeMean(sorted_particles, 0, N_top); - if (!std::isfinite(mean.x()) || !std::isfinite(mean.y()) || !std::isfinite(mean.z())) { - mean = tf2::Vector3(0.0, 0.0, 0.0); - } - double roll, pitch, yaw; - tf2::Matrix3x3(sorted_particles[0].pose.getRotation()).getRPY(roll, pitch, yaw); - tf2::Quaternion q; - q.setRPY(0.0, 0.0, yaw); - - tf2::Transform est; - est.setOrigin(mean); - est.setRotation(q); - return est; -} - -void -AMCLLocalizer::publishEstimatedPose(const tf2::Transform & est_pose) -{ - if (particles_.empty()) {return;} - - const std::size_t N = particles_.size(); - const std::size_t N_top = N / 2; - - tf2::Vector3 mean = est_pose.getOrigin(); - tf2::Matrix3x3 cov = computeCovariance(particles_, 0, N_top, mean); - double yaw_variance = computeYawVariance(particles_, 0, N_top); - - geometry_msgs::msg::PoseWithCovarianceStamped msg; - msg.header.stamp = get_node()->now(); - msg.header.frame_id = get_tf_prefix() + "map"; - - msg.pose.pose.position.x = mean.x(); - msg.pose.pose.position.y = mean.y(); - msg.pose.pose.position.z = 0.0; - msg.pose.pose.orientation = tf2::toMsg(est_pose.getRotation()); - - for (int r = 0; r < 3; ++r) { - for (int c = 0; c < 3; ++c) { - msg.pose.covariance[6 * r + c] = cov[r][c]; - } - } - msg.pose.covariance[6 * 5 + 5] = yaw_variance; - - estimate_pub_->publish(msg); -} - -nav_msgs::msg::Odometry -AMCLLocalizer::get_pose() -{ - nav_msgs::msg::Odometry odom_msg; - - odom_msg.header.stamp = get_node()->now(); - odom_msg.header.frame_id = get_tf_prefix() + "map"; - odom_msg.child_frame_id = get_tf_prefix() + "base_footprint"; - - pose_ = getEstimatedPose(); - tf2::Transform est_pose = pose_; - - odom_msg.pose.pose.position.x = est_pose.getOrigin().x(); - odom_msg.pose.pose.position.y = est_pose.getOrigin().y(); - odom_msg.pose.pose.position.z = est_pose.getOrigin().z(); - odom_msg.pose.pose.orientation = tf2::toMsg(est_pose.getRotation()); - - if (!particles_.empty()) { - const std::size_t N = particles_.size(); - const std::size_t N_top = N / 2; - - std::vector sorted_particles = particles_; - std::sort(sorted_particles.begin(), sorted_particles.end(), - [](const Particle & a, const Particle & b) { - return a.weight > b.weight; - }); - - tf2::Vector3 mean = computeMean(sorted_particles, 0, N_top); - tf2::Matrix3x3 cov = computeCovariance(sorted_particles, 0, N_top, mean); - double yaw_var = computeYawVariance(sorted_particles, 0, N_top); - - for (int r = 0; r < 3; ++r) { - for (int c = 0; c < 3; ++c) { - odom_msg.pose.covariance[6 * r + c] = cov[r][c]; - } - } - - odom_msg.pose.covariance[6 * 5 + 5] = yaw_var; - } - - odom_msg.twist.twist.linear.x = 0.0; - odom_msg.twist.twist.linear.y = 0.0; - odom_msg.twist.twist.linear.z = 0.0; - odom_msg.twist.twist.angular.x = 0.0; - odom_msg.twist.twist.angular.y = 0.0; - odom_msg.twist.twist.angular.z = 0.0; - - return odom_msg; -} - -} // namespace navmap - -} // namespace easynav - -#include -PLUGINLIB_EXPORT_CLASS(easynav::navmap::AMCLLocalizer, easynav::LocalizerMethodBase) diff --git a/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer_copy_v3.cpp b/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer_copy_v3.cpp deleted file mode 100644 index 3c8f23f9..00000000 --- a/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer_copy_v3.cpp +++ /dev/null @@ -1,1070 +0,0 @@ -// Copyright 2025 Intelligent Robotics Lab -// -// This file is part of the project Easy Navigation (EasyNav in short) -// licensed under the GNU General Public License v3.0. -// See for details. -// -// Easy Navigation program is free software: you can redistribute it and/or modify -// it under the terms of the GNU General Public License as published by -// the Free Software Foundation, either version 3 of the License, or -// (at your option) any later version. -// -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. -// -// You should have received a copy of the GNU General Public License -// along with this program. If not, see . - -/// \file -/// \brief AMCLLocalizer implementation (Bonxai + probabilistic inflation + ray casting). - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -#include "bonxai/bonxai.hpp" -#include "bonxai/probabilistic_map.hpp" - -#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" -#include "tf2/LinearMath/Vector3.hpp" - -#include "easynav_common/RTTFBuffer.hpp" -#include "easynav_common/types/Perceptions.hpp" -#include "easynav_common/types/PointPerception.hpp" -#include "easynav_common/types/IMUPerception.hpp" - -#include "navmap_core/NavMap.hpp" - -#include "easynav_navmap_localizer/AMCLLocalizer.hpp" -#include "easynav_localizer/LocalizerNode.hpp" - -namespace easynav -{ -namespace navmap -{ - -static constexpr unsigned char NO_INFORMATION = 255; -static constexpr unsigned char LETHAL_OBSTACLE = 254; -static constexpr unsigned char INSCRIBED_INFLATED_OBSTACLE = 253; -static constexpr unsigned char MAX_NON_OBSTACLE = 252; -static constexpr unsigned char FREE_SPACE = 0; - -// ---------- math helpers ---------- -static inline double sqr(double x) {return x * x;} - -// ---------- stats over particles ---------- -tf2::Vector3 computeMean( - const std::vector & particles, std::size_t start, - std::size_t count) -{ - tf2::Vector3 weighted_sum(0.0, 0.0, 0.0); - double total_weight = 0.0; - if (start >= particles.size() || count == 0) {return weighted_sum;} - std::size_t end = std::min(start + count, particles.size()); - - for (std::size_t i = start; i < end; ++i) { - const Particle & p = particles[i]; - const tf2::Vector3 & origin = p.pose.getOrigin(); - if (!std::isfinite(origin.x()) || !std::isfinite(origin.y()) || !std::isfinite(origin.z())) { - continue; - } - weighted_sum += origin * p.weight; - total_weight += p.weight; - } - return (total_weight > 0.0) ? (weighted_sum / total_weight) : tf2::Vector3(0.0, 0.0, 0.0); -} - -tf2::Matrix3x3 computeCovariance( - const std::vector & particles, - std::size_t start, std::size_t count, - const tf2::Vector3 & mean) -{ - double total_weight = 0.0; - double cov[3][3] = {{0.0}}; - if (start >= particles.size() || count == 0) {return tf2::Matrix3x3();} - std::size_t end = std::min(start + count, particles.size()); - - for (std::size_t i = start; i < end; ++i) { - const Particle & p = particles[i]; - tf2::Vector3 d = p.pose.getOrigin() - mean; - double w = p.weight; total_weight += w; - cov[0][0] += w * d.x() * d.x(); - cov[0][1] += w * d.x() * d.y(); - cov[0][2] += w * d.x() * d.z(); - cov[1][1] += w * d.y() * d.y(); - cov[1][2] += w * d.y() * d.z(); - cov[2][2] += w * d.z() * d.z(); - } - if (total_weight > 0.0) { - cov[0][0] /= total_weight; cov[0][1] /= total_weight; cov[0][2] /= total_weight; - cov[1][1] /= total_weight; cov[1][2] /= total_weight; cov[2][2] /= total_weight; - cov[1][0] = cov[0][1]; cov[2][0] = cov[0][2]; cov[2][1] = cov[1][2]; - } - return tf2::Matrix3x3( - cov[0][0], cov[0][1], cov[0][2], - cov[1][0], cov[1][1], cov[1][2], - cov[2][0], cov[2][1], cov[2][2]); -} - -double extractYaw(const tf2::Transform & T) -{ - double r, p, y; tf2::Matrix3x3(T.getRotation()).getRPY(r, p, y); return y; -} - -double computeYawVariance( - const std::vector & particles, std::size_t start, - std::size_t count) -{ - double sum_cos = 0.0, sum_sin = 0.0, total_weight = 0.0; - if (start >= particles.size() || count == 0) {return 0.0;} - std::size_t end = std::min(start + count, particles.size()); - for (std::size_t i = start; i < end; ++i) { - const Particle & p = particles[i]; - double yaw = extractYaw(p.pose), w = p.weight; - sum_cos += w * std::cos(yaw); - sum_sin += w * std::sin(yaw); - total_weight += w; - } - if (total_weight == 0.0) {return 0.0;} - double mc = sum_cos / total_weight, ms = sum_sin / total_weight; - double R = std::sqrt(mc * mc + ms * ms); R = std::clamp(R, 1e-12, 1.0); - double var = -2.0 * std::log(R); if (!std::isfinite(var) || var < 0.0) {var = 0.0;} - return var; -} - -// ---------- utils ---------- -static inline tf2::Vector3 project_onto_plane(const tf2::Vector3 & v, const tf2::Vector3 & n_unit) -{ - return v - n_unit * v.dot(n_unit); -} - -static inline tf2::Quaternion frame_from_normal_and_yaw( - const tf2::Vector3 & n_world_unit, - double yaw_world) -{ - tf2::Vector3 x_tgt(std::cos(yaw_world), std::sin(yaw_world), 0.0); - tf2::Vector3 x_tan = project_onto_plane(x_tgt, n_world_unit); - double xn = x_tan.length(); if (xn < 1e-9) { - tf2::Quaternion q; q.setRPY(0, 0, yaw_world); return q; - } - x_tan /= xn; - tf2::Vector3 z_axis = n_world_unit; - tf2::Vector3 y_axis = z_axis.cross(x_tan); double yn = y_axis.length(); - if (yn < 1e-9) {tf2::Quaternion q; q.setRPY(0, 0, yaw_world); return q;} - y_axis /= yn; - tf2::Matrix3x3 R(x_tan.x(), y_axis.x(), z_axis.x(), - x_tan.y(), y_axis.y(), z_axis.y(), - x_tan.z(), y_axis.z(), z_axis.z()); - tf2::Quaternion q; R.getRotation(q); return q; -} - -static inline tf2::Vector3 to_tf(const Eigen::Vector3f & v) -{ - return {static_cast(v.x()), static_cast(v.y()), static_cast(v.z())}; -} - -static inline top_count -// ===================== probabilistic inflate (brushfire) ===================== -using ProgressCallback = std::function; - -std::shared_ptr -inflate_map( - const std::shared_ptr & src, - double sigma, double p_min, ProgressCallback progress = nullptr) -{ - using namespace Bonxai; - if (!src || sigma <= 0.0) {return nullptr;} - - const double res = src->grid().voxelSize(); - auto dst = std::make_shared(res); - dst->setOptions(src->options()); - - std::vector seeds; src->getOccupiedVoxels(seeds); - if (seeds.empty()) {return dst;} - - const int R = static_cast(std::ceil(3.0 * sigma / res)); - if (R <= 0) { - auto acc = dst->grid().createAccessor(); - for (const auto & c : seeds) { - auto * cell = acc.value(c, true); - cell->probability_log = dst->options().clamp_max_log; - cell->update_id = 0; - } - return dst; - } - - const double inv_2sig2 = 1.0 / (2.0 * sigma * sigma); - std::vector lut_logod(R + 1); - for (int d = 0; d <= R; ++d) { - const double dist_m = d * res; - const double p = std::exp(-(dist_m * dist_m) * inv_2sig2); - lut_logod[d] = (p >= p_min) ? ProbabilisticMap::logods(static_cast(p)) : - std::numeric_limits::min(); - } - - std::vector> buckets(R + 1); - buckets[0] = seeds; - - struct KeyHash { size_t operator()(const CoordT & c) const noexcept - { - return (size_t)c.x * 73856093u ^ (size_t)c.y * 19349663u ^ (size_t)c.z * 83492791u; - } - }; - struct KeyEq { bool operator()(const CoordT & a, const CoordT & b) const noexcept - { - return a.x == b.x && a.y == b.y && a.z == b.z; - } - }; - - std::unordered_set vis; vis.reserve(seeds.size() * 4); - for (const auto & c : seeds) { - vis.insert(c); - } - - auto acc = dst->grid().createAccessor(); - const auto clamp_min = dst->options().clamp_min_log; - const auto clamp_max = dst->options().clamp_max_log; - - auto write_max = [&](const CoordT & c, int32_t v){ - if (v == std::numeric_limits::min()) {return;} - auto * cell = acc.value(c, true); - int32_t prop = std::max(cell->probability_log, v); - if (prop < clamp_min) {prop = clamp_min;} - if (prop > clamp_max) {prop = clamp_max;} - cell->probability_log = prop; - cell->update_id = 0; - }; - - for (const auto & c : seeds) { - write_max(c, lut_logod[0]); - } - - static const CoordT N6[6] = {{1, 0, 0}, {-1, 0, 0}, {0, 1, 0}, {0, -1, 0}, {0, 0, 1}, {0, 0, -1}}; - for (int d = 0; d < R; ++d) { - if (progress) {progress(static_cast(d) / static_cast(R));} - const int nd = d + 1; - for (const CoordT & c0 : buckets[d]) { - for (const CoordT & dn : N6) { - CoordT c{c0.x + dn.x, c0.y + dn.y, c0.z + dn.z}; - if (vis.find(c) != vis.end()) {continue;} - vis.insert(c); - buckets[nd].push_back(c); - write_max(c, lut_logod[nd]); - } - } - } - if (progress) {progress(1.0f);} - return dst; -} - -// ===================== ray cast (DDA, no-alloc) ===================== -namespace -{ - -inline bool ray_occluded_dda( - const Bonxai::ProbabilisticMap & occ_map, - const Bonxai::VoxelGrid::ConstAccessor & acc, - const Bonxai::CoordT & a, const Bonxai::CoordT & b, - int tail_skip_voxels) -{ - using Bonxai::CoordT; - using CellT = Bonxai::ProbabilisticMap::CellT; - - int x = a.x, y = a.y, z = a.z; - const int xe = b.x, ye = b.y, ze = b.z; - - const int dx = std::abs(xe - x); - const int dy = std::abs(ye - y); - const int dz = std::abs(ze - z); - - const int sx = (xe >= x) ? 1 : -1; - const int sy = (ye >= y) ? 1 : -1; - const int sz = (ze >= z) ? 1 : -1; - - int nsteps = std::max({dx, dy, dz}); - if (nsteps <= 0) {return false;} - - int ex = (dx >= dy && dx >= dz) ? dx / 2 : (dy >= dz ? dy / 2 : dz / 2); - int ey = ex, ez = ex; - - for (int step = 0; step < nsteps; ++step) { - if (step < nsteps - std::max(0, tail_skip_voxels) - 1) { - const CellT * cell = acc.value(CoordT{x, y, z}); - if (cell && cell->probability_log > occ_map.options().occupancy_threshold_log) { - return true; - } - } - if (dx >= dy && dx >= dz) { - x += sx; ey += dy; ez += dz; if (ey >= dx) {y += sy; ey -= dx;} if (ez >= dx) { - z += sz; ez -= dx; - } - } else if (dy >= dx && dy >= dz) { - y += sy; ex += dx; ez += dz; if (ex >= dy) {x += sx; ex -= dy;} if (ez >= dy) { - z += sz; ez -= dy; - } - } else { - z += sz; ex += dx; ey += dy; if (ex >= dz) {x += sx; ex -= dz;} if (ey >= dz) { - y += sy; ey -= dz; - } - } - } - return false; -} - -inline int compute_tail_skip(const Bonxai::ProbabilisticMap & map, double sigma, double p_min) -{ - const double res = map.grid().voxelSize(); - p_min = std::clamp(p_min, 1e-6, 0.999999); - const double r_allow = sigma * std::sqrt(-2.0 * std::log(p_min)); - return std::max(1, static_cast(std::ceil(r_allow / res))); -} - -struct CoordHash -{ - size_t operator()(const Bonxai::CoordT & c) const noexcept - { - return (size_t)c.x * 73856093u ^ (size_t)c.y * 19349663u ^ (size_t)c.z * 83492791u; - } -}; -struct CoordEq -{ - bool operator()(const Bonxai::CoordT & a, const Bonxai::CoordT & b) const noexcept - { - return a.x == b.x && a.y == b.y && a.z == b.z; - } -}; - -} // namespace - -// ---------- AMCLLocalizer ---------- -AMCLLocalizer::AMCLLocalizer() -{ - NavState::register_printer( - [](const nav_msgs::msg::Odometry & odom) { - std::ostringstream ret; - tf2::Quaternion q(odom.pose.pose.orientation.x, odom.pose.pose.orientation.y, - odom.pose.pose.orientation.z, odom.pose.pose.orientation.w); - double r, p, y; tf2::Matrix3x3(q).getRPY(r, p, y); - ret << "Odometry with pose: (x: " << odom.pose.pose.position.x - << ", y: " << odom.pose.pose.position.y << ", yaw: " << y << ")"; - return ret.str(); - }); -} - -AMCLLocalizer::~AMCLLocalizer() = default; - -std::expected AMCLLocalizer::on_initialize() -{ - auto node = get_node(); - const auto & plugin_name = get_plugin_name(); - - int num_particles; - double x_init, y_init, yaw_init, std_dev_xy, std_dev_yaw; - std::string perception_model; - - node->declare_parameter(plugin_name + ".num_particles", 100); - node->declare_parameter(plugin_name + ".initial_pose.x", 0.0); - node->declare_parameter(plugin_name + ".initial_pose.y", 0.0); - node->declare_parameter(plugin_name + ".initial_pose.yaw", 0.0); - node->declare_parameter(plugin_name + ".initial_pose.std_dev_xy", 0.5); - node->declare_parameter(plugin_name + ".initial_pose.std_dev_yaw", 0.5); - node->declare_parameter(plugin_name + ".reseed_freq", 1.0); - node->declare_parameter(plugin_name + ".noise_translation", 0.01); - node->declare_parameter(plugin_name + ".noise_rotation", 0.01); - node->declare_parameter(plugin_name + ".noise_translation_to_rotation", 0.01); - node->declare_parameter(plugin_name + ".min_noise_xy", 0.05); - node->declare_parameter(plugin_name + ".min_noise_yaw", 0.05); - node->declare_parameter(plugin_name + ".compute_odom_from_tf", false); - node->declare_parameter(plugin_name + ".perception_model", "occupancy"); - node->declare_parameter(plugin_name + ".top_keep_fraction", 0.2); - - // New descriptive params - node->declare_parameter(plugin_name + ".inflation_stddev", 1.5); - node->declare_parameter(plugin_name + ".inflation_prob_min", 0.01); - node->declare_parameter(plugin_name + ".correct_max_points", 1500); - node->declare_parameter(plugin_name + ".weights_tau", 0.7); - - node->get_parameter(plugin_name + ".num_particles", num_particles); - node->get_parameter(plugin_name + ".initial_pose.x", x_init); - node->get_parameter(plugin_name + ".initial_pose.y", y_init); - node->get_parameter(plugin_name + ".initial_pose.yaw", yaw_init); - node->get_parameter(plugin_name + ".initial_pose.std_dev_xy", std_dev_xy); - node->get_parameter(plugin_name + ".initial_pose.std_dev_yaw", std_dev_yaw); - node->get_parameter(plugin_name + ".noise_translation", noise_translation_); - node->get_parameter(plugin_name + ".noise_rotation", noise_rotation_); - node->get_parameter(plugin_name + ".noise_translation_to_rotation", - noise_translation_to_rotation_); - node->get_parameter(plugin_name + ".min_noise_xy", min_noise_xy_); - node->get_parameter(plugin_name + ".min_noise_yaw", min_noise_yaw_); - node->get_parameter(plugin_name + ".compute_odom_from_tf", compute_odom_from_tf_); - node->get_parameter(plugin_name + ".perception_model", perception_model); - - node->get_parameter(plugin_name + ".inflation_stddev", inflation_stddev_); - node->get_parameter(plugin_name + ".inflation_prob_min", inflation_prob_min_); - int tmp_max_pts = 0; - node->get_parameter(plugin_name + ".correct_max_points", tmp_max_pts); - correct_max_points_ = tmp_max_pts > 0 ? static_cast(tmp_max_pts) : std::size_t{1500}; - node->get_parameter(plugin_name + ".weights_tau", weights_tau_); - node->get_parameter(plugin_name + ".top_keep_fraction", top_keep_fraction_); - - if (!std::isfinite(top_keep_fraction_) || top_keep_fraction_ <= 0.0) {top_keep_fraction_ = 0.2;} - if (top_keep_fraction_ > 1.0) {top_keep_fraction_ = 1.0;} - - double reseed_freq; - node->get_parameter(plugin_name + ".reseed_freq", reseed_freq); - reseed_time_ = 1.0 / reseed_freq; - - RCLCPP_INFO(node->get_logger(), "Initialized AMCL with %d particles", num_particles); - RCLCPP_INFO(node->get_logger(), "Init pose (%.3f, %.3f, yaw=%.3f) std_xy=%.3f std_yaw=%.3f", - x_init, y_init, yaw_init, std_dev_xy, std_dev_yaw); - - std::normal_distribution noise_x(x_init, std_dev_xy); - std::normal_distribution noise_y(y_init, std_dev_xy); - std::normal_distribution noise_yaw(yaw_init, std_dev_yaw); - - particles_.resize(num_particles); - for (auto & p : particles_) { - tf2::Quaternion q; q.setRPY(0.0, 0.0, noise_yaw(rng_)); - p.pose.setRotation(q); - p.pose.setOrigin(tf2::Vector3(noise_x(rng_), noise_y(rng_), 0.0)); - p.hits = 0; p.possible_hits = 0; p.weight = 1.0 / num_particles; - } - - tf_broadcaster_ = std::make_unique(get_node()); - - auto node_typed = std::dynamic_pointer_cast(get_node()); - auto rt_cbg = node_typed->get_real_time_cbg(); - rclcpp::SubscriptionOptions options; options.callback_group = rt_cbg; - - if (!compute_odom_from_tf_) { - odom_sub_ = get_node()->create_subscription( - "odom", rclcpp::SensorDataQoS().reliable(), - std::bind(&AMCLLocalizer::odom_callback, this, std::placeholders::_1), options); - } - - particles_pub_ = get_node()->create_publisher( - node->get_fully_qualified_name() + std::string("/") + plugin_name + "/particles", 10); - estimate_pub_ = get_node()->create_publisher( - node->get_fully_qualified_name() + std::string("/") + plugin_name + "/pose", 10); - - last_reseed_ = get_node()->now(); - get_node()->get_logger().set_level(rclcpp::Logger::Level::Debug); - return {}; -} - -void AMCLLocalizer::odom_callback(nav_msgs::msg::Odometry::UniquePtr msg) -{ - if (compute_odom_from_tf_) {return;} - tf2::fromMsg(msg->pose.pose, odom_); - if (!initialized_odom_) {last_odom_ = odom_; initialized_odom_ = true;} -} - -void AMCLLocalizer::update_odom_from_tf() -{ - geometry_msgs::msg::TransformStamped tf_msg; - try { - tf_msg = RTTFBuffer::getInstance()->lookupTransform( - "odom", "base_footprint", tf2::TimePointZero, tf2::durationFromSec(0.0)); - } catch (const tf2::TransformException & ex) { - RCLCPP_WARN(get_node()->get_logger(), "TF failed: %s", ex.what()); - return; - } - tf2::fromMsg(tf_msg.transform, odom_); - initialized_odom_ = true; -} - -std::optional get_latest_imu_quat(const NavState & nav_state) -{ - if (!nav_state.has("imu")) {return std::nullopt;} - const auto imus = nav_state.get("imu"); - if (imus.empty() || !imus.back()) {return std::nullopt;} - const auto & imu_msg = imus.back()->data; - tf2::Quaternion q(imu_msg.orientation.x, imu_msg.orientation.y, - imu_msg.orientation.z, imu_msg.orientation.w); - if (q.length2() < 1e-12) {return std::nullopt;} - q.normalize(); - return q; -} - -void AMCLLocalizer::update_rt(NavState & nav_state) -{ - predict(nav_state); - nav_state.set("robot_pose", get_pose()); -} - -void AMCLLocalizer::update(NavState & nav_state) -{ - correct(nav_state); - if ((get_node()->now() - last_reseed_).seconds() > reseed_time_) { - reseed(); last_reseed_ = get_node()->now(); - } - nav_state.set("robot_pose", get_pose()); - publishParticles(); -} - -void AMCLLocalizer::predict(NavState & nav_state) -{ - if (!initialized_odom_) {if (compute_odom_from_tf_) {update_odom_from_tf();} return;} - if (compute_odom_from_tf_) {update_odom_from_tf();} - - tf2::Transform delta = last_odom_.inverseTimes(odom_); - const bool have_navmap = nav_state.has("map.navmap"); - if (have_navmap) {navmap_ = nav_state.get<::navmap::NavMap>("map.navmap");} - - const auto imu_q_opt = get_latest_imu_quat(nav_state); - - tf2::Vector3 t = delta.getOrigin(); - double dx = t.x(), dy = t.y(), dz = t.z(); - double trans_len = std::sqrt(dx * dx + dy * dy + dz * dz); - - double r, p, yaw; tf2::Matrix3x3(delta.getRotation()).getRPY(r, p, yaw); - double rot_len = std::abs(yaw); - - std::random_device rd; std::mt19937 gen(rd()); - - for (auto & p : particles_) { - std::normal_distribution n_dx(0.0, std::abs(dx) * noise_translation_); - std::normal_distribution n_dy(0.0, std::abs(dy) * noise_translation_); - std::normal_distribution n_dz(0.0, std::abs(dz) * noise_translation_); - std::normal_distribution n_yaw(0.0, - rot_len * noise_rotation_ + trans_len * noise_translation_to_rotation_); - - tf2::Vector3 noisy_t(dx + n_dx(gen), dy + n_dy(gen), dz + n_dz(gen)); - double noisy_y = yaw + n_yaw(gen); - tf2::Quaternion noisy_q; noisy_q.setRPY(0.0, 0.0, noisy_y); - p.pose = p.pose * tf2::Transform(noisy_q, noisy_t); - - if (have_navmap) { - ::navmap::NavMap::LocateOpts opts; - if (p.last_cid != std::numeric_limits::max()) { - opts.hint_cid = p.last_cid; opts.hint_surface = p.last_surface; - } - opts.planar_eps = 1e-4f; opts.height_eps = 0.50f; opts.use_downward_ray = true; - - tf2::Vector3 Pw = p.pose.getOrigin(); - std::size_t sidx = 0; ::navmap::NavCelId cid = std::numeric_limits::max(); - Eigen::Vector3f bary, hit_eig; - - const bool ok = navmap_.locate_navcel( - Eigen::Vector3f(static_cast(Pw.x()), static_cast(Pw.y()), - static_cast(Pw.z() + 0.5f)), - sidx, cid, bary, &hit_eig, opts); - - if (ok) { - tf2::Vector3 hit = to_tf(hit_eig); - p.pose.setOrigin(tf2::Vector3(Pw.x(), Pw.y(), hit.z())); - if (imu_q_opt.has_value()) { - p.pose.setRotation(*imu_q_opt); - } else { - const auto & cel = navmap_.navcels[cid]; - tf2::Vector3 n_world = to_tf(cel.normal); - double nlen = n_world.length(); - if (nlen > 1e-9) { - n_world /= nlen; - double rr, pp, yy; tf2::Matrix3x3(p.pose.getRotation()).getRPY(rr, pp, yy); - p.pose.setRotation(frame_from_normal_and_yaw(n_world, yy)); - } - } - p.last_cid = cid; p.last_surface = sidx; - } else { - p.pose.setOrigin(tf2::Vector3(Pw.x(), Pw.y(), Pw.z())); - if (imu_q_opt.has_value()) {p.pose.setRotation(*imu_q_opt);} - } - } - } - - last_odom_ = odom_; - pose_ = getEstimatedPose(); - tf2::Transform map2bf = pose_; - tf2::Transform map2odom = map2bf * odom_.inverse(); - publishTF(map2odom); - publishEstimatedPose(map2bf); -} - -// ---------- perception scoring helpers ---------- -struct SensorBundle -{ - std::string frame_id; - pcl::PointCloud cloud; // in sensor frame -}; - -static inline std::string get_frame_id_from(const PointPerception & pp) -{ - // Prefer pcl header frame_id; fallback to pp.frame_id if available - if (!pp.data.header.frame_id.empty()) {return pp.data.header.frame_id;} - // If your PointPerception has a member 'frame_id', use it (else this stays empty) - return std::string(); -} - -static SensorBundle make_sensor_bundle_downsampled( - const PointPerception & pp, double ds_vox, std::size_t cap_points) -{ - SensorBundle out; - out.frame_id = get_frame_id_from(pp); - const auto & in = pp.data; - - if (in.empty()) {out.cloud = pcl::PointCloud(); return out;} - - // Simple stride-based cap after a coarse voxel ds on upstream pipe if you have it. - out.cloud.header = in.header; - out.cloud.is_dense = in.is_dense; - - const std::size_t N = in.points.size(); - if (cap_points == 0 || N <= cap_points) { - out.cloud = in; // copy - } else { - out.cloud.points.reserve(cap_points); - const std::size_t stride = std::max(1, N / cap_points); - for (std::size_t i = 0; i < N && out.cloud.points.size() < cap_points; i += stride) { - out.cloud.points.push_back(in.points[i]); - } - out.cloud.width = static_cast(out.cloud.points.size()); - out.cloud.height = 1; - } - (void)ds_vox; // left here if you want to add a true voxel filter later - return out; -} - -static inline tf2::Transform lookup_bf_to_sensor(const std::string & sensor_frame) -{ - if (sensor_frame.empty()) {return tf2::Transform::getIdentity();} - geometry_msgs::msg::TransformStamped tf_msg = - RTTFBuffer::getInstance()->lookupTransform( - "base_footprint", sensor_frame, tf2::TimePointZero, tf2::durationFromSec(0.0)); - tf2::Transform T; tf2::fromMsg(tf_msg.transform, T); - return T; -} - -struct ScoreAgg { float hits = 0.0f; float possible = 0.0f; }; - -static ScoreAgg score_particle_sensor_cloud( - const tf2::Transform & particle_pose, - const pcl::PointCloud & cloud_sensor, - const tf2::Transform & T_bf_sensor, - const Bonxai::ProbabilisticMap & occ_map, - const Bonxai::ProbabilisticMap & inf_map, - int tail_skip) -{ - using Bonxai::CoordT; - ScoreAgg s; - if (cloud_sensor.empty()) {return s;} - - auto acc_occ = occ_map.grid().createConstAccessor(); - s.possible = static_cast(cloud_sensor.points.size()); - - tf2::Transform world_T_sensor = particle_pose * T_bf_sensor; - const tf2::Vector3 origin_w = world_T_sensor.getOrigin(); - const CoordT key_origin = occ_map.grid().posToCoord({origin_w.x(), origin_w.y(), origin_w.z()}); - - std::unordered_map end_counts; - end_counts.reserve(cloud_sensor.points.size()); - - for (const auto & pt : cloud_sensor.points) { - tf2::Vector3 pw = world_T_sensor * tf2::Vector3(pt.x, pt.y, pt.z); - if (!std::isfinite(pw.x())) {continue;} - const CoordT key_end = occ_map.grid().posToCoord({pw.x(), pw.y(), pw.z()}); - ++end_counts[key_end]; - } - - for (const auto & kv : end_counts) { - const CoordT & key_end = kv.first; - const int mult = kv.second; - - // Endpoint probability in inflated map - float p_end = inf_map.queryProbability(key_end); - - // Gate: if very low, do not spend on ray (keep small contribution) - constexpr float P_RAY_MIN = 0.2f; - if (p_end < P_RAY_MIN) { - s.hits += p_end * mult; - continue; - } - - // Confirm with free ray up to near-end - const bool blocked = ray_occluded_dda(occ_map, acc_occ, key_origin, key_end, tail_skip); - s.hits += (blocked ? 0.0f : p_end) * mult; - } - - return s; -} - -// ---------- correct() ---------- -void AMCLLocalizer::correct(NavState & nav_state) -{ - auto t0 = get_node()->now(); - if (!nav_state.has("points")) { - RCLCPP_WARN(get_node()->get_logger(), "No points perceptions yet"); - return; - } - const auto perceptions = nav_state.get("points"); - - if (!nav_state.has("map.bonxai")) { - RCLCPP_WARN(get_node()->get_logger(), "No Bonxai map yet"); - return; - } - - // Build inflated map once and cache in NavState - if (!nav_state.has("map.bonxai.inflated")) { - const auto original_map = nav_state.get_ptr("map.bonxai"); - auto logger = get_node()->get_logger(); - auto clock = get_node()->get_clock(); - auto progress_cb = [logger, clock](float f) { - RCLCPP_INFO_THROTTLE(logger, *clock, 2000, "Inflating... %.1f%%", - static_cast(f) * 100.0); - }; - std::shared_ptr inflated_map = - inflate_map(original_map, inflation_stddev_, inflation_prob_min_, progress_cb); - if (!inflated_map) { - RCLCPP_ERROR(get_node()->get_logger(), "Inflated map creation failed"); - return; - } - nav_state.set("map.bonxai.inflated", inflated_map); - } - - const auto original_map = nav_state.get_ptr("map.bonxai"); - const auto inflated_map = nav_state.get_ptr("map.bonxai.inflated"); - - // Compute tail-skip coherent with inflation kernel - const int TAIL_SKIP = compute_tail_skip(*original_map, inflation_stddev_, inflation_prob_min_); - - auto t1 = get_node()->now(); - - // Prepare per-sensor bundles (downsample/cap per sensor) - const double vox = inflated_map->grid().voxelSize(); - const double ds_vox = std::max(0.5, 2.0 * vox); - std::vector bundles; bundles.reserve(perceptions.size()); - for (const auto & h : perceptions) { - if (!h) {continue;} - bundles.emplace_back(make_sensor_bundle_downsampled(*h, ds_vox, correct_max_points_)); - } - - // Resolve base_footprint -> sensor transforms (cache per frame_id) - std::unordered_map T_bf_sensor_cache; - T_bf_sensor_cache.reserve(bundles.size()); - for (const auto & b : bundles) { - if (!T_bf_sensor_cache.count(b.frame_id)) { - try { - T_bf_sensor_cache[b.frame_id] = lookup_bf_to_sensor(b.frame_id); - } catch (const tf2::TransformException & ex) { - RCLCPP_WARN(get_node()->get_logger(), "TF bf->%s failed: %s", b.frame_id.c_str(), - ex.what()); - T_bf_sensor_cache[b.frame_id] = tf2::Transform::getIdentity(); - } - } - } - - auto t2 = get_node()->now(); - - // Score all particles against all sensors - for (auto & particle : particles_) { - float hits = 0.0f, possible = 0.0f; - for (const auto & b : bundles) { - if (b.cloud.empty()) {continue;} - const tf2::Transform & T_bf_sensor = T_bf_sensor_cache[b.frame_id]; - ScoreAgg s = score_particle_sensor_cloud( - particle.pose, b.cloud, T_bf_sensor, - *original_map, *inflated_map, TAIL_SKIP); - hits += s.hits; possible += s.possible; - } - particle.hits += hits; - particle.possible_hits += possible; - } - - auto t3 = get_node()->now(); - - // Weights update with power tau - for (auto & particle : particles_) { - if (particle.possible_hits > 0) { - double mean_p = static_cast(particle.hits) / - static_cast(particle.possible_hits); - mean_p = std::clamp(mean_p, 1e-6, 1.0); - particle.weight = std::pow(mean_p, weights_tau_); - } else { - particle.weight = 1e-6; - } - } - double total_w = 0.0; - for (const auto & p : particles_) { - total_w += p.weight; - } - if (total_w > 0.0) { - for (auto & p : particles_) { - p.weight /= total_w; - } - } - - auto t4 = get_node()->now(); - - // Light profiling - std::cerr << "Prepare maps: " << std::fixed << std::setprecision(6) << (t1 - t0).seconds() << - " s\n"; - std::cerr << "Prep sensors: " << std::fixed << std::setprecision(6) << (t2 - t1).seconds() << - " s\n"; - std::cerr << "Scoring: " << std::fixed << std::setprecision(6) << (t3 - t2).seconds() << - " s\n"; - std::cerr << "Weights: " << std::fixed << std::setprecision(6) << (t4 - t3).seconds() << - " s\n"; -} - -void AMCLLocalizer::reseed() -{ - if (particles_.empty()) {return;} - if (particles_[0].possible_hits == 0) {return;} - - const std::size_t N = particles_.size(); - const std::size_t N_top = std::max(1, N / 2); - - std::sort(particles_.begin(), particles_.end(), - [](const Particle & a, const Particle & b){return a.weight > b.weight;}); - - std::cerr << - "=================================================================================\n"; - for (std::size_t i = 0; i < particles_.size(); i++) { - std::cerr << "[" << i << "] " << particles_[i].hits << "/" << particles_[i].possible_hits - << " " << static_cast(particles_[i].last_cid) << std::endl; - } - - - tf2::Vector3 mean = computeMean(particles_, 0, N_top); - tf2::Matrix3x3 cov = computeCovariance(particles_, 0, N_top, mean); - - double a = cov[0][0], b = cov[0][1], c = cov[1][1]; - auto safe_cov_2x2 = [](double A, double B, double C){ - const double eps = 1e-9; double a2 = A, b2 = B, c2 = C; - if (!std::isfinite(a2) || a2 < eps) {a2 = eps;} - if (!std::isfinite(c2) || c2 < eps) {c2 = eps;} - if (!std::isfinite(b2)) {b2 = 0.0;} - double det = a2 * c2 - b2 * b2; - if (det < 0.0) { - double max_b = std::sqrt(a2 * c2) - eps; - if (max_b < 0.0) {max_b = 0.0;} - if (b2 > max_b) {b2 = max_b;} - if (b2 < -max_b) {b2 = -max_b;} - } - return std::tuple(a2, b2, c2); - }; - auto [a2, b2, c2] = safe_cov_2x2(a, b, c); - - double l00 = std::sqrt(std::max(a2, 0.0)); - double l10 = (l00 > 0.0) ? (b2 / l00) : 0.0; - double t = c2 - l10 * l10; if (t < 0.0) {t = 0.0;} - double l11 = std::sqrt(t); - - double yaw_var = computeYawVariance(particles_, 0, N_top); - if (!std::isfinite(yaw_var) || yaw_var < 0.0) {yaw_var = 0.0;} - double yaw_std = std::sqrt(yaw_var); if (!std::isfinite(yaw_std) || yaw_std < min_noise_yaw_) { - yaw_std = min_noise_yaw_; - } - - std::normal_distribution yaw_noise(0.0, yaw_std); - std::normal_distribution n01(0.0, 1.0); - std::normal_distribution index_dist(0.0, std::max(1.0, 0.05 * static_cast(N))); - - auto sample_xy_offset = [&]() -> std::pair { - double z0 = n01(rng_), z1 = n01(rng_); - double dx = l00 * z0, dy = l10 * z0 + l11 * z1; - if (!std::isfinite(dx) || !std::isfinite(dy)) { - std::normal_distribution nIso(0.0, min_noise_xy_); - return {nIso(rng_), nIso(rng_)}; - } - double r = std::hypot(dx, dy); - if (!(r >= min_noise_xy_)) { - if (r < 1e-12) { - std::normal_distribution nIso(0.0, min_noise_xy_); dx = nIso(rng_); - dy = nIso(rng_); - } else {double s = (min_noise_xy_ / r); dx *= s; dy *= s;} - } - return {dx, dy}; - }; - - for (std::size_t i = N_top; i < N; ++i) { - int selected_idx; int guard = 0; - do { - double idx_sample = std::round(index_dist(rng_)); - selected_idx = static_cast(idx_sample); - if (selected_idx < 0) {selected_idx = -selected_idx;} - if (selected_idx >= static_cast(N_top)) {selected_idx = static_cast(N_top) - 1;} - if (++guard > 16) {selected_idx = 0; break;} - } while (selected_idx < 0 || static_cast(selected_idx) >= N_top); - - const auto & ref = particles_[static_cast(selected_idx)]; - tf2::Vector3 origin = ref.pose.getOrigin(); - auto [dx_off, dy_off] = sample_xy_offset(); - - double rr, pp, yy; tf2::Matrix3x3(ref.pose.getRotation()).getRPY(rr, pp, yy); - double noisy_yaw = yy + yaw_noise(rng_); if (!std::isfinite(noisy_yaw)) {noisy_yaw = yy;} - tf2::Quaternion q; q.setRPY(0.0, 0.0, noisy_yaw); - if (q.length2() < 1e-20 || !std::isfinite(q.x()) || !std::isfinite(q.y()) || - !std::isfinite(q.z()) || !std::isfinite(q.w())) {q.setRPY(0.0, 0.0, yy);} - - tf2::Vector3 new_origin(origin.x() + dx_off, origin.y() + dy_off, origin.z()); - if (!std::isfinite(new_origin.x()) || !std::isfinite(new_origin.y()) || - !std::isfinite(new_origin.z())) - { - new_origin = tf2::Vector3(origin.x(), origin.y(), origin.z()); - } - - particles_[i].pose.setOrigin(new_origin); - particles_[i].pose.setRotation(q); - particles_[i].weight = ref.weight; - } - - for (auto & p : particles_) { - p.hits = 0; p.possible_hits = 0; - } - double tw = 0.0; for (const auto & p : particles_) { - if (std::isfinite(p.weight)) { - tw += p.weight; - } - } - if (tw > 0.0 && std::isfinite(tw)) { - for (auto & p : particles_) { - p.weight /= tw; if (!std::isfinite(p.weight) || p.weight < 0.0) { - p.weight = 0.0; - } - } - } else { - double w = 1.0 / static_cast(particles_.size()); - for (auto & p : particles_) { - p.weight = w; - } - } -} - -static inline bool is_finite_tf(const tf2::Transform & T) -{ - const auto & o = T.getOrigin(); const auto & q = T.getRotation(); - return std::isfinite(o.x()) && std::isfinite(o.y()) && std::isfinite(o.z()) && - std::isfinite(q.x()) && std::isfinite(q.y()) && std::isfinite(q.z()) && - std::isfinite(q.w()); -} - -void AMCLLocalizer::publishTF(const tf2::Transform & map2bf) -{ - if (!is_finite_tf(map2bf)) { - RCLCPP_ERROR(get_node()->get_logger(), "publishTF: invalid transform"); - return; - } - geometry_msgs::msg::TransformStamped tf_msg; - tf_msg.header.stamp = get_node()->now(); - tf_msg.header.frame_id = get_tf_prefix() + "map"; - tf_msg.child_frame_id = get_tf_prefix() + "odom"; - tf_msg.transform = tf2::toMsg(map2bf); - RTTFBuffer::getInstance()->setTransform(tf_msg, "easynav", false); - tf_broadcaster_->sendTransform(tf_msg); -} - -void AMCLLocalizer::publishParticles() -{ - geometry_msgs::msg::PoseArray array_msg; - array_msg.header.stamp = get_node()->now(); - array_msg.header.frame_id = get_tf_prefix() + "map"; - array_msg.poses.reserve(particles_.size()); - for (const auto & p : particles_) { - geometry_msgs::msg::Pose pose_msg; - pose_msg.position.x = p.pose.getOrigin().x(); - pose_msg.position.y = p.pose.getOrigin().y(); - pose_msg.position.z = p.pose.getOrigin().z(); - pose_msg.orientation = tf2::toMsg(p.pose.getRotation()); - array_msg.poses.push_back(pose_msg); - } - particles_pub_->publish(array_msg); -} - -tf2::Transform AMCLLocalizer::getEstimatedPose() const -{ - if (particles_.empty()) {return tf2::Transform::getIdentity();} - const std::size_t N = particles_.size(), N_top = N / 2; - std::vector sorted = particles_; - std::sort(sorted.begin(), sorted.end(), - [](const Particle & a, const Particle & b){return a.weight > b.weight;}); - tf2::Vector3 mean = computeMean(sorted, 0, N_top); - if (!std::isfinite(mean.x()) || !std::isfinite(mean.y()) || !std::isfinite(mean.z())) { - mean = tf2::Vector3(0, 0, 0); - } - double r, p, y; tf2::Matrix3x3(sorted[0].pose.getRotation()).getRPY(r, p, y); - tf2::Quaternion q; q.setRPY(0.0, 0.0, y); - tf2::Transform est; est.setOrigin(mean); est.setRotation(q); return est; -} - -void AMCLLocalizer::publishEstimatedPose(const tf2::Transform & est_pose) -{ - if (particles_.empty()) {return;} - const std::size_t N = particles_.size(), N_top = N / 2; - - tf2::Vector3 mean = est_pose.getOrigin(); - tf2::Matrix3x3 cov = computeCovariance(particles_, 0, N_top, mean); - double yaw_var = computeYawVariance(particles_, 0, N_top); - - geometry_msgs::msg::PoseWithCovarianceStamped msg; - msg.header.stamp = get_node()->now(); - msg.header.frame_id = get_tf_prefix() + "map"; - msg.pose.pose.position.x = mean.x(); - msg.pose.pose.position.y = mean.y(); - msg.pose.pose.position.z = mean.z(); - msg.pose.pose.orientation = tf2::toMsg(est_pose.getRotation()); - - for (int r = 0; r < 3; ++r) { - for (int c = 0; c < 3; ++c) { - msg.pose.covariance[6 * r + c] = cov[r][c]; - } - } - msg.pose.covariance[6 * 5 + 5] = yaw_var; - estimate_pub_->publish(msg); -} - -nav_msgs::msg::Odometry AMCLLocalizer::get_pose() -{ - nav_msgs::msg::Odometry odom_msg; - odom_msg.header.stamp = get_node()->now(); - odom_msg.header.frame_id = get_tf_prefix() + "map"; - odom_msg.child_frame_id = get_tf_prefix() + "base_footprint"; - - pose_ = getEstimatedPose(); - tf2::Transform est_pose = pose_; - odom_msg.pose.pose.position.x = est_pose.getOrigin().x(); - odom_msg.pose.pose.position.y = est_pose.getOrigin().y(); - odom_msg.pose.pose.position.z = est_pose.getOrigin().z(); - odom_msg.pose.pose.orientation = tf2::toMsg(est_pose.getRotation()); - - if (!particles_.empty()) { - const std::size_t N = particles_.size(), N_top = N / 2; - std::vector sorted = particles_; - std::sort(sorted.begin(), sorted.end(), - [](const Particle & a, const Particle & b){return a.weight > b.weight;}); - tf2::Vector3 mean = computeMean(sorted, 0, N_top); - tf2::Matrix3x3 cov = computeCovariance(sorted, 0, N_top, mean); - double yaw_var = computeYawVariance(sorted, 0, N_top); - for (int r = 0; r < 3; ++r) { - for (int c = 0; c < 3; ++c) { - odom_msg.pose.covariance[6 * r + c] = cov[r][c]; - } - } - odom_msg.pose.covariance[6 * 5 + 5] = yaw_var; - } - - odom_msg.twist.twist.linear.x = 0.0; - odom_msg.twist.twist.linear.y = 0.0; - odom_msg.twist.twist.linear.z = 0.0; - odom_msg.twist.twist.angular.x = 0.0; - odom_msg.twist.twist.angular.y = 0.0; - odom_msg.twist.twist.angular.z = 0.0; - return odom_msg; -} - -} // namespace navmap -} // namespace easynav - -#include -PLUGINLIB_EXPORT_CLASS(easynav::navmap::AMCLLocalizer, easynav::LocalizerMethodBase) diff --git a/localizers/easynav_simple_localizer/include/easynav_simple_localizer/AMCLLocalizer.hpp b/localizers/easynav_simple_localizer/include/easynav_simple_localizer/AMCLLocalizer.hpp index 65d379b3..22f60ab9 100644 --- a/localizers/easynav_simple_localizer/include/easynav_simple_localizer/AMCLLocalizer.hpp +++ b/localizers/easynav_simple_localizer/include/easynav_simple_localizer/AMCLLocalizer.hpp @@ -37,7 +37,7 @@ #include "nav_msgs/msg/odometry.hpp" #include "tf2/LinearMath/Transform.hpp" -#include "tf2_ros/transform_broadcaster.h" +#include "tf2_ros/transform_broadcaster.hpp" #include "easynav_core/LocalizerMethodBase.hpp" diff --git a/maps_managers/easynav_bonxai_maps_manager/include/easynav_bonxai_maps_manager/BonxaiMapsManager.hpp b/maps_managers/easynav_bonxai_maps_manager/include/easynav_bonxai_maps_manager/BonxaiMapsManager.hpp index 8d9dee6e..5d0d84c4 100644 --- a/maps_managers/easynav_bonxai_maps_manager/include/easynav_bonxai_maps_manager/BonxaiMapsManager.hpp +++ b/maps_managers/easynav_bonxai_maps_manager/include/easynav_bonxai_maps_manager/BonxaiMapsManager.hpp @@ -37,8 +37,8 @@ #include "std_srvs/srv/trigger.hpp" #include "sensor_msgs/msg/point_cloud2.hpp" -#include "tf2_ros/buffer.h" -#include "tf2_ros/transform_listener.h" +#include "tf2_ros/buffer.hpp" +#include "tf2_ros/transform_listener.hpp" #include "easynav_core/MapsManagerBase.hpp" diff --git a/maps_managers/easynav_bonxai_maps_manager/src/easynav_bonxai_maps_manager/BonxaiMapsManager.cpp b/maps_managers/easynav_bonxai_maps_manager/src/easynav_bonxai_maps_manager/BonxaiMapsManager.cpp index 212fae60..4f8182c6 100644 --- a/maps_managers/easynav_bonxai_maps_manager/src/easynav_bonxai_maps_manager/BonxaiMapsManager.cpp +++ b/maps_managers/easynav_bonxai_maps_manager/src/easynav_bonxai_maps_manager/BonxaiMapsManager.cpp @@ -99,7 +99,11 @@ BonxaiMapsManager::on_initialize() if (!std::isfinite(p.x()) || !std::isfinite(p.y()) || !std::isfinite(p.z())) {continue;} bonxai_map_->addHitPoint(p); - pcl_out.push_back({p.x(), p.y(), p.z()}); + pcl_out.push_back({ + static_cast(p.x()), + static_cast(p.y()), + static_cast(p.z()) + }); } bonxai_msg_.data.clear(); @@ -209,7 +213,11 @@ BonxaiMapsManager::update_from_pc2(const sensor_msgs::msg::PointCloud2 & pc2) bonxai_result.clear(); bonxai_map_->getOccupiedVoxels(bonxai_result); for (const auto & voxel : bonxai_result) { - pcl_out.push_back({voxel.x(), voxel.y(), voxel.z()}); + pcl_out.push_back({ + static_cast(voxel.x()), + static_cast(voxel.y()), + static_cast(voxel.z()) + }); } bonxai_msg_.data.clear(); @@ -267,7 +275,11 @@ BonxaiMapsManager::update_from_occ(const nav_msgs::msg::OccupancyGrid & occ) bonxai_result.clear(); bonxai_map_->getOccupiedVoxels(bonxai_result); for (const auto & voxel : bonxai_result) { - pcl_out.push_back({voxel.x(), voxel.y(), voxel.z()}); + pcl_out.push_back({ + static_cast(voxel.x()), + static_cast(voxel.y()), + static_cast(voxel.z()) + }); } bonxai_msg_.data.clear(); diff --git a/maps_managers/easynav_costmap_maps_manager/include/easynav_costmap_maps_manager/CostmapMapsManager.hpp b/maps_managers/easynav_costmap_maps_manager/include/easynav_costmap_maps_manager/CostmapMapsManager.hpp index 973af430..3499a0e6 100644 --- a/maps_managers/easynav_costmap_maps_manager/include/easynav_costmap_maps_manager/CostmapMapsManager.hpp +++ b/maps_managers/easynav_costmap_maps_manager/include/easynav_costmap_maps_manager/CostmapMapsManager.hpp @@ -33,8 +33,8 @@ #include "nav_msgs/msg/occupancy_grid.hpp" #include "std_srvs/srv/trigger.hpp" -#include "tf2_ros/buffer.h" -#include "tf2_ros/transform_listener.h" +#include "tf2_ros/buffer.hpp" +#include "tf2_ros/transform_listener.hpp" #include "easynav_core/MapsManagerBase.hpp" #include "easynav_costmap_common/costmap_2d.hpp" diff --git a/maps_managers/easynav_navmap_maps_manager/include/easynav_navmap_maps_manager/NavMapMapsManager.hpp b/maps_managers/easynav_navmap_maps_manager/include/easynav_navmap_maps_manager/NavMapMapsManager.hpp index 2d64f99d..c64609bf 100644 --- a/maps_managers/easynav_navmap_maps_manager/include/easynav_navmap_maps_manager/NavMapMapsManager.hpp +++ b/maps_managers/easynav_navmap_maps_manager/include/easynav_navmap_maps_manager/NavMapMapsManager.hpp @@ -36,8 +36,8 @@ #include "std_srvs/srv/trigger.hpp" #include "sensor_msgs/msg/point_cloud2.hpp" -#include "tf2_ros/buffer.h" -#include "tf2_ros/transform_listener.h" +#include "tf2_ros/buffer.hpp" +#include "tf2_ros/transform_listener.hpp" #include "easynav_core/MapsManagerBase.hpp" #include "navmap_core/NavMap.hpp" diff --git a/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/NavMapMapsManager.cpp b/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/NavMapMapsManager.cpp index 42eea81f..7725bcc9 100644 --- a/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/NavMapMapsManager.cpp +++ b/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/NavMapMapsManager.cpp @@ -205,7 +205,7 @@ NavMapMapsManager::on_initialize() } void -NavMapMapsManager::set_static_map(const ::navmap::NavMap & new_map) +NavMapMapsManager::set_static_map([[maybe_unused]] const ::navmap::NavMap & new_map) { // navmap_ = new_map; } diff --git a/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/filters/InflationFilter.cpp b/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/filters/InflationFilter.cpp index 3834c3c2..3c023944 100644 --- a/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/filters/InflationFilter.cpp +++ b/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/filters/InflationFilter.cpp @@ -95,7 +95,7 @@ bool InflationFilter::inflate_layer_u8( const size_t N = nm.navcels.size(); const float R = inflation_radius; - const float k = cost_scaling_factor; + // const float k = cost_scaling_factor; const float r_ins = std::clamp(inscribed_radius, 0.0f, R); // Precomputar centroides XY diff --git a/maps_managers/easynav_octomap_maps_manager/include/easynav_octomap_maps_manager/OctomapMapsManager.hpp b/maps_managers/easynav_octomap_maps_manager/include/easynav_octomap_maps_manager/OctomapMapsManager.hpp index 88bcfa60..4654a14d 100644 --- a/maps_managers/easynav_octomap_maps_manager/include/easynav_octomap_maps_manager/OctomapMapsManager.hpp +++ b/maps_managers/easynav_octomap_maps_manager/include/easynav_octomap_maps_manager/OctomapMapsManager.hpp @@ -35,8 +35,8 @@ #include "std_srvs/srv/trigger.hpp" #include "sensor_msgs/msg/point_cloud2.hpp" -#include "tf2_ros/buffer.h" -#include "tf2_ros/transform_listener.h" +#include "tf2_ros/buffer.hpp" +#include "tf2_ros/transform_listener.hpp" #include "easynav_core/MapsManagerBase.hpp" #include "octomap/octomap.h" diff --git a/maps_managers/easynav_octomap_maps_manager/src/easynav_octomap_maps_manager/OctomapMapsManager.cpp b/maps_managers/easynav_octomap_maps_manager/src/easynav_octomap_maps_manager/OctomapMapsManager.cpp index 6f682b69..8404d664 100644 --- a/maps_managers/easynav_octomap_maps_manager/src/easynav_octomap_maps_manager/OctomapMapsManager.cpp +++ b/maps_managers/easynav_octomap_maps_manager/src/easynav_octomap_maps_manager/OctomapMapsManager.cpp @@ -253,7 +253,7 @@ OctomapMapsManager::on_initialize() } void -OctomapMapsManager::update(::easynav::NavState & nav_state) +OctomapMapsManager::update([[maybe_unused]] ::easynav::NavState & nav_state) { EASYNAV_TRACE_EVENT; diff --git a/maps_managers/easynav_simple_maps_manager/include/easynav_simple_maps_manager/SimpleMapsManager.hpp b/maps_managers/easynav_simple_maps_manager/include/easynav_simple_maps_manager/SimpleMapsManager.hpp index ae8bdfc7..ff543893 100644 --- a/maps_managers/easynav_simple_maps_manager/include/easynav_simple_maps_manager/SimpleMapsManager.hpp +++ b/maps_managers/easynav_simple_maps_manager/include/easynav_simple_maps_manager/SimpleMapsManager.hpp @@ -33,8 +33,8 @@ #include "nav_msgs/msg/occupancy_grid.hpp" #include "std_srvs/srv/trigger.hpp" -#include "tf2_ros/buffer.h" -#include "tf2_ros/transform_listener.h" +#include "tf2_ros/buffer.hpp" +#include "tf2_ros/transform_listener.hpp" #include "easynav_core/MapsManagerBase.hpp" #include "easynav_simple_common/SimpleMap.hpp" diff --git a/planners/easynav_navmap_planner/src/easynav_navmap_planner/AStarPlanner.cpp b/planners/easynav_navmap_planner/src/easynav_navmap_planner/AStarPlanner.cpp index 8785212b..e1967352 100644 --- a/planners/easynav_navmap_planner/src/easynav_navmap_planner/AStarPlanner.cpp +++ b/planners/easynav_navmap_planner/src/easynav_navmap_planner/AStarPlanner.cpp @@ -234,8 +234,6 @@ std::vector AStarPlanner::a_star_path( open.push(Node{cid_start, h(cid_start, cid_goal)}); in_open[cid_start] = 1; - const size_t surface_goal = sidx_g; - while (!open.empty()) { const auto cur = open.top(); open.pop(); const NavCelId u = cur.cid; @@ -243,6 +241,7 @@ std::vector AStarPlanner::a_star_path( if (u == cid_goal) {break;} // Optional: restrict to the goal surface (comment if you want cross-surface paths via explicit neighbors) + // const size_t surface_goal = sidx_g; // if (surface_goal != sidx_s) { // // do nothing special; graph neighbors already encode connectivity // }