Update plugins to new sensors API

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"
 

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("points")) {
+  const auto & perceptions = nav_state.get_no_group<PointPerception>();
+
+  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<PointPerceptions>("points");
   const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info();
 
   const auto & filtered = PointPerceptionsOpsView(perceptions)

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<nav_msgs::msg::Odometry>("robot_pose").pose.pose;
-  const auto & perceptions = nav_state.get<PointPerceptions>("points");
+  const auto & perceptions = nav_state.get_no_group<PointPerception>();
   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})

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<double>::infinity();}
+  if (!nav_state.has_group("points")) {return std::numeric_limits<double>::infinity();}
 
-  const auto & perceptions = nav_state.get<PointPerceptions>("points");
+  const auto & perceptions = nav_state.get_no_group<PointPerception>();
   const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info();
 
   auto view = PointPerceptionsOpsView(perceptions);

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<PointPerceptions>("points");
+    const auto & perceptions = nav_state.get_no_group<PointPerception>();
 
     const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info();
     auto fused =

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,13 +566,12 @@ AMCLLocalizer::predict([[maybe_unused]] NavState & nav_state)
 void
 AMCLLocalizer::correct(NavState & nav_state)
 {
-  if (!nav_state.has("points")) {
-    RCLCPP_WARN(get_node()->get_logger(), "There is yet no points perceptions");
+  const auto & perceptions = nav_state.get_no_group<PointPerception>();
+  if (perceptions.empty()) {
+    RCLCPP_WARN(get_node()->get_logger(), "There are no points perceptions");
     return;
   }
 
-  const auto & perceptions = nav_state.get<PointPerceptions>("points");
-
   if (!nav_state.has("map.static")) {
     RCLCPP_WARN(get_node()->get_logger(), "There is yet no a map.static map");
     return;

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 <GeographicLib/UTMUPS.hpp>
@@ -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<IMUPerceptions>(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<IMUPerception>(std::string("imu"));
+              pose->pose.pose.orientation = imu_data.data.orientation;
             }
             ukf_global_->setPoseCallback(pose);
             first_pose_received_ = true;

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"
 
@@ -496,9 +496,8 @@ void AMCLLocalizer::update_odom_from_tf()
 std::optional<tf2::Quaternion> get_latest_imu_quat(const NavState & nav_state)
 {
   if (!nav_state.has("imu")) {return std::nullopt;}
-  const auto & imus = nav_state.get<IMUPerceptions>("imu");
-  if (imus.empty() || !imus.back()) {return std::nullopt;}
-  const auto & imu_msg = imus.back()->data;
+  const auto & imu = nav_state.get<IMUPerception>("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("points")) {
-    RCLCPP_WARN(get_node()->get_logger(), "No points perceptions yet");
+  const auto & perceptions = nav_state.get_no_group<PointPerception>();
+  if (perceptions.empty()) {
+    RCLCPP_WARN(get_node()->get_logger(), "There are no points perceptions");
     return;
   }
-  const auto & perceptions = nav_state.get<PointPerceptions>("points");
 
   if (!nav_state.has("map.bonxai")) {
     RCLCPP_WARN(get_node()->get_logger(), "No Bonxai map yet");

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,13 +368,12 @@ AMCLLocalizer::predict([[maybe_unused]] NavState & nav_state)
 
 void AMCLLocalizer::correct(NavState & nav_state)
 {
-  if (!nav_state.has("points")) {
-    RCLCPP_WARN(get_node()->get_logger(), "There is yet no points perceptions");
+  const auto & perceptions = nav_state.get_no_group<PointPerception>();
+  if (perceptions.empty()) {
+    RCLCPP_WARN(get_node()->get_logger(), "There are no points perceptions");
     return;
   }
 
-  const auto & perceptions = nav_state.get<PointPerceptions>("points");
-
   if (!nav_state.has("map.static")) {
     RCLCPP_WARN(get_node()->get_logger(), "There is yet no a map.static map");
     return;

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,12 +41,12 @@ ObstacleFilter::on_initialize()
 void
 ObstacleFilter::update(NavState & nav_state)
 {
-  if (!nav_state.has("points")) {
+  const auto & perceptions = nav_state.get_no_group<PointPerception>();
+  if (perceptions.empty()) {
+    RCLCPP_WARN(get_node()->get_logger(), "There are no points perceptions");
     return;
   }
 
-  const auto & perceptions = nav_state.get<PointPerceptions>("points");
-
   auto dynamic_map_ptr = nav_state.get_ptr<Costmap2D>("map.dynamic.filtered");
   Costmap2D & dynamic_map = *dynamic_map_ptr;
   const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info();

maps_managers/easynav_costmap_maps_manager/tests/costmap_mapsmanager_tests.cpp

@@ -15,7 +15,7 @@
 
 #include <gtest/gtest.h>
 
-#include "easynav_common/types/PointPerception.hpp"
+#include "easynav_sensors/types/PointPerception.hpp"
 #include "easynav_common/types/NavState.hpp"
 
 #include "rclcpp/rclcpp.hpp"

maps_managers/easynav_navmap_maps_manager/README.md

@@ -59,7 +59,7 @@ All parameters are declared under the plugin namespace, i.e.
 | `<plugin>.height_threshold` | `double` | `0.25` | Minimum vertical height (in meters) between max and min z to mark as an obstacle. |
 | `<plugin>.downsample` | `double` | `0.3` | Voxel size used to downsample point clouds before obstacle detection. |
 | `<plugin>.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**

maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/filters/ObstacleFilter.cpp

@@ -18,7 +18,7 @@
 #include <cstdint>
 
 #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,13 @@ ObstacleFilter::on_initialize()
 void ObstacleFilter::update(::easynav::NavState & nav_state)
 {
   if (!nav_state.has("map.navmap")) {return;}
-  if (!nav_state.has("points")) {return;}
 
-  const auto & perceptions = nav_state.get<PointPerceptions>("points");
+  const auto & perceptions = nav_state.get_no_group<PointPerception>();
+  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();
 

maps_managers/easynav_navmap_maps_manager/tests/navmap_mapsmanager_tests.cpp

@@ -15,7 +15,7 @@
 
 #include <gtest/gtest.h>
 
-#include "easynav_common/types/PointPerception.hpp"
+#include "easynav_sensors/types/PointPerception.hpp"
 #include "easynav_common/types/NavState.hpp"
 
 #include "rclcpp/rclcpp.hpp"

maps_managers/easynav_octomap_maps_manager/src/easynav_octomap_maps_manager/filters/InflationFilter.cpp

@@ -42,8 +42,8 @@
 #include <string>
 
 #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"
 

maps_managers/easynav_octomap_maps_manager/src/easynav_octomap_maps_manager/filters/ObstacleFilter.cpp

@@ -17,8 +17,8 @@
 #include <string>
 
 #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,13 @@ ObstacleFilter::update(::easynav::NavState & nav_state)
   if (!nav_state.has("map")) {
     return;
   }
-  if (!nav_state.has("points")) {
+
+  const auto & perceptions = nav_state.get_no_group<PointPerception>();
+  if (perceptions.empty()) {
+    RCLCPP_WARN(get_node()->get_logger(), "There are no points perceptions");
     return;
   }
 
-  const auto & perceptions = nav_state.get<PointPerceptions>("points");
   octomap_ = nav_state.get<::octomap::Octomap>("map");
   const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info();
 

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,20 @@ SimpleMapsManager::update(NavState & nav_state)
 
   dynamic_map_.deep_copy(static_map_);
 
-  if (!nav_state.has("points")) {
+  const auto & perceptions = nav_state.get_no_group<PointPerception>();
+  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<PointPerceptions>("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;

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,7 @@ TEST_F(SimpleMapsManagerTest, BasicDynamicUpdate)
 
   easynav::NavState navstate;
   auto perception = std::make_shared<easynav::PointPerception>();
-  navstate.set("points", easynav::PointPerceptions());
+  navstate.set("laser", easynav::PointPerception());
 
   perception->data.points.resize(6);
   perception->data.points[0].x = 1.0;
@@ -94,9 +94,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);