mars.orogen

name 'mars'

using_library "mars_interfaces", :typekit => false
using_library "mars_gui", :typekit => false
using_library "mars_app", :typekit => false
using_library "mars_sim", :typekit => false
using_library "lib_manager", :typekit => false
using_library "configmaps", :typekit => false
using_library "envire_core", :typekit => false

import_types_from "tasks/MarsControl.hpp"
import_types_from "jointTypes.hpp"
import_types_from "poseType.hpp"
import_types_from "sceneType.hpp"
import_types_from "base"


#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 'mars::Task'
# end
#
# Pay attention that all these tasks have to be started within 
# one deployment to be able to share their static resources.
task_context "Task" do
	needs_configuration

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

        operation("setPosition").
            argument("positions", "mars/Positions")

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

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

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

        property("use_now_instead_of_sim_time","/bool",false)

        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 task 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("scene_setup", "std::vector</mars/SceneConfig>")

        property('positions', 'std/vector<mars/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('initial_pose', 'mars/Pose' ).
	    doc('override initial joint positions in the scene (scn) file, i.e. set an initial pose')

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

        property('plugin_dir', '/std/string', "#{ENV['AUTOPROJ_CURRENT_ROOT']}/install/lib").
	    doc('the full path where the plugins can be found')

	property('plugins', '/std/vector</std/string>').
	    doc('additional mars plugins to load. This can be a relative path to the plugin_dir, or absolute.')

        property('simulation_property_list', '/std/vector</mars/SimulationProperty>').
	    doc('list of attributes containing plugin and property name as well as the desired value')

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

        property('enable_gui', 'bool', true).
	    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<mars::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', '/mars/Control')

        #This operation moves a node to a specific position, simpliar to the positions property but can be used during runtime
        operation("move_node").
            argument("arg","/mars/Positions")
        
        output_port('time', 'double')
        output_port('simulated_time', '/base/Time')

        port_driven 'control_action'

        exception_states :PHYSICS_ERROR
end

task_context "Plugin" do
    needs_configuration
    abstract
    error_states "LOST_MARS_CONNECTION"
end

#Do not use this prugin direct use Camera or DepthCamera instead
task_context "CameraPlugin" do
    subclasses "Plugin"
    abstract
    property('name', '/std/string').
	doc('name of the camera in the scene file')

end

task_context "Camera" do
    subclasses "CameraPlugin"
    needs_configuration

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

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

task_context "HighResRangeFinder" 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 DepthCamera'
end

task_context "Joints" do
    subclasses "Plugin"

    property("cmd_max_speed", 'bool', true).
        doc('True: For position the speed will be interpretated as max speed of the joint, otherwise the speed is set as target speed of joint.')

    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<mars/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"
    
    property("controlMode", "/mars/JointPositionAndSpeedControlMode", :IGNORE).doc('sets the how speed is interpreted in case speed and position is set :MAX_SPEED :IGNORE. unsupported: :SPEED_AT_POS')

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

	output_port( "current_values", "mars/JointCurrents")

    # 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 "Plugin"
	
	output_port("wrenches_deprecated", "std/vector< /base/samples/Wrench >")
	output_port("wrenches", "/base/samples/Wrenches")
	
	property('names', 'std/vector</std/string>').
        doc('Array of names of the ft sensors in the scene file.')
        property('name_remap', 'std/vector</std/string>').
            doc('If this array is set the sensors will be renamed for the external interface')
	
end

# This plugin can be used to "trigger" a simlation cycle,
# it can be useful for operating mars with more-than-realtime 
# mode and use the task_scheduler for a fixed execution sequence
task_context "Trigger" do
    subclasses "Plugin"

    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 "LaserRangeFinder" do
    subclasses "Plugin"

    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

# Can be used to simulate laser scanners with more than one vertical scan line
# (e.g. Velodyne).
task_context "RotatingLaserRangeFinder" do
    subclasses "Plugin"

    needs_configuration

    property('name', '/std/string', 'unknown_ray_sensor').
	    doc('Name of the RotatingRaySensor in the scene file')
	    
    property('min_range', 'double', 0.0).
        doc("Minimum valid range of the laser range finder")
        
    property('max_range', 'double', 20.0).
        doc("Maximum valid range of the laser range finder")

    property('output_type', '/std/string', "depthmap").
        doc("Output format. Select: 'depthmap', 'pointcloud' or 'both'")

    output_port('pointcloud', '/base/samples/Pointcloud').
        doc 'Received distances converted to a pointcloud'

    output_port("laser_scans", "base::samples::DepthMap").
        doc 'Received distances converted to DepthMap'
end

task_context "IMU" do
    subclasses "Plugin"
    
    property("provide_position", "/bool",false).
        doc("Set to true is this Sensor should output position readings")

    property("provide_orientation", "/bool",true).
        doc("Set to true is this Sensor should output orientation readings")

    property("provide_velocity", "/bool",true).
        doc("Set to true is this Sensor should output angular/translational velocities")


    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')
	
    operation('estimate_bias').
        argument('duration', '/uint16_t').
        returns('/bool').
        doc("Normally triggers bias estimation on the xsens imu, in simulation thios does nothing") 

	
end


task_context "Sonar" do
    subclasses "Plugin"
    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 "Altimeter" do
    subclasses "Plugin"
    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





###Here are comming AUV specific plugins for motion building

task_context "ForceApplier" do
    subclasses "Plugin"
    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

# This plugin models a artificial buoancy for AUVs,
# the auv will stop to "float" at Z-Value 0 (water surface)
task_context "AuvController" do
    subclasses "Plugin"
    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 'just_mars' do

    task( "mars", "mars::Task").
        realtime.
        priority(85)

    add_default_logger.
        realtime

end

deployment 'asguard_tests' do

    task( "mars", "mars::Task").
        realtime.
        priority(85)
    
    task("mars_actuators", "mars::Joints").
        realtime.
        priority(71)
    
    # This one is for the passive joint
    task('sysmon', 'mars::Joints'). 
        realtime.
        priority(71)
    
    task('xsens', 'mars::IMU').
        realtime.
        priority(25)
    
    task('velodyne', 'mars::RotatingLaserRangeFinder').
         periodic(1.0)
    
    add_default_logger

end

deployment 'crex_tests' do
    task( "mars", "mars::Task").
        realtime.
        priority(85)

    task("front_left_leg", "mars::Joints").
        realtime.
        priority(71)

    task("front_right_leg", "mars::Joints").
        realtime.
        priority(71)

    task("middle_left_leg", "mars::Joints").
        realtime.
        priority(71)

    task("middle_right_leg", "mars::Joints").
        realtime.
        priority(71)

    task("rear_left_leg", "mars::Joints").
        realtime.
        priority(71)

    task("rear_right_leg", "mars::Joints").
        realtime.
        priority(71)

    task("front_left_leg_contact", "mars::Joints").
        realtime.
        priority(71)

    task("front_right_leg_contact", "mars::Joints").
        realtime.
        priority(71)

    task("middle_left_leg_contact", "mars::Joints").
        realtime.
        priority(71)

    task("middle_right_leg_contact", "mars::Joints").
        realtime.
        priority(71)

    task("rear_left_leg_contact", "mars::Joints").
        realtime.
        priority(71)

    task("rear_right_leg_contact", "mars::Joints").
        realtime.
        priority(71)

    task("ft_sensors", "mars::ForceTorque6DOF").
        realtime.
        priority(71)

    task('xsens', 'mars::IMU').
        realtime.
        priority(25)

    task('velodyne', 'mars::RotatingLaserRangeFinder').
        periodic(1.0)

    add_default_logger.
        realtime
end