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"


task_context "Task" do

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

    property("send_zero_cmd_once", "bool", false).
            doc "Don't send zero motion_commands repetitively"
    property("send_zero_cmd_on_timeout", "bool", false).
            doc "If no pose update arrived within `max_latency`, send a zero command to make the robot stop. By default, no command is send"

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

    input_port("simulated_time", "base::Time").
        doc "When running on simulation or log data, you can connect this to a port which outputs the simulated current time. If this port is unconnected base::Time::now() is used."

    # Transformations
    transformer do
        transform "robot", "map"
        max_latency 0.01
    end

    # 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"

    output_port("current_trajectory", "std::vector<trajectory_follower/SubTrajectory>").
        doc "Pose error calculated by NURBSCurve3D"
    
    input_port("holonomic_trajectory", "trajectory_follower/SubTrajectory")

    operation("cancelCurrentTrajectory").
        returns("bool").
        doc("Stops following the current trajectory. Will start following the next incoming trajectory")

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

    # 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
    periodic 0.01
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

task_context "TrajectoryConverter" do
    # Properties
    property("velocity", "double", 0.1).
        doc("Driving velocity that will be set on the generated trajectory.")

    # Input ports
    input_port("poses", "std::vector<base/Pose>").
        doc("List of poses that form a trajectory")

    # Output ports
    output_port("trajectory", "std::vector<trajectory_follower/SubTrajectory>").
        doc("Trajectory the robot should follow")

    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