From aa61b7045e58609e4879226944e28318bd29e5c1 Mon Sep 17 00:00:00 2001 From: zdeng-UTexas Date: Wed, 24 Apr 2024 15:17:28 -0500 Subject: [PATCH] update param names, change goal for turn in place recovery --- config/navigation.lua | 28 +++++------ src/navigation/navigation.cc | 64 ++++++++++++++------------ src/navigation/navigation.h | 2 +- src/navigation/navigation_main.cc | 38 +++++++-------- src/navigation/navigation_parameters.h | 43 +++++++++-------- 5 files changed, 89 insertions(+), 86 deletions(-) diff --git a/config/navigation.lua b/config/navigation.lua index 82ab16f..9a38dad 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 = 15; + carrot_dist = 1; system_latency = 0.24; obstacle_margin = 0.15; num_options = 31; @@ -40,22 +40,22 @@ NavigationParameters = { camera_calibration_path = "config/camera_calibration_kinect.yaml"; model_path = "../preference_learning_models/jit_cost_model_outdoor_6dim.pt"; evaluator_type = "linear"; - intermediate_carrot_dist = 1; - local_costmap_inflation_size = 1; + intermediate_goal_dist = 10; + max_inflation_radius = 1; + min_inflation_radius = 0.2; local_costmap_resolution = 0.1; - local_costmap_radius = 10; - replan_inflation_size = 0.3; - global_costmap_inflation_size = 1; + local_costmap_size = 20; global_costmap_resolution = 0.1; - global_costmap_radius = 50; - global_costmap_origin_x = -20; - global_costmap_origin_y = -20; - range_min = 0.1; - range_max = 10.0; + global_costmap_size_x = 100; + global_costmap_size_y = 100; + global_costmap_origin_x = -10; + global_costmap_origin_y = 0; + lidar_range_min = 0.1; + lidar_range_max = 10.0; replan_carrot_dist = 2; - object_lifespan = 5; - inflation_coeff = 7; - distance_weight = 2; + object_lifespan = 15; + inflation_coeff = 8; + distance_weight = 3; }; AckermannSampler = { diff --git a/src/navigation/navigation.cc b/src/navigation/navigation.cc index d7629b6..31e4d82 100644 --- a/src/navigation/navigation.cc +++ b/src/navigation/navigation.cc @@ -173,10 +173,11 @@ void Navigation::Initialize(const NavigationParameters& params, const string& map_file) { // Initialize status message params_ = params; - int local_costmap_size = 2*static_cast(std::round(params_.local_costmap_radius/params_.local_costmap_resolution)); - costmap_ = costmap_2d::Costmap2D(local_costmap_size, local_costmap_size, params_.local_costmap_resolution, -params.local_costmap_radius, -params.local_costmap_radius); - int global_costmap_size = 2*static_cast(std::round(params_.global_costmap_radius/params_.global_costmap_resolution)); - global_costmap_ = costmap_2d::Costmap2D(global_costmap_size, global_costmap_size, params_.global_costmap_resolution, params.global_costmap_origin_x, params.global_costmap_origin_y); + int local_costmap_size = 2*static_cast(std::round(params_.local_costmap_size/params_.local_costmap_resolution)); + costmap_ = costmap_2d::Costmap2D(local_costmap_size, local_costmap_size, params_.local_costmap_resolution, -params.local_costmap_size/2, -params.local_costmap_size/2); + int global_costmap_size_x = static_cast(std::round(params_.global_costmap_size_x/params_.global_costmap_resolution)); + int global_costmap_size_y = static_cast(std::round(params_.global_costmap_size_y/params_.global_costmap_resolution)); + global_costmap_ = costmap_2d::Costmap2D(global_costmap_size_x, global_costmap_size_y, params_.global_costmap_resolution, params.global_costmap_origin_x, params.global_costmap_origin_y); planning_domain_ = GraphDomain(map_file, ¶ms_); LoadVectorMap(map_file); @@ -236,7 +237,7 @@ void Navigation::LoadVectorMap(const string& map_file){ //Assume map is given as uint32_t unsigned_mx, unsigned_my; bool in_map = global_costmap_.worldToMap(costmap_point.x(), costmap_point.y(), unsigned_mx, unsigned_my); if(in_map){ - int cell_inflation_size = std::ceil(params_.global_costmap_inflation_size/global_costmap_.getResolution()); + int cell_inflation_size = std::ceil(params_.max_inflation_radius/global_costmap_.getResolution()); int mx = static_cast(unsigned_mx); int my = static_cast(unsigned_my); for (int j = -cell_inflation_size; j <= cell_inflation_size; j++){ @@ -248,11 +249,11 @@ void Navigation::LoadVectorMap(const string& map_file){ //Assume map is given as if(j == 0 && k == 0){ cost = costmap_2d::LETHAL_OBSTACLE; } - else if(dist <= params_.replan_inflation_size){ + else if(dist <= params_.min_inflation_radius){ cost = costmap_2d::INSCRIBED_INFLATED_OBSTACLE; } else{ - cost = std::ceil(std::exp(-1 * params_.inflation_coeff * (dist - params_.replan_inflation_size)) * (costmap_2d::INSCRIBED_INFLATED_OBSTACLE-1)); + cost = std::ceil(std::exp(-1 * params_.inflation_coeff * (dist - params_.min_inflation_radius)) * (costmap_2d::INSCRIBED_INFLATED_OBSTACLE-1)); } global_costmap_.setCost(mx + j, my + k, std::max(cost, global_costmap_.getCost(mx + j, my + k))); inflation_cells[global_costmap_.getIndex(mx + j, my + k)] = std::max(cost, global_costmap_.getCost(mx + j, my + k)); @@ -311,7 +312,6 @@ void Navigation::SetOverride(const Vector2f& loc, float angle) { } void Navigation::Resume() { - cout << "resume goto" << endl; nav_state_ = NavigationState::kGoto; } @@ -611,17 +611,18 @@ vector Navigation::Plan(const Vector2f& initial, } costmap_.indexToCells(current_index, mx, my); - vector neighbors {-1, 0, 1, 0}; - for(size_t i = 0; i < neighbors.size(); i++){ - int new_row = mx + neighbors[i]; - int new_col = my + neighbors[(i+1)%4]; + // vector neighbors_x {-1, -1, -1, 0, 0, 1, 1, 1}; + // vector neighbors_y {-1, 0, 1, -1, 1, -1, 0, 1}; + vector neighbors_x {-1, 0, 0, 1, -1, -1, 1, 1}; + vector neighbors_y {0, -1, 1, 0, -1, 1, -1, 1}; + for(size_t i = 0; i < neighbors_x.size(); i++){ + int new_row = mx + neighbors_x[i]; + int new_col = my + neighbors_y[i]; uint32_t neighbor_index = costmap_.getIndex(new_row, new_col); //TODO: fix when robot is in an obstacle in the costmap if(new_row >= 0 && new_row < static_cast(costmap_.getSizeInCellsX()) && new_col >= 0 && new_col < static_cast(costmap_.getSizeInCellsY())) { - - double wx, wy; costmap_.mapToWorld(new_row, new_col, wx, wy); wx += robot_loc_.x(); @@ -632,7 +633,7 @@ vector Navigation::Plan(const Vector2f& initial, if(in_global_map){ max_costmap_cost = std::max(costmap_.getCost(new_row, new_col), global_costmap_.getCost(global_mx, global_my)); } - float new_cost = cost[current_index] + params_.distance_weight + max_costmap_cost; + float new_cost = cost[current_index] + params_.distance_weight * sqrt(pow(neighbors_x[i], 2) + pow(neighbors_y[i], 2)) + max_costmap_cost; if(cost.count(neighbor_index) == 0 || new_cost < cost[neighbor_index]){ cost[neighbor_index] = new_cost; parent[neighbor_index] = current_index; @@ -722,7 +723,7 @@ bool Navigation::IntermediatePlanStillValid(){ uint32_t mx, my; Vector2f relative_path_location = plan_path_[i].loc - robot_loc_; bool in_map = costmap_.worldToMap(relative_path_location.x(), relative_path_location.y(), mx, my); - if(in_map && costmap_.getCost(mx, my) == costmap_2d::LETHAL_OBSTACLE){ + if(in_map && (costmap_.getCost(mx, my) == costmap_2d::LETHAL_OBSTACLE || costmap_.getCost(mx, my) == costmap_2d::INSCRIBED_INFLATED_OBSTACLE) ){ return false; } } @@ -730,7 +731,7 @@ bool Navigation::IntermediatePlanStillValid(){ } bool Navigation::GetGlobalCarrot(Vector2f& carrot) { - for(float carrot_dist = params_.carrot_dist; carrot_dist > params_.intermediate_carrot_dist; carrot_dist -= 0.5){ + for(float carrot_dist = params_.intermediate_goal_dist; carrot_dist > params_.carrot_dist; carrot_dist -= 0.5){ if(GetCarrot(carrot, true, carrot_dist)){ Vector2f robot_frame_carrot = carrot - robot_loc_; uint32_t mx, my; @@ -743,11 +744,10 @@ bool Navigation::GetGlobalCarrot(Vector2f& carrot) { } return false; - // return GetCarrot(carrot, true, params_.carrot_dist); } -bool Navigation::GetIntermediateCarrot(Vector2f& carrot) { - return GetCarrot(carrot, false, params_.intermediate_carrot_dist); +bool Navigation::GetLocalCarrot(Vector2f& carrot) { + return GetCarrot(carrot, false, params_.carrot_dist); } bool Navigation::GetCarrot(Vector2f& carrot, bool global, float carrot_dist) { @@ -898,7 +898,13 @@ void Navigation::RunObstacleAvoidance(Vector2f& vel_cmd, float& ang_vel_cmd) { if (debug) printf("No best path found\n"); // No valid path found! // TODO: Change this to rotate instead of halt + Eigen::Vector2f local_target = local_target_; + Eigen::Vector2f temp_target; + GetCarrot(temp_target, false, params_.carrot_dist/2); + local_target = Rotation2Df(-robot_angle_) * (temp_target - robot_loc_); + // local_target_ = Rotation2Df(-robot_angle_) * (plan_path_[plan_path_.size() - 1].loc - robot_loc_); TurnInPlace(vel_cmd, ang_vel_cmd); + local_target_ = local_target; // Halt(vel_cmd, ang_vel_cmd); return; } @@ -1077,7 +1083,7 @@ vector Navigation::GetPredictedCloud() { } float Navigation::GetCarrotDist() { - return params_.intermediate_carrot_dist; + return params_.carrot_dist; } float Navigation::GetObstacleMargin() { @@ -1161,7 +1167,7 @@ bool Navigation::Run(const double& time, return true; } - int cell_inflation_size = std::ceil(params_.local_costmap_inflation_size/costmap_.getResolution()); + int cell_inflation_size = std::ceil(params_.max_inflation_radius/costmap_.getResolution()); int x_max = costmap_.getSizeInCellsX(); int y_max = costmap_.getSizeInCellsY(); @@ -1195,7 +1201,7 @@ bool Navigation::Run(const double& time, bool in_map = costmap_.worldToMap(relative_location_map_frame.x(), relative_location_map_frame.y(), unsigned_mx, unsigned_my); //TODO: change max distance based on lidar to base link transformation - if (in_map && relative_location_map_frame.norm() < (params_.range_max - params_.robot_length) && relative_location_map_frame.norm() > params_.range_min){ + if (in_map && relative_location_map_frame.norm() < (params_.lidar_range_max - params_.robot_length) && relative_location_map_frame.norm() > params_.lidar_range_min){ uint32_t index = costmap_.getIndex(unsigned_mx, unsigned_my); double wx, wy; costmap_.mapToWorld(unsigned_mx, unsigned_my, wx, wy); @@ -1296,11 +1302,11 @@ bool Navigation::Run(const double& time, if(j == 0 && k == 0){ cost = costmap_2d::LETHAL_OBSTACLE; } - else if(dist <= params_.replan_inflation_size){ + else if(dist <= params_.min_inflation_radius){ cost = costmap_2d::INSCRIBED_INFLATED_OBSTACLE; } else{ - cost = std::ceil(std::exp(-1 * params_.inflation_coeff * (dist - params_.replan_inflation_size)) * (costmap_2d::INSCRIBED_INFLATED_OBSTACLE-1)); + cost = std::ceil(std::exp(-1 * params_.inflation_coeff * (dist - params_.min_inflation_radius)) * (costmap_2d::INSCRIBED_INFLATED_OBSTACLE-1)); } costmap_.setCost(mx + j, my + k, std::max(cost, costmap_.getCost(mx + j, my + k))); inflation_cells[costmap_.getIndex(mx + j, my + k)] = std::max(cost, costmap_.getCost(mx + j, my + k)); @@ -1342,13 +1348,12 @@ bool Navigation::Run(const double& time, if (nav_state_ == NavigationState::kGoto) { // Get Carrot and check if done Vector2f carrot(0, 0); - bool foundCarrot = GetIntermediateCarrot(carrot); + bool foundCarrot = GetLocalCarrot(carrot); if (!foundCarrot) { Halt(cmd_vel, cmd_angle_vel); return false; } // Local Navigation - cout << "set local target" << endl; local_target_ = Rotation2Df(-robot_angle_) * (carrot - robot_loc_); } } @@ -1360,7 +1365,6 @@ bool Navigation::Run(const double& time, if (nav_state_ == NavigationState::kGoto && local_target_.squaredNorm() < Sq(params_.target_dist_tolerance) && robot_vel_.squaredNorm() < Sq(params_.target_vel_tolerance)) { - cout << "set turn in place" << endl; nav_state_ = NavigationState::kTurnInPlace; } else if (nav_state_ == NavigationState::kTurnInPlace && AngleDist(robot_angle_, nav_goal_angle_) < @@ -1409,8 +1413,8 @@ bool Navigation::Run(const double& time, local_target = override_target_; } const float theta = atan2(local_target.y(), local_target.x()); - if (local_target.squaredNorm() > params_.intermediate_carrot_dist) { - local_target = params_.intermediate_carrot_dist * local_target.normalized(); + if (local_target.squaredNorm() > params_.carrot_dist) { + local_target = params_.carrot_dist * local_target.normalized(); } if (!FLAGS_no_local) { if (fabs(theta) > params_.local_fov) { diff --git a/src/navigation/navigation.h b/src/navigation/navigation.h index eb4d074..1d4827f 100644 --- a/src/navigation/navigation.h +++ b/src/navigation/navigation.h @@ -136,7 +136,7 @@ class Navigation { std::vector GetGlobalPath(); bool GetGlobalCarrot(Eigen::Vector2f& carrot); - bool GetIntermediateCarrot(Eigen::Vector2f& carrot); + bool GetLocalCarrot(Eigen::Vector2f& carrot); bool GetCarrot(Eigen::Vector2f& carrot, bool global, float carrot_dist); // Enable or disable autonomy. void Enable(bool enable); diff --git a/src/navigation/navigation_main.cc b/src/navigation/navigation_main.cc index 755c2f2..f5fb15f 100644 --- a/src/navigation/navigation_main.cc +++ b/src/navigation/navigation_main.cc @@ -469,7 +469,7 @@ void PublishPath() { global_viz_msg_); } Vector2f carrot; - bool foundCarrot = navigation_.GetIntermediateCarrot(carrot); + bool foundCarrot = navigation_.GetLocalCarrot(carrot); if (foundCarrot) { carrot_pub_.publish(CarrotToNavMsgsPath(carrot)); } @@ -768,7 +768,7 @@ void LoadConfig(navigation::NavigationParameters* params) { REAL_PARAM(max_angular_accel); REAL_PARAM(max_angular_decel); REAL_PARAM(max_angular_speed); - REAL_PARAM(carrot_dist); + REAL_PARAM(intermediate_goal_dist); REAL_PARAM(system_latency); REAL_PARAM(obstacle_margin); NATURALNUM_PARAM(num_options); @@ -788,17 +788,17 @@ void LoadConfig(navigation::NavigationParameters* params) { STRING_PARAM(evaluator_type); STRING_PARAM(camera_calibration_path); REAL_PARAM(local_costmap_resolution); - REAL_PARAM(local_costmap_inflation_size); - REAL_PARAM(local_costmap_radius); - REAL_PARAM(replan_inflation_size); + REAL_PARAM(max_inflation_radius); + REAL_PARAM(local_costmap_size); + REAL_PARAM(min_inflation_radius); REAL_PARAM(global_costmap_resolution); - REAL_PARAM(global_costmap_inflation_size); - REAL_PARAM(global_costmap_radius); + REAL_PARAM(global_costmap_size_x); + REAL_PARAM(global_costmap_size_y); REAL_PARAM(global_costmap_origin_x); REAL_PARAM(global_costmap_origin_y); - REAL_PARAM(intermediate_carrot_dist); - REAL_PARAM(range_min); - REAL_PARAM(range_max); + REAL_PARAM(carrot_dist); + REAL_PARAM(lidar_range_min); + REAL_PARAM(lidar_range_max); REAL_PARAM(replan_carrot_dist); REAL_PARAM(object_lifespan); REAL_PARAM(inflation_coeff); @@ -814,7 +814,7 @@ void LoadConfig(navigation::NavigationParameters* params) { CONFIG_max_angular_accel, CONFIG_max_angular_decel, CONFIG_max_angular_speed); - params->carrot_dist = CONFIG_carrot_dist; + params->intermediate_goal_dist = CONFIG_intermediate_goal_dist; params->system_latency = CONFIG_system_latency; params->obstacle_margin = CONFIG_obstacle_margin; params->num_options = CONFIG_num_options; @@ -833,17 +833,17 @@ void LoadConfig(navigation::NavigationParameters* params) { params->model_path = CONFIG_model_path; params->evaluator_type = CONFIG_evaluator_type; params->local_costmap_resolution = CONFIG_local_costmap_resolution; - params->local_costmap_inflation_size = CONFIG_local_costmap_inflation_size; - params->local_costmap_radius = CONFIG_local_costmap_radius; - params->replan_inflation_size = CONFIG_replan_inflation_size; + params->max_inflation_radius = CONFIG_max_inflation_radius; + params->local_costmap_size = CONFIG_local_costmap_size; + params->min_inflation_radius = CONFIG_min_inflation_radius; params->global_costmap_resolution = CONFIG_global_costmap_resolution; - params->global_costmap_inflation_size = CONFIG_global_costmap_inflation_size; - params->global_costmap_radius = CONFIG_global_costmap_radius; + params->global_costmap_size_x = CONFIG_global_costmap_size_x; + params->global_costmap_size_y = CONFIG_global_costmap_size_y; params->global_costmap_origin_x = CONFIG_global_costmap_origin_x; params->global_costmap_origin_y = CONFIG_global_costmap_origin_y; - params->intermediate_carrot_dist = CONFIG_intermediate_carrot_dist; - params->range_min = CONFIG_range_min; - params->range_max = CONFIG_range_max; + 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->object_lifespan = CONFIG_object_lifespan; params->inflation_coeff = CONFIG_inflation_coeff; diff --git a/src/navigation/navigation_parameters.h b/src/navigation/navigation_parameters.h index e499daa..b7f1712 100644 --- a/src/navigation/navigation_parameters.h +++ b/src/navigation/navigation_parameters.h @@ -58,9 +58,8 @@ struct NavigationParameters { MotionLimits linear_limits; // Motion limits for angular motion. MotionLimits angular_limits; - // Distance of carrot from robot to compute local planner goal from - // global plan. - float carrot_dist; + // Distance along global plan to find intermediate goal + float intermediate_goal_dist; // System latency in seconds, including sensing latency, processing latency, // and actuation latency. float system_latency; @@ -98,27 +97,27 @@ struct NavigationParameters { std::string model_path; // Distance of carrot along intermediate path to compute local planner goal - float intermediate_carrot_dist; + float carrot_dist; // Robot buffer/inflation size - float local_costmap_inflation_size; + float max_inflation_radius; // Distance between grid locations in costmap float local_costmap_resolution; // Costmap size in each direction - float local_costmap_radius; + float local_costmap_size; // Minimum inflation distance to change intermediate plan - float replan_inflation_size; + float min_inflation_radius; // Same as local costmap parameters but for global costmap - float global_costmap_inflation_size; float global_costmap_resolution; - float global_costmap_radius; + float global_costmap_size_x; + float global_costmap_size_y; float global_costmap_origin_x; float global_costmap_origin_y; // Lidar scan min range - float range_min; + float lidar_range_min; // Lidar scan max range - float range_max; + float lidar_range_max; // Distance between intermediate goal and global carrot before replanning float replan_carrot_dist; @@ -140,7 +139,7 @@ struct NavigationParameters { dt(0.025), linear_limits(0.5, 0.5, 0.5), angular_limits(0.5, 0.5, 1.0), - carrot_dist(2.5), + intermediate_goal_dist(5), system_latency(0.24), obstacle_margin(0.15), num_options(41), @@ -156,18 +155,18 @@ struct NavigationParameters { target_angle_tolerance(0.05), use_kinect(true), evaluator_type("cost_map"), - intermediate_carrot_dist(2), - local_costmap_inflation_size(0.5), + carrot_dist(2), + max_inflation_radius(1), local_costmap_resolution(0.1), - local_costmap_radius(10), - replan_inflation_size(0.3), - global_costmap_inflation_size(0.5), + local_costmap_size(20), + min_inflation_radius(0.3), global_costmap_resolution(0.1), - global_costmap_radius(100), - global_costmap_origin_x(-100), - global_costmap_origin_y(-100), - range_min(0.1), - range_max(10), + global_costmap_size_x(100), + global_costmap_size_y(100), + global_costmap_origin_x(-50), + global_costmap_origin_y(-50), + lidar_range_min(0.1), + lidar_range_max(10), replan_carrot_dist(2), object_lifespan(5), inflation_coeff(5),