simple_path_planner.orogen

name "simple_path_planner"
# Optionally declare the version number
# version "0.1"

using_library "envire"
using_library "nav_graph_search"
import_types_from "base"
import_types_from "envire"	

task_context "Task" do
    needs_configuration
    
    exception_states 'GOAL_ON_OBSTACLE', 'NO_PATH_TO_GOAL', 'START_OUT_OF_GRID', 'GOAL_OUT_OF_GRID'

    property("recalculate_trajectory_distance_threshold", "double", 0.2).
        doc("If the distance between the current robot position and its position during the last recalculation exceeds this threshold, the trajectory will be recalculated. Set to 0 to disable.")

    property("replan_timeout_ms", "int", 0).
        doc("Used to initiate replannig after the defined amount of time (in milliseconds), set to 0 to disable")

    property("trajectory_z_offset", "double", 0.03).
        doc("An offset in Z that gets added, if the trajectory is projected on top of an MLS")
        
    property("statistics_path", "std/string").
        doc("If set the statistics will be written to the passed file within the stopHook")
        
    property("replanning_on_new_start_position", "bool", false).
        doc("By default only a new map or a new goal position initiates a replanning")
        
    property("replanning_on_new_goal_position", "bool", true).
        doc("If set to false by default only a new map will initiate a replanning")
        
    property("remove_obstacles_radius", "double", 0.0).
        doc("If > 0 before each planning within the surrounding area of the robot the obstacles will be removed, radius in meter")
        
    property("avoid_obstacles_on_goal", "bool", false).
        doc("If set to true and the goal position is set on an obstacle it will be moved towards the start position until a free cell has been found")    

    # Input
    input_port('envire_environment_in', ro_ptr('std/vector</envire/BinaryEvent>')).
        doc("Traversability map. Has to be received once")

    input_port('start_position_in', 'base/Vector3d').
        doc "Start position in the world, just using x and y"

    input_port('target_position_in', 'base/Vector3d').
        doc "Target position in the world, just using x and y"

    # Output
    output_port("trajectory_out", "std::vector</base/Trajectory>").
        doc("Trajectory from goal to start, special wrapping for the trajectory follower")

    output_port('debug_start_pos', 'base::Waypoint').
        doc "Start position of current planning try"
    output_port('debug_goal_pos', 'base::Waypoint').
        doc "goal position of current planning try"

    transformer do
        #Transformation from the body center point into the coordinate frame of the output trajectory
        transformation("body_center", "trajectory")
        #Transformation from the map to the body center point
        transformation("body_center", "map")
        #Transformation from the the body center to the input position
        transformation("body_center", "input_position")

        max_latency(0.1)
    end

    port_driven
end

deployment "simple_path_planner_deployment" do
    task("simple_path_planner", "Task")
end