gazebo_usv.orogen

name "gazebo_usv"

# If new data types need to be defined, they have to be put in a separate C++
# header, and this header will be loaded here
import_types_from "gazebo_usvTypes.hpp"

import_types_from "std"
import_types_from "base"
import_types_from "control_base"

using_library "gazebo"
using_library "base-logging"
using_library "uuv_tether_control"
import_types_from "uuv_tether_control/TetherDragAndInertiaConfig.hpp"
import_types_from "uuv_tether_control/TetherDragAndInertiaState.hpp"
using_task_library "rock_gazebo"

task_context 'WindSourceTask', subclasses: 'rock_gazebo::ModelPluginTask' do
    needs_configuration

    exception_states 'NO_TOPIC_CONNECTION'

    # Wind velocity input port
    input_port 'wind_velocity', '/base/Vector3d'

    # Wind pose output port
    output_port 'wind_pose', '/base/samples/RigidBodyState'
end

task_context 'WaveSourceTask', subclasses: 'rock_gazebo::ModelPluginTask' do
    needs_configuration

    exception_states 'NO_TOPIC_CONNECTION'

    # Wave attributes input port
    input_port 'wave_attributes', '/gazebo_usv/WaveAttributes'
end

task_context "DirectForceApplicationTask", subclasses: "rock_gazebo::ModelPluginTask" do
    needs_configuration

    # The link name follows the format: "${model}/${link}"
    property "link_name", "/std/string"

    # The force, as a vector, to be applied on ${link_name}
    input_port "force", "/base/Vector3d"
end

task_context 'TetherSimulationTask' do
    needs_configuration

    # The max length that the tether can achieve
    property 'max_tether_length', '/double'

    property 'tether_config', '/uuv_tether_control/TetherDragAndInertiaConfig'

    # The rov's link where the tether is attached
    input_port 'rov_attachment', '/base/samples/RigidBodyState'

    # The usv's link where the tether is attached   
    input_port 'usv_attachment', '/base/samples/RigidBodyState'

    # Water speed over ground, expressed in the rov 
    input_port 'water_speed', '/base/Vector3d'

    output_port 'rov_force', 'base/Vector3d'

    output_port 'usv_force', '/base/Vector3d'

    port_driven
end

# Task to limit the propulsion command and output the saturation signal when
# the inputed command is out of the range
task_context "ThrusterLimitationTask" do
    needs_configuration

    # Control limits
    # To replicate the same behaviour of the live system, set the min and max speeds to be
    # the opposite of each other.
    property "limits", "/base/JointLimits"

    # The input thruster command
    input_port "cmd_in", "/base/commands/Joints"
    # The output thruster command
    output_port "cmd_out", "/base/commands/Joints"
    # The thruster saturation signal
    output_port "saturation_signal", "/control_base/SaturationSignal"

    exception_states "INVALID_COMMAND_SIZE", "INVALID_COMMAND_PARAMETER"

    periodic 0.1
end