avalonControl.orogen

name "avalon_control"
version "0.1"

import_types_from "base"
#import_types_from 'motcon_controller'
import_types_from "hbridge"
#import_types_from "offshore_pipeline_detector"

import_types_from "tasks/pidcontroller.h"
import_types_from "AvalonControl.hpp"
import_types_from "auv_control"
type_export_policy :used

task_context 'PositionControlTask' do
    property 'x_pid',   '/avalon_motor_controller/PIDSettings'
    property 'y_pid',   '/avalon_motor_controller/PIDSettings'

    # Maximum duration between two commands, in seconds
    property('timeout', 'double', 1)

    property('optimal_heading_distance', 'double', 2.0)
    
    property('position_variance_threshold', 'double', 2.0).
        doc('threshold for position variance; if position is unstable, the auv should not move!')
    
    input_port('pose_samples', '/base/samples/RigidBodyState').
        doc("the current pose estimate")

    input_port('position_commands', '/base/AUVPositionCommand').
        doc("the input commands")

#    input_port('pipeline_samples', '/controlData/Pipeline')

    output_port('motion_commands', '/base/AUVMotionCommand').
        doc("the output commands")
    
    runtime_states :WAITING_FOR_ORIENTATION, :WAITING_FOR_COMMAND, :WAITING_FOR_VALID_ORIENTATION, :POSITION_VARIANCE_TO_HIGH

    port_driven :pose_samples
end

task_context 'MotionControlTask' do
    # PID settings for the depth controller. Generates values for the middle
    # vertical thruster
    property 'z_pid',   '/avalon_motor_controller/PIDSettings'
    # Factor applied to the PWM value generated for the middle vertical thruster
    # for depth control to generate the corresponding part for the rear thruster
    property('z_coupling_factor', 'double', 0)
   
    property('dagon_mode','/bool',false)

    input_port('dummy_feedback',"/base/actuators/Status")

    property('use_min_ground_distance', 'bool', true).
	doc("if true, avalon tryes to keep at leased the given distance to ground")
    property('min_ground_distance', 'double', 0.2).
	doc("Minimum distance to the ground, only applied if use_ peoperty is set")
    input_port("ground_distance", "base::samples::RigidBodyState").
	doc("needs to be connected if ground distance should be kept, also use_ peoeprty needs to be true")

    # PID settings for the heading controller. Generates values for the rear
    # thruster
    property 'heading_pid', '/avalon_motor_controller/PIDSettings'

    #Target Pitch value to reach for the controller in RAD
    property('pitch_target','double',0)

    # Factor applied to commands in m/s to compute the PWM for middle thruster
    property('y_factor', 'double', 0)
    # Factor applied to the PWM for the middle horizontal thruster to get the
    # corresponding part for the rear horizontal thruster
    property('y_coupling_factor', 'double', 0)

    # PID settings for the pitch controller. Generates values for the rear
    # thruster
    property 'pitch_pid',   '/avalon_motor_controller/PIDSettings'

    # Factor applied to commands in m/s to compute the PWM for left and right
    # thrusters
    property('x_factor', 'double', 0)

    # Maximum duration between two commands, in seconds
    property('timeout', 'double', 1)

    property('cutoff','std/vector<double>'). #[0.65,0.65,0,65,0.65,0.5,0.65]).
    	doc("Maximum allowed Value between 0 and 1 that is an cutoff for maximum pwm value 0.8 means 80% PWM")

    property('joint_names', 'std/vector<string>').
	doc("Names of the thruster-joints")

    #Only for dagon
    property('turn_coupling_factor',"double")

    input_port('pose_samples', '/base/samples/RigidBodyState').
        doc("the current pose estimate")

    input_port('motion_commands', '/base/AUVMotionCommand').
        doc("the input commands")

    # The speed commands
    output_port("hbridge_commands","base/actuators/Command")
    
    # The speed commands
    output_port("joint_commands","base/commands/Joints")
    input_port("joints_in","base/samples/Joints")

    # The internal state of the controller
    output_port 'debug', '/avalon_control/MotionControllerState'
	
	output_port('estimated_ground_pos', 'double')

    error_states :TIMEOUT, :CUTOFF_VECTOR_INVALID, :JOINT_NAMES_INVALID
    runtime_states :WAITING_FOR_ORIENTATION, :WAITING_FOR_COMMAND

#    port_driven :pose_samples
    periodic 0.1
end

task_context "FakeWriter" do

    property('speed_x','double',0).dynamic
    property('speed_y','double',0).dynamic
    property('Z','double',0).dynamic
    property('heading','double',0).dynamic

    output_port('motion_commands', '/base/AUVMotionCommand')

    periodic(0.1)
end

task_context "RelFakeWriter" do
    output_port('position_command',"base::AUVPositionCommand")
    property('x','double',0).dynamic
    property('y','double',0).dynamic
    property('z','double',0).dynamic
    property('heading','double',0).dynamic

    periodic(0.1)
end

task_context "TrajectoryFollower" do
    output_port('next_position',"base::Waypoint")
    output_port('position_command',"base::AUVPositionCommand")
    input_port('pose_samples',"base::samples::RigidBodyState")

    property("trajectory", "/base/Trajectory") 

    #If END_REACHED reset thespline to the start position, result in a infity loop, 
    #usefull for demo missions, but should be false in daily work
    property('loop_spline','bool',false)

    #Deny reverse jump on a trajectory locally, to prevent influence of swining controllers on small step sizes
    property('deny_reverse','bool',true)
    property('use_zero_heading','bool',false).doc("use the zero heading for debuggin, instead calculated heading")
    property('timeout_at_end_before_mark_end_reached','double',10)
    property('final_heading','double').doc("defaults to unset, so not taken into account")
    property('step_width','double',2.0)
    property('geometrical_resolution','/double', 0.1)

    #If the is not set, the next position is take either from step_with from the current location based,
    #independat if the vehicle might jup on the spline. For calculation the NEXT position to the spline for the vehicle is taken
    #if this is set, the value indicates the maximum-allowed jump-value on the spline. This means the spline is started at the beginning, and the allowed
    #next position on the spline cannot be more far away than this property. The result will be that the vehicle follows the complete spline from beginning to end
    #independant id another spoint of the spline is closer to the vehicle.
    property('max_spline_jump_distance','double')
   
    output_port 'next_pos_on_spline', '/double' 
    output_port 'last_pos_on_spline', '/double' 
    output_port 'segment_dist', '/double' 

    output_port 'world_command', 'base::LinearAngular6DCommand'

    runtime_states(:ALIGN_AT_END, :REACHED_END, :WAITING_FOR_POSE, :CANNOT_FIND_CLOSED_POINT)

    port_driven 
end

task_context "MotionFeedbackTask" do
   # A configuration property (here, a std::string). Its value can be retrieved
   # in the C++ code with # _config_value.get() and _config_value.set(new_value).
   property "config_value", "/std/string"

   # An input port, i.e. an object from which the component gets data from
   # other components' outputs
   #
   # Data can be retrieved using _input.read(value), which returns true if data
   # was available, and false otherwise. _input.connected() returns if this
   # input is connected to an output or not.
   input_port('hbridge_feedback', '/base/actuators/Status').
       doc("Feedback of HBridge commands. Contains the actual motor_status output of the HBridge.")

   # An output port, i.e. an object to which the component pushes data so that
   # it is transmitted to other components' inputs
   #
   # Data can be written using _output.write(value). _output.connected() returns
   # if this output is connected to an input or not.
   output_port('hbridge_status', '/base/actuators/Status').
       doc("Feedback of HBridge commands. Contains the actual motor_status output of the HBridge.")

   # If you want that component's updateHook() to be executed when the "input"
   # port gets data, uncomment this
   port_driven "hbridge_feedback"
end

# Speed control on Avalon is using all 6 thrusters
#
#  - the depth control is using both middle and rear vertical thrusters. A PID
#    controller generates the value for the middle thruster, and then the
#    depth_mb_ratio value is applied to generate the value for the rear
#    thruster.
#  - the pitch control is using the rear vertical thruster
#  - the striving control is using the middle and rear horizontal thruster. It
#    is open loop, and therefore only uses a linear mapping between the
#    commands in m/s and the actual thruster values.
#  - the heading control is using the rear horizontal thruster
#  - the forward speed control is an open-loop control that maps command in m/s
#    to the actual thruster values for left and right horizontal thrusters
# task_context 'SpeedControlTask' do
#     # PID settings for the depth controller. Generates values for the middle
#     # vertical thruster
#     property('z_pid', '/avalon_motor_controller/PIDSettings')
#     # Factor applied to the PWM value generated for the middle vertical thruster
#     # for depth control to generate the corresponding part for the rear thruster
#     property('z_coupling_factor', 'double', 0)
#
#     # PID settings for the pitch controller. Generates values for the rear
#     # thruster
#     property('pitch_pid', '/avalon_motor_controller/PIDSettings')
#
#     # Factor applied to commands in m/s to compute the PWM for middle thruster
#     property('y_factor', 'double', 0)
#     # Factor applied to the PWM for the middle horizontal thruster to get the
#     # corresponding part for the rear horizontal thruster
#     property('y_coupling_factor', 'double', 0)
#
#     # PID settings for the heading controller. Generates values for the rear
#     # thruster
#     property('heading_pid', '/avalon_motor_controller/PIDSettings')
#
#     # Factor applied to commands in m/s to compute the PWM for left and right
#     # thrusters
#     property('x_factor', 'double', 0)
#
#     input_port('pose_samples', '/base/samples/RigidBodyState').
#         needs_data_connection
#     input_port('speed_commands', '/avalon_control/SpeedCommand').
#         needs_data_connection
#
#     output_port("motor_commands", "/controlData/Motcon")
#     output_port('debug', '/avalon_control/SpeedControllerState')
#     port_driven :pose_samples
# end