topic subscription in polygon filter is not working

[YannickdeHoop]

The topic subscription to get a polygon is not working in the polygon filter. I think this is happening because the polygon filter is inherited from rclcpp::Node, even though the filter chain itself is also a node. This results in undefined behaviour. If you start a polygon filter in a filter chain, it will complain about two nodes with the same name

[jonbinney]

I think it currently creates its own node just to publish the polygon:

laser_filters/include/laser_filters/polygon_filter.h

Line 259 in 5fc3226

polygon_pub_ = create_publisher<geometry_msgs::msg::PolygonStamped>("polygon", rclcpp::QoS(1).transient_local().keep_last(1));

I've been trying to avoid passing the entire node handle into each filter in FilterBase in the filters package, because that would allow them to do things that could interfere with each other. Currently filters can create parameters and respond to parameter updates via an interface provided by FilterBase, and also log via the inherited logging_interface_ member variable of FilterBase. But they can't publish or subscribe to topics without creating their own node since they don't have a node handle.

This issue of needing multiple nodes, which causes problems, seems like a point in favor of giving up on isolating the filters from each other, and just passing a node handle to each of them. See this discussion on the filters package for a discussion of this option: ros/filters#91

It'd be great to have another person contributing their perspective to that issue. I'm also open to other ideas about how to design a filters API in ros2 that allows for most common use cases while still keeping filters easy to write and maintain, and not providing too many footguns.

[jonbinney]

Also there may be a more specific solution that could fix this particular problem with the polygon filter. In ros2 you can have multiple nodes in one executable, so I don't see why it would cause a problem. Maybe the multiple node thing is a red herring and this is related to namespaces?

[YannickdeHoop]

i dont mean the publisher but subscriber at line 364. This is a subscriber of the footprint which can be used instead of a static footprint in the parameters.
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.

[jonbinney]

Yeah, I've thought about redoing filters as components - it definitely has benefits. That seems like a big enough change to create a "filters2" package and really clean up the design, but I'll look into whether it could be done in this package without breaking the plugin api. I'm heading out on a backpacking trip right now, so it may be a couple weeks before I can help with your specific problem (which I think should be fixable without a big redesign)

[jonbinney]

What ros2 distro and what version of laser_filters are you using? I just started the polygon_filter_example.launch.py and didn't have an issue.

Are you renaming the node in your launch file? I think that could cause a problem because it would rename both the filter chain node and t eh polygon filter node.

[YannickdeHoop]

I am using humble. I am running multiple filter nodes for multiple lidar scanners so I indeed rename them

[jonbinney]

Makes sense - I'll try reproducing the issue using renaming and see if i can come up with a quick fix.

In the meantime could you add your comment about how we could do filters with ros2 components to this issue: ros/filters#91 ? I think it is a bit verbose, but maybe we could provide a python Launch class/function that simplifies things based on some conventions - for example assuming that the "filter" component subscribes to "filter_input" and publishes on "filter_output" or something like that. Maybe (i haven't thought this through) it would also be useful to provide a base class that inherits from Node and adds some niceties like knowing the default topics to subscribe to.

[jonbinney]

On jammy+humble using the current source version from the ros2 branch, I don't get see any errors/warnings printed when i use the following launch file:

from launch import LaunchDescription
from launch.substitutions import PathJoinSubstitution
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory


def generate_launch_description():
    return LaunchDescription(
        [
            Node(
                package="laser_filters",
                executable="scan_to_scan_filter_chain",
                name="foo",
                parameters=[
                    PathJoinSubstitution(
                        [get_package_share_directory("laser_filters"), "examples", "polygon_filter_example.yaml"]
                    )
                ],
            ),
            Node(
                package="laser_filters",
                executable="scan_to_scan_filter_chain",
                name="foo2",
                parameters=[
                    PathJoinSubstitution(
                        [get_package_share_directory("laser_filters"), "examples", "polygon_filter_example.yaml"]
                    )
                ],
            )
        ]
    )

I only see "WARNING: Be aware that there are nodes in the graph that share an exact name, which can have unintended side effects." if i change both nodes to be named "foo" and then run ros2 node list afterward. Could you share your launchfile?

[YannickdeHoop]

My launch is the same as your example ...
And if you use the polygon filter in your filterchain?

filter4:
  name: polygon_filter
  type: laser_filters/StaticLaserScanPolygonFilter
  params:
    footprint_topic: base_footprint_exclude
    polygon_frame: base_link
    polygon: '[]'

[YannickdeHoop]

FYI, I will be on holiday for 3 weeks. For further investigation/information you could involve @daniel-zellmann-lowpad

[jonbinney]

I tried with LaserScanPolygonFilter in the chain, and then tried again with StaticLaserScanPolygon filter in the chain. I also tried copying your exact yaml for filter4 into my filter chain (without any other filters). None of these caused any warnings to print unless I named both nodes the same name in the launch file. When you get back (or if @daniel-zellmann-lowpad has time), could you create a minimal launch.py and yaml file that causes the warning to print? Then I'll debug with that.

[sualeh-rgt]

I am having similar issue!
my launch executes a scan_to_scan_filter_chain node but i see two instances of same node in the node list, with a warning.

[jonbinney]

@sualeh-rgt could you share a minimal launch file that reproduces the issue? Also what version of the laser_filters package are you using, and on which rosdistro?

[sualeh-rgt]

@sualeh-rgt could you share a minimal launch file that reproduces the issue? Also what version of the laser_filters package are you using, and on which rosdistro?

thankyou for your reply! I went on to write my own node for for filtering. haha.
I'll try to write a launch that produces this in a day or two.

[sualeh-rgt]

hello again!
so I used the navigation tutorial launch to reproduce this issue.
is is ros2 jazzy

ros2 launch nav2_bringup tb3_simulation_launch.py headless:=False

in navigation2/nav2_bringup/launch/navigation_launch.py
added following

scan_filter_node = Node(
    package='laser_filters',
    executable='scan_to_scan_filter_chain',
    name='scan_filter_chain',
    output='screen',
    respawn=use_respawn,
    respawn_delay=2.0,
    parameters=[configured_params],
    arguments=['--ros-args', '--log-level', log_level],
    remappings=remappings,
)

ld.add_action(scan_filter_node)

in navigation2/nav2_bringup/params/nav2_params.yaml
added following

scan_filter_chain:
  ros__parameters:
    filter1:
      name: box_filter
      type: laser_filters/LaserScanBoxFilter
      params:
        box_frame: base_scan
        max_x: 0.5
        max_y: 0.5
        max_z: 0.5
        min_x: -0.5
        min_y: -0.5
        min_z: -0.5
        invert: false

after launching i got the following

ros2 node list
WARNING: Be aware that there are nodes in the graph that share an exact name, which can have unintended side effects.
/amcl
/behavior_server
/bt_navigator
/bt_navigator_navigate_through_poses_rclcpp_node
/bt_navigator_navigate_to_pose_rclcpp_node
/collision_monitor
/controller_server
/docking_server
/global_costmap/global_costmap
/launch_ros_30178
/lifecycle_manager_localization
/lifecycle_manager_navigation
/local_costmap/local_costmap
/map_server
/nav2_container
/nav2_rviz_docking_panel_node
/nav2_rviz_selector_node
/planner_server
/robot_state_publisher
/ros_gz_bridge
/route_server
/rviz
/rviz_navigation_dialog_action_client
/scan_filter_chain
/scan_filter_chain
/smoother_server
/transform_listener_impl_5a361f041d70
/transform_listener_impl_5e0c64438fc8
/transform_listener_impl_5e7fc73b5850
/transform_listener_impl_5e7fc7adf5b0
/transform_listener_impl_70db80002080
/velocity_smoother
/waypoint_follower

ros2 node info /scan_filter_chain
There are 2 nodes in the graph with the exact name "/scan_filter_chain". You are seeing information about only one of them.
/scan_filter_chain
  Subscribers:
    /parameter_events: rcl_interfaces/msg/ParameterEvent
    /scan: sensor_msgs/msg/LaserScan
  Publishers:
    /diagnostics: diagnostic_msgs/msg/DiagnosticArray
    /parameter_events: rcl_interfaces/msg/ParameterEvent
    /rosout: rcl_interfaces/msg/Log
    /scan_filter_chain/transition_event: lifecycle_msgs/msg/TransitionEvent
    /scan_filtered: sensor_msgs/msg/LaserScan
  Service Servers:
    /scan_filter_chain/change_state: lifecycle_msgs/srv/ChangeState
    /scan_filter_chain/describe_parameters: rcl_interfaces/srv/DescribeParameters
    /scan_filter_chain/get_available_states: lifecycle_msgs/srv/GetAvailableStates
    /scan_filter_chain/get_available_transitions: lifecycle_msgs/srv/GetAvailableTransitions
    /scan_filter_chain/get_parameter_types: rcl_interfaces/srv/GetParameterTypes
    /scan_filter_chain/get_parameters: rcl_interfaces/srv/GetParameters
    /scan_filter_chain/get_state: lifecycle_msgs/srv/GetState
    /scan_filter_chain/get_transition_graph: lifecycle_msgs/srv/GetAvailableTransitions
    /scan_filter_chain/get_type_description: type_description_interfaces/srv/GetTypeDescription
    /scan_filter_chain/list_parameters: rcl_interfaces/srv/ListParameters
    /scan_filter_chain/set_parameters: rcl_interfaces/srv/SetParameters
    /scan_filter_chain/set_parameters_atomically: rcl_interfaces/srv/SetParametersAtomically
  Service Clients:

  Action Servers:

  Action Clients: