auv_control.orogen

name "auv_control"
version "0.2"

import_types_from "base"
import_types_from "6dControl.hpp"
using_library "motor_controller"

typekit.export_types '/motor_controller/PIDSettings'

# Definition of the base interface for all AUV control components
#
# It also implements the base functionality of merging and validating the
# declared inputs, as well as managing the command input ports.
task_context "Base" do
    needs_configuration
    abstract

    # This property defines which parts of the command input is expected to be
    # set once we merged all the declared input ports. 
    property("expected_inputs", "auv_control::ExpectedInputs").
        dynamic
    
    # If true, the component will send a zero command before getting into an
    # exception state. Otherwise, it will not do anything
    property "keep_position_on_exception", "bool", true
  
    # If true write NaN on all axis, in keep position case
    property "nan_on_keep_position", "bool", false

    # The target state for this controller
    # 
    # This port is statically defined for simplicity reasons, additional ports
    # can be created using the addCommandInput operation
    input_port("cmd_in", "base::LinearAngular6DCommand")
    
    # This property defines the timeout for the cmd_in input port in seconds. 0
    # means that the timeout would be ignored.
    property "timeout_in", "double", 1

    # When used in a cascade, this input port can be used to feed the output of
    # the controllers before.
    #
    # Leave unconnected if you are not cascading controllers
    #
    # This port is statically defined for simplicity reasons, additional ports
    # can be created using the addCommandInput operation
    input_port('cmd_cascade', 'base::LinearAngular6DCommand')
    
    # This property defines the timeout for the cascade input port in seconds. 0
    # means that the timeout would be ignored.
    property "timeout_cascade", "double", 1
   
    #This property defines the safty behavior ath the merging of the input-ports.
    #If the property is on true (default) the merged command need to be like in
    #the expected_inputs property defined. Else the expected_inputs are ignored 
    #while the merged comand are unic.
    property "safe_mode", "bool", true

    # Create a new input to merge 
    #
    # Returns true if the port got added, and false if a port with the same name
    # already exists
    operation("addCommandInput").
        argument("name", "string").
        argument("timeout", "double").
        returns('bool')
    dynamic_input_port(/cmd_\w+/, "base::LinearAngular6DCommand")    

    runtime_states :CONTROLLING, :CONTROLLING_UNSAFE
    error_states :INPUT_MISSING, :INPUT_COLLIDING, :INPUT_UNEXPECTED, :TIMEOUT, :WAIT_FOR_CONNECTED_INPUT_PORT, :WAIT_FOR_INPUT 

    # Deployed with a 10ms period by default
    periodic 0.01
end

# Base implementation of all the tasks that use one PID controller per axis to
# generate commands
task_context 'BasePIDController' do
    abstract
    subclasses "Base"

    # Ideal Settings for the PID controllers
    property("pid_settings", "base::LinearAngular6DPIDSettings").
        dynamic
    
    # Parallel Settings for the PID controllers
    property("parallel_pid_settings", "base::LinearAngular6DParallelPIDSettings").
        dynamic

    # Use the Ideal (false) or Parallel PID-Settings 
    property "use_parallel_pid_settings", "bool", false

    # Under this Value the axis are not controled. Output is 0.
    property "variance_threshold", "double"

    # The system state. Only the parts of the state that are controlled needs to
    # be available (i.e. if the command involves only orientation, only the
    # orientation part is really needed)
    input_port 'pose_samples', 'base::samples::RigidBodyState'

    # The output command. It is a velocity command expressed in the aligned
    # frame.
    output_port 'cmd_out', 'base::LinearAngular6DCommand'

    # The states of the pid-controllers
    output_port 'pid_state', '/auv_control/LinearAngular6DPIDState'
    
    error_states :WAIT_FOR_POSE_SAMPLE
    
    runtime_states :UNSURE_POSE_SAMPLE
    exception_states :POSE_SAMPLE_INVALID
end

# Controller that takes either positions or velocities, expressed in either the
# world or aligned frames, and outputs "whatever" in the same frame
task_context "PIDController" do
    subclasses "BasePIDController"

    # The command domain (true:position or false:velocity)
    property "position_control", "bool"
    # The command frame (true:world or false:aligned)
    property "world_frame", "bool"
end

# Task that get a target position in the world frame and outputs a position
# command in the aligned frame.
#
# It will work in any of the three control domains (position, velocity and
# effor), you just have to set position_control to true for the position domain.
#
# Note that this task expects to either have both x,y inputs or none. Giving it
# only X or only Y will result in a failure to start.
task_context "WorldToAligned" do
    subclasses "Base"

    # The domain of what we are converting (true:position or false:velocity or
    # efforts)
    property "position_control", "bool"

    # The system state. What is required depends on which parts of the state are
    # given as input
    input_port 'pose_samples', 'base::samples::RigidBodyState'

    # The output command.
    output_port 'cmd_out', 'base::LinearAngular6DCommand'
    
    error_states :WAIT_FOR_POSE_SAMPLE
    exception_states :POSE_SAMPLE_INVALID
end

# Controller that takes either velocities or efforts expressed in the aligned
# frame as input and outputs the same commands, but expressed in the body frame.
task_context "AlignedToBody" do
    subclasses "Base"

    # The system pose in the world frame
    #
    # Only the orientation is being used (to be more precise, only the pitch and
    # roll angles)
    input_port 'orientation_samples', 'base::samples::RigidBodyState'
    
    # This property defines the timeout for the cmd_in input port in seconds. 0
    # means that the timeout would be ignored.
    property "timeout_orientation", "double", 1

    # The output command. It is expressed in the body frame, and is of the same
    # nature than the input (efforts if the inputs are efforts, velocities if
    # the inputs are velocities)
    output_port 'cmd_out', 'base::LinearAngular6DCommand'
    
    error_states :WAIT_FOR_ORIENTATION_SAMPLE
    exception_states :ORIENTATION_SAMPLE_INVALID
end

# Generates thruster commands based on a thruster matrix and a force-torque
# input expressed in the system's body frame.
#
# It assumes zero values on the axis that are not set
#
# It basically projects the (merged) input vector onto the thrusters using the
# matrix
task_context "AccelerationController" do
    subclasses "Base"

    # Matrix with size of 6 * n. n means the count of thrusters that are used.
    # The rows 0 to 2 of the matrix are the linear axis. The lines 3 to 5 of the
    # matrix are the angular axis.
    property "matrix", "base::MatrixXd"
    
    # Weights that indicate which thrusters should be prioritized in
    # cases where multiple solutions are possible. The thrusters with lower
    # weights will be prioritized. The property size should be equal to the number of
    # thrusters and it must have only positive numbers. If there's no preference
    # between the thrusters, just assign the same weight to all of them.
    property "thrusters_weights", "base::VectorXd"

    # Names of the thrusters
    #
    # Leave empty to use no names
    property "names", "std::vector<std::string>"  

    # Limits of the thrusters
    #
    # Leave empty if you don't want to limit anything (is that really a good
    # idea ?)
    property "limits", "base::JointLimits"

    # Lists which command parameter are being controlled on a per-joint basis.
    #
    # If left empty, uses RAW by default
    property "control_modes", "std::vector<base::JointState::MODE>"
    
    # TRUE: allows the SVD solution for calculating the thrusters commands 
    #       similarly to pseudo-inverse solution.
    # FALSE: the thruster commands will be calcultated by transposing the 
    #        thruster matrix and multiplying it by the input.
    property "svd_calculation", "bool", true
    
    # Generated motor commands
    output_port "cmd_out", "base::commands::Joints"

    # The expected generated effort (as opposed to the input effort)
    output_port "expected_effort", "/base/LinearAngular6DCommand"
    
    exception_states :WRONG_SIZE_OF_CONTROLMODES, :WRONG_SIZE_OF_LIMITS, :WRONG_SIZE_OF_NAMES, :INVALID_NAME_IN_LIMITS 
end

# Generates a single constant command in the configured domain
task_context "ConstantCommand" do
    # The desired command
    property 'cmd', '/base/LinearAngular6DCommand'

    # The output command.
    output_port 'cmd_out', 'base::LinearAngular6DCommand'

    periodic(0.01)
end

# In this class the desired command, where depth is given in distance TO the floor in positive way!
task_context "ConstantCommandGroundFollower" do 
    subclasses "ConstantCommand"

    input_port "altimeter", "base::samples::RigidBodyState"
    input_port "depth", "base::samples::RigidBodyState"
    output_port "floor_position", "double"
    
    runtime_states(:NO_DEPTH_READING, :NO_ALTIMETER_READING)
    exception_states(:INVALID_DEPTH_READING,:INVALID_NEGATIVE_ALTIMETER_READING, :INVALID_TARGET_DEPTH_CONFIG) 
end
    

# Follows a set of waypoints, where each waypoint is defined 
task_context "WaypointNavigator" do
    needs_configuration

    # The trajectory to follow, expressed as a set of waypoints in the world
    # frame
    input_port "trajectory", "std::vector<base::LinearAngular6DWaypoint>"
    
    # The current system pose
    input_port "pose_sample", "base::samples::RigidBodyState"

    # The output command, as a pose in world frame that can be used by the
    # auv_control controllers
    output_port "cmd_out", "base::LinearAngular6DCommand"
   
    # Shows error between current and desired waypoint for debuging,
    # current waypoint is navigated by this controller and remaining waypoints zo follow 
    output_port("waypoint_info", "base::LinearAngular6DWaypointInfo")

    runtime_states :WAIT_FOR_WAYPOINTS, :KEEP_WAYPOINT,
        :FOLLOWING_WAYPOINTS, :POSE_SAMPLE_MISSING

    periodic(0.01)
end

# Task to convert MotionCommand2D into a velocity
#
# This allows to reuse control algorithms that deal with 2D motions (such as the
# trajectory_follower)
task_context "MotionCommand2DConverter" do
    needs_configuration

    # Command input as a velocity motion in the plan
    input_port "cmd_in", "base::MotionCommand2D"
    # Command output as an aligned velocity. Only the heading and X velocities
    # are set
    output_port "cmd_out", "base::LinearAngular6DCommand"

    port_driven "cmd_in"
end

# Task that allows a vehicle to drive long distances in an optimal 
# orientation. So the vehicle can drive faster to his target 
# position. This controler works with Commands in AlignedPosition 
# Frame.
task_context "OptimalHeadingController" do
    subclasses "Base"

    # The optimal orientation of the vehicle, if the target position
    # is on an orientation of 0.0
    property "optimal_heading", "double", 0.0

    # The distance to target position, over that the vehicle used
    # the optimal heading
    property "optimal_heading_distance", "double"
    
    # The system state. What is required depends on which parts of the state are
    # given as input
    input_port 'orientation_samples', 'base::samples::RigidBodyState'
    
    # The output command.
    output_port 'cmd_out', 'base::LinearAngular6DCommand'

    error_states :WAIT_FOR_ORIENTATION_SAMPLE
end