Skip to content

Commit

Permalink
fix when goal is set in an obstacle
Browse files Browse the repository at this point in the history
  • Loading branch information
@alexcliu
alexcliu committed Apr 12, 2024
1 parent 71f1f8d commit e920bda
Show file tree
Hide file tree
Showing 4 changed files with 29 additions and 39 deletions.
16 changes: 8 additions & 8 deletions config/navigation.lua
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,19 +39,19 @@ 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 = 2;
local_costmap_inflation_size = 0.4;
intermediate_carrot_dist = 1;
local_costmap_inflation_size = 0.5;
local_costmap_resolution = 0.1;
local_costmap_radius = 10;
global_costmap_inflation_size = 0.4;
replan_inflation_size = 0.3;
global_costmap_inflation_size = 0.5;
global_costmap_resolution = 0.2;
global_costmap_radius = 70;
-- global_costmap_origin_x_positive = false;
global_costmap_origin_x = -50;
-- global_costmap_origin_y_positive = false;
global_costmap_origin_y = -50;
global_costmap_radius = 50;
global_costmap_origin_x = -10;
global_costmap_origin_y = 0;
range_min = 0.1;
range_max = 10.0;
replan_carrot_dist = 2;
};

AckermannSampler = {
Expand Down
44 changes: 14 additions & 30 deletions src/navigation/navigation.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -598,6 +598,7 @@ vector<GraphDomain::State> Navigation::Plan(const Vector2f& initial,
int new_col = my + neighbors[(i+1)%4];
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<int>(costmap_.getSizeInCellsX()) && new_col >= 0
&& new_col < static_cast<int>(costmap_.getSizeInCellsY()) && (costmap_.getCost(new_row, new_col) != costmap_2d::LETHAL_OBSTACLE)
&& (costmap_.getCost(new_row, new_col) != costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
Expand Down Expand Up @@ -683,16 +684,15 @@ bool Navigation::IntermediatePlanStillValid(){

if (plan_path_.size() < 2 || !intermediate_path_found_) return false;

// TODO: Add parameter for when to look for new goal or use different heuristic
// TODO: Add parameter for when to look for new goal or use different heuristic, may not be necessary with carrot replan
if ((nav_goal_loc_ - plan_path_[0].loc).norm() > sqrt(2 * params_.local_costmap_resolution) / 2
&& (robot_loc_ - plan_path_[0].loc).norm() < 1){
return false;
}

// TODO: add parameter for distance between intermediate path and global carrot to replan
Vector2f global_carrot;
GetGlobalCarrot(global_carrot);
if((intermediate_goal_ - global_carrot).norm() > 2){
if((intermediate_goal_ - global_carrot).norm() > params_.replan_carrot_dist){
return false;
}

Expand All @@ -710,9 +710,16 @@ 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){
if(GetCarrot(carrot, true, carrot_dist)){
return true;
Vector2f robot_frame_carrot = carrot - robot_loc_;
uint32_t mx, my;
bool in_map = costmap_.worldToMap(robot_frame_carrot.x(), robot_frame_carrot.y(), mx, my);
if(in_map && costmap_.getCost(mx, my) != costmap_2d::LETHAL_OBSTACLE
&& costmap_.getCost(mx, my) != costmap_2d::INSCRIBED_INFLATED_OBSTACLE){
return true;
}
}
}

return false;
// return GetCarrot(carrot, true, params_.carrot_dist);
}
Expand All @@ -722,15 +729,10 @@ bool Navigation::GetIntermediateCarrot(Vector2f& carrot) {
}

bool Navigation::GetCarrot(Vector2f& carrot, bool global, float carrot_dist) {
// float carrot_dist = params_.intermediate_carrot_dist;
// float carrot_dist = carrot_dist_test;

vector<GraphDomain::State> plan_path = plan_path_;
// if(global){
// plan_path = global_plan_path_;
// carrot_dist = params_.carrot_dist;
// }
// const float kSqCarrotDist = Sq(params_.carrot_dist);
if(global){
plan_path = global_plan_path_;
}
const float kSqCarrotDist = Sq(carrot_dist);

CHECK_GE(plan_path.size(), 2u);
Expand Down Expand Up @@ -1101,7 +1103,6 @@ bool Navigation::Run(const double& time,
Vector2f& cmd_vel,
float& cmd_angle_vel) {
const bool kDebug = FLAGS_v > 0;
cout << "begin run" << endl;
if (!initialized_) {
if (kDebug) printf("Parameters and maps not initialized\n");
return false;
Expand Down Expand Up @@ -1156,13 +1157,9 @@ bool Navigation::Run(const double& time,
costmap_.mapToWorld(robot_row, robot_col, robot_wx, robot_wy);
Vector2f robot_location(robot_wx, robot_wy);

cout << "add points to costmap" << endl;

// Add new points to costmap
for (size_t i = 0; i < point_cloud_.size(); i++){
if(point_cloud_[i].x() < -0.5f && point_cloud_[i].y() > 0){
cout << "point: (" << point_cloud_[i].x() << ", " << point_cloud_[i].y() << ")" << endl;
}
uint32_t unsigned_mx, unsigned_my;
Vector2f relative_location_map_frame = Rotation2Df(robot_angle_) * point_cloud_[i];
bool in_map = costmap_.worldToMap(relative_location_map_frame.x(), relative_location_map_frame.y(), unsigned_mx, unsigned_my);
Expand Down Expand Up @@ -1232,7 +1229,6 @@ bool Navigation::Run(const double& time,
iter++;
}

// TODO: test
// Add old obstacles that are still in map to new costmap
for (const auto& point : prev_obstacles_) {
uint32_t unsigned_mx, unsigned_my;
Expand Down Expand Up @@ -1260,7 +1256,6 @@ bool Navigation::Run(const double& time,
inflation_cells[costmap_.getIndex(mx + j, my + k)] = true;
else
inflation_cells[costmap_.getIndex(mx + j, my + k)] = false;
// inflation_cells.insert(costmap_.getIndex(mx + j, my + k));
}
}
}
Expand All @@ -1282,17 +1277,6 @@ bool Navigation::Run(const double& time,
costmap_obstacles_.push_back(Vector2f(wx, wy) + robot_loc_);
}

// for (const auto& index : inflation_cells) {
// uint32_t mx = 0;
// uint32_t my = 0;
// costmap_.indexToCells(index, mx, my);
// costmap_.setCost(mx, my, costmap_2d::LETHAL_OBSTACLE);

// double wx, wy;
// costmap_.mapToWorld(mx, my, wx, wy);
// costmap_obstacles_.push_back(Vector2f(wx, wy) + robot_loc_);
// }

// Record last seen locations of points
prev_robot_loc_ = robot_loc_;
prev_obstacles_.clear();
Expand Down
2 changes: 2 additions & 0 deletions src/navigation/navigation_main.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -799,6 +799,7 @@ void LoadConfig(navigation::NavigationParameters* params) {
REAL_PARAM(intermediate_carrot_dist);
REAL_PARAM(range_min);
REAL_PARAM(range_max);
REAL_PARAM(replan_carrot_dist);

config_reader::ConfigReader reader({FLAGS_robot_config});
params->dt = CONFIG_dt;
Expand Down Expand Up @@ -840,6 +841,7 @@ void LoadConfig(navigation::NavigationParameters* params) {
params->intermediate_carrot_dist = CONFIG_intermediate_carrot_dist;
params->range_min = CONFIG_range_min;
params->range_max = CONFIG_range_max;
params->replan_carrot_dist = CONFIG_replan_carrot_dist;

// TODO Rather than loading camera homography from a file, compute it from camera transformation info
LoadCameraCalibrationCV(CONFIG_camera_calibration_path, &params->K, &params->D, &params->H);
Expand Down
6 changes: 5 additions & 1 deletion src/navigation/navigation_parameters.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -120,6 +120,9 @@ struct NavigationParameters {
// Lidar scan max range
float range_max;

// Distance between intermediate goal and global carrot before replanning
float replan_carrot_dist;


cv::Mat K;
cv::Mat D;
Expand Down Expand Up @@ -157,7 +160,8 @@ struct NavigationParameters {
global_costmap_origin_x(-100),
global_costmap_origin_y(-100),
range_min(0.1),
range_max(10){
range_max(10),
replan_carrot_dist(2){
}
};
} // namespace navigation
Expand Down

0 comments on commit e920bda

Please sign in to comment.