added tracking conf

src/sherpa/launch/calibration.launch.py

@@ -0,0 +1,104 @@
+import os.path
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch_ros.substitutions import FindPackageShare
+from launch.conditions import IfCondition
+import math
+
+def generate_launch_description():
+    # ----- microros -----
+    servo_uros_node = Node(
+        package='micro_ros_agent',
+        executable='micro_ros_agent',
+        arguments=["serial", "--dev", "/dev/ttyACM0", "--baud", "921600"],
+    )
+
+    servo_angle_tf_node = Node(
+        package='sherpa_lidar_utils',
+        executable='servo_angle_tf.py',
+    )
+
+    # ----- lidar position -----
+    lidar_position_base_tf_node = Node(
+        package='tf2_ros',
+        executable='static_transform_publisher',
+        arguments=['0', '0', '0', '0', '0', '0', '1', 'velodyne', 'lidar_base'],
+    )
+
+    # ----- lidar topics -----
+    vlp16_driver_node = Node(
+        package='velodyne_driver',
+        executable='velodyne_driver_node',
+        arguments=["--ros-args", "-p", "model:=VLP16", "-p", "rpm:=600.0", "-p", "device_ip:=10.42.200.201"],
+        remappings=[
+            ('/velodyne_packets', '/lidar/vlp16/packets'),
+        ],
+    )
+
+    vlp16_pointcloud_node = Node(
+        package='velodyne_pointcloud',
+        executable='velodyne_transform_node',
+        arguments=["--ros-args", "-p", f"calibration:={os.path.join(get_package_share_directory('sherpa'), 'config', 'vlp-16-pointcloud.yaml')}", "-p", "model:=VLP16"],
+        remappings=[
+            ('/velodyne_packets', '/lidar/vlp16/packets'),
+            ('/velodyne_points', '/lidar/vlp16/points'),
+        ]
+    )
+
+    mid40_driver_node = Node(
+        package='livox_ros2_driver',
+        executable='livox_ros2_driver_node',
+        arguments=[],
+        remappings=[
+            ('/livox/lidar', '/lidar/mid40/points'),
+        ],
+    )
+
+    # ----- pointcloud downsampling -----
+    downsample_vlp16_node = Node(
+        package='target_tracking',
+        executable='cloud_preprocessing_node',
+        parameters=[
+            {"topic_in": "/lidar/vlp16/points"},
+            {"topic_out": "/lidar/vlp16/downsampled"},
+            {"x_min": 0.0},
+            {"x_max": 100.0},
+            {"z_min": -5.0},
+            {"z_max": 10.0},
+            {"tan_h_fov": math.pi}, # 180°  
+            {"tan_v_fov": math.pi / 5}, # ~36°  
+        ],
+    )
+
+    downsample_mid40_node = Node(
+        package='target_tracking',
+        executable='cloud_preprocessing_node',
+        parameters=[
+            {"topic_in": "/lidar/mid40/points"},
+            {"topic_out": "/lidar/mid40/downsampled"},
+            {"x_min": 0.0},
+            {"x_max": 100.0},
+            {"z_min": -5.0},
+            {"z_max": 10.0},
+            {"tan_h_fov": math.pi / 4}, # 45°  
+            {"tan_v_fov": math.pi / 4}, # 45°  
+        ],
+    )
+
+    # ----- create ld -----
+    ld = LaunchDescription()   
+
+    ld.add_action(servo_uros_node)
+    ld.add_action(servo_angle_tf_node)
+    ld.add_action(lidar_position_base_tf_node)
+    ld.add_action(vlp16_driver_node)
+    ld.add_action(vlp16_pointcloud_node)
+    ld.add_action(mid40_driver_node)
+    ld.add_action(downsample_vlp16_node)
+    ld.add_action(downsample_mid40_node)
+    
+    return ld

src/sherpa/launch/tracking.launch.py

@@ -0,0 +1,159 @@
+import os.path
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch_ros.substitutions import FindPackageShare
+from launch.conditions import IfCondition
+import math
+
+def generate_launch_description():
+    # ----- microros -----
+    servo_uros_node = Node(
+        package='micro_ros_agent',
+        executable='micro_ros_agent',
+        arguments=["serial", "--dev", "/dev/ttyACM0", "--baud", "921600"],
+    )
+
+    servo_angle_tf_node = Node(
+        package='sherpa_lidar_utils',
+        executable='servo_angle_tf.py',
+    )
+
+    # ----- lidar position -----
+    lidar_position_tf_node = Node(
+        package='sherpa_lidar_utils',
+        executable='publish_calibration.py',
+        arguments=["--ros-args", "-p", "calibration:=/home/sherpa/lidar_calibration"],
+    )
+
+    lidar_position_base_tf_node = Node(
+        package='tf2_ros',
+        executable='static_transform_publisher',
+        arguments=['0', '0', '0', '0', '0', '0', '1', 'velodyne', 'lidar_base'],
+    )
+
+    # ----- lidar topics -----
+    vlp16_driver_node = Node(
+        package='velodyne_driver',
+        executable='velodyne_driver_node',
+        arguments=["--ros-args", "-p", "model:=VLP16", "-p", "rpm:=600.0", "-p", "device_ip:=10.42.200.201"],
+        remappings=[
+            ('/velodyne_packets', '/lidar/vlp16/packets'),
+        ],
+    )
+
+    vlp16_pointcloud_node = Node(
+        package='velodyne_pointcloud',
+        executable='velodyne_transform_node',
+        arguments=["--ros-args", "-p", f"calibration:={os.path.join(get_package_share_directory('sherpa'), 'config', 'vlp-16-pointcloud.yaml')}", "-p", "model:=VLP16"],
+        remappings=[
+            ('/velodyne_packets', '/lidar/vlp16/packets'),
+            ('/velodyne_points', '/lidar/vlp16/points'),
+        ]
+    )
+
+    mid40_driver_node = Node(
+        package='livox_ros2_driver',
+        executable='livox_ros2_driver_node',
+        arguments=[],
+        remappings=[
+            ('/livox/lidar', '/lidar/mid40/points'),
+        ],
+    )
+
+    # ----- pointcloud downsampling -----
+    downsample_vlp16_node = Node(
+        package='target_tracking',
+        executable='cloud_preprocessing_node',
+        parameters=[
+            {"topic_in": "/lidar/vlp16/points"},
+            {"topic_out": "/lidar/vlp16/downsampled"},
+            {"x_min": 0.0},
+            {"x_max": 100.0},
+            {"z_min": -5.0},
+            {"z_max": 10.0},
+            {"tan_h_fov": math.pi}, # 180°  
+            {"tan_v_fov": math.pi / 5}, # ~36°  
+        ],
+    )
+
+    downsample_mid40_node = Node(
+        package='target_tracking',
+        executable='cloud_preprocessing_node',
+        parameters=[
+            {"topic_in": "/lidar/mid40/points"},
+            {"topic_out": "/lidar/mid40/downsampled"},
+            {"x_min": 0.0},
+            {"x_max": 100.0},
+            {"z_min": -5.0},
+            {"z_max": 10.0},
+            {"tan_h_fov": math.pi / 4}, # 45°  
+            {"tan_v_fov": math.pi / 4}, # 45°  
+        ],
+    )
+
+    # ----- pointcloud merging -----
+    pointcloud_merger_node = Node(
+        package='scans_merger',
+        executable='scans_merger',
+        parameters=[
+            {"destination_frame": "lidar_base"},
+            {"input_cloud_1": "/lidar/vlp16/downsampled"},
+            {"input_cloud_2": "/lidar/mid40/downsampled"},
+            {"merged_cloud": "/lidar/merged/points"},
+        ],
+    )
+
+    # ----- tracking -----
+    cloud_clustering_node = Node(
+        package='target_tracking',
+        executable='cloud_clustering_node',
+        parameters=[
+            {"topic_in": "/lidar/merged/points"},
+            {"frame_id": 'lidar_base'},
+            {"z_dim_scale": 1.0},
+            #{"cluster_tolerance": 0.065},
+            #{"min_cluster_size": 450},
+            #{"max_cluster_size": 1650},
+            {"min_cluster_size": 125},
+            {"max_cluster_size": 2250},
+            {"cluster_tolerance": 0.15},
+        ],
+        remappings=[
+            ('/cluster5', '/tracking/cluster0'),
+            ('/cluster4', '/tracking/cluster1'),
+            ('/cluster3', '/tracking/cluster2'),
+            ('/cluster2', '/tracking/cluster3'),
+            ('/cluster1', '/tracking/cluster4'),
+            ('/cluster0', '/tracking/cluster5'),
+        ],
+    )
+
+    servo_tracking_node = Node(
+        package='sherpa_lidar_utils',
+        executable='track_cluster.py',
+        remappings=[
+            ('/tracking/target', '/tracking/cluster0'),
+        ],
+    )
+
+    # ----- create ld -----
+    ld = LaunchDescription()   
+
+    ld.add_action(servo_uros_node)
+    ld.add_action(servo_angle_tf_node)
+    ld.add_action(lidar_position_tf_node)
+    ld.add_action(lidar_position_base_tf_node)
+    ld.add_action(vlp16_driver_node)
+    ld.add_action(vlp16_pointcloud_node)
+    ld.add_action(mid40_driver_node)
+    ld.add_action(downsample_vlp16_node)
+    ld.add_action(downsample_mid40_node)
+    ld.add_action(pointcloud_merger_node)
+    ld.add_action(cloud_clustering_node)
+    ld.add_action(servo_tracking_node)
+
+    return ld