Design ideas for more ros2-ish implementation of filters

[Nosille]

I really enjoyed using this library in ROS1, and thought I would quickly port my filters into ROS2. However, I am hitting roadblocks on several fronts. Mainly due to the number of standard ROS2 features that require explicit references to the Node class (clock, logger, params, services, etc.). Currently my biggest issue is adding a tf listener without access to the clock. However, I also enjoyed creating lazy intermediate publishers in each filter in ros1 for debug purposes. I cannot find a way to create this type of publishers in the current framework. Are there known solutions to these issues?

[jonbinney]

Inside a filter you can access the logger via logging_interface_->get_logger(), see for example https://github.com/ros-perception/laser_filters/blob/8e641ce7530610ae82da0963a36e4002842ccda1/include/laser_filters/speckle_filter.h#L180

Parameter access should usually be done using FilterBase::getParam for consistency, but if you really need to you can access the parameter interface via the params_interface_ member, see for example https://github.com/ros-perception/laser_filters/blob/8e641ce7530610ae82da0963a36e4002842ccda1/include/laser_filters/scan_mask_filter.h#L61

For getting transforms,, some use cases can be handled by using a message filter in a separate node to transform the messages before they get to your filter node. That makes things more modular, and lets you look at the transformed messages when debugging by subscribing to that topic.

Publishing and subscribing is really outside the scope of what this package does in ROS2. I think that is mostly a good thing - the idea of filters is to make it very easy to write code that takes a FooMsg in, does some stuff to it, and outputs another FooMsg, without any side effects or other publishers. Once you want to have your own pub/sub, or keep a long time buffer of TF messages (as the TF listener does), you should probably just be writing your own ROS2 node.

There might be a niche for something kind of like filters, but which has a node for every filter. This could still be run in one process since ROS2 allows compiling multiple rclcpp nodes into one binary. But I think that would be a different package, and it isn't clear what that gains vs just writing some ROS2 nodes and compiling them into one binary yourself.

[Nosille]

Thank you for the quick and thorough response! The logger instructions help a lot.

I wish there was a better solution to the tf transform stuff. Most of my stuff works on pointcloud2 topics. It is very hard to filter pointclouds without performing multiple transforms.

[jonbinney]

Actually yeah - when filters was initially ported to ros2, i thought about adding tf2 support specifically. The idea would be that if it is enabled, the filter chain would create its own listener and provide access to it via a tf2::BufferCoreInterface. There are a couple of issues that come up:

• how long should the tf2 buffer length be? Different filters might want different buffer lengths depending on their needs. This could be a launch parameter
• how to handle waiting on a transform? that functionality isn't in the BufferCoreInterface, I'd need to refresh my memory on how that would work. What would the expected behavior if a transform isn't available - should filters wait indefinitely, hanging the entire chain?

I'm not sure I'll have time to tackle this in the near future, but I'd be interested to hear your thoughts.

[Nosille]

I have been rolling several ideas around in my head. From a complete rework of my code where all the filters operate as independent lifecycle nodes and the main node controls them using the lifecycle management tools, to something along the lines of what you discussed with a built-in tf buffer. I have settled on the easiest solution being to pass a node pointer through FilterBase to the filters. What do you think about the attached additional class?

template<typename T>
class NodeFilterBase : public FilterBase<T>
{
  NodeFilterBase()
  : node_(nullptr)
  {
  }

  /**
   * \brief Virtual Destructor
   */
  virtual ~NodeFilterBase() = default;

  virtual bool configure()
  {
    return true;
  }

  /**
   * \brief Configure the filter from the parameter server
   * \param node shared pointer to a node providing ros interface services
   * \param filter_name parameter from which to read the configuration
   */
  bool configure(
    rclcpp::Node::SharedPtr node,
    const std::string & param_prefix,
    const std::string & filter_name)
  {
    node_ = node;

    return FilterBase<T>::configure(param_prefix, filter_name, node_->get_node_logging_interface(), node_->get_node_parameters_interface());
  }

  /**
   * \brief Get the node
   */
  inline const rclcpp::Node::SharedPtr & getNode() {return node_;}

protected:
  /// Link to ros node for publishers, subscribers, clocks, etc.
  rclcpp::Node::SharedPtr node_;
};

[Nosille]

I played around a little more with this on a FilterChain and while it is simple at the FilterBase level, passing everything through the FilterChain is more difficult than I had hoped. I may give it a try, but your tf solution is worth pursuing.

In response to your questions, I have always used the default buffer duration (I think it is 10 sec). For live data processing, I have found this value to be more than sufficient. You could always expose get/set functions for the rare case a filter needs more history. It terms of wait time, the buffer simply accumulates tf data. Checking time validity occurs at point of use, so I think your code will not need to deal with this issue, the filter author can address how long to wait on a case-by-case basis. Most tf examples use the lookupTransform function which has a wait_for_tf argument. I usually set a parameter at the filter level for this value.

[jonbinney]

Yeah that all makes sense. Thanks for doing the experiment with passing in the node handle. That could be made to work, but in my opinion provides too many opportunities for one filter to break the functionality of another filter in the chain (since they all are in the same node.

Like I said I probably won't have time to work on this in the near term, but it's useful to have this discussion to come back to when I do get around to it.

[Nosille]

I have continued down the path of sharing a node ptr from the chain node to all of the filters. It took a few small changes to filter_base and filter_chain. I believe it is implemented in a way that will not break any existing use cases. I made the node ptr optional (defaults to a null_ptr) and is set with an alternative configure function.

Let me know if you think it is worth a pull request.

My Fork

Here is a library of example filters that take advantage of the changes.

filter_chains filters_pointcloud2

[jonbinney]

Thanks for trying that out! It's a cool idea and I like your implementation of it, but I don't think it's the right choice for the filters library - passing in the node handle gives filters too many ways to accidentally break other filters in the chain. At some point it might be worth creating another package - "superfilters?" That really makes use of this, and potentially uses more ROS2 functionality to make each filter its own node (which would make it harder for them to break each other using the node handle).

[jonbinney]

I am going to rename this to "design ideas for more ros2-ish implementation of filters" since it has morphed into that. Otherwise I always forget where these ideas are :-)

[YannickdeHoop]

Did you ever take a look at ROS 2 components? components_ROS2 Components are separate nodes which can be launched dynamically. They can have intra process communication so you dont have overhead. In this way you can create kind of a pipeline, like nodelets in ros1.

noise_filter

#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/laser_scan.hpp>

class NoiseFilter : public rclcpp::Node
{
public:
  NoiseFilter(const rclcpp::NodeOptions & options)
  : Node("noise_filter", options)
  {
    sub_ = this->create_subscription<sensor_msgs::msg::LaserScan>(
      "input_scan", 10,
      std::bind(&NoiseFilter::callback, this, std::placeholders::_1)
    );
    pub_ = this->create_publisher<sensor_msgs::msg::LaserScan>("filtered_scan", 10);
  }

private:
  void callback(const sensor_msgs::msg::LaserScan::SharedPtr msg)
  {
    auto out = *msg;
    for (auto & r : out.ranges)
    {
      if (r < 0.1 || r > 10.0)  // Example: filter out too close/far points
        r = std::numeric_limits<float>::quiet_NaN();
    }
    pub_->publish(out);
  }

  rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr sub_;
  rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr pub_;
};

#include "rclcpp_components/register_node_macro.hpp"
RCLCPP_COMPONENTS_REGISTER_NODE(NoiseFilter)
launch file

from launch import LaunchDescription
from launch_ros.actions import ComposableNodeContainer
from launch_ros.descriptions import ComposableNode

def generate_launch_description():
    return LaunchDescription([
        ComposableNodeContainer(
            name='lidar_filter_chain',
            namespace='',
            package='rclcpp_components',
            executable='component_container',
            composable_node_descriptions=[
                ComposableNode(
                    package='my_lidar_filters',
                    plugin='NoiseFilter',
                    name='noise_filter',
                    extra_arguments=[{'use_intra_process_comms': True}]
                ),
                ComposableNode(
                    package='my_lidar_filters',
                    plugin='otherFilter',
                    name='other_filter',
                    extra_arguments=[{'use_intra_process_comms': True}]
                ),
            ],
            output='screen',
        )
    ])

I think something like this should work. I didnt use components myself yet but i think this could be an idea.