Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
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
7 changes: 3 additions & 4 deletions control_toolbox/include/control_toolbox/filter_traits.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -15,8 +15,6 @@
#ifndef CONTROL_TOOLBOX__FILTER_TRAITS_HPP_
#define CONTROL_TOOLBOX__FILTER_TRAITS_HPP_

#define EIGEN_INITIALIZE_MATRICES_BY_NAN

#include <fmt/format.h>
#include <Eigen/Dense>
#include <algorithm>
Expand Down Expand Up @@ -129,8 +127,9 @@ struct FilterTraits<geometry_msgs::msg::WrenchStamped>

static void initialize(StorageType & storage)
{
// Evocation of the default constructor through EIGEN_INITIALIZE_MATRICES_BY_NAN
storage = StorageType();
// NaN marks the state as uninitialized, so the first update initializes it.
// Explicit, because EIGEN_INITIALIZE_MATRICES_BY_NAN is ignored if Eigen was included before
storage = StorageType::Constant(std::numeric_limits<double>::quiet_NaN());
}

static bool is_nan(const StorageType & storage) { return storage.hasNaN(); }
Expand Down
21 changes: 21 additions & 0 deletions control_toolbox/test/low_pass_filter_tests.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,9 @@

#include <gmock/gmock.h>

// included before the filter headers on purpose: the filters must not rely on
// EIGEN_INITIALIZE_MATRICES_BY_NAN, which has no effect if Eigen was included before
#include <Eigen/Core>
#include <limits>
#include <vector>

Expand Down Expand Up @@ -56,6 +59,24 @@ TEST(LowPassFilterTest, WithoutInitialStateFirstUpdateReturnsInput)
EXPECT_NEAR(out, 10.0, 1e-12);
}

TEST(LowPassFilterTest, WrenchWithoutInitialStateFirstUpdateReturnsInput)
{
control_toolbox::LowPassFilter<geometry_msgs::msg::WrenchStamped> filter(1000.0, 20.5, 1.25);

ASSERT_TRUE(filter.configure());

geometry_msgs::msg::WrenchStamped in;
in.header.frame_id = "world";
in.wrench.force.x = 10.0;
in.wrench.torque.z = -5.0;

geometry_msgs::msg::WrenchStamped out;
ASSERT_TRUE(filter.update(in, out));

EXPECT_NEAR(out.wrench.force.x, 10.0, 1e-12);
EXPECT_NEAR(out.wrench.torque.z, -5.0, 1e-12);
}

TEST(LowPassFilterTest, VectorFirstUpdateReturnsInitialState)
{
control_toolbox::LowPassFilter<std::vector<double>> filter(1000.0, 20.5, 1.25);
Expand Down
Loading