lidarlib_ros: ROS2 bridge for liblidarlib (GS1-5 LaserScan)

Co-Authored-By: Claude Fable 5 <noreply@anthropic.com>

launch/lidar.launch.py

@@ -0,0 +1,113 @@
+import os
+
+import yaml
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, LogInfo
+from launch.conditions import IfCondition
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+# Vi tri lap dat tung frame lidar so voi base_link: frame -> (x, y, z, yaw) met/rad.
+# Frame khong liet ke o day mac dinh dat tai goc base_link (0,0,0,0) -> van co TF
+# nen RViz khong con vut message. Sua toa do thuc te tai day.
+MOUNT_POSES = {
+    'front': (0.0, 0.0, 0.0, 0.0),
+    'rear': (0.0, 0.0, 0.0, 0.0),
+    'sick1': (0.0, 0.00, 0.0, 0.0),
+}
+
+
+def lidars_from_config(params_file):
+    """Doc config.yaml -> danh sach dict thong tin moi lidar trong 'lidars'."""
+    with open(params_file, 'r') as f:
+        data = yaml.safe_load(f)
+    params = data['lidarlib_node']['ros__parameters']
+    lidars = []
+    for name in params.get('lidars', []):
+        block = params.get(name, {}) or {}
+        lidars.append({
+            'name': name,
+            'brand': block.get('brand', '?'),
+            'model': block.get('model', '?'),
+            'ip': block.get('ip', '?'),
+            'port': block.get('port', '?'),
+            'inverted': block.get('inverted', False),
+            'topic': block.get('topic', f'scan_{name}'),
+            'frame_id': block.get('frame_id', name),
+        })
+    return lidars
+
+
+def lidar_frames_from_config(params_file):
+    """Doc config.yaml -> danh sach (frame_id) cua moi lidar trong 'lidars'."""
+    return [ld['frame_id'] for ld in lidars_from_config(params_file)]
+
+
+def generate_launch_description():
+    pkg = get_package_share_directory('lidarlib_ros')
+    params_file = os.path.join(pkg, 'config', 'lidar.yaml')
+    rviz_config = os.path.join(pkg, 'rviz', 'lidar.rviz')
+
+    use_rviz = LaunchConfiguration('rviz')
+
+    lidars = lidars_from_config(params_file)
+
+    # In thong tin cau hinh ra man hinh de tien theo doi/test.
+    log_nodes = [
+        LogInfo(msg='========== LIDAR LAUNCH INFO =========='),
+        LogInfo(msg=f'Params file : {params_file}'),
+        LogInfo(msg=f'RViz config : {rviz_config}'),
+        LogInfo(msg=f'So luong lidar: {len(lidars)}'),
+        LogInfo(msg='----------------------------------------'),
+    ]
+
+    # Tu sinh 1 static TF cho MOI frame lidar co trong config -> them lidar la co TF.
+    tf_nodes = []
+    for ld in lidars:
+        frame = ld['frame_id']
+        x, y, z, yaw = MOUNT_POSES.get(frame, (0.0, 0.0, 0.0, 0.0))
+        default_pose = frame not in MOUNT_POSES
+        log_nodes.append(LogInfo(
+            msg=(f"[{ld['name']}] brand={ld['brand']} model={ld['model']} "
+                 f"ip={ld['ip']}:{ld['port']} inverted={ld['inverted']} "
+                 f"topic={ld['topic']} frame={frame} "
+                 f"TF(x={x} y={y} z={z} yaw={yaw})"
+                 f"{'  <-- mac dinh (0,0,0), sua trong MOUNT_POSES' if default_pose else ''}")))
+        tf_nodes.append(Node(
+            package='tf2_ros',
+            executable='static_transform_publisher',
+            name=f'base_to_{frame}',
+            arguments=[str(x), str(y), str(z), str(yaw), '0', '0', 'base_link', frame],
+        ))
+
+    log_nodes.append(LogInfo(msg='========================================'))
+
+    return LaunchDescription([
+        DeclareLaunchArgument(
+            'rviz', default_value='true',
+            description='Mo RViz2 de hien thi LaserScan'),
+
+        *log_nodes,
+
+        # Node cau noi lidarlib -> sensor_msgs/LaserScan (nhieu lidar 1 node)
+        Node(
+            package='lidarlib_ros',
+            executable='lidarlib_node',
+            name='lidarlib_node',
+            output='screen',
+            parameters=[params_file],
+        ),
+
+        *tf_nodes,
+
+        # RViz2 voi config san
+        Node(
+            package='rviz2',
+            executable='rviz2',
+            name='rviz2',
+            arguments=['-d', rviz_config],
+            condition=IfCondition(use_rviz),
+            output='screen',
+        ),
+    ])