localization.orogen

name "localization"

pcl_registration = Utilrb::PkgConfig.each_package.find { |name| /^pcl_registration/.match(name) }
if(!pcl_registration)
  raise "cannot find the pcl_registration pkg-config package"
end
using_library pcl_registration

pcl_io = Utilrb::PkgConfig.each_package.find { |name| /^pcl_io/.match(name) }
if(!pcl_io)
  raise "cannot find the pcl_io pkg-config package"
end
using_library pcl_io

using_library 'depth_map_preprocessing'
using_library 'base-logging'

import_types_from 'base'
import_types_from 'LocalizationConfig.hpp'

task_context "Task" do
    needs_configuration

    # Load initial point cloud in PLY format
    property('ply_path', 'string', "")

    # Start pose of the robot in the map frame
    property('start_pose', '/base/Pose')

    # icp configuration
    property('gicp_configuration', '/localization/GICPConfiguration')

    # Diagonal of the pose process noise covariance
    property('process_noise_diagonal', '/base/Vector6d')

    # Subsamples the input pointclouds according to this configuration
    property('subsampling', '/localization/SubSampling', :None)
    property('subsampling_resolution', '/base/Vector3d')

    # Name of the frame, of the provided transformations by this component. E.g. body to localization
    property('output_frame_name', 'string', 'localization')

    # If true the result of the alignment will be written to debug_map_pointcloud
    property('write_debug_pointcloud', 'bool', false)

    ## input ports
    # updates the model pointcloud
    input_port('model_pointcloud', '/base/samples/Pointcloud')

    # allows an external and unaligned pose update
    input_port('pose_update', '/base/samples/RigidBodyState')
        
    ## output ports
    # timestamped samples of the current pose of the robot
    output_port('pose_samples', '/base/samples/RigidBodyState')

    # Shows the aligned measurement in the reference map
    output_port('debug_map_pointcloud', '/base/samples/Pointcloud')

    # icp debug information
    output_port('icp_debug_information', '/localization/ICPDebugInformation')

    ## states
    runtime_states :MISSING_TRANSFORMATION, :ICP_ALIGNMENT_FAILED
end

task_context "PointcloudInMLS" do
    subclasses "Task"
    needs_configuration

    ## input ports
    input_port('pointcloud_samples', '/base/samples/Pointcloud').
        doc 'pointcloud data in body frame'
    
    ## transformer config
    transformer do
        max_latency( 0.2 )

        transformation("pointcloud", "body")
        transformation("body", "odometry")

        align_port("pointcloud_samples", 0.1)
    end
    
    port_driven
end

task_context "VelodyneInMLS" do
    subclasses "Task"
    needs_configuration

    ## point cloud filter parameter
    property('maximum_angle_to_neighbor', 'double', 0.95).
        doc('valid neigbors have an angle not greater than this value.').
        doc('the angle is always defined from the origin to the more distant point.')
    property('minimum_valid_neighbors', 'int', 2).
        doc('minimum amount of valid neigbors')

    ## input ports
    input_port('lidar_samples', '/base/samples/DepthMap').
        doc 'timestamped 3d laser scans'
    
    ## transformer config
    transformer do
        max_latency( 0.2 )

        transformation("velodyne", "body")
        transformation("body", "odometry")

        align_port("lidar_samples", 0.1)
    end
    
    port_driven
end