Skip to content

Fix navigation ESKF mathematics and ROS contracts #740

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Draft
ApatShe wants to merge 2 commits into
base: dev/eskf_fuse_depth
Choose a base branch
from
Draft

Fix navigation ESKF mathematics and ROS contracts #740

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
9c0b862
Fix navigation ESKF mathematics, validation and ROS state contracts
Sep 16, 2026
d8aa47e
Document ESKF validation and satisfy repository style checks
Sep 16, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
122 changes: 36 additions & 86 deletions auv_setup/launch/state_estimation.launch.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2,8 +2,13 @@

from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription, OpaqueFunction
from launch.actions import (
DeclareLaunchArgument,
IncludeLaunchDescription,
OpaqueFunction,
)
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import ComposableNodeContainer
from launch_ros.descriptions import ComposableNode

Expand All @@ -16,13 +21,6 @@
def launch_setup(context, *args, **kwargs):
drone, namespace = resolve_drone_and_namespace(context)

drone_params = os.path.join(
get_package_share_directory("auv_setup"),
"config",
"robots",
f"{drone}.yaml",
)

drone_description_launch = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(
Expand All @@ -43,83 +41,6 @@ def launch_setup(context, *args, **kwargs):
package="rclcpp_components",
executable="component_container_mt",
composable_node_descriptions=[
ComposableNode(
package="eskf",
plugin="ESKFNode",
name="eskf_node",
namespace=namespace,
parameters=[
drone_params,
{
"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.imu_frame_r": [
-1.0,
0.0,
0.0,
0.0,
1.0,
0.0,
0.0,
0.0,
-1.0,
],
"transform.imu_frame_t": [0.0, 0.0, 0.0],
"transform.dvl_frame_r": [
0.0,
-1.0,
0.0,
1.0,
0.0,
0.0,
0.0,
0.0,
1.0,
],
"transform.dvl_frame_t": [0.4, 0.0, 0.2],
"transform.depth_frame_t": [0.0, 0.0, 0.0],
"use_tf_transforms": True,
"publish_tf": True,
"publish_pose": True,
"publish_twist": True,
"publish_rate_ms": 5,
"add_gravity_to_imu": True,
"frame_prefix": namespace,
"initial_gyro_bias": [0.0, 0.0, 0.0],
"initial_accel_bias": [0.0, 0.0, -0.05],
},
],
extra_arguments=[{"use_intra_process_comms": True}],
),
ComposableNode(
package="mru_ros_interface",
plugin="MruRosInterface",
Expand Down Expand Up @@ -195,13 +116,42 @@ def launch_setup(context, *args, **kwargs):
arguments=["--ros-args", "--log-level", "error"],
)

return [drone_description_launch, container]
use_sim = LaunchConfiguration("use_sim").perform(context)
arguments = {
"drone": drone,
"namespace": namespace,
"use_sim": use_sim,
"environment": LaunchConfiguration("environment").perform(context),
"debug_output": LaunchConfiguration("debug_output").perform(context),
}
if use_sim.lower() != "true":
arguments.update(
{
"imu_topic": f"/{namespace}/imu/data_raw",
"dvl_topic": f"/{namespace}/nucleus/dvl",
"pressure_topic": f"/{namespace}/nucleus/pressure",
}
)
estimator = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(
get_package_share_directory("eskf"), "launch", "eskf.launch.py"
)
),
launch_arguments=arguments.items(),
)
return [drone_description_launch, estimator] + (
[] if use_sim.lower() == "true" else [container]
)


def generate_launch_description():
return LaunchDescription(
declare_drone_and_namespace_args()
+ [
DeclareLaunchArgument("use_sim", default_value="false"),
DeclareLaunchArgument("environment", default_value="trondheim_freshwater"),
DeclareLaunchArgument("debug_output", default_value="true"),
OpaqueFunction(function=launch_setup),
]
)
130 changes: 44 additions & 86 deletions navigation/eskf/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,88 +1,46 @@
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()

if(NOT CMAKE_BUILD_TYPE)
set(CMAKE_BUILD_TYPE Release)
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)
cmake_minimum_required(VERSION 3.16)
project(eskf LANGUAGES CXX)
set(CMAKE_CXX_STANDARD 20)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
option(ESKF_BUILD_ROS "Build the ROS 2 adapter" ON)
find_package(Eigen3 REQUIRED)
find_package(tf2 REQUIRED)
find_package(vortex_msgs REQUIRED)
find_package(vortex_utils REQUIRED)
find_package(vortex_utils_ros REQUIRED)
find_package(spdlog REQUIRED)
find_package(fmt REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(tf2_eigen REQUIRED)

if(NOT DEFINED EIGEN3_INCLUDE_DIR)
set(EIGEN3_INCLUDE_DIR ${EIGEN3_INCLUDE_DIRS})
add_library(eskf_core src/eskf.cpp)
set_target_properties(eskf_core PROPERTIES POSITION_INDEPENDENT_CODE ON)
target_include_directories(eskf_core PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include> $<INSTALL_INTERFACE:include>)
target_link_libraries(eskf_core PUBLIC Eigen3::Eigen)
include(CTest)
if(BUILD_TESTING)
add_executable(eskf_core_tests test/core_tests.cpp)
target_link_libraries(eskf_core_tests PRIVATE eskf_core)
add_test(NAME eskf_core_tests COMMAND eskf_core_tests)
endif()
if(ESKF_BUILD_ROS)
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclcpp_components REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(std_msgs REQUIRED)
find_package(std_srvs REQUIRED)
find_package(nav_msgs REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(tf2_eigen REQUIRED)
find_package(spdlog REQUIRED)
add_library(eskf_component SHARED src/eskf_ros.cpp)
target_link_libraries(eskf_component PUBLIC eskf_core spdlog::spdlog)
ament_target_dependencies(eskf_component PUBLIC
rclcpp rclcpp_components sensor_msgs std_msgs std_srvs
nav_msgs geometry_msgs tf2_ros tf2_eigen)
rclcpp_components_register_node(eskf_component PLUGIN "ESKFNode" EXECUTABLE eskf_node)
install(TARGETS eskf_core eskf_component EXPORT export_eskf ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin)
install(DIRECTORY include/ DESTINATION include)
install(DIRECTORY launch config DESTINATION share/${PROJECT_NAME})
ament_export_targets(export_eskf HAS_LIBRARY_TARGET)
ament_export_dependencies(Eigen3 rclcpp sensor_msgs std_msgs std_srvs nav_msgs geometry_msgs tf2_ros tf2_eigen spdlog)
if(BUILD_TESTING)
find_package(Python3 REQUIRED COMPONENTS Interpreter)
# Run after installation with its setup.bash sourced (see README).
install(PROGRAMS test/ros_contract_test.py DESTINATION lib/${PROJECT_NAME})
endif()
ament_package()
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
vortex_utils_ros
spdlog
fmt
tf2_ros
tf2_eigen
)

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()
Loading
Loading