localization.orogen

name "localization"
# Optionally declare the version number
# version "0.1"

# If new data types need to be defined, they have to be put in a separate C++
# header, and this header will be loaded here
using_library "localization" # Localization library
using_library "envire_core" # Environment representation library

# If the component/deployments use data types that are defined in other oroGen
# projects, these projects should be imported there as well.
import_types_from "base"
import_types_from "localization/core/Types.hpp"

import_types_from "localizationTypes.hpp"

# Declare the State Optimization class
task_context "Task" do
    # This is the default from now on, and should not be removed. Rock will
    # transition to a setup where all components use a configuration step.
    needs_configuration

    #**************************
    #***** Task Properties ****
    #**************************
    property('localization_source_frame', '/std/string').
        doc 'Frame of the localization (normally body frame)'+
            'The localization target frame is taken from the world frame of the transformer.'

    property('maximum_number_sensor_poses', 'unsigned int', 0).
        doc 'Maximum number of sensor camera poses in the filter state.'

    property('measurement_covariance', 'base::MatrixXd').
        doc "Measurement uncertainty in meters at the sensor plane projection."

    property('update_period', 'double').
        doc "Desired update period. It cannot be smaller that the visual features samples period."

    property("update_type", "localization::UpdateType", :EKF).
        doc "Filter update type."

    #******************************
    #******* Input ports  *********
    #******************************

    #******* Proprioceptive sensors *********
    input_port('delta_pose_samples', '/base/samples/RigidBodyState').
        needs_reliable_connection.
        doc 'Delta pose displacement.'

    #******* Exteroceptive sensors *********

    # Exteroceptive update samples to correct the prediction (Visual, ICP, etc..).
    input_port('visual_features_samples', 'visual_stereo::ExteroFeatures').
        doc 'Visual features samples.'

    ##########################
    # Transformer
    ##########################
    transformer do
	    transform "navigation", "world" # navigation in world in "Source IN target" convention
	    transform "sensor", "body" # sensor in body in "Source IN target" convention
    	align_port("delta_pose_samples", 0)
    	align_port("visual_features_samples", 0)
    	max_latency(0.02)
    end

    #******************************
    #******* Output ports  ********
    #******************************
    output_port('pose_samples_out', '/base/samples/RigidBodyState').
	    doc 'Corrected estimated robot pose.'

    output_port('body_pose_samples_out', '/base/samples/RigidBodyState').
	    doc 'Corrected estimated robot pose.'

    property('output_debug', 'bool', false).
        doc "Set to true in case you want to have filter debug information"

    output_port('filter_info_out', 'localization/FilterInfo').
    	doc 'State estimation information.'

    output_port("features_point_samples_out",  'base::samples::Pointcloud').
        doc 'Features 3d point of the Opportunistic features in the localization.'


    port_driven

end