zbw/yiliao2026
1
0
Fork 1
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

能够正常建图。下一步：修改里程计数据发布者，由odom（base）改为odom_combined（ekf）

Browse Source
This commit is contained in:
Orange
2026-06-11 14:51:15 +08:00
parent 436e8a2f5c
commit 4b3ca15584
10 changed files with 543 additions and 207 deletions
Download Patch File Download Diff File Expand all files Collapse all files

13
README.md
View File

@@ -82,7 +82,17 @@
---
<br></br>
# 三、其它说明
# 三、指令
启动雷达：
`ros2 launch lslidar_driver lsn10_launch.py `
open new terminal
```bash
ros2 topic pub -1 /lslidar_order std_msgs/msg/Int8 data:\ 1\ # (open radar)
ros2 topic pub -1 /lslidar_order std_msgs/msg/Int8 data:\ 0\ # (close radar)
```
# 四、其它说明
我设置了一些自定义指令，方便终端调试：
```bash
rosbuild <package_name> # 编译指定包
@@ -90,3 +100,4 @@ build_debug <package_name> # 以调试模式编译指定包
foxglove # 启动foxbridge
```
# 五、日志

3
src/gc_navigation2_real/maps/Readme.txt Normal file
View File

@@ -0,0 +1,3 @@
# 实车地图存放目录
# 使用 map_saver_cli 保存地图到此目录：
# ros2 run nav2_map_server map_saver_cli -f ~/test_ws/src/gc_navigation2_real/maps/your_map_name

14
src/gc_navigation2_slamtoolbox/config/slam_toolbox_async.yaml
View File

@@ -12,7 +12,7 @@ slam_toolbox:
# ============================ ROS 基础参数 =========================================
odom_frame: odom # 里程计坐标系
map_frame: map # 地图坐标系
base_frame: base_footprint # 机器人基座坐标系（与 URDF 根帧一致）
base_frame: base_link # 机器人基座坐标系（与 URDF 根帧一致）
scan_topic: /scan # 激光雷达话题
use_map_saver: true # 启用地图保存功能
mode: mapping # mapping = 定位 + 实时建图（地图可更新）
@@ -21,12 +21,12 @@ slam_toolbox:
# 启动时暂不加载（注释掉），通过服务在启动后手动加载：
# ros2 service call /slam_toolbox/deserialize_map slam_toolbox/srv/DeserializePoseGraph "{filename: '/home/guoch/test_ws/src/gc_navigation2_slamtoolbox/maps/my_map.posegraph', match_type: 1}"
# map_file_name: /home/guoch/test_ws/src/gc_navigation2_slamtoolbox/maps/my_map.posegraph
map_start_pose: [0.0, 0.0, 0.0] # 初始位姿 [x, y, yaw]（map 坐标系下）
map_start_pose: [-2.0, -2.0, 0.0] # 初始位姿 [x, y, yaw]（map 坐标系下）
# map_start_at_dock: true # 或从 docking 位姿启动（与 pose 互斥）
# ============================ 调试与性能 ===========================================
debug_logging: false # 调试日志（生产环境关闭）
throttle_scans: 1 # 每 N 帧激光处理一次（1=全部处理）
throttle_scans: 10 # 每 N 帧激光处理一次（1=全部处理）
transform_publish_period: 0.02 # TF 发布周期（秒），0=不发布里程计
map_update_interval: 3.0 # /map 话题更新间隔（秒），越小越实时
resolution: 0.05 # 地图分辨率（米/像素）
@@ -42,11 +42,11 @@ slam_toolbox:
# ============================ 通用建图参数 =========================================
use_scan_matching: true # 启用扫描匹配
use_scan_barycenter: true # 使用扫描重心
minimum_travel_distance: 0.5 # 最小移动距离触发处理（米）
minimum_travel_heading: 0.5 # 最小转向角度触发处理（弧度）
minimum_travel_distance: 0.1 # 最小移动距离触发处理（米）
minimum_travel_heading: 0.1 # 最小转向角度触发处理（弧度）
check_min_dist_and_heading_precisely: false
scan_buffer_size: 10 # 扫描缓冲区大小（越大匹配越稳定）
scan_buffer_maximum_scan_distance: 10.0 # 缓冲区最大扫描距离
scan_buffer_size: 5 # 扫描缓冲区大小（越大匹配越稳定）
scan_buffer_maximum_scan_distance: 5.0 # 缓冲区最大扫描距离
link_match_minimum_response_fine: 0.1 # 精细匹配最小响应
link_scan_maximum_distance: 1.5 # 链接扫描最大距离

73
src/gc_navigation2_slamtoolbox/config/slam_toolbox_mapping.yaml Normal file
View File

@@ -0,0 +1,73 @@
slam_toolbox:
ros__parameters:
# === Solver ===
solver_plugin: solver_plugins::CeresSolver
ceres_linear_solver: SPARSE_NORMAL_CHOLESKY
ceres_preconditioner: SCHUR_JACOBI
ceres_trust_strategy: LEVENBERG_MARQUARDT
ceres_dogleg_type: TRADITIONAL_DOGLEG
ceres_loss_function: None
# === ROS 基础 ===
odom_frame: odom
map_frame: map
base_frame: base_link
scan_topic: /scan
mode: mapping
use_map_saver: true
# === 调试与性能 ===
debug_logging: false
throttle_scans: 1
transform_publish_period: 0.02
map_update_interval: 3.0
resolution: 0.05
min_laser_range: 0.15
max_laser_range: 20.0
minimum_time_interval: 0.5
transform_timeout: 0.2
tf_buffer_duration: 30.0
stack_size_to_use: 40000000
enable_interactive_mode: true
# === 建图参数 ===
use_scan_matching: true
use_scan_barycenter: true
minimum_travel_distance: 0.5
minimum_travel_heading: 0.1
scan_buffer_size: 10
scan_buffer_maximum_scan_distance: 10.0
link_match_minimum_response_fine: 0.1
link_scan_maximum_distance: 1.5
# === 回环检测 ===
do_loop_closing: true
loop_match_minimum_chain_size: 10
loop_match_maximum_variance_coarse: 3.0
loop_match_minimum_response_coarse: 0.35
loop_match_minimum_response_fine: 0.45
loop_search_maximum_distance: 3.0
# === 扫描匹配 ===
correlation_search_space_dimension: 0.5
correlation_search_space_resolution: 0.01
correlation_search_space_smear_deviation: 0.1
loop_search_space_dimension: 8.0
loop_search_space_resolution: 0.05
loop_search_space_smear_deviation: 0.03
# === 匹配器参数 ===
distance_variance_penalty: 0.5
angle_variance_penalty: 1.0
fine_search_angle_offset: 0.00349
coarse_search_angle_offset: 0.349
coarse_angle_resolution: 0.0349
minimum_angle_penalty: 0.9
minimum_distance_penalty: 0.5
use_response_expansion: true
min_pass_through: 2
occupancy_threshold: 0.1
# MessageFilter queue size (default 1, too small for high-rate lidar)
scan_queue_size: 20

80
src/gc_navigation2_slamtoolbox/launch/gc_nav2_with_slam_online_real.launch.py Normal file
View File

@@ -0,0 +1,80 @@
# ============================================================================
# gc_nav2_with_slam_online_real.launch.py
# 功能：真机实时建图 — slam_toolbox 从零建图 + Nav2 导航
# ============================================================================
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from launch_ros.parameter_descriptions import ParameterValue
from launch.substitutions import Command
def generate_launch_description():
"""真机实时建图：slam_toolbox 从零建图 + Nav2 导航"""
ld = LaunchDescription()
# === 1. 包路径 ===
pkg_dir = get_package_share_directory('gc_navigation2_slamtoolbox')
nav2_bringup_dir = get_package_share_directory('nav2_bringup')
origincar_urdf_dir = get_package_share_directory('origincar_description')
# === 2. 参数 ===
use_sim_time = LaunchConfiguration('use_sim_time', default='false')
nav2_param_path = LaunchConfiguration('params_file', default=os.path.join(
pkg_dir, 'params', 'gc_navigation_slam.yaml'))
slam_params_file = os.path.join(pkg_dir, 'config', 'slam_toolbox_mapping.yaml')
# === 3. slam_toolbox：实时建图 → 直接发布到 /map ===
slam_toolbox_node = Node(
package='slam_toolbox',
executable='async_slam_toolbox_node',
name='slam_toolbox',
output='screen',
parameters=[slam_params_file,
{'use_sim_time': use_sim_time}],
)
# === 4. Nav2 导航栈 ===
navigation_launch = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
[nav2_bringup_dir, '/launch', '/navigation_launch.py']),
launch_arguments={
'use_sim_time': use_sim_time,
'params_file': nav2_param_path,
}.items(),
)
# === 5. 机器人模型 + TF ===
model = DeclareLaunchArgument(
name='model',
default_value=os.path.join(origincar_urdf_dir, 'urdf', 'origincar.urdf'))
robot_description = ParameterValue(
Command(['xacro ', LaunchConfiguration('model')]), value_type=str)
robot_state_publisher = Node(
package='robot_state_publisher',
executable='robot_state_publisher',
parameters=[{'robot_description': robot_description,
'use_sim_time': use_sim_time, 'publish_frequency': 30.0}],
)
base_footprint_tf = Node(
package='tf2_ros',
executable='static_transform_publisher',
name='base_footprint_to_base_link',
arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link'],
)
# === 6. 组装 ===
ld.add_action(model)
ld.add_action(robot_state_publisher)
ld.add_action(base_footprint_tf)
ld.add_action(slam_toolbox_node)
ld.add_action(navigation_launch)
return ld

40
src/gc_navigation2_slamtoolbox/params/gc_navigation_slam.yaml
View File

@@ -1,17 +1,17 @@
slam_toolbox:
ros__parameters:
use_sim_time: True
use_sim_time: False
bt_navigator:
ros__parameters:
use_sim_time: True
use_sim_time: False
global_frame: map
robot_base_frame: base_footprint
odom_topic: /odom
bt_loop_duration: 50
default_server_timeout: 20
# 阿克曼底盘专用 BT：移除 Spin，恢复行为 = 清代价地图 → 后退 → 等待
default_nav_to_pose_bt_xml: /home/guoch/test_ws/install/gc_navigation2_slamtoolbox/share/gc_navigation2_slamtoolbox/config/nav_to_pose_ackermann.xml
default_nav_to_pose_bt_xml: /home/sunrise/yiliao_ws/install/gc_navigation2_slamtoolbox/share/gc_navigation2_slamtoolbox/config/nav_to_pose_ackermann.xml
plugin_lib_names:
- nav2_compute_path_to_pose_action_bt_node
- nav2_compute_path_through_poses_action_bt_node
@@ -59,11 +59,11 @@ bt_navigator:
bt_navigator_rclcpp_node:
ros__parameters:
use_sim_time: True
use_sim_time: False
controller_server:
ros__parameters:
use_sim_time: True
use_sim_time: False
controller_frequency: 20.0
FollowPath:
plugin: "nav2_mppi_controller::MPPIController"
@@ -142,7 +142,7 @@ controller_server:
controller_server_rclcpp_node:
ros__parameters:
use_sim_time: True
use_sim_time: False
local_costmap:
local_costmap:
@@ -152,7 +152,7 @@ local_costmap:
transform_tolerance: 0.5
global_frame: odom
robot_base_frame: base_link
use_sim_time: True
use_sim_time: False
rolling_window: true
width: 3
height: 3
@@ -192,21 +192,21 @@ local_costmap:
always_send_full_costmap: True
local_costmap_client:
ros__parameters:
use_sim_time: True
use_sim_time: False
local_costmap_rclcpp_node:
ros__parameters:
use_sim_time: True
use_sim_time: False
global_costmap:
global_costmap:
ros__parameters:
use_sim_time: True
use_sim_time: False
transform_tolerance: 0.5
update_frequency: 1.0
publish_frequency: 1.0
global_frame: map
robot_base_frame: base_link
use_sim_time: True
use_sim_time: False
footprint: "[[0.14, 0.085],
[0.14, -0.085],
[-0.14, -0.085],
@@ -239,14 +239,14 @@ global_costmap:
always_send_full_costmap: True
global_costmap_client:
ros__parameters:
use_sim_time: True
use_sim_time: False
global_costmap_rclcpp_node:
ros__parameters:
use_sim_time: True
use_sim_time: False
map_saver:
ros__parameters:
use_sim_time: True
use_sim_time: False
save_map_timeout: 5.0
free_thresh_default: 0.25
occupied_thresh_default: 0.65
@@ -255,7 +255,7 @@ map_saver:
planner_server:
ros__parameters:
planner_plugins: ["GridBased"]
use_sim_time: True
use_sim_time: False
GridBased:
plugin: "nav2_smac_planner/SmacPlannerHybrid"
@@ -291,11 +291,11 @@ planner_server:
planner_server_rclcpp_node:
ros__parameters:
use_sim_time: True
use_sim_time: False
smoother_server:
ros__parameters:
use_sim_time: True
use_sim_time: False
smoother_plugins: ["simple_smoother"]
simple_smoother:
plugin: "nav2_smoother::SimpleSmoother"
@@ -321,7 +321,7 @@ behavior_server:
global_frame: odom
robot_base_frame: base_link
transform_tolerance: 0.5
use_sim_time: True
use_sim_time: False
simulate_ahead_time: 2.0
max_rotational_vel: 1.0
min_rotational_vel: 0.4
@@ -329,12 +329,12 @@ behavior_server:
robot_state_publisher:
ros__parameters:
use_sim_time: True
use_sim_time: False
waypoint_follower:
ros__parameters:
loop_rate: 20
use_sim_time: True
use_sim_time: False
stop_on_failure: false
waypoint_task_executor_plugin: "wait_at_waypoint"
wait_at_waypoint:

343
src/gc_navigation2_slamtoolbox/params/gc_navigation_slam.yaml.bak Normal file
View File

@@ -0,0 +1,343 @@
slam_toolbox:
ros__parameters:
use_sim_time: True
bt_navigator:
ros__parameters:
use_sim_time: True
global_frame: map
robot_base_frame: base_footprint
odom_topic: /odom
bt_loop_duration: 50
default_server_timeout: 20
# 阿克曼底盘专用 BT：移除 Spin，恢复行为 = 清代价地图 → 后退 → 等待
default_nav_to_pose_bt_xml: /home/guoch/test_ws/install/gc_navigation2_slamtoolbox/share/gc_navigation2_slamtoolbox/config/nav_to_pose_ackermann.xml
plugin_lib_names:
- nav2_compute_path_to_pose_action_bt_node
- nav2_compute_path_through_poses_action_bt_node
- nav2_smooth_path_action_bt_node
- nav2_follow_path_action_bt_node
- nav2_spin_action_bt_node
- nav2_wait_action_bt_node
- nav2_back_up_action_bt_node
- nav2_drive_on_heading_bt_node
- nav2_clear_costmap_service_bt_node
- nav2_is_stuck_condition_bt_node
- nav2_goal_reached_condition_bt_node
- nav2_goal_updated_condition_bt_node
- nav2_globally_updated_goal_condition_bt_node
- nav2_is_path_valid_condition_bt_node
- nav2_initial_pose_received_condition_bt_node
- nav2_reinitialize_global_localization_service_bt_node
- nav2_rate_controller_bt_node
- nav2_distance_controller_bt_node
- nav2_speed_controller_bt_node
- nav2_truncate_path_action_bt_node
- nav2_truncate_path_local_action_bt_node
- nav2_goal_updater_node_bt_node
- nav2_recovery_node_bt_node
- nav2_pipeline_sequence_bt_node
- nav2_round_robin_node_bt_node
- nav2_transform_available_condition_bt_node
- nav2_time_expired_condition_bt_node
- nav2_path_expiring_timer_condition
- nav2_distance_traveled_condition_bt_node
- nav2_single_trigger_bt_node
- nav2_is_battery_low_condition_bt_node
- nav2_navigate_through_poses_action_bt_node
- nav2_navigate_to_pose_action_bt_node
- nav2_remove_passed_goals_action_bt_node
- nav2_planner_selector_bt_node
- nav2_controller_selector_bt_node
- nav2_goal_checker_selector_bt_node
- nav2_controller_cancel_bt_node
- nav2_path_longer_on_approach_bt_node
- nav2_wait_cancel_bt_node
- nav2_spin_cancel_bt_node
- nav2_back_up_cancel_bt_node
- nav2_drive_on_heading_cancel_bt_node
bt_navigator_rclcpp_node:
ros__parameters:
use_sim_time: True
controller_server:
ros__parameters:
use_sim_time: True
controller_frequency: 20.0
FollowPath:
plugin: "nav2_mppi_controller::MPPIController"
time_steps: 36
model_dt: 0.05
batch_size: 1000
vx_std: 0.2
vy_std: 0.0
wz_std: 0.4
vx_max: 0.5
vx_min: -0.35
vy_max: 0.0
wz_max: 1.9
iteration_count: 1
temperature: 0.3
gamma: 0.015
motion_model: "Ackermann"
visualize: false
TrajectoryVisualizer:
trajectory_step: 5
time_step: 3
AckermannConstraints:
min_turning_r: 0.4
critics: ["ConstraintCritic", "CostCritic", "GoalCritic", "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", "PathAngleCritic", "PreferForwardCritic"]
ConstraintCritic:
enabled: true
cost_power: 1
cost_weight: 4.0
GoalCritic:
enabled: true
cost_power: 1
cost_weight: 5.0
threshold_to_consider: 1.4
GoalAngleCritic:
enabled: true
cost_power: 1
cost_weight: 3.0
threshold_to_consider: 0.5
PreferForwardCritic:
enabled: true
cost_power: 1
cost_weight: 5.0
threshold_to_consider: 0.5
CostCritic:
enabled: true
cost_power: 1
cost_weight: 3.81
critical_cost: 300.0
consider_footprint: true
collision_cost: 1000000.0
near_goal_distance: 1.0
trajectory_point_step: 2
PathAlignCritic:
enabled: true
cost_power: 1
cost_weight: 14.0
max_path_occupancy_ratio: 0.05
trajectory_point_step: 4
threshold_to_consider: 0.5
offset_from_furthest: 20
use_path_orientations: false
PathFollowCritic:
enabled: true
cost_power: 1
cost_weight: 5.0
offset_from_furthest: 5
threshold_to_consider: 1.4
PathAngleCritic:
enabled: true
cost_power: 1
cost_weight: 2.0
offset_from_furthest: 4
threshold_to_consider: 0.5
max_angle_to_furthest: 1.0
forward_preference: true
controller_server_rclcpp_node:
ros__parameters:
use_sim_time: True
local_costmap:
local_costmap:
ros__parameters:
update_frequency: 5.0
publish_frequency: 2.0
transform_tolerance: 0.5
global_frame: odom
robot_base_frame: base_link
use_sim_time: True
rolling_window: true
width: 3
height: 3
resolution: 0.05
footprint: "[[0.14, 0.085],
[0.14, -0.085],
[-0.14, -0.085],
[-0.14, 0.085]]"
footprint_padding: 0.02
plugins: ["voxel_layer", "inflation_layer"]
inflation_layer:
plugin: "nav2_costmap_2d::InflationLayer"
cost_scaling_factor: 3.0
inflation_radius: 0.55
voxel_layer:
plugin: "nav2_costmap_2d::VoxelLayer"
enabled: True
publish_voxel_map: True
origin_z: 0.0
z_resolution: 0.05
z_voxels: 16
max_obstacle_height: 2.0
mark_threshold: 0
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
static_layer:
map_subscribe_transient_local: True
always_send_full_costmap: True
local_costmap_client:
ros__parameters:
use_sim_time: True
local_costmap_rclcpp_node:
ros__parameters:
use_sim_time: True
global_costmap:
global_costmap:
ros__parameters:
use_sim_time: True
transform_tolerance: 0.5
update_frequency: 1.0
publish_frequency: 1.0
global_frame: map
robot_base_frame: base_link
use_sim_time: True
footprint: "[[0.14, 0.085],
[0.14, -0.085],
[-0.14, -0.085],
[-0.14, 0.085]]"
footprint_padding: 0.02
resolution: 0.05
track_unknown_space: true
plugins: ["static_layer", "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
static_layer:
plugin: "nav2_costmap_2d::StaticLayer"
map_subscribe_transient_local: True
inflation_layer:
plugin: "nav2_costmap_2d::InflationLayer"
cost_scaling_factor: 3.0
inflation_radius: 0.55
always_send_full_costmap: True
global_costmap_client:
ros__parameters:
use_sim_time: True
global_costmap_rclcpp_node:
ros__parameters:
use_sim_time: True
map_saver:
ros__parameters:
use_sim_time: True
save_map_timeout: 5.0
free_thresh_default: 0.25
occupied_thresh_default: 0.65
map_subscribe_transient_local: True
planner_server:
ros__parameters:
planner_plugins: ["GridBased"]
use_sim_time: True
GridBased:
plugin: "nav2_smac_planner/SmacPlannerHybrid"
downsample_costmap: false
downsampling_factor: 1
tolerance: 0.25
allow_unknown: true
max_iterations: 1000000
max_on_approach_iterations: 1000
max_planning_time: 5.0
motion_model_for_search: "REEDS_SHEPP"
angle_quantization_bins: 72
analytic_expansion_ratio: 3.5
analytic_expansion_max_length: 3.0
minimum_turning_radius: 0.40
reverse_penalty: 1.3 # 后退惩罚，越小越愿意后退（默认 2.0，设为 1.3 允许灵活倒车）
change_penalty: 0.0
non_straight_penalty: 1.2
cost_penalty: 2.0
retrospective_penalty: 0.015
lookup_table_size: 20.0
cache_obstacle_heuristic: false
viz_expansions: false
smooth_path: True
smoother:
max_iterations: 1000
w_smooth: 0.3
w_data: 0.2
tolerance: 1.0e-10
do_refinement: true
refinement_num: 2
planner_server_rclcpp_node:
ros__parameters:
use_sim_time: True
smoother_server:
ros__parameters:
use_sim_time: 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:
costmap_topic: local_costmap/costmap_raw
footprint_topic: local_costmap/published_footprint
cycle_frequency: 10.0
behavior_plugins: ["spin", "backup", "wait"]
spin:
plugin: "nav2_behaviors/Spin" # 需保留以匹配默认 BT XML
backup:
plugin: "nav2_behaviors/BackUp"
backup_dist: 0.8
backup_speed: 0.18
wait:
plugin: "nav2_behaviors/Wait"
wait_duration: 0.5
global_frame: odom
robot_base_frame: base_link
transform_tolerance: 0.5
use_sim_time: True
simulate_ahead_time: 2.0
max_rotational_vel: 1.0
min_rotational_vel: 0.4
rotational_acc_lim: 3.2
robot_state_publisher:
ros__parameters:
use_sim_time: True
waypoint_follower:
ros__parameters:
loop_rate: 20
use_sim_time: True
stop_on_failure: false
waypoint_task_executor_plugin: "wait_at_waypoint"
wait_at_waypoint:
plugin: "nav2_waypoint_follower::WaitAtWaypoint"
enabled: True
waypoint_pause_duration: 200

182
src/origincar_base/config/ekf.yaml
View File

@@ -1,222 +1,48 @@
### ekf config file ###
ekf_filter_node:
ros__parameters:
# 启用通过话题设置初始位姿
use_pose_with_covariance_stamped: true # 允许通过话题重置
pose0: /set_pose # 指定话题名称
pose0_config: [true, true, false, # 使用 x, y
false, false, true] # 使用 yaw
pose0_differential: false
# The frequency, in Hz, at which the filter will output a position estimate. Note that the filter will not begin
# computation until it receives at least one message from one of the inputs. It will then run continuously at the
# frequency specified here, regardless of whether it receives more measurements. Defaults to 30 if unspecified.
frequency: 30.0
# The period, in seconds, after which we consider a sensor to have timed out. In this event, we carry out a predict
# cycle on the EKF without correcting it. This parameter can be thought of as the minimum frequency with which the
# filter will generate new output. Defaults to 1 / frequency if not specified.
sensor_timeout: 2.0
# ekf_localization_node and ukf_localization_node both use a 3D omnidirectional motion model. If this parameter is
# set to true, no 3D information will be used in your state estimate. Use this if you are operating in a planar
# environment and want to ignore the effect of small variations in the ground plane that might otherwise be detected
# by, for example, an IMU. Defaults to false if unspecified.
two_d_mode: true
# Use this parameter to provide an offset to the transform generated by ekf_localization_node. This can be used for
# future dating the transform, which is required for interaction with some other packages. Defaults to 0.0 if
# unspecified.
transform_time_offset: 0.0
# Use this parameter to provide specify how long the tf listener should wait for a transform to become available.
# Defaults to 0.0 if unspecified.
transform_timeout: 0.2
# If you're having trouble, try setting this to true, and then echo the /diagnostics_agg topic to see if the node is
# unhappy with any settings or data.
print_diagnostics: false
# Debug settings. Not for the faint of heart. Outputs a ludicrous amount of information to the file specified by
# debug_out_file. I hope you like matrices! Please note that setting this to true will have strongly deleterious
# effects on the performance of the node. Defaults to false if unspecified.
debug: false
# Defaults to "robot_localization_debug.txt" if unspecified. Please specify the full path.
debug_out_file: /path/to/debug/file.txt
# Whether to broadcast the transformation over the /tf topic. Defaults to true if unspecified.
publish_tf: true
# Whether to publish the acceleration state. Defaults to false if unspecified.
publish_acceleration: false
# REP-105 (http://www.ros.org/reps/rep-0105.html) specifies four principal coordinate frames: base_link, odom, map, and
# earth. base_link is the coordinate frame that is affixed to the robot. Both odom and map are world-fixed frames.
# The robot's position in the odom frame will drift over time, but is accurate in the short term and should be
# continuous. The odom frame is therefore the best frame for executing local motion plans. The map frame, like the odom
# frame, is a world-fixed coordinate frame, and while it contains the most globally accurate position estimate for your
# robot, it is subject to discrete jumps, e.g., due to the fusion of GPS data or a correction from a map-based
# localization node. The earth frame is used to relate multiple map frames by giving them a common reference frame.
# ekf_localization_node and ukf_localization_node are not concerned with the earth frame.
# Here is how to use the following settings:
# 1. Set the map_frame, odom_frame, and base_link frames to the appropriate frame names for your system.
# 1a. If your system does not have a map_frame, just remove it, and make sure "world_frame" is set to the value of
# odom_frame.
# 2. If you are fusing continuous position data such as wheel encoder odometry, visual odometry, or IMU data, set
# "world_frame" to your odom_frame value. This is the default behavior for robot_localization's state estimation nodes.
# 3. If you are fusing global absolute position data that is subject to discrete jumps (e.g., GPS or position updates
# from landmark observations) then:
# 3a. Set your "world_frame" to your map_frame value
# 3b. MAKE SURE something else is generating the odom->base_link transform. Note that this can even be another state
# estimation node from robot_localization! However, that instance should *not* fuse the global data.
map_frame: map # Defaults to "map" if unspecified
odom_frame: odom_combined # Defaults to "odom" if unspecified
base_link_frame: base_footprint # Defaults to "base_link" if unspecified
world_frame: odom_combined # Defaults to the value of odom_frame if unspecified
map_frame: map
odom_frame: odom
base_link_frame: base_link
world_frame: odom
# The filter accepts an arbitrary number of inputs from each input message type (nav_msgs/Odometry,
# geometry_msgs/PoseWithCovarianceStamped, geometry_msgs/TwistWithCovarianceStamped,
# sensor_msgs/Imu). To add an input, simply append the next number in the sequence to its "base" name, e.g., odom0,
# odom1, twist0, twist1, imu0, imu1, imu2, etc. The value should be the topic name. These parameters obviously have no
# default values, and must be specified.
odom0: odom
# Each sensor reading updates some or all of the filter's state. These options give you greater control over which
# values from each measurement are fed to the filter. For example, if you have an odometry message as input, but only
# want to use its Z position value, then set the entire vector to false, except for the third entry. The order of the
# values is x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Note that not some message types
# do not provide some of the state variables estimated by the filter. For example, a TwistWithCovarianceStamped message
# has no pose information, so the first six values would be meaningless in that case. Each vector defaults to all false
# if unspecified, effectively making this parameter required for each sensor.
odom0_config: [true, false, false,
false, false, false,
true, true, false,
false, false, true,
false, false, false]
# If you have high-frequency data or are running with a low frequency parameter value, then you may want to increase
# the size of the subscription queue so that more measurements are fused.
odom0_queue_size: 10
# [ADVANCED] Large messages in ROS can exhibit strange behavior when they arrive at a high frequency. This is a result
# of Nagle's algorithm. This option tells the ROS subscriber to use the tcpNoDelay option, which disables Nagle's
# algorithm.
odom0_nodelay: false
# [ADVANCED] When measuring one pose variable with two sensors, a situation can arise in which both sensors under-
# report their covariances. This can lead to the filter rapidly jumping back and forth between each measurement as they
# arrive. In these cases, it often makes sense to (a) correct the measurement covariances, or (b) if velocity is also
# measured by one of the sensors, let one sensor measure pose, and the other velocity. However, doing (a) or (b) isn't
# always feasible, and so we expose the differential parameter. When differential mode is enabled, all absolute pose
# data is converted to velocity data by differentiating the absolute pose measurements. These velocities are then
# integrated as usual. NOTE: this only applies to sensors that provide pose measurements; setting differential to true
# for twist measurements has no effect.
odom0_differential: true
# [ADVANCED] When the node starts, if this parameter is true, then the first measurement is treated as a "zero point"
# for all future measurements. While you can achieve the same effect with the differential paremeter, the key
# difference is that the relative parameter doesn't cause the measurement to be converted to a velocity before
# integrating it. If you simply want your measurements to start at 0 for a given sensor, set this to true.
odom0_relative: false
# [ADVANCED] If your data is subject to outliers, use these threshold settings, expressed as Mahalanobis distances, to
# control how far away from the current vehicle state a sensor measurement is permitted to be. Each defaults to
# numeric_limits<double>::max() if unspecified. It is strongly recommended that these parameters be removed if not
# required. Data is specified at the level of pose and twist variables, rather than for each variable in isolation.
# For messages that have both pose and twist data, the parameter specifies to which part of the message we are applying
# the thresholds.
# odom0_pose_rejection_threshold: 5.0
# odom0_twist_rejection_threshold: 1.0
imu0: /imu/data_raw
imu0_config: [false, false, false,
false, false, true,
false, false, false,
false, false, true,
false, false, false]
imu0_nodelay: false
imu0_differential: false
imu0_relative: true
imu0_queue_size: 10
imu0_pose_rejection_threshold: 20.0 # Note the difference in parameter names
imu0_twist_rejection_threshold: 1.542 #
imu0_linear_acceleration_rejection_threshold: 10.0 #
# [ADVANCED] Some IMUs automatically remove acceleration due to gravity, and others don't. If yours doesn't, please set
# this to true, and *make sure* your data conforms to REP-103, specifically, that the data is in ENU frame.
imu0_remove_gravitational_acceleration: true
# [ADVANCED] The EKF and UKF models follow a standard predict/correct cycle. During prediction, if there is no
# acceleration reference, the velocity at time t+1 is simply predicted to be the same as the velocity at time t. During
# correction, this predicted value is fused with the measured value to produce the new velocity estimate. This can be
# problematic, as the final velocity will effectively be a weighted average of the old velocity and the new one. When
# this velocity is the integrated into a new pose, the result can be sluggish covergence. This effect is especially
# noticeable with LIDAR data during rotations. To get around it, users can try inflating the process_noise_covariance
# for the velocity variable in question, or decrease the variance of the variable in question in the measurement
# itself. In addition, users can also take advantage of the control command being issued to the robot at the time we
# make the prediction. If control is used, it will get converted into an acceleration term, which will be used during
# predicition. Note that if an acceleration measurement for the variable in question is available from one of the
# inputs, the control term will be ignored.
# Whether or not we use the control input during predicition. Defaults to false.
use_control: false
# Whether the input (assumed to be cmd_vel) is a geometry_msgs/Twist or geometry_msgs/TwistStamped message. Defaults to
# false.
stamped_control: false
# The last issued control command will be used in prediction for this period. Defaults to 0.2.
control_timeout: 0.2
# Which velocities are being controlled. Order is vx, vy, vz, vroll, vpitch, vyaw.
control_config: [true, false, false, false, false, true]
# Places limits on how large the acceleration term will be. Should match your robot's kinematics.
acceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 3.4]
# Acceleration and deceleration limits are not always the same for robots.
deceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 4.5]
# If your robot cannot instantaneously reach its acceleration limit, the permitted change can be controlled with these
# gains
acceleration_gains: [0.8, 0.0, 0.0, 0.0, 0.0, 0.9]
# If your robot cannot instantaneously reach its deceleration limit, the permitted change can be controlled with these
# gains
deceleration_gains: [1.0, 0.0, 0.0, 0.0, 0.0, 1.0]
# [ADVANCED] The process noise covariance matrix can be difficult to tune, and can vary for each application, so it is
# exposed as a configuration parameter. This matrix represents the noise we add to the total error after each
# prediction step. The better the omnidirectional motion model matches your system, the smaller these values can be.
# However, if users find that a given variable is slow to converge, one approach is to increase the
# process_noise_covariance diagonal value for the variable in question, which will cause the filter's predicted error
# to be larger, which will cause the filter to trust the incoming measurement more during correction. The values are
# ordered as x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Defaults to the matrix below if
# unspecified.
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,
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,
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,
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,
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,
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,
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,
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,
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,
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,
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,
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,
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,
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,
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]
# [ADVANCED] This represents the initial value for the state estimate error covariance matrix. Setting a diagonal
# value (variance) to a large value will result in rapid convergence for initial measurements of the variable in
# question. Users should take care not to use large values for variables that will not be measured directly. The values
# are ordered as x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Defaults to the matrix below
#if unspecified.
initial_estimate_covariance: [1e-9, 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.0, 1e-9, 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.0, 1e-9, 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.0, 1e-9, 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.0, 1e-9, 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.0, 1e-9, 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.0, 1e-9, 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.0, 1e-9, 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.0, 1e-9, 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.0, 1e-9, 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.0, 1e-9, 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.0, 1e-9, 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.0, 1e-9, 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.0, 1e-9, 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.0, 1e-9]

BIN
src/origincar_base/scripts/__pycache__/cmd_vel_to_ackermann_drive.cpython-310.pyc Normal file
View File

Binary file not shown.

0
src/origincar_base/scripts/cmd_vel_to_ackermann_drive.py Normal file → Executable file
View File