trajectory_follower.orogen

name "trajectory_follower"
version "0.1"

import_types_from "base"

using_library "trajectory_follower"
using_library "base-lib"

import_types_from "trajectory_follower/TrajectoryFollowerTypes.hpp"
import_types_from "trajectory_follower/SubTrajectory.hpp"

task_context "Task" do

    # Properties
    property("follower_config", "trajectory_follower/FollowerConfig" ).
            doc "Combined follower config"

    # Input ports
    input_port("trajectory", "std::vector</base/Trajectory>").
        doc("Base-Trajectory the robot should follow").
        needs_buffered_connection   
    
    input_port("subtrajectory", "std::vector<trajectory_follower/SubTrajectory>").
        doc("Subtrajectory the robot should follow").
        needs_buffered_connection

    input_port("robot_pose", "/base/samples/RigidBodyState").
        doc "Position and orientation of the Robot"

    # Output ports
    output_port("motion_command", "base/commands/Motion2D").
        doc "Drive command that should steer the robot to the target Pose"

    output_port("follower_data", "trajectory_follower/FollowerData").
    	doc "Pose error calculated by NURBSCurve3D"
    	
    input_port("holonomic_trajectory", "trajectory_follower/SubTrajectory")

    # Runtime state for when trajectory finished or when actively following one
    runtime_states :FINISHED_TRAJECTORIES, :FOLLOWING_TRAJECTORY, :TURN_ON_SPOT, :LATERAL, :SLAM_POSE_INVALID

    # Runtime error state entered when the initial stability test failed for a
    # particular trajectory. Note that the component might switch back to
    # runtime state if a new trajectory / new pose is received
    error_states :STABILITY_FAILED

    needs_configuration
    port_driven
end

task_context "TurnVelocityToSteerAngleTask" do
    # Properties
    property("ackerman_ratio", "double", 0.5 ).
            doc "Ackermann ratio"

    property("wheel_base", "double" ).
            doc "Wheel base of the vehicle"
          
    property("max_steering_angle", "double", 0.523).
	doc "Steering angle upper limit in radians"

    # Input ports
    input_port("motion_command_in", "base/commands/Motion2D").
        doc "Motion command as forward velocity and turn velocity"

    # Output ports
    output_port("motion_command", "base/commands/Motion2D").
        doc "Motion command as forward velocity and steering angle"

    needs_configuration
    port_driven
end

deployment "test_trajectory" do
    do_not_install
    trajectory_task = task("trajectory", "Task")
    
    add_default_logger
    if !corba_enabled?
        browse trajectory_task
    end
end