mirror of
https://github.com/introlab/rtabmap_ros.git
synced 2026-10-04 16:57:46 +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 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, 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, 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)
|
+ [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, 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)
|
+ [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.
|
* 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 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)
|
[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