ros-perception / laser_filters

Assorted filters designed to operate on 2D planar laser scanners, which use the sensor_msgs/LaserScan type.
BSD 3-Clause "New" or "Revised" License
174 stars 205 forks source link

I can't launch LaserScanSpeckleFilter #207

Open cihattalat opened 1 day ago

cihattalat commented 1 day ago

I've downloaded ros2 branch, built succesfully with colcon build and sourced my workspace.

Then I wrote my fleet_laser_filter_launch.py file:

#!/usr/bin/env python3
import launch_ros.actions
from launch import LaunchDescription

def generate_launch_description():
    laser_filter1 = launch_ros.actions.Node(
        package="laser_filters",
        executable="scan_to_scan_filter_chain",
        name="filter1",
        parameters=[{
            'filter1': {
                'name': 'filter1',
                'type': 'laser_filters/LaserScanAngularBoundsFilter',
                'params': {
                    'lower_angle': -2.3387411977,
                    'upper_angle': 2.3387411977
                }
            }
        }],
        remappings=[
            ('/scan', '/akiba_1/scan'),
            ('/scan_filtered', '/akiba_1/scan_filtered'),
        ]
    )

    laser_filter2 = launch_ros.actions.Node(
        package="laser_filters",
        executable="scan_to_scan_filter_chain",
        name="filter2",
        parameters=[{
            'filter1': {
                'name': 'filter2',
                'type': 'laser_filters/LaserScanAngularBoundsFilter',
                'params': {
                    'lower_angle': -2.3387411977,
                    'upper_angle': 2.3387411977
                }
            }
        }],
        remappings=[
            ('/scan', '/akiba_1/scan_front'),
            ('/scan_filtered', '/akiba_1/scan_front_filtered'),
        ]
    )

    dist_node1 = launch_ros.actions.Node(
        package="laser_filters",
        executable="scan_to_scan_filter_chain",
        name="laser_filter_distance1",
        parameters=[{
            'filter1': {
                'name': 'speckle_filter',
                'type': 'laser_filters/LaserScanSpeckleFilter',
                'params': {
                    'filter_type': 0,
                    'max_range': 2.0,
                    'max_range_difference': 0.1,
                    'filter_window': 2
                }
            }
        }],
        remappings=[
            ('/scan', '/akiba_1/scan_front'),
            ('/scan_filtered', '/akiba_1/scan_front_filtered_distance'),
        ]
    )

    dist_node2 = launch_ros.actions.Node(
        package="laser_filters",
        executable="scan_to_scan_filter_chain",
        name="laser_filter_distance2",
        parameters=[{
            'filter1': {
                'name': 'speckle_filter2',
                'type': 'laser_filters/LaserScanSpeckleFilter',
                'params': {
                    'filter_type': 0,
                    'max_range': 2.0,
                    'max_range_difference': 0.1,
                    'filter_window': 2
                }
            }
        }],
        remappings=[
            ('/scan', '/akiba_1/scan'),
            ('/scan_filtered', '/akiba_1/scan_filtered_distance'),
        ]
    )

    return LaunchDescription([
        laser_filter1,
        laser_filter2,
        dist_node1,
        dist_node2
    ])

if __name__ == "__main__":
    generate_launch_description()

I'm trying to run with ros2 launch main_package fleet_laser_filter_launch.py but it gives the below error:

[INFO] [launch]: All log files can be found below /home/asus1/.ros/log/2024-11-06-11-50-18-698113-asus1-527061
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [scan_to_scan_filter_chain-1]: process started with pid [527062]
[INFO] [scan_to_scan_filter_chain-2]: process started with pid [527064]
[INFO] [scan_to_scan_filter_chain-3]: process started with pid [527066]
[INFO] [scan_to_scan_filter_chain-4]: process started with pid [527068]
[scan_to_scan_filter_chain-2] 1730883018.776762 [44] scan_to_sc: config: //CycloneDDS/Domain/General: 'NetworkInterfaceAddress': deprecated element (CYCLONEDDS_URI+0 line 1)
[scan_to_scan_filter_chain-1] 1730883018.776762 [44] scan_to_sc: config: //CycloneDDS/Domain/General: 'NetworkInterfaceAddress': deprecated element (CYCLONEDDS_URI+0 line 1)
[scan_to_scan_filter_chain-3] 1730883018.776764 [44] scan_to_sc: config: //CycloneDDS/Domain/General: 'NetworkInterfaceAddress': deprecated element (CYCLONEDDS_URI+0 line 1)
[scan_to_scan_filter_chain-4] 1730883018.776766 [44] scan_to_sc: config: //CycloneDDS/Domain/General: 'NetworkInterfaceAddress': deprecated element (CYCLONEDDS_URI+0 line 1)
[ERROR] [scan_to_scan_filter_chain-4]: process has died [pid 527068, exit code -11, cmd '/home/asus1/akiba_V2/install/laser_filters/lib/laser_filters/scan_to_scan_filter_chain --ros-args -r __node:=laser_filter_distance2 --params-file /tmp/launch_params_xn5h3r47 -r /scan:=/akiba_1/scan -r /scan_filtered:=/akiba_1/scan_filtered_distance'].
[ERROR] [scan_to_scan_filter_chain-3]: process has died [pid 527066, exit code -11, cmd '/home/asus1/akiba_V2/install/laser_filters/lib/laser_filters/scan_to_scan_filter_chain --ros-args -r __node:=laser_filter_distance1 --params-file /tmp/launch_params_h3mybc2v -r /scan:=/akiba_1/scan_front -r /scan_filtered:=/akiba_1/scan_front_filtered_distance'].

But when I try to run ScanShadowsFilter instead of LaserScanSpeckleFilter it runs successfully:

#!/usr/bin/env python3
import launch_ros.actions
from launch import LaunchDescription

def generate_launch_description():
    laser_filter1 = launch_ros.actions.Node(
        package="laser_filters",
        executable="scan_to_scan_filter_chain",
        name="filter1",
        parameters=[{
            'filter1': {
                'name': 'filter1',
                'type': 'laser_filters/LaserScanAngularBoundsFilter',
                'params': {
                    'lower_angle': -2.3387411977,
                    'upper_angle': 2.3387411977
                }
            }
        }],
        remappings=[
            ('/scan', '/akiba_1/scan'),
            ('/scan_filtered', '/akiba_1/scan_filtered'),
        ]
    )

    laser_filter2 = launch_ros.actions.Node(
        package="laser_filters",
        executable="scan_to_scan_filter_chain",
        name="filter2",
        parameters=[{
            'filter1': {
                'name': 'filter2',
                'type': 'laser_filters/LaserScanAngularBoundsFilter',
                'params': {
                    'lower_angle': -2.3387411977,
                    'upper_angle': 2.3387411977
                }
            }
        }],
        remappings=[
            ('/scan', '/akiba_1/scan_front'),
            ('/scan_filtered', '/akiba_1/scan_front_filtered'),
        ]
    )

    dist_node1 = launch_ros.actions.Node(
        package="laser_filters",
        executable="scan_to_scan_filter_chain",
        name="laser_filter_distance1",
        parameters=[{
            'filter1': {
                'name': 'shadows_filter1',
                'type': 'laser_filters/ScanShadowsFilter',
                'params': {
                    'min_range_threshold': 0.05,
                    'max_range_threshold': 2.0
                }
            }
        }],
        remappings=[
            ('/scan', '/akiba_1/scan_front'),
            ('/scan_filtered', '/akiba_1/scan_front_filtered_distance'),
        ]
    )

    dist_node2 = launch_ros.actions.Node(
        package="laser_filters",
        executable="scan_to_scan_filter_chain",
        name="laser_filter_distance2",
        parameters=[{
            'filter1': {
                'name': 'shadows_filter2',
                'type': 'laser_filters/ScanShadowsFilter',
                'params': {
                    'min_range_threshold': 0.05,
                    'max_range_threshold': 2.0
                }
            }
        }],
        remappings=[
            ('/scan', '/akiba_1/scan'),
            ('/scan_filtered', '/akiba_1/scan_filtered_distance'),
        ]
    )

    return LaunchDescription([
        laser_filter1,
        laser_filter2,
        dist_node1,
        dist_node2
    ])

if __name__ == "__main__":
    generate_launch_description()

Why?

yigitboracagiran commented 22 hours ago

When I run it with turtlebot and remove the laser_filter2 and dist_node2, because of turtlebot only has 1 lidar, then edit the remappings it worked.