Skip to content

Commit 0896d2d

Browse files
fmricofmrico
authored
Merge pull request #37 from midemig/rolling
A Kalman Filter for Easynav
2 parents 2b6eb67 + 9d681ae commit 0896d2d

12 files changed

Lines changed: 5261 additions & 2 deletions

File tree

‎.github/workflows/rolling.yaml‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -26,7 +26,7 @@ jobs:
2626
- name: build and test
2727
uses: ros-tooling/action-ros-ci@0.4.5
2828
with:
29-
package-name: easynav_common easynav_controller easynav_core easynav_interfaces easynav_localizer easynav_maps_manager easynav_planner easynav_sensors easynav_support_py easynav_system easynav_tools easynav_bonxai_maps_manager easynav_costmap_common easynav_costmap_localizer easynav_costmap_maps_manager easynav_costmap_planner easynav_gps_localizer easynav_mppi_controller easynav_navmap_localizer easynav_navmap_maps_manager easynav_navmap_planner easynav_serest_controller easynav_simple_common easynav_simple_controller easynav_simple_localizer easynav_simple_maps_manager easynav_simple_planner easynav_vff_controller easynav_mpc_controller
29+
package-name: easynav_common easynav_controller easynav_core easynav_interfaces easynav_localizer easynav_maps_manager easynav_planner easynav_sensors easynav_support_py easynav_system easynav_tools easynav_bonxai_maps_manager easynav_costmap_common easynav_costmap_localizer easynav_costmap_maps_manager easynav_costmap_planner easynav_gps_localizer easynav_fusion_localizer easynav_mppi_controller easynav_navmap_localizer easynav_navmap_maps_manager easynav_navmap_planner easynav_serest_controller easynav_simple_common easynav_simple_controller easynav_simple_localizer easynav_simple_maps_manager easynav_simple_planner easynav_vff_controller easynav_mpc_controller
3030
target-ros2-distro: rolling
3131
vcs-repo-file-url: ${GITHUB_WORKSPACE}/.github/thirdparty.repos
3232
skip-tests: false

‎.github/workflows/rolling_cron.yaml‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -23,7 +23,7 @@ jobs:
2323
- name: build and test
2424
uses: ros-tooling/action-ros-ci@0.4.5
2525
with:
26-
package-name: easynav_common easynav_controller easynav_core easynav_interfaces easynav_localizer easynav_maps_manager easynav_planner easynav_sensors easynav_support_py easynav_system easynav_tools easynav_bonxai_maps_manager easynav_costmap_common easynav_costmap_localizer easynav_costmap_maps_manager easynav_costmap_planner easynav_gps_localizer easynav_mppi_controller easynav_navmap_localizer easynav_navmap_maps_manager easynav_navmap_planner easynav_serest_controller easynav_simple_common easynav_simple_controller easynav_simple_localizer easynav_simple_maps_manager easynav_simple_planner easynav_vff_controller
26+
package-name: easynav_common easynav_controller easynav_core easynav_interfaces easynav_localizer easynav_maps_manager easynav_planner easynav_sensors easynav_support_py easynav_system easynav_tools easynav_bonxai_maps_manager easynav_costmap_common easynav_costmap_localizer easynav_costmap_maps_manager easynav_costmap_planner easynav_gps_localizer easynav_fusion_localizer easynav_mppi_controller easynav_navmap_localizer easynav_navmap_maps_manager easynav_navmap_planner easynav_serest_controller easynav_simple_common easynav_simple_controller easynav_simple_localizer easynav_simple_maps_manager easynav_simple_planner easynav_vff_controller
2727
target-ros2-distro: rolling
2828
ref: rolling
2929
vcs-repo-file-url: ${GITHUB_WORKSPACE}/.github/thirdparty.repos

‎localizers/easynav_fusion_localizer/CHANGELOG.rst‎

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,8 @@
1+
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
2+
Changelog for package easynav_fusion_localizer
3+
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
4+
5+
0.0.1 (2025-11-23)
6+
------------------
7+
* First version
8+
* Contributors: Miguel Ángel de Miguel

‎localizers/easynav_fusion_localizer/CMakeLists.txt‎

Lines changed: 96 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,96 @@
1+
cmake_minimum_required(VERSION 3.20)
2+
project(easynav_fusion_localizer)
3+
4+
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
5+
add_compile_options(-Wall -Wextra -Wpedantic)
6+
endif()
7+
8+
set(CMAKE_CXX_STANDARD 23)
9+
set(CMAKE_CXX_STANDARD_REQUIRED ON)
10+
set(CMAKE_CXX_EXTENSIONS OFF)
11+
12+
find_package(ament_cmake REQUIRED)
13+
find_package(rclcpp REQUIRED)
14+
find_package(rclcpp_lifecycle REQUIRED)
15+
find_package(easynav_core REQUIRED)
16+
find_package(easynav_common REQUIRED)
17+
find_package(easynav_localizer REQUIRED)
18+
find_package(pluginlib REQUIRED)
19+
find_package(geographic_msgs REQUIRED)
20+
find_package(robot_localization REQUIRED)
21+
find_package(std_srvs REQUIRED)
22+
find_package(angles REQUIRED)
23+
24+
# Geographiclib installs FindGeographicLib.cmake to this non-standard location
25+
set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "/usr/share/cmake/geographiclib/")
26+
find_package(GeographicLib REQUIRED)
27+
28+
include_directories(${GeographicLib_INCLUDE_DIRS})
29+
30+
31+
set(dependencies
32+
rclcpp
33+
rclcpp_lifecycle
34+
easynav_core
35+
easynav_common
36+
easynav_localizer
37+
pluginlib
38+
geographic_msgs
39+
robot_localization
40+
)
41+
42+
add_library(fusion_localizer SHARED
43+
src/easynav_fusion_localizer/FusionLocalizer.cpp
44+
src/easynav_fusion_localizer/ukf_wrapper.cpp
45+
)
46+
target_include_directories(fusion_localizer PUBLIC
47+
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
48+
$<INSTALL_INTERFACE:include>
49+
${robot_localization_INCLUDE_DIRS}
50+
${std_srvs_INCLUDE_DIRS}
51+
)
52+
target_link_libraries(fusion_localizer
53+
easynav_core::easynav_core
54+
easynav_common::easynav_common
55+
easynav_localizer::easynav_localizer
56+
pluginlib::pluginlib
57+
${geographic_msgs_TARGETS}
58+
GeographicLib
59+
${GeographicLib_LIBRARIES}
60+
${robot_localization_LIBRARIES}
61+
angles::angles
62+
yaets::yaets
63+
)
64+
65+
target_compile_definitions(fusion_localizer PUBLIC EASYNAV_DEBUG_WITH_YAETS)
66+
67+
install(DIRECTORY include/
68+
DESTINATION include/
69+
)
70+
71+
install(TARGETS
72+
fusion_localizer
73+
EXPORT export_${PROJECT_NAME}
74+
ARCHIVE DESTINATION lib
75+
LIBRARY DESTINATION lib
76+
RUNTIME DESTINATION lib/${PROJECT_NAME}
77+
)
78+
79+
if(BUILD_TESTING)
80+
find_package(ament_lint_auto REQUIRED)
81+
set(ament_cmake_copyright_FOUND TRUE)
82+
set(ament_cmake_cpplint_FOUND TRUE)
83+
ament_lint_auto_find_test_dependencies()
84+
endif()
85+
86+
ament_export_include_directories(include)
87+
ament_export_libraries(
88+
fusion_localizer
89+
)
90+
ament_export_targets(
91+
export_${PROJECT_NAME}
92+
)
93+
# Register the localization plugins
94+
pluginlib_export_plugin_description_file(easynav_core easynav_fusion_localizer_plugins.xml)
95+
ament_export_dependencies(${dependencies})
96+
ament_package()

‎localizers/easynav_fusion_localizer/README.md‎

Lines changed: 54 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,54 @@
1+
# easynav_gps_localizer
2+
3+
[![ROS 2: kilted](https://img.shields.io/badge/ROS%202-kilted-blue)](#) [![ROS 2: rolling](https://img.shields.io/badge/ROS%202-rolling-blue)](#)
4+
5+
## Description
6+
Odometry fusion localizer based on Robot Localization that fuses any n odometry sources.
7+
8+
## Authors and Maintainers
9+
- **Authors:** Intelligent Robotics Lab
10+
- **Maintainers:** Miguel Ángel de Miguel <midemig@gmail.com>
11+
12+
## Supported ROS 2 Distributions
13+
| Distribution | Status |
14+
|---|---|
15+
| rolling | ![rolling](https://img.shields.io/badge/rolling-supported-brightgreen) |
16+
17+
## Plugin (pluginlib)
18+
- **Plugin Name:** `easynav_fusion_localizer/FusionLocalizer`
19+
- **Type:** `easynav::FusionLocalizer`
20+
- **Base Class:** `easynav::LocalizerMethodBase`
21+
- **Library:** `fusion_localizer`
22+
- **Description:** Odometry fusion localizer based on Robot Localization that fuses any n odometry sources.
23+
24+
## Parameters
25+
26+
TODO
27+
28+
See [robot_localization documentation](https://docs.ros.org/en/melodic/api/robot_localization/html/configuring_robot_localization.html)
29+
30+
## Interfaces (Topics and Services)
31+
32+
### Subscriptions and Publications
33+
| Direction | Topic | Type | Purpose | QoS |
34+
|---|---|---|---|---|
35+
| Publisher | `odometry/filtered` | `nav_msgs/msg/Odometry` | Odometry fused from all sources. | SensorDataQoS |
36+
37+
38+
### Services
39+
This package does not create service servers or clients.
40+
41+
## NavState Keys
42+
| Key | Type | Access | Notes |
43+
|---|---|---|---|
44+
| `robot_pose` | `nav_msgs::msg::Odometry` | **Write** | GPS-based odometry estimate. |
45+
46+
47+
## TF Frames
48+
| Role | Transform | Notes |
49+
|---|---|---|
50+
| Publishes | `map -> odom` | Static or slowly varying transform aligning UTM to local odometry frame. |
51+
52+
53+
## License
54+
GPL-3.0-only

‎localizers/easynav_fusion_localizer/config/example_params.yaml‎

Lines changed: 133 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,133 @@
1+
controller_node:
2+
ros__parameters:
3+
use_sim_time: true
4+
controller_types: [dummy]
5+
dummy:
6+
rt_freq: 30.0
7+
plugin: easynav_controller/DummyController
8+
cycle_time_nort: 0.01
9+
cycle_time_rt: 0.0001
10+
11+
localizer_node:
12+
ros__parameters:
13+
use_sim_time: true
14+
localizer_types: [Ukf]
15+
dummy:
16+
rt_freq: 30.0
17+
plugin: easynav_localizer/DummyLocalizer
18+
cycle_time_nort: 0.01
19+
cycle_time_rt: 0.001
20+
Ukf:
21+
rt_freq: 50.0
22+
freq: 5.0
23+
reseed_freq: 0.1
24+
plugin: easynav_fusion_localizer/FusionLocalizer
25+
latitude_origin: 40.2834931
26+
longitude_origin: -3.8207253999999997
27+
altitude_origin: 746.184
28+
local_filter:
29+
two_d_mode: true
30+
publish_tf: true
31+
32+
# --- Reference Frames ---
33+
map_frame: map
34+
odom_frame: odom
35+
base_link_frame: base_link
36+
world_frame: map
37+
38+
# --- INPUT 1: GPS Odometry ---
39+
gps0: /gps/fix
40+
gps0_config:
41+
[true, true, false,
42+
false, false, false,
43+
false, false, false,
44+
false, false, false,
45+
false, false, false]
46+
gps0_differential: false
47+
48+
#--- INPUT 2: Wheel Odometry ---
49+
odom0: /robotnik_base_control/odom
50+
odom0_config:
51+
[false, false, false,
52+
false, false, false,
53+
true, true, false,
54+
false, false, true,
55+
false, false, false]
56+
odom0_differential: false
57+
58+
# --- INPUT 3: IMU ---
59+
imu0: /imu/data
60+
imu0_config:
61+
[false, false, false,
62+
true, true, false,
63+
false, false, false,
64+
true, true, true,
65+
false, false, false]
66+
imu0_differential: false
67+
imu0_remove_gravitational_acceleration: true
68+
69+
twist0: /gps/fix_velocity
70+
twist0_config:
71+
[false, false, false,
72+
false, false, false,
73+
true, false, false,
74+
false, false, false,
75+
false, false, false]
76+
twist0_differential: false
77+
78+
# --- Process Variances ---
79+
process_noise_covariance: [0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
80+
0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
81+
0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
82+
0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
83+
0.0, 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
84+
0.0, 0.0, 0.0, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
85+
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
86+
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
87+
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.04, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
88+
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0,
89+
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0,
90+
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.02, 0.0, 0.0, 0.0,
91+
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0,
92+
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0,
93+
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.015]
94+
95+
96+
maps_manager_node:
97+
ros__parameters:
98+
use_sim_time: true
99+
map_types: [dummy]
100+
dummy:
101+
freq: 10.0
102+
plugin: easynav_maps_manager/DummyMapsManager
103+
cycle_time_nort: 0.1
104+
cycle_time_rt: 0.0001
105+
106+
planner_node:
107+
ros__parameters:
108+
use_sim_time: true
109+
planner_types: [dummy]
110+
dummy:
111+
freq: 1.0
112+
plugin: easynav_planner/DummyPlanner
113+
cycle_time_nort: 0.2
114+
cycle_time_rt: 0.0001
115+
116+
sensors_node:
117+
ros__parameters:
118+
use_sim_time: true
119+
forget_time: 0.5
120+
sensors: [imu, gps]
121+
perception_default_frame: odom
122+
imu:
123+
topic: /imu/data
124+
type: sensor_msgs/msg/Imu
125+
gps:
126+
topic: /gps/fix
127+
type: sensor_msgs/msg/NavSatFix
128+
129+
system_node:
130+
ros__parameters:
131+
use_sim_time: true
132+
position_tolerance: 0.1
133+
angle_tolerance: 0.05

‎localizers/easynav_fusion_localizer/easynav_fusion_localizer_plugins.xml‎

Lines changed: 9 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,9 @@
1+
<class_libraries>
2+
<library path="fusion_localizer">
3+
<class name="easynav_fusion_localizer/FusionLocalizer" type="easynav::FusionLocalizer" base_class_type="easynav::LocalizerMethodBase">
4+
<description>
5+
A Kalman Filter implementation for the Localizer Method.
6+
</description>
7+
</class>
8+
</library>
9+
</class_libraries>

0 commit comments

Comments
 (0)