heading_calculator.orogen

name "heading_calculator"

import_types_from "base"

task_context "Task" do
    property('goal_distance', 'double', 1.0).
        doc 'Distance to the cuurent goal position along the trajectory, in meters'
        
    property('geometric_resolution', 'double', 0.02).
        doc 'Length of a single spline element'

    property('required_dist_to_goal', 'double', 0.15).
        doc('The end of the trajectory is assumed to be reached if the distance between the').
        doc(' robot and the end of the spline is smaller or equal than this parameter.'). 
        doc(' This parameter will be ignored if it is set smaller than 0.')

    input_port("trajectory", "std::vector</base/Trajectory>").
        doc("(Global) trajectory which will be used to calculate the heading")
        
    input_port("pose_samples", "base/samples/RigidBodyState").
        doc "Robot pose within the trajectory frame."
        
    output_port('heading', 'double').
	    doc 'Heading in NorthWestUp in radians'
	    
	output_port('heading_debug_deg', 'double').
	    doc 'Heading in NorthWestUp in degree'
	    
	port_driven 

    runtime_states :TARGET_REACHED
end