mars_core.orogen

name 'simulation'

using_library "mars_interfaces", :typekit => false
using_library "mars_gui", :typekit => false
using_library "mars_app", :typekit => false
using_library "main_gui", :typekit => false
using_library "mars_sim", :typekit => false
using_library "lib_manager", :typekit => false
using_library "mars_graphics", :typekit => false
using_library "QtCore", :typekit => false
using_library "QtGui", :typekit => false

import_types_from "tasks/MarsControl.hpp"
import_types_from "servoTypes.hpp"
import_types_from "jointTypes.hpp"
import_types_from "base"

using_library "actuator_dispatcher"

#Disable default deployments, most of the time stand_alone deployments are useless
#since the simulation needs plugins within the same deployment to work
#self.define_default_deployments = false

# Core orogen module that brings up the mars simulation and
# makes it accessible as a orogen module
# use subclassing to derive robot specific modules, e.g.
#
# task_context 'RobotSimulation' do
#         subclasses 'simulation::Mars'
# end
#
# Pay attention that all these tasks have to be started within 
# one deployment to be able to share their static resources.
task_context "Mars" do
	needs_configuration

        operation("loadScene").
            argument("path","/std/string")

        property('add_floor',"/bool",false)

        property('gravity',"/base/Vector3d").dynamic

        property("show_coordinate_system","bool",false).
            dynamic

        property("reaction_to_physics_error","/std/string","shutdown").
            doc("Possible Values: abort (killing sim), reset (ressing scene and simulation), warn (keep simulation running an print warnings), shutdown (stop physics but keep mars-running and set this tas to the error state)").
            dynamic


        property('initial_scene', '/std/string').
	    doc('the full path to the initial scene, this is needed because operations are not accessible during configuration within syskit (deadlock)')

	    property('initial_scenes', '/std/vector</std/string>').
	    doc('the full path to the initial scene, this is needed because operations are not accessible during configuration within syskit (deadlock)')

        property('positions', 'std/vector<simulation/Positions>' ).
	    doc('override positions in the scene (scn) file, e.g. move the terrain to create different experiment setups without changing the scene file itself')

        property('config_dir', '/std/string',"#{ENV['AUTOPROJ_CURRENT_ROOT']}/install/configuration/mars_default").
	    doc('the full path to the main configuration folder')

        property('distributed_simulation', 'bool').
	    doc('switch to active the distributed simulation if needed')

        property('enable_gui', 'bool').
	    doc('start the simulation graphical interface with this module')

        property('controller_port', 'int').
            doc('set the controller port, e.g. 1600 for communication with monster')

        property('sim_step_size', 'double',0.01).
            dynamic.
            doc('The Step-size that each mars-step calculates')

        property('raw_options', '/std/vector<simulation::Option>').
            doc('forward the original mars arguments, such as -c 1600 for setting the ode port - option consists of name, here: -c and parameter: here 1600')

		property('realtime_calc', 'bool', false).
            doc('if true, simulation runs in real time or slower, but not faster')

		property('start_sim', 'bool', false).
			doc('starts the simulation after loading the initial scene(s)')

        # Add a task context
        # operation('addPlugin').
        #       returns('int').
        #       argument('taskcontext', '/
        # method addPlugin(input: TaskContext ) return bool
        # method removePlugin(input: TaskContext) return bool

        input_port('control_action', '/simulation/Control')

        output_port('time', 'double')
        output_port('simulated_time', '/base/Time')

        port_driven 'control_action'

        exception_states :PHYSICS_ERROR
end

task_context "MarsPlugin" do
    needs_configuration

    error_states "LOST_MARS_CONNECTION"

end

task_context "CameraPlugin" do
    subclasses "MarsPlugin"
    property('name', '/std/string').
	doc('name of the camera in the scene file')
end

task_context "MarsCamera" do
    subclasses "CameraPlugin"
    needs_configuration

    output_port 'frame', ro_ptr('base::samples::frame::Frame')
end

task_context "MarsDepthCamera" do
    subclasses "CameraPlugin"
    needs_configuration
    
    output_port 'distance_image', ro_ptr('base::samples::DistanceImage')
end

task_context "MarsHighResRangeFinder" do
    subclasses "CameraPlugin"
    needs_configuration

    property('left_limit', 'double', -Math::PI/4.0).
        doc('Left limit of the vertical opening angle in radians')
    property('right_limit', 'double', Math::PI/4.0).
        doc('Right limit of the vertical opening angle in radians')
    property('resolution_vertical', 'double', Math::PI/180.0).
        doc('Vertical angular resolution in radians')

    property('upper_limit', 'double', Math::PI/4.0).
        doc('Upper limit of the horizontal opening angle in radians')
    property('lower_limit', 'double', -Math::PI/4.0).
        doc('Lower limit of the horizontal opening angle in radians')
    property('resolution_horizontal', 'double', Math::PI/180.0).
        doc('Vertical angular resolution in radians')

    property('minimum_distance', 'double', 1.0).
        doc('Smaller distances (meter) are discarded')
    property('maximum_distance', 'double', 80.0).
        doc('Larger distances (meter) are discarded')

    operation('addCamera').
        doc('Adds another camera which will be used for pointcloud creation').
        returns("bool").
        argument('name', 'std::string', "Name of the camera within the scene file").
        argument('orientation', 'double', "To get a full 360 degree view you have to add four 90-cameras with 0, 90, 180 and 270") 

    output_port('pointcloud', '/base/samples/Pointcloud').
        doc 'Created by using the distance image of the MarsDepthCamera'
end

# E.g. (ruby script): Two actuators 'rear_left' and 'rear_right'.
# actuator_names = ["rear_left", "rear_right"]
# actuator_indices = [1, 2]  <- Using a negative index to invert, using 0 to deactivate
# actuators = TaskContext.get 'mars_actuator'
# actuators.names = actuator_names
# actuators.dispatch("name_of_the_actuator_set", actuator_indices)  <- Has to be done before configure and start!
# 
# DEPRECATION: don't use this anymore since its based on the old base/actuators types
# use Joints instead
#
task_context "MarsActuator" do
    subclasses "MarsPlugin"

    operation('dispatch').
        doc('creates an output port of actuator status and an input port for setting actuator commands').
        returns("bool").
        argument('name', 'std::string', "the name of the board set. Ports called status_name and cmd_name will be created").
        argument('actuatorMap', '/std/vector</int>', "A map that maps the actuator id to the position in the Command structure. Note that a negative value can be given to autoamtically invert the commands")

    dynamic_input_port(/cmd_\w+/, "base/actuators/Command").
        doc "commands for a set of actuators set up by #dispatch"
    dynamic_output_port(/status_\w+/, "base/actuators/Status").
        doc "status of a set of actuators set up by #dispatch"

    property('names', 'std/vector</std/string>').
	doc('Array of names of the motor in the scene file. The names get mapped to motor ids starting from zero')

    port_driven
end

task_context "Joints" do
    subclasses "MarsPlugin"

    property('names', 'std/vector</std/string>').
        doc('Array of names of the motor in the scene file. The names get mapped to motor ids starting from zero')
    property('name_remap', 'std/vector</std/string>').
        doc('If this array is set the joints will be renamed for the external interface')
    property('parallel_kinematics', 'std/vector<simulation/ParallelKinematic>').
        doc('maps a single joint command to two joints to control parallel kinematics simulated by two joints')
    property('scaling', 'std/vector<double>').
	doc('Optional array of scale values that are applied to the mars joint values. Needs to be empty or the same size as names.')
    property('offset', 'std/vector<double>').
	doc('Optional array of offset values that added to the scaled mars joint values. Needs to be empty or the same size as names.')
    # optional configuration for the rigid_body_states 
    # this module will generate on the 
    property "joint_transform", "base/JointTransformVector"

    output_port( "status_samples", "base/samples/Joints" ).
	doc "status values for the joints"
    input_port( "command", "base/commands/Joints" ).
	doc "command values for the joints"

    # if the joint_transform configuration is set, this port
    # will output the joints in form of RigidBodyStates
    #
    # note: you will need a buffered connection if multiple joints
    # are served by this module in order to not miss any transforms
    output_port "transforms", "base/samples/RigidBodyState"

end

task_context "ForceTorque6DOF" do
	
	subclasses "MarsPlugin"
	
	output_port("wrenches", "std/vector< /base/samples/Wrench >")
	
	property('names', 'std/vector</std/string>').
        doc('Array of names of the ft sensors in the scene file.')
	
end

task_context "MarsTrigger" do
    subclasses "MarsPlugin"

    property "do_step","bool",false
    doc("This components triggeres the mars simulation cycle if the update-hook is called, 
        this is useful if an sequence between componentes is needed, like it is possible with the
        trigger_component")
end

task_context "MarsLaserRangeFinder" do
    subclasses "MarsPlugin"

    needs_configuration

    property('remission_values',  'bool', 0).
        doc 'include remission values in output if set to true and available in the device'
    property('start_step',  'int', -1).
        doc 'the step at which to start acquisition'
    property('end_step',    'int', -1).
        doc 'the step at which to end acquisition'
    property('scan_skip',   'int', 0).
        doc 'how much acquisitions to ignore between two acquisitions to report'
    property('merge_count', 'int', 1).
        doc 'how much ranges measurement to merge into one single reported measurement'

    property('min_range', 'double', 0.0).
        doc("Minimum valid range of the laser range finder")

    output_port('scans', 'base/samples/LaserScan').
        max_sizes('remission' => 2000, 'ranges' => 2000).
        doc "the laser scans themselves"
    property('name', '/std/string', 'laser_scanner').
	doc('name of the sensor in the scene file')
end

task_context "MarsIMU" do
    subclasses "MarsPlugin"
    
    property("imu_frame", "std/string", "imu").
	doc "The name of the imu frame."
    property("world_frame", "std/string", "world").
	doc "The name of the world frame."
    property('rotate_node_relative', '/std/vector<double>' ).
	doc('if the robot is moved my mars (yaml), rotation of the IMU may be wrong and can be set here (in degrees)')	
    property("position_sigma", "double", 0.0).
	doc "Standard deviation of the position, that will be applied to the measurement."
    property("orientation_sigma", "double", 0.0).
	doc "Standard deviation of the orientation, that will be applied to the measurement."
    property("velocity_sigma", "double", 0.0).
	doc "Standard deviation of the velocity, that will be applied to the measurement."
    property("angular_velocity_sigma", "double", 0.0).
	doc "Standard deviation of the angular velocity, that will be applied to the measurement."

    output_port('orientation_samples', '/base/samples/RigidBodyState').
        doc 'provides timestamped IMUReading samples containing the orientation estimate as reported by the IMU.'
    output_port('calibrated_sensors', '/base/samples/IMUSensors').
        doc 'provides timestamped IMUReading samples containing the calibrated sensor readings.'
    
    needs_configuration
    
    property('name', '/std/string', 'imu').
	doc('name of the node in the scene file from which to get the imu data')

    output_port('pose_samples', '/base/samples/RigidBodyState').
        doc 'provides timestamped IMUReading samples containing the complete pose'
end

# 
# this is not a very good name, since this seems to be very specific to
# underwater thrusters. Maybe call it Thrusters?
#
task_context "Actuators" do
    subclasses "MarsPlugin"
    needs_configuration

    property('node_name', 'std/string')
	doc 'the name of the vehicle in the scene file'

    property('amount_of_actuators', 'int')
	doc 'the amount of actuators the vehicle has'

    property('maximum_thruster_force', 'std/vector<double>')
	doc 'The maximum thruster foce for each actuator. Is also used as a factor. The size of the vector must equal to amount_of_actuators'

    property('thruster_position', 'std/vector</base/Vector3d>')
	doc 'Positions of the thrusters on the vehicle. The size of the vector must equal to amount_of_actuators'

    property('thruster_direction', 'std/vector</base/Vector3d>')
	doc 'Directions of the thruster force on the vehicle. The size of the vector must equal to amount_of_actuators'

    input_port("command", "base/actuators/Command").
        doc("actuator command").
        needs_buffered_connection 
    
    output_port("status", "base/actuators/Status")
    port_driven :command
end

task_context "ForceApplier" do
    subclasses "MarsPlugin"
    needs_configuration
    
    property('node_name', 'std/string')
	doc 'the name of the vehicle in the scene file'
	
    property('amount_of_actuators', 'int')
	doc 'the amount of actuators the vehicle has'
	
    property('maximum_thruster_force', 'std/vector<double>')
	doc 'The maximum thruster foce for each actuator. Is also used as a factor. The size of the vector must equal to amount_of_actuators'
	
    property('thruster_position', 'std/vector</base/Vector3d>')
	doc 'Positions of the thrusters on the vehicle. The size of the vector must equal to amount_of_actuators'

    property('joint_names','std/vector</string>').
        doc 'The names of the joints, array indexes must equal to the positions'

    property('thruster_direction', 'std/vector</base/Vector3d>')
	doc 'Directions of the thruster force on the vehicle. The size of the vector must equal to amount_of_actuators'
	
    input_port("command", "base/commands/Joints").
        doc("actuator command as joint type").
        needs_buffered_connection
    output_port("status", "base/samples/Joints")
    port_driven :command
end

task_context "Sonar" do
    subclasses "MarsPlugin"
    needs_configuration

    property('node_name', 'std/string').
	doc 'the name of the vehicle in the scene file'

    property("left_limit","double").dynamic.
	doc 'maximum left angle'
    property("right_limit","double").dynamic.
	doc 'maximum right angle'
    property("resolution","double").dynamic.
	doc 'resolution of the sonar beam'
    property("maximum_distance","double").dynamic.
	doc 'maximum distance of the sonar beam'
    property("ping_pong_mode","bool").dynamic.
        doc('if true ping pong mode is activated')


    output_port("sonar_beam","base::samples::SonarBeam").
        doc('top sonar beam')
end


task_context "MarsAltimeter" do
    subclasses "MarsPlugin"
    property('node_name', 'std/string')
	doc 'the name of the vehicle in the scene file'

    needs_configuration

    output_port("ground_distance", "/base/samples/RigidBodyState").
        doc('current ground distance simulating echo sounder')
end

task_context "AuvController" do
    subclasses "MarsPlugin"
    property('node_name', 'std/string')
	doc 'the name of the vehicle in the scene file'

    needs_configuration

    property('position','/base/Vector3d').
        dynamic

    property('orientation','/base/Quaterniond').
        dynamic

    property('cob','/base/Vector3d').
        doc('Point where the Buoyancy should be applied')

    property('buoyancy_force','double')
end

task_context "AuvMotion" do
    doc("This task models the dynamics of underwater vehicles, according to the fossen model.
	  A full 6d dynamic model without couple-effects is used.
	  Thruster-dynamics are modeld by a voltage-based model.
	  For basic functions, all properties need to be set.
	  For more information about the model, see: Fossen, T.I.: Guidance and control of ocean vehicles")

    subclasses "ForceApplier"
    
    property("linear_damp", "/base/Vector6d").
      doc("Linear damping coefficients, in 6 degrees of freedom")    
    
    property("square_damp", "/base/Vector6d").
      doc("Square damping coefficients, in 6 degrees of freedom")
    
    property("thruster_coefficients" , "/std/vector<double>").
      doc("The square coefficients of the thrusters, the size of the list must be the amount of actuators")
        
    property("voltage", "double", 0).
      doc("Maximum volatage of the thrusters, used for pwm to force calculation")
      
    property("cob", "/base/Vector3d").
      doc("Center of Bouyancy, with respect to the origin of the vehicle")
      
    property("buoyancy_force", "double", 0).
      doc("Buoyancy force applied to the vehicle")

end



#
#deployment "simulation" do
#        sim = task('mars_simulation', 'simulation::Mars')
#        acc = task('mars_actuator', 'simulation::MarsActuator')
#        cam = task('mars_camera', 'simulation::MarsCamera')
#	add_default_logger
#end
#


deployment "simulation" do
	task('simulation', 'simulation::Mars')
	task('actuators', 'simulation::Actuators')
	add_default_logger
end