From ff6879c8af1799692b5987e161004531680fe10e Mon Sep 17 00:00:00 2001 From: zdeng-UTexas Date: Sat, 27 Apr 2024 18:09:59 -0500 Subject: [PATCH] changed a* heuristic to octile distance and added recovery carrot param --- config/navigation.lua | 9 +++++---- src/navigation/navigation.cc | 16 +++++++++++----- src/navigation/navigation_main.cc | 6 ++++-- src/navigation/navigation_parameters.h | 12 ++++++++---- 4 files changed, 28 insertions(+), 15 deletions(-) diff --git a/config/navigation.lua b/config/navigation.lua index 9a38dad..a612e50 100644 --- a/config/navigation.lua +++ b/config/navigation.lua @@ -21,7 +21,7 @@ NavigationParameters = { max_angular_accel = 0.5; max_angular_decel = 0.5; max_angular_speed = 1.0; - carrot_dist = 1; + carrot_dist = 1.5; system_latency = 0.24; obstacle_margin = 0.15; num_options = 31; @@ -42,8 +42,8 @@ NavigationParameters = { evaluator_type = "linear"; intermediate_goal_dist = 10; max_inflation_radius = 1; - min_inflation_radius = 0.2; - local_costmap_resolution = 0.1; + min_inflation_radius = 0.3; + local_costmap_resolution = 0.2; local_costmap_size = 20; global_costmap_resolution = 0.1; global_costmap_size_x = 100; @@ -52,10 +52,11 @@ NavigationParameters = { global_costmap_origin_y = 0; lidar_range_min = 0.1; lidar_range_max = 10.0; - replan_carrot_dist = 2; + replan_dist = 2; object_lifespan = 15; inflation_coeff = 8; distance_weight = 3; + recovery_carrot_dist = 0.5; }; AckermannSampler = { diff --git a/src/navigation/navigation.cc b/src/navigation/navigation.cc index 10b6fb7..9eabac3 100644 --- a/src/navigation/navigation.cc +++ b/src/navigation/navigation.cc @@ -634,7 +634,13 @@ vector Navigation::Plan(const Vector2f& initial, if(cost.count(neighbor_index) == 0 || new_cost < cost[neighbor_index]){ cost[neighbor_index] = new_cost; parent[neighbor_index] = current_index; - float heuristic_cost = (Vector2f(goal_map_x, goal_map_y) - Vector2f(new_row, new_col)).norm(); + + float dx = abs(goal_map_x - new_row); + float dy = abs(goal_map_y - new_col); + + float heuristic_cost = max(dx, dy) + (sqrt(2.0) - 1.0) * min(dx, dy); + + // float heuristic_cost = (Vector2f(goal_map_x, goal_map_y) - Vector2f(new_row, new_col)).norm(); intermediate_queue.Push(neighbor_index, -(new_cost + heuristic_cost)); } } @@ -713,7 +719,7 @@ bool Navigation::IntermediatePlanStillValid(){ Vector2f global_carrot; GetGlobalCarrot(global_carrot); - if((intermediate_goal_ - global_carrot).norm() > params_.replan_carrot_dist){ + if((intermediate_goal_ - global_carrot).norm() > params_.replan_dist){ return false; } @@ -903,7 +909,7 @@ void Navigation::RunObstacleAvoidance(Vector2f& vel_cmd, float& ang_vel_cmd) { } if (paths.size() == 0) { // No options, just stop. - // Halt(vel_cmd, ang_vel_cmd); + Halt(vel_cmd, ang_vel_cmd); if (debug) printf("No paths found\n"); return; } @@ -911,9 +917,9 @@ void Navigation::RunObstacleAvoidance(Vector2f& vel_cmd, float& ang_vel_cmd) { if (best_path == nullptr) { if (debug) printf("No best path found\n"); // No valid path found! - // TODO: Change this to rotate instead of halt Eigen::Vector2f prev_local_target = local_target_; - Eigen::Vector2f temp_target = GetPathGoal(params_.carrot_dist/2); + // Eigen::Vector2f temp_target = GetPathGoal(params_.carrot_dist/2); + Eigen::Vector2f temp_target = GetPathGoal(params_.recovery_carrot_dist); local_target_ = Rotation2Df(-robot_angle_) * (temp_target - robot_loc_); TurnInPlace(vel_cmd, ang_vel_cmd); local_target_ = prev_local_target; diff --git a/src/navigation/navigation_main.cc b/src/navigation/navigation_main.cc index cbb94b1..8671215 100644 --- a/src/navigation/navigation_main.cc +++ b/src/navigation/navigation_main.cc @@ -799,10 +799,11 @@ void LoadConfig(navigation::NavigationParameters* params) { REAL_PARAM(carrot_dist); REAL_PARAM(lidar_range_min); REAL_PARAM(lidar_range_max); - REAL_PARAM(replan_carrot_dist); + REAL_PARAM(replan_dist); REAL_PARAM(object_lifespan); REAL_PARAM(inflation_coeff); REAL_PARAM(distance_weight); + REAL_PARAM(recovery_carrot_dist); config_reader::ConfigReader reader({FLAGS_robot_config}); params->dt = CONFIG_dt; @@ -844,10 +845,11 @@ void LoadConfig(navigation::NavigationParameters* params) { params->carrot_dist = CONFIG_carrot_dist; params->lidar_range_min = CONFIG_lidar_range_min; params->lidar_range_max = CONFIG_lidar_range_max; - params->replan_carrot_dist = CONFIG_replan_carrot_dist; + params->replan_dist = CONFIG_replan_dist; params->object_lifespan = CONFIG_object_lifespan; params->inflation_coeff = CONFIG_inflation_coeff; params->distance_weight = CONFIG_distance_weight; + params->recovery_carrot_dist = CONFIG_recovery_carrot_dist; // TODO Rather than loading camera homography from a file, compute it from camera transformation info LoadCameraCalibrationCV(CONFIG_camera_calibration_path, ¶ms->K, ¶ms->D, ¶ms->H); diff --git a/src/navigation/navigation_parameters.h b/src/navigation/navigation_parameters.h index b7f1712..e8a5187 100644 --- a/src/navigation/navigation_parameters.h +++ b/src/navigation/navigation_parameters.h @@ -120,14 +120,17 @@ struct NavigationParameters { float lidar_range_max; // Distance between intermediate goal and global carrot before replanning - float replan_carrot_dist; + float replan_dist; // How long an object should stay in the costmap if not continuously observed float object_lifespan; + // Coefficient for exponential inflation cost float inflation_coeff; - + // Weight for distance cost vs. inflation cost float distance_weight; + // Distance of carrot when using turn in place recovery + float recovery_carrot_dist; cv::Mat K; @@ -167,10 +170,11 @@ struct NavigationParameters { global_costmap_origin_y(-50), lidar_range_min(0.1), lidar_range_max(10), - replan_carrot_dist(2), + replan_dist(2), object_lifespan(5), inflation_coeff(5), - distance_weight(2){ + distance_weight(2), + recovery_carrot_dist(0.5){ } }; } // namespace navigation