motion_planners.orogen

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

using_library "motion_planners"
using_library "planning_environment"
using_library "octomap"

import_types_from "motion_planners/Config.hpp"
import_types_from "motion_planners/planner_factory/abstract/AbstractPlannerConfig.hpp"
import_types_from "planning_environment/Config.hpp"


import_types_from "base"

task_context "PlannerTask" do abstract

    # Configuration needed
    needs_configuration

    runtime_states  'GOAL_RECEIVED', 'PLANNING', 'MISSING_TRANSFORMATION', 'PATH_FOUND', 'NO_PATH_FOUND', 'TIMEOUT',
                    'START_STATE_IN_COLLISION', 'GOAL_STATE_IN_COLLISION',
                    'INVALID_START_STATE', 'INVALID_GOAL_STATE',  'UNRECOGNIZED_GOAL_TYPE',
                    'APPROXIMATE_SOLUTION','CRASH','NO_JOINT_STATUS',
                    'START_JOINTANGLES_NOT_AVAILABLE', 'GOAL_JOINTANGLES_NOT_AVAILABLE',
                    'CONSTRAINED_POSE_NOT_WITHIN_BOUNDS', 
                    'KDL_TREE_FAILED', 'KDL_CHAIN_FAILED', 'URDF_FAILED', 'NO_KINEMATIC_SOLVER_FOUND', 'IK_FOUND',
                    'NO_IK_SOLUTION', 'NO_FK_SOLUTION', 'IK_TIMEOUT', 'IK_JOINTLIMITS_VIOLATED', 'INVALID_KINEMATIC_STATE',
                    'NO_KINEMATIC_CONFIG_FILE', 'KINEMATIC_CONFIG_READ_ERROR',
                    'PLANNING_REQUEST_SUCCESS', 'ROBOTMODEL_INITIALISATION_FAILED',
                    'PLANNER_INITIALISATION_FAILED', 'MODEL_OBJECT_ERROR', 'UNKNOWN_STATE'

    # Planner Config
    property("config", "/motion_planners/Config")

    # Input port.
    input_port("joints_status", "/base/samples/Joints").
        doc("Current joints angles")

    input_port("octomap_in", "/planning_environment/OctomapContainer").
        doc("Octomap data representing the environment in the body frame")
                
    input_port("known_object", "/motion_planners/ModelObject").
        doc("Add/Remove known model object in the environment. This object is considered as an external object")

    input_port("grasp_object", "/motion_planners/ModelObject").
        doc("Add/Remove an object at any link of the robot. This object is considered as a robot's link")

    # Output port.
    output_port("solving_time", "/double").
        doc("Time taken for solving the problem. This time represent how long it took for the
            solver to solve the problem.")

    output_port("environment_out", "/base/samples/Pointcloud").
        doc("Poincloud data representing the environment")

    output_port("collided_links", "/collision_detection/CollisionLinksName").
        doc("Collision objects names")    

    output_port("debug_target_pose", "/base/samples/RigidBodyState").
        doc("Target pose")

    output_port("debug_check_pose_solution", "/base/commands/Joints").
        doc("The joint angle for the passed pose.") 

    operation("resetState")

    port_driven

end


task_context "SingleArmPlannerTask", subclasses: "PlannerTask" do

    # Configuration needed
    needs_configuration

    input_port("target_joints_angle", "/base/commands/Joints").
        doc("Target joints angles")

    input_port("target_pose", "/base/samples/RigidBodyState").
        doc("Target pose")
        
    input_port("target_group_state", "/std/string").
        doc("Target joints for the planning group stored in srdf under a tag")  


    input_port("debug_check_pose", "/base/samples/RigidBodyState").
        doc("Pose which are checked for collisions and the joint angle are determined with the IK without planning.")

    # Currently only optimization based planner can make use of this predicted trajectory.
    # The first and last value of the predicted trajectory should be the initial and target joint value.
    input_port("predicted_trajectory", "/base/JointsTrajectory").
        doc("Predicted trajectory for the given problem.")

    input_port("constrainted_pose", "/motion_planners/ConstraintPlanning").
        doc("Constrained target pose")

    output_port("planned_trajectory", "/base/JointsTrajectory").
        doc("The planned trajectory which should be followed")

    output_port("debug_num_of_iterations", "/std/size_t").
        doc("Number of iteration used to solve the planning problem. Only optimization based planner will output this debug message.")

    output_port("debug_initial_trajectory", "/base/JointsTrajectory").
        doc("Output the initial trajectory used for solving the planning problem. Only optimization based planner will output this debug message.")

    port_driven

#   Uncomment the following lines and also in the task side,
#   if the poincloud is not in the body frame
#    transformer do
#        transform "pointcloud", "body"
#        align_port "environment_in"
#        max_latency 0.1
#    end

end