pose_estimation.orogen

name "pose_estimation"
# version "0.1"

using_library "pose_estimation"

import_types_from "base"
import_types_from "pose_estimationTypes.hpp"
import_types_from "pose_estimation/orientation_estimator/OrientationUKFConfig.hpp"

task_context "OrientationEstimator" do
    needs_configuration

    #******************************
    #***** Filter Properties ******
    #******************************
    # Maximum time delta of a prediction step
    property( "max_time_delta", "double", 2.0 )

    # The initial orientation in euler angles
    property( "initial_orientation", "/base/Vector3d" )

    # The initial covariance diagonal
    property( "initial_orientation_cov_diag", "/base/Vector3d" )

    # Filter configuration
    property( "filter_config", "/pose_estimation/OrientationUKFConfig" )

    # The target frame in the generated poses. The source frame is the body frame (as declared in the transformer configuration)
    property( 'target_frame', '/std/string', 'world' )

    #******************************
    #******* Input Ports **********
    #******************************
    # Velocity samples
    input_port( "velocity_samples", "/base/samples/RigidBodyState" ).
            needs_reliable_connection

    # IMU acceleration and angular velocity samples
    input_port('imu_sensor_samples', '/base/samples/IMUSensors').
            needs_reliable_connection

    #******************************
    #******* Output Ports *********
    #******************************
    # Timestamped samples of the current estimated orientation and velocity of the robot.
    output_port( "orientation_samples", "/base/samples/RigidBodyState" )

    # Secondary filter states
    output_port( "secondary_states", "/pose_estimation/OrientationUKFSecondaryStates")

    #******************************
    #** Aggregator Parameters *****
    #******************************
    transformer do
        transformation("imu", "body")
        transformation("velocity_provider", "body")

        align_port("velocity_samples", 0.1)
        align_port("imu_sensor_samples", 0.01)

        max_latency(0.05)
    end

    #******************************
    #********* Operations *********
    #******************************
    # Add an offset to the current heading and reinitialize the filter.
    #
    # This is commonly used to align the heading estimate to the true north.
    operation('addHeadingOffset').
        returns('bool').
        argument('heading_offset', 'double', 'heading_offset').
        argument('variance', 'double', 'Initial variance of the new heading state in (rad/s)^2')

    #******************************
    #******** Task States *********
    #******************************
    runtime_states :MISSING_TRANSFORMATION, :TRANSFORMATION_ALIGNMENT_FAILURES
    exception_states :CRITICAL_ALIGNMENT_FAILURE

    periodic 0.01
end

task_context "RBSFilter" do
    abstract
    needs_configuration
   
    #******************************
    #***** Filter Properties ******
    #******************************
    # Deprecated, only the UKF is available
    property( "filter_type", "/pose_estimation/deprecated/FilterType", :UKF )

    property( "max_time_delta", "double", 2.0 )

    property( "initial_state", "/base/samples/RigidBodyState" ).
        doc("the initial state, NaN values will be ignored.")

    property( "process_noise", "/pose_estimation/ProcessNoise" ).
        doc("Filter process noise, NaN values will be ignored and set to a healthy but likely overconfident value.")
    
    
    property( 'target_frame', '/std/string', 'world' ).
    	doc "The target frame in the generated poses. The source frame is the body frame (as declared in the transformer configuration)"
    
    #******************************
    #******* Output Ports *********
    #******************************
    output_port( "pose_samples", "/base/samples/RigidBodyState" ).
        doc "timestamped samples of the current estimated pose of the robot."


    #******************************
    #********* Operations *********
    #******************************
    operation('resetState').
        returns('bool').
        doc 'resets the current state to the initial state.'

    #******************************
    #******** Task States ********* 
    #******************************
    runtime_states :MISSING_TRANSFORMATION, :TRANSFORMATION_ALIGNMENT_FAILURES
    exception_states :CRITICAL_ALIGNMENT_FAILURE

    periodic 0.01
end

task_context "UWPoseEstimator" do
    subclasses "RBSFilter"
    needs_configuration

    #******************************
    #******* Input Ports **********
    #******************************
    input_port( "acceleration_samples", "/base/samples/RigidBodyAcceleration" ).
        needs_reliable_connection.
        doc "acceleration samples with uncertainty from an IMU, in case this port is connected the measurement uncertainty is used instead of the process noise."

    input_port( "orientation_samples", "/base/samples/RigidBodyState" ).
	needs_reliable_connection.
	doc "orientation samples from an IMU and or a FOG, containing the orientation and the angular velocity."

    input_port( "depth_samples", "/base/samples/RigidBodyState" ).
	needs_reliable_connection.
	doc "depth samples from a pressure sensor, containing the absolute z-position in world coordinates."

    input_port( "dvl_velocity_samples", "/base/samples/RigidBodyState" ).
	needs_reliable_connection.
	doc "velocity samples from a DVL, containing the velocity."
	
    input_port( "model_velocity_samples", "/base/samples/RigidBodyState" ).
	needs_reliable_connection.
	doc "model based velocity samples, containing the xy velocity."

    input_port( "lbl_position_samples", "/base/samples/RigidBodyState" ).
	needs_reliable_connection.
	doc "position samples from a LBL or USBL, containing the absolute position in world coordinates."
	
    input_port( "xy_position_samples", "/base/samples/RigidBodyState" ).
	needs_reliable_connection.
	doc "position samples from a localization module, containing the xy position in world coordinates."
	
    input_port( "gps_position_samples", "/base/samples/RigidBodyState" ).
	needs_reliable_connection.
	doc "position samples from a gps-sensor, containing position in gps-coordinates"

    input_port( "xyz_position_samples", "/base/samples/RigidBodyState" ).
	needs_reliable_connection.
	doc "position samples from a localization module, containing the xyz position in world coordinates."
	

    #******************************
    #** Aggregator Parameters ***** 
    #******************************
    transformer do
        transformation("imu", "body")
        transformation("pressure_sensor", "body")
        transformation("dvl", "body")
        transformation("lbl", "body")
        transformation("gps", "body")
        
        align_port("acceleration_samples", 0.01)
	align_port("orientation_samples", 0.01)
	align_port("depth_samples", 0.01)
	align_port("dvl_velocity_samples", 0.01)
	align_port("model_velocity_samples", 0.01)
	align_port("lbl_position_samples", 0.01)
	align_port("xy_position_samples", 0.01)
	align_port("gps_position_samples", 0.01)
	align_port("xyz_position_samples", 0.01)
	max_latency(0.1)
    end

    find_property('transformer_max_latency').dynamic

    periodic 0.01
end

task_context "HighDelayPoseEstimator" do
    subclasses "RBSFilter"
    needs_configuration

    #******************************
    #******* Input Ports **********
    #******************************
    input_port( "pose_samples_fast", "/base/samples/RigidBodyState" ).
        needs_reliable_connection.
        doc "Pose samples with will be processed fast to create a actual pose"

    input_port( "pose_samples_slow", "/base/samples/RigidBodyState" ).
        needs_reliable_connection.
        doc "Pose samples which will be precessed when a new delayed meassurement arives"

    input_port( "xy_position_samples", "/base/samples/RigidBodyState" ).
        needs_reliable_connection.
        doc "Position samples with can have a high delay"


    #******************************
    #** Aggregator Parameters ***** 
    #******************************
    transformer do
	transformation("xy_map", "target_map")
        align_port("pose_samples_slow", 0.01)
        align_port("xy_position_samples", 5.0)
        max_latency(30.0)
    end

    periodic 0.01
end

task_context "VehiclePoseEstimator" do
    subclasses "RBSFilter"
    needs_configuration

    #******************************
    #******* Input Ports **********
    #******************************
    input_port( "orientation_samples", "/base/samples/RigidBodyState" ).
	needs_reliable_connection.
	doc "orientation samples from an IMU and or a FOG, containing the orientation and the angular velocity."

    input_port( "velocity_samples", "/base/samples/RigidBodyState" ).
	needs_reliable_connection.
	doc "velocity samples from the odometry, containing the velocity."

    input_port( "position_samples", "/base/samples/RigidBodyState" ).
	needs_reliable_connection.
	doc "position samples from a slam component, containing the absolute position in world coordinates."


    #******************************
    #** Aggregator Parameters ***** 
    #******************************
    transformer do
        transformation("imu", "body")
        
	align_port("orientation_samples", 0.01)
	align_port("velocity_samples", 0.01)
	align_port("position_samples", 0.01)
	max_latency(0.1)
    end

    periodic 0.01
end

# This component allows to find a initial heading based on the ability of the IMU
# to measure the rotation of the earth.
# It utilizes multiple orientation filters with different initial headings and
# writes out their average heading after they converge to a close result.
task_context "OrientationInitialization" do
    needs_configuration

    #******************************
    #***** Filter Properties ******
    #******************************
    # Maximum time delta of a prediction step
    property( "max_time_delta", "double", 2.0 )

    # The initial orientation in euler angles
    property( "initial_orientation", "/base/Vector3d" )

    # The initial covariance diagonal
    property( "initial_orientation_cov_diag", "/base/Vector3d" )

    # Filter configuration
    property( "filter_config", "/pose_estimation/OrientationUKFConfig" )

    # The target frame in the generated poses. The source frame is the body frame (as declared in the transformer configuration)
    property( 'target_frame', '/std/string', 'world' )

    # Convergence threshold as yaw angle difference
    property( "convergence_threshold", "double", 0.0174 )

    #******************************
    #******* Input Ports **********
    #******************************
    # Velocity samples
    input_port( "velocity_samples", "/base/samples/RigidBodyState" ).
            needs_reliable_connection

    # IMU acceleration and angular velocity samples
    input_port('imu_sensor_samples', '/base/samples/IMUSensors').
            needs_reliable_connection

    #******************************
    #******* Output Ports *********
    #******************************
    # Timestamped samples of the current estimated orientation of the robot.
    output_port( "orientation_samples", "/base/samples/RigidBodyState" )

    #******************************
    #** Aggregator Parameters *****
    #******************************
    transformer do
        transformation("imu", "body")
        transformation("velocity_provider", "body")

        align_port("velocity_samples", 0.1)
        align_port("imu_sensor_samples", 0.01)

        max_latency(0.05)
    end

    #******************************
    #******** Task States *********
    #******************************
    runtime_states :MISSING_TRANSFORMATION, :TRANSFORMATION_ALIGNMENT_FAILURES,
                   :INITIALIZATION_COMPLETE, :INITIALIZATION
    exception_states :CRITICAL_ALIGNMENT_FAILURE

    periodic 0.01
end