mirror of
https://github.com/introlab/rtabmap_ros.git
synced 2026-10-04 08:47:45 +08:00
* demo(turtlebot3): add FusionCore + icp_odometry feedback loop demo Adds a TurtleBot3 Gazebo Harmonic demo (ref issue #1418 Option A) where FusionCore (wheel + IMU UKF) and icp_odometry tighten each other in a feedback loop: FusionCore's stable odom frame serves as the scan-match initial guess via guess_frame_id, and the ICP result feeds back into FusionCore as a second velocity source. rtabmap SLAM consumes the ICP output for mapping/loop-closure. Nav2 uses /fusion/odom. Files added: launch/turtlebot3/fusioncore/turtlebot3_fusioncore_icp.launch.py launch/turtlebot3/fusioncore/turtlebot3_sim_fusioncore_icp_demo.launch.py launch/turtlebot3/fusioncore/README.md params/fusioncore_tb3.yaml params/fusioncore_tb3_bridge.yaml (TF-conflict-free bridge) params/turtlebot3_fusioncore_icp_nav2_params.yaml * demo(turtlebot3): add FusionCore demo entry to rtabmap_demos README * fix(fusioncore-demo): remove Odom/ResetCountdown from rtabmap, strip GPS params * docs(demos): add FusionCore icp_odometry demo GIF to README * fix(fusioncore-demo): add imu.frame_id override and docking_server params * fix(fusioncore-demo): remove docking_server from nav2 params * Fixing turtlebot3 demos on Jazzy * Updated humble * Working turtlebot3 demos on humble and jazzy * Fixed Twist->TwistStamped. Fixed rviz2 crashing because missing docking server * demo(turtlebot3): add FusionCore + icp_odometry feedback loop demo Adds a TurtleBot3 Gazebo Harmonic demo (ref issue #1418 Option A) where FusionCore (wheel + IMU UKF) and icp_odometry tighten each other in a feedback loop: FusionCore's stable odom frame serves as the scan-match initial guess via guess_frame_id, and the ICP result feeds back into FusionCore as a second velocity source. rtabmap SLAM consumes the ICP output for mapping/loop-closure. Nav2 uses /fusion/odom. Files added: launch/turtlebot3/fusioncore/turtlebot3_fusioncore_icp.launch.py launch/turtlebot3/fusioncore/turtlebot3_sim_fusioncore_icp_demo.launch.py launch/turtlebot3/fusioncore/README.md params/fusioncore_tb3.yaml params/fusioncore_tb3_bridge.yaml (TF-conflict-free bridge) params/turtlebot3_fusioncore_icp_nav2_params.yaml * demo(turtlebot3): add FusionCore demo entry to rtabmap_demos README * fix(fusioncore-demo): remove Odom/ResetCountdown from rtabmap, strip GPS params * docs(demos): add FusionCore icp_odometry demo GIF to README * fix(fusioncore-demo): add imu.frame_id override and docking_server params * fix(fusioncore-demo): remove docking_server from nav2 params * Fixing turtlebot3 demos on Jazzy * Updated humble * Fixed Twist->TwistStamped. Fixed rviz2 crashing because missing docking server * fix(fusioncore-demo): tune IMU noise for Gazebo sim, add real-hardware comments * docs(fusioncore-demo): add sim vs real hardware note to README --------- Co-authored-by: manankharwar <[email protected]> Co-authored-by: matlabbe <[email protected]>
This commit is contained in:
Manan Kharwar
co-authored by
manankharwar
matlabbe
manankharwar
matlabbe
parent
d9cd332c53
commit
aec4f91f61
@@ -8,6 +8,7 @@
|
||||
+ [Turtlebot3 Nav2 and RGB-D SLAM](#turtlebot3-nav2-and-rgb-d-slam)
|
||||
+ [Turtlebot3 Nav2, 2D LiDAR and RGB-D SLAM](#turtlebot3-nav2-2d-lidar-and-rgb-d-slam)
|
||||
+ [Turtlebot3 Nav2, Fake 2D LiDAR and RGB-D SLAM](#turtlebot3-nav2-fake-2d-lidar-and-rgb-d-slam)
|
||||
+ [Turtlebot3 Nav2, 2D LiDAR SLAM with FusionCore (IMU + wheel UKF)](#turtlebot3-nav2-2d-lidar-slam-with-fusioncore-imu--wheel-ukf)
|
||||
+ [Champ Quadruped Nav2, Elevation Map and VSLAM](#champ-quadruped-nav2-elevation-map-and-vslam)
|
||||
+ [Clearpath Husky Nav2, 2D LiDAR and RGB-D SLAM](#clearpath-husky-nav2-2d-lidar-and-rgb-d-slam)
|
||||
+ [Clearpath Husky Nav2, 3D LiDAR and RGB-D SLAM](#clearpath-husky-nav2-3d-lidar-and-rgb-d-slam)
|
||||
@@ -59,6 +60,12 @@
|
||||
* Yellow: The map.
|
||||
|
||||
|
||||
### Turtlebot3 Nav2, 2D LiDAR SLAM with FusionCore (IMU + wheel UKF)
|
||||
[turtlebot3_sim_fusioncore_icp_demo.launch.py](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_demos/launch/turtlebot3/fusioncore/turtlebot3_sim_fusioncore_icp_demo.launch.py) (Jazzy + Gazebo Harmonic)
|
||||
|
||||
FusionCore (wheel + IMU UKF) and `icp_odometry` run in a feedback loop: FusionCore's stable `odom` frame seeds scan matching via `guess_frame_id`, and the ICP result feeds back into FusionCore as a second velocity source. See [README](launch/turtlebot3/fusioncore/README.md) for architecture details.
|
||||
|
||||
|
||||
### Champ Quadruped Nav2, Elevation Map and VSLAM
|
||||
[champ_sim_vslam.launch.py](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_demos/launch/champ/champ_sim_vslam.launch.py)
|
||||
|
||||
|
||||
@@ -0,0 +1,91 @@
|
||||
# FusionCore + icp_odometry: TurtleBot3 Gazebo Demo
|
||||
|
||||
This demo shows a feedback loop between [FusionCore](https://github.com/manankharwar/fusioncore) and rtabmap's `icp_odometry` where each node tightens the other.
|
||||
|
||||
## Architecture
|
||||
|
||||
```
|
||||
/imu ──────────────────────┐
|
||||
/odom (wheel) ──────────────┤──→ FusionCore (UKF)
|
||||
/rtabmap/icp_odometry ──────┘ │
|
||||
↑ │ publishes: odom → base_footprint TF
|
||||
│ │ /fusion/odom
|
||||
│ guess_frame_id: odom│
|
||||
└────── icp_odometry ←───────┘
|
||||
│ (publish_tf: false)
|
||||
│
|
||||
└──→ /rtabmap/icp_odometry ──→ rtabmap SLAM ──→ map → odom TF
|
||||
```
|
||||
|
||||
**What each node contributes:**
|
||||
|
||||
| Node | Input | Provides |
|
||||
|---|---|---|
|
||||
| FusionCore | wheels + IMU | stable `odom` frame, continuous state at 100 Hz |
|
||||
| icp_odometry | `/scan` + FusionCore's `odom` as initial guess | scan-level pose corrections |
|
||||
| FusionCore encoder2 | icp_odometry output | tighter velocity corrections from ICP |
|
||||
| rtabmap SLAM | icp_odometry output | global map, loop closures |
|
||||
|
||||
FusionCore gives `icp_odometry` a stable initial guess via `guess_frame_id: odom`.
|
||||
Better initial guesses mean scan matching succeeds more often and with lower error.
|
||||
The ICP result feeds back into FusionCore as a second velocity source (`encoder2`),
|
||||
tightening the state estimate further. `Odom/ResetCountdown: 1` lets the system
|
||||
auto-recover if ICP loses tracking.
|
||||
|
||||
## Simulation vs real hardware
|
||||
|
||||
In Gazebo, the DiffDrive plugin produces near-perfect wheel velocities with no slip or
|
||||
encoder noise, while the simulated MPU9250 injects Gaussian noise. FusionCore fusing
|
||||
both means the noisy IMU slightly degrades what is already a perfect odometry source,
|
||||
so the `map → odom` correction on each scan update will be slightly larger than in the
|
||||
standard wheel-odometry-only demo. On real hardware this completely inverts: wheel
|
||||
encoders accumulate slip, terrain variation, and mechanical error that dwarfs IMU noise,
|
||||
and fusion pays off measurably. The sim-tuned IMU noise values in `fusioncore_tb3.yaml`
|
||||
(`gyro_noise: 0.002`, `accel_noise: 0.02`) reduce unnecessary filter uncertainty in
|
||||
simulation; real MPU9250 users should use the hardware spec values noted in that file.
|
||||
|
||||
## Quick start
|
||||
|
||||
```bash
|
||||
export TURTLEBOT3_MODEL=waffle
|
||||
|
||||
ros2 launch rtabmap_demos turtlebot3_sim_fusioncore_icp_demo.launch.py
|
||||
```
|
||||
|
||||
Optional arguments:
|
||||
|
||||
```bash
|
||||
# Localization mode (requires saved map from a previous mapping run)
|
||||
ros2 launch rtabmap_demos turtlebot3_sim_fusioncore_icp_demo.launch.py localization:=true
|
||||
|
||||
# Different Gazebo world
|
||||
ros2 launch rtabmap_demos turtlebot3_sim_fusioncore_icp_demo.launch.py world:=house
|
||||
```
|
||||
|
||||
## Prerequisites
|
||||
|
||||
```bash
|
||||
sudo apt install ros-jazzy-fusioncore-ros ros-jazzy-turtlebot3-gazebo ros-jazzy-rtabmap-ros ros-jazzy-nav2-bringup
|
||||
export TURTLEBOT3_MODEL=waffle
|
||||
```
|
||||
|
||||
## Files
|
||||
|
||||
| File | Purpose |
|
||||
|---|---|
|
||||
| `turtlebot3_sim_fusioncore_icp_demo.launch.py` | Complete demo: Gazebo + FusionCore + rtabmap + Nav2 |
|
||||
| `turtlebot3_fusioncore_icp.launch.py` | Core only: FusionCore + icp_odometry + rtabmap (no Gazebo) |
|
||||
| `../../params/fusioncore_tb3.yaml` | FusionCore config for TB3 Waffle |
|
||||
| `../../params/turtlebot3_fusioncore_icp_nav2_params.yaml` | Nav2 config using `/fusion/odom` |
|
||||
|
||||
## Topic and TF summary
|
||||
|
||||
| Topic / TF | Publisher | Subscribers |
|
||||
|---|---|---|
|
||||
| `/imu` | Gazebo | FusionCore |
|
||||
| `/odom` | Gazebo (wheel) | FusionCore |
|
||||
| `/scan` | Gazebo (lidar) | icp_odometry, rtabmap |
|
||||
| `/rtabmap/icp_odometry` | icp_odometry | FusionCore (encoder2), rtabmap |
|
||||
| `/fusion/odom` | FusionCore | Nav2 |
|
||||
| TF `odom → base_footprint` | FusionCore | icp_odometry (guess), Nav2 |
|
||||
| TF `map → odom` | rtabmap | Nav2 |
|
||||
@@ -0,0 +1,165 @@
|
||||
"""
|
||||
FusionCore + icp_odometry feedback loop for TurtleBot3.
|
||||
|
||||
Architecture (Option A from rtabmap_ros issue #1418):
|
||||
|
||||
FusionCore (wheels + IMU)
|
||||
|-- publishes: odom -> base_footprint TF, /fusion/odom
|
||||
|-- provides initial pose guess to icp_odometry via guess_frame_id
|
||||
|
||||
icp_odometry (/scan)
|
||||
|-- guess_frame_id: odom (uses FusionCore's stable odom as scan match seed)
|
||||
|-- publish_tf: false (FusionCore owns the odom TF)
|
||||
|-- publishes: /rtabmap/icp_odometry
|
||||
|
||||
FusionCore encoder2
|
||||
|-- topic: /rtabmap/icp_odometry
|
||||
|-- ICP corrections fed back as a second velocity source
|
||||
|
||||
rtabmap SLAM
|
||||
|-- subscribes to /rtabmap/icp_odometry for mapping
|
||||
|-- Odom/ResetCountdown: 1 for auto-recovery if ICP loses tracking
|
||||
|-- publishes: map -> odom TF
|
||||
"""
|
||||
|
||||
import os
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import (DeclareLaunchArgument, EmitEvent,
|
||||
OpaqueFunction, RegisterEventHandler, TimerAction)
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch_ros.actions import LifecycleNode, Node
|
||||
from launch_ros.event_handlers import OnStateTransition
|
||||
from launch_ros.events.lifecycle import ChangeState
|
||||
from lifecycle_msgs.msg import Transition
|
||||
|
||||
|
||||
def launch_setup(context, *args, **kwargs):
|
||||
use_sim_time = LaunchConfiguration('use_sim_time')
|
||||
localization = LaunchConfiguration('localization').perform(context)
|
||||
localization = localization in ('True', 'true')
|
||||
|
||||
pkg_demos = get_package_share_directory('rtabmap_demos')
|
||||
fusioncore_config = os.path.join(pkg_demos, 'params', 'fusioncore_tb3.yaml')
|
||||
|
||||
# ── FusionCore lifecycle node ─────────────────────────────────────────────
|
||||
fc = LifecycleNode(
|
||||
package='fusioncore_ros',
|
||||
executable='fusioncore_node',
|
||||
name='fusioncore',
|
||||
namespace='',
|
||||
output='screen',
|
||||
parameters=[fusioncore_config, {'use_sim_time': use_sim_time}],
|
||||
remappings=[
|
||||
('/imu/data', '/imu'), # TB3 Gazebo IMU topic
|
||||
('/odom/wheels', '/odom'), # TB3 Gazebo wheel odometry topic
|
||||
],
|
||||
)
|
||||
|
||||
# Wait 2 s for node to spin up, then configure
|
||||
configure = TimerAction(
|
||||
period=2.0,
|
||||
actions=[EmitEvent(event=ChangeState(
|
||||
lifecycle_node_matcher=lambda a: a is fc,
|
||||
transition_id=Transition.TRANSITION_CONFIGURE,
|
||||
))],
|
||||
)
|
||||
|
||||
# As soon as configuring -> inactive, activate
|
||||
activate = RegisterEventHandler(OnStateTransition(
|
||||
target_lifecycle_node=fc,
|
||||
start_state='configuring',
|
||||
goal_state='inactive',
|
||||
entities=[EmitEvent(event=ChangeState(
|
||||
lifecycle_node_matcher=lambda a: a is fc,
|
||||
transition_id=Transition.TRANSITION_ACTIVATE,
|
||||
))],
|
||||
))
|
||||
|
||||
# ── icp_odometry ──────────────────────────────────────────────────────────
|
||||
icp_parameters = {
|
||||
'frame_id': 'base_footprint',
|
||||
'odom_frame_id': 'odom',
|
||||
'guess_frame_id': 'odom',
|
||||
'publish_tf': False,
|
||||
'publish_null_when_lost': False,
|
||||
'use_sim_time': use_sim_time,
|
||||
'Reg/Strategy': '1',
|
||||
'Reg/Force3DoF': 'true',
|
||||
'Odom/ResetCountdown': '1',
|
||||
'RGBD/NeighborLinkRefining': 'True',
|
||||
'Grid/RangeMin': '0.2',
|
||||
}
|
||||
|
||||
icp_odometry_node = Node(
|
||||
package='rtabmap_odom',
|
||||
executable='icp_odometry',
|
||||
output='screen',
|
||||
parameters=[icp_parameters],
|
||||
remappings=[
|
||||
('scan', '/scan'),
|
||||
('odom', '/rtabmap/icp_odometry'),
|
||||
],
|
||||
)
|
||||
|
||||
# ── rtabmap SLAM ──────────────────────────────────────────────────────────
|
||||
slam_parameters = {
|
||||
'frame_id': 'base_footprint',
|
||||
'odom_frame_id': 'odom',
|
||||
'use_sim_time': use_sim_time,
|
||||
'subscribe_depth': False,
|
||||
'subscribe_rgb': False,
|
||||
'subscribe_scan': True,
|
||||
'approx_sync': True,
|
||||
'use_action_for_goal': True,
|
||||
'Reg/Strategy': '1',
|
||||
'Reg/Force3DoF': 'true',
|
||||
'RGBD/NeighborLinkRefining': 'True',
|
||||
'Grid/RangeMin': '0.2',
|
||||
'Optimizer/GravitySigma': '0',
|
||||
}
|
||||
|
||||
if localization:
|
||||
slam_parameters['Mem/IncrementalMemory'] = 'False'
|
||||
slam_parameters['Mem/InitWMWithAllNodes'] = 'True'
|
||||
|
||||
rtabmap_args = [] if localization else ['-d']
|
||||
|
||||
rtabmap_node = Node(
|
||||
package='rtabmap_slam',
|
||||
executable='rtabmap',
|
||||
output='screen',
|
||||
parameters=[slam_parameters],
|
||||
remappings=[
|
||||
('scan', '/scan'),
|
||||
('odom', '/rtabmap/icp_odometry'),
|
||||
],
|
||||
arguments=rtabmap_args,
|
||||
)
|
||||
|
||||
rtabmap_viz_node = Node(
|
||||
package='rtabmap_viz',
|
||||
executable='rtabmap_viz',
|
||||
output='screen',
|
||||
parameters=[slam_parameters, {'odometry_node_name': 'icp_odometry'}],
|
||||
remappings=[
|
||||
('scan', '/scan'),
|
||||
('odom', '/rtabmap/icp_odometry'),
|
||||
],
|
||||
)
|
||||
|
||||
return [fc, configure, activate, icp_odometry_node, rtabmap_node, rtabmap_viz_node]
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
return LaunchDescription([
|
||||
DeclareLaunchArgument(
|
||||
'use_sim_time', default_value='true',
|
||||
description='Use simulation (Gazebo) clock if true'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'localization', default_value='false',
|
||||
description='Launch in localization mode (requires existing map)'),
|
||||
|
||||
OpaqueFunction(function=launch_setup),
|
||||
])
|
||||
+181
@@ -0,0 +1,181 @@
|
||||
"""
|
||||
Complete TurtleBot3 demo: Gazebo (new) + FusionCore + icp_odometry + Nav2.
|
||||
|
||||
Launches in order:
|
||||
1. Gazebo Harmonic (via ros_gz_sim) with turtlebot3_world
|
||||
2. Robot state publisher + spawn TurtleBot3
|
||||
3. Custom ros_gz_bridge WITHOUT the odom TF (FusionCore owns odom->base_footprint)
|
||||
4. FusionCore lifecycle node (configure -> activate automatically)
|
||||
5. icp_odometry using FusionCore's odom frame as scan-match initial guess
|
||||
6. rtabmap SLAM subscribing to icp_odometry output
|
||||
7. Nav2 using /fusion/odom
|
||||
|
||||
Usage:
|
||||
export TURTLEBOT3_MODEL=waffle
|
||||
ros2 launch rtabmap_demos turtlebot3_sim_fusioncore_icp_demo.launch.py
|
||||
|
||||
# Localization mode (requires existing map):
|
||||
ros2 launch rtabmap_demos turtlebot3_sim_fusioncore_icp_demo.launch.py localization:=true
|
||||
|
||||
# Different world:
|
||||
ros2 launch rtabmap_demos turtlebot3_sim_fusioncore_icp_demo.launch.py world:=house
|
||||
|
||||
Note on TF ownership:
|
||||
The standard turtlebot3_gazebo bridge forwards the DiffDrive TF to ROS, which
|
||||
conflicts with FusionCore's odom->base_footprint. This demo uses a custom bridge
|
||||
config (fusioncore_tb3_bridge.yaml) that suppresses the Gazebo TF entry.
|
||||
FusionCore is the sole publisher of odom->base_footprint.
|
||||
"""
|
||||
|
||||
import os
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import (AppendEnvironmentVariable, DeclareLaunchArgument,
|
||||
IncludeLaunchDescription, OpaqueFunction)
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
|
||||
from launch_ros.actions import Node
|
||||
from launch_ros.substitutions import FindPackageShare
|
||||
|
||||
|
||||
def launch_setup(context, *args, **kwargs):
|
||||
if 'TURTLEBOT3_MODEL' not in os.environ:
|
||||
os.environ['TURTLEBOT3_MODEL'] = 'waffle'
|
||||
|
||||
tb3_model = os.environ['TURTLEBOT3_MODEL']
|
||||
pkg_tb3_gz = get_package_share_directory('turtlebot3_gazebo')
|
||||
pkg_ros_gz = get_package_share_directory('ros_gz_sim')
|
||||
pkg_nav2 = get_package_share_directory('nav2_bringup')
|
||||
pkg_demos = get_package_share_directory('rtabmap_demos')
|
||||
|
||||
world_name = LaunchConfiguration('world').perform(context)
|
||||
world_file = os.path.join(pkg_tb3_gz, 'worlds', f'turtlebot3_{world_name}.world')
|
||||
|
||||
# ── Gazebo server + client ────────────────────────────────────────────────
|
||||
gz_server = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
os.path.join(pkg_ros_gz, 'launch', 'gz_sim.launch.py')),
|
||||
launch_arguments={
|
||||
'gz_args': f'-r -s -v2 {world_file}',
|
||||
'on_exit_shutdown': 'true',
|
||||
}.items(),
|
||||
)
|
||||
gz_client = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
os.path.join(pkg_ros_gz, 'launch', 'gz_sim.launch.py')),
|
||||
launch_arguments={'gz_args': '-g -v2', 'on_exit_shutdown': 'true'}.items(),
|
||||
)
|
||||
|
||||
# ── Robot state publisher ─────────────────────────────────────────────────
|
||||
robot_state_publisher = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
os.path.join(pkg_tb3_gz, 'launch', 'robot_state_publisher.launch.py')),
|
||||
launch_arguments={'use_sim_time': 'true'}.items(),
|
||||
)
|
||||
|
||||
# ── Spawn TurtleBot3 (entity only, no bridge) ─────────────────────────────
|
||||
urdf_path = os.path.join(pkg_tb3_gz, 'models',
|
||||
f'turtlebot3_{tb3_model}', 'model.sdf')
|
||||
spawn_robot = Node(
|
||||
package='ros_gz_sim',
|
||||
executable='create',
|
||||
arguments=[
|
||||
'-name', tb3_model,
|
||||
'-file', urdf_path,
|
||||
'-x', LaunchConfiguration('x_pose'),
|
||||
'-y', LaunchConfiguration('y_pose'),
|
||||
'-z', '0.01',
|
||||
],
|
||||
output='screen',
|
||||
)
|
||||
|
||||
# ── Custom bridge: all topics EXCEPT odom TF ──────────────────────────────
|
||||
# FusionCore publishes odom->base_footprint; suppress the Gazebo DiffDrive TF.
|
||||
bridge_config = os.path.join(pkg_demos, 'params', 'fusioncore_tb3_bridge.yaml')
|
||||
bridge = Node(
|
||||
package='ros_gz_bridge',
|
||||
executable='parameter_bridge',
|
||||
arguments=['--ros-args', '-p', f'config_file:={bridge_config}'],
|
||||
output='screen',
|
||||
)
|
||||
|
||||
# Camera image bridge (waffle only)
|
||||
image_bridge = Node(
|
||||
package='ros_gz_image',
|
||||
executable='image_bridge',
|
||||
arguments=['/camera/image_raw'],
|
||||
output='screen',
|
||||
)
|
||||
|
||||
# ── FusionCore + icp_odometry + rtabmap ───────────────────────────────────
|
||||
fusioncore_icp = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
os.path.join(pkg_demos, 'launch', 'turtlebot3', 'fusioncore',
|
||||
'turtlebot3_fusioncore_icp.launch.py')),
|
||||
launch_arguments=[
|
||||
('localization', LaunchConfiguration('localization')),
|
||||
('use_sim_time', 'true'),
|
||||
],
|
||||
)
|
||||
|
||||
# ── Nav2 ──────────────────────────────────────────────────────────────────
|
||||
nav2 = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
os.path.join(pkg_nav2, 'launch', 'navigation_launch.py')),
|
||||
launch_arguments=[
|
||||
('use_sim_time', 'true'),
|
||||
('params_file', PathJoinSubstitution(
|
||||
[FindPackageShare('rtabmap_demos'), 'params',
|
||||
'turtlebot3_fusioncore_icp_nav2_params.yaml'])),
|
||||
],
|
||||
)
|
||||
|
||||
rviz = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
os.path.join(pkg_nav2, 'launch', 'rviz_launch.py')),
|
||||
)
|
||||
|
||||
set_gz_resource_path = AppendEnvironmentVariable(
|
||||
'GZ_SIM_RESOURCE_PATH',
|
||||
os.path.join(pkg_tb3_gz, 'models'),
|
||||
)
|
||||
|
||||
nodes = [
|
||||
set_gz_resource_path,
|
||||
gz_server,
|
||||
gz_client,
|
||||
robot_state_publisher,
|
||||
spawn_robot,
|
||||
bridge,
|
||||
fusioncore_icp,
|
||||
nav2,
|
||||
rviz,
|
||||
]
|
||||
if tb3_model == 'waffle':
|
||||
nodes.insert(6, image_bridge)
|
||||
|
||||
return nodes
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
return LaunchDescription([
|
||||
DeclareLaunchArgument(
|
||||
'localization', default_value='false',
|
||||
description='Launch in localization mode.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'world', default_value='world',
|
||||
choices=['world', 'house', 'dqn_stage1', 'dqn_stage2',
|
||||
'dqn_stage3', 'dqn_stage4'],
|
||||
description='Turtlebot3 Gazebo world.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'x_pose', default_value='-2.0',
|
||||
description='Initial X position in Gazebo.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'y_pose', default_value='-0.5',
|
||||
description='Initial Y position in Gazebo.'),
|
||||
|
||||
OpaqueFunction(function=launch_setup),
|
||||
])
|
||||
@@ -0,0 +1,63 @@
|
||||
# FusionCore config for TurtleBot3 Waffle (Gazebo Harmonic)
|
||||
#
|
||||
# Platform: TurtleBot3 Waffle (simulated)
|
||||
# IMU: MPU9250 (9-axis, magnetometer unreliable in sim)
|
||||
# GPS: None (indoor / simulation)
|
||||
# LiDAR: HLS-LFCD2 via /scan (used by icp_odometry, not fused here directly)
|
||||
# encoder2: rtabmap icp_odometry output on /rtabmap/icp_odometry
|
||||
#
|
||||
# Architecture: FusionCore (wheels + IMU) provides the odom frame.
|
||||
# icp_odometry uses that odom frame as its initial scan-matching guess
|
||||
# (guess_frame_id: odom). The ICP output feeds back into FusionCore
|
||||
# as a second velocity source (encoder2). Each tightens the other.
|
||||
|
||||
fusioncore:
|
||||
ros__parameters:
|
||||
base_frame: base_footprint
|
||||
odom_frame: odom
|
||||
publish_rate: 100.0
|
||||
publish.force_2d: true
|
||||
|
||||
# Gazebo Harmonic prefixes sensor frames with the model name (waffle/imu_link/tb3_imu).
|
||||
# Override to the TF frame that robot_state_publisher actually publishes.
|
||||
imu.frame_id: "imu_link"
|
||||
|
||||
# MPU9250: magnetometer disabled (unreliable in sim / near motors)
|
||||
imu.has_magnetometer: false
|
||||
# Simulation-tuned noise values for Gazebo Harmonic MPU9250 plugin.
|
||||
# Real MPU9250 hardware: gyro ~0.005 rad/s, accel ~0.1 m/s2.
|
||||
# Gazebo injects lower noise than the real sensor, so tighter values
|
||||
# reduce unnecessary filter uncertainty in sim without affecting real-hardware users
|
||||
# (who should revert to the hardware spec values above).
|
||||
imu.gyro_noise: 0.002 # rad/s (Gazebo sim-tuned; real MPU9250: 0.005)
|
||||
imu.accel_noise: 0.02 # m/s2 (Gazebo sim-tuned; real MPU9250: 0.1)
|
||||
imu.remove_gravitational_acceleration: false
|
||||
|
||||
# Wheel odometry noise (TB3 differential drive, simulated)
|
||||
encoder.vel_noise: 0.05 # m/s
|
||||
encoder.yaw_noise: 0.02 # rad/s
|
||||
|
||||
# ICP odometry as second velocity source
|
||||
encoder2.topic: "/rtabmap/icp_odometry"
|
||||
|
||||
outlier_rejection: true
|
||||
outlier_threshold_imu: 15.09
|
||||
outlier_threshold_enc: 11.34
|
||||
|
||||
adaptive.imu: true
|
||||
adaptive.encoder: true
|
||||
adaptive.window: 50
|
||||
adaptive.alpha: 0.01
|
||||
|
||||
zupt.enabled: true
|
||||
zupt.velocity_threshold: 0.08 # m/s: slightly loose for ICP jitter
|
||||
zupt.angular_threshold: 0.05 # rad/s
|
||||
zupt.noise_sigma: 0.01
|
||||
|
||||
ukf.q_position: 0.01
|
||||
ukf.q_orientation: 1.0e-9
|
||||
ukf.q_velocity: 0.1
|
||||
ukf.q_angular_vel: 0.1
|
||||
ukf.q_acceleration: 1.0
|
||||
ukf.q_gyro_bias: 1.0e-5
|
||||
ukf.q_accel_bias: 1.0e-5
|
||||
@@ -0,0 +1,47 @@
|
||||
# ros_gz_bridge config for the FusionCore + icp_odometry demo.
|
||||
#
|
||||
# Identical to turtlebot3_waffle_bridge.yaml EXCEPT the 'tf' entry is removed.
|
||||
# FusionCore publishes odom -> base_footprint TF directly, so the DiffDrive
|
||||
# plugin's TF must not be forwarded to avoid a competing transform.
|
||||
|
||||
- ros_topic_name: "clock"
|
||||
gz_topic_name: "clock"
|
||||
ros_type_name: "rosgraph_msgs/msg/Clock"
|
||||
gz_type_name: "gz.msgs.Clock"
|
||||
direction: GZ_TO_ROS
|
||||
|
||||
- ros_topic_name: "joint_states"
|
||||
gz_topic_name: "joint_states"
|
||||
ros_type_name: "sensor_msgs/msg/JointState"
|
||||
gz_type_name: "gz.msgs.Model"
|
||||
direction: GZ_TO_ROS
|
||||
|
||||
- ros_topic_name: "odom"
|
||||
gz_topic_name: "odom"
|
||||
ros_type_name: "nav_msgs/msg/Odometry"
|
||||
gz_type_name: "gz.msgs.Odometry"
|
||||
direction: GZ_TO_ROS
|
||||
|
||||
- ros_topic_name: "cmd_vel"
|
||||
gz_topic_name: "cmd_vel"
|
||||
ros_type_name: "geometry_msgs/msg/TwistStamped"
|
||||
gz_type_name: "gz.msgs.Twist"
|
||||
direction: ROS_TO_GZ
|
||||
|
||||
- ros_topic_name: "imu"
|
||||
gz_topic_name: "imu"
|
||||
ros_type_name: "sensor_msgs/msg/Imu"
|
||||
gz_type_name: "gz.msgs.IMU"
|
||||
direction: GZ_TO_ROS
|
||||
|
||||
- ros_topic_name: "scan"
|
||||
gz_topic_name: "scan"
|
||||
ros_type_name: "sensor_msgs/msg/LaserScan"
|
||||
gz_type_name: "gz.msgs.LaserScan"
|
||||
direction: GZ_TO_ROS
|
||||
|
||||
- ros_topic_name: "camera/camera_info"
|
||||
gz_topic_name: "camera/camera_info"
|
||||
ros_type_name: "sensor_msgs/msg/CameraInfo"
|
||||
gz_type_name: "gz.msgs.CameraInfo"
|
||||
direction: GZ_TO_ROS
|
||||
@@ -0,0 +1,280 @@
|
||||
# Nav2 parameters for FusionCore + icp_odometry TurtleBot3 demo.
|
||||
#
|
||||
# Key difference from stock nav2 params:
|
||||
# - odom_topic: /fusion/odom (FusionCore output, not /odom or /odometry/filtered)
|
||||
# - No AMCL: rtabmap handles the map -> odom transform via its SLAM output.
|
||||
|
||||
bt_navigator:
|
||||
ros__parameters:
|
||||
use_sim_time: true
|
||||
global_frame: map
|
||||
robot_base_frame: base_footprint
|
||||
odom_topic: /fusion/odom
|
||||
bt_loop_duration: 10
|
||||
default_server_timeout: 20
|
||||
wait_for_service_timeout: 1000
|
||||
action_server_result_timeout: 900.0
|
||||
navigators: ['navigate_to_pose', 'navigate_through_poses']
|
||||
navigate_to_pose:
|
||||
plugin: 'nav2_bt_navigator::NavigateToPoseNavigator'
|
||||
navigate_through_poses:
|
||||
plugin: 'nav2_bt_navigator::NavigateThroughPosesNavigator'
|
||||
|
||||
controller_server:
|
||||
ros__parameters:
|
||||
use_sim_time: true
|
||||
enable_stamped_cmd_vel: True
|
||||
controller_frequency: 20.0
|
||||
min_x_velocity_threshold: 0.001
|
||||
min_y_velocity_threshold: 0.5
|
||||
min_theta_velocity_threshold: 0.001
|
||||
failure_tolerance: 0.3
|
||||
odom_topic: /fusion/odom
|
||||
progress_checker_plugins: ['progress_checker']
|
||||
goal_checker_plugins: ['general_goal_checker']
|
||||
controller_plugins: ['FollowPath']
|
||||
progress_checker:
|
||||
plugin: 'nav2_controller::SimpleProgressChecker'
|
||||
required_movement_radius: 0.5
|
||||
movement_time_allowance: 10.0
|
||||
general_goal_checker:
|
||||
stateful: true
|
||||
plugin: 'nav2_controller::SimpleGoalChecker'
|
||||
xy_goal_tolerance: 0.25
|
||||
yaw_goal_tolerance: 0.25
|
||||
FollowPath:
|
||||
plugin: 'nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController'
|
||||
desired_linear_vel: 0.2
|
||||
lookahead_dist: 0.6
|
||||
min_lookahead_dist: 0.3
|
||||
max_lookahead_dist: 0.9
|
||||
lookahead_time: 1.5
|
||||
rotate_to_heading_angular_vel: 1.8
|
||||
transform_tolerance: 0.1
|
||||
use_velocity_scaled_lookahead_dist: false
|
||||
min_approach_linear_velocity: 0.05
|
||||
approach_velocity_scaling_dist: 0.6
|
||||
use_collision_detection: true
|
||||
max_allowed_time_to_collision_up_to_goal: 1.0
|
||||
use_regulated_linear_velocity_scaling: true
|
||||
use_fixed_curvature_lookahead: false
|
||||
curvature_feedforward_gain: 1.0
|
||||
use_cost_regulated_linear_velocity_scaling: false
|
||||
regulated_linear_scaling_min_radius: 0.9
|
||||
regulated_linear_scaling_min_speed: 0.25
|
||||
use_rotate_to_heading: true
|
||||
allow_reversing: false
|
||||
rotate_to_heading_min_angle: 0.785
|
||||
max_angular_accel: 3.2
|
||||
max_robot_pose_search_dist: 10.0
|
||||
|
||||
local_costmap:
|
||||
local_costmap:
|
||||
ros__parameters:
|
||||
use_sim_time: true
|
||||
update_frequency: 5.0
|
||||
publish_frequency: 2.0
|
||||
global_frame: odom
|
||||
robot_base_frame: base_footprint
|
||||
rolling_window: true
|
||||
width: 3
|
||||
height: 3
|
||||
resolution: 0.05
|
||||
robot_radius: 0.22
|
||||
plugins: ['obstacle_layer', 'inflation_layer']
|
||||
obstacle_layer:
|
||||
plugin: 'nav2_costmap_2d::ObstacleLayer'
|
||||
enabled: true
|
||||
observation_sources: scan
|
||||
scan:
|
||||
topic: /scan
|
||||
max_obstacle_height: 2.0
|
||||
clearing: true
|
||||
marking: true
|
||||
data_type: 'LaserScan'
|
||||
raytrace_max_range: 3.0
|
||||
raytrace_min_range: 0.0
|
||||
obstacle_max_range: 2.5
|
||||
obstacle_min_range: 0.0
|
||||
inflation_layer:
|
||||
plugin: 'nav2_costmap_2d::InflationLayer'
|
||||
cost_scaling_factor: 3.0
|
||||
inflation_radius: 0.55
|
||||
always_send_full_costmap: true
|
||||
|
||||
global_costmap:
|
||||
global_costmap:
|
||||
ros__parameters:
|
||||
use_sim_time: true
|
||||
update_frequency: 1.0
|
||||
publish_frequency: 1.0
|
||||
global_frame: map
|
||||
robot_base_frame: base_footprint
|
||||
robot_radius: 0.22
|
||||
resolution: 0.05
|
||||
track_unknown_space: true
|
||||
plugins: ['static_layer', 'obstacle_layer', 'inflation_layer']
|
||||
static_layer:
|
||||
plugin: 'nav2_costmap_2d::StaticLayer'
|
||||
map_subscribe_transient_local: true
|
||||
obstacle_layer:
|
||||
plugin: 'nav2_costmap_2d::ObstacleLayer'
|
||||
enabled: true
|
||||
observation_sources: scan
|
||||
scan:
|
||||
topic: /scan
|
||||
max_obstacle_height: 2.0
|
||||
clearing: true
|
||||
marking: true
|
||||
data_type: 'LaserScan'
|
||||
raytrace_max_range: 3.0
|
||||
raytrace_min_range: 0.0
|
||||
obstacle_max_range: 2.5
|
||||
obstacle_min_range: 0.0
|
||||
inflation_layer:
|
||||
plugin: 'nav2_costmap_2d::InflationLayer'
|
||||
cost_scaling_factor: 3.0
|
||||
inflation_radius: 0.55
|
||||
always_send_full_costmap: true
|
||||
|
||||
planner_server:
|
||||
ros__parameters:
|
||||
use_sim_time: true
|
||||
expected_planner_frequency: 20.0
|
||||
planner_plugins: ['GridBased']
|
||||
GridBased:
|
||||
plugin: 'nav2_navfn_planner::NavfnPlanner'
|
||||
tolerance: 0.5
|
||||
use_astar: false
|
||||
allow_unknown: true
|
||||
|
||||
smoother_server:
|
||||
ros__parameters:
|
||||
use_sim_time: true
|
||||
enable_stamped_cmd_vel: True
|
||||
smoother_plugins: ['simple_smoother']
|
||||
simple_smoother:
|
||||
plugin: 'nav2_smoother::SimpleSmoother'
|
||||
tolerance: 1.0e-10
|
||||
max_its: 1000
|
||||
do_refinement: true
|
||||
|
||||
behavior_server:
|
||||
ros__parameters:
|
||||
use_sim_time: true
|
||||
enable_stamped_cmd_vel: True
|
||||
local_costmap_topic: local_costmap/costmap_raw
|
||||
global_costmap_topic: global_costmap/costmap_raw
|
||||
local_footprint_topic: local_costmap/published_footprint
|
||||
global_footprint_topic: global_costmap/published_footprint
|
||||
cycle_frequency: 10.0
|
||||
behavior_plugins: ['spin', 'backup', 'drive_on_heading', 'assisted_teleop', 'wait']
|
||||
spin:
|
||||
plugin: 'nav2_behaviors::Spin'
|
||||
backup:
|
||||
plugin: 'nav2_behaviors::BackUp'
|
||||
drive_on_heading:
|
||||
plugin: 'nav2_behaviors::DriveOnHeading'
|
||||
wait:
|
||||
plugin: 'nav2_behaviors::Wait'
|
||||
assisted_teleop:
|
||||
plugin: 'nav2_behaviors::AssistedTeleop'
|
||||
local_frame: odom
|
||||
global_frame: map
|
||||
robot_base_frame: base_footprint
|
||||
transform_tolerance: 0.1
|
||||
simulate_ahead_time: 2.0
|
||||
max_rotational_vel: 1.0
|
||||
min_rotational_vel: 0.4
|
||||
rotational_acc_lim: 3.2
|
||||
|
||||
velocity_smoother:
|
||||
ros__parameters:
|
||||
use_sim_time: true
|
||||
enable_stamped_cmd_vel: True
|
||||
smoothing_frequency: 20.0
|
||||
scale_velocities: false
|
||||
feedback: 'OPEN_LOOP'
|
||||
max_velocity: [0.26, 0.0, 1.0]
|
||||
min_velocity: [-0.26, 0.0, -1.0]
|
||||
max_accel: [2.5, 0.0, 3.2]
|
||||
max_decel: [-2.5, 0.0, -3.2]
|
||||
odom_topic: /fusion/odom
|
||||
odom_duration: 0.1
|
||||
deadband_velocity: [0.0, 0.0, 0.0]
|
||||
velocity_timeout: 1.0
|
||||
|
||||
collision_monitor:
|
||||
ros__parameters:
|
||||
use_sim_time: true
|
||||
enable_stamped_cmd_vel: True
|
||||
base_frame_id: base_footprint
|
||||
odom_frame_id: odom
|
||||
cmd_vel_in_topic: cmd_vel_smoothed
|
||||
cmd_vel_out_topic: cmd_vel
|
||||
state_topic: collision_monitor_state
|
||||
transform_error_pub_topic: transform_error
|
||||
polygons: ['FootprintApproach']
|
||||
FootprintApproach:
|
||||
type: polygon
|
||||
action_type: approach
|
||||
footprint_topic: local_costmap/published_footprint
|
||||
time_before_collision: 1.2
|
||||
simulation_time_step: 0.1
|
||||
min_points: 6
|
||||
visualize: false
|
||||
enabled: true
|
||||
observation_sources: ['scan']
|
||||
scan:
|
||||
type: scan
|
||||
topic: /scan
|
||||
min_height: 0.15
|
||||
max_height: 2.0
|
||||
enabled: true
|
||||
|
||||
|
||||
docking_server:
|
||||
ros__parameters:
|
||||
enable_stamped_cmd_vel: True
|
||||
controller_frequency: 50.0
|
||||
initial_perception_timeout: 5.0
|
||||
wait_charge_timeout: 5.0
|
||||
dock_approach_timeout: 30.0
|
||||
undock_linear_tolerance: 0.05
|
||||
undock_angular_tolerance: 0.1
|
||||
max_retries: 3
|
||||
base_frame: "base_footprint"
|
||||
fixed_frame: "odom"
|
||||
dock_backwards: false
|
||||
dock_prestaging_tolerance: 0.5
|
||||
|
||||
# Types of docks
|
||||
dock_plugins: ['simple_charging_dock']
|
||||
simple_charging_dock:
|
||||
plugin: 'opennav_docking::SimpleChargingDock'
|
||||
docking_threshold: 0.05
|
||||
staging_x_offset: -0.7
|
||||
use_external_detection_pose: true
|
||||
use_battery_status: false # true
|
||||
use_stall_detection: false # true
|
||||
|
||||
external_detection_timeout: 1.0
|
||||
external_detection_translation_x: -0.18
|
||||
external_detection_translation_y: 0.0
|
||||
external_detection_rotation_roll: -1.57
|
||||
external_detection_rotation_pitch: -1.57
|
||||
external_detection_rotation_yaw: 0.0
|
||||
filter_coef: 0.1
|
||||
|
||||
controller:
|
||||
k_phi: 3.0
|
||||
k_delta: 2.0
|
||||
v_linear_min: 0.15
|
||||
v_linear_max: 0.15
|
||||
use_collision_detection: true
|
||||
costmap_topic: "local_costmap/costmap_raw"
|
||||
footprint_topic: "local_costmap/published_footprint"
|
||||
transform_tolerance: 0.1
|
||||
projection_time: 5.0
|
||||
simulation_step: 0.1
|
||||
dock_collision_threshold: 0.3
|
||||
Reference in New Issue
Block a user