sonar_feature_detector.orogen

name "sonar_feature_detector"

using_library "uw_localization"
using_library "sonar_detectors"
using_task_library "auv_control"

import_types_from "base"
import_types_from "uw_localization/types/map.hpp"
#import_types_from "sonar_feature_detectorTypes.hpp"
import_types_from "sonar_detectors/SonarDetectorTypes.hpp"
import_types_from "auv_control/6dControl.hpp"

task_context "Task" do
  
  input_port("grid_maps", "uw_localization/SimpleGrid").
    doc("GridMap, conatining a grid of sonar features")
    
  input_port("pose_samples", "base/samples/RigidBodyState").
    doc("Pose sample with the actual position of the vehicle")
    
  output_port("features", "sonar_detectors/SonarFeatures").
    doc("A vector potenial sonar features")
    
  output_port("next_target", "base/Vector3d").
    doc("Position of the next servoing target")
    
  output_port("next_target_feature", "sonar_detectors/SonarFeature").
    doc("Feature information of the next servoing target")
    
  output_port("next_target_command", "base/LinearAngular6DCommand").
    doc("Position command of the next servoing target")
    
  property("map_origin", "base/Vector2d").
    doc("Origin of our map")
    
  property("map_span", "base/Vector2d").
    doc("Span of our map")
    
  property("minimum_wall_distance", "double", 1.0).
    doc("Ignore features, which are closer than x meters tto the wall")
    
  property("filter_border_structures", "bool", true).
    doc("Filter out structures, which are connected with the wall-borders")
    
  property("optimal_object_size", "double", 2.0).
    doc("Rate features with this size as best")
    
  property("minimum_object_cells", "int", 1).
    doc("Objects must have at least this many cells")
    
  property("minimum_number_of_targets", "int", 0).
    doc("Minimum number of target-features, before starting target servoing")
    
  property("confidence_weight", "double", 1.0).
    doc("Weight factor for optimal path search. If this property is a high value, paths to a feature with high confidence will be rated better")
  
  property("servoing_depth", "double", -4.0).
    doc("Depth for target-servoing")
  
  operation("fix_map").
    doc("Fix the current map-features and go into servoing mode")
    
  port_driven 
  
  runtime_states "BUILDING_MAP", "TARGET_SERVOING", "REACHED_TARGET", "SERVOING_FINISHED", "NOT_ENOUGH_TARGETS"
    
end