from launch import LaunchDescription
from launch_ros.actions import Node
import math

################### user configure parameters for ros2 start ###################
# Topics/Frames
frame_id        = 'velodyne'
<<<<<<< Updated upstream
topic_preprocessing_in  = 'filtered_points'
topic_preprocessing_out = 'new_filtered'
=======
topic_preprocessing_in  = 'merged_cloud'
topic_preprocessing_out = 'filtered_points'
>>>>>>> Stashed changes

# Preprocessing
x_min           = 0.0
x_max           = 20.0
z_min           = -0.5
z_max           = 1.5
tan_h_fov       = math.pi / 4 # ±45°
tan_v_fov       = math.pi / 6 # ±30°

# Clustering
<<<<<<< Updated upstream
z_dim_scale     = 0.1
cluster_tolerance   = 0.3
min_cluster_size    = 10
max_cluster_size    = 1000
min_width   = 0.0
min_height  = 0.0
min_length  = 0.0
max_width   = 1.5
max_height  = 2.5
max_length  = 1.5
=======
z_dim_scale     = 1.0
cluster_tolerance   = 0.1
min_cluster_size    = 350
max_cluster_size    = 1500
>>>>>>> Stashed changes

################### user configure parameters for ros2 end ##################### 

cloud_preprocessing_params = [
    {"topic_in": topic_preprocessing_in},
    {"topic_out": topic_preprocessing_out},
    {"x_min": x_min},
    {"x_max": x_max},
    {"z_min": z_min},
    {"z_max": z_max},
    {"tan_h_fov": tan_h_fov},
    {"tan_v_fov": tan_v_fov}  
]

cloud_clustering_params = [
    {"topic_in": topic_preprocessing_out},
    {"frame_id": frame_id},
    {"z_dim_scale": z_dim_scale},
    {"cluster_tolerance": cluster_tolerance},
    {"min_cluster_size": min_cluster_size},
    {"max_cluster_size": max_cluster_size},
    {"min_width": min_width},
    {"min_height": min_height},
    {"min_length": min_length},
    {"max_width": max_width},
    {"max_height": max_height},
    {"max_length": max_length}
]

def generate_launch_description():
    return LaunchDescription([
        Node(
            package='target_tracking',
            executable='cloud_preprocessing_node',
            name='cloud_preprocessing',
            parameters=cloud_preprocessing_params,
            output={
                'stdout': 'screen',
                'stderr': 'screen',
            }
        ),
        Node(
            package='target_tracking',
            executable='cloud_clustering_node',
            name='cloud_clustering',
            parameters=cloud_clustering_params,
            output={
                'stdout': 'screen',
                'stderr': 'screen',
            }
        )
    ])