From f02c20cc5c00e2eca0789c40c12bd4b6f0ca77ab Mon Sep 17 00:00:00 2001 From: HiepLM Date: Wed, 18 Mar 2026 06:51:10 +0000 Subject: [PATCH] add some files --- .gitignore | 1 + CMakeLists.txt | 16 +++++++ config/costmap_common_params.yaml | 30 ++++++++++--- config/costmap_global_params.yaml | 2 +- config/costmap_local_params.yaml | 2 +- config/hybrid_local_planner_params.yaml | 59 +++++++++++++++++++++++++ src/Libraries/laser_geometry | 2 +- 7 files changed, 103 insertions(+), 9 deletions(-) create mode 100644 config/hybrid_local_planner_params.yaml diff --git a/.gitignore b/.gitignore index 7e42b36..6bde50a 100644 --- a/.gitignore +++ b/.gitignore @@ -422,3 +422,4 @@ build install devel +obstacle \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index f949b57..9c91c70 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -138,6 +138,22 @@ if (NOT TARGET pnkx_local_planner) add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Packages/local_planners/pnkx_local_planner) endif() +if (NOT TARGET robot_angles) + add_subdirectory(${CMAKE_SOURCE_DIR}/obstacle/angles) +endif() + +if (NOT TARGET grid_map_core) + add_subdirectory(${CMAKE_SOURCE_DIR}/obstacle/grid_map_core) +endif() + +if (NOT TARGET robot_base_local_planner) + add_subdirectory(${CMAKE_SOURCE_DIR}/obstacle/base_local_planner) +endif() + +if (NOT TARGET hybrid_local_planner) + add_subdirectory(${CMAKE_SOURCE_DIR}/obstacle/hybrid_local_planner) +endif() + if (NOT TARGET robot_actionlib_msgs) add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Libraries/robot_actionlib_msgs) endif() diff --git a/config/costmap_common_params.yaml b/config/costmap_common_params.yaml index f606ea9..14cebc4 100755 --- a/config/costmap_common_params.yaml +++ b/config/costmap_common_params.yaml @@ -1,4 +1,5 @@ -position_planner_name: PNKXLocalPlanner +# position_planner_name: PNKXLocalPlanner +position_planner_name: HybridLocalPlanner docking_planner_name: PNKXDockingLocalPlanner go_straight_planner_name: PNKXGoStraightLocalPlanner rotate_planner_name: PNKXRotateLocalPlanner @@ -26,17 +27,33 @@ virtual_walls_map: lethal_cost_threshold: 100 obstacles: - observation_sources: f_scan_marking f_scan_clearing b_scan_marking b_scan_clearing - f_scan_marking: - topic: /f_scan + observation_sources: l_scan_marking l_scan_clearing r_scan_marking r_scan_clearing b_scan_marking b_scan_clearing + l_scan_marking: + topic: /l_scan data_type: LaserScan clearing: false marking: true inf_is_valid: true min_obstacle_height: 0.0 max_obstacle_height: 0.25 - f_scan_clearing: - topic: /f_scan + l_scan_clearing: + topic: /l_scan + data_type: LaserScan + clearing: true + marking: false + inf_is_valid: true + min_obstacle_height: 0.0 + max_obstacle_height: 0.25 + r_scan_marking: + topic: /r_scan + data_type: LaserScan + clearing: false + marking: true + inf_is_valid: true + min_obstacle_height: 0.0 + max_obstacle_height: 0.25 + r_scan_clearing: + topic: /r_scan data_type: LaserScan clearing: true marking: false @@ -59,5 +76,6 @@ obstacles: inf_is_valid: true min_obstacle_height: 0.0 max_obstacle_height: 0.25 + diff --git a/config/costmap_global_params.yaml b/config/costmap_global_params.yaml index 811ab13..08551f2 100755 --- a/config/costmap_global_params.yaml +++ b/config/costmap_global_params.yaml @@ -4,7 +4,7 @@ global_costmap: global_frame: map update_frequency: 1.0 publish_frequency: 1.0 - raytrace_range: 2.0 + raytrace_range: 3.5 resolution: 0.05 z_resolution: 0.2 rolling_window: false diff --git a/config/costmap_local_params.yaml b/config/costmap_local_params.yaml index ecd9e9d..c13fc5b 100755 --- a/config/costmap_local_params.yaml +++ b/config/costmap_local_params.yaml @@ -5,7 +5,7 @@ local_costmap: update_frequency: 6.0 publish_frequency: 6.0 rolling_window: true - raytrace_range: 2.0 + raytrace_range: 3.5 resolution: 0.05 z_resolution: 0.15 z_voxels: 8 diff --git a/config/hybrid_local_planner_params.yaml b/config/hybrid_local_planner_params.yaml new file mode 100644 index 0000000..43b510e --- /dev/null +++ b/config/hybrid_local_planner_params.yaml @@ -0,0 +1,59 @@ +LocalPlannerAdapter: + library_path: liblocal_planner_adapter + yaw_goal_tolerance: 0.017 + xy_goal_tolerance: 0.03 + min_approach_linear_velocity: 0.06 + +HybridLocalPlanner: +# base_local_planner: "hybrid_local_planner/HybridLocalPlanner" +# HybridLocalPlanner: + library_path: libhybrid_local_planner + odom_topic: odom + # Trajectory + max_global_plan_lookahead_dist: 4.0 + global_plan_viapoint_sep: 0.5 + global_plan_prune_distance: 0.0 + global_plan_goal_sep: 0.05 + + # Robot + + robot_max_v_ac: 0.4 + robot_max_w_ac: 0.4 + robot_max_v_pt: 1.0 + robot_max_w_pt: 0.4 + robot_max_v_backwards_pt: -0.2 + acc_lim_x: 1.0 + acc_lim_theta: 2.0 + turn_around_priority: True + stop_dist: 0.5 + dec_dist: 1.0 + + + # GoalTolerance + + xy_goal_tolerance: 0.1 + yaw_goal_tolerance: 0.07 + + # Optimization + + # PP Parameters + w_vel: 0.8 + w_omega: 1.0 + # DWA Parameters + enable_backward_motion: false + w_targetheading_ac: 1.7 + w_velocity_ac: 0.2 + w_clearance_ac: 0.2 + w_pathDistance_ac: 0.05 + w_smoothness_ac: 0.3 + w_targetheading_pt: 0.2 + w_velocity_pt: 0.8 + w_clearance_pt: 0.1 + w_pathDistance_pt: 2.1 + w_smoothness_pt: 0.3 + time_horizon: 3.0 + velocity_resolution: 0.015 + + segment_transition_threshold: 0.01 # Ngưỡng khoảng cách chuyển segment + calibration_factor: 1.5 # Hệ số hiệu chuẩn + use_obstacle_avoidance: true # Bật tắt tránh vật cản \ No newline at end of file diff --git a/src/Libraries/laser_geometry b/src/Libraries/laser_geometry index 50062ef..7e70a03 160000 --- a/src/Libraries/laser_geometry +++ b/src/Libraries/laser_geometry @@ -1 +1 @@ -Subproject commit 50062ef54970ffb6a88f550cfd7dac7a6d587041 +Subproject commit 7e70a03bc004074075bb03db66e59b75bf3f19f5