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

planner_cspace: reconstuct GridAstarModel3D only when necessary #751

Merged
merged 2 commits into from
Feb 21, 2025
Merged

planner_cspace: reconstuct GridAstarModel3D only when necessary #751

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
0465a76
Make cbParameter faster
nhatao Feb 19, 2025
fe098d5
Add local_range
nhatao Feb 20, 2025
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
7 changes: 6 additions & 1 deletion 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 @@ -90,7 +90,7 @@ class GridAstarModel3D : public GridAstarModelBase<3, 2>
const BlockMemGridmapBase<char, 3, 2>& cm_;
const BlockMemGridmapBase<char, 3, 2>& cm_hyst_;
const BlockMemGridmapBase<char, 3, 2>& cm_rough_;
const CostCoeff& cc_;
CostCoeff cc_;
int range_;
RotationCache rot_cache_;
MotionCache motion_cache_;
Expand All @@ -112,6 +112,11 @@ class GridAstarModel3D : public GridAstarModelBase<3, 2>
const int range,
const float path_interpolation_resolution = 0.5,
const float grid_enumeration_resolution = 0.1);
void updateCostParameters(
const Vecf& euclid_cost_coef,
const CostCoeff& cc,
const int local_range);

void enableHysteresis(const bool enable);
void createEuclidCostCache();
float euclidCost(const Vec& v) const;
Expand Down
16 changes: 13 additions & 3 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 @@ -103,9 +103,7 @@ GridAstarModel3D::GridAstarModel3D(
min_boundary_;
ROS_INFO("x:%d, y:%d grids around the boundary is ignored on path search", min_boundary_[0], min_boundary_[1]);

createEuclidCostCache();

motion_primitives_ = MotionPrimitiveBuilder::build(map_info_, cc_, range_);
updateCostParameters(euclid_cost_coef_, cc_, local_range_);
search_list_rough_.clear();
Vec d;
for (d[0] = -range_; d[0] <= range_; d[0]++)
Expand All @@ -120,6 +118,18 @@ GridAstarModel3D::GridAstarModel3D(
}
}

void GridAstarModel3D::updateCostParameters(
const Vecf& euclid_cost_coef,
const CostCoeff& cc,
const int local_range)
{
euclid_cost_coef_ = euclid_cost_coef;
cc_ = cc;
local_range_ = local_range;
createEuclidCostCache();
motion_primitives_ = MotionPrimitiveBuilder::build(map_info_, cc_, range_);
}

void GridAstarModel3D::enableHysteresis(const bool enable)
{
hysteresis_ = enable;
Expand Down
29 changes: 19 additions & 10 deletions planner_cspace/src/planner_3d.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -964,7 +964,7 @@ class Planner3dNode
map_info_.angular_resolution != msg->info.angular_resolution)
{
map_info_ = msg->info;
resetGridAstarModel();
resetGridAstarModel(true);
ROS_DEBUG("Search model updated");
}
else
Expand Down Expand Up @@ -1291,8 +1291,9 @@ class Planner3dNode
parameter_server_.setCallback(boost::bind(&Planner3dNode::cbParameter, this, _1, _2));
}

void resetGridAstarModel()
void resetGridAstarModel(const bool force_reset)
{
const int previous_range = range_;
range_ = static_cast<int>(search_range_ / map_info_.linear_resolution);
hist_ignore_range_ = std::lround(hist_ignore_range_f_ / map_info_.linear_resolution);
hist_ignore_range_max_ = std::lround(hist_ignore_range_max_f_ / map_info_.linear_resolution);
Expand All @@ -1305,13 +1306,21 @@ class Planner3dNode
goal_tolerance_ang_ = std::lround(goal_tolerance_ang_f_ / map_info_.angular_resolution);
cc_.angle_resolution_aspect_ = 2.0 / tanf(map_info_.angular_resolution);

model_.reset(
new GridAstarModel3D(
map_info_,
ec_,
local_range_,
cost_estim_cache_.gridmap(), cm_, cm_hyst_, cm_rough_,
cc_, range_, path_interpolation_resolution_, grid_enumeration_resolution_));
const bool reset_required = force_reset || (previous_range != range_);
Copy link
Owner

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think the change of the map_info_ is better to be checked here instead of passing as an argument to prevent distributing the logic.

Copy link
Owner

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

But doing this might need larger change.
(pass new map_info, parameters as arguments of resetGridAstarModel and skip calling GridAstarModel3D::updateCostParameters when unnecessary)
So, it looks ok to merge it as is for now.

Copy link
Owner

@at-wat at-wat Feb 21, 2025 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Copy link
Owner

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

note: path_interpolation_resolution_ and grid_enumeration_resolution_ aren't dynamic reconfigurable now

if (reset_required)
{
model_.reset(
new GridAstarModel3D(
map_info_,
ec_,
local_range_,
cost_estim_cache_.gridmap(), cm_, cm_hyst_, cm_rough_,
cc_, range_, path_interpolation_resolution_, grid_enumeration_resolution_));
}
else
{
model_->updateCostParameters(ec_, cc_, local_range_);
}
}

void cbParameter(const Planner3DConfig& config, const uint32_t /* level */)
Expand Down Expand Up @@ -1371,7 +1380,7 @@ class Planner3dNode

if (map_info_.linear_resolution != 0.0 && map_info_.angular_resolution != 0.0)
{
resetGridAstarModel();
resetGridAstarModel(false);
const Astar::Vec size2d(static_cast<int>(map_info_.width), static_cast<int>(map_info_.height), 1);
const DistanceMap::Params dmp =
{
Expand Down