uw_particle_localization.orogen

name "uw_particle_localization"

using_library "uw_localization"
using_library "machine_learning"
using_library "uwv_dynamic_model"
using_library "sonar_detectors"
using_task_library "sonar_wall_hough"
using_task_library "sonar_feature_estimator"

import_types_from "base"
import_types_from "visual_detectors/Types.hpp"
import_types_from "tasks/Types.hpp"
import_types_from "LocalizationConfig.hpp"
import_types_from "offshore_pipeline_detector"
import_types_from "uw_localization/types/particle.hpp"
import_types_from "uw_localization/types/info.hpp"
#import_types_from "uw_localization/types/map.hpp"
import_types_from "uw_localization/types/environment.hpp"
#import_types_from "uw_localization/maps/grid_map.hpp"
import_types_from "uw_localization/types/map.hpp"
#import_types_from "battery_management"
import_types_from "sonar_wall_hough/tasks/DebugTypes.hpp"
import_types_from "sonar_feature_estimator/FeatureEstimationDebugTypes.hpp"
import_types_from "sonar_detectors/SonarDetectorTypes.hpp"


task_context "Task" do
  
   # ----------------------------------------------------------------------
   # output samples 
   # ----------------------------------------------------------------------
   
   output_port("pose_samples", "/base/samples/RigidBodyState").
	doc("poses measured by the particle filter")
   
   output_port("environment", "/uw_localization/Environment").
        doc("current map environment")

   output_port("dead_reckoning_samples", "/base/samples/RigidBodyState").
        doc("poses measured only by motion model")
        
   output_port("full_dead_reckoning", "/base/samples/RigidBodyState").
	doc("poes measured only by motion model, in al 6 degrees of freedom. Only available with the advanced motion model")

   output_port("particles", "/uw_localization/ParticleSet").
        doc("current particle set used in this filter")

   output_port("debug_sonar_beam", "/uw_localization/PointInfo").
        doc("Debug information for best sonar perceptions")

   output_port("stats", "/uw_localization/Stats").
        doc("various values representing current localization state")
        
   output_port("depth_grid", "/base/samples/Pointcloud")
   
   output_port("grid_map", "/uw_localization/SimpleGrid")
   
   output_port("debug_filtered_obstacles", "sonar_detectors/ObstacleFeatures")

   # ----------------------------------------------------------------------
   # input samples 
   # ----------------------------------------------------------------------

   input_port("laser_samples", "/base/samples/LaserScan").
        doc("relative distance sample to an obstacle in the environment").
        needs_reliable_connection

   input_port("speed_samples", "/base/samples/RigidBodyState").
        doc("current motion sample retrieved from our motion model").
        needs_reliable_connection   

   input_port('pipeline_samples', '/controlData::Pipeline').
        doc("visual pipeline samples from bottom_camera when pipeline detected").
        needs_reliable_connection

   input_port("pose_update", "/base/samples/RigidBodyState").
        doc("position samples from another localization").
        needs_reliable_connection
        
   input_port("gps_pose_samples", "/base/samples/RigidBodyState").
	doc("Position Information estimated by GPS").
	needs_reliable_connection
   
#   input_port("battery_status", "/battery_management/batteryInformation").
#	doc("Status of the batteries for use in the motion model")
	
   input_port("buoy_samples_orange", "/avalon/feature/Buoy").
	doc("buoy sample of the buoy-detection")
	
   input_port("buoy_samples_white", "/avalon/feature/Buoy")	
	
    input_port("thruster_samples", "base/samples/Joints").
	  doc("Samples of thruster actions").
	  needs_reliable_connection
	  
    input_port("orientation_samples", "/base/samples/RigidBodyState").
	  doc("current orientation of the vehicle").
	  needs_reliable_connection
	  
    input_port("echosounder_samples", "base/samples/RigidBodyState").
          doc("Echosounder samples of the vehicle").
          needs_reliable_connection
          
    input_port("obstacle_samples", "sonar_detectors/ObstacleFeatures").
          doc("list of obstacle features, estimated by the sonar").
          needs_reliable_connection
          
    input_port("structur_samples", "bool").
          doc("Boolean of the structure-detection").
          needs_reliable_connection
   
   # ----------------------------------------------------------------------
   # properties
   # ----------------------------------------------------------------------

   property("debug", "bool", false).
        doc("write all debug informations to output ports")

   property("particle_number", "int", 40).
        doc("number of used particles")

   property("minimum_depth", "double", 0.0).
        doc("minimum depth for collecting perception samples")

   property("hough_interspersal_ratio", "double", 0.0).
        doc("ratio for generating new particles after each hough measurement")
        
   property("hough_timeout", "double", 20.0).
        doc("Timeout ,after not recieving any hough input for x seconds")
        
   property("hough_timeout_interspersal", "double", 0.0)
        doc("ratio for generating new random particles, after hough timeout")

   property("effective_sample_size_threshold", "double", 0.0).
	doc("effective sample size threshold controls the resampling timing and uses values between 0.0 and 1.0 particles")

   property("reset_timeout", "double", 10.0).
        doc("automatic reset of particle filter after not receiving any perception for X seconds")
        
   property("speed_samples_timeout", "double", 1.0).
        doc("If both speed_samples and thruster_samples are used").
        doc("Use thruster_samples if we have a timeout of x seconds on the speed_samples")

   property("minimum_perceptions", "int", 5).
	doc("minimum number of perceptions before starting resampling")

   property("init_position", "/std/vector<double>").
        doc("initial centre position for all particles")

   property("init_variance", "/std/vector<double>").
        doc("spatial covariance from initial centre position for initialization")

   property("yaml_map", "/std/string").
	doc("start map for localization algorithm")
	
   property("yaml_depth_map", "/std/string").
        doc("Start depth map")
        
   property("yaml_depth_output_map", "/std/string").
        doc("Save the depth map in this file")

   property("sonar_maximum_distance", "double", 20.0).
        doc("set maximum distance for filtering sonar samples")

   property("sonar_minimum_distance", "double", 1.0).
        doc("set maximum distance for filtering sonar samples")

   property("sonar_covariance", "double", 2.0).
        doc("sets sonar covariance for defining measurement uncertainty")

   property("pipeline_covariance", "double", 1.0).
        doc("sets pipeline covariance for defining measurement uncertainty")
        
   property("buoy_covariance", "double", 1.0).
	doc("set buoy detetction covariance for defining measurement uncertainty")

   property("pure_random_motion", "bool", false).
        doc("ignore velocity in speed samples for dynamic update")

   property("static_motion_covariance", "/std/vector<double>").
        doc("sets a static covariance for the motion model")
        
   property("static_speed_covariance", "/std/vector<double>").
        doc("sets a static covariance for the speed samples")

   property("sonar_importance", "double", 1.0).
	doc("sets importance ratio for sonar perception updates")

   property("pipeline_importance", "double", 0.5).
        doc("sets importance ratio for pipeline perception updates")
        
   property("buoy_importance", "double", 0.5).
	doc("sets importance ratio for buoy detection updates")

   property("perception_history_number", "int", 8).
        doc("set history size for determing uncertainty_degree of best measurement samples")

   property("init_sample_rejection", "int", 100).
        doc("dont use the first n sonar samples for state estimation")
        
   property("orientation_offset", "double", 0.0)
   
   property("filter_zeros", "bool", true).
        doc("Filter out particles with zero confidence (e.g. particle is out of map)")
        
   property("use_markov", "bool" , true)
   
   property("avg_particle_position", "bool", true).
        doc("Calculate position by calculating the avg particle position")
        
   property("use_best_feature_only", "bool", true).
        doc("Only use the best modeled feature for particle rating").
        doc("If false, feature ratings will be multiplied")
        
   #Sensor-transformation------------------------------------------------------------------
      
   
   property("sonar_position", "base/Vector3d").
	doc("transformation of the sonar, with respect to the origin of avalon")
	
   property("pipeline_position", "base/Vector3d").
	doc("transformation of the pipeline-camera, with respect to the origin of avalon")
	
   property("gps_position", "base/Vector3d").
	doc("transformation of the gps sensor, with respect to the origin of avalon")
	
  property("buoy_cam_position", "base/Vector3d").
	doc("transformation of the buoy detection cam, with respect to the origin of avalon")
	
  property("buoy_cam_rotation", "base/Vector3d").
	doc("Rotation of the buoy detection cam in the body frame")
	
  property("dvl_rotation", "base/Vector3d").
      doc("Rotation of the dvl")
	
   
   #GPS-settings------------------------------------------------------------------------------
   
   
   
   property("utm_relative_angle", "double", 0).
	doc("gives the angle difference between the coordinate-system and the utm-north") 
      
   property("gps_covarianz", "double", 1.0).
	doc("sets gps covariance for defining measurement uncertainty")
	
    property("gps_importance" , "double", 1.0).
	doc("sets importance ratio for gps perception updates")
	
    property("gps_interspersal_ratio" , "double", 0.01).
	doc("ratio for generating new particle after each gps_sample")	
	
   #Covarianz modification---------------------------------------------------------------------
   
   
   
   property("sonar_vertical_angle", "double" , 0.52).
	doc("vertical opening angle of the sonar in radian")
	
   property("sonar_covariance_reflection_factor" , "double" , 1.0).
	doc("Factor for the sonar covariance.").
	doc("This factor is used, when the sonar-distance is in an range, at which a refelction from the water surface is possible")
	
   property("sonar_covariance_corner_factor", "double" , 1.0).
	doc("Factor for the sonar covariance.").
	doc("This factor is used to size up the covariance, when the sonar leads up to a corner")
	
   property("position_covariance_threshold", "double", 1.0).
      doc("Output variances below this sigma threshold will be set the value of the threshold")
	
#------- Dynamic model parameter

    property("advanced_motion_model" , "bool" , false).
	doc("uses the advanced motion model implemented in modul dagon/uwv_dynamic_model")
	
    property("max_velocity_drift", "double", 1.0).
       doc("Maximum velocity drift threshold in the dynamic-step, in m/s").
       doc("The maximum difference between the dead reckoning velocity and the randomized particle velocity")
    
    property("param_length" , "double" , 1.4).
	doc("length of avalon")
	
    property("param_radius" , "double" , 0.15).
	doc("radius of avalon")

    property("param_mass" , "double" , 65.0).
	doc("mass of avalon")
	
    property("param_thrusterCoefficient" , "/std/vector<double>" ).
	doc("Truster Coefficents, given as an row-wise 3x6 Matrix").
	doc("First row: constant factor").
	doc("Second row: linear factor").
	doc("Third row: square factor")
	
    property("param_thrusterVoltage" ,"double" , 25.4).
	doc("Thruster Volatage")

    property("param_TCM" , "/std/vector<double>" ).
	doc("Thruster Coefficient Matrix, which sets the direction of the thruster force")
    
    property("param_linDamp" , "base/MatrixXd")
    property("param_sqDamp" , "base/MatrixXd")
    property("param_linDampNeg" , "base/MatrixXd")
    property("param_sqDampNeg" , "base/MatrixXd")
    
    property("param_centerOfGravity", "base/Vector3d")
    property("param_centerOfBuoyancy", "base/Vector3d")
    
    property("param_floating" , "bool" , true).
	doc("true, if density of avalon = density of water")
	
    property("joint_names" ,"std/vector<string>")	        
        
    
    #---- Slam properties
    
    property("use_slam", "bool", false).
      doc("Use dp-slam algorithm for additional mapping")
      
    property("use_mapping_only", "bool", true).
      doc("Use dp-slam for mapping nly, created maps are not used for lokalisation").
      doc("Takes only effect, if use_slam is true")
      
    property("single_depth_map", "bool", true).
      doc("Use only a single depth map, instead of a map for all particle")
      
    property("echosounder_variance", "double", 0.5).
      doc("Variance of the echosounder samples")
    
    property("feature_weight_reduction", "double", 0.8).
      doc("Reducce the weight of features by this value")
      
    property("feature_observation_range", "double", 10.0).
      doc("Only reduce feature_weights inside this range")
      
    property("feature_observation_minimum_range", "double", 1.0).
      doc("Only use features inside this range")
      
    property("feature_grid_resolution", "double", 0.5).
      doc("Size of one grid-cell in meter")
    
    property("feature_filter_threshold", "double", 0.0).
      doc("Only use input-features with values obove ths threshold").
      doc("If value is 0, no features will be filtered")
      
    property("feature_confidence", "double", 0.5).
      doc("Confidence of single sonar features.").
      doc("This value should be between 0 and 1")
      
    property("feature_empty_cell_confidence", "double", 0.5).
      doc("Confidence, that an observed cell is empty").
      doc("This value should be between 0 and 1")
    
    property("feature_confidence_threshold", "double", 0.2).
      doc("Only use map-features above this threshold for the map").
      doc("Features below this value will be removed during filtering")
      
    property("feature_output_confidence_threshold", "double", 0.5).
      doc("Only features above this threshold will be shown in the output-map")
      
    property("feature_observation_count_threshold", "int", 5).
      doc("If we observed a feature this many time without removing it, this feature will be saved")
    
            
        
   # ----------------------------------------------------------------------
   # stream aligner
   # ----------------------------------------------------------------------
   stream_aligner do
       max_latency 1.0
       align_port "laser_samples", 0.1
       align_port "orientation_samples", 0.01
       align_port "speed_samples", 0.01
       align_port "thruster_samples", 0.01
       align_port "pipeline_samples", 0.1
       #align_port "pose_update", 0.1
       align_port "gps_pose_samples", 0.1
       align_port "echosounder_samples", 0.1
       align_port "obstacle_samples", 0.1
   end
 
   port_driven :laser_samples
   port_driven :orientation_samples
   port_driven :thruster_samples
   port_driven :speed_samples
   port_driven :pipeline_samples
   port_driven :pose_update
   port_driven :gps_pose_samples
   port_driven :buoy_samples_white
   port_driven :buoy_samples_orange
   port_driven :echosounder_samples
   port_driven :obstacle_samples
   port_driven :structur_samples
   
   runtime_states "LOCALIZING", "NO_ORIENTATION", "NO_SONAR", "ABOVE_SURFACE", "NO_HOUGH", "NO_JOINTS_NO_DVL", "INVALID_VALUES"
   
end

task_context "OrientationCorrection" do
###################
# This Task corrects orientation samples to the wanted world-frame.
# For Correction magnetic north and 

  operation("reset").
    argument("angle", "double").
    returns("bool")

  
  property("buffer_size", "int", 10).
    doc("Size of the offset_buffer")
    
  property("min_buffer_size" , "int", 3).
    doc("Minimum number of offset_samples, used for offset calculation")  
  
  
  input_port("orientation_input", "base/samples/RigidBodyState").
    doc("Orientationsamples of the orientation-estimator")
  
  
  input_port("orientation_offset", "sonar_wall_hough/PositionQuality").
    doc("Offsetsamples of the sonar-localization")
  
  
  output_port("orientation_output", "base/samples/RigidBodyState").
    doc("North-Corrected orientation samples. Only the imu is used for correction")
    
  output_port("orientation_offset_corrected", "base/samples/RigidBodyState").
    doc("North- and Offset-corrected orientation samples")
    
  runtime_states "RESET"
  
  port_driven
end

#####################
# This Task provides a dynamic model for underwater vehicle.
# It can be used for a deadreckoning localization approach
######################
task_context "MotionModel" do

    
    input_port("thruster_samples", "base/samples/Joints").
	  doc("Samples of thruster actions").
	  needs_reliable_connection
	  
    input_port("orientation_samples", "/base/samples/RigidBodyState").
	  doc("current orientation of the vehicle").
	  needs_reliable_connection
    
    output_port("pose_samples", "/base/samples/RigidBodyState").triggered_on("orientation_samples")
  
  
    #---Motion model parameter----------------------------------------------------------------    
    
    property("advanced_motion_model" , "bool" , false).
	doc("uses the advanced motion model implemented in modul dagon/uwv_dynamic_model")    
    
    property("param_length" , "double" , 1.4).
	doc("length of avalon")
	
    property("param_radius" , "double" , 0.15).
	doc("radius of avalon")

    property("param_mass" , "double" , 65.0).
	doc("mass of avalon")
	
    property("param_thrusterCoefficient" , "/std/vector<double>" ).
	doc("Truster Coefficents, given as an row-wise 3x6 Matrix").
	doc("First row: constant factor").
	doc("Second row: linear factor").
	doc("Third row: square factor")
	
    property("param_thrusterVoltage" ,"double" , 25.4).
	doc("Thruster Volatage")

    property("param_TCM" , "/std/vector<double>" ).
	doc("Thruster Coefficient Matrix, which sets the direction of the thruster force")
    
    property("param_linDamp" , "base/MatrixXd")
    property("param_sqDamp" , "base/MatrixXd")
    property("param_linDampNeg" , "base/MatrixXd")
    property("param_sqDampNeg" , "base/MatrixXd")
    
    property("param_centerOfGravity", "base/Vector3d")
    property("param_centerOfBuoyancy", "base/Vector3d")
    
    property("param_floating" , "bool" , true).
	doc("true, if density of avalon = density of water")
	
    property("joint_names" ,"std/vector<string>")
    
    property("velocity_covariance", "base/Vector3d").
	doc("covariance of the estimated velocities.")
    
    stream_aligner do
        max_latency 1.0
        align_port "thruster_samples", 0.01
        align_port "orientation_samples", 0.01
    end
          
    port_driven
   
end

###############################################################################################################
# This Task provides a full rigid body state, by fusion of direct sensor input and localization-data
# This task works WITHOUT a stream-aligner, so the data-delay should be on a minimum, but data may be missaligned
###############################################################################################################
task_context "FastFusion" do
   
   input_port("position_samples", "/base/samples/RigidBodyState").
      doc("Contains x- and y-positions").
      doc("Alternativ to the velocity_samples, this port can also contain linear velocities")
   
   input_port("depth_samples", "/base/samples/RigidBodyState").
      doc("Contains depth and depth velocity")
      
   input_port("orientation_samples", "/base/samples/RigidBodyState").
      doc("Contains orientation and angular velocity")
      
   input_port("velocity_samples", "/base/samples/RigidBodyState").
      doc("contains linear velocites").
      doc("Velocity in body-frame")
   
   
   output_port("pose_samples", "/base/samples/RigidBodyState")
   
   property("velocity_timeout", "double", 0.5).
      doc("Timeout for velocity samples, after which the position_samples-veelocity is used")
   
   
   port_driven   
   
end



#Declares a deployment, i.e. an actual executable that contains various tasks.
deployment "uw_particle_localization_test" do
    # This is a test deployment that should not be installed
    do_not_install
    #add_default_logger

    # Declares it as periodic, with a period of 100ms
    task("uw_particle_localization", "Task")
end

deployment "orientation_correction_test" do
  do_not_install
  #add_default_logger
  
  task("orientation_correction", "OrientationCorrection")
end