exploration.orogen

    name "exploration"
version "0.1"

using_library "envire"
using_library "exploration"
import_types_from 'base'
import_types_from 'envire'
import_types_from "exploration/Config.hpp"
import_types_from "exploration/ExplorationPlannerTypes.hpp"
import_types_from "explorationTypes.hpp"

import_types_from "base"
    
task_context "Task" do
    needs_configuration
    
    property("config", "exploration/Config").
        doc("Exploration config file.")

    property("traversability_map_id", "std/string", "/traversability").
    doc("Try to extract this traversability map, otherwise the first traversability map will be used (if it is the only one)")
    
    property("polygons", "/std/vector<exploration/ConfPolygon>").
    doc("List of polygons that represents the robot's view")
    
    property("min_goal_distance", "double", 1.0).
    doc("Since some planners don't work very well with goalPoses too close to the robot's pose, a minimal goalDistance (in m) can be set. Goals that are not satisfying this condition will be thrown out.")
    
    property("robot_length_x_m", "double", 1.0).
	doc("Length of the robot in meter")

    property("robot_width_y_m", "double", 1.0).
        doc("Width of the robot in meter")

    property("flush_map_interval_sec", "double", 6.0).
        doc("Interval to send the exploration map.")
    
    operation("clearPlannerMap").
        doc("Clears the internal exploration map. All obstacles will be readded by receiving the next trav map.")
    
    input_port('envire_environment_in', ro_ptr('std/vector</envire/BinaryEvent>')).
        doc("Traversability map. Has to be received once")

    output_port('all_goals_debug', 'std/vector<base/samples/RigidBodyState>').
        doc "Contains all goals before getCheapest() is called / before sorting out"    
        
    output_port('goals_out', 'std/vector<base/samples/RigidBodyState>').
        doc "Calculated goal, using x and y"
        
    output_port('goal_out_best', 'base/samples/RigidBodyState').
        doc("Just the first (best) goal of the goal_out list.")

    input_port("pose_samples", "base/samples/RigidBodyState").
        needs_reliable_connection.
        doc "Robot pose in traversability frame"

    output_port('explore_map', ro_ptr('std/vector</envire/BinaryEvent>')).
        doc("Debug port, gives the current explored map as envire::Traversability map")

    operation("calculateGoals").
        doc("Triggers the calculation of a new list of goals")

    operation("sendNextGoal").returns('/bool').
        doc("Send next from the list of goals")

    port_driven 'envire_environment_in', 'pose_samples'

    # If no new exploration points can be found
    # EXPLORATION_DONE will be set, otherwise the state is RUNNING.
    runtime_states :EXPLORATION_DONE
    #periodic(1.0)
end