autowarefoundation / lucid_vision_driver

Lucid Vision Camera ROS2 Driver with Arena SDK
Apache License 2.0
17 stars 18 forks source link

Failed to Load multiple nodes/cameras. #2

Open lionator opened 1 year ago

lionator commented 1 year ago

I want to load 2 cameras simultaneously, but the second camera throws an error:

ros2 launch lucid_vision_driver test_node_container.launch.py [INFO] [launch]: All log files can be found below /home/vwp/.ros/log/2023-04-20-11-04-32-212034-datalynx-8259 [INFO] [launch]: Default logging verbosity is set to INFO [INFO] [component_container-1]: process started with pid [8271] [component_container-1] [INFO] [1681981472.596805394] [perception.object_detection.lucid_camera_node_container]: Load Library: /home/vwp/autoware/install/arena_camera/lib/libarena_camera_node.so [component_container-1] [INFO] [1681981472.694545831] [perception.object_detection.lucid_camera_node_container]: Found class: rclcpp_components::NodeFactoryTemplate<ArenaCameraNode> [component_container-1] [INFO] [1681981472.695266744] [perception.object_detection.lucid_camera_node_container]: Instantiate class: rclcpp_components::NodeFactoryTemplate<ArenaCameraNode> [component_container-1] Camera readed from yaml file. Camera Name:camera_1 Frame id:camera_top/camera_link Serial no:231301220 Pixel_format:bgr8 FPS:19 [component_container-1] [WARN] [1681981474.116701298] [ARENA_CAMERA_HANDLER]: Not possible to set exposure value when auto exposure is enabled. [component_container-1] [WARN] [1681981474.120789087] [ARENA_CAMERA_HANDLER]: Not possible to set gain value when auto gain is enabled. [component_container-1] Camera:1852729698 is created. [component_container-1] [INFO] [1681981474.126826927] [arena_camera_node_right]: camera calibration URL: file:///home/vwp/autoware/src/vwp/external/lucid_vision_driver/config/test.yaml [component_container-1] Camera idx:1852729698 acquisition thread. [component_container-1] [INFO] [1681981474.150741900] [ARENA_CAMERA]: Not possible to use hardware based binning for horizontal binning:4 , software binning will be used. [component_container-1] [INFO] [1681981474.167400934] [ARENA_CAMERA]: Not possible to use hardware based binning for vertical binning:4 , software binning will be used. [INFO] [launch_ros.actions.load_composable_nodes]: Loaded node '/arena_camera_node_right' in container '/perception/object_detection/lucid_camera_node_container' [component_container-1] [INFO] [1681981474.393411868] [perception.object_detection.lucid_camera_node_container]: Found class: rclcpp_components::NodeFactoryTemplate<ArenaCameraNode> [component_container-1] [INFO] [1681981474.393524752] [perception.object_detection.lucid_camera_node_container]: Instantiate class: rclcpp_components::NodeFactoryTemplate<ArenaCameraNode> [component_container-1] Camera readed from yaml file. Camera Name:camera_2 Frame id:camera_top/camera_link Serial no:231301251 Pixel_format:bgr8 FPS:19 [component_container-1] [ERROR] [1681981474.604538974] [perception.object_detection.lucid_camera_node_container]: Component constructor threw an exception [ERROR] [launch_ros.actions.load_composable_nodes]: Failed to load node 'arena_camera_node_left' of type 'ArenaCameraNode' in container '/perception/object_detection/lucid_camera_node_container': Component constructor threw an exception

Here from my launch file:

`def generate_launch_description(): launch_arguments = []

context = LaunchContext()
camera_param_path = os.path.join(
    FindPackageShare("arena_camera").perform(context),
    "param/test.param.yaml"
)
with open(camera_param_path, "r") as f:
    camera_yaml_param = yaml.safe_load(f)["/**"]["ros__parameters"]

nodes = []

nodes.append(
    ComposableNode(
        package="arena_camera",
        plugin="ArenaCameraNode",
        name="arena_camera_node_right",
        parameters=[{"camera_name": camera_yaml_param['camera_name'],
                     "frame_id": camera_yaml_param['frame_id'],
                     "pixel_format": camera_yaml_param['pixel_format'],
                     "serial_no": 231301220, #camera_yaml_param['serial_no'],
                     "camera_info_url": camera_yaml_param['camera_info_url'],
                     "fps": camera_yaml_param['fps'],
                     "horizontal_binning": camera_yaml_param['horizontal_binning'],
                     "vertical_binning": camera_yaml_param['vertical_binning'],
                     "use_default_device_settings": camera_yaml_param['use_default_device_settings'],
                     "exposure_auto": camera_yaml_param['exposure_auto'],
                     "exposure_target": camera_yaml_param['exposure_target'],
                     "gain_auto": camera_yaml_param['gain_auto'],
                     "gain_target": camera_yaml_param['gain_target'],
                     "gamma_target": camera_yaml_param['gamma_target'],
                     }],
        remappings=[
        ],
        extra_arguments=[
            {"use_intra_process_comms": True}
        ],
    )
)

nodes.append(
    ComposableNode(
        package="arena_camera",
        plugin="ArenaCameraNode",
        name="arena_camera_node_left",
        parameters=[{"camera_name": "camera_2",#camera_yaml_param['camera_name'],
                     "frame_id": "camera_top/camera_link",#camera_yaml_param['frame_id'],
                     "pixel_format": camera_yaml_param['pixel_format'],
                     "serial_no": 231301251, #camera_yaml_param['serial_no'],
                     "camera_info_url": camera_yaml_param['camera_info_url'],
                     "fps": camera_yaml_param['fps'],
                     "horizontal_binning": camera_yaml_param['horizontal_binning'],
                     "vertical_binning": camera_yaml_param['vertical_binning'],
                     "use_default_device_settings": camera_yaml_param['use_default_device_settings'],
                     "exposure_auto": camera_yaml_param['exposure_auto'],
                     "exposure_target": camera_yaml_param['exposure_target'],
                     "gain_auto": camera_yaml_param['gain_auto'],
                     "gain_target": camera_yaml_param['gain_target'],
                     "gamma_target": camera_yaml_param['gamma_target'],
                     }],
        remappings=[
        ],
        extra_arguments=[
            {"use_intra_process_comms": True}
        ],
    )
)

container = ComposableNodeContainer(
    name="lucid_camera_node_container",
    namespace="/perception/object_detection",
    package="rclcpp_components",
    executable="component_container",
    composable_node_descriptions=nodes,
    output="both",
)

return LaunchDescription(
    [
        *launch_arguments,
        container
    ]
)

Do i miss something here?

VRichardJP commented 1 year ago

Could it be a collision (name? hardware?) It would help to know what exception is thrown here:

[ERROR] [launch_ros.actions.load_composable_nodes]: Failed to load node 'arena_camera_node_left' of type 'ArenaCameraNode' in container '/perception/object_detection/lucid_camera_node_container': Component constructor threw an exception

You could also try run the container with gdb to see what is triggering the error:

container = ComposableNodeContainer(
    name="lucid_camera_node_container",
    namespace="/perception/object_detection",
    package="rclcpp_components",
    executable="component_container",
    composable_node_descriptions=nodes,
    output="both",
    # add 'prefix' here:
    prefix="gdb -ex=r --args",
)
lionator commented 1 year ago

I have two different names: arena_camera_node_left and arena_camera_node_right. Hardware should work. In the ArenaSDK GUI on Windows they work together.

gdb output:

ros2 launch lucid_vision_driver test_node_container.launch.py [INFO] [launch]: All log files can be found below /home/vwp/.ros/log/2023-04-20-11-46-30-509993-datalynx-10213 [INFO] [launch]: Default logging verbosity is set to INFO [INFO] [gdb-1]: process started with pid [10225] [gdb-1] GNU gdb (Ubuntu 12.1-0ubuntu1~22.04) 12.1 [gdb-1] Copyright (C) 2022 Free Software Foundation, Inc. [gdb-1] License GPLv3+: GNU GPL version 3 or later http://gnu.org/licenses/gpl.html [gdb-1] This is free software: you are free to change and redistribute it. [gdb-1] There is NO WARRANTY, to the extent permitted by law. [gdb-1] Type "show copying" and "show warranty" for details. [gdb-1] This GDB was configured as "x86_64-linux-gnu". [gdb-1] Type "show configuration" for configuration details. [gdb-1] For bug reporting instructions, please see: [gdb-1] https://www.gnu.org/software/gdb/bugs/. [gdb-1] Find the GDB manual and other documentation resources online at: [gdb-1] http://www.gnu.org/software/gdb/documentation/. [gdb-1] [gdb-1] For help, type "help". [gdb-1] Type "apropos word" to search for commands related to "word"... [gdb-1] Reading symbols from /opt/ros/humble/lib/rclcpp_components/component_container... [gdb-1] (No debugging symbols found in /opt/ros/humble/lib/rclcpp_components/component_container) [gdb-1] Starting program: /opt/ros/humble/lib/rclcpp_components/component_container --ros-args -r __node:=lucid_camera_node_container -r __ns:=/perception/object_detection [gdb-1] [Thread debugging using libthread_db enabled] [gdb-1] Using host libthread_db library "/lib/x86_64-linux-gnu/libthread_db.so.1". [gdb-1] [New Thread 0x7ffff6d92640 (LWP 10295)] [gdb-1] [New Thread 0x7ffff6591640 (LWP 10296)] [gdb-1] [New Thread 0x7ffff5d90640 (LWP 10297)] [gdb-1] [New Thread 0x7ffff558f640 (LWP 10298)] [gdb-1] [New Thread 0x7ffff4d8e640 (LWP 10299)] [gdb-1] [New Thread 0x7fffe7fff640 (LWP 10300)] [gdb-1] [New Thread 0x7fffe77fe640 (LWP 10301)] [gdb-1] [New Thread 0x7fffe6ffd640 (LWP 10302)] [gdb-1] [New Thread 0x7fffe67fc640 (LWP 10303)] [gdb-1] [INFO] [1681983991.138643217] [perception.object_detection.lucid_camera_node_container]: Load Library: /home/vwp/autoware/install/arena_camera/lib/libarena_camera_node.so [gdb-1] [New Thread 0x7fffc27ff640 (LWP 10304)] [gdb-1] [New Thread 0x7fffc1ffe640 (LWP 10305)] [gdb-1] [New Thread 0x7fffb97fd640 (LWP 10306)] [gdb-1] [New Thread 0x7fffb0ffc640 (LWP 10307)] [gdb-1] [New Thread 0x7fffa87fb640 (LWP 10308)] [gdb-1] [New Thread 0x7fff9fffa640 (LWP 10309)] [gdb-1] [New Thread 0x7fff977f9640 (LWP 10310)] [gdb-1] [New Thread 0x7fff8eff8640 (LWP 10311)] [gdb-1] [New Thread 0x7fff867f7640 (LWP 10312)] [gdb-1] [New Thread 0x7fff7dff6640 (LWP 10313)] [gdb-1] [New Thread 0x7fff757f5640 (LWP 10314)] [gdb-1] [New Thread 0x7fff6cff4640 (LWP 10315)] [gdb-1] [New Thread 0x7fff5c7f3640 (LWP 10316)] [gdb-1] [New Thread 0x7fff5bff2640 (LWP 10317)] [gdb-1] [New Thread 0x7fff4b7f1640 (LWP 10318)] [gdb-1] [New Thread 0x7fff4aff0640 (LWP 10319)] [gdb-1] [New Thread 0x7fff427ef640 (LWP 10320)] [gdb-1] [New Thread 0x7fff39fee640 (LWP 10321)] [gdb-1] [New Thread 0x7fff317ed640 (LWP 10322)] [gdb-1] [New Thread 0x7fff28fec640 (LWP 10323)] [gdb-1] [New Thread 0x7fff207eb640 (LWP 10324)] [gdb-1] [New Thread 0x7fff17fea640 (LWP 10325)] [gdb-1] [New Thread 0x7fff077e9640 (LWP 10326)] [gdb-1] [New Thread 0x7fff06fe8640 (LWP 10327)] [gdb-1] [New Thread 0x7ffefe7e7640 (LWP 10328)] [gdb-1] [New Thread 0x7ffef5fe6640 (LWP 10329)] [gdb-1] [New Thread 0x7ffeed7e5640 (LWP 10330)] [gdb-1] [New Thread 0x7ffee4fe4640 (LWP 10331)] [gdb-1] [New Thread 0x7ffedc7e3640 (LWP 10332)] [gdb-1] [New Thread 0x7ffed3fe2640 (LWP 10333)] [gdb-1] [New Thread 0x7ffec37e1640 (LWP 10334)] [gdb-1] [New Thread 0x7ffebafe0640 (LWP 10335)] [gdb-1] [New Thread 0x7ffeb27df640 (LWP 10336)] [gdb-1] [New Thread 0x7ffea9fde640 (LWP 10337)] [gdb-1] [New Thread 0x7ffea17dd640 (LWP 10338)] [gdb-1] [New Thread 0x7ffe98fdc640 (LWP 10339)] [gdb-1] [New Thread 0x7ffe907db640 (LWP 10340)] [gdb-1] [New Thread 0x7ffe87fda640 (LWP 10341)] [gdb-1] [New Thread 0x7ffe7f7d9640 (LWP 10342)] [gdb-1] [New Thread 0x7ffe76fd8640 (LWP 10343)] [gdb-1] [New Thread 0x7ffe6e7d7640 (LWP 10344)] [gdb-1] [New Thread 0x7ffe65fd6640 (LWP 10345)] [gdb-1] [New Thread 0x7ffe5d7d5640 (LWP 10346)] [gdb-1] [New Thread 0x7ffe54fd4640 (LWP 10347)] [gdb-1] [New Thread 0x7ffe4c7d3640 (LWP 10348)] [gdb-1] [New Thread 0x7ffe43fd2640 (LWP 10349)] [gdb-1] [New Thread 0x7ffe3b7d1640 (LWP 10350)] [gdb-1] [New Thread 0x7ffe32fd0640 (LWP 10351)] [gdb-1] [New Thread 0x7ffe2a7cf640 (LWP 10352)] [gdb-1] [New Thread 0x7ffe21fce640 (LWP 10353)] [gdb-1] [New Thread 0x7ffe197cd640 (LWP 10354)] [gdb-1] [New Thread 0x7ffe10fcc640 (LWP 10355)] [gdb-1] [New Thread 0x7ffe087cb640 (LWP 10356)] [gdb-1] [New Thread 0x7ffdfffca640 (LWP 10357)] [gdb-1] [New Thread 0x7ffdf77c9640 (LWP 10358)] [gdb-1] [New Thread 0x7ffdeefc8640 (LWP 10359)] [gdb-1] [New Thread 0x7ffde67c7640 (LWP 10360)] [gdb-1] [New Thread 0x7ffdddfc6640 (LWP 10361)] [gdb-1] [New Thread 0x7ffdd57c5640 (LWP 10362)] [gdb-1] [New Thread 0x7ffdd4fc4640 (LWP 10363)] [gdb-1] [New Thread 0x7ffdcc7c3640 (LWP 10364)] [gdb-1] [New Thread 0x7ffdbbfc2640 (LWP 10365)] [gdb-1] [New Thread 0x7ffdb37c1640 (LWP 10366)] [gdb-1] [INFO] [1681983991.922718798] [perception.object_detection.lucid_camera_node_container]: Found class: rclcpp_components::NodeFactoryTemplate [gdb-1] [INFO] [1681983991.923634284] [perception.object_detection.lucid_camera_node_container]: Instantiate class: rclcpp_components::NodeFactoryTemplate [gdb-1] Camera readed from yaml file. Camera Name:camera_1 Frame id:camera_top/camera_link Serial no:231301220 Pixel_format:bgr8 FPS:19 [gdb-1] [New Thread 0x7ffdaafc0640 (LWP 10367)] [gdb-1] [New Thread 0x7ffdaa7bf640 (LWP 10368)] [gdb-1] [Thread 0x7ffdaafc0640 (LWP 10367) exited] [gdb-1] [New Thread 0x7ffdaa7bf640 (LWP 10369)] [gdb-1] [Thread 0x7ffdaa7bf640 (LWP 10368) exited] [gdb-1] [New Thread 0x7ffdaafc0640 (LWP 10370)] [gdb-1] [Thread 0x7ffdaa7bf640 (LWP 10369) exited] [gdb-1] [New Thread 0x7ffdaafc0640 (LWP 10371)] [gdb-1] [Thread 0x7ffdaafc0640 (LWP 10370) exited] [gdb-1] [New Thread 0x7ffdaa7bf640 (LWP 10372)] [gdb-1] [Thread 0x7ffdaafc0640 (LWP 10371) exited] [gdb-1] [Thread 0x7ffdaa7bf640 (LWP 10372) exited] [gdb-1] [New Thread 0x7ffdaa7bf640 (LWP 10373)] [gdb-1] [New Thread 0x7ffdaafc0640 (LWP 10374)] [gdb-1] [New Thread 0x7ffda9bff640 (LWP 10375)] [gdb-1] [New Thread 0x7ffda93fe640 (LWP 10376)] [gdb-1] [New Thread 0x7ffda8bfd640 (LWP 10377)] [gdb-1] [New Thread 0x7ffda3fff640 (LWP 10378)] [gdb-1] [New Thread 0x7ffda37fe640 (LWP 10379)] [gdb-1] [New Thread 0x7ffda2ffd640 (LWP 10380)] [gdb-1] [Thread 0x7ffda93fe640 (LWP 10376) exited] [gdb-1] [Thread 0x7ffda9bff640 (LWP 10375) exited] [gdb-1] [Thread 0x7ffdaa7bf640 (LWP 10373) exited] [gdb-1] [New Thread 0x7ffda93fe640 (LWP 10381)] [gdb-1] [New Thread 0x7ffdaa7bf640 (LWP 10382)] [gdb-1] [Thread 0x7ffdaa7bf640 (LWP 10382) exited] [gdb-1] [Thread 0x7ffda93fe640 (LWP 10381) exited] [gdb-1] [Thread 0x7ffdaafc0640 (LWP 10374) exited] [gdb-1] [New Thread 0x7ffdaafc0640 (LWP 10383)] [gdb-1] [New Thread 0x7ffdaa7bf640 (LWP 10384)] [gdb-1] [Thread 0x7ffdaa7bf640 (LWP 10384) exited] [gdb-1] [Thread 0x7ffdaafc0640 (LWP 10383) exited] [gdb-1] [Thread 0x7ffda8bfd640 (LWP 10377) exited] [gdb-1] [New Thread 0x7ffda8bfd640 (LWP 10385)] [gdb-1] [New Thread 0x7ffdaa7bf640 (LWP 10386)] [gdb-1] [Thread 0x7ffdaa7bf640 (LWP 10386) exited] [gdb-1] [Thread 0x7ffda8bfd640 (LWP 10385) exited] [gdb-1] [Thread 0x7ffda3fff640 (LWP 10378) exited] [gdb-1] [New Thread 0x7ffda3fff640 (LWP 10387)] [gdb-1] [New Thread 0x7ffdaa7bf640 (LWP 10388)] [gdb-1] [Thread 0x7ffdaa7bf640 (LWP 10388) exited] [gdb-1] [Thread 0x7ffda3fff640 (LWP 10387) exited] [gdb-1] [Thread 0x7ffda37fe640 (LWP 10379) exited] [gdb-1] [New Thread 0x7ffda37fe640 (LWP 10389)] [gdb-1] [New Thread 0x7ffdaa7bf640 (LWP 10390)] [gdb-1] [Thread 0x7ffdaa7bf640 (LWP 10390) exited] [gdb-1] [Thread 0x7ffda37fe640 (LWP 10389) exited] [gdb-1] [Thread 0x7ffda2ffd640 (LWP 10380) exited] [gdb-1] [New Thread 0x7ffda2ffd640 (LWP 10391)] [gdb-1] [WARN] [1681983993.347198123] [ARENA_CAMERA_HANDLER]: Not possible to set exposure value when auto exposure is enabled. [gdb-1] [WARN] [1681983993.351594241] [ARENA_CAMERA_HANDLER]: Not possible to set gain value when auto gain is enabled. [gdb-1] Camera:1852729698 is created. [gdb-1] [INFO] [1681983993.369586925] [arena_camera_node_right]: camera calibration URL: file:///home/vwp/autoware/src/vwp/external/lucid_vision_driver/config/test.yaml [gdb-1] Camera idx:1852729698 acquisition thread. [gdb-1] [INFO] [1681983993.393586995] [ARENA_CAMERA]: Not possible to use hardware based binning for horizontal binning:4 , software binning will be used. [gdb-1] [INFO] [1681983993.410258200] [ARENA_CAMERA]: Not possible to use hardware based binning for vertical binning:4 , software binning will be used. [gdb-1] [New Thread 0x7ffdaa7bf640 (LWP 10392)] [gdb-1] [New Thread 0x7ffda37fe640 (LWP 10393)] [gdb-1] [New Thread 0x7ffda3fff640 (LWP 10394)] [INFO] [launch_ros.actions.load_composable_nodes]: Loaded node '/arena_camera_node_right' in container '/perception/object_detection/lucid_camera_node_container' [gdb-1] [INFO] [1681983993.600278635] [perception.object_detection.lucid_camera_node_container]: Found class: rclcpp_components::NodeFactoryTemplate [gdb-1] [INFO] [1681983993.600367298] [perception.object_detection.lucid_camera_node_container]: Instantiate class: rclcpp_components::NodeFactoryTemplate [gdb-1] Camera readed from yaml file. Camera Name:camera_2 Frame id:camera_top/camera_link Serial no:231301251 Pixel_format:bgr8 FPS:19 [gdb-1] [New Thread 0x7ffd97fff640 (LWP 10396)] [gdb-1] [New Thread 0x7ffd97bfe640 (LWP 10398)] [gdb-1] [New Thread 0x7ffd977fd640 (LWP 10397)] [gdb-1] [New Thread 0x7ffd973fc640 (LWP 10399)] [gdb-1] [New Thread 0x7ffd96bfa640 (LWP 10401)] [gdb-1] [New Thread 0x7ffd96ffb640 (LWP 10400)] [gdb-1] [New Thread 0x7ffd967f9640 (LWP 10402)] [gdb-1] [New Thread 0x7ffd95bf6640 (LWP 10404)] [gdb-1] [New Thread 0x7ffd95ff7640 (LWP 10403)] [gdb-1] [New Thread 0x7ffd953f4640 (LWP 10406)] [gdb-1] [New Thread 0x7ffd957f5640 (LWP 10407)] [gdb-1] [New Thread 0x7ffd94ff3640 (LWP 10408)] [gdb-1] [New Thread 0x7ffd963f8640 (LWP 10405)] [gdb-1] [New Thread 0x7ffd94bf2640 (LWP 10409)] [gdb-1] [New Thread 0x7ffd6bfff640 (LWP 10410)] [gdb-1] [New Thread 0x7ffd6b3fc640 (LWP 10412)] [gdb-1] [New Thread 0x7ffd6bbfe640 (LWP 10414)] [gdb-1] [New Thread 0x7ffd6affb640 (LWP 10415)] [gdb-1] [New Thread 0x7ffd6a7f9640 (LWP 10417)] [gdb-1] [New Thread 0x7ffd6abfa640 (LWP 10416)] [gdb-1] [New Thread 0x7ffd6b7fd640 (LWP 10413)] [gdb-1] [New Thread 0x7ffd947f1640 (LWP 10411)] [gdb-1] [New Thread 0x7ffd693f4640 (LWP 10418)] [gdb-1] [New Thread 0x7ffd68ff3640 (LWP 10419)] [gdb-1] [New Thread 0x7ffd2ffff640 (LWP 10421)] [gdb-1] [New Thread 0x7ffd68bf2640 (LWP 10420)] [gdb-1] [New Thread 0x7ffd43fff640 (LWP 10422)] [gdb-1] [New Thread 0x7ffd433fc640 (LWP 10430)] [gdb-1] [New Thread 0x7ffd42bfa640 (LWP 10432)] [gdb-1] [New Thread 0x7ffd437fd640 (LWP 10431)] [gdb-1] [New Thread 0x7ffd687f1640 (LWP 10427)] [gdb-1] [New Thread 0x7ffd42ffb640 (LWP 10429)] [gdb-1] [New Thread 0x7ffd6a3f8640 (LWP 10424)] [gdb-1] [New Thread 0x7ffd69bf6640 (LWP 10428)] [gdb-1] [New Thread 0x7ffd69ff7640 (LWP 10425)] [gdb-1] [New Thread 0x7ffd43bfe640 (LWP 10423)] [gdb-1] [New Thread 0x7ffd697f5640 (LWP 10426)] [gdb-1] [New Thread 0x7ffd413f4640 (LWP 10433)] [gdb-1] [New Thread 0x7ffd2effb640 (LWP 10435)] [gdb-1] [New Thread 0x7ffd423f8640 (LWP 10434)] [gdb-1] [New Thread 0x7ffd41ff7640 (LWP 10438)] [gdb-1] [New Thread 0x7ffd427f9640 (LWP 10437)] [gdb-1] [New Thread 0x7ffd2e7f9640 (LWP 10444)] [gdb-1] [New Thread 0x7ffd2e3f8640 (LWP 10446)] [gdb-1] [New Thread 0x7ffd2dff7640 (LWP 10448)] [gdb-1] [New Thread 0x7ffd2fbfe640 (LWP 10447)] [gdb-1] [New Thread 0x7ffd2f3fc640 (LWP 10445)] [gdb-1] [New Thread 0x7ffd2f7fd640 (LWP 10442)] [gdb-1] [New Thread 0x7ffd2ebfa640 (LWP 10443)] [gdb-1] [New Thread 0x7ffd2dbf6640 (LWP 10450)] [gdb-1] [New Thread 0x7ffd417f5640 (LWP 10449)] [gdb-1] [New Thread 0x7ffd40ff3640 (LWP 10436)] [gdb-1] [New Thread 0x7ffd41bf6640 (LWP 10439)] [gdb-1] [New Thread 0x7ffd40bf2640 (LWP 10440)] [gdb-1] [New Thread 0x7ffd407f1640 (LWP 10441)] [gdb-1] [New Thread 0x7ffd077fd640 (LWP 10451)] [gdb-1] [New Thread 0x7ffd07bfe640 (LWP 10454)] [gdb-1] [New Thread 0x7ffd07fff640 (LWP 10455)] [gdb-1] [New Thread 0x7ffd2cff3640 (LWP 10458)] [gdb-1] [New Thread 0x7ffd2d7f5640 (LWP 10457)] [gdb-1] [New Thread 0x7ffd2d3f4640 (LWP 10452)] [gdb-1] [New Thread 0x7ffd2cbf2640 (LWP 10453)] [gdb-1] [New Thread 0x7ffd2c7f1640 (LWP 10456)] [gdb-1] [ERROR] [1681983993.810585531] [perception.object_detection.lucid_camera_node_container]: Component constructor threw an exception [ERROR] [launch_ros.actions.load_composable_nodes]: Failed to load node 'arena_camera_node_left' of type 'ArenaCameraNode' in container '/perception/object_detection/lucid_camera_node_container': Component constructor threw an exception

VRichardJP commented 1 year ago

Do you mean gdb does not let you examine the program state when the exception is thrown? For example:

terminate called after throwing an instance of 'std::runtime_error'
  what():  an error

Program received signal SIGABRT, Aborted.
__pthread_kill_implementation (no_tid=0, signo=6, threadid=140737348101952) at ./nptl/pthread_kill.c:44
44      ./nptl/pthread_kill.c: No such file or directory.
(gdb) 
lionator commented 1 year ago

Now i get this here:

[gdb-1] Thread 1 "component_conta" received signal SIGSEGV, Segmentation fault. [gdb-1] ArenaCamera::stop_stream (this=0x555000e4cb1d) at /home/vwp/autoware/src/vwp/external/lucid_vision_driver/src/arena_camera.cpp:113 [gdb-1] 113 void ArenaCamera::stop_stream() { m_device->StopStream(); }

Does this help?

VRichardJP commented 1 year ago

If it is not the case yet, could you compile your workspace with debug symbols (e.g. -DCMAKE_BUILD_TYPE=Debug) and ask gdb to show the full backtrace? ((gdb) bt)

lionator commented 1 year ago

okay it worked now: `Thread 1 "component_conta" received signal SIGSEGV, Segmentation fault. ArenaCamera::stop_stream (this=0x555000e4ded9) at /home/vwp/autoware/src/vwp/external/lucid_vision_driver/src/arena_camera.cpp:113 113 void ArenaCamera::stop_stream() { m_device->StopStream(); } (gdb) bt

0 ArenaCamera::stop_stream (this=0x555000e4ded9) at /home/vwp/autoware/src/vwp/external/lucid_vision_driver/src/arena_camera.cpp:113

1 0x00007ffff427877c in ArenaCamerasHandler::stop_stream (this=this@entry=0x555555b09000) at /home/vwp/autoware/src/vwp/external/lucid_vision_driver/src/arena_cameras_handler.cpp:75

2 0x00007ffff42787e9 in ArenaCamerasHandler::~ArenaCamerasHandler (this=this@entry=0x555555b09000, __in_chrg=) at /home/vwp/autoware/src/vwp/external/lucid_vision_driver/src/arena_cameras_handler.cpp:94

3 0x00007ffff425dff6 in std::default_delete::operator() (__ptr=0x555555b09000, this=) at /usr/include/c++/11/bits/unique_ptr.h:79

4 std::unique_ptr<ArenaCamerasHandler, std::default_delete >::~unique_ptr (this=0x555555ada9f0, __in_chrg=) at /usr/include/c++/11/bits/unique_ptr.h:361

5 ArenaCameraNode::ArenaCameraNode (this=0x555555ada670, node_options=...) at /home/vwp/autoware/src/vwp/external/lucid_vision_driver/src/arena_camera_node.cpp:53

6 0x00007ffff426f39d in gnu_cxx::new_allocator::construct<ArenaCameraNode, rclcpp::NodeOptions const&> (this=, p=0x555555ada670) at /usr/include/c++/11/ext/new_allocator.h:162

7 std::allocator_traits<std::allocator >::construct<ArenaCameraNode, rclcpp::NodeOptions const&> (p=0x555555ada670, a=...) at /usr/include/c++/11/bits/alloc_traits.h:516

8 std::_Sp_counted_ptr_inplace<ArenaCameraNode, std::allocator, (__gnu_cxx::_Lock_policy)2>::_Sp_counted_ptr_inplace<rclcpp::NodeOptions const&> (__a=..., this=0x555555ada660) at /usr/include/c++/11/bits/shared_ptr_base.h:519

9 std::shared_count<(__gnu_cxx::_Lock_policy)2>::shared_count<ArenaCameraNode, std::allocator, rclcpp::NodeOptions const&> (a=..., p=: , this=) at /usr/include/c++/11/bits/shared_ptr_base.h:650

10 std::shared_ptr<ArenaCameraNode, (__gnu_cxx::_Lock_policy)2>::shared_ptr<std::allocator, rclcpp::NodeOptions const&> (__tag=..., this=) at /usr/include/c++/11/bits/shared_ptr_base.h:1342

11 std::shared_ptr::shared_ptr<std::allocator, rclcpp::NodeOptions const&> (__tag=..., this=) at /usr/include/c++/11/bits/shared_ptr.h:409

12 std::allocate_shared<ArenaCameraNode, std::allocator, rclcpp::NodeOptions const&> (__a=...) at /usr/include/c++/11/bits/shared_ptr.h:863

13 std::make_shared<ArenaCameraNode, rclcpp::NodeOptions const&> () at /usr/include/c++/11/bits/shared_ptr.h:879

14 rclcpp_components::NodeFactoryTemplate::create_node_instance (this=, options=...) at /opt/ros/humble/include/rclcpp_components/rclcpp_components/node_factory_template.hpp:45

15 0x00007ffff7f9d671 in rclcpp_components::ComponentManager::on_load_node(std::shared_ptr, std::shared_ptr<composition_interfaces::srv::LoadNodeRequest<std::allocator > >, std::shared_ptr<composition_interfaces::srv::LoadNodeResponse<std::allocator > >) () from /opt/ros/humble/lib/libcomponent_manager.so

16 0x00007ffff7f9e625 in std::_Function_handler<void (std::shared_ptr, std::shared_ptr<composition_interfaces::srv::LoadNodeRequest<std::allocator > >, std::shared_ptr<composition_interfaces::srv::LoadNodeResponse<std::allocator > >), std::_Bind<void (rclcpp_components::ComponentManager::(rclcpp_components::ComponentManager, std::_Placeholder<1>, std::_Placeholder<2>, std::_Placeholder<3>))(std::shared_ptr, std::shared_ptr<composition_interfaces::srv::LoadNodeRequest<std::allocator > >, std::shared_ptr<composition_interfaces::srv::LoadNodeResponse<std::allocator > >)> >::_M_invoke(std::_Any_data const&, std::shared_ptr&&, std::shared_ptr<composition_interfaces::srv::LoadNodeRequest<std::allocator > >&&, std::shared_ptr<composition_interfaces::srv::LoadNodeResponse<std::allocator > >&&) () from /opt/ros/humble/lib/libcomponent_manager.so

17 0x00007ffff7fa7547 in ?? () from /opt/ros/humble/lib/libcomponent_manager.so

18 0x00007ffff7e73966 in ?? () from /opt/ros/humble/lib/librclcpp.so

19 0x00007ffff7e7137a in rclcpp::Executor::execute_service(std::shared_ptr) () from /opt/ros/humble/lib/librclcpp.so

20 0x00007ffff7e716e6 in rclcpp::Executor::execute_any_executable(rclcpp::AnyExecutable&) () from /opt/ros/humble/lib/librclcpp.so

21 0x00007ffff7e78f40 in rclcpp::executors::SingleThreadedExecutor::spin() () from /opt/ros/humble/lib/librclcpp.so

22 0x00005555555565d7 in ?? ()

23 0x00007ffff7629d90 in __libc_start_call_main (main=main@entry=0x555555556310, argc=argc@entry=6, argv=argv@entry=0x7fffffff4608) at ../sysdeps/nptl/libc_start_call_main.h:58

24 0x00007ffff7629e40 in __libc_start_main_impl (main=0x555555556310, argc=6, argv=0x7fffffff4608, init=, fini=, rtld_fini=, stack_end=0x7fffffff45f8) at ../csu/libc-start.c:392

25 0x0000555555556775 in ?? ()`

VRichardJP commented 1 year ago

It is easier to understand when properly formatted:

#0  ArenaCamera::stop_stream (this=0x555000e4ded9) at /home/vwp/autoware/src/vwp/external/lucid_vision_driver/src/arena_camera.cpp:113 
#1  0x00007ffff427877c in ArenaCamerasHandler::stop_stream (this=this@entry=0x555555b09000) at /home/vwp/autoware/src/vwp/external/lucid_vision_driver/src/arena_cameras_handler.cpp:75 
#2  0x00007ffff42787e9 in ArenaCamerasHandler::~ArenaCamerasHandler (this=this@entry=0x555555b09000, __in_chrg=<optimized out>) at /home/vwp/autoware/src/vwp/external/lucid_vision_driver/src/arena_cameras_handler.cpp:94 
#3  0x00007ffff425dff6 in std::default_delete<ArenaCamerasHandler>::operator() (__ptr=0x555555b09000, this=<optimized out>) at /usr/include/c++/11/bits/unique_ptr.h:79 
#4  std::unique_ptr<ArenaCamerasHandler, std::default_delete<ArenaCamerasHandler> >::~unique_ptr (this=0x555555ada9f0, __in_chrg=<optimized out>) at /usr/include/c++/11/bits/unique_ptr.h:361 
#5  ArenaCameraNode::ArenaCameraNode (this=0x555555ada670, node_options=...) at /home/vwp/autoware/src/vwp/external/lucid_vision_driver/src/arena_camera_node.cpp:53 
#6  0x00007ffff426f39d in __gnu_cxx::new_allocator<ArenaCameraNode>::construct<ArenaCameraNode, rclcpp::NodeOptions const&> (this=<optimized out>, __p=0x555555ada670) at /usr/include/c++/11/ext/new_allocator.h:162 
#7  std::allocator_traits<std::allocator<ArenaCameraNode> >::construct<ArenaCameraNode, rclcpp::NodeOptions const&> (__p=0x555555ada670, __a=...) at /usr/include/c++/11/bits/alloc_traits.h:516 
#8  std::_Sp_counted_ptr_inplace<ArenaCameraNode, std::allocator<ArenaCameraNode>, (__gnu_cxx::_Lock_policy)2>::_Sp_counted_ptr_inplace<rclcpp::NodeOptions const&> (__a=..., this=0x555555ada660) at /usr/include/c++/11/bits/shared_ptr_base.h:519 
#9  std::__shared_count<(__gnu_cxx::_Lock_policy)2>::__shared_count<ArenaCameraNode, std::allocator<ArenaCameraNode>, rclcpp::NodeOptions const&> (__a=..., __p=<synthetic pointer>: <optimized out>, this=<synthetic pointer>) at /usr/include/c++/11/bits/shared_ptr_base.h:650 
#10 std::__shared_ptr<ArenaCameraNode, (__gnu_cxx::_Lock_policy)2>::__shared_ptr<std::allocator<ArenaCameraNode>, rclcpp::NodeOptions const&> (__tag=..., this=<synthetic pointer>) at /usr/include/c++/11/bits/shared_ptr_base.h:1342 
#11 std::shared_ptr<ArenaCameraNode>::shared_ptr<std::allocator<ArenaCameraNode>, rclcpp::NodeOptions const&> (__tag=..., this=<synthetic pointer>) at /usr/include/c++/11/bits/shared_ptr.h:409 
#12 std::allocate_shared<ArenaCameraNode, std::allocator<ArenaCameraNode>, rclcpp::NodeOptions const&> (__a=...) at /usr/include/c++/11/bits/shared_ptr.h:863 
#13 std::make_shared<ArenaCameraNode, rclcpp::NodeOptions const&> () at /usr/include/c++/11/bits/shared_ptr.h:879 
#14 rclcpp_components::NodeFactoryTemplate<ArenaCameraNode>::create_node_instance (this=<optimized out>, options=...) at /opt/ros/humble/include/rclcpp_components/rclcpp_components/node_factory_template.hpp:45 

As you can see here, ros2 component loader is creating a new node:

#14 rclcpp_components::NodeFactoryTemplate<ArenaCameraNode>::create_node_instance

Down to here, we are in the node constructor:

#5  ArenaCameraNode::ArenaCameraNode (this=0x555555ada670, node_options=...) at /home/vwp/autoware/src/vwp/external/lucid_vision_driver/src/arena_camera_node.cpp:53 

Actually, we are exactly at the end on the constructor:

https://github.com/autowarefoundation/lucid_vision_driver/blob/9b6bc16c043a46cabd43792b1a326b279e2f7e5b/src/arena_camera_node.cpp#L51-L53

And then, the destructor of a unique_ptr is called, but there is no temporary unique_ptr in the constructor:

#4  std::unique_ptr<ArenaCamerasHandler, std::default_delete<ArenaCamerasHandler> >::~unique_ptr (this=0x555555ada9f0, __in_chrg=<optimized out>) at /usr/include/c++/11/bits/unique_ptr.h:361 

Something outside of the code is definitely breaking your program. As I suspected initially, it looks like you have a name collision:

https://github.com/autowarefoundation/lucid_vision_driver/blob/9b6bc16c043a46cabd43792b1a326b279e2f7e5b/src/arena_camera_node.cpp#L31-L32

Even if you set name="arena_camera_node_right" and name="arena_camera_node_left" in your launch file, I think the two names are ignored and you end up loading the same "arena_camera_node" twice. You could try to change the signature of the constructor to make it look like this:

arena_camera_node.cpp:

ArenaCameraNode::ArenaCameraNode(const std::string & filter_name, rclcpp::NodeOptions node_options)
: rclcpp::Node(filter_name, node_options.allow_undeclared_parameters(true))

arena_camera_node.h:

  explicit ArenaCameraNode(const std::string & filter_name = "arena_camera_node", rclcpp::NodeOptions node_options);

Then recompile and run

lionator commented 1 year ago

Thanks for your reply and help.

It doesn't work. I still can't connect to both camera, just to one. I just found out, that once i connected to one camera i cannot connect to the other camera, even when i try to start only one camera (i just changed the serial number in the yaml file). I have to reboot or i have to completely unplug both and plug them into another slot of my pcie card and then i can connect to the other one. I hope that makes sense.

It is strange because on Windows it works, so it can't be the pcie controller's fault.

lionator commented 1 year ago

I can now connect multiple cameras with the ArenaSDK Examples. I forgot to set up a network bridge. Here this helped: setting-up-a-network-bridge

I still can't get multiple cameras working with this package. Have you ever used this package with more than one camera? If yes, how did your launch file looked like?

becaant commented 1 year ago

Hello,

lionator, I have been facing the exact same problem and have yet to make any progress. VRichardJP, I am just following up on lionator's question with, have you ever used this package with more than one camera? If yes, how did your launch file looked like?

Thanks

StasKuczma commented 1 year ago

Code works with multiple cameras if You use Nodes instead of ComposableNodes and Containers

import os 
from ament_index_python.packages import get_package_share_directory
import launch
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.actions import SetLaunchConfiguration
from launch.conditions import IfCondition
from launch.conditions import UnlessCondition
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import ComposableNodeContainer
from launch_ros.actions import LoadComposableNodes
from launch_ros.descriptions import ComposableNode
from launch_ros.substitutions import FindPackageShare
from launch.substitutions import PathJoinSubstitution
from launch_ros.actions import Node

from launch import LaunchContext

import yaml

def generate_launch_description():
    launch_arguments = []

    context = LaunchContext()
    camera_param_path_left = os.path.join(
        FindPackageShare("lucid_vision_driver").perform(context),
        "param/param.left.yaml"
    )

    camera_param_path_right = os.path.join(
        FindPackageShare("lucid_vision_driver").perform(context),
        "param/param.right.yaml"
    )

    camera_param_path_front = os.path.join(
        FindPackageShare("lucid_vision_driver").perform(context),
        "param/param.front.yaml"
    )
    camera_param_path_rear = os.path.join(
        FindPackageShare("lucid_vision_driver").perform(context),
        "param/param.rear.yaml"
    )
    camera_left = Node(
        package="lucid_vision_driver",
        executable="arena_camera_node_exe",
        name="arena_camera_node_left",
        namespace="camera_lucid",
        parameters=[camera_param_path_left],
        remappings=[
        ],
    )
    camera_right = Node(
        package="lucid_vision_driver",
        executable="arena_camera_node_exe",
        name="arena_camera_node_right",
        namespace="camera_lucid",
        parameters=[camera_param_path_right],
        remappings=[
        ],
    )

    camera_front = Node(
        package="lucid_vision_driver",
        executable="arena_camera_node_exe",
        name="arena_camera_node_front",
        namespace="camera_lucid",
        parameters=[camera_param_path_front],
        remappings=[
        ],
    )
    camera_rear = Node(
        package="lucid_vision_driver",
        executable="arena_camera_node_exe",
        name="arena_camera_node_rear",
        namespace="camera_lucid",
        parameters=[camera_param_path_rear],
        remappings=[
        ],
    )
    return LaunchDescription(
        [
            *launch_arguments,
            camera_left,
            camera_right,
            camera_front,
            camera_rear,
        ]
    )

I know it is not the same but could be useful