forked from zbw/yiliao2026
Please enter the commit message for your changes. Lines starting
with '#' will be ignored, and an empty message aborts the commit. On branch master Your branch is ahead of 'origin/master' by 2 commits. (use "git push" to publish your local commits) Changes to be committed: modified: .gitignore modified: README.md new file: bashes/auto-wifi-connect.service new file: bashes/auto-wifi-connect.sh deleted: keyboard_control.py new file: my_model/image.png new file: path_follower_demo.py new file: scripts/PIDtracking.py new file: scripts/__pycache__/publish_sine_path.cpython-310.pyc new file: scripts/publish_sine_path.py modified: src/gc_navigation2_slamtoolbox/params/gc_navigation_slam.yaml modified: src/gc_navigation2_slamtoolbox/params/gc_navigation_slam.yaml.bak new file: src/gc_navigation2_slamtoolbox/params/gc_navigation_slam.yaml.bak2 modified: src/origincar_base/config/ekf.yaml new file: src/origincar_base/config/ekf.yaml.bak modified: src/origincar_base/launch/base_serial.launch.py new file: src/origincar_base/launch/base_serial.launch.py.bak modified: src/origincar_base/launch/origincar_bringup.launch.py new file: src/past_control/CMakeLists.txt new file: src/past_control/config/past_control.yaml new file: src/past_control/include/past_control/tools.h new file: src/past_control/launch/past_control.launch.py new file: src/past_control/msg/Obstacle.msg new file: src/past_control/msg/ObstacleArray.msg new file: src/past_control/package.xml new file: src/past_control/src/lane_follower_node.cpp new file: src/past_control/src/obstacle_detector_node.cpp new file: src/past_control/src/racing_orchestrator.cpp new file: src/planner/CMakeLists.txt new file: src/planner/config/planner.yaml new file: src/planner/launch/planner.launch.py new file: src/planner/package.xml new file: src/planner/src/planner_version.cpp modified: src/qr_detection/src/qr_dete_depth.cpp new file: src/racing_control/CMakeLists.txt new file: src/racing_control/include/racing_control/racing_control.hpp new file: src/racing_control/package.xml new file: src/racing_control/src/racing_control.cpp modified: src/vlm_detect/setup.py new file: src/vlm_detect/vlm_detect/__pycache__/__init__.cpython-310.pyc new file: src/vlm_detect/vlm_detect/__pycache__/tts_node.cpython-310.pyc new file: src/vlm_detect/vlm_detect/test_publisher.py new file: src/vlm_detect/vlm_detect/tts_node.py modified: src/vlm_detect/vlm_detect/vlm_node.py new file: tools/measure_turning_radius.py new file: tools/set_volume.py new file: tools/udp_to_cmdvel.py new file: tools/windows_keyboard_control.py new file: vlm_server.py new file: "\350\260\203\350\257\225\350\256\260\345\275\225.Assets/1.png" renamed: "\350\260\203\350\257\225\350\256\260\345\275\225.log" -> "\350\260\203\350\257\225\350\256\260\345\275\225.md"
This commit is contained in:
@@ -1,7 +1,6 @@
|
||||
slam_toolbox:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
|
||||
bt_navigator:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
@@ -10,165 +9,111 @@ bt_navigator:
|
||||
odom_topic: /odom
|
||||
bt_loop_duration: 50
|
||||
default_server_timeout: 20
|
||||
# 阿克曼底盘专用 BT：移除 Spin，恢复行为 = 清代价地图 → 后退 → 等待
|
||||
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
|
||||
- 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
|
||||
|
||||
- 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: False
|
||||
|
||||
controller_server:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
controller_frequency: 20.0
|
||||
FollowPath:
|
||||
plugin: "nav2_mppi_controller::MPPIController"
|
||||
time_steps: 20
|
||||
model_dt: 0.05
|
||||
batch_size: 200
|
||||
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: 2.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: 7.0 # PathAlignCritic
|
||||
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
|
||||
|
||||
plugin: nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController
|
||||
desired_linear_vel: 0.3
|
||||
max_angular_vel: 1.9
|
||||
min_angular_vel: 0.2
|
||||
lookahead_dist: 0.5
|
||||
min_lookahead_dist: 0.3
|
||||
max_lookahead_dist: 0.8
|
||||
lookahead_time: 1.5
|
||||
rotate_to_heading_angular_vel: 0.8
|
||||
transform_tolerance: 0.5
|
||||
use_velocity_scaled_lookahead_dist: True
|
||||
min_approach_linear_vel: 0.1
|
||||
approach_velocity_scaling_dist: 0.5
|
||||
use_collision_detection: True
|
||||
max_allowed_time_to_collision: 1.0
|
||||
use_cost_regulated_linear_velocity_scaling: True
|
||||
cost_scaling_dist: 0.6
|
||||
cost_scaling_gain: 1.0
|
||||
regulated_linear_scaling_min_radius: 0.35
|
||||
regulated_linear_scaling_min_speed: 0.1
|
||||
use_rotate_to_heading: True
|
||||
rotate_to_heading_min_angle: 0.5
|
||||
max_robot_pose_search_dist: 10.0
|
||||
use_interpolation: False
|
||||
allow_reversing: False
|
||||
controller_server_rclcpp_node:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
|
||||
local_costmap:
|
||||
local_costmap:
|
||||
ros__parameters:
|
||||
update_frequency: 1.0
|
||||
publish_frequency: 2.0
|
||||
transform_tolerance: 0.5
|
||||
transform_tolerance: 2.0
|
||||
global_frame: odom
|
||||
robot_base_frame: base_link
|
||||
use_sim_time: False
|
||||
rolling_window: 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: '[[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"]
|
||||
plugins:
|
||||
- voxel_layer
|
||||
- inflation_layer
|
||||
inflation_layer:
|
||||
plugin: "nav2_costmap_2d::InflationLayer"
|
||||
plugin: nav2_costmap_2d::InflationLayer
|
||||
cost_scaling_factor: 3.0
|
||||
inflation_radius: 0.2
|
||||
inflation_radius: 0.3
|
||||
voxel_layer:
|
||||
plugin: "nav2_costmap_2d::VoxelLayer"
|
||||
plugin: nav2_costmap_2d::VoxelLayer
|
||||
enabled: True
|
||||
publish_voxel_map: True
|
||||
origin_z: 0.0
|
||||
@@ -182,7 +127,7 @@ local_costmap:
|
||||
max_obstacle_height: 2.0
|
||||
clearing: True
|
||||
marking: True
|
||||
data_type: "LaserScan"
|
||||
data_type: LaserScan
|
||||
raytrace_max_range: 3.0
|
||||
raytrace_min_range: 0.0
|
||||
obstacle_max_range: 2.5
|
||||
@@ -196,27 +141,25 @@ local_costmap:
|
||||
local_costmap_rclcpp_node:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
|
||||
global_costmap:
|
||||
global_costmap:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
transform_tolerance: 0.5
|
||||
transform_tolerance: 2.0
|
||||
update_frequency: 1.0
|
||||
publish_frequency: 1.0
|
||||
global_frame: map
|
||||
robot_base_frame: base_link
|
||||
use_sim_time: False
|
||||
footprint: "[[0.14, 0.085],
|
||||
[0.14, -0.085],
|
||||
[-0.14, -0.085],
|
||||
[-0.14, 0.085]]"
|
||||
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"]
|
||||
track_unknown_space: True
|
||||
plugins:
|
||||
- static_layer
|
||||
- obstacle_layer
|
||||
- inflation_layer
|
||||
obstacle_layer:
|
||||
plugin: "nav2_costmap_2d::ObstacleLayer"
|
||||
plugin: nav2_costmap_2d::ObstacleLayer
|
||||
enabled: True
|
||||
observation_sources: scan
|
||||
scan:
|
||||
@@ -224,18 +167,18 @@ global_costmap:
|
||||
max_obstacle_height: 2.0
|
||||
clearing: True
|
||||
marking: True
|
||||
data_type: "LaserScan"
|
||||
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"
|
||||
plugin: nav2_costmap_2d::StaticLayer
|
||||
map_subscribe_transient_local: True
|
||||
inflation_layer:
|
||||
plugin: "nav2_costmap_2d::InflationLayer"
|
||||
plugin: nav2_costmap_2d::InflationLayer
|
||||
cost_scaling_factor: 3.0
|
||||
inflation_radius: 0.2
|
||||
inflation_radius: 0.3
|
||||
always_send_full_costmap: True
|
||||
global_costmap_client:
|
||||
ros__parameters:
|
||||
@@ -243,7 +186,6 @@ global_costmap:
|
||||
global_costmap_rclcpp_node:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
|
||||
map_saver:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
@@ -251,93 +193,90 @@ map_saver:
|
||||
free_thresh_default: 0.25
|
||||
occupied_thresh_default: 0.65
|
||||
map_subscribe_transient_local: True
|
||||
|
||||
planner_server:
|
||||
ros__parameters:
|
||||
planner_plugins: ["GridBased"]
|
||||
planner_plugins:
|
||||
- GridBased
|
||||
use_sim_time: False
|
||||
|
||||
GridBased:
|
||||
plugin: "nav2_smac_planner/SmacPlannerHybrid"
|
||||
downsample_costmap: false
|
||||
plugin: nav2_smac_planner/SmacPlannerHybrid
|
||||
downsample_costmap: False
|
||||
downsampling_factor: 1
|
||||
tolerance: 0.25
|
||||
allow_unknown: true
|
||||
allow_unknown: True
|
||||
max_iterations: 100000
|
||||
max_on_approach_iterations: 1000
|
||||
max_planning_time: 5.0
|
||||
motion_model_for_search: "REEDS_SHEPP"
|
||||
motion_model_for_search: REEDS_SHEPP
|
||||
angle_quantization_bins: 72
|
||||
analytic_expansion_ratio: 3.5
|
||||
analytic_expansion_ratio: 2.0
|
||||
analytic_expansion_max_length: 3.0
|
||||
minimum_turning_radius: 0.40
|
||||
reverse_penalty: 1.3 # 后退惩罚，越小越愿意后退（默认 2.0，设为 1.3 允许灵活倒车）
|
||||
minimum_turning_radius: 0.35
|
||||
reverse_penalty: 1.3
|
||||
change_penalty: 0.0
|
||||
non_straight_penalty: 0.5
|
||||
non_straight_penalty: 0.0
|
||||
cost_penalty: 5.0
|
||||
retrospective_penalty: 0.015
|
||||
lookup_table_size: 5.0
|
||||
cache_obstacle_heuristic: false
|
||||
viz_expansions: false
|
||||
cache_obstacle_heuristic: False
|
||||
viz_expansions: False
|
||||
smooth_path: True
|
||||
|
||||
smoother:
|
||||
max_iterations: 1000
|
||||
w_smooth: 0.3
|
||||
w_smooth: 0.4
|
||||
w_data: 0.2
|
||||
tolerance: 1.0e-10
|
||||
do_refinement: true
|
||||
refinement_num: 2
|
||||
|
||||
do_refinement: True
|
||||
refinement_num: 4
|
||||
planner_server_rclcpp_node:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
|
||||
smoother_server:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
smoother_plugins: ["simple_smoother"]
|
||||
smoother_plugins:
|
||||
- simple_smoother
|
||||
simple_smoother:
|
||||
plugin: "nav2_smoother::SimpleSmoother"
|
||||
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"]
|
||||
behavior_plugins:
|
||||
- spin
|
||||
- backup
|
||||
- wait
|
||||
spin:
|
||||
plugin: "nav2_behaviors/Spin" # 需保留以匹配默认 BT XML
|
||||
plugin: nav2_behaviors/Spin
|
||||
backup:
|
||||
plugin: "nav2_behaviors/BackUp"
|
||||
plugin: nav2_behaviors/BackUp
|
||||
backup_dist: 0.8
|
||||
backup_speed: 0.18
|
||||
wait:
|
||||
plugin: "nav2_behaviors/Wait"
|
||||
plugin: nav2_behaviors/Wait
|
||||
wait_duration: 0.5
|
||||
global_frame: odom
|
||||
robot_base_frame: base_link
|
||||
transform_tolerance: 0.5
|
||||
transform_tolerance: 2.0
|
||||
use_sim_time: False
|
||||
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: False
|
||||
|
||||
waypoint_follower:
|
||||
ros__parameters:
|
||||
loop_rate: 20
|
||||
use_sim_time: False
|
||||
stop_on_failure: false
|
||||
waypoint_task_executor_plugin: "wait_at_waypoint"
|
||||
stop_on_failure: False
|
||||
waypoint_task_executor_plugin: wait_at_waypoint
|
||||
wait_at_waypoint:
|
||||
plugin: "nav2_waypoint_follower::WaitAtWaypoint"
|
||||
plugin: nav2_waypoint_follower::WaitAtWaypoint
|
||||
enabled: True
|
||||
waypoint_pause_duration: 200
|
||||
|
||||
@@ -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,17 +59,17 @@ 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"
|
||||
time_steps: 36
|
||||
time_steps: 20
|
||||
model_dt: 0.05
|
||||
batch_size: 1000
|
||||
batch_size: 200
|
||||
vx_std: 0.2
|
||||
vy_std: 0.0
|
||||
wz_std: 0.4
|
||||
@@ -105,7 +105,7 @@ controller_server:
|
||||
PreferForwardCritic:
|
||||
enabled: true
|
||||
cost_power: 1
|
||||
cost_weight: 5.0
|
||||
cost_weight: 2.0
|
||||
threshold_to_consider: 0.5
|
||||
CostCritic:
|
||||
enabled: true
|
||||
@@ -119,7 +119,7 @@ controller_server:
|
||||
PathAlignCritic:
|
||||
enabled: true
|
||||
cost_power: 1
|
||||
cost_weight: 14.0
|
||||
cost_weight: 7.0 # PathAlignCritic
|
||||
max_path_occupancy_ratio: 0.05
|
||||
trajectory_point_step: 4
|
||||
threshold_to_consider: 0.5
|
||||
@@ -142,17 +142,17 @@ controller_server:
|
||||
|
||||
controller_server_rclcpp_node:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
use_sim_time: False
|
||||
|
||||
local_costmap:
|
||||
local_costmap:
|
||||
ros__parameters:
|
||||
update_frequency: 5.0
|
||||
update_frequency: 1.0
|
||||
publish_frequency: 2.0
|
||||
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
|
||||
@@ -166,7 +166,7 @@ local_costmap:
|
||||
inflation_layer:
|
||||
plugin: "nav2_costmap_2d::InflationLayer"
|
||||
cost_scaling_factor: 3.0
|
||||
inflation_radius: 0.55
|
||||
inflation_radius: 0.2
|
||||
voxel_layer:
|
||||
plugin: "nav2_costmap_2d::VoxelLayer"
|
||||
enabled: True
|
||||
@@ -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],
|
||||
@@ -235,18 +235,18 @@ global_costmap:
|
||||
inflation_layer:
|
||||
plugin: "nav2_costmap_2d::InflationLayer"
|
||||
cost_scaling_factor: 3.0
|
||||
inflation_radius: 0.55
|
||||
inflation_radius: 0.2
|
||||
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"
|
||||
@@ -263,7 +263,7 @@ planner_server:
|
||||
downsampling_factor: 1
|
||||
tolerance: 0.25
|
||||
allow_unknown: true
|
||||
max_iterations: 1000000
|
||||
max_iterations: 100000
|
||||
max_on_approach_iterations: 1000
|
||||
max_planning_time: 5.0
|
||||
motion_model_for_search: "REEDS_SHEPP"
|
||||
@@ -273,10 +273,10 @@ planner_server:
|
||||
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
|
||||
non_straight_penalty: 0.5
|
||||
cost_penalty: 5.0
|
||||
retrospective_penalty: 0.015
|
||||
lookup_table_size: 20.0
|
||||
lookup_table_size: 5.0
|
||||
cache_obstacle_heuristic: false
|
||||
viz_expansions: false
|
||||
smooth_path: True
|
||||
@@ -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:
|
||||
|
||||
@@ -0,0 +1,343 @@
|
||||
slam_toolbox:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
|
||||
bt_navigator:
|
||||
ros__parameters:
|
||||
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/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
|
||||
- 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: False
|
||||
|
||||
controller_server:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
controller_frequency: 20.0
|
||||
FollowPath:
|
||||
plugin: "nav2_mppi_controller::MPPIController"
|
||||
time_steps: 20
|
||||
model_dt: 0.05
|
||||
batch_size: 200
|
||||
vx_std: 0.2
|
||||
vy_std: 0.0
|
||||
wz_std: 0.8
|
||||
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: 2.0
|
||||
GoalAngleCritic:
|
||||
enabled: true
|
||||
cost_power: 1
|
||||
cost_weight: 3.0
|
||||
threshold_to_consider: 0.5
|
||||
PreferForwardCritic:
|
||||
enabled: true
|
||||
cost_power: 1
|
||||
cost_weight: 2.0
|
||||
threshold_to_consider: 0.5
|
||||
CostCritic:
|
||||
enabled: true
|
||||
cost_power: 1
|
||||
cost_weight: 8.0
|
||||
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: 5.0 # PathAlignCritic
|
||||
max_path_occupancy_ratio: 0.05
|
||||
trajectory_point_step: 4
|
||||
threshold_to_consider: 0.5
|
||||
offset_from_furthest: 10
|
||||
use_path_orientations: false
|
||||
PathFollowCritic:
|
||||
enabled: true
|
||||
cost_power: 1
|
||||
cost_weight: 3.0
|
||||
offset_from_furthest: 5
|
||||
threshold_to_consider: 1.4
|
||||
PathAngleCritic:
|
||||
enabled: true
|
||||
cost_power: 1
|
||||
cost_weight: 1.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: False
|
||||
|
||||
local_costmap:
|
||||
local_costmap:
|
||||
ros__parameters:
|
||||
update_frequency: 1.0
|
||||
publish_frequency: 2.0
|
||||
transform_tolerance: 2.0
|
||||
global_frame: odom
|
||||
robot_base_frame: base_link
|
||||
use_sim_time: False
|
||||
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.3
|
||||
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: False
|
||||
local_costmap_rclcpp_node:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
|
||||
global_costmap:
|
||||
global_costmap:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
transform_tolerance: 2.0
|
||||
update_frequency: 1.0
|
||||
publish_frequency: 1.0
|
||||
global_frame: map
|
||||
robot_base_frame: base_link
|
||||
use_sim_time: False
|
||||
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.3
|
||||
always_send_full_costmap: True
|
||||
global_costmap_client:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
global_costmap_rclcpp_node:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
|
||||
map_saver:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
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: False
|
||||
|
||||
GridBased:
|
||||
plugin: "nav2_smac_planner/SmacPlannerHybrid"
|
||||
downsample_costmap: false
|
||||
downsampling_factor: 1
|
||||
tolerance: 0.25
|
||||
allow_unknown: true
|
||||
max_iterations: 100000
|
||||
max_on_approach_iterations: 1000
|
||||
max_planning_time: 5.0
|
||||
motion_model_for_search: "REEDS_SHEPP"
|
||||
angle_quantization_bins: 72
|
||||
analytic_expansion_ratio: 2.0
|
||||
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: 0.0
|
||||
cost_penalty: 5.0
|
||||
retrospective_penalty: 0.015
|
||||
lookup_table_size: 5.0
|
||||
cache_obstacle_heuristic: false
|
||||
viz_expansions: false
|
||||
smooth_path: True
|
||||
|
||||
smoother:
|
||||
max_iterations: 1000
|
||||
w_smooth: 0.4
|
||||
w_data: 0.2
|
||||
tolerance: 1.0e-10
|
||||
do_refinement: true
|
||||
refinement_num: 4
|
||||
|
||||
planner_server_rclcpp_node:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
|
||||
smoother_server:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
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: 2.0
|
||||
use_sim_time: False
|
||||
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: False
|
||||
|
||||
waypoint_follower:
|
||||
ros__parameters:
|
||||
loop_rate: 20
|
||||
use_sim_time: False
|
||||
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
|
||||
@@ -20,7 +20,7 @@ ekf_filter_node:
|
||||
odom0_config: [true, false, false,
|
||||
false, false, false,
|
||||
true, true, false,
|
||||
false, false, true,
|
||||
false, false, false,
|
||||
false, false, false]
|
||||
odom0_queue_size: 10
|
||||
odom0_nodelay: false
|
||||
|
||||
48
src/origincar_base/config/ekf.yaml.bak
Normal file
48
src/origincar_base/config/ekf.yaml.bak
Normal file
@@ -0,0 +1,48 @@
|
||||
### ekf config file ###
|
||||
ekf_filter_node:
|
||||
ros__parameters:
|
||||
frequency: 20.0
|
||||
sensor_timeout: 2.0
|
||||
two_d_mode: true
|
||||
transform_time_offset: 0.0
|
||||
transform_timeout: 0.2
|
||||
print_diagnostics: false
|
||||
debug: false
|
||||
publish_tf: true
|
||||
publish_acceleration: false
|
||||
|
||||
map_frame: map
|
||||
odom_frame: odom
|
||||
base_link_frame: base_footprint
|
||||
world_frame: odom
|
||||
|
||||
odom0: odom
|
||||
odom0_config: [true, false, false,
|
||||
false, false, false,
|
||||
true, true, false,
|
||||
false, false, true,
|
||||
false, false, false]
|
||||
odom0_queue_size: 10
|
||||
odom0_nodelay: false
|
||||
odom0_differential: true
|
||||
odom0_relative: false
|
||||
|
||||
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_remove_gravitational_acceleration: true
|
||||
|
||||
use_control: false
|
||||
stamped_control: false
|
||||
control_timeout: 0.2
|
||||
control_config: [true, false, false, false, false, true]
|
||||
acceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 3.4]
|
||||
deceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 4.5]
|
||||
acceleration_gains: [0.8, 0.0, 0.0, 0.0, 0.0, 0.9]
|
||||
@@ -1,51 +1,24 @@
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch.conditions import IfCondition, UnlessCondition
|
||||
import launch_ros.actions
|
||||
|
||||
def generate_launch_description():
|
||||
akmcar = LaunchConfiguration('akmcar', default='false')
|
||||
|
||||
robot_parameters = [
|
||||
{'usart_port_name': '/dev/ttyACM0',
|
||||
'serial_baud_rate': 115200,
|
||||
'robot_frame_id': 'base_link',
|
||||
'odom_frame_id': 'odom',
|
||||
'cmd_vel': 'cmd_vel',
|
||||
'akm_cmd_vel': 'none',
|
||||
'product_number': 0,
|
||||
# Odom covariance (higher = more uncertainty, EKF trusts odom less)
|
||||
'odom_pose_cov_x': 0.01,
|
||||
'odom_pose_cov_y': 0.01,
|
||||
'odom_pose_cov_yaw': 0.0225}
|
||||
]
|
||||
|
||||
return LaunchDescription([
|
||||
DeclareLaunchArgument(
|
||||
'akmcar',
|
||||
default_value='false',
|
||||
description='Use simulation (Gazebo) clock if true'
|
||||
),
|
||||
|
||||
launch_ros.actions.Node(
|
||||
condition=IfCondition(akmcar),
|
||||
package='origincar_base',
|
||||
executable='origincar_base_node',
|
||||
parameters=robot_parameters + [{'akm_cmd_vel': 'ackermann_cmd'}],
|
||||
remappings=[('/cmd_vel', 'cmd_vel')],
|
||||
),
|
||||
|
||||
launch_ros.actions.Node(
|
||||
condition=IfCondition(akmcar),
|
||||
package='origincar_base',
|
||||
executable='cmd_vel_to_ackermann_drive.py',
|
||||
name='cmd_vel_to_ackermann_drive',
|
||||
),
|
||||
|
||||
launch_ros.actions.Node(
|
||||
condition=UnlessCondition(akmcar),
|
||||
package='origincar_base',
|
||||
executable='origincar_base_node',
|
||||
parameters=robot_parameters + [{'akm_cmd_vel': 'none'}],
|
||||
parameters=robot_parameters,
|
||||
)
|
||||
])
|
||||
|
||||
51
src/origincar_base/launch/base_serial.launch.py.bak
Normal file
51
src/origincar_base/launch/base_serial.launch.py.bak
Normal file
@@ -0,0 +1,51 @@
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch.conditions import IfCondition, UnlessCondition
|
||||
import launch_ros.actions
|
||||
|
||||
def generate_launch_description():
|
||||
akmcar = LaunchConfiguration('akmcar', default='false')
|
||||
|
||||
robot_parameters = [
|
||||
{'usart_port_name': '/dev/ttyACM0',
|
||||
'serial_baud_rate': 115200,
|
||||
'robot_frame_id': 'base_link',
|
||||
'odom_frame_id': 'odom',
|
||||
'cmd_vel': 'cmd_vel',
|
||||
'product_number': 0,
|
||||
# Odom covariance (higher = more uncertainty, EKF trusts odom less)
|
||||
'odom_pose_cov_x': 0.01,
|
||||
'odom_pose_cov_y': 0.01,
|
||||
'odom_pose_cov_yaw': 0.0225}
|
||||
]
|
||||
|
||||
return LaunchDescription([
|
||||
DeclareLaunchArgument(
|
||||
'akmcar',
|
||||
default_value='false',
|
||||
description='Use simulation (Gazebo) clock if true'
|
||||
),
|
||||
|
||||
launch_ros.actions.Node(
|
||||
condition=IfCondition(akmcar),
|
||||
package='origincar_base',
|
||||
executable='origincar_base_node',
|
||||
parameters=robot_parameters + [{'akm_cmd_vel': 'ackermann_cmd'}],
|
||||
remappings=[('/cmd_vel', 'cmd_vel')],
|
||||
),
|
||||
|
||||
launch_ros.actions.Node(
|
||||
condition=IfCondition(akmcar),
|
||||
package='origincar_base',
|
||||
executable='cmd_vel_to_ackermann_drive.py',
|
||||
name='cmd_vel_to_ackermann_drive',
|
||||
),
|
||||
|
||||
launch_ros.actions.Node(
|
||||
condition=UnlessCondition(akmcar),
|
||||
package='origincar_base',
|
||||
executable='origincar_base_node',
|
||||
parameters=robot_parameters + [{'akm_cmd_vel': 'none'}],
|
||||
)
|
||||
])
|
||||
@@ -23,7 +23,7 @@ def generate_launch_description():
|
||||
carto_slam = LaunchConfiguration('carto_slam', default='false')
|
||||
carto_slam_dec = DeclareLaunchArgument('carto_slam',default_value='false')
|
||||
|
||||
akmcar = LaunchConfiguration('akmcar', default='true')
|
||||
akmcar = LaunchConfiguration('akmcar', default='false')
|
||||
akmcar_dec = DeclareLaunchArgument('akmcar', default_value='true',
|
||||
description='阿克曼底盘模式 (true=阿克曼, false=差速)')
|
||||
|
||||
|
||||
85
src/past_control/CMakeLists.txt
Normal file
85
src/past_control/CMakeLists.txt
Normal file
@@ -0,0 +1,85 @@
|
||||
cmake_minimum_required(VERSION 3.5)
|
||||
project(past_control)
|
||||
|
||||
if(NOT CMAKE_C_STANDARD)
|
||||
set(CMAKE_C_STANDARD 99)
|
||||
endif()
|
||||
if(NOT CMAKE_CXX_STANDARD)
|
||||
set(CMAKE_CXX_STANDARD 14)
|
||||
endif()
|
||||
|
||||
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
||||
add_compile_options(-Wall -Wextra -Wpedantic)
|
||||
endif()
|
||||
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(rclcpp REQUIRED)
|
||||
find_package(rclcpp_action REQUIRED)
|
||||
find_package(std_msgs REQUIRED)
|
||||
find_package(nav_msgs REQUIRED)
|
||||
find_package(nav2_msgs REQUIRED)
|
||||
find_package(geometry_msgs REQUIRED)
|
||||
find_package(sensor_msgs REQUIRED)
|
||||
find_package(tf2 REQUIRED)
|
||||
find_package(tf2_ros REQUIRED)
|
||||
find_package(visualization_msgs REQUIRED)
|
||||
find_package(origincar_msg REQUIRED)
|
||||
find_package(rosidl_default_generators REQUIRED)
|
||||
find_package(builtin_interfaces REQUIRED)
|
||||
|
||||
# generate custom messages
|
||||
rosidl_generate_interfaces(${PROJECT_NAME}
|
||||
"msg/Obstacle.msg"
|
||||
"msg/ObstacleArray.msg"
|
||||
DEPENDENCIES std_msgs
|
||||
ADD_LINTER_TESTS
|
||||
)
|
||||
|
||||
include_directories(include)
|
||||
|
||||
# lane_follower_node
|
||||
add_executable(lane_follower_node
|
||||
src/lane_follower_node.cpp
|
||||
)
|
||||
ament_target_dependencies(lane_follower_node
|
||||
rclcpp std_msgs geometry_msgs
|
||||
)
|
||||
|
||||
# obstacle_detector_node
|
||||
add_executable(obstacle_detector_node
|
||||
src/obstacle_detector_node.cpp
|
||||
)
|
||||
ament_target_dependencies(obstacle_detector_node
|
||||
rclcpp std_msgs geometry_msgs nav_msgs tf2 tf2_ros visualization_msgs
|
||||
)
|
||||
rosidl_get_typesupport_target(cpp_typesupport_target ${PROJECT_NAME} rosidl_typesupport_cpp)
|
||||
target_link_libraries(obstacle_detector_node ${cpp_typesupport_target})
|
||||
|
||||
# racing_orchestrator — subscribes to /plan (Nav2 planner_server) + PID following
|
||||
add_executable(racing_orchestrator
|
||||
src/racing_orchestrator.cpp
|
||||
)
|
||||
ament_target_dependencies(racing_orchestrator
|
||||
rclcpp rclcpp_action std_msgs nav_msgs nav2_msgs geometry_msgs tf2 tf2_ros origincar_msg visualization_msgs
|
||||
)
|
||||
|
||||
install(TARGETS
|
||||
lane_follower_node
|
||||
obstacle_detector_node
|
||||
racing_orchestrator
|
||||
DESTINATION lib/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
install(
|
||||
DIRECTORY launch config
|
||||
DESTINATION share/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
if(BUILD_TESTING)
|
||||
find_package(ament_lint_auto REQUIRED)
|
||||
set(ament_cmake_copyright_FOUND TRUE)
|
||||
set(ament_cmake_cpplint_FOUND TRUE)
|
||||
ament_lint_auto_find_test_dependencies()
|
||||
endif()
|
||||
|
||||
ament_package()
|
||||
51
src/past_control/config/past_control.yaml
Normal file
51
src/past_control/config/past_control.yaml
Normal file
@@ -0,0 +1,51 @@
|
||||
# past_control — Racing control stack configuration
|
||||
# No a_star_planner_node: global planning handled by Nav2 planner_server via /plan
|
||||
|
||||
# ============================================================
|
||||
# lane_follower_node — Visual lane centering
|
||||
# ============================================================
|
||||
lane_follower_node:
|
||||
ros__parameters:
|
||||
follow_linear_speed: 0.3
|
||||
follow_angular_ratio: 1.0
|
||||
image_width: 640.0
|
||||
|
||||
# ============================================================
|
||||
# obstacle_detector_node — DNN-based obstacle perception
|
||||
# ============================================================
|
||||
obstacle_detector_node:
|
||||
ros__parameters:
|
||||
confidence_threshold: 0.5
|
||||
processing_latency: 0.1
|
||||
camera_hfov: 1.0472 # 60 degrees
|
||||
camera_height: 0.3
|
||||
camera_pitch: 0.0
|
||||
max_detection_range: 3.0
|
||||
publish_rate: 10.0
|
||||
|
||||
# ============================================================
|
||||
# racing_orchestrator — Task FSM + PID path following (Nav2 /plan) + arbitration
|
||||
# ============================================================
|
||||
racing_orchestrator:
|
||||
ros__parameters:
|
||||
# PID path following (path source: Nav2 planner_server /plan)
|
||||
follow_linear_speed: 0.4
|
||||
guide_step: 5
|
||||
arrive_square: 0.25 # distance^2 threshold (0.5m)
|
||||
angular_kp: 10.0
|
||||
angular_ki: 0.0
|
||||
angular_kd: 0.1
|
||||
angular_integral_max: 1.0
|
||||
angular_output_max: 2.0
|
||||
angular_max_err: 3.14
|
||||
|
||||
# Obstacle avoidance fallback
|
||||
avoid_linear_speed: 0.2
|
||||
avoid_angular_z: 0.8
|
||||
|
||||
# QR scan mode
|
||||
qr_scan_speed: 0.15
|
||||
qr_scan_duration: 3.0
|
||||
|
||||
# Control loop rate
|
||||
control_rate: 20.0
|
||||
87
src/past_control/include/past_control/tools.h
Normal file
87
src/past_control/include/past_control/tools.h
Normal file
@@ -0,0 +1,87 @@
|
||||
#ifndef PAST_CONTROL__TOOLS_H
|
||||
#define PAST_CONTROL__TOOLS_H
|
||||
|
||||
#include <algorithm>
|
||||
#include <chrono>
|
||||
|
||||
namespace past_control
|
||||
{
|
||||
|
||||
inline double limit(double val, double min_val, double max_val)
|
||||
{
|
||||
return std::min(std::max(val, min_val), max_val);
|
||||
}
|
||||
|
||||
class PID
|
||||
{
|
||||
public:
|
||||
PID()
|
||||
: kp_(0.0), ki_(0.0), kd_(0.0),
|
||||
integral_max_(0.0), output_max_(0.0),
|
||||
integral_(0.0), prev_error_(0.0), first_run_(true),
|
||||
max_err_(0.0)
|
||||
{
|
||||
}
|
||||
|
||||
void init(double kp, double ki, double kd,
|
||||
double integral_max, double output_max, double max_err = 0.0)
|
||||
{
|
||||
kp_ = kp;
|
||||
ki_ = ki;
|
||||
kd_ = kd;
|
||||
integral_max_ = integral_max;
|
||||
output_max_ = output_max;
|
||||
max_err_ = max_err;
|
||||
reset();
|
||||
}
|
||||
|
||||
void reset()
|
||||
{
|
||||
integral_ = 0.0;
|
||||
prev_error_ = 0.0;
|
||||
first_run_ = true;
|
||||
}
|
||||
|
||||
double update(double error, double dt = 0.02)
|
||||
{
|
||||
// Clamp error if max_err_ > 0
|
||||
if (max_err_ > 0.0) {
|
||||
error = limit(error, -max_err_, max_err_);
|
||||
}
|
||||
|
||||
// Proportional
|
||||
double p_out = kp_ * error;
|
||||
|
||||
// Integral (with clamping)
|
||||
integral_ += error * dt;
|
||||
integral_ = limit(integral_, -integral_max_, integral_max_);
|
||||
double i_out = ki_ * integral_;
|
||||
|
||||
// Derivative (skip on first call)
|
||||
double d_out = 0.0;
|
||||
if (!first_run_) {
|
||||
double derivative = (error - prev_error_) / dt;
|
||||
d_out = kd_ * derivative;
|
||||
}
|
||||
first_run_ = false;
|
||||
prev_error_ = error;
|
||||
|
||||
// Output clamping
|
||||
double output = p_out + i_out + d_out;
|
||||
output = limit(output, -output_max_, output_max_);
|
||||
|
||||
return output;
|
||||
}
|
||||
|
||||
private:
|
||||
double kp_, ki_, kd_;
|
||||
double integral_max_, output_max_;
|
||||
double integral_;
|
||||
double prev_error_;
|
||||
bool first_run_;
|
||||
double max_err_; // initialized to 0 in constructor
|
||||
};
|
||||
|
||||
} // namespace past_control
|
||||
|
||||
#endif // PAST_CONTROL__TOOLS_H
|
||||
52
src/past_control/launch/past_control.launch.py
Normal file
52
src/past_control/launch/past_control.launch.py
Normal file
@@ -0,0 +1,52 @@
|
||||
"""
|
||||
Launch file for past_control — racing control stack.
|
||||
|
||||
Launches 3 nodes (a_star_planner removed — Nav2 planner_server handles global planning):
|
||||
1. lane_follower_node — visual lane centering
|
||||
2. obstacle_detector_node — DNN obstacle perception
|
||||
3. racing_orchestrator — task FSM + PID following (/plan from Nav2) + cmd_vel arbitration
|
||||
|
||||
Usage:
|
||||
ros2 launch past_control past_control.launch.py
|
||||
"""
|
||||
|
||||
import os
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
from launch import LaunchDescription
|
||||
from launch_ros.actions import Node
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
|
||||
pkg_dir = get_package_share_directory("past_control")
|
||||
config_path = os.path.join(pkg_dir, "config", "past_control.yaml")
|
||||
|
||||
lane_follower = Node(
|
||||
package="past_control",
|
||||
executable="lane_follower_node",
|
||||
name="lane_follower_node",
|
||||
output="screen",
|
||||
parameters=[config_path],
|
||||
)
|
||||
|
||||
obstacle_detector = Node(
|
||||
package="past_control",
|
||||
executable="obstacle_detector_node",
|
||||
name="obstacle_detector_node",
|
||||
output="screen",
|
||||
parameters=[config_path],
|
||||
)
|
||||
|
||||
racing_orchestrator = Node(
|
||||
package="past_control",
|
||||
executable="racing_orchestrator",
|
||||
name="racing_orchestrator",
|
||||
output="screen",
|
||||
parameters=[config_path],
|
||||
)
|
||||
|
||||
return LaunchDescription([
|
||||
lane_follower,
|
||||
obstacle_detector,
|
||||
racing_orchestrator,
|
||||
])
|
||||
5
src/past_control/msg/Obstacle.msg
Normal file
5
src/past_control/msg/Obstacle.msg
Normal file
@@ -0,0 +1,5 @@
|
||||
std_msgs/Header header
|
||||
float64 x
|
||||
float64 y
|
||||
float64 radius
|
||||
string type
|
||||
1
src/past_control/msg/ObstacleArray.msg
Normal file
1
src/past_control/msg/ObstacleArray.msg
Normal file
@@ -0,0 +1 @@
|
||||
Obstacle[] obstacles
|
||||
36
src/past_control/package.xml
Normal file
36
src/past_control/package.xml
Normal file
@@ -0,0 +1,36 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>past_control</name>
|
||||
<version>0.0.0</version>
|
||||
<description>Racing control stack: lane follower, obstacle detector, and racing orchestrator (Nav2 /plan subscriber + PID following)</description>
|
||||
<maintainer email="2314753575@qq.com">sunrise</maintainer>
|
||||
<license>TODO: License declaration</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<buildtool_depend>rosidl_default_generators</buildtool_depend>
|
||||
<member_of_group>rosidl_interface_packages</member_of_group>
|
||||
|
||||
<depend>rclcpp</depend>
|
||||
<depend>rclcpp_action</depend>
|
||||
<depend>std_msgs</depend>
|
||||
<depend>nav_msgs</depend>
|
||||
<depend>nav2_msgs</depend>
|
||||
<depend>geometry_msgs</depend>
|
||||
<depend>sensor_msgs</depend>
|
||||
<depend>tf2</depend>
|
||||
<depend>tf2_ros</depend>
|
||||
<depend>visualization_msgs</depend>
|
||||
<depend>origincar_msg</depend>
|
||||
<depend>builtin_interfaces</depend>
|
||||
|
||||
<exec_depend>rosidl_default_runtime</exec_depend>
|
||||
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
81
src/past_control/src/lane_follower_node.cpp
Normal file
81
src/past_control/src/lane_follower_node.cpp
Normal file
@@ -0,0 +1,81 @@
|
||||
#include <memory>
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "geometry_msgs/msg/twist.hpp"
|
||||
#include "std_msgs/msg/float32_multi_array.hpp"
|
||||
#include "std_msgs/msg/float32_multi_array.hpp"
|
||||
|
||||
class LaneFollowerNode : public rclcpp::Node
|
||||
{
|
||||
public:
|
||||
LaneFollowerNode()
|
||||
: Node("lane_follower_node")
|
||||
{
|
||||
this->declare_parameter("follow_linear_speed", 0.3);
|
||||
this->declare_parameter("follow_angular_ratio", 1.0);
|
||||
this->declare_parameter("image_width", 640.0);
|
||||
|
||||
follow_linear_speed_ = this->get_parameter("follow_linear_speed").as_double();
|
||||
follow_angular_ratio_ = this->get_parameter("follow_angular_ratio").as_double();
|
||||
image_width_ = this->get_parameter("image_width").as_double();
|
||||
|
||||
// Subscriber: racing track center detection
|
||||
// Expects Float32MultiArray with [center_x] in image coordinates,
|
||||
// or a custom format. We assume center_x relative to image center.
|
||||
track_sub_ = this->create_subscription<std_msgs::msg::Float32MultiArray>(
|
||||
"racing_track_center_detection", 10,
|
||||
std::bind(&LaneFollowerNode::trackCallback, this, std::placeholders::_1));
|
||||
|
||||
// Publisher: lane cmd_vel (NOT /cmd_vel directly)
|
||||
cmd_vel_pub_ = this->create_publisher<geometry_msgs::msg::Twist>(
|
||||
"/lane_cmd_vel", 10);
|
||||
|
||||
RCLCPP_INFO(this->get_logger(),
|
||||
"LaneFollowerNode started. linear=%.2f, angular_ratio=%.2f",
|
||||
follow_linear_speed_, follow_angular_ratio_);
|
||||
}
|
||||
|
||||
private:
|
||||
void trackCallback(const std_msgs::msg::Float32MultiArray::SharedPtr msg)
|
||||
{
|
||||
if (msg->data.empty()) {
|
||||
// No track detected — stop
|
||||
geometry_msgs::msg::Twist cmd;
|
||||
cmd.linear.x = 0.0;
|
||||
cmd.angular.z = 0.0;
|
||||
cmd_vel_pub_->publish(cmd);
|
||||
return;
|
||||
}
|
||||
|
||||
// center_x from detection: pixel offset from image center
|
||||
// range: [-image_width/2, image_width/2]
|
||||
double center_offset = msg->data[0]; // in pixels
|
||||
|
||||
// Normalize to [-1, 1]
|
||||
double normalized_error = center_offset / (image_width_ / 2.0);
|
||||
normalized_error = std::max(-1.0, std::min(1.0, normalized_error));
|
||||
|
||||
// Angular velocity: steer toward center line
|
||||
// Positive error = line is to the right → positive angular to turn right
|
||||
double angular_z = follow_angular_ratio_ * normalized_error;
|
||||
|
||||
geometry_msgs::msg::Twist cmd;
|
||||
cmd.linear.x = follow_linear_speed_;
|
||||
cmd.angular.z = angular_z;
|
||||
cmd_vel_pub_->publish(cmd);
|
||||
}
|
||||
|
||||
rclcpp::Subscription<std_msgs::msg::Float32MultiArray>::SharedPtr track_sub_;
|
||||
rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_pub_;
|
||||
|
||||
double follow_linear_speed_;
|
||||
double follow_angular_ratio_;
|
||||
double image_width_;
|
||||
};
|
||||
|
||||
int main(int argc, char* argv[])
|
||||
{
|
||||
rclcpp::init(argc, argv);
|
||||
rclcpp::spin(std::make_shared<LaneFollowerNode>());
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
222
src/past_control/src/obstacle_detector_node.cpp
Normal file
222
src/past_control/src/obstacle_detector_node.cpp
Normal file
@@ -0,0 +1,222 @@
|
||||
#include <memory>
|
||||
#include <mutex>
|
||||
#include <cmath>
|
||||
#include <vector>
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "std_msgs/msg/float32_multi_array.hpp"
|
||||
#include "nav_msgs/msg/odometry.hpp"
|
||||
#include "visualization_msgs/msg/marker_array.hpp"
|
||||
#include "geometry_msgs/msg/point.hpp"
|
||||
#include "std_msgs/msg/header.hpp"
|
||||
#include "tf2/LinearMath/Quaternion.h"
|
||||
#include "tf2/LinearMath/Matrix3x3.h"
|
||||
|
||||
#include "past_control/msg/obstacle_array.hpp"
|
||||
#include "past_control/msg/obstacle.hpp"
|
||||
|
||||
// DNN detection format (per the existing hobot_dnn convention)
|
||||
// Typically a custom array of detections: [class_id, x1, y1, x2, y2, confidence, ...]
|
||||
// We'll use Float32MultiArray for input
|
||||
|
||||
class ObstacleDetectorNode : public rclcpp::Node
|
||||
{
|
||||
public:
|
||||
ObstacleDetectorNode()
|
||||
: Node("obstacle_detector_node"), has_odom_(false)
|
||||
{
|
||||
this->declare_parameter("confidence_threshold", 0.5);
|
||||
this->declare_parameter("processing_latency", 0.1); // seconds
|
||||
this->declare_parameter("camera_hfov", 1.0472); // 60 degrees
|
||||
this->declare_parameter("camera_height", 0.3); // meters above ground
|
||||
this->declare_parameter("camera_pitch", 0.0); // radians
|
||||
this->declare_parameter("max_detection_range", 3.0); // meters
|
||||
this->declare_parameter("publish_rate", 10.0);
|
||||
|
||||
confidence_threshold_ = this->get_parameter("confidence_threshold").as_double();
|
||||
processing_latency_ = this->get_parameter("processing_latency").as_double();
|
||||
camera_hfov_ = this->get_parameter("camera_hfov").as_double();
|
||||
camera_height_ = this->get_parameter("camera_height").as_double();
|
||||
camera_pitch_ = this->get_parameter("camera_pitch").as_double();
|
||||
max_detection_range_ = this->get_parameter("max_detection_range").as_double();
|
||||
|
||||
// Subscribers
|
||||
dnn_sub_ = this->create_subscription<std_msgs::msg::Float32MultiArray>(
|
||||
"hobot_dnn_detection", 10,
|
||||
std::bind(&ObstacleDetectorNode::dnnCallback, this, std::placeholders::_1));
|
||||
|
||||
odom_sub_ = this->create_subscription<nav_msgs::msg::Odometry>(
|
||||
"/odom", 10,
|
||||
std::bind(&ObstacleDetectorNode::odomCallback, this, std::placeholders::_1));
|
||||
|
||||
// Publishers
|
||||
obstacles_pub_ = this->create_publisher<past_control::msg::ObstacleArray>(
|
||||
"/obstacles", 10);
|
||||
|
||||
marker_pub_ = this->create_publisher<visualization_msgs::msg::MarkerArray>(
|
||||
"/obstacle_markers", 10);
|
||||
|
||||
double rate = this->get_parameter("publish_rate").as_double();
|
||||
int period_ms = static_cast<int>(1000.0 / rate);
|
||||
timer_ = this->create_wall_timer(
|
||||
std::chrono::milliseconds(period_ms),
|
||||
std::bind(&ObstacleDetectorNode::publishLoop, this));
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "ObstacleDetectorNode started");
|
||||
}
|
||||
|
||||
private:
|
||||
void dnnCallback(const std_msgs::msg::Float32MultiArray::SharedPtr msg)
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
|
||||
// Parse DNN detections
|
||||
// Expected format: interleaved [class_id, x_center, y_center, width, height, conf]
|
||||
// Normalized coordinates (0-1) within image frame
|
||||
const int fields_per_detection = 6;
|
||||
int num_detections = msg->data.size() / fields_per_detection;
|
||||
|
||||
latest_detections_.clear();
|
||||
for (int i = 0; i < num_detections; ++i) {
|
||||
int base = i * fields_per_detection;
|
||||
Detection det;
|
||||
det.class_id = static_cast<int>(msg->data[base + 0]);
|
||||
det.x_center = msg->data[base + 1]; // normalized [0,1]
|
||||
det.y_center = msg->data[base + 2]; // normalized [0,1]
|
||||
det.width = msg->data[base + 3]; // normalized
|
||||
det.height = msg->data[base + 4]; // normalized
|
||||
det.confidence = msg->data[base + 5];
|
||||
|
||||
if (det.confidence >= confidence_threshold_) {
|
||||
latest_detections_.push_back(det);
|
||||
}
|
||||
}
|
||||
|
||||
has_detections_ = true;
|
||||
}
|
||||
|
||||
void odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg)
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
cur_x_ = msg->pose.pose.position.x;
|
||||
cur_y_ = msg->pose.pose.position.y;
|
||||
|
||||
// Extract yaw from quaternion
|
||||
double qx = msg->pose.pose.orientation.x;
|
||||
double qy = msg->pose.pose.orientation.y;
|
||||
double qz = msg->pose.pose.orientation.z;
|
||||
double qw = msg->pose.pose.orientation.w;
|
||||
cur_yaw_ = std::atan2(2.0*(qw*qz + qx*qy), 1.0 - 2.0*(qy*qy + qz*qz));
|
||||
has_odom_ = true;
|
||||
}
|
||||
|
||||
void publishLoop()
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
|
||||
past_control::msg::ObstacleArray obs_array;
|
||||
visualization_msgs::msg::MarkerArray marker_array;
|
||||
|
||||
if (!has_odom_ || !has_detections_) {
|
||||
// Publish empty
|
||||
obstacles_pub_->publish(obs_array);
|
||||
marker_pub_->publish(marker_array);
|
||||
return;
|
||||
}
|
||||
|
||||
int marker_id = 0;
|
||||
for (const auto& det : latest_detections_) {
|
||||
// Convert pixel coords to world coords using simple pinhole model
|
||||
// x_center normalized [0,1] → angle offset from camera optical axis
|
||||
double pixel_offset = det.x_center - 0.5; // [-0.5, 0.5]
|
||||
double angle_offset = pixel_offset * camera_hfov_; // radians
|
||||
|
||||
double obstacle_angle = cur_yaw_ + angle_offset;
|
||||
|
||||
// Estimate distance from bounding box height
|
||||
// Larger bbox = closer object (simple inverse relationship)
|
||||
double est_distance = (1.0 - det.height) * max_detection_range_ + 0.5;
|
||||
|
||||
// World coordinates of obstacle
|
||||
double obs_x = cur_x_ + est_distance * std::cos(obstacle_angle);
|
||||
double obs_y = cur_y_ + est_distance * std::sin(obstacle_angle);
|
||||
double obs_radius = 0.15; // default obstacle radius
|
||||
|
||||
// Add to obstacle array
|
||||
past_control::msg::Obstacle obs;
|
||||
obs.header.stamp = this->now();
|
||||
obs.header.frame_id = "odom";
|
||||
obs.x = obs_x;
|
||||
obs.y = obs_y;
|
||||
obs.radius = obs_radius;
|
||||
obs.type = "circle";
|
||||
obs_array.obstacles.push_back(obs);
|
||||
|
||||
// Add marker
|
||||
visualization_msgs::msg::Marker marker;
|
||||
marker.header.stamp = this->now();
|
||||
marker.header.frame_id = "odom";
|
||||
marker.ns = "obstacles";
|
||||
marker.id = marker_id++;
|
||||
marker.type = visualization_msgs::msg::Marker::CYLINDER;
|
||||
marker.action = visualization_msgs::msg::Marker::ADD;
|
||||
marker.pose.position.x = obs_x;
|
||||
marker.pose.position.y = obs_y;
|
||||
marker.pose.position.z = 0.0;
|
||||
marker.pose.orientation.w = 1.0;
|
||||
marker.scale.x = obs_radius * 2;
|
||||
marker.scale.y = obs_radius * 2;
|
||||
marker.scale.z = 0.3;
|
||||
marker.color.r = 1.0f;
|
||||
marker.color.g = 0.0f;
|
||||
marker.color.b = 0.0f;
|
||||
marker.color.a = 0.8f;
|
||||
marker.lifetime = rclcpp::Duration::from_seconds(0.5);
|
||||
marker_array.markers.push_back(marker);
|
||||
}
|
||||
|
||||
obstacles_pub_->publish(obs_array);
|
||||
marker_pub_->publish(marker_array);
|
||||
}
|
||||
|
||||
struct Detection
|
||||
{
|
||||
int class_id;
|
||||
double x_center, y_center;
|
||||
double width, height;
|
||||
double confidence;
|
||||
};
|
||||
|
||||
// Subscribers
|
||||
rclcpp::Subscription<std_msgs::msg::Float32MultiArray>::SharedPtr dnn_sub_;
|
||||
rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
|
||||
|
||||
// Publishers
|
||||
rclcpp::Publisher<past_control::msg::ObstacleArray>::SharedPtr obstacles_pub_;
|
||||
rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr marker_pub_;
|
||||
|
||||
// Timer
|
||||
rclcpp::TimerBase::SharedPtr timer_;
|
||||
|
||||
// State
|
||||
std::vector<Detection> latest_detections_;
|
||||
double cur_x_, cur_y_, cur_yaw_;
|
||||
bool has_odom_, has_detections_;
|
||||
std::mutex mutex_;
|
||||
|
||||
// Parameters
|
||||
double confidence_threshold_;
|
||||
double processing_latency_;
|
||||
double camera_hfov_;
|
||||
double camera_height_;
|
||||
double camera_pitch_;
|
||||
double max_detection_range_;
|
||||
};
|
||||
|
||||
int main(int argc, char* argv[])
|
||||
{
|
||||
rclcpp::init(argc, argv);
|
||||
rclcpp::spin(std::make_shared<ObstacleDetectorNode>());
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
448
src/past_control/src/racing_orchestrator.cpp
Normal file
448
src/past_control/src/racing_orchestrator.cpp
Normal file
@@ -0,0 +1,448 @@
|
||||
#include <chrono>
|
||||
#include <memory>
|
||||
#include <mutex>
|
||||
#include <cmath>
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "rclcpp_action/rclcpp_action.hpp"
|
||||
#include "nav_msgs/msg/path.hpp"
|
||||
#include "nav_msgs/msg/odometry.hpp"
|
||||
#include "geometry_msgs/msg/twist.hpp"
|
||||
#include "geometry_msgs/msg/pose_stamped.hpp"
|
||||
#include "nav2_msgs/action/compute_path_to_pose.hpp"
|
||||
#include "origincar_msg/msg/sign.hpp"
|
||||
#include "tf2/LinearMath/Quaternion.h"
|
||||
#include "tf2/LinearMath/Matrix3x3.h"
|
||||
|
||||
#include "past_control/tools.h"
|
||||
|
||||
using namespace std::chrono_literals;
|
||||
|
||||
// Task states
|
||||
enum class TaskState
|
||||
{
|
||||
IDLE = 0,
|
||||
GOING = 1,
|
||||
RETURN = 2,
|
||||
QR_SCAN = 3,
|
||||
RESET = 4
|
||||
};
|
||||
|
||||
class RacingOrchestrator : public rclcpp::Node
|
||||
{
|
||||
public:
|
||||
using ComputePathToPose = nav2_msgs::action::ComputePathToPose;
|
||||
using GoalHandleComputePathToPose = rclcpp_action::ClientGoalHandle<ComputePathToPose>;
|
||||
|
||||
RacingOrchestrator()
|
||||
: Node("racing_orchestrator"),
|
||||
state_(TaskState::IDLE),
|
||||
has_odom_(false), has_goal_(false),
|
||||
has_path_(false), got_lane_cmd_(false),
|
||||
is_back_(false), activate_avoid_(false),
|
||||
waypoint_index_(0), guide_step_(5),
|
||||
follow_linear_speed_(0.4),
|
||||
sub_target_(true),
|
||||
cmd_vel_linear_x_(0.0), cmd_vel_angular_z_(0.0),
|
||||
cur_x_(0.0), cur_y_(0.0), cur_yaw_(0.0)
|
||||
{
|
||||
// Parameters
|
||||
// PID path following
|
||||
this->declare_parameter("follow_linear_speed", 0.4);
|
||||
this->declare_parameter("guide_step", 5);
|
||||
this->declare_parameter("arrive_square", 0.25);
|
||||
this->declare_parameter("angular_kp", 1.5);
|
||||
this->declare_parameter("angular_ki", 0.0);
|
||||
this->declare_parameter("angular_kd", 0.1);
|
||||
this->declare_parameter("angular_integral_max", 1.0);
|
||||
this->declare_parameter("angular_output_max", 2.0);
|
||||
this->declare_parameter("angular_max_err", 3.14);
|
||||
|
||||
// Obstacle avoidance
|
||||
this->declare_parameter("avoid_linear_speed", 0.2);
|
||||
this->declare_parameter("avoid_angular_z", 0.8);
|
||||
|
||||
// QR scan params
|
||||
this->declare_parameter("qr_scan_speed", 0.15);
|
||||
this->declare_parameter("qr_scan_duration", 3.0);
|
||||
|
||||
// Control rate
|
||||
this->declare_parameter("control_rate", 20.0);
|
||||
|
||||
// Load params
|
||||
follow_linear_speed_ = this->get_parameter("follow_linear_speed").as_double();
|
||||
guide_step_ = this->get_parameter("guide_step").as_int();
|
||||
arrive_square_ = this->get_parameter("arrive_square").as_double();
|
||||
avoid_linear_speed_ = this->get_parameter("avoid_linear_speed").as_double();
|
||||
avoid_angular_z_ = this->get_parameter("avoid_angular_z").as_double();
|
||||
qr_scan_speed_ = this->get_parameter("qr_scan_speed").as_double();
|
||||
qr_scan_duration_ = this->get_parameter("qr_scan_duration").as_double();
|
||||
|
||||
// Init PID
|
||||
double kp = this->get_parameter("angular_kp").as_double();
|
||||
double ki = this->get_parameter("angular_ki").as_double();
|
||||
double kd = this->get_parameter("angular_kd").as_double();
|
||||
double i_max = this->get_parameter("angular_integral_max").as_double();
|
||||
double o_max = this->get_parameter("angular_output_max").as_double();
|
||||
double max_err = this->get_parameter("angular_max_err").as_double();
|
||||
|
||||
angular_pid_.init(kp, ki, kd, i_max, o_max, max_err);
|
||||
|
||||
// Subscribers
|
||||
sign_sub_ = this->create_subscription<origincar_msg::msg::Sign>(
|
||||
"sign4return", 10,
|
||||
std::bind(&RacingOrchestrator::signCallback, this, std::placeholders::_1));
|
||||
|
||||
sign_foxglove_sub_ = this->create_subscription<origincar_msg::msg::Sign>(
|
||||
"sign_foxglove", 10,
|
||||
std::bind(&RacingOrchestrator::signFoxgloveCallback, this, std::placeholders::_1));
|
||||
|
||||
path_sub_ = this->create_subscription<nav_msgs::msg::Path>(
|
||||
"/plan", 10,
|
||||
std::bind(&RacingOrchestrator::pathCallback, this, std::placeholders::_1));
|
||||
|
||||
goal_sub_ = this->create_subscription<geometry_msgs::msg::PoseStamped>(
|
||||
"/goal_pose", 10,
|
||||
std::bind(&RacingOrchestrator::goalCallback, this, std::placeholders::_1));
|
||||
|
||||
odom_sub_ = this->create_subscription<nav_msgs::msg::Odometry>(
|
||||
"/odom", 10,
|
||||
std::bind(&RacingOrchestrator::odomCallback, this, std::placeholders::_1));
|
||||
|
||||
lane_cmd_sub_ = this->create_subscription<geometry_msgs::msg::Twist>(
|
||||
"/lane_cmd_vel", 10,
|
||||
std::bind(&RacingOrchestrator::laneCmdCallback, this, std::placeholders::_1));
|
||||
|
||||
// Publishers
|
||||
cmd_vel_pub_ = this->create_publisher<geometry_msgs::msg::Twist>("/cmd_vel", 10);
|
||||
|
||||
// ComputePathToPose action client (triggers Nav2 planner_server)
|
||||
planner_client_ = rclcpp_action::create_client<ComputePathToPose>(
|
||||
this, "compute_path_to_pose");
|
||||
|
||||
// Control loop
|
||||
double rate = this->get_parameter("control_rate").as_double();
|
||||
int period_ms = static_cast<int>(1000.0 / rate);
|
||||
timer_ = this->create_wall_timer(
|
||||
std::chrono::milliseconds(period_ms),
|
||||
std::bind(&RacingOrchestrator::controlLoop, this));
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "RacingOrchestrator started. State: IDLE");
|
||||
}
|
||||
|
||||
private:
|
||||
// ─────────────────────────────────────────────
|
||||
// A. Task State Machine — sign callbacks
|
||||
// ─────────────────────────────────────────────
|
||||
|
||||
void signCallback(const origincar_msg::msg::Sign::SharedPtr msg)
|
||||
{
|
||||
processSign(msg->sign_data);
|
||||
}
|
||||
|
||||
void signFoxgloveCallback(const origincar_msg::msg::Sign::SharedPtr msg)
|
||||
{
|
||||
processSign(msg->sign_data);
|
||||
}
|
||||
|
||||
void processSign(int sign_data)
|
||||
{
|
||||
RCLCPP_INFO(this->get_logger(), "Received sign: %d", sign_data);
|
||||
|
||||
if (sign_data == -1) {
|
||||
state_ = TaskState::RESET;
|
||||
has_path_ = false;
|
||||
global_path_ = nav_msgs::msg::Path();
|
||||
waypoint_index_ = 0;
|
||||
angular_pid_.reset();
|
||||
RCLCPP_INFO(this->get_logger(), "State -> RESET");
|
||||
}
|
||||
else if (sign_data == 3 || sign_data == 4) {
|
||||
state_ = TaskState::QR_SCAN;
|
||||
qr_scan_start_ = this->now();
|
||||
RCLCPP_INFO(this->get_logger(), "State -> QR_SCAN");
|
||||
}
|
||||
else if (sign_data == 5) {
|
||||
state_ = TaskState::GOING;
|
||||
is_back_ = false;
|
||||
sub_target_ = true;
|
||||
has_path_ = false;
|
||||
waypoint_index_ = 0;
|
||||
angular_pid_.reset();
|
||||
// Trigger Nav2 replanning if we have a goal
|
||||
if (has_goal_) {
|
||||
requestPlan();
|
||||
}
|
||||
RCLCPP_INFO(this->get_logger(), "State -> GOING (forward)");
|
||||
}
|
||||
else if (sign_data == -2) {
|
||||
state_ = TaskState::RETURN;
|
||||
is_back_ = true;
|
||||
has_path_ = false;
|
||||
waypoint_index_ = 0;
|
||||
angular_pid_.reset();
|
||||
if (has_goal_) {
|
||||
requestPlan();
|
||||
}
|
||||
RCLCPP_INFO(this->get_logger(), "State -> RETURN");
|
||||
}
|
||||
}
|
||||
|
||||
// ─────────────────────────────────────────────
|
||||
// B. Callbacks for path, goal, odom, lane cmd_vel
|
||||
// ─────────────────────────────────────────────
|
||||
|
||||
void pathCallback(const nav_msgs::msg::Path::SharedPtr msg)
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
if (msg->poses.empty()) {
|
||||
has_path_ = false;
|
||||
return;
|
||||
}
|
||||
global_path_ = *msg;
|
||||
has_path_ = true;
|
||||
waypoint_index_ = 0;
|
||||
angular_pid_.reset();
|
||||
|
||||
// Auto-transition IDLE -> GOING when receiving an external path
|
||||
// (e.g. publish_sine_path.py publishes /plan without /goal_pose)
|
||||
if (state_ == TaskState::IDLE || state_ == TaskState::RESET) {
|
||||
state_ = TaskState::GOING;
|
||||
is_back_ = false;
|
||||
sub_target_ = true;
|
||||
RCLCPP_INFO(this->get_logger(),
|
||||
"Auto State -> GOING (external path, %zu waypoints)",
|
||||
msg->poses.size());
|
||||
}
|
||||
}
|
||||
|
||||
void goalCallback(const geometry_msgs::msg::PoseStamped::SharedPtr msg)
|
||||
{
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
goal_pose_ = *msg;
|
||||
has_goal_ = true;
|
||||
RCLCPP_INFO(this->get_logger(), "New goal: (%.2f, %.2f)",
|
||||
msg->pose.position.x, msg->pose.position.y);
|
||||
|
||||
// Auto-transition IDLE → GOING if not already active
|
||||
if (state_ == TaskState::IDLE || state_ == TaskState::RESET) {
|
||||
state_ = TaskState::GOING;
|
||||
is_back_ = false;
|
||||
sub_target_ = true;
|
||||
has_path_ = false;
|
||||
waypoint_index_ = 0;
|
||||
angular_pid_.reset();
|
||||
RCLCPP_INFO(this->get_logger(), "Auto State -> GOING (goal received)");
|
||||
}
|
||||
}
|
||||
// Request plan outside mutex to avoid blocking other callbacks
|
||||
requestPlan();
|
||||
}
|
||||
|
||||
void odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg)
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
cur_x_ = msg->pose.pose.position.x;
|
||||
cur_y_ = msg->pose.pose.position.y;
|
||||
|
||||
double qx = msg->pose.pose.orientation.x;
|
||||
double qy = msg->pose.pose.orientation.y;
|
||||
double qz = msg->pose.pose.orientation.z;
|
||||
double qw = msg->pose.pose.orientation.w;
|
||||
cur_yaw_ = std::atan2(2.0 * (qw * qz + qx * qy),
|
||||
1.0 - 2.0 * (qy * qy + qz * qz));
|
||||
has_odom_ = true;
|
||||
}
|
||||
|
||||
void laneCmdCallback(const geometry_msgs::msg::Twist::SharedPtr msg)
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
lane_cmd_vel_ = *msg;
|
||||
got_lane_cmd_ = true;
|
||||
}
|
||||
|
||||
// ─────────────────────────────────────────────
|
||||
// C. ComputePathToPose action client
|
||||
// ─────────────────────────────────────────────
|
||||
|
||||
void requestPlan()
|
||||
{
|
||||
if (!planner_client_->wait_for_action_server(std::chrono::seconds(2))) {
|
||||
RCLCPP_WARN(this->get_logger(),
|
||||
"Planner action server (compute_path_to_pose) not available");
|
||||
return;
|
||||
}
|
||||
|
||||
auto goal_msg = ComputePathToPose::Goal();
|
||||
goal_msg.goal = goal_pose_;
|
||||
goal_msg.planner_id = "GridBased";
|
||||
|
||||
auto send_goal_options = rclcpp_action::Client<ComputePathToPose>::SendGoalOptions();
|
||||
send_goal_options.result_callback =
|
||||
[this](const GoalHandleComputePathToPose::WrappedResult& result) {
|
||||
if (result.code == rclcpp_action::ResultCode::SUCCEEDED) {
|
||||
RCLCPP_INFO(this->get_logger(), "Plan received: %zu waypoints",
|
||||
result.result->path.poses.size());
|
||||
} else {
|
||||
RCLCPP_WARN(this->get_logger(), "Plan request failed");
|
||||
}
|
||||
};
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "Requesting plan from Nav2 planner_server...");
|
||||
planner_client_->async_send_goal(goal_msg, send_goal_options);
|
||||
}
|
||||
|
||||
// ─────────────────────────────────────────────
|
||||
// D. Main Control Loop — cmd_vel arbitration
|
||||
// ─────────────────────────────────────────────
|
||||
|
||||
void controlLoop()
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
|
||||
// Priority 1: Task state control
|
||||
switch (state_)
|
||||
{
|
||||
case TaskState::IDLE:
|
||||
publishStop();
|
||||
return;
|
||||
|
||||
case TaskState::RESET:
|
||||
publishStop();
|
||||
state_ = TaskState::IDLE;
|
||||
return;
|
||||
|
||||
case TaskState::QR_SCAN:
|
||||
{
|
||||
auto elapsed = this->now() - qr_scan_start_;
|
||||
if (elapsed.seconds() < 1.0) {
|
||||
geometry_msgs::msg::Twist cmd;
|
||||
cmd.linear.x = qr_scan_speed_;
|
||||
cmd.angular.z = 0.0;
|
||||
cmd_vel_pub_->publish(cmd);
|
||||
}
|
||||
else if (elapsed.seconds() < qr_scan_duration_) {
|
||||
publishStop();
|
||||
}
|
||||
else {
|
||||
state_ = TaskState::GOING;
|
||||
RCLCPP_INFO(this->get_logger(), "QR scan complete, resuming");
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
case TaskState::GOING:
|
||||
case TaskState::RETURN:
|
||||
break;
|
||||
}
|
||||
|
||||
// Priority 2: Obstacle avoidance
|
||||
if (activate_avoid_) {
|
||||
geometry_msgs::msg::Twist cmd;
|
||||
cmd.linear.x = avoid_linear_speed_;
|
||||
cmd.angular.z = avoid_angular_z_;
|
||||
cmd_vel_pub_->publish(cmd);
|
||||
return;
|
||||
}
|
||||
|
||||
// Priority 3: Lane following (sub_target_ == false)
|
||||
if (!sub_target_ && got_lane_cmd_) {
|
||||
cmd_vel_pub_->publish(lane_cmd_vel_);
|
||||
return;
|
||||
}
|
||||
|
||||
// Priority 4: PID path following (path from Nav2 planner_server via /plan)
|
||||
if (sub_target_ && has_path_ && has_odom_) {
|
||||
if (waypoint_index_ >= static_cast<int>(global_path_.poses.size())) {
|
||||
RCLCPP_INFO(this->get_logger(), "Path complete - stopping");
|
||||
publishStop();
|
||||
has_path_ = false;
|
||||
return;
|
||||
}
|
||||
|
||||
int target_idx = std::min(waypoint_index_ + guide_step_,
|
||||
static_cast<int>(global_path_.poses.size()) - 1);
|
||||
const auto& target = global_path_.poses[target_idx];
|
||||
|
||||
double dx = target.pose.position.x - cur_x_;
|
||||
double dy = target.pose.position.y - cur_y_;
|
||||
double distance_sq = dx * dx + dy * dy;
|
||||
|
||||
if (distance_sq < arrive_square_) {
|
||||
waypoint_index_++;
|
||||
angular_pid_.reset();
|
||||
}
|
||||
|
||||
double target_yaw = std::atan2(dy, dx);
|
||||
double heading_error = target_yaw - cur_yaw_;
|
||||
|
||||
while (heading_error > M_PI) heading_error -= 2.0 * M_PI;
|
||||
while (heading_error < -M_PI) heading_error += 2.0 * M_PI;
|
||||
|
||||
double dt = 1.0 / this->get_parameter("control_rate").as_double();
|
||||
double angular_z = angular_pid_.update(heading_error, dt);
|
||||
|
||||
geometry_msgs::msg::Twist cmd;
|
||||
cmd.linear.x = follow_linear_speed_;
|
||||
cmd.angular.z = angular_z;
|
||||
cmd_vel_pub_->publish(cmd);
|
||||
return;
|
||||
}
|
||||
|
||||
// Priority 5: No path - stop and wait
|
||||
publishStop();
|
||||
}
|
||||
|
||||
void publishStop()
|
||||
{
|
||||
geometry_msgs::msg::Twist cmd;
|
||||
cmd.linear.x = 0.0;
|
||||
cmd.angular.z = 0.0;
|
||||
cmd_vel_pub_->publish(cmd);
|
||||
}
|
||||
|
||||
// ─────────────────────────────────────────────
|
||||
// Members (order must match initializer list)
|
||||
// ─────────────────────────────────────────────
|
||||
|
||||
TaskState state_;
|
||||
bool has_odom_, has_goal_;
|
||||
bool has_path_, got_lane_cmd_;
|
||||
bool is_back_, activate_avoid_;
|
||||
int waypoint_index_;
|
||||
int guide_step_;
|
||||
double follow_linear_speed_;
|
||||
bool sub_target_;
|
||||
double cmd_vel_linear_x_, cmd_vel_angular_z_;
|
||||
double cur_x_, cur_y_, cur_yaw_;
|
||||
double arrive_square_;
|
||||
past_control::PID angular_pid_;
|
||||
double avoid_linear_speed_;
|
||||
double avoid_angular_z_;
|
||||
double qr_scan_speed_;
|
||||
double qr_scan_duration_;
|
||||
rclcpp::Time qr_scan_start_;
|
||||
nav_msgs::msg::Path global_path_;
|
||||
geometry_msgs::msg::Twist lane_cmd_vel_;
|
||||
geometry_msgs::msg::PoseStamped goal_pose_;
|
||||
|
||||
rclcpp::Subscription<origincar_msg::msg::Sign>::SharedPtr sign_sub_;
|
||||
rclcpp::Subscription<origincar_msg::msg::Sign>::SharedPtr sign_foxglove_sub_;
|
||||
rclcpp::Subscription<nav_msgs::msg::Path>::SharedPtr path_sub_;
|
||||
rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr goal_sub_;
|
||||
rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
|
||||
rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr lane_cmd_sub_;
|
||||
rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_pub_;
|
||||
rclcpp_action::Client<ComputePathToPose>::SharedPtr planner_client_;
|
||||
rclcpp::TimerBase::SharedPtr timer_;
|
||||
std::mutex mutex_;
|
||||
};
|
||||
|
||||
int main(int argc, char* argv[])
|
||||
{
|
||||
rclcpp::init(argc, argv);
|
||||
rclcpp::spin(std::make_shared<RacingOrchestrator>());
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
33
src/planner/CMakeLists.txt
Normal file
33
src/planner/CMakeLists.txt
Normal file
@@ -0,0 +1,33 @@
|
||||
cmake_minimum_required(VERSION 3.5)
|
||||
project(planner)
|
||||
|
||||
if(NOT CMAKE_CXX_STANDARD)
|
||||
set(CMAKE_CXX_STANDARD 17)
|
||||
endif()
|
||||
|
||||
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
||||
add_compile_options(-Wall -Wextra)
|
||||
endif()
|
||||
|
||||
find_package(ament_cmake REQUIRED)
|
||||
|
||||
# Dummy executable to satisfy ament_cmake + install of launch/config
|
||||
add_executable(planner_version src/planner_version.cpp)
|
||||
|
||||
install(TARGETS planner_version
|
||||
DESTINATION lib/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
install(
|
||||
DIRECTORY launch config
|
||||
DESTINATION share/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
if(BUILD_TESTING)
|
||||
find_package(ament_lint_auto REQUIRED)
|
||||
set(ament_cmake_copyright_FOUND TRUE)
|
||||
set(ament_cmake_cpplint_FOUND TRUE)
|
||||
ament_lint_auto_find_test_dependencies()
|
||||
endif()
|
||||
|
||||
ament_package()
|
||||
232
src/planner/config/planner.yaml
Normal file
232
src/planner/config/planner.yaml
Normal file
@@ -0,0 +1,232 @@
|
||||
# ============================================================================
|
||||
# planner.yaml — Nav2 minimal global planning stack
|
||||
# 节点: slam_toolbox + planner_server + global_costmap + local_costmap
|
||||
# 来源: gc_navigation2_slamtoolbox (slam + nav params) 剪裁
|
||||
# ============================================================================
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# slam_toolbox — online_async 实时建图 → 发布 /map + map→odom transform
|
||||
# 来源: slam_toolbox_mapping.yaml
|
||||
# ---------------------------------------------------------------------------
|
||||
slam_toolbox:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
|
||||
# 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 base
|
||||
odom_frame: odom
|
||||
map_frame: map
|
||||
base_frame: base_link
|
||||
scan_topic: /scan
|
||||
mode: mapping
|
||||
use_map_saver: true
|
||||
|
||||
# Debug & performance
|
||||
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
|
||||
|
||||
# Mapping
|
||||
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
|
||||
|
||||
# Loop closure
|
||||
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
|
||||
|
||||
# Scan matching
|
||||
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
|
||||
|
||||
# Matcher params
|
||||
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
|
||||
scan_queue_size: 20
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# planner_server — SmacPlannerHybrid 全局路径规划 → /plan
|
||||
# 来源: gc_navigation_slam.yaml planner_server section
|
||||
# ---------------------------------------------------------------------------
|
||||
planner_server:
|
||||
ros__parameters:
|
||||
planner_plugins:
|
||||
- GridBased
|
||||
use_sim_time: False
|
||||
GridBased:
|
||||
plugin: nav2_smac_planner/SmacPlannerHybrid
|
||||
downsample_costmap: False
|
||||
downsampling_factor: 1
|
||||
tolerance: 0.25
|
||||
allow_unknown: True
|
||||
max_iterations: 100000
|
||||
max_on_approach_iterations: 1000
|
||||
max_planning_time: 5.0
|
||||
motion_model_for_search: REEDS_SHEPP
|
||||
angle_quantization_bins: 72
|
||||
analytic_expansion_ratio: 2.0
|
||||
analytic_expansion_max_length: 3.0
|
||||
minimum_turning_radius: 0.35
|
||||
reverse_penalty: 1.3
|
||||
change_penalty: 0.0
|
||||
non_straight_penalty: 0.0
|
||||
cost_penalty: 5.0
|
||||
retrospective_penalty: 0.015
|
||||
lookup_table_size: 5.0
|
||||
cache_obstacle_heuristic: False
|
||||
viz_expansions: False
|
||||
smooth_path: True
|
||||
smoother:
|
||||
max_iterations: 1000
|
||||
w_smooth: 0.4
|
||||
w_data: 0.2
|
||||
tolerance: 1.0e-10
|
||||
do_refinement: True
|
||||
refinement_num: 4
|
||||
|
||||
planner_server_rclcpp_node:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# global_costmap — 全局代价地图（订阅 /scan + /map）
|
||||
# 来源: gc_navigation_slam.yaml global_costmap section
|
||||
# ---------------------------------------------------------------------------
|
||||
global_costmap:
|
||||
global_costmap:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
transform_tolerance: 2.0
|
||||
update_frequency: 1.0
|
||||
publish_frequency: 1.0
|
||||
global_frame: map
|
||||
robot_base_frame: base_link
|
||||
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.2
|
||||
always_send_full_costmap: True
|
||||
global_costmap_client:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
global_costmap_rclcpp_node:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# local_costmap — 局部代价地图（obstacle_detector 可注入）
|
||||
# 来源: gc_navigation_slam.yaml local_costmap section
|
||||
# ---------------------------------------------------------------------------
|
||||
local_costmap:
|
||||
local_costmap:
|
||||
ros__parameters:
|
||||
update_frequency: 1.0
|
||||
publish_frequency: 2.0
|
||||
transform_tolerance: 2.0
|
||||
global_frame: odom
|
||||
robot_base_frame: base_link
|
||||
use_sim_time: False
|
||||
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.2
|
||||
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: False
|
||||
local_costmap_rclcpp_node:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
77
src/planner/launch/planner.launch.py
Normal file
77
src/planner/launch/planner.launch.py
Normal file
@@ -0,0 +1,77 @@
|
||||
# ============================================================================
|
||||
# planner.launch.py
|
||||
# Nav2 最小全局规划模块
|
||||
# 启动: slam_toolbox + planner_server + lifecycle_manager
|
||||
# costmap 由 planner_server 内部实例化，不需要单独启动
|
||||
# 不启动: controller / behavior / bt_navigator / velocity_smoother / smoother
|
||||
#
|
||||
# 参考: gc_nav2_with_slam_online_real.launch.py
|
||||
# ============================================================================
|
||||
|
||||
import os
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch_ros.actions import Node
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
"""启动 Nav2 最小规划栈：slam + planner"""
|
||||
ld = LaunchDescription()
|
||||
|
||||
# === 包路径 ===
|
||||
pkg_dir = get_package_share_directory('planner')
|
||||
|
||||
# === 参数 ===
|
||||
use_sim_time = LaunchConfiguration('use_sim_time', default='false')
|
||||
params_file = LaunchConfiguration(
|
||||
'params_file',
|
||||
default=os.path.join(pkg_dir, 'config', 'planner.yaml'))
|
||||
|
||||
declare_params = DeclareLaunchArgument(
|
||||
'params_file',
|
||||
default_value=os.path.join(pkg_dir, 'config', 'planner.yaml'),
|
||||
description='Path to the planner parameters file')
|
||||
ld.add_action(declare_params)
|
||||
|
||||
# =====================================================================
|
||||
# 1. slam_toolbox — 异步建图 (online_async)，发布 /map + map→odom
|
||||
# =====================================================================
|
||||
slam_toolbox_node = Node(
|
||||
package='slam_toolbox',
|
||||
executable='async_slam_toolbox_node',
|
||||
name='slam_toolbox',
|
||||
output='screen',
|
||||
parameters=[params_file,
|
||||
{'use_sim_time': use_sim_time}],
|
||||
)
|
||||
ld.add_action(slam_toolbox_node)
|
||||
|
||||
# =====================================================================
|
||||
# 2. planner_server — SmacPlannerHybrid，ComputePathToPose action
|
||||
# planner_server 内部实例化 global_costmap（从 planner.yaml 读取参数）
|
||||
# =====================================================================
|
||||
planner_server_node = Node(
|
||||
package='nav2_planner',
|
||||
executable='planner_server',
|
||||
name='planner_server',
|
||||
output='screen',
|
||||
parameters=[params_file],
|
||||
)
|
||||
ld.add_action(planner_server_node)
|
||||
|
||||
# =====================================================================
|
||||
# 3. lifecycle_manager — 自动激活 planner_server (lifecycle 节点)
|
||||
# =====================================================================
|
||||
lifecycle_manager_node = Node(
|
||||
package='nav2_lifecycle_manager',
|
||||
executable='lifecycle_manager',
|
||||
name='lifecycle_manager_planner',
|
||||
output='screen',
|
||||
parameters=[{'autostart': True},
|
||||
{'node_names': ['planner_server']}],
|
||||
)
|
||||
ld.add_action(lifecycle_manager_node)
|
||||
|
||||
return ld
|
||||
26
src/planner/package.xml
Normal file
26
src/planner/package.xml
Normal file
@@ -0,0 +1,26 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>planner</name>
|
||||
<version>0.0.0</version>
|
||||
<description>Minimal Nav2 global planning stack: slam_toolbox + planner_server + costmaps + lifecycle_manager</description>
|
||||
<maintainer email="2314753575@qq.com">sunrise</maintainer>
|
||||
<license>TODO: License declaration</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<depend>rclcpp</depend>
|
||||
<depend>slam_toolbox</depend>
|
||||
<depend>nav2_planner</depend>
|
||||
<depend>nav2_costmap_2d</depend>
|
||||
<depend>nav2_lifecycle_manager</depend>
|
||||
<depend>nav2_common</depend>
|
||||
<depend>nav2_util</depend>
|
||||
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
11
src/planner/src/planner_version.cpp
Normal file
11
src/planner/src/planner_version.cpp
Normal file
@@ -0,0 +1,11 @@
|
||||
#include <iostream>
|
||||
|
||||
int main(int argc, char* argv[])
|
||||
{
|
||||
std::cout << "planner package: Nav2 minimal global planning stack" << std::endl;
|
||||
std::cout << " - slam_toolbox (online_async)" << std::endl;
|
||||
std::cout << " - planner_server (SmacPlannerHybrid)" << std::endl;
|
||||
std::cout << " - global_costmap + local_costmap" << std::endl;
|
||||
std::cout << " - lifecycle_manager" << std::endl;
|
||||
return 0;
|
||||
}
|
||||
@@ -22,11 +22,13 @@ public:
|
||||
std::bind(&MinimalHbmemSubscriber::image_callback, this, std::placeholders::_1));
|
||||
|
||||
// 创建订阅器，订阅 'sign4return' 话题
|
||||
subscription_sign_ =
|
||||
this->create_subscription<std_msgs::msg::Int32>(
|
||||
"sign4return",
|
||||
10,
|
||||
std::bind(&MinimalHbmemSubscriber::sign_callback, this, std::placeholders::_1));
|
||||
// subscription_sign_ =
|
||||
// this->create_subscription<std_msgs::msg::Int32>(
|
||||
// "sign4return",
|
||||
// 10,
|
||||
// std::bind(&MinimalHbmemSubscriber::sign_callback, this, std::placeholders::_1));
|
||||
// 创建发布器，发布 'sign4return' 话题
|
||||
sign_publisher_ = this->create_publisher<std_msgs::msg::Int32>("sign4return", 10);
|
||||
|
||||
// 创建 publisher，topic 为 "qr_results"
|
||||
publisher_ =
|
||||
@@ -87,6 +89,23 @@ private:
|
||||
symbol->get_type_name().c_str(), symbol->get_data().c_str());
|
||||
}
|
||||
qr_results_buff_ = qr_results; // 更新缓存
|
||||
|
||||
// 检测字符合法性
|
||||
// if (ResultLegal(qr_results)){
|
||||
// // 发布sign4return话题
|
||||
// auto message = std_msgs::msg::Int32();
|
||||
// message.data = 5; // 5表示检测到二维码
|
||||
// sign_publisher_->publish(message);
|
||||
// RCLCPP_INFO(this->get_logger(), "QR Results Legal");
|
||||
// }
|
||||
// else{
|
||||
// RCLCPP_INFO(this->get_logger(), "\033[31m QR Results Illegal \033[00m");
|
||||
// }
|
||||
|
||||
// 发布sign4return话题
|
||||
auto message = std_msgs::msg::Int32();
|
||||
message.data = 5; // 5表示检测到二维码
|
||||
sign_publisher_->publish(message);
|
||||
} else {
|
||||
qr_results = "QR Code not detected";
|
||||
RCLCPP_INFO(this->get_logger(), "QR Code not detected");
|
||||
@@ -104,24 +123,36 @@ private:
|
||||
}
|
||||
|
||||
// 消息回调函数，处理 sign4return 话题
|
||||
void sign_callback(const std_msgs::msg::Int32::SharedPtr msg)
|
||||
{
|
||||
if (msg->data == 0)
|
||||
{
|
||||
detect_qr_code_ = true; // 启动二维码检测
|
||||
RCLCPP_INFO(this->get_logger(), "QR detection started");
|
||||
}
|
||||
else if (msg->data == 5)
|
||||
{
|
||||
detect_qr_code_ = false; // 停止二维码检测
|
||||
RCLCPP_INFO(this->get_logger(), "QR detection stopped");
|
||||
}
|
||||
// void sign_callback(const std_msgs::msg::Int32::SharedPtr msg)
|
||||
// {
|
||||
// if (msg->data == 0)
|
||||
// {
|
||||
// detect_qr_code_ = true; // 启动二维码检测
|
||||
// RCLCPP_INFO(this->get_logger(), "QR detection started");
|
||||
// }
|
||||
// else if (msg->data == 5)
|
||||
// {
|
||||
// detect_qr_code_ = false; // 停止二维码检测
|
||||
// RCLCPP_INFO(this->get_logger(), "QR detection stopped");
|
||||
// }
|
||||
// }
|
||||
|
||||
// 识别结果合法性检测
|
||||
bool ResultLegal(std::string& input){
|
||||
// 合法字符串集合
|
||||
static const std::unordered_set<std::string> kValidStrings = {
|
||||
"1", "2", "顺时针", "逆时针", "顺", "逆"
|
||||
};
|
||||
return kValidStrings.find(input) != kValidStrings.end();
|
||||
}
|
||||
|
||||
|
||||
// /aurora/rgb/image_raw 订阅器
|
||||
rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr subscription_image_;
|
||||
// sign4return 订阅器
|
||||
rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr subscription_sign_;
|
||||
// rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr subscription_sign_;
|
||||
// sign4return 发布器
|
||||
rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr sign_publisher_;
|
||||
// QR code results 发布器
|
||||
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
|
||||
|
||||
|
||||
26
src/racing_control/CMakeLists.txt
Normal file
26
src/racing_control/CMakeLists.txt
Normal file
@@ -0,0 +1,26 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(racing_control)
|
||||
|
||||
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
||||
add_compile_options(-Wall -Wextra -Wpedantic)
|
||||
endif()
|
||||
|
||||
# find dependencies
|
||||
find_package(ament_cmake REQUIRED)
|
||||
# uncomment the following section in order to fill in
|
||||
# further dependencies manually.
|
||||
# find_package(<dependency> REQUIRED)
|
||||
|
||||
if(BUILD_TESTING)
|
||||
find_package(ament_lint_auto REQUIRED)
|
||||
# the following line skips the linter which checks for copyrights
|
||||
# comment the line when a copyright and license is added to all source files
|
||||
set(ament_cmake_copyright_FOUND TRUE)
|
||||
# the following line skips cpplint (only works in a git repo)
|
||||
# comment the line when this package is in a git repo and when
|
||||
# a copyright and license is added to all source files
|
||||
set(ament_cmake_cpplint_FOUND TRUE)
|
||||
ament_lint_auto_find_test_dependencies()
|
||||
endif()
|
||||
|
||||
ament_package()
|
||||
35
src/racing_control/include/racing_control/racing_control.hpp
Normal file
35
src/racing_control/include/racing_control/racing_control.hpp
Normal file
@@ -0,0 +1,35 @@
|
||||
/*
|
||||
功能描述：
|
||||
1. 状态指令：
|
||||
1 - 比赛开始，进行任务一寻找二维码
|
||||
2 - 找到二维码，执行任务一到任务二过渡阶段导航
|
||||
3 - 到达任务二起始阶段，执行顺/逆时针绕圈（可集成图生文）
|
||||
4 - 减速拍照，图像传入图生文节点，随后回到3（备用）
|
||||
5 - 完整走完一圈，执行任务三
|
||||
2. 全部流程
|
||||
启动小车->slamtoolbox开始建图，并开始发布导航命令（这时候小车还不能动）->打开电机开关，小车开始行动->走到一半扫到二维码，发布在/qr_results上->停掉二维码节点（sign=5）和导航1，同时开始导航2到任务二入口并顺逆时针转圈->到达指定位置触发一次vlm请求->开始语音播报同时完成任务二、三
|
||||
*/
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
|
||||
static int QR_SEARCHING = 1;
|
||||
static int ENTRY = 2;
|
||||
static int CIRCLE = 3;
|
||||
static int VLM = 4;
|
||||
static int TASK3 = 5;
|
||||
|
||||
|
||||
class RacingControl : public rclcpp::Node
|
||||
{
|
||||
public:
|
||||
RacingControl() : Node("racing_control")
|
||||
{
|
||||
// 初始化状态指令
|
||||
state_command_ = QR_SEARCHING;
|
||||
}
|
||||
private:
|
||||
// 状态指令变量
|
||||
int state_command_;
|
||||
};
|
||||
|
||||
|
||||
18
src/racing_control/package.xml
Normal file
18
src/racing_control/package.xml
Normal file
@@ -0,0 +1,18 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>racing_control</name>
|
||||
<version>0.0.0</version>
|
||||
<description>TODO: Package description</description>
|
||||
<maintainer email="2314753575@qq.com">sunrise</maintainer>
|
||||
<license>TODO: License declaration</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
0
src/racing_control/src/racing_control.cpp
Normal file
0
src/racing_control/src/racing_control.cpp
Normal file
@@ -21,6 +21,8 @@ setup(
|
||||
entry_points={
|
||||
'console_scripts': [
|
||||
'vlm_node = vlm_detect.vlm_node:main',
|
||||
'test_publisher = vlm_detect.test_publisher:main',
|
||||
'tts_node = vlm_detect.tts_node:main',
|
||||
],
|
||||
},
|
||||
)
|
||||
|
||||
BIN
src/vlm_detect/vlm_detect/__pycache__/__init__.cpython-310.pyc
Normal file
BIN
src/vlm_detect/vlm_detect/__pycache__/__init__.cpython-310.pyc
Normal file
Binary file not shown.
BIN
src/vlm_detect/vlm_detect/__pycache__/tts_node.cpython-310.pyc
Normal file
BIN
src/vlm_detect/vlm_detect/__pycache__/tts_node.cpython-310.pyc
Normal file
Binary file not shown.
88
src/vlm_detect/vlm_detect/test_publisher.py
Normal file
88
src/vlm_detect/vlm_detect/test_publisher.py
Normal file
@@ -0,0 +1,88 @@
|
||||
#!/usr/bin/env python3
|
||||
# -*- coding: utf-8 -*-
|
||||
"""
|
||||
测试发布者：发送图片和触发信号给 VLM 节点
|
||||
用法: ros2 run vlm_detect test_publisher --ros-args -p image_path:="/path/to/image.jpg"
|
||||
"""
|
||||
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from std_msgs.msg import Int32, String
|
||||
from sensor_msgs.msg import CompressedImage
|
||||
import cv2
|
||||
import time
|
||||
|
||||
|
||||
class TestPublisher(Node):
|
||||
def __init__(self):
|
||||
super().__init__("test_publisher")
|
||||
|
||||
# 声明参数
|
||||
self.declare_parameter(
|
||||
"image_path", "/home/sunrise/yiliao_ws/my_model/image.png"
|
||||
)
|
||||
self.declare_parameter("interval", 5.0) # 每 N 秒触发一次
|
||||
|
||||
image_path = self.get_parameter("image_path").value
|
||||
interval = self.get_parameter("interval").value
|
||||
|
||||
# 发布者
|
||||
self.image_pub = self.create_publisher(CompressedImage, "/image_mjpeg", 10)
|
||||
self.sign_pub = self.create_publisher(Int32, "/sign4return", 10)
|
||||
|
||||
# 订阅结果
|
||||
self.result_sub = self.create_subscription(
|
||||
String, "/vlm_result", self.result_callback, 10
|
||||
)
|
||||
|
||||
# 加载图片
|
||||
self.image_data = cv2.imread(image_path)
|
||||
if self.image_data is None:
|
||||
self.get_logger().error(f"无法读取图片: {image_path}")
|
||||
raise FileNotFoundError(f"Image not found: {image_path}")
|
||||
|
||||
encode_params = [cv2.IMWRITE_JPEG_QUALITY, 90]
|
||||
_, jpeg_data = cv2.imencode(".jpg", self.image_data, encode_params)
|
||||
self.jpeg_bytes = jpeg_data.tobytes()
|
||||
self.get_logger().info(f"已加载图片: {image_path}")
|
||||
|
||||
# 定时器
|
||||
self.timer = self.create_timer(interval, self.timer_callback)
|
||||
self.get_logger().info(f"每 {interval}s 发送一次图片和触发信号")
|
||||
|
||||
def timer_callback(self):
|
||||
# 1. 发送压缩图像
|
||||
img_msg = CompressedImage()
|
||||
img_msg.header.stamp = self.get_clock().now().to_msg()
|
||||
img_msg.format = "jpeg"
|
||||
img_msg.data = self.jpeg_bytes
|
||||
self.image_pub.publish(img_msg)
|
||||
self.get_logger().info("已发送图片")
|
||||
|
||||
# 2. 等待一小段时间让订阅者收到图片
|
||||
time.sleep(1.0)
|
||||
|
||||
# 3. 发送触发信号 (sign=9)
|
||||
sign_msg = Int32()
|
||||
sign_msg.data = 9
|
||||
self.sign_pub.publish(sign_msg)
|
||||
self.get_logger().info("已发送触发信号 (sign=9)，等待 VLM 结果...")
|
||||
|
||||
def result_callback(self, msg):
|
||||
self.get_logger().info(f"🔍 VLM 识别结果: {msg.data}")
|
||||
|
||||
|
||||
def main(args=None):
|
||||
rclpy.init(args=args)
|
||||
node = TestPublisher()
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
42
src/vlm_detect/vlm_detect/tts_node.py
Normal file
42
src/vlm_detect/vlm_detect/tts_node.py
Normal file
@@ -0,0 +1,42 @@
|
||||
#!/usr/bin/env python3
|
||||
import rclpy, subprocess, requests, os
|
||||
from rclpy.node import Node
|
||||
from std_msgs.msg import String
|
||||
|
||||
VLM_HOST = "http://192.168.10.173:8000"
|
||||
# USB Audio Device (Card 1)
|
||||
AUDIO_SINK = "alsa_output.usb-C-Media_Electronics_Inc._USB_Audio_Device-00.analog-stereo"
|
||||
AUDIO_ENV = {**os.environ, "PULSE_SINK": AUDIO_SINK}
|
||||
|
||||
class TTSNode(Node):
|
||||
def __init__(self):
|
||||
super().__init__("tts_node")
|
||||
self.sub = self.create_subscription(String, "/vlm_result", self.callback, 10)
|
||||
self.get_logger().info("TTS 播报节点已启动 (USB Audio Device, edge-tts 自然语音)")
|
||||
|
||||
def callback(self, msg):
|
||||
text = msg.data
|
||||
self.get_logger().info(f"播报: {text}")
|
||||
try:
|
||||
resp = requests.post(f"{VLM_HOST}/v1/tts",
|
||||
json={"text": text, "voice": "zh-CN-XiaoxiaoNeural"}, timeout=60)
|
||||
mp3 = "/tmp/tts_out.mp3"
|
||||
with open(mp3, "wb") as f:
|
||||
f.write(resp.content)
|
||||
subprocess.Popen(["ffplay", "-nodisp", "-autoexit", mp3],
|
||||
stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL,
|
||||
env=AUDIO_ENV)
|
||||
except Exception as e:
|
||||
self.get_logger().error(f"TTS 失败,降级 espeak: {e}")
|
||||
subprocess.Popen(["espeak-ng", "-v", "zh", "-s", "150", text],
|
||||
env=AUDIO_ENV)
|
||||
|
||||
def main(args=None):
|
||||
rclpy.init(args=args)
|
||||
node = TTSNode()
|
||||
try: rclpy.spin(node)
|
||||
except KeyboardInterrupt: pass
|
||||
finally: node.destroy_node(); rclpy.shutdown()
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -15,14 +15,14 @@ import numpy as np
|
||||
|
||||
class VLMProcessor(Node):
|
||||
def __init__(self):
|
||||
super().__init__('vlm_dtetct')
|
||||
super().__init__('vlm_detect')
|
||||
|
||||
# 初始化 OpenAI 客户端
|
||||
self.client = OpenAI(
|
||||
base_url="http://192.168.1.103:8000/v1",
|
||||
api_key="EMPTY"
|
||||
base_url="http://192.168.10.173:8000/v1", # 本地 API 地址
|
||||
api_key="EMPTY", # 不需要真实 API key
|
||||
)
|
||||
|
||||
|
||||
# ROS2 组件
|
||||
self.bridge = CvBridge()
|
||||
self.latest_image = None
|
||||
@@ -31,7 +31,7 @@ class VLMProcessor(Node):
|
||||
# 订阅图像话题
|
||||
self.image_sub = self.create_subscription(
|
||||
CompressedImage,
|
||||
'/image',
|
||||
'/image_mjpeg',
|
||||
self.image_callback,
|
||||
10
|
||||
)
|
||||
@@ -57,18 +57,20 @@ class VLMProcessor(Node):
|
||||
"""保存最新的图像"""
|
||||
with self.image_lock:
|
||||
try:
|
||||
# bridge = CvBridge()
|
||||
np_arr = np.frombuffer(msg.data, np.uint8)
|
||||
# 使用 OpenCV 解码图像
|
||||
cv_image = cv2.imdecode(np_arr, cv2.IMREAD_COLOR)
|
||||
# cv_image =bridge.imgmsg_to_cv2(msg,desired_encoding='bgr8')
|
||||
self.latest_image = cv_image
|
||||
self.get_logger().debug("recive picture")
|
||||
except Exception as e:
|
||||
self.get_logger().error("err picture")
|
||||
self.get_logger().error(f"err picture: {e}")
|
||||
|
||||
def sign_callback(self, msg):
|
||||
"""处理触发信号"""
|
||||
if msg.data == 9:
|
||||
self.get_logger().info("收到触发信号 (6)，开始处理图像...")
|
||||
self.get_logger().info(f"收到触发信号 ({msg.data})，开始处理图像...")
|
||||
|
||||
# 检查是否有可用图像
|
||||
with self.image_lock:
|
||||
@@ -79,12 +81,12 @@ class VLMProcessor(Node):
|
||||
# 保存临时图像文件
|
||||
temp_path = "/tmp/vlm_temp_image.jpg"
|
||||
cv2.imwrite(temp_path, self.latest_image)
|
||||
self.get_logger().info("已保存临时图像}")
|
||||
self.get_logger().info(f"已保存临时图像: {temp_path}")
|
||||
|
||||
# 处理图像
|
||||
try:
|
||||
description = self.process_image(temp_path)
|
||||
self.get_logger().info(f"图像描述结果:")
|
||||
self.get_logger().info(f"图像描述结果: {description}")
|
||||
|
||||
# 发布结果
|
||||
result_msg = String()
|
||||
@@ -94,7 +96,7 @@ class VLMProcessor(Node):
|
||||
# 清理临时文件
|
||||
os.remove(temp_path)
|
||||
except Exception as e:
|
||||
self.get_logger().error(f"处理图像时出错:{e}")
|
||||
self.get_logger().error(f"处理图像时出错: {e}")
|
||||
|
||||
def process_image(self, image_path):
|
||||
"""使用 VLM 模型处理图像"""
|
||||
@@ -106,26 +108,26 @@ class VLMProcessor(Node):
|
||||
start_time = time.time()
|
||||
|
||||
response = self.client.chat.completions.create(
|
||||
model="/home/yyh/vllm_test/model/InternVL3-1B",
|
||||
model="./OpenGVLab/InternVL3-1B/",
|
||||
messages=[
|
||||
{
|
||||
"role": "user",
|
||||
"content": [
|
||||
{"type": "text", "text": "描述图片中的动漫病人"},
|
||||
{"type": "text", "text": "描述图片中有一个病人的特征，字数控制在20字以内。"},
|
||||
{
|
||||
"type": "image_url",
|
||||
"image_url": {
|
||||
"url": f"data:image/jpeg;base64,{base64_image}"
|
||||
}
|
||||
}
|
||||
},
|
||||
},
|
||||
]
|
||||
}
|
||||
],
|
||||
max_tokens=100
|
||||
max_tokens=100,
|
||||
)
|
||||
|
||||
processing_time = time.time() - start_time
|
||||
self.get_logger().info("VLM 处理耗时秒")
|
||||
self.get_logger().info(f"VLM 处理耗时 {processing_time:.1f}s")
|
||||
|
||||
return response.choices[0].message.content
|
||||
|
||||
|
||||
Reference in New Issue
Block a user