Port sensing_launch

[fred-apex-ai]

issues blocking the way

external dependencies

• velodyne driver, velodyne_pointcloud: take the existing versions for ros2 instead of fork
• livox driver: no official foxy version yet, built successfully with a little patch https://github.com/fred-apex-ai/livox_ros2_driver
• usb_cam -> no official ros2 version yet, PR in review, trying its foxy version for time being https://github.com/flynneva/usb_cam/tree/foxy
• missing fix_distortion nodelet ported to ROS2
• tamagawa imu driver , only ROS1 version exists https://github.com/MapIV/tamagawa_imu_driver and we have our own fork in pilot.auto that still needs to be ported to ros2
• c94_f9p_rover config file for ublox not found in repositories -> Issues in running GPS poser AutowareArchitectureProposal.iv#137
• LaunchConfiguration not available for ComposableNode when called from xml inside <group>. Apparently race condition fixed but backport not released yet [foxy] Do not use event handler for loading composable nodes (#170) ros2/launch_ros#208 -> wait for release, until then test in car with master branch

ROS2 changes

• nodelet -> composable nodes within lidar.xml: create container in python, include in xml as in add preprocessor.launch.xml launch AutowareArchitectureProposal.iv#123
• nodelet -> composable nodes within velodyne_VLP*.launch.xml
• how to create node in python launch conditional on launch arg? Possible in xml

still to port

• aip_s1
  • lidar
  • camera
  • gnss
  • imu (blocked by tamagawa_imu_driver)
• velodyne 16
• velodyne 32C
• velodyne 128
• aip_x1 (identical to aip_s1)
• aip_x2 (identical to aip_s1)
• aip_xx1
• aip_xx2
• aip_customized (identical to aip_s1)
• livox
• check if s1 and friends have right set of nodelets

final run time check

fix any trivial error, ignore errors with unavailable sensors, create an issue for anything else

ros2 launch sensing_launch sensing.launch.xml sensor_model:=aip_xx1

• aip_s1
• aip_x1
• aip_xx1
• aip_xx2
• aip_customized

[esteve]

@fred-apex-ai Autoware.Auto should have equivalents for velodyne_driver and velodyne_pointcloud, but I have no idea about livox_ros_driver. Where can it be installed from?

Edit: livox_ros_driver can be installed from https://github.com/Livox-SDK/livox_ros_driver, but they don't have a ROS 2 version

[mitsudome-r]

For Livox:
https://github.com/Livox-SDK/livox_ros2_driver

For Velodyne, please use upstream ros2 branch.

[mitsudome-r]

@TakaHoribe Could you comment about using containers for nodelets, and how to call python launch from xml launch?

[mitsudome-r]

relevant ROS Answer discussion: https://answers.ros.org/question/333404/launch-composed-nodes-using-the-launch-xml-frontend/

[TakaHoribe]

@fred-apex-ai Here is an example. In this launch, I create the ComposableContainer and push the ComposableNode in composable_node_description. Maybe you can look around the code and see what's going on.
https://github.com/tier4/pilot.auto/blob/ros2/sensing/preprocessor/pointcloud/pointcloud_preprocessor/launch/preprocessor.launch.py

This tutorials for composition maybe helpful.

As an example to load the python launch file from xml, I created the tier4/AutowareArchitectureProposal.iv#123. Please check it.

[fred-apex-ai]

@TakaHoribe I was testing the pointcloud_preprocessor with this command

colcon build --packages-up-to sensing_launch && ros2 launch sensing_launch sensing.launch.xml sensor_model:=aip_s1

I remain with this error

Found class: rclcpp_components::NodeFactoryTemplate<pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent>
[component_container-1] [INFO] [1606768022.137686544] [sensing.lidar.pointcloud_preprocessor.pointcloud_preprocessor_container]: Instantiate class: rclcpp_components::NodeFactoryTemplate<pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent>
[component_container-1] [INFO] [1606768022.141397556] [concatenate_data]: Subscribing to 3 user given topics as inputs:
[component_container-1] [INFO] [1606768022.141457058] [concatenate_data]:  - /sensing/lidar/top/outlier_filtered/pointcloud
[component_container-1] [INFO] [1606768022.141466537] [concatenate_data]:  - /sensing/lidar/left/outlier_filtered/pointcloud
[component_container-1] [INFO] [1606768022.141473772] [concatenate_data]:  - /sensing/lidar/right/outlier_filtered/pointcloud
[INFO] [launch_ros.actions.load_composable_nodes]: Loaded node '/concatenate_data' in container '/sensing/lidar/pointcloud_preprocessor/pointcloud_preprocessor_container'
[component_container-1] [WARN] [1606752560.413682191] [concatenate_data]: Skipped /sensing/lidar/left/outlier_filtered/pointcloud,/sensing/lidar/right/outlier_filtered/pointcloud,/sensing/lidar/top/outlier_filtered/pointcloud. Please confirm topic.(not_subscribed_topic_name size = )143
[component_container-1] terminate called after throwing an instance of 'rclcpp::exceptions::RCLInvalidArgument'

It looks like the publish() method in https://github.com/tier4/Pilot.Auto/blob/ros2/sensing/preprocessor/pointcloud/pointcloud_preprocessor/src/concatenate_data/concatenate_data_nodelet.cpp#L230 is called. I thought that since I don't have lidars connected, there should be no messages published. But it seems to be time-triggered and fails when there is nothing to concatenate and thus an expected failure on my laptop? Can you confirm this?

[fred-apex-ai]

@mitsudome-r I did what I could to allow you to test in this PR and opened issues in this repo to watch out for when testing. The branch is freshly rebased

[fred-apex-ai]

Reviewer notes

sensor configurations

There are a lot of files changes but many are copied upon request for now. It would be better to extract the common part but @mitsudome-r said it would be easier for them to change it soon, so I made copies:

the folders aip_customized, aip_s1, aip_x1, aip_x2, and aip_xx2have identical content except for the path in which they look up their own config, so it's sufficient to reviewaip_s1andaip_xx1`.

nodelets

I ported the nodelet sections into *.launch.py files, since there was so much commonality, I just created velodyne_node_container.launch.py to cover all three 16, 32C, and 128 versions of the velodyne sensor to avoid duplication. Please check that all values have been transferred correctly.

building

If you want to test the code for the aip_xx1 section, you need to install the livox drivers. I created a patched version to install dependencies more easily and will create PR for that upstream https://github.com/fred-apex-ai/livox_ros2_driver

[mitsudome-r]

We have done tests with actual sensors over on Monday and Tuesday.
Here are some of the feedback:

1. For interpolation-nodelet, I tested with the ros2 branch in velodyne_vls repository. However, the driver didn't run in specified frequency, so I still need more investigation on this.

2. We also found a weird issue where it fails to pass arguments to velodyne_node_container.launch.py from sensing.launch.xml. The launch file is encapsulated multiple times in order of velodyne_VLP32C.launch.xml, lidar.launch.xml, then sensing_launch. When we call velodyne_VLP32C.launch.xml, we were able to launch properly, but when we called sensing_launch, it failed to run with the correct due to missing arguments. E.g. it couldn't find 'calibration' argument which was passed from velodyne_VLP32.launch.xml.

[mitsudome-r]

@fred-apex-ai About the second issue mentioned above, I was testing with a simpler launch files, and found out that the arguments is passing correctly. It might have been that the newest launch_ros was not installed properly when we did the test. I will try out with sensing_launch again after making sure that launch_ros is installed.

LaunchConfiguration not available for ComposableNode when called from xml inside . Apparently race condition fixed but backport not released yet ros2/launch_ros#208 -> wait for release, until then test in car with master branch.

[mitsudome-r]

These must be

'input_frame', LaunchConfiguration('base_frame'),
'output_frame': LaunchConfiguration('base_frame'),

as in https://github.com/tier4/autoware_launcher.iv.universe/blob/master/sensing_launch/launch/velodyne_VLS128.launch#L69

[fred-apex-ai]

done in cc23b23. Thanks for mentioning, I missed that amidst the many changes

[mitsudome-r]

@fred-apex-ai I noticed that we have to declare the arguments first in order to use LaunchConfiguration.
So we have to have DeclareLaunchArgument('base_frame', default_value='base_link') at the beginning of the launch file.
You would also have to import DeclareLaunchArgument from launch.actions modules.

[fred-apex-ai]

@mitsudome-r I have made that change and also updated lidar.launch.xml to pass that launch argument but I left it as a default. You want to be able to set it somewhere to pass it, where would you do that? Just adding a launch argument and leaving it as default at the same value as before doesn't get you anything, right? Please check 3a8b54a

[fred-apex-ai]

The same question holds for velodyne*.launch.xml. They could modify base_frame in the past but it looked as though nobody did so I just coded it up in velodyne_node_container.launch.py as default

[mitsudome-r]

For interpolation-nodelet, I tested with the ros2 branch in velodyne_vls repository. However, the driver didn't run in specified frequency, so I still need more investigation on this.

For the issue above, I found out that it was actually due to issues with topic tools. https://answers.ros.org/question/350753/ros2-topic-hz-provides-wrong-rate-for-larger-msgs/

I have checked with C++ version of the tool that I created and it seems like the topic is published at correct frequency.
https://github.com/mitsudome-r/topic_tools/tree/add-hz-checker

[mitsudome-r]

There are still some additional changes that has to be made.
You can compare it with https://github.com/mitsudome-r/autoware_launcher.iv.universe/tree/port-sensing-launch to see the changes that is mentioned in my comments.