Feature/eskf landmark valve

mission/landmark_server/config/landmark_server_config.yaml

@@ -1,8 +1,7 @@
 /**:
   ros__parameters:
-    target_frame: "odom"
+    target_frame: "orca/odom"
     timer_rate_ms: 200
-    enu_ned_rotation: true
 
     track_config:
       default:

mission/landmark_server/include/landmark_server/landmark_server_ros.hpp

@@ -170,8 +170,6 @@ class LandmarkServerNode : public rclcpp::Node {
     std::shared_ptr<tf2_ros::Buffer> tf2_buffer_;
     std::shared_ptr<tf2_ros::TransformListener> tf2_listener_;
 
-    bool enu_ned_rotation_{false};
-
     std::shared_ptr<LandmarkPollingGoalHandle> active_landmark_polling_goal_;
     std::shared_ptr<LandmarkConvergenceGoalHandle>
         active_landmark_convergence_goal_;

mission/landmark_server/src/landmark_server_convergence.cpp

@@ -1,6 +1,7 @@
 #include <spdlog/spdlog.h>
 #include <rclcpp_action/client.hpp>
 #include <vortex/utils/ros/ros_conversions.hpp>
+#include <vortex/utils/waypoint_utils.hpp>
 #include <vortex_msgs/msg/waypoint.hpp>
 #include <vortex_msgs/msg/waypoint_mode.hpp>
 #include "landmark_server/landmark_server_ros.hpp"
@@ -464,27 +465,13 @@ geometry_msgs::msg::Pose LandmarkServerNode::compute_target_pose(
     const vortex::filtering::Track& track,
     const geometry_msgs::msg::Pose& convergence_offset) {
     const auto landmark_pose = track.to_pose();
-
-    const Eigen::Vector3d p_landmark = landmark_pose.pos_vector();
-    const Eigen::Quaterniond q_landmark =
-        landmark_pose.ori_quaternion().normalized();
-
-    const Eigen::Vector3d p_offset(convergence_offset.position.x,
-                                   convergence_offset.position.y,
-                                   convergence_offset.position.z);
-
-    const Eigen::Quaterniond q_offset =
-        Eigen::Quaterniond(
-            convergence_offset.orientation.w, convergence_offset.orientation.x,
-            convergence_offset.orientation.y, convergence_offset.orientation.z)
-            .normalized();
-
-    const Eigen::Vector3d p_target = p_landmark + p_offset;
-
-    const Eigen::Quaterniond q_target = (q_landmark * q_offset).normalized();
-
-    return vortex::utils::ros_conversions::eigen_to_pose_msg(p_target,
-                                                             q_target);
+    const auto base = vortex::utils::types::Pose::from_eigen(
+        landmark_pose.pos_vector(), landmark_pose.ori_quaternion());
+    const auto offset =
+        vortex::utils::ros_conversions::ros_pose_to_pose(convergence_offset);
+    const auto target =
+        vortex::utils::waypoints::apply_pose_offset(base, offset);
+    return vortex::utils::ros_conversions::to_pose_msg(target);
 }
 
 }  // namespace vortex::mission

mission/landmark_server/src/landmark_server_ros.cpp

@@ -65,7 +65,6 @@ void LandmarkServerNode::create_reference_publisher() {
 void LandmarkServerNode::create_pose_subscription() {
     std::string landmark_topic =
         this->declare_parameter<std::string>("topics.landmarks");
-    enu_ned_rotation_ = this->declare_parameter<bool>("enu_ned_rotation");
     target_frame_ = this->declare_parameter<std::string>("target_frame");
     auto qos_sensor_profile =
         vortex::utils::qos_profiles::sensor_data_profile(1);
@@ -98,12 +97,6 @@ void LandmarkServerNode::create_pose_subscription() {
             }
 
             auto new_measurements = ros_msg_to_landmarks(pose_tf);
-            if (enu_ned_rotation_) {
-                std::ranges::for_each(new_measurements, [](auto& m) {
-                    m.pose.set_ori(vortex::utils::math::enu_ned_rotation(
-                        m.pose.ori_quaternion()));
-                });
-            }
             {
                 std::lock_guard<std::mutex> lock(measurements_mtx_);
                 measurements_ = std::move(new_measurements);
@@ -273,6 +266,15 @@ void LandmarkServerNode::timer_callback() {
 
     convergence_update();
 
+    if (active_landmark_polling_goal_ &&
+        active_landmark_polling_goal_->is_canceling()) {
+        auto result =
+            std::make_shared<vortex_msgs::action::LandmarkPolling_Result>();
+        active_landmark_polling_goal_->canceled(result);
+        active_landmark_polling_goal_ = nullptr;
+        return;
+    }
+
     if (active_landmark_polling_goal_ &&
         active_landmark_polling_goal_->is_active()) {
         const auto goal = active_landmark_polling_goal_->get_goal();
@@ -294,6 +296,9 @@ void LandmarkServerNode::timer_callback() {
         if (!found) {
             return;
         }
+        spdlog::info(
+            "LandmarkPolling: found landmark(s) for type={}, subtype={}", type,
+            subtype);
         vortex_msgs::msg::LandmarkArray landmarks =
             tracks_to_landmark_msgs(type, subtype);
         auto polling_result =

navigation/eskf/CMakeLists.txt

@@ -36,39 +36,65 @@ include_directories(${EIGEN3_INCLUDE_DIR})
 
 include_directories(include)
 
-set(LIB_NAME "${PROJECT_NAME}_component")
+set(CORE_LIB_NAME "${PROJECT_NAME}")
 
-add_library(${LIB_NAME} SHARED
-  src/eskf.cpp
-  src/eskf_ros.cpp
+add_library(${CORE_LIB_NAME} SHARED
+  src/lib/eskf.cpp
 )
 
-ament_target_dependencies(${LIB_NAME} PUBLIC
+target_include_directories(${CORE_LIB_NAME} PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>
+)
+
+ament_target_dependencies(${CORE_LIB_NAME}
+  Eigen3
+  vortex_utils
+)
+
+add_library(${PROJECT_NAME}::${CORE_LIB_NAME} ALIAS ${CORE_LIB_NAME})
+
+set(COMPONENT_LIB_NAME "${PROJECT_NAME}_component")
+
+set(LANDMARK_DIR "${CMAKE_CURRENT_SOURCE_DIR}/../landmark_egomotion")
+
+add_library(${COMPONENT_LIB_NAME} SHARED
+  src/ros/eskf_ros.cpp
+  ${LANDMARK_DIR}/src/lib/landmark_egomotion.cpp
+)
+
+target_include_directories(${COMPONENT_LIB_NAME} PRIVATE
+  ${LANDMARK_DIR}/include
+)
+
+ament_target_dependencies(${COMPONENT_LIB_NAME}
   rclcpp
   rclcpp_components
+  Eigen3
   geometry_msgs
   nav_msgs
-  Eigen3
   tf2
+  tf2_ros
+  tf2_eigen
   vortex_msgs
   vortex_utils
   vortex_utils_ros
   spdlog
   fmt
-  tf2_ros
-  tf2_eigen
 )
 
+target_link_libraries(${COMPONENT_LIB_NAME} ${PROJECT_NAME})
+
 rclcpp_components_register_node(
-  ${LIB_NAME}
+  ${COMPONENT_LIB_NAME}
   PLUGIN "ESKFNode"
   EXECUTABLE ${PROJECT_NAME}_node
 )
 
-ament_export_targets(export_${LIB_NAME})
+ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)
 
-install(TARGETS ${LIB_NAME}
-  EXPORT export_${LIB_NAME}
+install(TARGETS ${CORE_LIB_NAME} ${COMPONENT_LIB_NAME}
+  EXPORT export_${PROJECT_NAME}
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION bin
@@ -79,10 +105,22 @@ install(
   DESTINATION include
 )
 
+install(
+  DIRECTORY ${LANDMARK_DIR}/include/
+  DESTINATION include
+)
+
 install(DIRECTORY
   launch
   config
   DESTINATION share/${PROJECT_NAME}/
 )
 
+ament_export_dependencies(
+  Eigen3
+  vortex_utils
+)
+ament_export_include_directories(include)
+ament_export_libraries(${CORE_LIB_NAME})
+
 ament_package()

navigation/eskf/config/eskf_params.yaml

@@ -1,6 +1,8 @@
 /**:
   eskf_node:
     ros__parameters:
+      dvl_topic: /dvl/sim
+      odom_topic: odom_ESKF
       diag_Q_std: [0.05, 0.05, 0.1, 0.01, 0.01, 0.02, 0.001, 0.001, 0.001, 0.0001, 0.0001, 0.0001]
       diag_p_init: [1.0, 1.0, 1.0, 0.5, 0.5, 0.5, 0.1, 0.1, 0.1, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001]
       transform:
@@ -30,3 +32,29 @@
       atmospheric_pressure: 101000.0 # [Pa]
       water_density: 1026.0 # [kg/m3]
       gravity: 9.82841
+
+      use_landmark_egomotion: true
+      vo:
+        landmarks_topic: /aruco_detector/landmarks
+
+        base_frame: orca/base_link
+        camera_frame: orca/front_camera_optical
+
+        nis_gate_pose: 16.812
+        nis_gate_velocity: 11.345
+        disable_gating: true
+        dropout_timeout: 10.0
+        rejects_limit: 5
+
+        pos_floor: 0.05
+        att_floor: 0.02
+        vel_floor: 0.2
+        dt_min: 0.02
+        dt_max: 15.0
+
+        # Sliding-window smoother
+        use_sw: true
+        window_size: 20
+        max_age: 15.0
+        huber_deg: 20.0
+        gate_deg: 60.0

navigation/eskf/config/eskf_params_real_world.yaml

@@ -31,6 +31,32 @@
       water_density: 1026.0 # [kg/m3]
       gravity: 9.82841
 
+      use_landmark_egomotion: true
+      vo:
+        landmarks_topic: /aruco_detector/landmarks
+
+        base_frame: nautilus/base_link
+        camera_frame: "/cam/camera_info"
+
+        nis_gate_pose: 16.812
+        nis_gate_velocity: 11.345
+        disable_gating: true
+        dropout_timeout: 10.0
+        rejects_limit: 5
+
+        pos_floor: 0.1
+        att_floor: 0.05
+        vel_floor: 0.4
+        dt_min: 0.02
+        dt_max: 15.0
+
+        # Sliding-window smoother
+        use_sw: true
+        window_size: 20
+        max_age: 15.0
+        huber_deg: 20.0
+        gate_deg: 60.0
+
 # USED SENSORS
   # IMU: Kongsberg mini MRU
   # DVL & Depth: Nucleus 1000

navigation/eskf/include/eskf/eskf.hpp → navigation/eskf/include/eskf/lib/eskf.hpp

@@ -1,14 +1,15 @@
-#ifndef ESKF__ESKF_HPP_
-#define ESKF__ESKF_HPP_
+#ifndef ESKF__LIB__ESKF_HPP_
+#define ESKF__LIB__ESKF_HPP_
 
 #include <eigen3/Eigen/Dense>
 #include <utility>
-#include "eskf/typedefs.hpp"
+#include "eskf/lib/typedefs.hpp"
 #include "typedefs.hpp"
 
 class ESKF {
    public:
     explicit ESKF(const EskfParams& params);
+    virtual ~ESKF() = default;
 
     // @brief Update the nominal state and error state
     // @param imu_meas: IMU measurement
@@ -29,6 +30,16 @@ class ESKF {
 
     inline Eigen::Vector3d get_gravity() const { return g_; }
 
+   protected:
+    // @brief Inject the error state into the nominal state and reset the error
+    void injection_and_reset();
+
+    // Member variable for the current error state
+    StateEuler current_error_state_{};
+
+    // Member variable for the current nominal state
+    StateQuat current_nom_state_{};
+
    private:
     // @brief Predict the nominal state
     // @param imu_meas: IMU measurement
@@ -56,9 +67,6 @@ class ESKF {
     template <SensorModelConcept SensorT>
     void measurement_update(const SensorT& meas);
 
-    // @brief Inject the error state into the nominal state and reset the error
-    void injection_and_reset();
-
     // @brief Van Loan discretization
     // @param A_c: Continuous state transition matrix
     // @param G_c: Continuous input matrix
@@ -74,12 +82,6 @@ class ESKF {
     // Normalized Innovation Squared
     double nis_{};
 
-    // Member variable for the current error state
-    StateEuler current_error_state_{};
-
-    // Member variable for the current nominal state
-    StateQuat current_nom_state_{};
-
     // gravity
     Eigen::Vector3d g_{0.0, 0.0, -9.82841};
 
@@ -107,4 +109,4 @@ double compute_nis(const Eigen::VectorXd& innovation, const Eigen::MatrixXd& S);
 
 #include "eskf.tpp"  // including template implementation
 
-#endif  // ESKF__ESKF_HPP_
+#endif  // ESKF__LIB__ESKF_HPP_

navigation/eskf/include/eskf/typedefs.hpp → navigation/eskf/include/eskf/lib/typedefs.hpp

@@ -2,8 +2,8 @@
  * @file typedefs.hpp
  * @brief Contains the typedef and structs for the eskf.
  */
-#ifndef ESKF__TYPEDEFS_HPP_
-#define ESKF__TYPEDEFS_HPP_
+#ifndef ESKF__LIB__TYPEDEFS_HPP_
+#define ESKF__LIB__TYPEDEFS_HPP_
 
 #include <concepts>
 #include <eigen3/Eigen/Dense>
@@ -135,4 +135,4 @@ struct SensorDepth {
     Eigen::MatrixXd noise_covariance() const;
 };
 
-#endif  // ESKF__TYPEDEFS_HPP_
+#endif  // ESKF__LIB__TYPEDEFS_HPP_