Skip to content

Commit

Permalink
fix(aip_x2_launch): read callibration from individual_params (#255)
Browse files Browse the repository at this point in the history
* read callibration from individual_params

Signed-off-by: kotaro-hihara <[email protected]>

* ci(pre-commit): autofix

---------

Signed-off-by: kotaro-hihara <[email protected]>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
  • Loading branch information
2 people authored and Autumn60 committed Sep 17, 2024
1 parent 67bfa17 commit af3e883
Show file tree
Hide file tree
Showing 2 changed files with 11 additions and 22 deletions.
16 changes: 8 additions & 8 deletions aip_x2_launch/launch/lidar.launch.xml
Original file line number Diff line number Diff line change
Expand Up @@ -37,7 +37,7 @@
<!--arg name="is_channel_order_top2down" value="true"/-->
<!--arg name="horizontal_resolution" value="0.4"/-->
<!--arg name="enable_blockage_diag" value="$(var enable_blockage_diag)"/-->
<!--arg name="calibration" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x2/pandar/front_lower.csv" /-->
<arg name="calibration_file" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x2/pandar/front_lower.csv" />
<arg name="launch_driver" value="$(var launch_driver)" />
<!--arg name="dual_return_filter_param_file" value="$(var dual_return_filter_param_file)" /-->
<arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)" />
Expand Down Expand Up @@ -70,7 +70,7 @@
<!--arg name="is_channel_order_top2down" value="true"/-->
<!--arg name="horizontal_resolution" value="0.4"/-->
<!--arg name="enable_blockage_diag" value="$(var enable_blockage_diag)"/-->
<!--arg name="calibration" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x2/pandar/front_lower.csv" /-->
<arg name="calibration_file" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x2/pandar/front_upper.csv" />
<arg name="launch_driver" value="$(var launch_driver)" />
<!--arg name="dual_return_filter_param_file" value="$(var dual_return_filter_param_file)" /-->
<arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)" />
Expand Down Expand Up @@ -103,7 +103,7 @@
<!--arg name="is_channel_order_top2down" value="true"/-->
<!--arg name="horizontal_resolution" value="0.4"/-->
<!--arg name="enable_blockage_diag" value="$(var enable_blockage_diag)"/-->
<!--arg name="calibration" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x2/pandar/front_lower.csv" /-->
<arg name="calibration_file" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x2/pandar/left_upper.csv" />
<arg name="launch_driver" value="$(var launch_driver)" />
<!--arg name="dual_return_filter_param_file" value="$(var dual_return_filter_param_file)" /-->
<arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)" />
Expand Down Expand Up @@ -136,7 +136,7 @@
<!--arg name="is_channel_order_top2down" value="true"/-->
<!--arg name="horizontal_resolution" value="0.4"/-->
<!--arg name="enable_blockage_diag" value="$(var enable_blockage_diag)"/-->
<!--arg name="calibration" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x2/pandar/front_lower.csv" /-->
<arg name="calibration_file" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x2/pandar/left_lower.csv" />
<arg name="launch_driver" value="$(var launch_driver)" />
<!--arg name="dual_return_filter_param_file" value="$(var dual_return_filter_param_file)" /-->
<arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)" />
Expand Down Expand Up @@ -169,7 +169,7 @@
<!--arg name="is_channel_order_top2down" value="true"/-->
<!--arg name="horizontal_resolution" value="0.4"/-->
<!--arg name="enable_blockage_diag" value="$(var enable_blockage_diag)"/-->
<!--arg name="calibration" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x2/pandar/front_lower.csv" /-->
<arg name="calibration_file" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x2/pandar/right_upper.csv" />
<arg name="launch_driver" value="$(var launch_driver)" />
<!--arg name="dual_return_filter_param_file" value="$(var dual_return_filter_param_file)" /-->
<arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)" />
Expand Down Expand Up @@ -202,7 +202,7 @@
<!--arg name="is_channel_order_top2down" value="true"/-->
<!--arg name="horizontal_resolution" value="0.4"/-->
<!--arg name="enable_blockage_diag" value="$(var enable_blockage_diag)"/-->
<!--arg name="calibration" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x2/pandar/front_lower.csv" /-->
<arg name="calibration_file" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x2/pandar/right_lower.csv" />
<arg name="launch_driver" value="$(var launch_driver)" />
<!--arg name="dual_return_filter_param_file" value="$(var dual_return_filter_param_file)" /-->
<arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)" />
Expand Down Expand Up @@ -235,7 +235,7 @@
<!--arg name="is_channel_order_top2down" value="true"/-->
<!--arg name="horizontal_resolution" value="0.4"/-->
<!--arg name="enable_blockage_diag" value="$(var enable_blockage_diag)"/-->
<!--arg name="calibration" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x2/pandar/front_lower.csv" /-->
<arg name="calibration_file" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x2/pandar/rear_lower.csv" />
<arg name="launch_driver" value="$(var launch_driver)" />
<!--arg name="dual_return_filter_param_file" value="$(var dual_return_filter_param_file)" /-->
<arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)" />
Expand Down Expand Up @@ -268,7 +268,7 @@
<!--arg name="is_channel_order_top2down" value="true"/-->
<!--arg name="horizontal_resolution" value="0.4"/-->
<!--arg name="enable_blockage_diag" value="$(var enable_blockage_diag)"/-->
<!--arg name="calibration" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x2/pandar/front_lower.csv" /-->
<arg name="calibration_file" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x2/pandar/rear_upper.csv" />
<arg name="launch_driver" value="$(var launch_driver)" />
<!--arg name="dual_return_filter_param_file" value="$(var dual_return_filter_param_file)" /-->
<arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)" />
Expand Down
17 changes: 3 additions & 14 deletions aip_x2_launch/launch/nebula_node_container.launch.py
Original file line number Diff line number Diff line change
Expand Up @@ -75,18 +75,6 @@ def create_parameter_dict(*args):
# Model and make
sensor_model = LaunchConfiguration("sensor_model").perform(context)
sensor_make, sensor_extension = get_lidar_make(sensor_model)
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,
)
assert os.path.exists(
sensor_calib_fp
), "Sensor calib file under calibration/ was not found: {}".format(sensor_calib_fp)

nodes = []

Expand Down Expand Up @@ -136,7 +124,6 @@ def create_parameter_dict(*args):
name=sensor_make.lower() + "_driver_ros_wrapper_node",
parameters=[
{
"calibration_file": sensor_calib_fp,
"sensor_model": sensor_model,
**create_parameter_dict(
"host_ip",
Expand All @@ -157,6 +144,7 @@ def create_parameter_dict(*args):
"ptp_transport_type",
"ptp_switch_type",
"ptp_domain",
"calibration_file",
),
"launch_hw": True,
},
Expand Down Expand Up @@ -290,7 +278,6 @@ def create_parameter_dict(*args):
parameters=[
{
"sensor_model": sensor_model,
"calibration_file": sensor_calib_fp,
**create_parameter_dict(
"sensor_ip",
"host_ip",
Expand All @@ -309,6 +296,7 @@ def create_parameter_dict(*args):
"ptp_transport_type",
"ptp_switch_type",
"ptp_domain",
"calibration_file",
),
}
],
Expand Down Expand Up @@ -360,6 +348,7 @@ def add_launch_arg(name: str, default_value=None, description=None):
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("calibration_file", "")

set_container_executable = SetLaunchConfiguration(
"container_executable",
Expand Down

0 comments on commit af3e883

Please sign in to comment.