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..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("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("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..f80ca7ff 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_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 659dd4e6..9a32f1ff 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_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 65d5e626..1fe63e46 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_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 30c3bbbb..7c26a2c0 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,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(); + if (perceptions.empty()) { + RCLCPP_WARN(get_node()->get_logger(), "There are no points perceptions"); return; } - const auto & perceptions = nav_state.get("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 7982a9a7..f1cd27f5 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 @@ -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 79117a91..0371eab1 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" @@ -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("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("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..42165823 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,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(); + if (perceptions.empty()) { + RCLCPP_WARN(get_node()->get_logger(), "There are no points perceptions"); return; } - const auto & perceptions = nav_state.get("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 b354f31e..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 @@ -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(); + if (perceptions.empty()) { + RCLCPP_WARN(get_node()->get_logger(), "There are no points perceptions"); return; } - const auto & perceptions = nav_state.get("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_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..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 @@ -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,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("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_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..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 @@ -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,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(); + if (perceptions.empty()) { + RCLCPP_WARN(get_node()->get_logger(), "There are no points perceptions"); return; } - const auto & perceptions = nav_state.get("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..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 @@ -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(); + 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("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..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 @@ -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(); - 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);