Skip to content

Commit

Permalink
more updates with some TODOs
Browse files Browse the repository at this point in the history
  • Loading branch information
@SteveMacenski
SteveMacenski committed Mar 12, 2024
1 parent a0608bd commit 6f32322
Show file tree
Hide file tree
Showing 12 changed files with 195 additions and 111 deletions.
60 changes: 34 additions & 26 deletions nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -243,15 +243,15 @@ inline bool withinPositionGoalTolerance(
const models::Path & path)
{
const auto goal_idx = path.x.shape(0) - 1;
const auto goal_x = path.x(goal_idx);
const auto goal_y = path.y(goal_idx);
const float goal_x = path.x(goal_idx);
const float goal_y = path.y(goal_idx);

const auto pose_tolerance_sq = pose_tolerance * pose_tolerance;
const float pose_tolerance_sq = pose_tolerance * pose_tolerance;

auto dx = robot.position.x - goal_x;
auto dy = robot.position.y - goal_y;
const float dx = static_cast<float>(robot.position.x) - goal_x;
const float dy = static_cast<float>(robot.position.y) - goal_y;

auto dist_sq = dx * dx + dy * dy;
float dist_sq = dx * dx + dy * dy;

if (dist_sq < pose_tolerance_sq) {
return true;
Expand Down Expand Up @@ -345,8 +345,9 @@ inline size_t findPathTrajectoryInitialPoint(const CriticData & data)
float min_distance_by_path = std::numeric_limits<float>::max();
size_t min_id = 0;
for (size_t j = 0; j < dists.shape(0); j++) {
if (dists(j) < min_distance_by_path) {
min_distance_by_path = dists(j);
const float & dist = dists(j);
if (dist < min_distance_by_path) {
min_distance_by_path = dist;
min_id = j;
}
}
Expand Down Expand Up @@ -377,26 +378,22 @@ inline void findPathCosts(
unsigned int map_x, map_y;
const size_t path_segments_count = data.path.x.shape(0) - 1;
data.path_pts_valid = std::vector<bool>(path_segments_count, false);
const bool tracking_unknown = costmap_ros->getLayeredCostmap()->isTrackingUnknown();
for (unsigned int idx = 0; idx < path_segments_count; idx++) {
const auto path_x = data.path.x(idx);
const auto path_y = data.path.y(idx);
if (!costmap->worldToMap(path_x, path_y, map_x, map_y)) {
if (!costmap->worldToMap(data.path.x(idx), data.path.y(idx), map_x, map_y)) {
(*data.path_pts_valid)[idx] = false;
continue;
}

switch (costmap->getCost(map_x, map_y)) {
using namespace nav2_costmap_2d; // NOLINT
case (LETHAL_OBSTACLE):
case (nav2_costmap_2d::LETHAL_OBSTACLE):
(*data.path_pts_valid)[idx] = false;
continue;
case (INSCRIBED_INFLATED_OBSTACLE):
case (nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE):
(*data.path_pts_valid)[idx] = false;
continue;
case (NO_INFORMATION):
const bool is_tracking_unknown =
costmap_ros->getLayeredCostmap()->isTrackingUnknown();
(*data.path_pts_valid)[idx] = is_tracking_unknown ? true : false;
case (nav2_costmap_2d::NO_INFORMATION):
(*data.path_pts_valid)[idx] = tracking_unknown ? true : false;
continue;
}

Expand Down Expand Up @@ -699,21 +696,32 @@ inline unsigned int removePosesAfterFirstInversion(nav_msgs::msg::Path & path)
* @return dist Distance to look for
* @return init Starting index to indec from
*/
inline size_t findClosestPathPt(
const std::vector<float> & vec, const float dist, const size_t init)
inline unsigned int findClosestPathPt(
const std::vector<float> & vec, const float dist, const unsigned int init = 0)
{
for (size_t i = init; i != vec.size(); i++) {
if (vec[i] > dist) {
if (i > 0 && dist - vec[i - 1] < vec[i] - dist) {
float distim1 = init != 0u ? vec[init - 1] : vec[0];
float disti = 0.0f;
const unsigned int size = vec.size();
for (unsigned int i = init; i != size; i++) {
disti = vec[i];
if (disti > dist) {
if (i > 0 && dist - distim1 < disti - dist) {
return i - 1;
} else {
return i;
}
return i;
}
distim1 = disti;
}
return vec.size() - 1;
return size - 1;
}


// A struct to hold pose data in floating point resolution
struct Pose2D
{
float x, y, theta;
};

} // namespace mppi::utils

#endif // NAV2_MPPI_CONTROLLER__TOOLS__UTILS_HPP_
55 changes: 38 additions & 17 deletions nav2_mppi_controller/src/critics/constraint_critic.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -49,11 +49,18 @@ void ConstraintCritic::score(CriticData & data)
// Differential motion model
auto diff = dynamic_cast<DiffDriveMotionModel *>(data.motion_model.get());
if (diff != nullptr) {
data.costs += xt::pow(
xt::sum(
(std::move(xt::maximum(data.state.vx - max_vel_, 0.0f)) +
std::move(xt::maximum(min_vel_ - data.state.vx, 0.0f))) *
data.model_dt, {1}, immediate) * weight_, power_);
if (power_ > 1u) {
data.costs += xt::pow(
xt::sum(
(std::move(xt::maximum(data.state.vx - max_vel_, 0.0f)) +
std::move(xt::maximum(min_vel_ - data.state.vx, 0.0f))) *
data.model_dt, {1}, immediate) * weight_, power_);
} else {
data.costs += xt::sum(
(std::move(xt::maximum(data.state.vx - max_vel_, 0.0f)) +
std::move(xt::maximum(min_vel_ - data.state.vx, 0.0f))) *
data.model_dt, {1}, immediate) * weight_;
}
return;
}

Expand All @@ -62,11 +69,18 @@ void ConstraintCritic::score(CriticData & data)
if (omni != nullptr) {
auto sgn = xt::eval(xt::where(data.state.vx > 0.0f, 1.0f, -1.0f));
auto vel_total = sgn * xt::hypot(data.state.vx, data.state.vy);
data.costs += xt::pow(
xt::sum(
(std::move(xt::maximum(vel_total - max_vel_, 0.0f)) +
std::move(xt::maximum(min_vel_ - vel_total, 0.0f))) *
data.model_dt, {1}, immediate) * weight_, power_);
if (power_ > 1u) {
data.costs += xt::pow(
xt::sum(
(std::move(xt::maximum(vel_total - max_vel_, 0.0f)) +
std::move(xt::maximum(min_vel_ - vel_total, 0.0f))) *
data.model_dt, {1}, immediate) * weight_, power_);
} else {
data.costs += xt::sum(
(std::move(xt::maximum(vel_total - max_vel_, 0.0f)) +
std::move(xt::maximum(min_vel_ - vel_total, 0.0f))) *
data.model_dt, {1}, immediate) * weight_;
}
return;
}

Expand All @@ -77,13 +91,20 @@ void ConstraintCritic::score(CriticData & data)
auto & wz = data.state.wz;
auto out_of_turning_rad_motion = xt::maximum(
acker->getMinTurningRadius() - (xt::fabs(vx) / xt::fabs(wz)), 0.0f);

data.costs += xt::pow(
xt::sum(
(std::move(xt::maximum(data.state.vx - max_vel_, 0.0f)) +
std::move(xt::maximum(min_vel_ - data.state.vx, 0.0f)) +
std::move(out_of_turning_rad_motion)) *
data.model_dt, {1}, immediate) * weight_, power_);
if (power_ > 1u) {
data.costs += xt::pow(
xt::sum(
(std::move(xt::maximum(data.state.vx - max_vel_, 0.0f)) +
std::move(xt::maximum(min_vel_ - data.state.vx, 0.0f)) +
std::move(out_of_turning_rad_motion)) *
data.model_dt, {1}, immediate) * weight_, power_);
} else {
data.costs += xt::sum(
(std::move(xt::maximum(data.state.vx - max_vel_, 0.0f)) +
std::move(xt::maximum(min_vel_ - data.state.vx, 0.0f)) +
std::move(out_of_turning_rad_motion)) *
data.model_dt, {1}, immediate) * weight_;
}
return;
}
}
Expand Down
11 changes: 8 additions & 3 deletions nav2_mppi_controller/src/critics/cost_critic.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -108,7 +108,6 @@ void CostCritic::score(CriticData & data)
}

auto && repulsive_cost = xt::xtensor<float, 1>::from_shape({data.costs.shape(0)});
repulsive_cost.fill(0.0f);

unsigned int x_i = 0u, y_i = 0u;
float pose_cost;
Expand All @@ -118,10 +117,11 @@ void CostCritic::score(CriticData & data)
bool all_trajectories_collide = true;
for (size_t i = 0; i < data.trajectories.x.shape(0); ++i) {
bool trajectory_collide = false;
const auto traj_x = xt::view(data.trajectories.x, i, xt::all());
const auto traj_x = xt::view(data.trajectories.x, i, xt::all()); // TODO higher level or don't use views at all!
const auto traj_y = xt::view(data.trajectories.y, i, xt::all());
const auto traj_yaw = xt::view(data.trajectories.yaws, i, xt::all());
pose_cost = 0.0f;
repulsive_cost[i] = 0.0f;

for (size_t j = 0; j < traj_len; j++) {
// The getCost doesn't use orientation
Expand Down Expand Up @@ -157,7 +157,12 @@ void CostCritic::score(CriticData & data)
}
}

data.costs += xt::pow((std::move(repulsive_cost) * (weight_ / static_cast<float>(traj_len))), power_);
if (power_ > 1u) {
data.costs += xt::pow((std::move(repulsive_cost) * (weight_ / static_cast<float>(traj_len))), power_);
} else {
data.costs += std::move(repulsive_cost) * (weight_ / static_cast<float>(traj_len));
}

data.fail_flag = all_trajectories_collide;
}

Expand Down
11 changes: 8 additions & 3 deletions nav2_mppi_controller/src/critics/goal_angle_critic.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,9 +44,14 @@ void GoalAngleCritic::score(CriticData & data)
const auto goal_idx = data.path.x.shape(0) - 1;
const float goal_yaw = data.path.yaws(goal_idx);

data.costs += xt::pow(
xt::mean(xt::abs(utils::shortest_angular_distance(data.trajectories.yaws, goal_yaw)), {1}) *
weight_, power_);
if (power_ > 1u) {
data.costs += xt::pow(
xt::mean(xt::abs(utils::shortest_angular_distance(data.trajectories.yaws, goal_yaw)), {1}) *
weight_, power_);
} else {
data.costs += xt::mean(
xt::abs(utils::shortest_angular_distance(data.trajectories.yaws, goal_yaw)), {1}) * weight_;
}
}

} // namespace mppi::critics
Expand Down
11 changes: 8 additions & 3 deletions nav2_mppi_controller/src/critics/goal_critic.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -49,9 +49,14 @@ void GoalCritic::score(CriticData & data)
const auto traj_x = xt::view(data.trajectories.x, xt::all(), xt::all());
const auto traj_y = xt::view(data.trajectories.y, xt::all(), xt::all());

data.costs += xt::pow(
xt::mean(xt::hypot(traj_x - goal_x, traj_y - goal_y),
{1}, immediate) * weight_, power_);
if (power_ > 1u) {
data.costs += xt::pow(
xt::mean(xt::hypot(traj_x - goal_x, traj_y - goal_y),
{1}, immediate) * weight_, power_);
} else {
data.costs += xt::mean(xt::hypot(traj_x - goal_x, traj_y - goal_y),
{1}, immediate) * weight_;
}
}

} // namespace mppi::critics
Expand Down
18 changes: 12 additions & 6 deletions nav2_mppi_controller/src/critics/obstacles_critic.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -121,9 +121,7 @@ void ObstaclesCritic::score(CriticData & data)
}

auto && raw_cost = xt::xtensor<float, 1>::from_shape({data.costs.shape(0)});
raw_cost.fill(0.0f);
auto && repulsive_cost = xt::xtensor<float, 1>::from_shape({data.costs.shape(0)});
repulsive_cost.fill(0.0f);

const size_t traj_len = data.trajectories.x.shape(1);
bool all_trajectories_collide = true;
Expand All @@ -132,6 +130,8 @@ void ObstaclesCritic::score(CriticData & data)
float traj_cost = 0.0f;
const auto & traj = data.trajectories;
CollisionCost pose_cost;
raw_cost[i] = 0.0f;
repulsive_cost[i] = 0.0f;

for (size_t j = 0; j < traj_len; j++) {
pose_cost = costAtPose(traj.x(i, j), traj.y(i, j), traj.yaws(i, j));
Expand Down Expand Up @@ -169,10 +169,16 @@ void ObstaclesCritic::score(CriticData & data)
auto && repulsive_cost_normalized =
(repulsive_cost - xt::amin(repulsive_cost, immediate)) / traj_len;

data.costs += xt::pow(
(critical_weight_ * raw_cost) +
(repulsion_weight_ * repulsive_cost_normalized),
power_);
if (power_ > 1u) {
data.costs += xt::pow(
(critical_weight_ * raw_cost) +
(repulsion_weight_ * repulsive_cost_normalized),
power_);
} else {
data.costs += (critical_weight_ * raw_cost) +
(repulsion_weight_ * repulsive_cost_normalized);
}

data.fail_flag = all_trajectories_collide;
}

Expand Down
Loading

0 comments on commit 6f32322

Please sign in to comment.