Ros2 v0.8.0 sensing launch (autowarefoundation#57)

* restore file name Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * Update livox_horizon.launch (autowarefoundation#89) * fix pass through filter launch (autowarefoundation#90) * fix pass through filter launch * change if statement style * update aip_x1 sensing launch (autowarefoundation#100) * fix livox launch arg (autowarefoundation#108) * add usb_cam depend (autowarefoundation#118) * update aip_x1 camera.launch (autowarefoundation#119) * update imu.launch (autowarefoundation#120) * fix veodyne setting in aip_x1/lidar.launch (autowarefoundation#125) * Add velodyne_monitor to velodyne_*.launch (autowarefoundation#101) Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Uupdate aip_x1 lidar.launch (autowarefoundation#143) * Format gnss.launch (autowarefoundation#145) Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Add use_gnss arg to aip_x1 gnss.launch (autowarefoundation#146) Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * support individual params (autowarefoundation#137) * support individual params * remove kvaser_hardware_id.txt * Launch velodyne_monitor only when launch_driver is true (autowarefoundation#163) Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * [sensing_launch] ros2 porting: use container for livox point preprocessor Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * [sensing_launch] ros2-porting: fix vehicle_info params Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * Revert "restore file name" This reverts commit 37d7ac4f6a2a617b003b4e2a5ac96c48b332ade0. * [sensing_launch] ros2-porting: fix vehicle_info for livox preprocessor launch Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * [sensing_launch] ros2-porting: fix vehicle_info for api_** points_preprocessor.launch.py Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * fix launch Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * fix livox launch Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * added suffix ".xml" to "velodyne_monitor.launch" in the launch files * added use_sim_time with AW_ROS2_USE_SIM_TIME envvar for the parameters in the *.launch.py (autowarefoundation#61) * added use_sim_time with AW_ROS2_USE_SIM_TIME envvar for the parameters * changed to use EnvironmentVariable function for use_sim_time parameter * changed indent * removed an empty line Co-authored-by: hosokawa <hosokawa@sng-3f-ros2-eval.l.sng.tier4.jp> * fixed typo on the arg bd_code_param_path lines (autowarefoundation#63) Co-authored-by: hosokawa <hosokawa@sng-3f-ros2-eval.l.sng.tier4.jp> * [sensing_launch]: Fix indentation in gnss launch Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * [sensing_launch]: Add missing dependency in package.xml Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * [sensing_launch]: Fix velodyne launch Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * [sensing_launch]: Fix livox launch Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * [sensing_launch]: Add arg for vehicle parameter file in lidar launch Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * [sensing_launch]: Cleanup Signed-off-by: Autoware <autoware@tier4.jp> * Add new line Signed-off-by: Autoware <autoware@tier4.jp> * [sensing_launch]: Add default config for xx1 Signed-off-by: Autoware <autoware@tier4.jp> * [sensing_launch]: Fix indentation Signed-off-by: Autoware <autoware@tier4.jp> Co-authored-by: Yukihiro Saito <yukky.saito@gmail.com> Co-authored-by: Taichi Higashide <taichi.higashide@tier4.jp> Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Co-authored-by: hiroyuki obinata <58019445+obi-t4@users.noreply.github.com> Co-authored-by: hosokawa <hosokawa@sng-3f-ros2-eval.l.sng.tier4.jp> Co-authored-by: HOSOKAWA Ikuto <hosokawa.ikuto@gmail.com> Co-authored-by: wep21 <border_goldenmarket@yahoo.co.jp> Co-authored-by: Autoware <autoware@tier4.jp>
1222-takeshi · Dec 10, 2021 · 4a79f17 · 4a79f17
1 parent 349296e
commit 4a79f17
Show file tree

Hide file tree

Showing 39 changed files with 1,561 additions and 813 deletions.
diff --git a/launch/sensing_launch/CMakeLists.txt b/launch/sensing_launch/CMakeLists.txt
@@ -12,4 +12,5 @@ endif()
 ament_auto_package(INSTALL_TO_SHARE
   launch
   data
+  config
 )
diff --git a/launch/sensing_launch/config/aip_x1/default/livox_front_center_bd_code.yaml b/launch/sensing_launch/config/aip_x1/default/livox_front_center_bd_code.yaml
@@ -0,0 +1,3 @@
+/**:
+  ros__parameters:
+    cmdline_input_bd_code: '100000000000000'
diff --git a/launch/sensing_launch/config/aip_x1/default/livox_front_left_bd_code.yaml b/launch/sensing_launch/config/aip_x1/default/livox_front_left_bd_code.yaml
@@ -0,0 +1,3 @@
+/**:
+  ros__parameters:
+    cmdline_input_bd_code: '100000000000000'
diff --git a/launch/sensing_launch/config/aip_x1/default/livox_front_right_bd_code.yaml b/launch/sensing_launch/config/aip_x1/default/livox_front_right_bd_code.yaml
@@ -0,0 +1,3 @@
+/**:
+  ros__parameters:
+    cmdline_input_bd_code: '100000000000000'
diff --git a/launch/sensing_launch/config/aip_xx1/default/livox_front_left_bd_code.yaml b/launch/sensing_launch/config/aip_xx1/default/livox_front_left_bd_code.yaml
@@ -0,0 +1,3 @@
+/**:
+  ros__parameters:
+    cmdline_input_bd_code: '100000000000000'
diff --git a/launch/sensing_launch/config/aip_xx1/default/livox_front_right_bd_code.yaml b/launch/sensing_launch/config/aip_xx1/default/livox_front_right_bd_code.yaml
@@ -0,0 +1,3 @@
+/**:
+  ros__parameters:
+    cmdline_input_bd_code: '100000000000000'
diff --git a/launch/sensing_launch/launch/aip_customized/camera.launch.xml b/launch/sensing_launch/launch/aip_customized/camera.launch.xml
@@ -16,7 +16,7 @@
         <param name="image_width" value="2880" />
         <param name="image_height" value="1860" />
         <param name="pixel_format" value="yuyv" />
-        <param name="framerate" value="24" />
+        <param name="frame_rate" value="24" />
       </node>
     </group>
   </group>

diff --git a/launch/sensing_launch/launch/aip_customized/gnss.launch.xml b/launch/sensing_launch/launch/aip_customized/gnss.launch.xml
@@ -2,29 +2,29 @@
 
   <arg name="launch_driver" default="true" />
 
-    <group>
-      <push-ros-namespace namespace="gnss"/>
+  <group>
+    <push-ros-namespace namespace="gnss"/>
 
-      <!-- Ublox Driver -->
-      <node pkg="ublox_gps" name="ublox" exec="ublox_gps_node" if="$(var launch_driver)">
-        <remap from="ublox/fix" to="ublox/nav_sat_fix" />
-        <param from="$(find-pkg-share ublox_gps)/c94_f9p_rover.yaml"/>
-      </node>
+    <!-- Ublox Driver -->
+    <node pkg="ublox_gps" name="ublox" exec="ublox_gps_node" if="$(var launch_driver)">
+      <remap from="ublox/fix" to="ublox/nav_sat_fix" />
+      <param from="$(find-pkg-share ublox_gps)/c94_f9p_rover.yaml"/>
+    </node>
 
-      <!-- NavSatFix to MGRS Pose -->
-      <include file="$(find-pkg-share gnss_poser)/launch/gnss_poser.launch.xml">
-        <arg name="input_topic_fix" value="ublox/nav_sat_fix" />
-        <arg name="input_topic_navpvt" value="ublox/navpvt" />
+    <!-- NavSatFix to MGRS Pose -->
+    <include file="$(find-pkg-share gnss_poser)/launch/gnss_poser.launch.xml">
+      <arg name="input_topic_fix" value="ublox/nav_sat_fix" />
+      <arg name="input_topic_navpvt" value="ublox/navpvt" />
 
-        <arg name="output_topic_gnss_pose" value="pose" />
-        <arg name="output_topic_gnss_pose_cov" value="pose_with_covariance" />
-        <arg name="output_topic_gnss_fixed" value="fixed" />
+      <arg name="output_topic_gnss_pose" value="pose" />
+      <arg name="output_topic_gnss_pose_cov" value="pose_with_covariance" />
+      <arg name="output_topic_gnss_fixed" value="fixed" />
 
-        <arg name="coordinate_system" value="1" doc="0:UTM, 1:MGRS, 2:PLANE" />
-        <arg name="use_ublox_receiver" value="true" />
+      <arg name="coordinate_system" value="1" doc="0:UTM, 1:MGRS, 2:PLANE" />
+      <arg name="use_ublox_receiver" value="true" />
 
-        <arg name="gnss_frame" value="gnss_link" />
-      </include>
+      <arg name="gnss_frame" value="gnss_link" />
+    </include>
 
-    </group>
+  </group>
 </launch>
diff --git a/launch/sensing_launch/launch/aip_customized/lidar.launch.xml b/launch/sensing_launch/launch/aip_customized/lidar.launch.xml
@@ -1,6 +1,9 @@
 <launch>
 
   <arg name="launch_driver" default="true" />
+  <arg name="use_concat_filter" default="true" />
+  <arg name="vehicle_param_file" />
+  <arg name="vehicle_mirror_param_file" />
 
   <group>
     <push-ros-namespace namespace="lidar"/>
@@ -12,6 +15,8 @@
         <arg name="device_ip" value="192.168.1.201"/>
         <arg name="port" value="2368"/>
         <arg name="launch_driver" value="$(var launch_driver)" />
+        <arg name="vehicle_param_file" value="$(var vehicle_param_file)" />
+        <arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)" />
       </include>
     </group>
 
@@ -22,6 +27,8 @@
         <arg name="device_ip" value="192.168.1.202"/>
         <arg name="port" value="2369"/>
         <arg name="launch_driver" value="$(var launch_driver)" />
+        <arg name="vehicle_param_file" value="$(var vehicle_param_file)" />
+        <arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)" />
       </include>
     </group>
 
@@ -32,11 +39,15 @@
         <arg name="device_ip" value="192.168.1.203"/>
         <arg name="port" value="2370"/>
         <arg name="launch_driver" value="$(var launch_driver)" />
+        <arg name="vehicle_param_file" value="$(var vehicle_param_file)" />
+        <arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)" />
       </include>
     </group>
 
     <include file="$(find-pkg-share sensing_launch)/launch/aip_customized/pointcloud_preprocessor.launch.py">
       <arg name="base_frame" value="base_link" />
+      <arg name="use_concat_filter" value="$(var use_concat_filter)" />
+      <arg name="vehicle_param_file" value="$(var vehicle_param_file)" />
     </include>
 
   </group>

diff --git a/launch/sensing_launch/launch/aip_customized/pointcloud_preprocessor.launch.py b/launch/sensing_launch/launch/aip_customized/pointcloud_preprocessor.launch.py
@@ -13,103 +13,167 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
+import os
+import yaml
+
 import launch
-from launch.actions import DeclareLaunchArgument
+from launch.actions import DeclareLaunchArgument, OpaqueFunction
+from launch.conditions import IfCondition, UnlessCondition
 from launch.substitutions import LaunchConfiguration
-from launch_ros.actions import ComposableNodeContainer
+from launch_ros.actions import ComposableNodeContainer, LoadComposableNodes
 from launch_ros.descriptions import ComposableNode
+from launch.substitutions import EnvironmentVariable
+
+def get_vehicle_info(context):
+    path = LaunchConfiguration('vehicle_param_file').perform(context)
+    with open(path, 'r') as f:
+        p = yaml.safe_load(f)['/**']['ros__parameters']
+    p['vehicle_length'] = p['front_overhang'] + p['wheel_base'] + p['rear_overhang']
+    p['vehicle_width'] = p['wheel_tread'] + p['left_overhang'] + p['right_overhang']
+    p['min_longitudinal_offset'] = -p['rear_overhang']
+    p['max_longitudinal_offset'] = p['front_overhang'] + p['wheel_base']
+    p['min_lateral_offset'] = -(p['wheel_tread'] / 2.0 + p['right_overhang'])
+    p['max_lateral_offset'] = p['wheel_tread'] / 2.0 + p['left_overhang']
+    p['min_height_offset'] = 0.0
+    p['max_height_offset'] = p['vehicle_height']
+    return p
+
+def get_vehicle_mirror_info(context):
+    path = LaunchConfiguration('vehicle_mirror_param_file').perform(context)
+    with open(path, 'r') as f:
+        p = yaml.safe_load(f)
+    return p
+
+
+def launch_setup(context, *args, **kwargs):
+
+    pkg = 'pointcloud_preprocessor'
+
+    vehicle_info = get_vehicle_info(context)
+
+    # set concat filter as a component
+    concat_component = ComposableNode(
+        package=pkg,
+        plugin='pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent',
+        name='concatenate_data',
+        remappings=[('output', 'concatenated/pointcloud')],
+        parameters=[{
+            'input_topics': ['/sensing/lidar/top/outlier_filtered/pointcloud',
+                            '/sensing/lidar/left/outlier_filtered/pointcloud',
+                            '/sensing/lidar/right/outlier_filtered/pointcloud'],
+            'output_frame': 'base_link',
+            'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', default_value='False'),
+        }]
+    )
+
+    passthrough_component = ComposableNode(
+        package=pkg,
+        plugin='pointcloud_preprocessor::PassThroughFilterComponent',
+        name='passthrough_filter',
+        remappings=[
+            ('input', 'top/outlier_filtered/pointcloud'),
+            ('output', 'concatenated/pointcloud'),
+        ],
+        parameters=[{
+            'output_frame': 'base_link',
+            'min_z': vehicle_info['min_height_offset'],
+            'max_z': vehicle_info['max_height_offset'],
+            'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', default_value='False'),
+        }]
+    )
+
+    # set crop box filter as a component
+    cropbox_component = ComposableNode(
+        package=pkg,
+        plugin='pointcloud_preprocessor::CropBoxFilterComponent',
+        name='crop_box_filter',
+        remappings=[
+            ('input', 'concatenated/pointcloud'),
+            ('output', 'measurement_range_cropped/pointcloud'),
+        ],
+        parameters=[{
+            'input_frame': LaunchConfiguration('base_frame'),
+            'output_frame': LaunchConfiguration('base_frame'),
+            'min_x': -50.0,
+            'max_x': 100.0,
+            'min_y': -50.0,
+            'max_y': 50.0,
+            'min_z': vehicle_info['min_height_offset'],
+            'max_z': vehicle_info['max_height_offset'],
+            'negative': False,
+            'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', default_value='False'),
+        }]
+    )
+
+    ground_component = ComposableNode(
+        package=pkg,
+        plugin='pointcloud_preprocessor::RayGroundFilterComponent',
+        name='ray_ground_filter',
+        remappings=[
+            ('input', 'measurement_range_cropped/pointcloud'),
+            ('output', 'no_ground/pointcloud')
+        ],
+        parameters=[{
+            "general_max_slope": 10.0,
+            "local_max_slope": 10.0,
+            "min_height_threshold": 0.2,
+            'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', default_value='False'),
+        }]
+    )
+
+    relay_component = ComposableNode(
+        package='topic_tools',
+        plugin='topic_tools::RelayNode',
+        name='relay',
+        parameters=[{
+            "input_topic": "/sensing/lidar/top/rectified/pointcloud",
+            "output_topic": "/sensing/lidar/pointcloud",
+            "type": "sensor_msgs/msg/PointCloud2",
+            'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', default_value='False'),
+        }],
+    )
+
+    # set container to run all required components in the same process
+    container = ComposableNodeContainer(
+        name='pointcloud_preprocessor_container',
+        namespace='pointcloud_preprocessor',
+        package='rclcpp_components',
+        executable='component_container',
+        composable_node_descriptions=[
+            cropbox_component,
+            ground_component,
+            relay_component,
+        ],
+        output='screen',
+        parameters=[{
+            'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', default_value='False'),
+        }],
+    )
+
+    # load concat or passthrough filter
+    concat_loader = LoadComposableNodes(
+        composable_node_descriptions=[concat_component],
+        target_container=container,
+        condition=launch.conditions.IfCondition(LaunchConfiguration('use_concat_filter')),
+    )
+
+    passthrough_loader = LoadComposableNodes(
+        composable_node_descriptions=[passthrough_component],
+        target_container=container,
+        condition=launch.conditions.UnlessCondition(LaunchConfiguration('use_concat_filter')),
+    )
+
+    return [container, concat_loader, passthrough_loader]
 
 def generate_launch_description():
 
-  pkg = 'pointcloud_preprocessor'
-
-  launch_arguments = []
-
-  def add_launch_arg(name: str, default_value=None):
-    # a default_value of None is equivalent to not passing that kwarg at all
-    launch_arguments.append(DeclareLaunchArgument(name, default_value=default_value))
-
-  add_launch_arg('base_frame', 'base_link')
-
-  # set concat filter as a component
-  concat_component = ComposableNode(
-      package=pkg,
-      plugin='pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent',
-      name='concatenate_data',
-      remappings=[('/output', 'concatenated/pointcloud')],
-      parameters=[
-          {
-              'input_topics': ['/sensing/lidar/top/outlier_filtered/pointcloud',
-                               '/sensing/lidar/left/outlier_filtered/pointcloud',
-                               '/sensing/lidar/right/outlier_filtered/pointcloud'],
-              'output_frame': 'base_link',
-          }
-      ]
-  )
-
-  # set crop box filter as a component
-  cropbox_component = ComposableNode(
-      package=pkg,
-      plugin='pointcloud_preprocessor::CropBoxFilterComponent',
-      name='crop_box_filter',
-      remappings=[
-          ('/input', 'concatenated/pointcloud'),
-          ('/output', 'mesurement_range_cropped/pointcloud'),
-          ('/min_z', '/vehicle_info/min_height_offset'),
-          ('/max_z', '/vehicle_info/max_height_offset'),
-      ],
-      parameters=[
-          {
-              'input_frame': LaunchConfiguration('base_frame'),
-              'output_frame': LaunchConfiguration('base_frame'),
-              'min_x': -50.0,
-              'max_x': 100.0,
-              'min_y': -50.0,
-              'max_y': 50.0,
-              'negative': False,
-          }
-      ]
-  )
-
-  ground_component = ComposableNode(
-      package=pkg,
-      plugin='pointcloud_preprocessor::RayGroundFilterComponent',
-      name='ray_ground_filter',
-      remappings=[
-          ('/input', 'mesurement_range_cropped/pointcloud'),
-          ('/output', 'no_ground/pointcloud')
-      ],
-      parameters=[{
-        "general_max_slope": 10.0,
-        "local_max_slope": 10.0,
-        "min_height_threshold": 0.2,
-      }]
-  )
-
-  relay_component = ComposableNode(
-      package='topic_tools',
-      plugin='topic_tools::RelayNode',
-      name='relay',
-      parameters=[{
-        "input_topic": "/sensing/lidar/top/rectified/pointcloud",
-        "output_topic": "/sensing/lidar/pointcloud",
-        "type": "sensor_msgs/msg/PointCloud2",
-      }],
-  )
-
-  # set container to run all required components in the same process
-  container = ComposableNodeContainer(
-      name='pointcloud_preprocessor_container',
-      namespace='pointcloud_preprocessor',
-      package='rclcpp_components',
-      executable='component_container',
-      composable_node_descriptions=[
-          concat_component,
-          cropbox_component,
-          ground_component,
-          relay_component,
-      ],
-      output='screen',
-  )
-
-  return launch.LaunchDescription(launch_arguments + [container])
+    launch_arguments = []
+
+    def add_launch_arg(name: str, default_value=None):
+        launch_arguments.append(DeclareLaunchArgument(name, default_value=default_value))
+
+    add_launch_arg('base_frame', 'base_link')
+    add_launch_arg('use_concat_filter', 'use_concat_filter')
+    add_launch_arg('vehicle_param_file')
+
+    return launch.LaunchDescription(launch_arguments + [OpaqueFunction(function=launch_setup)])
diff --git a/launch/sensing_launch/launch/aip_s1/camera.launch.xml b/launch/sensing_launch/launch/aip_s1/camera.launch.xml
@@ -16,7 +16,7 @@
         <param name="image_width" value="2880" />
         <param name="image_height" value="1860" />
         <param name="pixel_format" value="yuyv" />
-        <param name="framerate" value="24" />
+        <param name="frame_rate" value="24" />
       </node>
     </group>
   </group>
-Original file line number
+Diff line change
@@ Expand Up / @@ -12,4 +12,5 @@ endif() @@
     ament_auto_package(INSTALL_TO_SHARE
       launch
       data
+      config
     )