Skip to content

Commit

Permalink
feat: fix to be able to use nebula (#19)
Browse files Browse the repository at this point in the history 
* Merge https://github.com/knzo25/awsim_sensor_kit_launch/tree/test/awsim_nebula_integration

Signed-off-by: Shintaro Sakoda <[email protected]>

* Fixed some code

Signed-off-by: Shintaro Sakoda <[email protected]>

* Fixed order

Signed-off-by: Shintaro Sakoda <[email protected]>

* FIxed order

Signed-off-by: Shintaro Sakoda <[email protected]>

---------

Signed-off-by: Shintaro Sakoda <[email protected]>
  • Loading branch information
@SakodaShintaro
SakodaShintaro authored Oct 2, 2024
1 parent df41e52 commit aafb524
Show file tree
Hide file tree
Showing 5 changed files with 246 additions and 24 deletions.
36 changes: 30 additions & 6 deletions awsim_sensor_kit_launch/launch/lidar.launch.xml
View file
Original file line number Diff line number Diff line change
@@ -1,4 +1,6 @@
<launch>
<arg name="launch_driver" default="true"/>
<arg name="host_ip" default="255.255.255.255"/>
<arg name="use_concat_filter" default="true"/>
<arg name="vehicle_mirror_param_file"/>
<arg name="pointcloud_container_name" default="pointcloud_container"/>
Expand All @@ -9,27 +11,49 @@

<group>
<push-ros-namespace namespace="top"/>
<include file="$(find-pkg-share common_awsim_sensor_launch)/launch/velodyne_lidar.launch.xml">
<include file="$(find-pkg-share common_awsim_sensor_launch)/launch/velodyne_VLP16.launch.xml">
<arg name="max_range" value="250.0"/>
<arg name="sensor_frame" value="velodyne_top"/>
<arg name="sensor_ip" value="127.0.0.1"/>
<arg name="host_ip" value="$(var host_ip)"/>
<arg name="data_port" value="2368"/>
<arg name="scan_phase" value="300.0"/>
<arg name="launch_driver" value="$(var launch_driver)"/>
<arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)"/>
<arg name="use_distortion_corrector" value="$(var use_distortion_corrector)"/>
<arg name="container_name" value="pointcloud_container"/>
</include>
</group>

<group>
<push-ros-namespace namespace="left"/>
<include file="$(find-pkg-share common_awsim_sensor_launch)/launch/velodyne_lidar.launch.xml">
<include file="$(find-pkg-share common_awsim_sensor_launch)/launch/velodyne_VLP16.launch.xml">
<arg name="max_range" value="5.0"/>
<arg name="sensor_frame" value="velodyne_left"/>
<arg name="sensor_ip" value="127.0.0.1"/>
<arg name="host_ip" value="$(var host_ip)"/>
<arg name="data_port" value="2369"/>
<arg name="scan_phase" value="180.0"/>
<arg name="cloud_min_angle" value="300"/>
<arg name="cloud_max_angle" value="60"/>
<arg name="launch_driver" value="$(var launch_driver)"/>
<arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)"/>
<arg name="use_distortion_corrector" value="$(var use_distortion_corrector)"/>
<arg name="container_name" value="pointcloud_container"/>
</include>
</group>

<group>
<push-ros-namespace namespace="right"/>
<include file="$(find-pkg-share common_awsim_sensor_launch)/launch/velodyne_lidar.launch.xml">
<include file="$(find-pkg-share common_awsim_sensor_launch)/launch/velodyne_VLP16.launch.xml">
<arg name="max_range" value="5.0"/>
<arg name="sensor_frame" value="velodyne_right"/>
<arg name="sensor_ip" value="127.0.0.1"/>
<arg name="host_ip" value="$(var host_ip)"/>
<arg name="data_port" value="2370"/>
<arg name="scan_phase" value="180.0"/>
<arg name="cloud_min_angle" value="300"/>
<arg name="cloud_max_angle" value="60"/>
<arg name="launch_driver" value="$(var launch_driver)"/>
<arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)"/>
<arg name="use_distortion_corrector" value="$(var use_distortion_corrector)"/>
<arg name="container_name" value="pointcloud_container"/>
</include>
</group>
Expand Down
5 changes: 5 additions & 0 deletions common_awsim_sensor_launch/config/distortion_corrector_node.param.yaml
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,5 @@
/**:
ros__parameters:
base_frame: base_link
use_imu: true
use_3d_distortion_correction: false
176 changes: 172 additions & 4 deletions .../launch/velodyne_node_container.launch.py → ...ch/launch/nebula_node_container.launch.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,10 +23,17 @@
from launch_ros.actions import LoadComposableNodes
from launch_ros.descriptions import ComposableNode
from launch_ros.parameter_descriptions import ParameterFile
from launch_ros.substitutions import FindPackageShare
import yaml
from ament_index_python.packages import get_package_share_directory
from pathlib import Path

def get_lidar_make(sensor_name):
if sensor_name[:6].lower() == "pandar":
return "Hesai", ".csv"
elif sensor_name[:3].lower() in ["hdl", "vlp", "vls"]:
return "Velodyne", ".yaml"
return "unrecognized_sensor_model"

def get_vehicle_info(context):
# TODO(TIER IV): Use Parameter Substitution after we drop Galactic support
Expand Down Expand Up @@ -62,6 +69,99 @@ def create_parameter_dict(*args):

nodes = []

# Model and make
sensor_model = LaunchConfiguration("sensor_model").perform(context)
sensor_make, sensor_extension = get_lidar_make(sensor_model)
nebula_decoders_share_dir = Path(get_package_share_directory("nebula_decoders"))

# Calibration file
sensor_calib_fp = (
nebula_decoders_share_dir
/ "calibration"
/ sensor_make.lower()
/ (sensor_model + sensor_extension)
)
assert (
sensor_calib_fp.exists()
), f"Sensor calib file under calibration/ was not found: {sensor_calib_fp}"
sensor_calib_fp = str(sensor_calib_fp)

nodes = []

nodes.append(
ComposableNode(
package="glog_component",
plugin="GlogComponent",
name="glog_component",
)
)

nodes.append(
ComposableNode(
package="nebula_ros",
plugin=sensor_make + "DriverRosWrapper",
name=sensor_make.lower() + "_driver_ros_wrapper_node",
parameters=[
{
"calibration_file": sensor_calib_fp,
"sensor_model": sensor_model,
**create_parameter_dict(
"host_ip",
"sensor_ip",
"data_port",
"return_mode",
"min_range",
"max_range",
"frame_id",
"scan_phase",
"cloud_min_angle",
"cloud_max_angle",
"dual_return_distance_threshold",
"setup_sensor",
"retry_hw",
),
},
],
remappings=[
# cSpell:ignore knzo25
# TODO(knzo25): fix the remapping once nebula gets updated
("pandar_points", "pointcloud_raw_ex"),
("velodyne_points", "pointcloud_raw_ex"),
],
extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
)
)

nodes.append(
ComposableNode(
package="nebula_ros",
plugin=sensor_make + "HwMonitorRosWrapper",
name=sensor_make.lower() + "_hw_monitor_ros_wrapper_node",
parameters=[
{
"sensor_model": sensor_model,
**create_parameter_dict(
"return_mode",
"frame_id",
"scan_phase",
"sensor_ip",
"host_ip",
"data_port",
"gnss_port",
"packet_mtu_size",
"rotation_speed",
"cloud_min_angle",
"cloud_max_angle",
"diag_span",
"dual_return_distance_threshold",
"delay_monitor_ms",
),
},
],
extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
)
)

cropbox_parameters = create_parameter_dict("input_frame", "output_frame")
cropbox_parameters["negative"] = True

Expand Down Expand Up @@ -147,6 +247,45 @@ def create_parameter_dict(*args):
composable_node_descriptions=nodes,
)

driver_component = ComposableNode(
package="nebula_ros",
plugin=sensor_make + "HwInterfaceRosWrapper",
# node is created in a global context, need to avoid name clash
name=sensor_make.lower() + "_hw_interface_ros_wrapper_node",
parameters=[
{
"sensor_model": sensor_model,
"calibration_file": sensor_calib_fp,
**create_parameter_dict(
"sensor_ip",
"host_ip",
"scan_phase",
"return_mode",
"frame_id",
"rotation_speed",
"data_port",
"gnss_port",
"cloud_min_angle",
"cloud_max_angle",
"packet_mtu_size",
"dual_return_distance_threshold",
"setup_sensor",
"ptp_profile",
"ptp_transport_type",
"ptp_switch_type",
"ptp_domain",
"retry_hw",
),
}
],
)

driver_component_loader = LoadComposableNodes(
composable_node_descriptions=[driver_component],
target_container=container,
condition=IfCondition(LaunchConfiguration("launch_driver")),
)

distortion_component = ComposableNode(
package="autoware_pointcloud_preprocessor",
plugin="autoware::pointcloud_preprocessor::DistortionCorrectorComponent",
Expand Down Expand Up @@ -185,7 +324,7 @@ def create_parameter_dict(*args):
condition=launch.conditions.UnlessCondition(LaunchConfiguration("use_distortion_corrector")),
)

return [container, distortion_loader, distortion_relay_loader]
return [container, driver_component_loader, distortion_loader, distortion_relay_loader]


def generate_launch_description():
Expand All @@ -201,21 +340,50 @@ def add_launch_arg(name: str, default_value=None, description=None):
get_package_share_directory("common_awsim_sensor_launch")
)

add_launch_arg("sensor_model", description="sensor model name")
add_launch_arg("config_file", "", description="sensor configuration file")
add_launch_arg("launch_driver", "True", "do launch driver")
add_launch_arg("setup_sensor", "True", "configure sensor")
add_launch_arg("retry_hw", "false", "retry hw")
add_launch_arg("sensor_ip", "192.168.1.201", "device ip address")
add_launch_arg("host_ip", "255.255.255.255", "host ip address")
add_launch_arg("scan_phase", "0.0")
add_launch_arg("base_frame", "base_link", "base frame id")
add_launch_arg("container_name", "velodyne_composable_node_container", "container name")
add_launch_arg("min_range", "0.3", "minimum view range for Velodyne sensors")
add_launch_arg("max_range", "300.0", "maximum view range for Velodyne sensors")
add_launch_arg("cloud_min_angle", "0", "minimum view angle setting on device")
add_launch_arg("cloud_max_angle", "360", "maximum view angle setting on device")
add_launch_arg("data_port", "2368", "device data port number")
add_launch_arg("gnss_port", "2380", "device gnss port number")
add_launch_arg("packet_mtu_size", "1500", "packet mtu size")
add_launch_arg("rotation_speed", "600", "rotational frequency")
add_launch_arg("dual_return_distance_threshold", "0.1", "dual return distance threshold")
add_launch_arg("frame_id", "lidar", "frame id")
add_launch_arg("input_frame", LaunchConfiguration("base_frame"), "use for cropbox")
add_launch_arg("output_frame", LaunchConfiguration("base_frame"), "use for cropbox")
add_launch_arg(
"vehicle_mirror_param_file", description="path to the file of vehicle mirror position yaml"
)
add_launch_arg("use_multithread", "False", "use multithread")
add_launch_arg("use_intra_process", "False", "use ROS2 component container communication")
add_launch_arg("use_intra_process", "False", "use ROS 2 component container communication")
add_launch_arg("output_as_sensor_frame", "False", "output final pointcloud in sensor frame")
add_launch_arg("frame_id", "base_link", "frame id")
add_launch_arg(
"ring_outlier_filter_node_param_path",
str(common_sensor_share_dir / "config" / "ring_outlier_filter_node.param.yaml"),
description="path to parameter file of ring outlier filter node",
description="path to parameter file of ring outlier filter node")
add_launch_arg("container_name", "nebula_node_container")
add_launch_arg("ptp_profile", "1588v2")
add_launch_arg("ptp_transport_type", "L2")
add_launch_arg("ptp_switch_type", "TSN")
add_launch_arg("ptp_domain", "0")
add_launch_arg("output_as_sensor_frame", "True", "output final pointcloud in sensor frame")
add_launch_arg("diag_span", "1000", "")
add_launch_arg("delay_monitor_ms", "2000", "")

add_launch_arg(
"distortion_corrector_node_param_file",
[FindPackageShare("common_sensor_launch"), "/config/distortion_corrector_node.param.yaml"],
)

set_container_executable = SetLaunchConfiguration(
Expand Down
39 changes: 39 additions & 0 deletions common_awsim_sensor_launch/launch/velodyne_VLP16.launch.xml
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,39 @@
<launch>

<!-- Params -->
<arg name="launch_driver" default="true"/>

<arg name="model" default="VLP16"/>
<arg name="max_range" default="130.0"/>
<arg name="min_range" default="0.4"/>
<arg name="sensor_frame" default="velodyne"/>
<arg name="return_mode" default="SingleStrongest"/>
<arg name="sensor_ip" default="192.168.1.201"/>
<arg name="host_ip" default="255.255.255.255"/>
<arg name="data_port" default="2368"/>
<arg name="scan_phase" default="0.0"/>
<arg name="cloud_min_angle" default="0"/>
<arg name="cloud_max_angle" default="360"/>
<arg name="vehicle_mirror_param_file"/>
<arg name="container_name" default="velodyne_node_container"/>

<include file="$(find-pkg-share common_awsim_sensor_launch)/launch/nebula_node_container.launch.py">
<arg name="launch_driver" value="$(var launch_driver)"/>
<arg name="sensor_model" value="$(var model)"/>
<arg name="return_mode" value="$(var return_mode)"/>
<arg name="max_range" value="$(var max_range)"/>
<arg name="min_range" value="$(var min_range)"/>
<arg name="frame_id" value="$(var sensor_frame)"/>
<arg name="sensor_ip" value="$(var sensor_ip)"/>
<arg name="host_ip" value="$(var host_ip)"/>
<arg name="data_port" value="$(var data_port)"/>
<arg name="scan_phase" value="$(var scan_phase)"/>
<arg name="cloud_min_angle" value="$(var cloud_min_angle)"/>
<arg name="cloud_max_angle" value="$(var cloud_max_angle)"/>
<arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)"/>
<arg name="use_intra_process" value="true"/>
<arg name="use_multithread" value="true"/>
<arg name="container_name" value="$(var container_name)"/>
</include>

</launch>
14 changes: 0 additions & 14 deletions common_awsim_sensor_launch/launch/velodyne_lidar.launch.xml
View file

This file was deleted.

0 comments on commit aafb524

Please sign in to comment.