diff --git a/aip_x1_launch/config/concatenate_and_time_sync_node.param.yaml b/aip_x1_launch/config/concatenate_and_time_sync_node.param.yaml
index cb5e873c..a31bc4ea 100644
--- a/aip_x1_launch/config/concatenate_and_time_sync_node.param.yaml
+++ b/aip_x1_launch/config/concatenate_and_time_sync_node.param.yaml
@@ -2,8 +2,7 @@
ros__parameters:
debug_mode: false
has_static_tf_only: false
- rosbag_replay: false
- rosbag_length: 20.0
+ rosbag_length: 10.0
maximum_queue_size: 5
timeout_sec: 0.2
is_motion_compensated: true
diff --git a/aip_x1_launch/launch/lidar.launch.xml b/aip_x1_launch/launch/lidar.launch.xml
index f1796100..600b5121 100644
--- a/aip_x1_launch/launch/lidar.launch.xml
+++ b/aip_x1_launch/launch/lidar.launch.xml
@@ -48,10 +48,10 @@
-
+
diff --git a/aip_x1_launch/launch/pointcloud_preprocessor.launch.py b/aip_x1_launch/launch/pointcloud_preprocessor.launch.py
index ff400622..849b3e7e 100644
--- a/aip_x1_launch/launch/pointcloud_preprocessor.launch.py
+++ b/aip_x1_launch/launch/pointcloud_preprocessor.launch.py
@@ -12,9 +12,7 @@
# See the License for the specific language governing permissions and
# limitations under the License.
-import os
-from ament_index_python.packages import get_package_share_directory
import launch
from launch.actions import DeclareLaunchArgument
from launch.actions import OpaqueFunction
@@ -64,19 +62,10 @@ def generate_launch_description():
def add_launch_arg(name: str, default_value=None):
launch_arguments.append(DeclareLaunchArgument(name, default_value=default_value))
- aip_x1_launch_share_dir = get_package_share_directory("aip_x1_launch")
-
add_launch_arg("use_multithread", "False")
add_launch_arg("use_intra_process", "False")
add_launch_arg("pointcloud_container_name", "pointcloud_container")
- add_launch_arg(
- "concatenate_and_time_sync_node_param_path",
- os.path.join(
- aip_x1_launch_share_dir,
- "config",
- "concatenate_and_time_sync_node.param.yaml",
- ),
- )
+ add_launch_arg("concatenate_and_time_sync_node_param_path")
set_container_executable = SetLaunchConfiguration(
"container_executable",
diff --git a/aip_x2_launch/config/concatenate_and_time_sync_node.param.yaml b/aip_x2_launch/config/concatenate_and_time_sync_node.param.yaml
deleted file mode 100644
index f726354b..00000000
--- a/aip_x2_launch/config/concatenate_and_time_sync_node.param.yaml
+++ /dev/null
@@ -1,27 +0,0 @@
-/**:
- ros__parameters:
- debug_mode: false
- has_static_tf_only: false
- rosbag_replay: false
- rosbag_length: 20.0
- maximum_queue_size: 5
- timeout_sec: 0.2
- is_motion_compensated: true
- publish_synchronized_pointcloud: true
- keep_input_frame_in_synchronized_pointcloud: true
- publish_previous_but_late_pointcloud: false
- synchronized_pointcloud_postfix: pointcloud
- input_twist_topic_type: twist
- input_topics: [
- "/sensing/lidar/rear_upper/pointcloud_before_sync", # 0.044
- "/sensing/lidar/rear_lower/pointcloud_before_sync", # 0.049
- "/sensing/lidar/left_upper/pointcloud_before_sync", # 0.05
- "/sensing/lidar/left_lower/pointcloud_before_sync", # 0.05
- "/sensing/lidar/front_upper/pointcloud_before_sync", # 0.075
- "/sensing/lidar/front_lower/pointcloud_before_sync", # 0.074
- "/sensing/lidar/right_upper/pointcloud_before_sync", # 0.090
- "/sensing/lidar/right_lower/pointcloud_before_sync", # 0.00
- ]
- output_frame: base_link
- lidar_timestamp_offsets: [0.0, 0.005, 0.006, 0.006, 0.031, 0.03, 0.046, 0.056]
- lidar_timestamp_noise_window: [0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01]
diff --git a/aip_x2_launch/launch/lidar.launch.xml b/aip_x2_launch/launch/lidar.launch.xml
index 9ccf52af..4c86d954 100644
--- a/aip_x2_launch/launch/lidar.launch.xml
+++ b/aip_x2_launch/launch/lidar.launch.xml
@@ -210,10 +210,10 @@
-
+
diff --git a/aip_x2_launch/launch/pointcloud_preprocessor.launch.py b/aip_x2_launch/launch/pointcloud_preprocessor.launch.py
index 3f9705cb..d59ab15c 100644
--- a/aip_x2_launch/launch/pointcloud_preprocessor.launch.py
+++ b/aip_x2_launch/launch/pointcloud_preprocessor.launch.py
@@ -12,9 +12,7 @@
# See the License for the specific language governing permissions and
# limitations under the License.
-import os
-from ament_index_python.packages import get_package_share_directory
import launch
from launch.actions import DeclareLaunchArgument
from launch.actions import OpaqueFunction
@@ -65,20 +63,10 @@ def generate_launch_description():
def add_launch_arg(name: str, default_value=None):
launch_arguments.append(DeclareLaunchArgument(name, default_value=default_value))
- aip_x2_launch_share_dir = get_package_share_directory("aip_x2_launch")
-
add_launch_arg("use_multithread", "True")
add_launch_arg("use_intra_process", "True")
add_launch_arg("pointcloud_container_name", "pointcloud_container")
-
- add_launch_arg(
- "concatenate_and_time_sync_node_param_path",
- os.path.join(
- aip_x2_launch_share_dir,
- "config",
- "concatenate_and_time_sync_node.param.yaml",
- ),
- )
+ add_launch_arg("concatenate_and_time_sync_node_param_path")
set_container_executable = SetLaunchConfiguration(
"container_executable",
diff --git a/aip_xx1_gen2_launch/config/concatenate_and_time_sync_node.param.yaml b/aip_xx1_gen2_launch/config/concatenate_and_time_sync_node.param.yaml
deleted file mode 100644
index bd1dc498..00000000
--- a/aip_xx1_gen2_launch/config/concatenate_and_time_sync_node.param.yaml
+++ /dev/null
@@ -1,24 +0,0 @@
-/**:
- ros__parameters:
- debug_mode: false
- has_static_tf_only: false
- rosbag_replay: false
- rosbag_length: 20.0
- maximum_queue_size: 5
- timeout_sec: 0.2
- is_motion_compensated: false # no need to compensate for motion as lidar scan at the same time.
- publish_synchronized_pointcloud: true
- keep_input_frame_in_synchronized_pointcloud: true
- publish_previous_but_late_pointcloud: false
- synchronized_pointcloud_postfix: pointcloud
- input_twist_topic_type: twist
- input_topics: [
- "/sensing/lidar/top/pointcloud_before_sync", # 0.99
- "/sensing/lidar/front_left/pointcloud_before_sync", # 0.99
- "/sensing/lidar/front_right/pointcloud_before_sync", # 0.99
- "/sensing/lidar/side_left/pointcloud_before_sync", # 0.99
- "/sensing/lidar/side_right/pointcloud_before_sync", # 0.99
- ]
- output_frame: base_link
- lidar_timestamp_offsets: [0.0, 0.0, 0.0, 0.0, 0.0]
- lidar_timestamp_noise_window: [0.01, 0.01, 0.01, 0.01, 0.01]
diff --git a/aip_xx1_gen2_launch/config/lidar_gen2.yaml b/aip_xx1_gen2_launch/config/lidar_gen2.yaml
index 8480292f..84e3e102 100644
--- a/aip_xx1_gen2_launch/config/lidar_gen2.yaml
+++ b/aip_xx1_gen2_launch/config/lidar_gen2.yaml
@@ -50,20 +50,3 @@ launches:
scan_phase: 270.0
cloud_min_angle: 90
cloud_max_angle: 270
-
-preprocessor:
- input_topics:
- - /sensing/lidar/top/pointcloud_before_sync
- - /sensing/lidar/side_left/pointcloud_before_sync
- - /sensing/lidar/side_right/pointcloud_before_sync
- - /sensing/lidar/front_left/pointcloud_before_sync
- - /sensing/lidar/front_right/pointcloud_before_sync
- input_offset:
- - 0.035
- - 0.025
- - 0.025
- - 0.025
- - 0.025
- timeout_sec: 0.095
- input_twist_topic_type: twist
- publish_synchronized_pointcloud: true
diff --git a/aip_xx1_gen2_launch/launch/lidar.launch.py b/aip_xx1_gen2_launch/launch/lidar.launch.py
index 81a791b7..6cb981b9 100644
--- a/aip_xx1_gen2_launch/launch/lidar.launch.py
+++ b/aip_xx1_gen2_launch/launch/lidar.launch.py
@@ -15,9 +15,8 @@
from copy import deepcopy
import os
-from typing import Any
-from typing import List
+from launch_ros.substitutions import FindPackageShare
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
@@ -32,15 +31,6 @@
import yaml
-def join_list_of_arguments(arguments: List[Any]) -> str:
- """Join a list of arguments into a string, used by Include Launch Description.
-
- Example:
- join_list_of_arguments([1,2,3]) -> "[1, 2, 3]"
- """
- return f"[{', '.join([str(arg) for arg in arguments])}]"
-
-
def generate_launch_dictionary():
path_dictionary = {
"hesai_OT128": AnyLaunchDescriptionSource(
@@ -122,7 +112,7 @@ def load_yaml(yaml_file_path):
)
sub_launch_actions.append(sub_launch_action)
- processor_dict = config["preprocessor"]
+ # processor_dict = config["preprocessor"]
sub_launch_actions.append(
IncludeLaunchDescription(
PythonLaunchDescriptionSource(
@@ -133,18 +123,19 @@ def load_yaml(yaml_file_path):
)
),
launch_arguments=[
- ("base_frame", "base_link"),
("use_multithread", "true"),
("use_intra_process", "true"),
("use_pointcloud_container", LaunchConfiguration("use_pointcloud_container")),
("pointcloud_container_name", LaunchConfiguration("pointcloud_container_name")),
- ("input_topics", join_list_of_arguments(processor_dict["input_topics"])),
- ("input_offset", join_list_of_arguments(processor_dict["input_offset"])),
- ("timeout_sec", str(processor_dict["timeout_sec"])),
- ("input_twist_topic_type", str(processor_dict["input_twist_topic_type"])),
(
- "publish_synchronized_pointcloud",
- str(processor_dict["publish_synchronized_pointcloud"]),
+ "concatenate_and_time_sync_node_param_path",
+ os.path.join(
+ FindPackageShare("individual_params").find(),
+ "config",
+ LaunchConfiguration("vehicle_id").perform(None),
+ "aip_xx1_gen2",
+ "concatenate_and_time_sync_node.param.yaml",
+ ),
),
],
)
diff --git a/aip_xx1_gen2_launch/launch/pointcloud_preprocessor.launch.py b/aip_xx1_gen2_launch/launch/pointcloud_preprocessor.launch.py
index 04e5eca8..d97f562c 100644
--- a/aip_xx1_gen2_launch/launch/pointcloud_preprocessor.launch.py
+++ b/aip_xx1_gen2_launch/launch/pointcloud_preprocessor.launch.py
@@ -12,9 +12,6 @@
# See the License for the specific language governing permissions and
# limitations under the License.
-import os
-
-from ament_index_python.packages import get_package_share_directory
import launch
from launch.actions import DeclareLaunchArgument
from launch.actions import OpaqueFunction
@@ -65,21 +62,12 @@ def generate_launch_description():
def add_launch_arg(name: str, default_value=None):
launch_arguments.append(DeclareLaunchArgument(name, default_value=default_value))
- aip_xx1_gen2_launch_share_dir = get_package_share_directory("aip_xx1_gen2_launch")
-
add_launch_arg("use_multithread", "False")
add_launch_arg("use_intra_process", "False")
add_launch_arg("pointcloud_container_name", "pointcloud_container")
add_launch_arg("individual_container_name", "concatenate_container")
+ add_launch_arg("concatenate_and_time_sync_node_param_path")
- add_launch_arg(
- "concatenate_and_time_sync_node_param_path",
- os.path.join(
- aip_xx1_gen2_launch_share_dir,
- "config",
- "concatenate_and_time_sync_node.param.yaml",
- ),
- )
set_container_executable = SetLaunchConfiguration(
"container_executable",
diff --git a/aip_xx1_launch/config/concatenate_and_time_sync_node.param.yaml b/aip_xx1_launch/config/concatenate_and_time_sync_node.param.yaml
deleted file mode 100644
index 86ef53b0..00000000
--- a/aip_xx1_launch/config/concatenate_and_time_sync_node.param.yaml
+++ /dev/null
@@ -1,23 +0,0 @@
-/**:
- ros__parameters:
- debug_mode: false
- has_static_tf_only: false
- rosbag_replay: false
- rosbag_length: 20.0
- maximum_queue_size: 5
- timeout_sec: 0.2
- is_motion_compensated: true
- publish_synchronized_pointcloud: true
- keep_input_frame_in_synchronized_pointcloud: true
- publish_previous_but_late_pointcloud: false
- synchronized_pointcloud_postfix: pointcloud
- input_twist_topic_type: twist
- input_topics: [
- "/sensing/lidar/top/pointcloud_before_sync", # 0.08
- "/sensing/lidar/left/pointcloud_before_sync", # 0.00
- "/sensing/lidar/right/pointcloud_before_sync", # 0.00
- "/sensing/lidar/rear/pointcloud_before_sync", # 0.00
- ]
- output_frame: base_link
- lidar_timestamp_offsets: [0.0, 0.02, 0.02, 0.02]
- lidar_timestamp_noise_window: [0.01, 0.01, 0.01, 0.01]
diff --git a/aip_xx1_launch/launch/lidar.launch.xml b/aip_xx1_launch/launch/lidar.launch.xml
index 3a628503..140fc2f6 100644
--- a/aip_xx1_launch/launch/lidar.launch.xml
+++ b/aip_xx1_launch/launch/lidar.launch.xml
@@ -118,10 +118,10 @@
-->
-
+
diff --git a/aip_xx1_launch/launch/pointcloud_preprocessor.launch.py b/aip_xx1_launch/launch/pointcloud_preprocessor.launch.py
index d1fa63a1..871ce365 100644
--- a/aip_xx1_launch/launch/pointcloud_preprocessor.launch.py
+++ b/aip_xx1_launch/launch/pointcloud_preprocessor.launch.py
@@ -12,9 +12,6 @@
# See the License for the specific language governing permissions and
# limitations under the License.
-import os
-
-from ament_index_python.packages import get_package_share_directory
import launch
from launch.actions import DeclareLaunchArgument
from launch.actions import OpaqueFunction
@@ -65,19 +62,11 @@ def generate_launch_description():
def add_launch_arg(name: str, default_value=None):
launch_arguments.append(DeclareLaunchArgument(name, default_value=default_value))
- aip_xx1_launch_share_dir = get_package_share_directory("aip_xx1_launch")
add_launch_arg("use_multithread", "False")
add_launch_arg("use_intra_process", "False")
add_launch_arg("pointcloud_container_name", "pointcloud_container")
- add_launch_arg(
- "concatenate_and_time_sync_node_param_path",
- os.path.join(
- aip_xx1_launch_share_dir,
- "config",
- "concatenate_and_time_sync_node.param.yaml",
- ),
- )
+ add_launch_arg("concatenate_and_time_sync_node_param_path")
set_container_executable = SetLaunchConfiguration(
"container_executable",