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
@@ -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