Skip to content

Commit

Permalink
Merge pull request ros-navigation#11 from botsandus/AUTO-612_mppi_9_sync
Browse files Browse the repository at this point in the history 
AUTO-612 Savitsky Golay Filter order 9
  • Loading branch information
@doisyg
doisyg authored Mar 6, 2023
2 parents 876a4d5 + 219a01f commit 88b79b3
Show file tree
Hide file tree
Showing 4 changed files with 99 additions and 23 deletions.
9 changes: 1 addition & 8 deletions nav2_mppi_controller/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -75,14 +75,7 @@ install(DIRECTORY include/
DESTINATION include/
)

if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
find_package(ament_cmake_gtest REQUIRED)
set(ament_cmake_copyright_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
add_subdirectory(test)
# add_subdirectory(benchmark)
endif()


ament_export_libraries(${libraries})
ament_export_dependencies(${dependencies_pkgs})
Expand Down
2 changes: 1 addition & 1 deletion nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -248,7 +248,7 @@ class Optimizer

models::State state_;
models::ControlSequence control_sequence_;
std::array<mppi::models::Control, 2> control_history_;
std::array<mppi::models::Control, 4> control_history_;
models::Trajectories generated_trajectories_;
models::Path path_;
xt::xtensor<float, 1> costs_;
Expand Down
109 changes: 95 additions & 14 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 @@ -437,14 +437,14 @@ inline double posePointAngle(const geometry_msgs::msg::Pose & pose, double point
*/
inline void savitskyGolayFilter(
models::ControlSequence & control_sequence,
std::array<mppi::models::Control, 2> & control_history,
std::array<mppi::models::Control, 4> & control_history,
const models::OptimizerSettings & settings)
{
// Savitzky-Golay Quadratic, 5-point Coefficients
xt::xarray<float> filter = {-3.0, 12.0, 17.0, 12.0, -3.0};
filter /= 35.0;
// Savitzky-Golay Quadratic, 7-point Coefficients
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);
const unsigned int num_sequences = control_sequence.vx.shape(0) - 1;

// Too short to smooth meaningfully
if (num_sequences < 10) {
Expand All @@ -456,64 +456,145 @@ inline void savitskyGolayFilter(
};

auto applyFilterOverAxis =
[&](xt::xtensor<float, 1> & sequence, const float hist_0, const float hist_1) -> void
[&](xt::xtensor<float, 1> & 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 + 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 + 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)});

for (idx = 2; idx != num_sequences - 3; idx++) {
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 + 2),
sequence(idx + 3),
sequence(idx + 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 + 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
applyFilterOverAxis(control_sequence.vx, control_history[0].vx, control_history[1].vx);
applyFilterOverAxis(control_sequence.vy, control_history[0].vy, control_history[1].vy);
applyFilterOverAxis(control_sequence.wz, control_history[0].wz, control_history[1].wz);
applyFilterOverAxis(
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_history[1].vy, control_history[2].vy, control_history[3].vy);
applyFilterOverAxis(
control_sequence.wz, control_history[0].wz,
control_history[1].wz, control_history[2].wz, control_history[3].wz);

// Update control history
unsigned int offset = settings.shift_control_sequence ? 1 : 0;
control_history[0] = control_history[1];
control_history[1] = {
control_history[1] = control_history[2];
control_history[2] = control_history[3];
control_history[3] = {
control_sequence.vx(offset),
control_sequence.vy(offset),
control_sequence.wz(offset)};
Expand Down
2 changes: 2 additions & 0 deletions nav2_mppi_controller/src/optimizer.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -119,6 +119,8 @@ void Optimizer::reset()
control_sequence_.reset(settings_.time_steps);
control_history_[0] = {0.0, 0.0, 0.0};
control_history_[1] = {0.0, 0.0, 0.0};
control_history_[2] = {0.0, 0.0, 0.0};
control_history_[3] = {0.0, 0.0, 0.0};

costs_ = xt::zeros<float>({settings_.batch_size});
generated_trajectories_.reset(settings_.batch_size, settings_.time_steps);
Expand Down

0 comments on commit 88b79b3

Please sign in to comment.