Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

planner_cspace: add new parameters for cost function #720

Merged
merged 8 commits into from
Sep 13, 2023
Merged

planner_cspace: add new parameters for cost function #720

Show file tree
Hide file tree
Changes from 6 commits
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
e2b660d
costmap_cspace: add cutoff_cost
nhatao Sep 11, 2023
8b992e2
Fix typo
nhatao Sep 11, 2023
1c3f058
planner_cspace: add new parameters
nhatao Sep 12, 2023
7887092
Revert "Fix typo"
nhatao Sep 13, 2023
a45c97f
Revert "costmap_cspace: add cutoff_cost"
nhatao Sep 13, 2023
d9b9df1
Merge branch 'master' of https://github.com/at-wat/neonavigation into…
nhatao Sep 13, 2023
627f00f
Update planner_cspace/cfg/Planner3D.cfg
nhatao Sep 13, 2023
054d081
Update comment
at-wat Sep 13, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 2 additions & 0 deletions planner_cspace/cfg/Planner3D.cfg
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,8 +19,10 @@ gen.add("weight_backward", double_t, 0, "", 0.9, 0.0, 1000.0)
gen.add("weight_ang_vel", double_t, 0, "", 1.0, 0.0, 1000.0)
gen.add("weight_costmap", double_t, 0, "", 50.0, 0.0, 1000.0)
gen.add("weight_costmap_turn", double_t, 0, "", 0.0, 0.0, 1000.0)
gen.add("weight_costmap_turn_heuristics", double_t, 0, "", 100.0, 0.0, 1000.0)
gen.add("weight_remembered", double_t, 0, "", 1000.0, 0.0, 1000.0)
gen.add("cost_in_place_turn", double_t, 0, "", 30.0, 0.0, 1000.0)
gen.add("turn_penalty_cost_threshold", int_t, 0, "", 0, 0, 100)
nhatao marked this conversation as resolved.
Show resolved Hide resolved
gen.add("hysteresis_max_dist", double_t, 0, "", 0.1, 0.0, 10.0)
gen.add("hysteresis_expand", double_t, 0, "", 0.1, 0.0, 10.0)
gen.add("weight_hysteresis", double_t, 0, "", 5.0, 0.0, 1000.0)
Expand Down
2 changes: 2 additions & 0 deletions planner_cspace/include/planner_cspace/planner_3d/grid_astar_model.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -56,6 +56,7 @@ class CostCoeff
float weight_ang_vel_;
float weight_costmap_;
float weight_costmap_turn_;
float weight_costmap_turn_heuristics_;
float weight_remembered_;
float weight_hysteresis_;
float in_place_turn_;
Expand All @@ -65,6 +66,7 @@ class CostCoeff
float max_vel_;
float max_ang_vel_;
float angle_resolution_aspect_;
int turn_penalty_cost_threshold_;
};

class GridAstarModel3D : public GridAstarModelBase<3, 2>
Expand Down
15 changes: 9 additions & 6 deletions planner_cspace/src/grid_astar_model_3dof.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -176,14 +176,17 @@ float GridAstarModel3D::cost(
const auto c = cm_[pos];
if (c > 99)
return -1;
sum += c;
if (c >= cc_.turn_penalty_cost_threshold_)
{
sum += c;
}
}

cost +=
sum * map_info_.angular_resolution * euclid_cost_coef_[2] / euclid_cost_coef_[0] +
sum * map_info_.angular_resolution * cc_.weight_costmap_turn_ / 100.0;
const float turn_cost_ratio = cc_.weight_costmap_turn_ / 100.0;
const float turn_heuristic_cost_ratio =
euclid_cost_coef_[2] / euclid_cost_coef_[0] * cc_.weight_costmap_turn_heuristics_ / 100.0;
const float turn_cost_multiplier = map_info_.angular_resolution * (turn_cost_ratio + turn_heuristic_cost_ratio);
// simplified from sum * map_info_.angular_resolution * abs(d[2]) * cc_.weight_costmap_turn_ / (100.0 * abs(d[2]))
at-wat marked this conversation as resolved.
Show resolved Hide resolved
return cc_.in_place_turn_ + cost;
return cost + cc_.in_place_turn_ + sum * turn_cost_multiplier;
}

const Vec d2(d[0] + range_, d[1] + range_, next[2]);
Expand Down
2 changes: 2 additions & 0 deletions planner_cspace/src/planner_3d.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1297,6 +1297,8 @@ class Planner3dNode
cc_.hysteresis_max_dist_ = config.hysteresis_max_dist;
cc_.hysteresis_expand_ = config.hysteresis_expand;
cc_.weight_hysteresis_ = config.weight_hysteresis;
cc_.weight_costmap_turn_heuristics_ = config.weight_costmap_turn_heuristics;
cc_.turn_penalty_cost_threshold_ = config.turn_penalty_cost_threshold;

goal_tolerance_lin_f_ = config.goal_tolerance_lin;
goal_tolerance_ang_f_ = config.goal_tolerance_ang;
Expand Down