Skip to content

Add in-place filter updates #95

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
Maik13579 wants to merge 1 commit into
base: ros2
Choose a base branch
from
Open

Add in-place filter updates #95

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
6 changes: 6 additions & 0 deletions default_plugins.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -58,6 +58,12 @@
This is a increment filter which works on a stream of ints.
</description>
</class>
<class name="filters/InPlaceIncrementFilterInt" type="filters::InPlaceIncrementFilter&lt;int&gt;"
base_class_type="filters::FilterBase&lt;int&gt;">
<description>
This is a increment filter which updates a stream of ints in place.
</description>
</class>
<class name="filters/MultiChannelIncrementFilterInt" type="filters::MultiChannelIncrementFilter&lt;int&gt;"
base_class_type="filters::MultiChannelFilterBase&lt;int&gt;">
<description>
Expand Down
23 changes: 23 additions & 0 deletions include/filters/filter_base.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -312,6 +312,29 @@ class FilterBase
rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logging_interface_;
};

/**
* \brief Optional base class for filters that can update data in place.
*/
template<typename T>
class InPlaceFilter : public FilterBase<T>
{
public:
/**
* \brief Update data in place.
* \param data A reference to the data to be filtered.
*/
virtual bool update(T & data) = 0;

/**
* \brief Compatibility implementation for the standard FilterBase API.
*/
bool update(const T & data_in, T & data_out) override
{
data_out = data_in;
return update(data_out);
}
};


template<typename T>
class MultiChannelFilterBase : public FilterBase<T>
Expand Down
89 changes: 67 additions & 22 deletions include/filters/filter_chain.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -194,34 +194,67 @@ class FilterChain
if (!configured_) {
throw std::runtime_error("The update cannot be called without configuring the filter chain!");
}
bool result;
size_t list_size = reference_pointers_.size();
if (list_size == 0) {

if (reference_pointers_.empty()) {
data_out = data_in;
result = true;
} else if (list_size == 1) {
result = reference_pointers_[0]->update(data_in, data_out);
} else if (list_size == 2) {
result = reference_pointers_[0]->update(data_in, buffer0_);
if (result == false) {return false;} // don't keep processing on failure
result = result && reference_pointers_[1]->update(buffer0_, data_out);
} else {
result = reference_pointers_[0]->update(data_in, buffer0_); // first copy in
for (size_t i = 1; i < reference_pointers_.size() - 1 && result; ++i) {
// all but first and last (never called if size=2)
if (i % 2 == 1) {
result = result && reference_pointers_[i]->update(buffer0_, buffer1_);
} else {
result = result && reference_pointers_[i]->update(buffer1_, buffer0_);
return true;
}

// The first filter cannot update in place because the chain input is const.
bool result = reference_pointers_[0]->update(data_in, data_out);
if (!result) {
return false;
}

T * current = &data_out;
T * next = &buffer0_;
for (size_t i = 1; i < reference_pointers_.size(); ++i) {
auto in_place_filter =
dynamic_cast<filters::InPlaceFilter<T> *>(reference_pointers_[i].get());

@jonbinney jonbinney Jun 15, 2026

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Does this dynamic cast need to happen on every update call? It probably only needs to be done once when the filter plugins are loaded, and then the resulting pointers cached, right? I'm just bringing this up to get your thoughts though - I actually don't think the slight speed increase from caching would be enough to justify the increased code complexity.

@Maik13579 Maik13579 Jun 15, 2026

Copy link
Copy Markdown
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

A dynamic_cast is not "huge", but it is not free either. I think for filters that run on point clouds or images, the performance boost would likely be negligible compared to the actual filter work. But if someone uses this on higher-frequency data, for example IMU data, I can see the repeated RTTI check being less ideal.

I think caching this once in configure() would be smart, and I do not see it adding much complexity. We can just store the optional inplace pointers directly:

std::vector<InPlaceFilterBase<T> *> inplace_filters_;

Then configure() does the dynamic_cast once when the plugins are loaded, and update() only checks whether the cached pointer is nullptr.

To be honest, I think the cost of the dynamic_cast itself is probably very small, maybe somewhere around 10-200 ns depending on the type hierarchy, compiler, and cache state. So even for something like a 300 Hz IMU pipeline with 10 filters, we would likely still only be talking about a fraction of a millisecond per second overall. (That is only a rough estimate though)

The main reason I lean toward caching it is not because I expect a meaningful performance win, but because the result is static after the plugins are loaded. Whether a filter supports the inplace interface is configuration-time state, not something that changes on every update() call. If we can cache that with a small and straightforward change, I think it makes sense and does not add much complexity.

@Maik13579 Maik13579 Jun 15, 2026

Copy link
Copy Markdown
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

But I will leave it up to you. If you prefer caching it, I can make that change.

@Maik13579 Maik13579 Jun 15, 2026 •
edited
Loading

Copy link
Copy Markdown
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The CI failure is a bit confusing. ament_cmake_xmllint is listed in the Rolling distribution metadata:

https://github.com/ros/rosdistro/blob/a8ab8e4c579059707963f85076d191beaf0119e1/rolling/distribution.yaml#L325-L326

There also appears to be a prebuilt Noble .deb available for it:

https://packages.ros.org/ros2/ubuntu/pool/main/r/ros-rolling-ament-cmake-xmllint/

So this does not look like the package is simply missing from Rolling. The failure seems to be specifically in rosdep resolving the key for Rolling on Ubuntu Noble.

This will probably get fixed on the ROS infrastructure side, so we can also just wait a few days and rerun the CI.

@jonbinney jonbinney Jun 19, 2026

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Lets leave caching out of this PR. It could always be done in a different PR if we really want it. CI still failing.... I'm traveling so I'll check on it again next week.

@Maik13579 Maik13579 Jun 19, 2026

Copy link
Copy Markdown
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Documentation says rolling is now on Ubuntu 26.04 Resolute (https://docs.ros.org/en/rolling/Installation.html)

Ubuntu Noble is 24.04

@Maik13579 Maik13579 Jun 19, 2026

Copy link
Copy Markdown
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

osrf/docker_images#878

ros-controls/mujoco_ros2_control#178

We need to wait until the upstream ros:rolling Docker images are updating

if (in_place_filter) {
result = in_place_filter->update(*current);
} else {
// Fall back to the existing two-buffer path for filters without in-place support.
result = reference_pointers_[i]->update(*current, *next);
if (result) {
std::swap(current, next);
}
}
if (list_size % 2 == 1) { // odd number last deposit was in buffer1
result = result && reference_pointers_.back()->update(buffer1_, data_out);
if (!result) {
return false;
}
}

if (current != &data_out) {
data_out = std::move(*current);
}
return true;
}

/**
* \brief process data in place through each of the filters added sequentially
*/
bool update(T & data)
{
if (!configured_) {
throw std::runtime_error("The update cannot be called without configuring the filter chain!");
}

for (auto & filter : reference_pointers_) {
auto in_place_filter = dynamic_cast<filters::InPlaceFilter<T> *>(filter.get());
if (in_place_filter) {
if (!in_place_filter->update(data)) {
return false;
}
} else {
result = result && reference_pointers_.back()->update(buffer0_, data_out);
// Filters without in-place support still work by using an internal buffer.
if (!filter->update(data, buffer0_)) {
return false;
}
data = std::move(buffer0_);
}
}
return result;
return true;
}

/**
Expand Down Expand Up @@ -306,6 +339,18 @@ class FilterChain
return reference_pointers_.size();
}

/**
* \brief Check whether every configured filter supports in-place updates.
*/
bool can_update_fully_in_place() const
{
return std::all_of(
reference_pointers_.begin(), reference_pointers_.end(),
[](const auto & filter) {
return dynamic_cast<const filters::InPlaceFilter<T> *>(filter.get()) != nullptr;
});
}

rcl_interfaces::msg::SetParametersResult reconfigureCB(std::vector<rclcpp::Parameter> parameters)
{
auto result = rcl_interfaces::msg::SetParametersResult();
Expand Down
48 changes: 48 additions & 0 deletions include/filters/increment.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -88,6 +88,54 @@ bool IncrementFilter<T>::update(const T & data_in, T & data_out)
return true;
}

/**
* \brief A increment filter which updates data in place.
*/
template<typename T>
class InPlaceIncrementFilter : public InPlaceFilter<T>
{
public:
/**
* \brief Construct the filter with the expected width and height
*/
InPlaceIncrementFilter();

/**
* \brief Destructor to clean up
*/
~InPlaceIncrementFilter() override;

bool configure() override;

/**
* \brief Update the filter in place
*/
bool update(T & data) override;
};

template<typename T>
InPlaceIncrementFilter<T>::InPlaceIncrementFilter()
{
}

template<typename T>
InPlaceIncrementFilter<T>::~InPlaceIncrementFilter()
{
}

template<typename T>
bool InPlaceIncrementFilter<T>::configure()
{
return true;
}

template<typename T>
bool InPlaceIncrementFilter<T>::update(T & data)
{
++data;
return true;
}

/**
* \brief A increment filter which works on arrays.
*/
Expand Down
1 change: 1 addition & 0 deletions src/increment.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -32,6 +32,7 @@


PLUGINLIB_EXPORT_CLASS(filters::IncrementFilter<int>, filters::FilterBase<int>)
PLUGINLIB_EXPORT_CLASS(filters::InPlaceIncrementFilter<int>, filters::FilterBase<int>)
PLUGINLIB_EXPORT_CLASS(
filters::MultiChannelIncrementFilter<int>,
filters::MultiChannelFilterBase<int>)
71 changes: 71 additions & 0 deletions test/test_chain.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -378,6 +378,76 @@ TEST_F(ChainTest, TenIncrementChains) {
EXPECT_EQ(11, v1a);
}

TEST_F(ChainTest, InPlaceFallbackUpdate) {
filters::FilterChain<int> chain("int");

std::vector<rclcpp::Parameter> overrides;
overrides.emplace_back("InPlaceFallback.filter1.name", std::string("increment1"));
overrides.emplace_back(
"InPlaceFallback.filter1.type", std::string("filters/InPlaceIncrementFilterInt"));
auto node = make_node_with_params(overrides);

ASSERT_TRUE(
chain.configure(
"InPlaceFallback", node->get_node_logging_interface(),
node->get_node_parameters_interface()));
EXPECT_TRUE(chain.can_update_fully_in_place());

int v1 = 1;
int v1a = 9;
EXPECT_TRUE(chain.update(v1, v1a));
EXPECT_EQ(1, v1);
EXPECT_EQ(2, v1a);
}

TEST_F(ChainTest, InPlaceUpdate) {
filters::FilterChain<int> chain("int");

std::vector<rclcpp::Parameter> overrides;
overrides.emplace_back("InPlaceIncrements.filter1.name", std::string("increment1"));
overrides.emplace_back(
"InPlaceIncrements.filter1.type", std::string("filters/InPlaceIncrementFilterInt"));
overrides.emplace_back("InPlaceIncrements.filter2.name", std::string("increment2"));
overrides.emplace_back(
"InPlaceIncrements.filter2.type", std::string("filters/InPlaceIncrementFilterInt"));
auto node = make_node_with_params(overrides);

ASSERT_TRUE(
chain.configure(
"InPlaceIncrements", node->get_node_logging_interface(),
node->get_node_parameters_interface()));
EXPECT_TRUE(chain.can_update_fully_in_place());

int v1 = 1;
EXPECT_TRUE(chain.update(v1));
EXPECT_EQ(3, v1);
}

TEST_F(ChainTest, MixedInPlaceUpdate) {
filters::FilterChain<int> chain("int");

std::vector<rclcpp::Parameter> overrides;
overrides.emplace_back("MixedIncrements.filter1.name", std::string("increment1"));
overrides.emplace_back(
"MixedIncrements.filter1.type", std::string("filters/InPlaceIncrementFilterInt"));
overrides.emplace_back("MixedIncrements.filter2.name", std::string("increment2"));
overrides.emplace_back("MixedIncrements.filter2.type", std::string("filters/IncrementFilterInt"));
overrides.emplace_back("MixedIncrements.filter3.name", std::string("increment3"));
overrides.emplace_back(
"MixedIncrements.filter3.type", std::string("filters/InPlaceIncrementFilterInt"));
auto node = make_node_with_params(overrides);

ASSERT_TRUE(
chain.configure(
"MixedIncrements", node->get_node_logging_interface(),
node->get_node_parameters_interface()));
EXPECT_FALSE(chain.can_update_fully_in_place());

int v1 = 1;
EXPECT_TRUE(chain.update(v1));
EXPECT_EQ(4, v1);
}

TEST_F(ChainTest, TenMultiChannelIncrementChains) {
filters::MultiChannelFilterChain<int> chain("int");
std::vector<int> v1;
Expand Down Expand Up @@ -467,6 +537,7 @@ TEST_F(ChainTest, TestChainLength) {
chain.configure(
"ZeroFilters", node->get_node_logging_interface(), node->get_node_parameters_interface()));
EXPECT_EQ(chain.get_length(), 0);
EXPECT_TRUE(chain.can_update_fully_in_place());
chain.clear();

ASSERT_TRUE(
Expand Down
Loading