rawControlCommandConverter.orogen

name "raw_control_command_converter"
version "0.0"

import_types_from "controldev"

import_types_from "base"
import_types_from "auv_control"
task_context "Movement" do

    input_port("raw_command", "controldev/RawCommand").
        doc("Port for raw command structures (composed of joystick and sliderbox values)")
   
    input_port("orientation_readings","/base/samples/RigidBodyState").
    	doc("Orientation reading for human control")
    
    output_port("motion_command", "base/AUVMotionCommand").
        doc("Port for motion command structures (translation, rotation)")

    output_port("world_command", "base::LinearAngular6DCommand")
        doc("Port for the part of the command that is in Worldcoordinats")

    output_port("world_command_depth", "base::LinearAngular6DCommand")
        doc("Port for the part of the command that is in Worldcoordinats")
    
    output_port("aligned_velocity_command", "base::LinearAngular6DCommand")
        doc("Port for the part of the command that ar speeds")
        

    input_port("ground_distance", "base::samples::RigidBodyState").
        doc("For doing ground following behaviour we need the distance to the ground")

    property('diveScale', 'double', 3.0);
    property('turnScale', 'double', 5.0);
    property('absolute_heading', 'bool', false)
    property('do_ground_following', 'bool', false)
    property('delta_depth_control', 'bool', false).
        doc("Should do delta-depth Control. Means incomming Values are interpeted as delta-targets (like speed_control)")

    error_states "GOT_POSE_WITHOUT_DEPTH", "SHOULD_DO_GROUND_FOLLOWING_WITHOUT_GROUND_DISTANCE"

    port_driven 'raw_command'
end

task_context "Position" do

    input_port("raw_command", "controldev/RawCommand").
        doc("Port for raw command structures (composed of joystick and sliderbox values)")
   
    input_port("pose_samples","/base/samples/RigidBodyState").
    	doc("Orientation reading for human control")

    output_port("position_command", "base/AUVPositionCommand").
        doc("Port for the AUV Position Command, reads from orientation input and tryes to calculate an delta position based on the joystick command")
    
    output_port("world_command", "base::LinearAngular6DCommand")
        doc("Port for the part of the command that is in Worldcoordinats")

    port_driven
end