Skip to content

Commit

Permalink
fix: modify nebula_node_container.launch.py to use nebula v0.2.1 (#360
Browse files Browse the repository at this point in the history 
)

* Fixed nebula_node_container.launch.py to use nebula v0.2.1

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

* ci(pre-commit): autofix

* Fixed to reduce diff

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

* Restore driver_component_loader and blockage_diag_loader

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

* ci(pre-commit): autofix

* Added blockage_diagnostics_param_file

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

* Restore pandar_points remapping

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

* Restore used parameters

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

* Restore retry_hw

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

---------

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
  • Loading branch information
@SakodaShintaro @pre-commit-ci
SakodaShintaro and pre-commit-ci[bot] authored Jan 23, 2025
1 parent 3192a8d commit 98b0689
Showing 1 changed file with 65 additions and 76 deletions.
141 changes: 65 additions & 76 deletions common_sensor_launch/launch/nebula_node_container.launch.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,7 +26,6 @@
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


Expand All @@ -35,6 +34,8 @@ def get_lidar_make(sensor_name):
return "Hesai", ".csv"
elif sensor_name[:3].lower() in ["hdl", "vlp", "vls"]:
return "Velodyne", ".yaml"
elif sensor_name.lower() in ["helios", "bpearl"]:
return "Robosense", None
return "unrecognized_sensor_model"


Expand All @@ -56,6 +57,13 @@ def get_vehicle_info(context):
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)["/**"]["ros__parameters"]
return p


def launch_setup(context, *args, **kwargs):
def load_composable_node_param(param_path):
with open(LaunchConfiguration(param_path).perform(context), "r") as f:
Expand All @@ -73,15 +81,28 @@ def create_parameter_dict(*args):
nebula_decoders_share_dir = get_package_share_directory("nebula_decoders")

# Calibration file
sensor_calib_fp = os.path.join(
nebula_decoders_share_dir,
"calibration",
sensor_make.lower(),
sensor_model + sensor_extension,
if sensor_extension is not None: # Velodyne and Hesai
sensor_calib_fp = os.path.join(
nebula_decoders_share_dir,
"calibration",
sensor_make.lower(),
sensor_model + sensor_extension,
)
assert os.path.exists(
sensor_calib_fp
), "Sensor calib file under calibration/ was not found: {}".format(sensor_calib_fp)
else: # Robosense
sensor_calib_fp = ""

# Pointcloud preprocessor parameters
distortion_corrector_node_param = ParameterFile(
param_file=LaunchConfiguration("distortion_correction_node_param_path").perform(context),
allow_substs=True,
)
ring_outlier_filter_node_param = ParameterFile(
param_file=LaunchConfiguration("ring_outlier_filter_node_param_path").perform(context),
allow_substs=True,
)
assert os.path.exists(
sensor_calib_fp
), "Sensor calib file under calibration/ was not found: {}".format(sensor_calib_fp)

nodes = []

Expand All @@ -96,16 +117,18 @@ def create_parameter_dict(*args):
nodes.append(
ComposableNode(
package="nebula_ros",
plugin=sensor_make + "DriverRosWrapper",
name=sensor_make.lower() + "_driver_ros_wrapper_node",
plugin=sensor_make + "RosWrapper",
name=sensor_make.lower() + "_ros_wrapper_node",
parameters=[
{
"calibration_file": sensor_calib_fp,
"sensor_model": sensor_model,
"launch_hw": LaunchConfiguration("launch_driver"),
**create_parameter_dict(
"host_ip",
"sensor_ip",
"data_port",
"gnss_port",
"return_mode",
"min_range",
"max_range",
Expand All @@ -114,8 +137,9 @@ def create_parameter_dict(*args):
"cloud_min_angle",
"cloud_max_angle",
"dual_return_distance_threshold",
"rotation_speed",
"packet_mtu_size",
"setup_sensor",
"retry_hw",
),
},
],
Expand All @@ -129,45 +153,6 @@ def create_parameter_dict(*args):
)
)

# There is an issue where hw_monitor crashes due to data race,
# so the monitor will now only be launched when explicitly specified with a launch command.
launch_hw_monitor: bool = IfCondition(LaunchConfiguration("launch_hw_monitor")).evaluate(
context
)
launch_driver: bool = IfCondition(LaunchConfiguration("launch_driver")).evaluate(context)
if launch_hw_monitor and launch_driver:
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 All @@ -193,7 +178,7 @@ def create_parameter_dict(*args):
)
)

mirror_info = load_composable_node_param("vehicle_mirror_param_file")
mirror_info = get_vehicle_mirror_info(context)
cropbox_parameters["min_x"] = mirror_info["min_longitudinal_offset"]
cropbox_parameters["max_x"] = mirror_info["max_longitudinal_offset"]
cropbox_parameters["min_y"] = mirror_info["min_lateral_offset"]
Expand Down Expand Up @@ -226,16 +211,11 @@ def create_parameter_dict(*args):
("~/input/pointcloud", "mirror_cropped/pointcloud_ex"),
("~/output/pointcloud", "rectified/pointcloud_ex"),
],
parameters=[load_composable_node_param("distortion_corrector_node_param_file")],
parameters=[distortion_corrector_node_param],
extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
)
)

ring_outlier_filter_node_param = ParameterFile(
param_file=LaunchConfiguration("ring_outlier_filter_node_param_file").perform(context),
allow_substs=True,
)

# Ring Outlier Filter is the last component in the pipeline, so control the output frame here
if LaunchConfiguration("output_as_sensor_frame").perform(context).lower() == "true":
ring_outlier_output_frame = {"output_frame": LaunchConfiguration("frame_id")}
Expand Down Expand Up @@ -348,14 +328,11 @@ def add_launch_arg(name: str, default_value=None, description=None):
DeclareLaunchArgument(name, default_value=default_value, description=description)
)

common_sensor_share_dir = get_package_share_directory("common_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(
"launch_hw_monitor",
"False",
"do launch hardware monitor. Due to an issue where hw_monitor crashes due to data conflicts, the monitor in launched only when explicitly specified",
)
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")
Expand All @@ -374,37 +351,49 @@ def add_launch_arg(name: str, default_value=None, description=None):
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 ROS 2 component container communication")
add_launch_arg("container_name", "nebula_node_container")
add_launch_arg("lidar_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("output_as_sensor_frame", "True", "output final pointcloud in sensor frame")

add_launch_arg("enable_blockage_diag", "true")
add_launch_arg("horizontal_ring_id", "64")
add_launch_arg("vertical_bins", "128")
add_launch_arg("is_channel_order_top2down", "true")
add_launch_arg("horizontal_resolution", "0.4")
add_launch_arg(
"blockage_diagnostics_param_file",
[FindPackageShare("common_sensor_launch"), "/config/blockage_diagnostics.param.yaml"],
os.path.join(
common_sensor_share_dir,
"config",
"blockage_diagnostics.param.yaml",
),
description="path to parameter file of blockage diagnostics node",
)
add_launch_arg(
"vehicle_mirror_param_file",
description="path to the file of vehicle mirror position yaml",
)
add_launch_arg(
"distortion_corrector_node_param_file",
[FindPackageShare("common_sensor_launch"), "/config/distortion_corrector_node.param.yaml"],
"distortion_correction_node_param_path",
os.path.join(
common_sensor_share_dir,
"config",
"distortion_corrector_node.param.yaml",
),
description="path to parameter file of distortion correction node",
)
add_launch_arg(
"ring_outlier_filter_node_param_file",
[FindPackageShare("common_sensor_launch"), "/config/ring_outlier_filter_node.param.yaml"],
"ring_outlier_filter_node_param_path",
os.path.join(
common_sensor_share_dir,
"config",
"ring_outlier_filter_node.param.yaml",
),
description="path to parameter file of ring outlier filter node",
)

set_container_executable = SetLaunchConfiguration(
Expand Down

0 comments on commit 98b0689

Please sign in to comment.