diff --git a/.github/workflows/ros-node-tests.yml b/.github/workflows/ros-node-tests.yml index 4200df85..5031c027 100644 --- a/.github/workflows/ros-node-tests.yml +++ b/.github/workflows/ros-node-tests.yml @@ -16,6 +16,7 @@ jobs: setup_script: "tests/setup.sh" test_scripts: '[ "tests/ros_node_tests/dp_node_test.sh", + "tests/ros_node_tests/eskf_node_test.sh", "tests/ros_node_tests/lqr_autopilot_node_test.sh", "tests/ros_node_tests/reference_filter_node_test.sh", "tests/ros_node_tests/los_node_test.sh", diff --git a/navigation/eskf/CMakeLists.txt b/navigation/eskf/CMakeLists.txt new file mode 100644 index 00000000..e9f46490 --- /dev/null +++ b/navigation/eskf/CMakeLists.txt @@ -0,0 +1,78 @@ +cmake_minimum_required(VERSION 3.8) +project(eskf) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 20) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(tf2 REQUIRED) +find_package(vortex_msgs REQUIRED) +find_package(vortex_utils REQUIRED) +find_package(spdlog REQUIRED) +find_package(fmt REQUIRED) + +if(NOT DEFINED EIGEN3_INCLUDE_DIR) + set(EIGEN3_INCLUDE_DIR ${EIGEN3_INCLUDE_DIRS}) +endif() + +include_directories(${EIGEN3_INCLUDE_DIR}) + +include_directories(include) + +set(LIB_NAME "${PROJECT_NAME}_component") + +add_library(${LIB_NAME} SHARED + src/eskf.cpp + src/eskf_ros.cpp +) + +ament_target_dependencies(${LIB_NAME} PUBLIC + rclcpp + rclcpp_components + geometry_msgs + nav_msgs + Eigen3 + tf2 + vortex_msgs + vortex_utils + spdlog + fmt +) + +rclcpp_components_register_node( + ${LIB_NAME} + PLUGIN "ESKFNode" + EXECUTABLE ${PROJECT_NAME}_node +) + +ament_export_targets(export_${LIB_NAME}) + +install(TARGETS ${LIB_NAME} + EXPORT export_${LIB_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install( + DIRECTORY include/ + DESTINATION include +) + +install(DIRECTORY + launch + config + DESTINATION share/${PROJECT_NAME}/ +) + +ament_package() diff --git a/navigation/eskf/config/eskf_params.yaml b/navigation/eskf/config/eskf_params.yaml new file mode 100644 index 00000000..f17dcd3d --- /dev/null +++ b/navigation/eskf/config/eskf_params.yaml @@ -0,0 +1,8 @@ +eskf_node: + ros__parameters: + imu_topic: /imu/data_raw + dvl_topic: /orca/twist + odom_topic: odom + diag_Q_std: [0.0124, 0.0124, 0.0124, 0.0026, 0.0026, 0.0026, 0.000392, 0.000392, 0.000392, 0.00001145, 0.0000145, 0.0000145] + 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, 0.001, 0.001, 0.001] + imu_frame: [0.0, 0.0, -1.0, 0.0, -1.0, 0.0, -1.0, 0.0, 0.0] diff --git a/navigation/eskf/include/eskf/eskf.hpp b/navigation/eskf/include/eskf/eskf.hpp new file mode 100644 index 00000000..6219335c --- /dev/null +++ b/navigation/eskf/include/eskf/eskf.hpp @@ -0,0 +1,87 @@ +#ifndef ESKF_HPP +#define ESKF_HPP + +#include +#include +#include "eskf/typedefs.hpp" +#include "typedefs.hpp" + +class ESKF { + public: + ESKF(const EskfParams& params); + + // @brief Update the nominal state and error state + // @param imu_meas: IMU measurement + // @param dt: Time step + void imu_update(const ImuMeasurement& imu_meas, const double dt); + + // @brief Update the nominal state and error state + // @param dvl_meas: DVL measurement + void dvl_update(const DvlMeasurement& dvl_meas); + + inline StateQuat get_nominal_state() const { return current_nom_state_; } + + inline double get_nis() const { return nis_; } + + private: + // @brief Predict the nominal state + // @param imu_meas: IMU measurement + // @param dt: Time step + // @return Predicted nominal state + void nominal_state_discrete(const ImuMeasurement& imu_meas, + const double dt); + + // @brief Predict the error state + // @param imu_meas: IMU measurement + // @param dt: Time step + // @return Predicted error state + void error_state_prediction(const ImuMeasurement& imu_meas, + const double dt); + + // @brief Calculate the NIS + // @param innovation: Innovation vector + // @param S: Innovation covariance matrix + void NIS(const Eigen::Vector3d& innovation, const Eigen::Matrix3d& S); + + // @brief Update the error state + // @param dvl_meas: DVL measurement + void measurement_update(const DvlMeasurement& dvl_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 + // @return Discrete state transition matrix and discrete input matrix + std::pair van_loan_discretization( + const Eigen::Matrix18d& A_c, + const Eigen::Matrix18x12d& G_c, + const double dt); + + // @brief Calculate the measurement matrix jakobian + // @return Measurement matrix + Eigen::Matrix3x19d calculate_hx(); + + // @brief Calculate the full measurement matrix + // @return Measurement matrix + Eigen::Matrix3x18d calculate_h_jacobian(); + + // @brief Calculate the measurement + // @return Measurement + Eigen::Vector3d calculate_h(); + + // Process noise covariance matrix + Eigen::Matrix12d Q_{}; + + // 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_{}; +}; + +#endif // ESKF_HPP diff --git a/navigation/eskf/include/eskf/eskf_ros.hpp b/navigation/eskf/include/eskf/eskf_ros.hpp new file mode 100644 index 00000000..4a9e699a --- /dev/null +++ b/navigation/eskf/include/eskf/eskf_ros.hpp @@ -0,0 +1,64 @@ +#ifndef ESKF_ROS_HPP +#define ESKF_ROS_HPP + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "eskf/eskf.hpp" +#include "eskf/typedefs.hpp" +#include "spdlog/spdlog.h" + +class ESKFNode : public rclcpp::Node { + public: + explicit ESKFNode( + const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); + + private: + // @brief Callback function for the imu topic + // @param msg: Imu message containing the imu data + void imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg); + + // @brief Callback function for the dvl topic + // @param msg: TwistWithCovarianceStamped message containing the dvl data + void dvl_callback( + const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg); + + // @brief Publish the odometry message + void publish_odom(); + + // @brief Set the subscriber and publisher for the node + void set_subscribers_and_publisher(); + + // @brief Set the parameters for the eskf + void set_parameters(); + + rclcpp::Subscription::SharedPtr imu_sub_; + + rclcpp::Subscription< + geometry_msgs::msg::TwistWithCovarianceStamped>::SharedPtr dvl_sub_; + + rclcpp::Publisher::SharedPtr odom_pub_; + + rclcpp::Publisher::SharedPtr nis_pub_; + + std::chrono::milliseconds time_step; + + rclcpp::TimerBase::SharedPtr odom_pub_timer_; + + std::unique_ptr eskf_; + + bool first_imu_msg_received_ = false; + + Eigen::Matrix3d R_imu_eskf_{}; + + rclcpp::Time last_imu_time_{}; +}; + +#endif // ESKF_ROS_HPP diff --git a/navigation/eskf/include/eskf/typedefs.hpp b/navigation/eskf/include/eskf/typedefs.hpp new file mode 100644 index 00000000..9360a590 --- /dev/null +++ b/navigation/eskf/include/eskf/typedefs.hpp @@ -0,0 +1,125 @@ +/** + * @file typedefs.hpp + * @brief Contains the typedef and structs for the eskf. + */ +#ifndef ESKF_TYPEDEFS_H +#define ESKF_TYPEDEFS_H + +#include +#include +#include + +namespace Eigen { +typedef Eigen::Matrix Vector19d; +typedef Eigen::Matrix Vector18d; +typedef Eigen::Matrix Matrix18d; +typedef Eigen::Matrix Matrix19d; +typedef Eigen::Matrix Matrix18x12d; +typedef Eigen::Matrix Matrix4x3d; +typedef Eigen::Matrix Matrix3x19d; +typedef Eigen::Matrix Matrix3x18d; +typedef Eigen::Matrix Matrix12d; +typedef Eigen::Matrix Matrix18d; +typedef Eigen::Matrix Matrix3x1d; +typedef Eigen::Matrix Matrix19x18d; +typedef Eigen::Matrix Matrix18x3d; +typedef Eigen::Matrix Matrix36d; +typedef Eigen::Matrix Matrix6d; +typedef Eigen::Matrix Matrix9d; +typedef Eigen::Matrix Matrix15d; +typedef Eigen::Matrix Vector15d; +typedef Eigen::Matrix Vector12d; +} // namespace Eigen + +template +Eigen::Matrix createDiagonalMatrix( + const std::vector& diag) { + return Eigen::Map>(diag.data()) + .asDiagonal(); +} +struct StateQuat { + Eigen::Vector3d pos = Eigen::Vector3d::Zero(); + Eigen::Vector3d vel = Eigen::Vector3d::Zero(); + Eigen::Quaterniond quat = Eigen::Quaterniond::Identity(); + Eigen::Vector3d gyro_bias = Eigen::Vector3d::Zero(); + Eigen::Vector3d accel_bias = Eigen::Vector3d::Zero(); + Eigen::Vector3d gravity = Eigen::Vector3d(0, 0, 9.81); + + StateQuat() = default; + + Eigen::Vector19d as_vector() const { + Eigen::Vector19d vec{}; + vec << pos, vel, quat.w(), quat.x(), quat.y(), quat.z(), gyro_bias, + accel_bias, gravity; + return vec; + } + + Eigen::Vector18d nees_error(const StateQuat& other) const { + Eigen::Vector18d vec{}; + Eigen::Vector3d euler_diff{}; + + euler_diff = (quat * other.quat.inverse()) + .toRotationMatrix() + .eulerAngles(0, 1, 2) + + Eigen::Vector3d(-M_PI, M_PI, -M_PI); + + vec << pos - other.pos, vel - other.vel, euler_diff, + gyro_bias - other.gyro_bias, accel_bias - other.accel_bias, + gravity - other.gravity; + return vec; + } + + StateQuat operator-(const StateQuat& other) const { + StateQuat diff{}; + diff.pos = pos - other.pos; + diff.vel = vel - other.vel; + diff.quat = quat * other.quat.inverse(); + diff.gyro_bias = gyro_bias - other.gyro_bias; + diff.accel_bias = accel_bias - other.accel_bias; + diff.gravity = gravity - other.gravity; + return diff; + } +}; + +struct StateEuler { + Eigen::Vector3d pos = Eigen::Vector3d::Zero(); + Eigen::Vector3d vel = Eigen::Vector3d::Zero(); + Eigen::Vector3d euler = Eigen::Vector3d::Zero(); + Eigen::Vector3d gyro_bias = Eigen::Vector3d::Zero(); + Eigen::Vector3d accel_bias = Eigen::Vector3d::Zero(); + Eigen::Vector3d gravity = Eigen::Vector3d(0, 0, 9.81); + + Eigen::Matrix18d covariance = Eigen::Matrix18d::Zero(); + + Eigen::Vector18d as_vector() const { + Eigen::Vector18d vec{}; + vec << pos, vel, euler, gyro_bias, accel_bias, gravity; + return vec; + } + + void set_from_vector(const Eigen::Vector18d& vec) { + pos = vec.block<3, 1>(0, 0); + vel = vec.block<3, 1>(3, 0); + euler = vec.block<3, 1>(6, 0); + gyro_bias = vec.block<3, 1>(9, 0); + accel_bias = vec.block<3, 1>(12, 0); + gravity = vec.block<3, 1>(15, 0); + } +}; + +struct ImuMeasurement { + Eigen::Vector3d accel = Eigen::Vector3d::Zero(); + Eigen::Vector3d gyro = Eigen::Vector3d::Zero(); +}; + +struct DvlMeasurement { + Eigen::Vector3d vel = Eigen::Vector3d::Zero(); + Eigen::Matrix3d cov = Eigen::Matrix3d::Zero(); +}; + +struct EskfParams { + Eigen::Matrix12d Q = Eigen::Matrix12d::Zero(); + Eigen::Matrix18d P = Eigen::Matrix18d::Zero(); +}; + +#endif // ESKF_TYPEDEFS_H diff --git a/navigation/eskf/launch/eskf.launch.py b/navigation/eskf/launch/eskf.launch.py new file mode 100644 index 00000000..d7b3a8a4 --- /dev/null +++ b/navigation/eskf/launch/eskf.launch.py @@ -0,0 +1,23 @@ +from os import path + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node + +eskf_params = path.join( + get_package_share_directory("eskf"), "config", "eskf_params.yaml" +) + + +def generate_launch_description(): + eskf_node = Node( + package="eskf", + executable="eskf_node", + name="eskf_node", + # namespace="orca", + parameters=[ + eskf_params, + ], + output="screen", + ) + return LaunchDescription([eskf_node]) diff --git a/navigation/eskf/package.xml b/navigation/eskf/package.xml new file mode 100644 index 00000000..58885560 --- /dev/null +++ b/navigation/eskf/package.xml @@ -0,0 +1,23 @@ + + + + eskf + 2.0.0 + Error-state Kalman filter + talhanc + MIT + + ament_cmake + + rclcpp + geometry_msgs + nav_msgs + eigen + tf2 + vortex_msgs + vortex_utils + + + ament_cmake + + diff --git a/navigation/eskf/src/eskf.cpp b/navigation/eskf/src/eskf.cpp new file mode 100644 index 00000000..a478be43 --- /dev/null +++ b/navigation/eskf/src/eskf.cpp @@ -0,0 +1,193 @@ +#include "eskf/eskf.hpp" +#include +#include +#include +#include +#include "eskf/typedefs.hpp" + +double compute_nis(const Eigen::Vector3d& innovation, + const Eigen::Matrix3d& S) { + Eigen::Matrix3d S_inv = S.inverse(); + return innovation.transpose() * S_inv * innovation; +} + +ESKF::ESKF(const EskfParams& params) : Q_(params.Q) { + // current_error_state_.covariance = params.P; +} + +std::pair ESKF::van_loan_discretization( + const Eigen::Matrix18d& A_c, + const Eigen::Matrix18x12d& G_c, + const double dt) { + Eigen::Matrix18d GQG_T = G_c * Q_ * G_c.transpose(); + Eigen::Matrix36d vanLoanMat = Eigen::Matrix36d::Zero(); + + vanLoanMat.topLeftCorner<18, 18>() = -A_c; + vanLoanMat.topRightCorner<18, 18>() = GQG_T; + vanLoanMat.bottomRightCorner<18, 18>() = A_c.transpose(); + + Eigen::Matrix36d vanLoanExp = (vanLoanMat * dt).exp(); + + Eigen::Matrix18d V1 = vanLoanExp.bottomRightCorner<18, 18>().transpose(); + Eigen::Matrix18d V2 = vanLoanExp.topRightCorner<18, 18>(); + + Eigen::Matrix18d A_d = V1; + Eigen::Matrix18d GQG_d = A_d * V2; + + return {A_d, GQG_d}; +} + +Eigen::Matrix3x19d ESKF::calculate_hx() { + Eigen::Matrix3x19d Hx = Eigen::Matrix3x19d::Zero(); + + Eigen::Quaterniond q = current_nom_state_.quat.normalized(); + Eigen::Matrix3d R_bn = q.toRotationMatrix(); + + Eigen::Vector3d v_n = current_nom_state_.vel; + + // Correct derivative w.r.t velocity (nominal state: v_n) + Hx.block<3, 3>(0, 3) = R_bn.transpose(); + + // Derivative w.r.t quaternion (nominal state: q) + // Compute partial derivative w.r.t quaternion directly: + double qw = q.w(); + Eigen::Vector3d q_vec(q.x(), q.y(), q.z()); + Eigen::Matrix3d I3 = Eigen::Matrix3d::Identity(); + + Eigen::Matrix dhdq; + dhdq.col(0) = 2 * (qw * v_n + q_vec.cross(v_n)); + dhdq.block<3, 3>(0, 1) = + 2 * (q_vec.dot(v_n) * I3 + q_vec * v_n.transpose() - + v_n * q_vec.transpose() - + qw * vortex::utils::math::get_skew_symmetric_matrix(v_n)); + + // Assign quaternion derivative (3x4 block at columns 6:9) + Hx.block<3, 4>(0, 6) = dhdq; + + return Hx; +} + +Eigen::Matrix3x18d ESKF::calculate_h_jacobian() { + Eigen::Matrix19x18d x_delta = Eigen::Matrix19x18d::Zero(); + x_delta.block<6, 6>(0, 0) = Eigen::Matrix6d::Identity(); + x_delta.block<4, 3>(6, 6) = + vortex::utils::math::get_transformation_matrix_attitude_quat( + current_nom_state_.quat); + x_delta.block<9, 9>(10, 9) = Eigen::Matrix9d::Identity(); + + Eigen::Matrix3x18d H = calculate_hx() * x_delta; + return H; +} + +Eigen::Vector3d ESKF::calculate_h() { + Eigen::Vector3d h; + Eigen::Matrix3d R_bn = + current_nom_state_.quat.normalized().toRotationMatrix().transpose(); + + h = R_bn * current_nom_state_.vel; + // 0.027293, 0.028089, 0.028089, 0.00255253, 0.00270035, 0.00280294, + return h; +} + +void ESKF::nominal_state_discrete(const ImuMeasurement& imu_meas, + const double dt) { + Eigen::Vector3d acc = + current_nom_state_.quat.normalized().toRotationMatrix() * + (imu_meas.accel - current_nom_state_.accel_bias) + + current_nom_state_.gravity; + Eigen::Vector3d gyro = (imu_meas.gyro - current_nom_state_.gyro_bias) * dt; + + current_nom_state_.pos = current_nom_state_.pos + + current_nom_state_.vel * dt + 0.5 * dt * dt * acc; + current_nom_state_.vel = current_nom_state_.vel + dt * acc; + + current_nom_state_.quat = + (current_nom_state_.quat * + vortex::utils::math::eigen_vector3d_to_quaternion(gyro)); + current_nom_state_.quat.normalize(); + + current_nom_state_.gyro_bias = current_nom_state_.gyro_bias; + current_nom_state_.accel_bias = current_nom_state_.accel_bias; + current_nom_state_.gravity = current_nom_state_.gravity; +} + +void ESKF::error_state_prediction(const ImuMeasurement& imu_meas, + const double dt) { + Eigen::Matrix3d R = current_nom_state_.quat.normalized().toRotationMatrix(); + Eigen::Vector3d acc = (imu_meas.accel - current_nom_state_.accel_bias); + Eigen::Vector3d gyro = (imu_meas.gyro - current_nom_state_.gyro_bias); + + Eigen::Matrix18d A_c = Eigen::Matrix18d::Zero(); + A_c.block<3, 3>(0, 3) = Eigen::Matrix3d::Identity(); + A_c.block<3, 3>(3, 6) = + -R * vortex::utils::math::get_skew_symmetric_matrix(acc); + A_c.block<3, 3>(6, 6) = + -vortex::utils::math::get_skew_symmetric_matrix(gyro); + A_c.block<3, 3>(3, 9) = -R; + A_c.block<3, 3>(9, 9) = -Eigen::Matrix3d::Identity(); + A_c.block<3, 3>(12, 12) = -Eigen::Matrix3d::Identity(); + A_c.block<3, 3>(6, 12) = -Eigen::Matrix3d::Identity(); + A_c.block<3, 3>(3, 15) = Eigen::Matrix3d::Identity(); + + Eigen::Matrix18x12d G_c = Eigen::Matrix18x12d::Zero(); + G_c.block<3, 3>(3, 0) = -R; + G_c.block<3, 3>(6, 3) = -Eigen::Matrix3d::Identity(); + G_c.block<3, 3>(9, 6) = Eigen::Matrix3d::Identity(); + G_c.block<3, 3>(12, 9) = Eigen::Matrix3d::Identity(); + + Eigen::Matrix18d A_d, GQG_d; + std::tie(A_d, GQG_d) = van_loan_discretization(A_c, G_c, dt); + + StateEuler next_error_state; + current_error_state_.covariance = + A_d * current_error_state_.covariance * A_d.transpose() + GQG_d; +} + +void ESKF::measurement_update(const DvlMeasurement& dvl_meas) { + Eigen::Matrix3x18d H = calculate_h_jacobian(); + Eigen::Matrix18d P = current_error_state_.covariance; + Eigen::Matrix3d R = dvl_meas.cov; + + Eigen::Matrix3d S = H * P * H.transpose() + R; + Eigen::Matrix18x3d K = P * H.transpose() * S.inverse(); + Eigen::Vector3d innovation = dvl_meas.vel - calculate_h(); + + nis_ = compute_nis(innovation, S); + current_error_state_.set_from_vector(K * innovation); + + Eigen::Matrix18d I_KH = Eigen::Matrix18d::Identity() - K * H; + current_error_state_.covariance = + I_KH * P * I_KH.transpose() + + K * R * K.transpose(); // Used joseph form for more stable calculations +} + +void ESKF::injection_and_reset() { + current_nom_state_.pos = current_nom_state_.pos + current_error_state_.pos; + current_nom_state_.vel = current_nom_state_.vel + current_error_state_.vel; + current_nom_state_.quat = current_nom_state_.quat * + vortex::utils::math::eigen_vector3d_to_quaternion( + current_error_state_.euler); + current_nom_state_.quat.normalize(); + current_nom_state_.gyro_bias = + current_nom_state_.gyro_bias + current_error_state_.gyro_bias; + current_nom_state_.accel_bias = + current_nom_state_.accel_bias + current_error_state_.accel_bias; + current_nom_state_.gravity = + current_nom_state_.gravity + current_error_state_.gravity; + + Eigen::Matrix18d G = Eigen::Matrix18d::Identity(); + + current_error_state_.covariance = + G * current_error_state_.covariance * G.transpose(); + current_error_state_.set_from_vector(Eigen::Vector18d::Zero()); +} + +void ESKF::imu_update(const ImuMeasurement& imu_meas, const double dt) { + nominal_state_discrete(imu_meas, dt); + error_state_prediction(imu_meas, dt); +} + +void ESKF::dvl_update(const DvlMeasurement& dvl_meas) { + measurement_update(dvl_meas); + injection_and_reset(); +} diff --git a/navigation/eskf/src/eskf_ros.cpp b/navigation/eskf/src/eskf_ros.cpp new file mode 100644 index 00000000..02e1c44f --- /dev/null +++ b/navigation/eskf/src/eskf_ros.cpp @@ -0,0 +1,163 @@ +#include "eskf/eskf_ros.hpp" +#include +#include +#include +#include "eskf/typedefs.hpp" + +auto start_message{R"( + ________ ______ ___ ____ ________ + |_ __ |.' ____ \ |_ ||_ _| |_ __ | + | |_ \_|| (___ \_| | |_/ / | |_ \_| + | _| _ _.____`. | __'. | _| + _| |__/ || \____) | _| | \ \_ _| |_ + |________| \______.'|____||____||_____| +)"}; + +ESKFNode::ESKFNode(const rclcpp::NodeOptions& options) + : Node("eskf_node", options) { + // time_step = std::chrono::milliseconds(1); + // odom_pub_timer_ = this->create_wall_timer( + // time_step, std::bind(&ESKFNode::publish_odom, this)); + + set_subscribers_and_publisher(); + + set_parameters(); + + spdlog::info(start_message); +} + +void ESKFNode::set_subscribers_and_publisher() { + auto qos_sensor_data = vortex::utils::qos_profiles::sensor_data_profile(1); + + this->declare_parameter("imu_topic"); + std::string imu_topic = this->get_parameter("imu_topic").as_string(); + imu_sub_ = this->create_subscription( + imu_topic, qos_sensor_data, + std::bind(&ESKFNode::imu_callback, this, std::placeholders::_1)); + + this->declare_parameter("dvl_topic"); + std::string dvl_topic = this->get_parameter("dvl_topic").as_string(); + dvl_sub_ = this->create_subscription< + geometry_msgs::msg::TwistWithCovarianceStamped>( + dvl_topic, qos_sensor_data, + std::bind(&ESKFNode::dvl_callback, this, std::placeholders::_1)); + + this->declare_parameter("odom_topic"); + std::string odom_topic = this->get_parameter("odom_topic").as_string(); + odom_pub_ = this->create_publisher( + odom_topic, qos_sensor_data); + + nis_pub_ = create_publisher( + "dvl/nis", vortex::utils::qos_profiles::reliable_profile()); +} + +void ESKFNode::set_parameters() { + std::vector R_imu_correction; + this->declare_parameter>("imu_frame"); + R_imu_correction = get_parameter("imu_frame").as_double_array(); + R_imu_eskf_ = Eigen::Map>( + R_imu_correction.data()); + + std::vector diag_Q_std; + this->declare_parameter>("diag_Q_std"); + + diag_Q_std = this->get_parameter("diag_Q_std").as_double_array(); + + if (diag_Q_std.size() != 12) { + throw std::runtime_error("diag_Q_std must have length 12"); + } + + Eigen::Matrix12d Q = Eigen::Map(diag_Q_std.data()) + .array() + .square() + .matrix() + .asDiagonal(); + + std::vector diag_p_init = + this->declare_parameter>("diag_p_init"); + if (diag_p_init.size() != 18) { + throw std::runtime_error("diag_p_init must have length 18"); + } + Eigen::Matrix18d P = createDiagonalMatrix<18>(diag_p_init); + EskfParams eskf_params{.Q = Q, .P = P}; + + eskf_ = std::make_unique(eskf_params); +} + +void ESKFNode::imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg) { + rclcpp::Time current_time = msg->header.stamp; + + if (!first_imu_msg_received_) { + last_imu_time_ = current_time; + first_imu_msg_received_ = true; + return; + } + + double dt = (current_time - last_imu_time_).nanoseconds() * 1e-9; + last_imu_time_ = current_time; + + Eigen::Vector3d raw_accel(msg->linear_acceleration.x, + msg->linear_acceleration.y, + msg->linear_acceleration.z); + + ImuMeasurement imu_measurement{}; + imu_measurement.accel = R_imu_eskf_ * raw_accel; + + Eigen::Vector3d raw_gyro(msg->angular_velocity.x, msg->angular_velocity.y, + msg->angular_velocity.z); + + imu_measurement.gyro = R_imu_eskf_ * raw_gyro; + + eskf_->imu_update(imu_measurement, dt); + publish_odom(); +} + +void ESKFNode::dvl_callback( + const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg) { + DvlMeasurement dvl_measurement{}; + dvl_measurement.vel << msg->twist.twist.linear.x, msg->twist.twist.linear.y, + msg->twist.twist.linear.z; + + dvl_measurement.cov << msg->twist.covariance[0], msg->twist.covariance[1], + msg->twist.covariance[2], msg->twist.covariance[6], + msg->twist.covariance[7], msg->twist.covariance[8], + msg->twist.covariance[12], msg->twist.covariance[13], + msg->twist.covariance[14]; + + eskf_->dvl_update(dvl_measurement); + + std_msgs::msg::Float64 nis_msg; + nis_msg.data = eskf_->get_nis(); + nis_pub_->publish(nis_msg); + + publish_odom(); +} + +void ESKFNode::publish_odom() { + nav_msgs::msg::Odometry odom_msg; + StateQuat nom_state = eskf_->get_nominal_state(); + + odom_msg.pose.pose.position.x = nom_state.pos.x(); + odom_msg.pose.pose.position.y = nom_state.pos.y(); + odom_msg.pose.pose.position.z = nom_state.pos.z(); + + odom_msg.pose.pose.orientation.w = nom_state.quat.w(); + odom_msg.pose.pose.orientation.x = nom_state.quat.x(); + odom_msg.pose.pose.orientation.y = nom_state.quat.y(); + odom_msg.pose.pose.orientation.z = nom_state.quat.z(); + + odom_msg.twist.twist.linear.x = nom_state.vel.x(); + odom_msg.twist.twist.linear.y = nom_state.vel.y(); + odom_msg.twist.twist.linear.z = nom_state.vel.z(); + + odom_msg.twist.twist.angular.x = nom_state.accel_bias.x(); + odom_msg.twist.twist.angular.y = nom_state.accel_bias.y(); + odom_msg.twist.twist.angular.z = nom_state.accel_bias.z(); + + // TODO: Covariance out + + odom_msg.header.stamp = this->now(); + odom_pub_->publish(odom_msg); +} + +RCLCPP_COMPONENTS_REGISTER_NODE(ESKFNode) diff --git a/tests/ros_node_tests/eskf_node_test.sh b/tests/ros_node_tests/eskf_node_test.sh new file mode 100644 index 00000000..43167a08 --- /dev/null +++ b/tests/ros_node_tests/eskf_node_test.sh @@ -0,0 +1,42 @@ +#!/bin/bash +set -e +set -o pipefail + +echo "Testing that the ESKF node is able to start up and publish odom" + +# Load ROS 2 environment +echo "Setting up ROS 2 environment..." +. /opt/ros/humble/setup.sh +. "${WORKSPACE:-$HOME/ros2_ws}/install/setup.bash" + +# Function to terminate processes safely on error +cleanup() { + echo "Error detected. Cleaning up..." + kill -TERM -"$ESKF_PID" || true + exit 1 +} +trap cleanup ERR + +# Launch eskf node +setsid ros2 launch eskf eskf.launch.py & +ESKF_PID=$! +echo "Launched eskf with PID: $ESKF_PID" + +# Check for ROS errors before continuing +if journalctl -u ros2 | grep -i "error"; then + echo "Error detected in ROS logs. Exiting..." + exit 1 +fi + +# Publish imu to get odom (in the background) +ros2 topic pub /imu/data_raw sensor_msgs/msg/Imu -r 10 & + +# Check if eskf correctly publishes odom +echo "Waiting for odom data..." +timeout 10s ros2 topic echo /odom --once +echo "Got odom data" + +# Terminate processes +kill -TERM -"$ESKF_PID" + +echo "Test completed successfully."