Skip to content

Commit

Permalink
Fixing ros-navigation#4661: MPPI ackermann reversing taking incorrect…
Browse files Browse the repository at this point in the history 
… sign sometimes (ros-navigation#4664)

* Fixing ros-navigation#4661: MPPI ackermann reversing taking incorrect sign sometimes

Signed-off-by: Steve Macenski <[email protected]>

* fixing unit test for type implicit cast

Signed-off-by: Steve Macenski <[email protected]>

---------

Signed-off-by: Steve Macenski <[email protected]>
  • Loading branch information
@SteveMacenski @mini-1235
SteveMacenski authored and mini-1235 committed Dec 12, 2024
1 parent 88af90c commit 8d1027e
Show file tree
Hide file tree
Showing 2 changed files with 64 additions and 183 deletions.
157 changes: 34 additions & 123 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 @@ -457,9 +457,8 @@ inline void savitskyGolayFilter(
xt::xarray<float> filter = {-21.0, 14.0, 39.0, 54.0, 59.0, 54.0, 39.0, 14.0, -21.0};
filter /= 231.0;

const unsigned int num_sequences = control_sequence.vx.shape(0) - 1;

// Too short to smooth meaningfully
const unsigned int num_sequences = control_sequence.vx.shape(0) - 1;
if (num_sequences < 20) {
return;
}
Expand All @@ -469,137 +468,49 @@ inline void savitskyGolayFilter(
};

auto applyFilterOverAxis =
[&](xt::xtensor<float, 1> & sequence,
const float hist_0, const float hist_1, const float hist_2, const float hist_3) -> void
[&](xt::xtensor<float, 1> & sequence, const xt::xtensor<float, 1> & initial_sequence,
const float hist_0, const float hist_1, const float hist_2, const float hist_3) -> void
{
unsigned int idx = 0;
sequence(idx) = applyFilter(
{
hist_0,
hist_1,
hist_2,
hist_3,
sequence(idx),
sequence(idx + 1),
sequence(idx + 2),
sequence(idx + 3),
sequence(idx + 4)});

idx++;
sequence(idx) = applyFilter(
{
hist_1,
hist_2,
hist_3,
sequence(idx - 1),
sequence(idx),
sequence(idx + 1),
sequence(idx + 2),
sequence(idx + 3),
sequence(idx + 4)});

idx++;
sequence(idx) = applyFilter(
{
hist_2,
hist_3,
sequence(idx - 2),
sequence(idx - 1),
sequence(idx),
sequence(idx + 1),
sequence(idx + 2),
sequence(idx + 3),
sequence(idx + 4)});

idx++;
sequence(idx) = applyFilter(
{
hist_3,
sequence(idx - 3),
sequence(idx - 2),
sequence(idx - 1),
sequence(idx),
sequence(idx + 1),
sequence(idx + 2),
sequence(idx + 3),
sequence(idx + 4)});

for (idx = 4; idx != num_sequences - 4; idx++) {
sequence(idx) = applyFilter(
{
sequence(idx - 4),
sequence(idx - 3),
sequence(idx - 2),
sequence(idx - 1),
sequence(idx),
sequence(idx + 1),
sequence(idx + 2),
sequence(idx + 3),
sequence(idx + 4)});
float pt_m4 = hist_0;
float pt_m3 = hist_1;
float pt_m2 = hist_2;
float pt_m1 = hist_3;
float pt = initial_sequence(0);
float pt_p1 = initial_sequence(1);
float pt_p2 = initial_sequence(2);
float pt_p3 = initial_sequence(3);
float pt_p4 = initial_sequence(4);

for (unsigned int idx = 0; idx != num_sequences; idx++) {
sequence(idx) = applyFilter({pt_m4, pt_m3, pt_m2, pt_m1, pt, pt_p1, pt_p2, pt_p3, pt_p4});
pt_m4 = pt_m3;
pt_m3 = pt_m2;
pt_m2 = pt_m1;
pt_m1 = pt;
pt = pt_p1;
pt_p1 = pt_p2;
pt_p2 = pt_p3;
pt_p3 = pt_p4;

if (idx + 5 < num_sequences) {
pt_p4 = initial_sequence(idx + 5);
} else {
// Return the last point
pt_p4 = initial_sequence(num_sequences);
}
}

idx++;
sequence(idx) = applyFilter(
{
sequence(idx - 4),
sequence(idx - 3),
sequence(idx - 2),
sequence(idx - 1),
sequence(idx),
sequence(idx + 1),
sequence(idx + 2),
sequence(idx + 3),
sequence(idx + 3)});

idx++;
sequence(idx) = applyFilter(
{
sequence(idx - 4),
sequence(idx - 3),
sequence(idx - 2),
sequence(idx - 1),
sequence(idx),
sequence(idx + 1),
sequence(idx + 2),
sequence(idx + 2),
sequence(idx + 2)});

idx++;
sequence(idx) = applyFilter(
{
sequence(idx - 4),
sequence(idx - 3),
sequence(idx - 2),
sequence(idx - 1),
sequence(idx),
sequence(idx + 1),
sequence(idx + 1),
sequence(idx + 1),
sequence(idx + 1)});

idx++;
sequence(idx) = applyFilter(
{
sequence(idx - 4),
sequence(idx - 3),
sequence(idx - 2),
sequence(idx - 1),
sequence(idx),
sequence(idx),
sequence(idx),
sequence(idx),
sequence(idx)});
};

// Filter trajectories
const models::ControlSequence initial_control_sequence = control_sequence;
applyFilterOverAxis(
control_sequence.vx, control_history[0].vx,
control_sequence.vx, initial_control_sequence.vx, control_history[0].vx,
control_history[1].vx, control_history[2].vx, control_history[3].vx);
applyFilterOverAxis(
control_sequence.vy, control_history[0].vy,
control_sequence.vy, initial_control_sequence.vy, control_history[0].vy,
control_history[1].vy, control_history[2].vy, control_history[3].vy);
applyFilterOverAxis(
control_sequence.wz, control_history[0].wz,
control_sequence.wz, initial_control_sequence.wz, control_history[0].wz,
control_history[1].wz, control_history[2].wz, control_history[3].wz);

// Update control history
Expand Down
90 changes: 30 additions & 60 deletions nav2_smoother/src/savitzky_golay_smoother.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -117,74 +117,44 @@ bool SavitzkyGolaySmoother::smoothImpl(
};

auto applyFilterOverAxes =
[&](std::vector<geometry_msgs::msg::PoseStamped> & plan_pts) -> void
[&](std::vector<geometry_msgs::msg::PoseStamped> & plan_pts,
const std::vector<geometry_msgs::msg::PoseStamped> & init_plan_pts) -> void
{
// Handle initial boundary conditions, first point is fixed
unsigned int idx = 1;
plan_pts[idx].pose.position = applyFilter(
{
plan_pts[idx - 1].pose.position,
plan_pts[idx - 1].pose.position,
plan_pts[idx - 1].pose.position,
plan_pts[idx].pose.position,
plan_pts[idx + 1].pose.position,
plan_pts[idx + 2].pose.position,
plan_pts[idx + 3].pose.position});

idx++;
plan_pts[idx].pose.position = applyFilter(
{
plan_pts[idx - 2].pose.position,
plan_pts[idx - 2].pose.position,
plan_pts[idx - 1].pose.position,
plan_pts[idx].pose.position,
plan_pts[idx + 1].pose.position,
plan_pts[idx + 2].pose.position,
plan_pts[idx + 3].pose.position});

// Apply nominal filter
for (idx = 3; idx < path_size - 4; ++idx) {
plan_pts[idx].pose.position = applyFilter(
{
plan_pts[idx - 3].pose.position,
plan_pts[idx - 2].pose.position,
plan_pts[idx - 1].pose.position,
plan_pts[idx].pose.position,
plan_pts[idx + 1].pose.position,
plan_pts[idx + 2].pose.position,
plan_pts[idx + 3].pose.position});
auto pt_m3 = init_plan_pts[0].pose.position;
auto pt_m2 = init_plan_pts[0].pose.position;
auto pt_m1 = init_plan_pts[0].pose.position;
auto pt = init_plan_pts[1].pose.position;
auto pt_p1 = init_plan_pts[2].pose.position;
auto pt_p2 = init_plan_pts[3].pose.position;
auto pt_p3 = init_plan_pts[4].pose.position;

// First point is fixed
for (unsigned int idx = 1; idx != path_size - 1; idx++) {
plan_pts[idx].pose.position = applyFilter({pt_m3, pt_m2, pt_m1, pt, pt_p1, pt_p2, pt_p3});
pt_m3 = pt_m2;
pt_m2 = pt_m1;
pt_m1 = pt;
pt = pt_p1;
pt_p1 = pt_p2;
pt_p2 = pt_p3;

if (idx + 4 < path_size - 1) {
pt_p3 = init_plan_pts[idx + 4].pose.position;
} else {
// Return the last point
pt_p3 = init_plan_pts[path_size - 1].pose.position;
}
}

// Handle terminal boundary conditions, last point is fixed
idx++;
plan_pts[idx].pose.position = applyFilter(
{
plan_pts[idx - 3].pose.position,
plan_pts[idx - 2].pose.position,
plan_pts[idx - 1].pose.position,
plan_pts[idx].pose.position,
plan_pts[idx + 1].pose.position,
plan_pts[idx + 2].pose.position,
plan_pts[idx + 2].pose.position});

idx++;
plan_pts[idx].pose.position = applyFilter(
{
plan_pts[idx - 3].pose.position,
plan_pts[idx - 2].pose.position,
plan_pts[idx - 1].pose.position,
plan_pts[idx].pose.position,
plan_pts[idx + 1].pose.position,
plan_pts[idx + 1].pose.position,
plan_pts[idx + 1].pose.position});
};

applyFilterOverAxes(path.poses);
const auto initial_path_poses = path.poses;
applyFilterOverAxes(path.poses, initial_path_poses);

// Lets do additional refinement, it shouldn't take more than a couple milliseconds
if (do_refinement_) {
for (int i = 0; i < refinement_num_; i++) {
applyFilterOverAxes(path.poses);
const auto reined_initial_path_poses = path.poses;
applyFilterOverAxes(path.poses, reined_initial_path_poses);
}
}

Expand Down

0 comments on commit 8d1027e

Please sign in to comment.