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

Refactors possible_inscribed_cost to possible_circumscribed_cost in collision checker for smac_planner and mppi_controller #4118

Merged
merged 1 commit into from
Feb 17, 2024
Merged

Refactors possible_inscribed_cost to possible_circumscribed_cost in collision checker for smac_planner and mppi_controller #4118

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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: 1 addition & 1 deletion nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -78,7 +78,7 @@ class CostCritic : public CriticFunction
protected:
nav2_costmap_2d::FootprintCollisionChecker<nav2_costmap_2d::Costmap2D *>
collision_checker_{nullptr};
float possibly_inscribed_cost_;
float possible_collision_cost_;

bool consider_footprint_{true};
float circumscribed_radius_{0};
Expand Down
2 changes: 1 addition & 1 deletion nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacles_critic.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -90,7 +90,7 @@ class ObstaclesCritic : public CriticFunction
float collision_cost_{0};
float inflation_scale_factor_{0}, inflation_radius_{0};

float possibly_inscribed_cost_;
float possible_collision_cost_;
float collision_margin_distance_;
float near_goal_distance_;
float circumscribed_cost_{0}, circumscribed_radius_{0};
Expand Down
8 changes: 4 additions & 4 deletions nav2_mppi_controller/src/critics/cost_critic.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -34,9 +34,9 @@ void CostCritic::initialize()
weight_ /= 254.0f;

collision_checker_.setCostmap(costmap_);
possibly_inscribed_cost_ = findCircumscribedCost(costmap_ros_);
possible_collision_cost_ = findCircumscribedCost(costmap_ros_);

if (possibly_inscribed_cost_ < 1.0f) {
if (possible_collision_cost_ < 1.0f) {
RCLCPP_ERROR(
logger_,
"Inflation layer either not found or inflation is not set sufficiently for "
Expand Down Expand Up @@ -96,7 +96,7 @@ void CostCritic::score(CriticData & data)

if (consider_footprint_) {
// footprint may have changed since initialization if user has dynamic footprints
possibly_inscribed_cost_ = findCircumscribedCost(costmap_ros_);
possible_collision_cost_ = findCircumscribedCost(costmap_ros_);
}

// If near the goal, don't apply the preferential term since the goal is near obstacles
Expand Down Expand Up @@ -161,7 +161,7 @@ bool CostCritic::inCollision(float cost, float x, float y, float theta)

// If consider_footprint_ check footprint scort for collision
if (consider_footprint_ &&
(cost >= possibly_inscribed_cost_ || possibly_inscribed_cost_ < 1.0f))
(cost >= possible_collision_cost_ || possible_collision_cost_ < 1.0f))
{
cost = static_cast<float>(collision_checker_.footprintCostAtPose(
x, y, theta, costmap_ros_->getRobotFootprint()));
Expand Down
8 changes: 4 additions & 4 deletions nav2_mppi_controller/src/critics/obstacles_critic.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -32,9 +32,9 @@ void ObstaclesCritic::initialize()
getParam(inflation_layer_name_, "inflation_layer_name", std::string(""));

collision_checker_.setCostmap(costmap_);
possibly_inscribed_cost_ = findCircumscribedCost(costmap_ros_);
possible_collision_cost_ = findCircumscribedCost(costmap_ros_);

if (possibly_inscribed_cost_ < 1.0f) {
if (possible_collision_cost_ < 1.0f) {
RCLCPP_ERROR(
logger_,
"Inflation layer either not found or inflation is not set sufficiently for "
Expand Down Expand Up @@ -111,7 +111,7 @@ void ObstaclesCritic::score(CriticData & data)

if (consider_footprint_) {
// footprint may have changed since initialization if user has dynamic footprints
possibly_inscribed_cost_ = findCircumscribedCost(costmap_ros_);
possible_collision_cost_ = findCircumscribedCost(costmap_ros_);
}

// If near the goal, don't apply the preferential term since the goal is near obstacles
Expand Down Expand Up @@ -212,7 +212,7 @@ CollisionCost ObstaclesCritic::costAtPose(float x, float y, float theta)
cost = collision_checker_.pointCost(x_i, y_i);

if (consider_footprint_ &&
(cost >= possibly_inscribed_cost_ || possibly_inscribed_cost_ < 1.0f))
(cost >= possible_collision_cost_ || possible_collision_cost_ < 1.0f))
{
cost = static_cast<float>(collision_checker_.footprintCostAtPose(
x, y, theta, costmap_ros_->getRobotFootprint()));
Expand Down
4 changes: 2 additions & 2 deletions nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -66,7 +66,7 @@ class GridCollisionChecker
void setFootprint(
const nav2_costmap_2d::Footprint & footprint,
const bool & radius,
const double & possible_inscribed_cost);
const double & possible_collision_cost);

/**
* @brief Check if in collision with costmap and footprint at pose
Expand Down Expand Up @@ -130,7 +130,7 @@ class GridCollisionChecker
float footprint_cost_;
bool footprint_is_radius_;
std::vector<float> angles_;
float possible_inscribed_cost_{-1};
float possible_collision_cost_{-1};
rclcpp::Logger logger_{rclcpp::get_logger("SmacPlannerCollisionChecker")};
rclcpp::Clock::SharedPtr clock_;
};
Expand Down
8 changes: 4 additions & 4 deletions nav2_smac_planner/src/collision_checker.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -51,9 +51,9 @@ GridCollisionChecker::GridCollisionChecker(
void GridCollisionChecker::setFootprint(
const nav2_costmap_2d::Footprint & footprint,
const bool & radius,
const double & possible_inscribed_cost)
const double & possible_collision_cost)
{
possible_inscribed_cost_ = static_cast<float>(possible_inscribed_cost);
possible_collision_cost_ = static_cast<float>(possible_collision_cost);
footprint_is_radius_ = radius;

// Use radius, no caching required
Expand Down Expand Up @@ -113,8 +113,8 @@ bool GridCollisionChecker::inCollision(
footprint_cost_ = static_cast<float>(costmap_->getCost(
static_cast<unsigned int>(x + 0.5), static_cast<unsigned int>(y + 0.5)));

if (footprint_cost_ < possible_inscribed_cost_) {
if (possible_inscribed_cost_ > 0) {
if (footprint_cost_ < possible_collision_cost_) {
if (possible_collision_cost_ > 0) {
return false;
} else {
RCLCPP_ERROR_THROTTLE(
Expand Down
Loading