Skip to content

Commit

Permalink
update param names, change goal for turn in place recovery
Browse files Browse the repository at this point in the history
  • Loading branch information
@alexcliu
alexcliu committed Apr 24, 2024
1 parent 1d25342 commit aa61b70
Show file tree
Hide file tree
Showing 5 changed files with 89 additions and 86 deletions.
28 changes: 14 additions & 14 deletions config/navigation.lua
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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 = {
Expand Down
64 changes: 34 additions & 30 deletions src/navigation/navigation.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<int>(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<int>(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<int>(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<int>(std::round(params_.global_costmap_size_x/params_.global_costmap_resolution));
int global_costmap_size_y = static_cast<int>(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, &params_);

LoadVectorMap(map_file);
Expand Down Expand Up @@ -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<int>(unsigned_mx);
int my = static_cast<int>(unsigned_my);
for (int j = -cell_inflation_size; j <= cell_inflation_size; j++){
Expand All @@ -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));
Expand Down Expand Up @@ -311,7 +312,6 @@ void Navigation::SetOverride(const Vector2f& loc, float angle) {
}

void Navigation::Resume() {
cout << "resume goto" << endl;
nav_state_ = NavigationState::kGoto;
}

Expand Down Expand Up @@ -611,17 +611,18 @@ vector<GraphDomain::State> Navigation::Plan(const Vector2f& initial,
}

costmap_.indexToCells(current_index, mx, my);
vector<int> 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<int> neighbors_x {-1, -1, -1, 0, 0, 1, 1, 1};
// vector<int> neighbors_y {-1, 0, 1, -1, 1, -1, 0, 1};
vector<int> neighbors_x {-1, 0, 0, 1, -1, -1, 1, 1};
vector<int> 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<int>(costmap_.getSizeInCellsX()) && new_col >= 0
&& new_col < static_cast<int>(costmap_.getSizeInCellsY())) {


double wx, wy;
costmap_.mapToWorld(new_row, new_col, wx, wy);
wx += robot_loc_.x();
Expand All @@ -632,7 +633,7 @@ vector<GraphDomain::State> 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;
Expand Down Expand Up @@ -722,15 +723,15 @@ 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;
}
}
return true;
}

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;
Expand All @@ -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) {
Expand Down Expand Up @@ -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;
}
Expand Down Expand Up @@ -1077,7 +1083,7 @@ vector<Vector2f> Navigation::GetPredictedCloud() {
}

float Navigation::GetCarrotDist() {
return params_.intermediate_carrot_dist;
return params_.carrot_dist;
}

float Navigation::GetObstacleMargin() {
Expand Down Expand Up @@ -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();
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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));
Expand Down Expand Up @@ -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_);
}
}
Expand All @@ -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_) <
Expand Down Expand Up @@ -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) {
Expand Down
2 changes: 1 addition & 1 deletion src/navigation/navigation.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -136,7 +136,7 @@ class Navigation {
std::vector<GraphDomain::State> 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);
Expand Down
38 changes: 19 additions & 19 deletions src/navigation/navigation_main.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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));
}
Expand Down Expand Up @@ -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);
Expand All @@ -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);
Expand All @@ -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;
Expand All @@ -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;
Expand Down

0 comments on commit aa61b70

Please sign in to comment.