Skip to content

Commit 2ac38e8

Browse files
SteveMacenskimasf7g
SteveMacenski
authored and
masf7g
committed
Fixing SGF in MPPI and Smoother (ros-navigation#4669)
Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>
1 parent 5707b2d commit 2ac38e8

File tree

2 files changed

+64
-183
lines changed

2 files changed

+64
-183
lines changed

‎nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp‎

Lines changed: 34 additions & 123 deletions
Original file line numberDiff line numberDiff line change
@@ -455,9 +455,8 @@ inline void savitskyGolayFilter(
455455
xt::xarray<float> filter = {-21.0, 14.0, 39.0, 54.0, 59.0, 54.0, 39.0, 14.0, -21.0};
456456
filter /= 231.0;
457457

458-
const unsigned int num_sequences = control_sequence.vx.shape(0) - 1;
459-
460458
// Too short to smooth meaningfully
459+
const unsigned int num_sequences = control_sequence.vx.shape(0) - 1;
461460
if (num_sequences < 20) {
462461
return;
463462
}
@@ -467,137 +466,49 @@ inline void savitskyGolayFilter(
467466
};
468467

469468
auto applyFilterOverAxis =
470-
[&](xt::xtensor<float, 1> & sequence,
471-
const float hist_0, const float hist_1, const float hist_2, const float hist_3) -> void
469+
[&](xt::xtensor<float, 1> & sequence, const xt::xtensor<float, 1> & initial_sequence,
470+
const float hist_0, const float hist_1, const float hist_2, const float hist_3) -> void
472471
{
473-
unsigned int idx = 0;
474-
sequence(idx) = applyFilter(
475-
{
476-
hist_0,
477-
hist_1,
478-
hist_2,
479-
hist_3,
480-
sequence(idx),
481-
sequence(idx + 1),
482-
sequence(idx + 2),
483-
sequence(idx + 3),
484-
sequence(idx + 4)});
485-
486-
idx++;
487-
sequence(idx) = applyFilter(
488-
{
489-
hist_1,
490-
hist_2,
491-
hist_3,
492-
sequence(idx - 1),
493-
sequence(idx),
494-
sequence(idx + 1),
495-
sequence(idx + 2),
496-
sequence(idx + 3),
497-
sequence(idx + 4)});
498-
499-
idx++;
500-
sequence(idx) = applyFilter(
501-
{
502-
hist_2,
503-
hist_3,
504-
sequence(idx - 2),
505-
sequence(idx - 1),
506-
sequence(idx),
507-
sequence(idx + 1),
508-
sequence(idx + 2),
509-
sequence(idx + 3),
510-
sequence(idx + 4)});
511-
512-
idx++;
513-
sequence(idx) = applyFilter(
514-
{
515-
hist_3,
516-
sequence(idx - 3),
517-
sequence(idx - 2),
518-
sequence(idx - 1),
519-
sequence(idx),
520-
sequence(idx + 1),
521-
sequence(idx + 2),
522-
sequence(idx + 3),
523-
sequence(idx + 4)});
524-
525-
for (idx = 4; idx != num_sequences - 4; idx++) {
526-
sequence(idx) = applyFilter(
527-
{
528-
sequence(idx - 4),
529-
sequence(idx - 3),
530-
sequence(idx - 2),
531-
sequence(idx - 1),
532-
sequence(idx),
533-
sequence(idx + 1),
534-
sequence(idx + 2),
535-
sequence(idx + 3),
536-
sequence(idx + 4)});
472+
float pt_m4 = hist_0;
473+
float pt_m3 = hist_1;
474+
float pt_m2 = hist_2;
475+
float pt_m1 = hist_3;
476+
float pt = initial_sequence(0);
477+
float pt_p1 = initial_sequence(1);
478+
float pt_p2 = initial_sequence(2);
479+
float pt_p3 = initial_sequence(3);
480+
float pt_p4 = initial_sequence(4);
481+
482+
for (unsigned int idx = 0; idx != num_sequences; idx++) {
483+
sequence(idx) = applyFilter({pt_m4, pt_m3, pt_m2, pt_m1, pt, pt_p1, pt_p2, pt_p3, pt_p4});
484+
pt_m4 = pt_m3;
485+
pt_m3 = pt_m2;
486+
pt_m2 = pt_m1;
487+
pt_m1 = pt;
488+
pt = pt_p1;
489+
pt_p1 = pt_p2;
490+
pt_p2 = pt_p3;
491+
pt_p3 = pt_p4;
492+
493+
if (idx + 5 < num_sequences) {
494+
pt_p4 = initial_sequence(idx + 5);
495+
} else {
496+
// Return the last point
497+
pt_p4 = initial_sequence(num_sequences);
498+
}
537499
}
538-
539-
idx++;
540-
sequence(idx) = applyFilter(
541-
{
542-
sequence(idx - 4),
543-
sequence(idx - 3),
544-
sequence(idx - 2),
545-
sequence(idx - 1),
546-
sequence(idx),
547-
sequence(idx + 1),
548-
sequence(idx + 2),
549-
sequence(idx + 3),
550-
sequence(idx + 3)});
551-
552-
idx++;
553-
sequence(idx) = applyFilter(
554-
{
555-
sequence(idx - 4),
556-
sequence(idx - 3),
557-
sequence(idx - 2),
558-
sequence(idx - 1),
559-
sequence(idx),
560-
sequence(idx + 1),
561-
sequence(idx + 2),
562-
sequence(idx + 2),
563-
sequence(idx + 2)});
564-
565-
idx++;
566-
sequence(idx) = applyFilter(
567-
{
568-
sequence(idx - 4),
569-
sequence(idx - 3),
570-
sequence(idx - 2),
571-
sequence(idx - 1),
572-
sequence(idx),
573-
sequence(idx + 1),
574-
sequence(idx + 1),
575-
sequence(idx + 1),
576-
sequence(idx + 1)});
577-
578-
idx++;
579-
sequence(idx) = applyFilter(
580-
{
581-
sequence(idx - 4),
582-
sequence(idx - 3),
583-
sequence(idx - 2),
584-
sequence(idx - 1),
585-
sequence(idx),
586-
sequence(idx),
587-
sequence(idx),
588-
sequence(idx),
589-
sequence(idx)});
590500
};
591501

592502
// Filter trajectories
503+
const models::ControlSequence initial_control_sequence = control_sequence;
593504
applyFilterOverAxis(
594-
control_sequence.vx, control_history[0].vx,
505+
control_sequence.vx, initial_control_sequence.vx, control_history[0].vx,
595506
control_history[1].vx, control_history[2].vx, control_history[3].vx);
596507
applyFilterOverAxis(
597-
control_sequence.vy, control_history[0].vy,
508+
control_sequence.vy, initial_control_sequence.vy, control_history[0].vy,
598509
control_history[1].vy, control_history[2].vy, control_history[3].vy);
599510
applyFilterOverAxis(
600-
control_sequence.wz, control_history[0].wz,
511+
control_sequence.wz, initial_control_sequence.wz, control_history[0].wz,
601512
control_history[1].wz, control_history[2].wz, control_history[3].wz);
602513

603514
// Update control history

‎nav2_smoother/src/savitzky_golay_smoother.cpp‎

Lines changed: 30 additions & 60 deletions
Original file line numberDiff line numberDiff line change
@@ -118,74 +118,44 @@ bool SavitzkyGolaySmoother::smoothImpl(
118118
};
119119

120120
auto applyFilterOverAxes =
121-
[&](std::vector<geometry_msgs::msg::PoseStamped> & plan_pts) -> void
121+
[&](std::vector<geometry_msgs::msg::PoseStamped> & plan_pts,
122+
const std::vector<geometry_msgs::msg::PoseStamped> & init_plan_pts) -> void
122123
{
123-
// Handle initial boundary conditions, first point is fixed
124-
unsigned int idx = 1;
125-
plan_pts[idx].pose.position = applyFilter(
126-
{
127-
plan_pts[idx - 1].pose.position,
128-
plan_pts[idx - 1].pose.position,
129-
plan_pts[idx - 1].pose.position,
130-
plan_pts[idx].pose.position,
131-
plan_pts[idx + 1].pose.position,
132-
plan_pts[idx + 2].pose.position,
133-
plan_pts[idx + 3].pose.position});
134-
135-
idx++;
136-
plan_pts[idx].pose.position = applyFilter(
137-
{
138-
plan_pts[idx - 2].pose.position,
139-
plan_pts[idx - 2].pose.position,
140-
plan_pts[idx - 1].pose.position,
141-
plan_pts[idx].pose.position,
142-
plan_pts[idx + 1].pose.position,
143-
plan_pts[idx + 2].pose.position,
144-
plan_pts[idx + 3].pose.position});
145-
146-
// Apply nominal filter
147-
for (idx = 3; idx < path_size - 4; ++idx) {
148-
plan_pts[idx].pose.position = applyFilter(
149-
{
150-
plan_pts[idx - 3].pose.position,
151-
plan_pts[idx - 2].pose.position,
152-
plan_pts[idx - 1].pose.position,
153-
plan_pts[idx].pose.position,
154-
plan_pts[idx + 1].pose.position,
155-
plan_pts[idx + 2].pose.position,
156-
plan_pts[idx + 3].pose.position});
124+
auto pt_m3 = init_plan_pts[0].pose.position;
125+
auto pt_m2 = init_plan_pts[0].pose.position;
126+
auto pt_m1 = init_plan_pts[0].pose.position;
127+
auto pt = init_plan_pts[1].pose.position;
128+
auto pt_p1 = init_plan_pts[2].pose.position;
129+
auto pt_p2 = init_plan_pts[3].pose.position;
130+
auto pt_p3 = init_plan_pts[4].pose.position;
131+
132+
// First point is fixed
133+
for (unsigned int idx = 1; idx != path_size - 1; idx++) {
134+
plan_pts[idx].pose.position = applyFilter({pt_m3, pt_m2, pt_m1, pt, pt_p1, pt_p2, pt_p3});
135+
pt_m3 = pt_m2;
136+
pt_m2 = pt_m1;
137+
pt_m1 = pt;
138+
pt = pt_p1;
139+
pt_p1 = pt_p2;
140+
pt_p2 = pt_p3;
141+
142+
if (idx + 4 < path_size - 1) {
143+
pt_p3 = init_plan_pts[idx + 4].pose.position;
144+
} else {
145+
// Return the last point
146+
pt_p3 = init_plan_pts[path_size - 1].pose.position;
147+
}
157148
}
158-
159-
// Handle terminal boundary conditions, last point is fixed
160-
idx++;
161-
plan_pts[idx].pose.position = applyFilter(
162-
{
163-
plan_pts[idx - 3].pose.position,
164-
plan_pts[idx - 2].pose.position,
165-
plan_pts[idx - 1].pose.position,
166-
plan_pts[idx].pose.position,
167-
plan_pts[idx + 1].pose.position,
168-
plan_pts[idx + 2].pose.position,
169-
plan_pts[idx + 2].pose.position});
170-
171-
idx++;
172-
plan_pts[idx].pose.position = applyFilter(
173-
{
174-
plan_pts[idx - 3].pose.position,
175-
plan_pts[idx - 2].pose.position,
176-
plan_pts[idx - 1].pose.position,
177-
plan_pts[idx].pose.position,
178-
plan_pts[idx + 1].pose.position,
179-
plan_pts[idx + 1].pose.position,
180-
plan_pts[idx + 1].pose.position});
181149
};
182150

183-
applyFilterOverAxes(path.poses);
151+
const auto initial_path_poses = path.poses;
152+
applyFilterOverAxes(path.poses, initial_path_poses);
184153

185154
// Lets do additional refinement, it shouldn't take more than a couple milliseconds
186155
if (do_refinement_) {
187156
for (int i = 0; i < refinement_num_; i++) {
188-
applyFilterOverAxes(path.poses);
157+
const auto reined_initial_path_poses = path.poses;
158+
applyFilterOverAxes(path.poses, reined_initial_path_poses);
189159
}
190160
}
191161

0 commit comments

Comments
 (0)