From 2d0b6d7c7850d265865b67d3aa066231e067c222 Mon Sep 17 00:00:00 2001 From: Francisco Miguel Moreno Date: Tue, 31 Mar 2026 19:01:02 +0200 Subject: [PATCH 1/3] Update plugins to new sensors API --- .../include/easynav_mpc_controller/MPCController.hpp | 2 +- .../src/easynav_mpc_controller/MPCController.cpp | 4 ++-- .../src/easynav_mppi_controller/MPPIController.cpp | 4 ++-- .../src/easynav_serest_controller/SerestController.cpp | 6 +++--- .../src/easynav_vff_controller/VffController.cpp | 4 ++-- .../src/easynav_costmap_localizer/AMCLLocalizer.cpp | 6 +++--- .../src/easynav_fusion_localizer/FusionLocalizer.cpp | 4 ++-- .../src/easynav_navmap_localizer/AMCLLocalizer.cpp | 8 ++++---- .../src/easynav_simple_localizer/AMCLLocalizer.cpp | 6 +++--- .../filters/ObstacleFilter.cpp | 6 +++--- .../tests/costmap_mapsmanager_tests.cpp | 2 +- maps_managers/easynav_navmap_maps_manager/README.md | 2 +- .../filters/ObstacleFilter.cpp | 6 +++--- .../tests/navmap_mapsmanager_tests.cpp | 2 +- .../filters/InflationFilter.cpp | 4 ++-- .../filters/ObstacleFilter.cpp | 8 ++++---- .../easynav_simple_maps_manager/SimpleMapsManager.cpp | 10 +++++++--- .../tests/simple_mapsmanager_tests.cpp | 10 +++++----- 18 files changed, 49 insertions(+), 45 deletions(-) diff --git a/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp b/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp index ebb7fb6f..40e8472d 100644 --- a/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp +++ b/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp @@ -37,7 +37,7 @@ #include "easynav_core/ControllerMethodBase.hpp" #include "easynav_common/types/NavState.hpp" -#include "easynav_common/types/PointPerception.hpp" +#include "easynav_sensors/types/PointPerception.hpp" #include "easynav_mpc_controller/MPCOptimizer.hpp" diff --git a/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp index 36116946..07487393 100644 --- a/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp +++ b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp @@ -142,7 +142,7 @@ MPCController::update_rt(NavState & nav_state) } } - if (!nav_state.has("path") || !nav_state.has("robot_pose") || !nav_state.has("points")) { + if (!nav_state.has("path") || !nav_state.has("robot_pose") || !nav_state.has_group("points")) { if(verbose_) { std::cout << "No Path, No Points or No Robot Pose" << std::endl; } @@ -209,7 +209,7 @@ MPCController::update_rt(NavState & nav_state) } const auto & last_pose = path.poses[local_horizon].pose.position; - const auto & perceptions = nav_state.get("points"); + const auto & perceptions = nav_state.get_group("points"); const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); const auto & filtered = PointPerceptionsOpsView(perceptions) diff --git a/controllers/easynav_mppi_controller/src/easynav_mppi_controller/MPPIController.cpp b/controllers/easynav_mppi_controller/src/easynav_mppi_controller/MPPIController.cpp index c0fc3471..5823981e 100644 --- a/controllers/easynav_mppi_controller/src/easynav_mppi_controller/MPPIController.cpp +++ b/controllers/easynav_mppi_controller/src/easynav_mppi_controller/MPPIController.cpp @@ -17,7 +17,7 @@ /// \brief Implementation of the MPPIController class. #include "easynav_mppi_controller/MPPIController.hpp" -#include "easynav_common/types/PointPerception.hpp" +#include "easynav_sensors/types/PointPerception.hpp" #include "easynav_common/RTTFBuffer.hpp" #include "easynav_system/GoalManager.hpp" @@ -182,7 +182,7 @@ MPPIController::update_rt(NavState & nav_state) } const auto & pose = nav_state.get("robot_pose").pose.pose; - const auto & perceptions = nav_state.get("points"); + const auto & perceptions = nav_state.get_group("points"); const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); const auto & filtered = PointPerceptionsOpsView(perceptions) .filter({-1.0, -1.0, -1.0}, {1.0, 1.0, 1.0}) 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 659dd4e6..a15bad53 100644 --- a/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp +++ b/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp @@ -23,7 +23,7 @@ #include "tf2/utils.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" -#include "easynav_common/types/PointPerception.hpp" +#include "easynav_sensors/types/PointPerception.hpp" #include "easynav_common/RTTFBuffer.hpp" #include "easynav_serest_controller/SerestController.hpp" @@ -295,9 +295,9 @@ SerestController::closest_obstacle_distance( } // 2) Analyze distance sensors - if (!nav_state.has("points")) {return std::numeric_limits::infinity();} + if (!nav_state.has_group("points")) {return std::numeric_limits::infinity();} - const auto & perceptions = nav_state.get("points"); + const auto & perceptions = nav_state.get_group("points"); const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); auto view = PointPerceptionsOpsView(perceptions); diff --git a/controllers/easynav_vff_controller/src/easynav_vff_controller/VffController.cpp b/controllers/easynav_vff_controller/src/easynav_vff_controller/VffController.cpp index 65d5e626..16ce5020 100644 --- a/controllers/easynav_vff_controller/src/easynav_vff_controller/VffController.cpp +++ b/controllers/easynav_vff_controller/src/easynav_vff_controller/VffController.cpp @@ -18,7 +18,7 @@ #include "easynav_vff_controller/VffController.hpp" #include "easynav_common/types/NavState.hpp" -#include "easynav_common/types/PointPerception.hpp" +#include "easynav_sensors/types/PointPerception.hpp" #include "nav_msgs/msg/odometry.hpp" #include "nav_msgs/msg/goals.hpp" @@ -252,7 +252,7 @@ void VffController::update_rt(NavState & nav_state) // Calculate the angle error double angle_error = normalize_angle(bearing - yaw); - const auto & perceptions = nav_state.get("points"); + const auto & perceptions = nav_state.get_group("points"); const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); auto fused = diff --git a/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp b/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp index 30c3bbbb..619e1d6f 100644 --- a/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp @@ -22,7 +22,7 @@ #include "tf2/LinearMath/Vector3.hpp" #include "easynav_common/RTTFBuffer.hpp" -#include "easynav_common/types/PointPerception.hpp" +#include "easynav_sensors/types/PointPerception.hpp" #include "easynav_costmap_common/costmap_2d.hpp" #include "easynav_costmap_common/cost_values.hpp" @@ -566,12 +566,12 @@ AMCLLocalizer::predict([[maybe_unused]] NavState & nav_state) void AMCLLocalizer::correct(NavState & nav_state) { - if (!nav_state.has("points")) { + if (!nav_state.has_group("points")) { RCLCPP_WARN(get_node()->get_logger(), "There is yet no points perceptions"); return; } - const auto & perceptions = nav_state.get("points"); + const auto & perceptions = nav_state.get_group("points"); if (!nav_state.has("map.static")) { RCLCPP_WARN(get_node()->get_logger(), "There is yet no a map.static map"); diff --git a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp index 7982a9a7..2bdf750f 100644 --- a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp +++ b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp @@ -4,8 +4,8 @@ #include "easynav_common/RTTFBuffer.hpp" -#include "easynav_common/types/IMUPerception.hpp" -#include "easynav_common/types/GNSSPerception.hpp" +#include "easynav_sensors/types/IMUPerception.hpp" +#include "easynav_sensors/types/GNSSPerception.hpp" #include "sensor_msgs/msg/nav_sat_status.hpp" #include 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 79117a91..0b127310 100644 --- a/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp @@ -34,8 +34,8 @@ #include "tf2/LinearMath/Vector3.hpp" #include "easynav_common/RTTFBuffer.hpp" -#include "easynav_common/types/PointPerception.hpp" -#include "easynav_common/types/IMUPerception.hpp" +#include "easynav_sensors/types/PointPerception.hpp" +#include "easynav_sensors/types/IMUPerception.hpp" #include "navmap_core/NavMap.hpp" @@ -689,11 +689,11 @@ static ScoreAgg score_particle_sensor_cloud( // ---------- correct() ---------- void AMCLLocalizer::correct(NavState & nav_state) { - if (!nav_state.has("points")) { + if (!nav_state.has_group("points")) { RCLCPP_WARN(get_node()->get_logger(), "No points perceptions yet"); return; } - const auto & perceptions = nav_state.get("points"); + const auto & perceptions = nav_state.get_group("points"); if (!nav_state.has("map.bonxai")) { RCLCPP_WARN(get_node()->get_logger(), "No Bonxai map yet"); diff --git a/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp b/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp index 6fc0b92a..2c50681c 100644 --- a/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp @@ -22,7 +22,7 @@ #include "tf2/LinearMath/Vector3.hpp" #include "easynav_common/RTTFBuffer.hpp" -#include "easynav_common/types/PointPerception.hpp" +#include "easynav_sensors/types/PointPerception.hpp" #include "easynav_simple_common/SimpleMap.hpp" #include "easynav_simple_localizer/AMCLLocalizer.hpp" @@ -368,12 +368,12 @@ AMCLLocalizer::predict([[maybe_unused]] NavState & nav_state) void AMCLLocalizer::correct(NavState & nav_state) { - if (!nav_state.has("points")) { + if (!nav_state.has_group("points")) { RCLCPP_WARN(get_node()->get_logger(), "There is yet no points perceptions"); return; } - const auto & perceptions = nav_state.get("points"); + const auto & perceptions = nav_state.get_group("points"); if (!nav_state.has("map.static")) { RCLCPP_WARN(get_node()->get_logger(), "There is yet no a map.static map"); diff --git a/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/filters/ObstacleFilter.cpp b/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/filters/ObstacleFilter.cpp index b354f31e..36a3dd26 100644 --- a/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/filters/ObstacleFilter.cpp +++ b/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/filters/ObstacleFilter.cpp @@ -18,7 +18,7 @@ #include "easynav_costmap_common/costmap_2d.hpp" #include "easynav_common/types/NavState.hpp" -#include "easynav_common/types/PointPerception.hpp" +#include "easynav_sensors/types/PointPerception.hpp" #include "easynav_costmap_common/costmap_2d.hpp" #include "easynav_costmap_common/cost_values.hpp" @@ -41,11 +41,11 @@ ObstacleFilter::on_initialize() void ObstacleFilter::update(NavState & nav_state) { - if (!nav_state.has("points")) { + if (!nav_state.has_group("points")) { return; } - const auto & perceptions = nav_state.get("points"); + const auto & perceptions = nav_state.get_group("points"); auto dynamic_map_ptr = nav_state.get_ptr("map.dynamic.filtered"); Costmap2D & dynamic_map = *dynamic_map_ptr; diff --git a/maps_managers/easynav_costmap_maps_manager/tests/costmap_mapsmanager_tests.cpp b/maps_managers/easynav_costmap_maps_manager/tests/costmap_mapsmanager_tests.cpp index f14df242..8440f0c2 100644 --- a/maps_managers/easynav_costmap_maps_manager/tests/costmap_mapsmanager_tests.cpp +++ b/maps_managers/easynav_costmap_maps_manager/tests/costmap_mapsmanager_tests.cpp @@ -15,7 +15,7 @@ #include -#include "easynav_common/types/PointPerception.hpp" +#include "easynav_sensors/types/PointPerception.hpp" #include "easynav_common/types/NavState.hpp" #include "rclcpp/rclcpp.hpp" diff --git a/maps_managers/easynav_navmap_maps_manager/README.md b/maps_managers/easynav_navmap_maps_manager/README.md index 9f925d2f..f019a211 100644 --- a/maps_managers/easynav_navmap_maps_manager/README.md +++ b/maps_managers/easynav_navmap_maps_manager/README.md @@ -59,7 +59,7 @@ All parameters are declared under the plugin namespace, i.e. | `.height_threshold` | `double` | `0.25` | Minimum vertical height (in meters) between max and min z to mark as an obstacle. | | `.downsample` | `double` | `0.3` | Voxel size used to downsample point clouds before obstacle detection. | | `.fuse_frame` | `string` | `"map"` | Frame in which points are fused before projection into NavMap. | -| **Input Key:** | | | Reads point clouds from `NavState` key `"points"`. | +| **Input Key:** | | | Reads list of point clouds from `NavState` group `"points"`. | | **Output Layer:** | | | Updates or creates NavMap layer `"obstacles"`. | #### **InflationFilter** diff --git a/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/filters/ObstacleFilter.cpp b/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/filters/ObstacleFilter.cpp index aeeed492..a031c277 100644 --- a/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/filters/ObstacleFilter.cpp +++ b/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/filters/ObstacleFilter.cpp @@ -18,7 +18,7 @@ #include #include "easynav_common/types/NavState.hpp" -#include "easynav_common/types/PointPerception.hpp" +#include "easynav_sensors/types/PointPerception.hpp" #include "easynav_common/RTTFBuffer.hpp" #include "navmap_core/NavMap.hpp" @@ -44,9 +44,9 @@ ObstacleFilter::on_initialize() void ObstacleFilter::update(::easynav::NavState & nav_state) { if (!nav_state.has("map.navmap")) {return;} - if (!nav_state.has("points")) {return;} + if (!nav_state.has_group("points")) {return;} - const auto & perceptions = nav_state.get("points"); + const auto & perceptions = nav_state.get_group("points"); navmap_ = nav_state.get<::navmap::NavMap>("map.navmap"); const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); diff --git a/maps_managers/easynav_navmap_maps_manager/tests/navmap_mapsmanager_tests.cpp b/maps_managers/easynav_navmap_maps_manager/tests/navmap_mapsmanager_tests.cpp index d06b8904..4c55b8fe 100644 --- a/maps_managers/easynav_navmap_maps_manager/tests/navmap_mapsmanager_tests.cpp +++ b/maps_managers/easynav_navmap_maps_manager/tests/navmap_mapsmanager_tests.cpp @@ -15,7 +15,7 @@ #include -#include "easynav_common/types/PointPerception.hpp" +#include "easynav_sensors/types/PointPerception.hpp" #include "easynav_common/types/NavState.hpp" #include "rclcpp/rclcpp.hpp" diff --git a/maps_managers/easynav_octomap_maps_manager/src/easynav_octomap_maps_manager/filters/InflationFilter.cpp b/maps_managers/easynav_octomap_maps_manager/src/easynav_octomap_maps_manager/filters/InflationFilter.cpp index 0703a15f..09a13363 100644 --- a/maps_managers/easynav_octomap_maps_manager/src/easynav_octomap_maps_manager/filters/InflationFilter.cpp +++ b/maps_managers/easynav_octomap_maps_manager/src/easynav_octomap_maps_manager/filters/InflationFilter.cpp @@ -42,8 +42,8 @@ #include #include "easynav_common/types/NavState.hpp" -#include "easynav_common/types/Perceptions.hpp" -#include "easynav_common/types/PointPerception.hpp" +#include "easynav_sensors/types/Perceptions.hpp" +#include "easynav_sensors/types/PointPerception.hpp" #include "octomap_core/Octomap.hpp" diff --git a/maps_managers/easynav_octomap_maps_manager/src/easynav_octomap_maps_manager/filters/ObstacleFilter.cpp b/maps_managers/easynav_octomap_maps_manager/src/easynav_octomap_maps_manager/filters/ObstacleFilter.cpp index 33a1abb9..987df0c3 100644 --- a/maps_managers/easynav_octomap_maps_manager/src/easynav_octomap_maps_manager/filters/ObstacleFilter.cpp +++ b/maps_managers/easynav_octomap_maps_manager/src/easynav_octomap_maps_manager/filters/ObstacleFilter.cpp @@ -17,8 +17,8 @@ #include #include "easynav_common/types/NavState.hpp" -#include "easynav_common/types/Perceptions.hpp" -#include "easynav_common/types/PointPerception.hpp" +#include "easynav_sensors/types/Perceptions.hpp" +#include "easynav_sensors/types/PointPerception.hpp" #include "octomap_core/Octomap.hpp" @@ -45,11 +45,11 @@ ObstacleFilter::update(::easynav::NavState & nav_state) if (!nav_state.has("map")) { return; } - if (!nav_state.has("points")) { + if (!nav_state.has_group("points")) { return; } - const auto & perceptions = nav_state.get("points"); + const auto & perceptions = nav_state.get_group("points"); octomap_ = nav_state.get<::octomap::Octomap>("map"); const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); diff --git a/maps_managers/easynav_simple_maps_manager/src/easynav_simple_maps_manager/SimpleMapsManager.cpp b/maps_managers/easynav_simple_maps_manager/src/easynav_simple_maps_manager/SimpleMapsManager.cpp index 9241f3ce..446954e7 100644 --- a/maps_managers/easynav_simple_maps_manager/src/easynav_simple_maps_manager/SimpleMapsManager.cpp +++ b/maps_managers/easynav_simple_maps_manager/src/easynav_simple_maps_manager/SimpleMapsManager.cpp @@ -17,7 +17,7 @@ /// \brief Implementation of the SimpleMapsManager class. #include "easynav_simple_maps_manager/SimpleMapsManager.hpp" -#include "easynav_common/types/PointPerception.hpp" +#include "easynav_sensors/types/PointPerception.hpp" #include "ament_index_cpp/get_package_share_directory.hpp" #include "ament_index_cpp/get_package_prefix.hpp" @@ -138,13 +138,17 @@ SimpleMapsManager::update(NavState & nav_state) dynamic_map_.deep_copy(static_map_); - if (!nav_state.has("points")) { + std::cout << "nav_state.has_group(points): " << nav_state.has_group("points") << std::endl; + if (!nav_state.has_group("points")) { nav_state.set("map.static", static_map_); nav_state.set("map.dynamic", dynamic_map_); return; } - const auto & perceptions = nav_state.get("points"); + const auto & perceptions = nav_state.get_group("points"); + std::cout << "perceptions size: " << perceptions.size() << std::endl; + std::cout << "perception[0] size: " << perceptions[0]->data.points.size() << std::endl; + std::cout << "perception[0] valid: " << perceptions[0]->valid << std::endl; const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); rclcpp::Time stamp; diff --git a/maps_managers/easynav_simple_maps_manager/tests/simple_mapsmanager_tests.cpp b/maps_managers/easynav_simple_maps_manager/tests/simple_mapsmanager_tests.cpp index 82c67758..74873bc0 100644 --- a/maps_managers/easynav_simple_maps_manager/tests/simple_mapsmanager_tests.cpp +++ b/maps_managers/easynav_simple_maps_manager/tests/simple_mapsmanager_tests.cpp @@ -17,7 +17,7 @@ #include "easynav_simple_common/SimpleMap.hpp" #include "easynav_common/RTTFBuffer.hpp" -#include "easynav_common/types/PointPerception.hpp" +#include "easynav_sensors/types/PointPerception.hpp" #include "easynav_simple_maps_manager/SimpleMapsManager.hpp" #include "rclcpp/rclcpp.hpp" @@ -68,7 +68,8 @@ TEST_F(SimpleMapsManagerTest, BasicDynamicUpdate) easynav::NavState navstate; auto perception = std::make_shared(); - navstate.set("points", easynav::PointPerceptions()); + navstate.set("laser", easynav::PointPerception()); + navstate.set_group("points", {"laser"}); perception->data.points.resize(6); perception->data.points[0].x = 1.0; @@ -94,9 +95,8 @@ TEST_F(SimpleMapsManagerTest, BasicDynamicUpdate) perception->frame_id = "map"; perception->valid = true; - easynav::PointPerceptions perceptions; - perceptions.push_back(perception); - navstate.set("points", perceptions); + navstate.set("laser", perception); + // navstate.set_group("points", {"laser"}); manager->update(navstate); From 21bef9d60312caaac26d6cddd6348d7be099989f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Fri, 3 Apr 2026 09:52:42 +0200 Subject: [PATCH 2/3] Adaptations to #94 MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../src/easynav_mpc_controller/MPCController.cpp | 6 +++--- .../src/easynav_mppi_controller/MPPIController.cpp | 2 +- .../easynav_serest_controller/SerestController.cpp | 2 +- .../src/easynav_vff_controller/VffController.cpp | 2 +- .../src/easynav_costmap_localizer/AMCLLocalizer.cpp | 7 +++---- .../src/easynav_fusion_localizer/FusionLocalizer.cpp | 8 +++----- .../src/easynav_navmap_localizer/AMCLLocalizer.cpp | 11 +++++------ .../src/easynav_simple_localizer/AMCLLocalizer.cpp | 7 +++---- .../filters/ObstacleFilter.cpp | 6 +++--- .../filters/ObstacleFilter.cpp | 8 ++++++-- .../filters/ObstacleFilter.cpp | 6 ++++-- .../easynav_simple_maps_manager/SimpleMapsManager.cpp | 9 ++++++--- 12 files changed, 39 insertions(+), 35 deletions(-) diff --git a/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp index 07487393..ac825d4c 100644 --- a/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp +++ b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp @@ -142,7 +142,9 @@ MPCController::update_rt(NavState & nav_state) } } - if (!nav_state.has("path") || !nav_state.has("robot_pose") || !nav_state.has_group("points")) { + const auto & perceptions = nav_state.get_no_group(); + + if (!nav_state.has("path") || !nav_state.has("robot_pose") || perceptions.empty()) { if(verbose_) { std::cout << "No Path, No Points or No Robot Pose" << std::endl; } @@ -208,8 +210,6 @@ MPCController::update_rt(NavState & nav_state) local_horizon = num_elements - 1; } const auto & last_pose = path.poses[local_horizon].pose.position; - - const auto & perceptions = nav_state.get_group("points"); const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); const auto & filtered = PointPerceptionsOpsView(perceptions) diff --git a/controllers/easynav_mppi_controller/src/easynav_mppi_controller/MPPIController.cpp b/controllers/easynav_mppi_controller/src/easynav_mppi_controller/MPPIController.cpp index 5823981e..f80ca7ff 100644 --- a/controllers/easynav_mppi_controller/src/easynav_mppi_controller/MPPIController.cpp +++ b/controllers/easynav_mppi_controller/src/easynav_mppi_controller/MPPIController.cpp @@ -182,7 +182,7 @@ MPPIController::update_rt(NavState & nav_state) } const auto & pose = nav_state.get("robot_pose").pose.pose; - const auto & perceptions = nav_state.get_group("points"); + const auto & perceptions = nav_state.get_no_group(); const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); const auto & filtered = PointPerceptionsOpsView(perceptions) .filter({-1.0, -1.0, -1.0}, {1.0, 1.0, 1.0}) 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 a15bad53..9a32f1ff 100644 --- a/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp +++ b/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp @@ -297,7 +297,7 @@ SerestController::closest_obstacle_distance( // 2) Analyze distance sensors if (!nav_state.has_group("points")) {return std::numeric_limits::infinity();} - const auto & perceptions = nav_state.get_group("points"); + const auto & perceptions = nav_state.get_no_group(); const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); auto view = PointPerceptionsOpsView(perceptions); diff --git a/controllers/easynav_vff_controller/src/easynav_vff_controller/VffController.cpp b/controllers/easynav_vff_controller/src/easynav_vff_controller/VffController.cpp index 16ce5020..1fe63e46 100644 --- a/controllers/easynav_vff_controller/src/easynav_vff_controller/VffController.cpp +++ b/controllers/easynav_vff_controller/src/easynav_vff_controller/VffController.cpp @@ -252,7 +252,7 @@ void VffController::update_rt(NavState & nav_state) // Calculate the angle error double angle_error = normalize_angle(bearing - yaw); - const auto & perceptions = nav_state.get_group("points"); + const auto & perceptions = nav_state.get_no_group(); const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); auto fused = diff --git a/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp b/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp index 619e1d6f..7c26a2c0 100644 --- a/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp @@ -566,13 +566,12 @@ AMCLLocalizer::predict([[maybe_unused]] NavState & nav_state) void AMCLLocalizer::correct(NavState & nav_state) { - if (!nav_state.has_group("points")) { - RCLCPP_WARN(get_node()->get_logger(), "There is yet no points perceptions"); + const auto & perceptions = nav_state.get_no_group(); + if (perceptions.empty()) { + RCLCPP_WARN(get_node()->get_logger(), "There are no points perceptions"); return; } - const auto & perceptions = nav_state.get_group("points"); - if (!nav_state.has("map.static")) { RCLCPP_WARN(get_node()->get_logger(), "There is yet no a map.static map"); return; diff --git a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp index 2bdf750f..f1cd27f5 100644 --- a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp +++ b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp @@ -142,11 +142,9 @@ void FusionLocalizer::update_rt(NavState & nav_state) if (!first_pose_received_) { RCLCPP_INFO(get_node()->get_logger(), "First valid GPS fix received. Initializing filter state."); - if(nav_state.has("imu")) { - auto imu_data = nav_state.get(std::string("imu")); - if (!imu_data.empty()) { - pose->pose.pose.orientation = imu_data[0]->data.orientation; - } + if (nav_state.has("imu")) { + auto imu_data = nav_state.get(std::string("imu")); + pose->pose.pose.orientation = imu_data.data.orientation; } ukf_global_->setPoseCallback(pose); first_pose_received_ = true; 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 0b127310..0371eab1 100644 --- a/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp @@ -496,9 +496,8 @@ void AMCLLocalizer::update_odom_from_tf() 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; + const auto & imu = nav_state.get("imu"); + const auto & imu_msg = imu.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;} @@ -689,11 +688,11 @@ static ScoreAgg score_particle_sensor_cloud( // ---------- correct() ---------- void AMCLLocalizer::correct(NavState & nav_state) { - if (!nav_state.has_group("points")) { - RCLCPP_WARN(get_node()->get_logger(), "No points perceptions yet"); + const auto & perceptions = nav_state.get_no_group(); + if (perceptions.empty()) { + RCLCPP_WARN(get_node()->get_logger(), "There are no points perceptions"); return; } - const auto & perceptions = nav_state.get_group("points"); if (!nav_state.has("map.bonxai")) { RCLCPP_WARN(get_node()->get_logger(), "No Bonxai map yet"); diff --git a/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp b/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp index 2c50681c..42165823 100644 --- a/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp @@ -368,13 +368,12 @@ AMCLLocalizer::predict([[maybe_unused]] NavState & nav_state) void AMCLLocalizer::correct(NavState & nav_state) { - if (!nav_state.has_group("points")) { - RCLCPP_WARN(get_node()->get_logger(), "There is yet no points perceptions"); + const auto & perceptions = nav_state.get_no_group(); + if (perceptions.empty()) { + RCLCPP_WARN(get_node()->get_logger(), "There are no points perceptions"); return; } - const auto & perceptions = nav_state.get_group("points"); - if (!nav_state.has("map.static")) { RCLCPP_WARN(get_node()->get_logger(), "There is yet no a map.static map"); return; diff --git a/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/filters/ObstacleFilter.cpp b/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/filters/ObstacleFilter.cpp index 36a3dd26..69e41672 100644 --- a/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/filters/ObstacleFilter.cpp +++ b/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/filters/ObstacleFilter.cpp @@ -41,12 +41,12 @@ ObstacleFilter::on_initialize() void ObstacleFilter::update(NavState & nav_state) { - if (!nav_state.has_group("points")) { + const auto & perceptions = nav_state.get_no_group(); + if (perceptions.empty()) { + RCLCPP_WARN(get_node()->get_logger(), "There are no points perceptions"); return; } - const auto & perceptions = nav_state.get_group("points"); - auto dynamic_map_ptr = nav_state.get_ptr("map.dynamic.filtered"); Costmap2D & dynamic_map = *dynamic_map_ptr; const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); diff --git a/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/filters/ObstacleFilter.cpp b/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/filters/ObstacleFilter.cpp index a031c277..87532a09 100644 --- a/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/filters/ObstacleFilter.cpp +++ b/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/filters/ObstacleFilter.cpp @@ -44,9 +44,13 @@ ObstacleFilter::on_initialize() void ObstacleFilter::update(::easynav::NavState & nav_state) { if (!nav_state.has("map.navmap")) {return;} - if (!nav_state.has_group("points")) {return;} - const auto & perceptions = nav_state.get_group("points"); + const auto & perceptions = nav_state.get_no_group(); + if (perceptions.empty()) { + RCLCPP_WARN(get_node()->get_logger(), "There are no points perceptions"); + return; + } + navmap_ = nav_state.get<::navmap::NavMap>("map.navmap"); const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); diff --git a/maps_managers/easynav_octomap_maps_manager/src/easynav_octomap_maps_manager/filters/ObstacleFilter.cpp b/maps_managers/easynav_octomap_maps_manager/src/easynav_octomap_maps_manager/filters/ObstacleFilter.cpp index 987df0c3..105ab371 100644 --- a/maps_managers/easynav_octomap_maps_manager/src/easynav_octomap_maps_manager/filters/ObstacleFilter.cpp +++ b/maps_managers/easynav_octomap_maps_manager/src/easynav_octomap_maps_manager/filters/ObstacleFilter.cpp @@ -45,11 +45,13 @@ ObstacleFilter::update(::easynav::NavState & nav_state) if (!nav_state.has("map")) { return; } - if (!nav_state.has_group("points")) { + + const auto & perceptions = nav_state.get_no_group(); + if (perceptions.empty()) { + RCLCPP_WARN(get_node()->get_logger(), "There are no points perceptions"); return; } - const auto & perceptions = nav_state.get_group("points"); octomap_ = nav_state.get<::octomap::Octomap>("map"); const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); diff --git a/maps_managers/easynav_simple_maps_manager/src/easynav_simple_maps_manager/SimpleMapsManager.cpp b/maps_managers/easynav_simple_maps_manager/src/easynav_simple_maps_manager/SimpleMapsManager.cpp index 446954e7..75691590 100644 --- a/maps_managers/easynav_simple_maps_manager/src/easynav_simple_maps_manager/SimpleMapsManager.cpp +++ b/maps_managers/easynav_simple_maps_manager/src/easynav_simple_maps_manager/SimpleMapsManager.cpp @@ -138,14 +138,17 @@ SimpleMapsManager::update(NavState & nav_state) dynamic_map_.deep_copy(static_map_); - std::cout << "nav_state.has_group(points): " << nav_state.has_group("points") << std::endl; - if (!nav_state.has_group("points")) { + const auto & perceptions = nav_state.get_no_group(); + if (perceptions.empty()) { + RCLCPP_WARN(get_node()->get_logger(), "There are no points perceptions"); + } + + if (perceptions.empty()) { nav_state.set("map.static", static_map_); nav_state.set("map.dynamic", dynamic_map_); return; } - const auto & perceptions = nav_state.get_group("points"); std::cout << "perceptions size: " << perceptions.size() << std::endl; std::cout << "perception[0] size: " << perceptions[0]->data.points.size() << std::endl; std::cout << "perception[0] valid: " << perceptions[0]->valid << std::endl; From cca59aa65552cb7d3c88356e18502910bd0a75fe Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Fri, 3 Apr 2026 11:18:16 +0200 Subject: [PATCH 3/3] Remove group points in tests MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../tests/simple_mapsmanager_tests.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/maps_managers/easynav_simple_maps_manager/tests/simple_mapsmanager_tests.cpp b/maps_managers/easynav_simple_maps_manager/tests/simple_mapsmanager_tests.cpp index 74873bc0..e6adb415 100644 --- a/maps_managers/easynav_simple_maps_manager/tests/simple_mapsmanager_tests.cpp +++ b/maps_managers/easynav_simple_maps_manager/tests/simple_mapsmanager_tests.cpp @@ -69,7 +69,6 @@ TEST_F(SimpleMapsManagerTest, BasicDynamicUpdate) easynav::NavState navstate; auto perception = std::make_shared(); navstate.set("laser", easynav::PointPerception()); - navstate.set_group("points", {"laser"}); perception->data.points.resize(6); perception->data.points[0].x = 1.0;