Reset pose from RViz2 for every localizer

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);
@@ -309,8 +309,14 @@ SerestController::closest_obstacle_distance(
   .downsample(0.3);
   const auto & fused = view.as_points();
 
-  if (last_input_ts_ < view.get_latest_stamp()) {
-    last_input_ts_ = view.get_latest_stamp();
+  {
+    const auto view_latest = view.get_latest_stamp();
+    const rclcpp::Time view_latest_same_clock(
+      view_latest.nanoseconds(),
+      last_input_ts_.get_clock_type());
+    if (last_input_ts_ < view_latest_same_clock) {
+      last_input_ts_ = view_latest_same_clock;
+    }
   }
 
   double min_dist = std::numeric_limits<double>::infinity();
@@ -378,10 +384,10 @@ SerestController::fetch_required_inputs(
   odom = nav_state.get<nav_msgs::msg::Odometry>("robot_pose");
 
   if (rclcpp::Time(path.header.stamp, last_input_ts_.get_clock_type()) > last_input_ts_) {
-    last_input_ts_ = path.header.stamp;
+    last_input_ts_ = rclcpp::Time(path.header.stamp, last_input_ts_.get_clock_type());
   }
   if (rclcpp::Time(odom.header.stamp, last_input_ts_.get_clock_type()) > last_input_ts_) {
-    last_input_ts_ = odom.header.stamp;
+    last_input_ts_ = rclcpp::Time(odom.header.stamp, last_input_ts_.get_clock_type());
   }
 
   if (path.poses.empty()) {

controllers/easynav_simple_controller/src/easynav_simple_controller/SimpleController.cpp

@@ -111,7 +111,10 @@ SimpleController::update_rt(NavState & nav_state)
   const auto & pose = nav_state.get<nav_msgs::msg::Odometry>("robot_pose").pose.pose;
   const auto & goal_pose = path.poses.back().pose;
 
-  rclcpp::Time latest_stamp = nav_state.get<nav_msgs::msg::Odometry>("robot_pose").header.stamp;
+  const auto clock_type = get_node()->get_clock()->get_clock_type();
+  rclcpp::Time latest_stamp(
+    nav_state.get<nav_msgs::msg::Odometry>("robot_pose").header.stamp,
+    clock_type);
   if (rclcpp::Time(path.poses.back().header.stamp,
       latest_stamp.get_clock_type()) > latest_stamp)
   {

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/CMakeLists.txt

@@ -58,7 +58,7 @@ if(BUILD_TESTING)
   ament_lint_auto_find_test_dependencies()
 
   find_package(ament_cmake_gtest REQUIRED)
-  # add_subdirectory(tests)
+  add_subdirectory(tests)
 endif()
 
 ament_export_include_directories("include/${PROJECT_NAME}")

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;