-- altera vhdl_input_version vhdl_2008
-- XXX: QSYS Fix (https://www.intel.com/content/www/us/en/support/programmable/articles/000079458.html)

library ieee;
use ieee.std_logic_1164.all;
use ieee.numeric_std.all;

use work.rtps_package.all;

package ros_package is
    
    constant GID_WIDTH : natural := 192;
    type GID_TYPE is array (0 to (GID_WIDTH/WORD_WIDTH)-1) of std_logic_vector(WORD_WIDTH-1 downto 0);
    
    type UUID_TYPE is array (0 to 15) of std_logic_vector(CDR_INT8_WIDTH-1 downto 0);
    constant UUID_WIDTH : natural := UUID_TYPE'length * CDR_INT8_WIDTH;
    
    constant UUID_UNKNOWN : UUID_TYPE := (others => (others => '0'));
    
    function to_unsigned(input : UUID_TYPE) return unsigned;
    function to_UUID(input : std_logic_vector) return UUID_TYPE;
    
    type ROS_TIME_TYPE is record
        sec     : std_logic_vector(CDR_LONG_WIDTH-1 downto 0);
        nanosec : std_logic_vector(CDR_LONG_WIDTH-1 downto 0);
    end record;
    
    constant ROS_TIME_ZERO      : ROS_TIME_TYPE := (sec => (others => '0'), nanosec => (others => '0'));
    constant ROS_TIME_INFINITE  : ROS_TIME_TYPE := (sec => (others => '1'), nanosec => (others => '1'));
    constant ROS_TIME_INVALID   : ROS_TIME_TYPE := (sec => (others => '1'), nanosec => x"fffffffe");
    
    subtype ROS_DURATION_TYPE is ROS_TIME_TYPE;
    
    constant ROS_DURATION_ZERO      : ROS_DURATION_TYPE := (sec => (others => '0'), nanosec => (others => '0'));
    constant ROS_DURATION_INFINITE  : ROS_DURATION_TYPE := (sec => (others => '1'), nanosec => (others => '1'));
    
    constant ROS_TIME_WIDTH : natural := 64;
    
    function to_unsigned(input : ROS_TIME_TYPE) return unsigned;
    function to_ROS_TIME(input : std_logic_vector) return ROS_TIME_TYPE;
    
    -- Helper Function
    function gen_duration(s,ns : integer) return ROS_DURATION_TYPE;
    function gen_duration(t : time) return ROS_DURATION_TYPE;
    
    type ROS_QOS_PROFILE_TYPE is record
        HISTORY_QOS                     : std_logic_vector(CDR_ENUMERATION_WIDTH-1 downto 0);
        HISTORY_DEPTH                   : std_logic_vector(CDR_LONG_WIDTH-1 downto 0);
        RELIABILITY_QOS                 : std_logic_vector(CDR_ENUMERATION_WIDTH-1 downto 0);
        DURABILITY_QOS                  : std_logic_vector(CDR_ENUMERATION_WIDTH-1 downto 0);
        DEADLINE_QOS                    : DURATION_TYPE;
        LIFESPAN_QOS                    : DURATION_TYPE;
        LIVELINESS_QOS                  : std_logic_vector(CDR_ENUMERATION_WIDTH-1 downto 0);
        LEASE_DURATION                  : DURATION_TYPE;
        AVOID_ROS_NAMESPACE_CONVENTION  : boolean;
    end record;
    
    constant ROS_QOS_PROFILE_SENSOR_DATA : ROS_QOS_PROFILE_TYPE := (
        HISTORY_QOS                     => KEEP_LAST_HISTORY_QOS,
        HISTORY_DEPTH                   => std_logic_vector(to_unsigned(5, CDR_LONG_WIDTH)),
        RELIABILITY_QOS                 => BEST_EFFORT_RELIABILITY_QOS,
        DURABILITY_QOS                  => VOLATILE_DURABILITY_QOS,
        DEADLINE_QOS                    => DEFAULT_DEADLINE_QOS,
        LIFESPAN_QOS                    => DEFAULT_LIFESPAN_QOS,
        LIVELINESS_QOS                  => DEFAULT_LIVELINESS_QOS,
        LEASE_DURATION                  => DEFAULT_LEASE_DURATION,
        AVOID_ROS_NAMESPACE_CONVENTION  => FALSE
    );
    
    constant ROS_QOS_PROFILE_PARAMETERS : ROS_QOS_PROFILE_TYPE := (
        HISTORY_QOS                     => KEEP_LAST_HISTORY_QOS,
        HISTORY_DEPTH                   => std_logic_vector(to_unsigned(1000, CDR_LONG_WIDTH)),
        RELIABILITY_QOS                 => RELIABLE_RELIABILITY_QOS,
        DURABILITY_QOS                  => VOLATILE_DURABILITY_QOS,
        DEADLINE_QOS                    => DEFAULT_DEADLINE_QOS,
        LIFESPAN_QOS                    => DEFAULT_LIFESPAN_QOS,
        LIVELINESS_QOS                  => DEFAULT_LIVELINESS_QOS,
        LEASE_DURATION                  => DEFAULT_LEASE_DURATION,
        AVOID_ROS_NAMESPACE_CONVENTION  => FALSE
    );
    
    constant ROS_QOS_PROFILE_DEFAULT : ROS_QOS_PROFILE_TYPE := (
        HISTORY_QOS                     => KEEP_LAST_HISTORY_QOS,
        HISTORY_DEPTH                   => std_logic_vector(to_unsigned(10, CDR_LONG_WIDTH)),
        RELIABILITY_QOS                 => RELIABLE_RELIABILITY_QOS,
        DURABILITY_QOS                  => VOLATILE_DURABILITY_QOS,
        DEADLINE_QOS                    => DEFAULT_DEADLINE_QOS,
        LIFESPAN_QOS                    => DEFAULT_LIFESPAN_QOS,
        LIVELINESS_QOS                  => DEFAULT_LIVELINESS_QOS,
        LEASE_DURATION                  => DEFAULT_LEASE_DURATION,
        AVOID_ROS_NAMESPACE_CONVENTION  => FALSE
    );
    
    constant ROS_QOS_PROFILE_TRANSIENT : ROS_QOS_PROFILE_TYPE := (
        HISTORY_QOS                     => KEEP_LAST_HISTORY_QOS,
        HISTORY_DEPTH                   => std_logic_vector(to_unsigned(10, CDR_LONG_WIDTH)),
        RELIABILITY_QOS                 => RELIABLE_RELIABILITY_QOS,
        DURABILITY_QOS                  => TRANSIENT_LOCAL_DURABILITY_QOS,
        DEADLINE_QOS                    => DEFAULT_DEADLINE_QOS,
        LIFESPAN_QOS                    => DEFAULT_LIFESPAN_QOS,
        LIVELINESS_QOS                  => DEFAULT_LIVELINESS_QOS,
        LEASE_DURATION                  => DEFAULT_LEASE_DURATION,
        AVOID_ROS_NAMESPACE_CONVENTION  => FALSE
    );
    
    constant ROS_QOS_PROFILE_SERVICES_DEFAULT : ROS_QOS_PROFILE_TYPE := (
        HISTORY_QOS                     => KEEP_LAST_HISTORY_QOS,
        HISTORY_DEPTH                   => std_logic_vector(to_unsigned(10, CDR_LONG_WIDTH)),
        RELIABILITY_QOS                 => RELIABLE_RELIABILITY_QOS,
        DURABILITY_QOS                  => VOLATILE_DURABILITY_QOS,
        DEADLINE_QOS                    => DEFAULT_DEADLINE_QOS,
        LIFESPAN_QOS                    => DEFAULT_LIFESPAN_QOS,
        LIVELINESS_QOS                  => DEFAULT_LIVELINESS_QOS,
        LEASE_DURATION                  => DEFAULT_LEASE_DURATION,
        AVOID_ROS_NAMESPACE_CONVENTION  => FALSE
    );
    
    constant ROS_QOS_PROFILE_PARAMETER_EVENTS : ROS_QOS_PROFILE_TYPE := (
        HISTORY_QOS                     => KEEP_LAST_HISTORY_QOS,
        HISTORY_DEPTH                   => std_logic_vector(to_unsigned(1000, CDR_LONG_WIDTH)),
        RELIABILITY_QOS                 => RELIABLE_RELIABILITY_QOS,
        DURABILITY_QOS                  => VOLATILE_DURABILITY_QOS,
        DEADLINE_QOS                    => DEFAULT_DEADLINE_QOS,
        LIFESPAN_QOS                    => DEFAULT_LIFESPAN_QOS,
        LIVELINESS_QOS                  => DEFAULT_LIVELINESS_QOS,
        LEASE_DURATION                  => DEFAULT_LEASE_DURATION,
        AVOID_ROS_NAMESPACE_CONVENTION  => FALSE
    );
    
    constant ROS_QOS_PROFILE_SYSTEM_DEFAULT : ROS_QOS_PROFILE_TYPE := (
        HISTORY_QOS                     => DEFAULT_HISTORY_QOS,
        HISTORY_DEPTH                   => DEFAULT_HISTORY_DEPTH,
        RELIABILITY_QOS                 => DEFAULT_RELIABILITY_QOS_R,
        DURABILITY_QOS                  => DEFAULT_DURABILITY_QOS,
        DEADLINE_QOS                    => DEFAULT_DEADLINE_QOS,
        LIFESPAN_QOS                    => DEFAULT_LIFESPAN_QOS,
        LIVELINESS_QOS                  => DEFAULT_LIVELINESS_QOS,
        LEASE_DURATION                  => DEFAULT_LEASE_DURATION,
        AVOID_ROS_NAMESPACE_CONVENTION  => FALSE
    );
    
    constant ROS_QOS_PROFILE_ACTION_STATUS_DEFAULT : ROS_QOS_PROFILE_TYPE := (
        HISTORY_QOS                     => KEEP_LAST_HISTORY_QOS,
        HISTORY_DEPTH                   => std_logic_vector(to_unsigned(1, CDR_LONG_WIDTH)),
        RELIABILITY_QOS                 => RELIABLE_RELIABILITY_QOS,
        DURABILITY_QOS                  => TRANSIENT_LOCAL_DURABILITY_QOS,
        DEADLINE_QOS                    => DEFAULT_DEADLINE_QOS,
        LIFESPAN_QOS                    => DEFAULT_LIFESPAN_QOS,
        LIVELINESS_QOS                  => DEFAULT_LIVELINESS_QOS,
        LEASE_DURATION                  => DEFAULT_LEASE_DURATION,
        AVOID_ROS_NAMESPACE_CONVENTION  => FALSE
    );
    
    
    type ROS_NODE_TYPE is record
        --context_id      : natural;
        name            : USER_STRING_TYPE;
        namespace       : USER_STRING_TYPE;
        domain_id       : natural;
        NUM_PUBS        : natural;
        NUM_SUBS        : natural;
        NUM_SERVICES    : natural;
        NUM_ACTIONS     : natural;
    end record;
    
    type ROS_TOPIC_TYPE is record
        node_id     : natural;
        TOPICNAME   : USER_STRING_TYPE;
        TYPENAME    : USER_STRING_TYPE;
        QOS         : ROS_QOS_PROFILE_TYPE;
        MAX_SIZE    : natural;
    end record;
    
    type ROS_SERVICE_TYPE is record
        node_id     : natural;
        SERVICENAME : USER_STRING_TYPE;
        RQ_TYPENAME : USER_STRING_TYPE;
        RR_TYPENAME : USER_STRING_TYPE;
        QOS         : ROS_QOS_PROFILE_TYPE;
        MAX_RQ_SIZE : natural;
        MAX_RR_SIZE : natural;
        is_client   : boolean;
    end record;
    
    type ROS_ACTION_TYPE is record
        node_id             : natural;
        ACTIONNAME          : USER_STRING_TYPE;
        GOAL_RQ_TYPENAME    : USER_STRING_TYPE;
        GOAL_RR_TYPENAME    : USER_STRING_TYPE;
        RESULT_RQ_TYPENAME  : USER_STRING_TYPE;
        RESULT_RR_TYPENAME  : USER_STRING_TYPE;
        FEEDBACK_TYPENAME   : USER_STRING_TYPE;
        GOAL_QOS            : ROS_QOS_PROFILE_TYPE;
        RESULT_QOS          : ROS_QOS_PROFILE_TYPE;
        FEEDBACK_QOS        : ROS_QOS_PROFILE_TYPE;
        CANCEL_QOS          : ROS_QOS_PROFILE_TYPE;
        MAX_GOAL_RQ_SIZE    : natural;
        MAX_GOAL_RR_SIZE    : natural;
        MAX_RESULT_RQ_SIZE  : natural;
        MAX_RESULT_RR_SIZE  : natural;
        MAX_FEEDBACK_SIZE   : natural;
        is_client           : boolean;
        enable_feedback     : boolean;
    end record;
    
    type REQUEST_ID_TYPE is record
        writer_guid     : GUID_TYPE;
        sequence_number : SEQUENCENUMBER_TYPE;
    end record;
    
    constant EMPTY_REQUEST_ID : REQUEST_ID_TYPE := (writer_guid => GUID_UNKNOWN, sequence_number => SEQUENCENUMBER_UNKNOWN);
    
    type SERVICE_INFO_TYPE is record
        source_timestamp    : TIME_TYPE;
        received_timestamp  : TIME_TYPE;
        request_id          : REQUEST_ID_TYPE;
    end record;
    
    constant EMPTY_SERVICE_INFO : SERVICE_INFO_TYPE := (source_timestamp => TIME_INVALID, received_timestamp => TIME_INVALID, request_id => EMPTY_REQUEST_ID);
    
    function to_gid(guid : GUID_TYPE) return GID_TYPE;
    
    type MESSAGE_INFO_TYPE is record
        source_timestamp    : TIME_TYPE;
        received_timestamp  : TIME_TYPE;
        publisher_gid       : GID_TYPE;
        from_intra_process  : boolean;
    end record;
    
    constant EMPTY_MESSAGE_INFO : MESSAGE_INFO_TYPE;-- Deferred to Package Body
    
    type ROS_NODE_ARRAY_TYPE is array (natural range <>) of ROS_NODE_TYPE;
    type ROS_TOPIC_ARRAY_TYPE is array (natural range <>) of ROS_TOPIC_TYPE;
    type ROS_SERVICE_ARRAY_TYPE is array (natural range <>) of ROS_SERVICE_TYPE;
    type ROS_ACTION_ARRAY_TYPE is array (natural range <>) of ROS_ACTION_TYPE;
    
    -- *ROS RETURN CODES*
    constant ROS_RETCODE_WIDTH  : natural := 32;
    constant ROS_RET_OK                             : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(0,ROS_RETCODE_WIDTH));
    constant ROS_RET_ERROR                          : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(1,ROS_RETCODE_WIDTH));
    constant ROS_RET_TIMEOUT                        : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(2,ROS_RETCODE_WIDTH));
    constant ROS_RET_UNSUPPORTED                    : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(3,ROS_RETCODE_WIDTH));
    constant ROS_RET_BAD_ALLOC                      : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(10,ROS_RETCODE_WIDTH));
    constant ROS_RET_INVALID_ARGUMENT               : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(11,ROS_RETCODE_WIDTH));
    constant ROS_RET_INCORRECT_RMW_IMPLEMENTATION   : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(12,ROS_RETCODE_WIDTH));
    -- RCL 1xx
    constant ROS_RET_ALREADY_INIT                   : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(100,ROS_RETCODE_WIDTH));
    constant ROS_RET_NOT_INIT                       : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(101,ROS_RETCODE_WIDTH));
    constant ROS_RET_MISMATCHED_RMW                 : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(102,ROS_RETCODE_WIDTH));
    constant ROS_RET_TOPIC_NAME_INVALID             : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(103,ROS_RETCODE_WIDTH));
    constant ROS_RET_SERVICE_NAME_INVALID           : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(104,ROS_RETCODE_WIDTH));
    constant ROS_RET_UNKNOWN_SUBSTITUTION           : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(105,ROS_RETCODE_WIDTH));
    constant ROS_RET_ALREADY_SHUTDOWN               : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(106,ROS_RETCODE_WIDTH));
    -- RCL NODE 2xx
    constant ROS_RET_NODE_INVALID                   : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(200,ROS_RETCODE_WIDTH));
    constant ROS_RET_NODE_INVALID_NAME              : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(201,ROS_RETCODE_WIDTH));
    constant ROS_RET_NODE_INVALID_NAMESPACE         : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(202,ROS_RETCODE_WIDTH));
    constant ROS_RET_NODE_NAME_NON_EXISTENT         : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(203,ROS_RETCODE_WIDTH));
    -- RCL PUBLISHER 3xx
    constant ROS_RET_PUBLISHER_INVALID              : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(300,ROS_RETCODE_WIDTH));
    -- RCL SUBSCRIPTION 4xx
    constant ROS_RET_SUBSCRIPTION_INVALID           : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(400,ROS_RETCODE_WIDTH));
    constant ROS_RET_SUBSCRIPTION_TAKE_FAILED       : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(401,ROS_RETCODE_WIDTH));
    -- RCL SERVICE CLIENT 5xx
    constant ROS_RET_CLIENT_INVALID                 : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(500,ROS_RETCODE_WIDTH));
    constant ROS_RET_CLIENT_TAKE_FAILED             : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(501,ROS_RETCODE_WIDTH));
    -- RCL SERVICE SERVER 6xx
    constant ROS_RET_SERVICE_INVALID                : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(600,ROS_RETCODE_WIDTH));
    constant ROS_RET_SERVICE_TAKE_FAILED            : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(601,ROS_RETCODE_WIDTH));
    -- RCL GUARD 7xx
    -- RCL TIMER 8xx
    constant ROS_RET_TIMER_INVALID                  : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(800,ROS_RETCODE_WIDTH));
    constant ROS_RET_TIMER_CANCELED                 : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(801,ROS_RETCODE_WIDTH));
    -- RCL WAIT 9xx
    constant ROS_RET_WAIT_SET_INVALID               : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(900,ROS_RETCODE_WIDTH));
    constant ROS_RET_WAIT_SET_EMPTY                 : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(901,ROS_RETCODE_WIDTH));
    constant ROS_RET_WAIT_SET_FULL                  : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(902,ROS_RETCODE_WIDTH));
    -- RCL ARGUMENT PARSING 1xxx
    constant ROS_RET_INVALID_REMAP_RULE             : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(1001,ROS_RETCODE_WIDTH));
    constant ROS_RET_WRONG_LEXEME                   : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(1002,ROS_RETCODE_WIDTH));
    constant ROS_RET_INVALID_ROS_ARGS               : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(1003,ROS_RETCODE_WIDTH));
    constant ROS_RET_INVALID_PARAM_RULE             : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(1010,ROS_RETCODE_WIDTH));
    constant ROS_RET_INVALID_LOG_LEVEL_RULE         : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(1020,ROS_RETCODE_WIDTH));
    -- RCL EVENT 20xx
    constant ROS_RET_EVENT_INVALID                  : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(2000,ROS_RETCODE_WIDTH));
    constant ROS_RET_EVENT_TAKE_FAILED              : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(2001,ROS_RETCODE_WIDTH));
    -- RCL ACTION 2xxx
    constant ROS_RET_ACTION_NAME_INVALID            : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(2000,ROS_RETCODE_WIDTH));
    constant ROS_RET_ACTION_GOAL_ACCEPTED           : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(2100,ROS_RETCODE_WIDTH));
    constant ROS_RET_ACTION_GOAL_REJECTED           : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(2101,ROS_RETCODE_WIDTH));
    constant ROS_RET_ACTION_CLIENT_INVALID          : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(2102,ROS_RETCODE_WIDTH));
    constant ROS_RET_ACTION_CLIENT_TAKE_FAILED      : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(2103,ROS_RETCODE_WIDTH));
    constant ROS_RET_ACTION_SERVER_INVALID          : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(2200,ROS_RETCODE_WIDTH));
    constant ROS_RET_ACTION_SERVER_TAKE_FAILED      : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(2201,ROS_RETCODE_WIDTH));
    constant ROS_RET_ACTION_GOAL_HANDLE_INVALID     : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(2300,ROS_RETCODE_WIDTH));
    constant ROS_RET_ACTION_GOAL_EVENT_INVALID      : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(2301,ROS_RETCODE_WIDTH));
    -- RCL LIFECYCLE STATE REGISTER 30xx
    constant ROS_RET_LIFECYCLE_STATE_REGISTERED     : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(3000,ROS_RETCODE_WIDTH));
    constant ROS_RET_LIFECYCLE_STATE_NOT_REGISTERED : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(3001,ROS_RETCODE_WIDTH));
    
    type ROS_TOPIC_OPCODE_TYPE is (NOP, PUBLISH, TAKE);
    type ROS_SERVICE_OPCODE_TYPE is (NOP, SEND_REQUEST, TAKE_REQUEST, SEND_RESPONSE, TAKE_RESPONSE);
    type ROS_ACTION_OPCODE_TYPE is (NOP, GET_GOAL, GET_LAST_GOAL, GET_FIRST_GOAL, GET_NEXT_GOAL, GET_PREVIOUS_GOAL, UPDATE_GOAL_STATE, PUBLISH_FEEDBACK, SEND_GOAL_REQUEST, TAKE_GOAL_RESPONSE, SEND_RESULT_REQUEST, TAKE_RESULT_RESPONSE, SEND_CANCEL_REQUEST, TAKE_CANCEL_RESPONSE, TAKE_FEEDBACK);
    
    constant ROS_SEQUENCE_ID_WIDTH : natural := 64;
    
    type ENDPOINT_ROS_NODE_MAPPING_ARRAY_TYPE is array (natural range <>) of natural;
    
    constant SERVICE_OVERHEAD_BYTES : natural := 16;
    
    constant GOAL_HANDLE_WIDTH      : natural := WORD_WIDTH;
    constant GOAL_HANDLE_UNKNOWN    : std_logic_vector(GOAL_HANDLE_WIDTH-1 downto 0) := (others => '1');
    
    function get_num_pubs(nodes : ROS_NODE_ARRAY_TYPE) return natural;
    function get_num_subs(nodes : ROS_NODE_ARRAY_TYPE) return natural;
    function get_num_services(nodes : ROS_NODE_ARRAY_TYPE) return natural;
    function get_num_actions(nodes : ROS_NODE_ARRAY_TYPE) return natural;
    function gen_num_readers(subs : ROS_TOPIC_ARRAY_TYPE; services : ROS_SERVICE_ARRAY_TYPE; actions : ROS_ACTION_ARRAY_TYPE) return natural;
    function gen_num_writers(pubs : ROS_TOPIC_ARRAY_TYPE; services : ROS_SERVICE_ARRAY_TYPE; actions : ROS_ACTION_ARRAY_TYPE) return natural;
    function get_domain_id(nodes : ROS_NODE_ARRAY_TYPE) return natural;
    function gen_fqn(ns : string; node : string; name : string) return USER_STRING_TYPE;
    procedure check_node_mapping(nodes : in ROS_NODE_ARRAY_TYPE; pubs : in ROS_TOPIC_ARRAY_TYPE; subs : in ROS_TOPIC_ARRAY_TYPE; services : in ROS_SERVICE_ARRAY_TYPE; actions : in ROS_ACTION_ARRAY_TYPE);
    procedure set_from_qos_profile(profile : in ROS_QOS_PROFILE_TYPE; config : inout CONFIG_TYPE);
    
    type SERVICE_INTERFACE_TYPE is record
        start_r                             : std_logic;
        ack_r                               : std_logic;
        opcode_r                            : DDS_READER_OPCODE_TYPE;
        instance_state_r                    : std_logic_vector(INSTANCE_STATE_KIND_WIDTH-1 downto 0);
        view_state_r                        : std_logic_vector(VIEW_STATE_KIND_WIDTH-1 downto 0);
        sample_state_r                      : std_logic_vector(SAMPLE_STATE_KIND_WIDTH-1 downto 0);
        instance_handle_r                   : INSTANCE_HANDLE_TYPE;
        max_samples_r                       : std_logic_vector(MAX_SAMPLES_WIDTH-1 downto 0);
        get_data_r                          : std_logic;
        done_r                              : std_logic;
        return_code_r                       : std_logic_vector(RETURN_CODE_WIDTH-1 downto 0);
        valid_in_r                          : std_logic;
        ready_in_r                          : std_logic;
        data_in_r                           : std_logic_vector(WORD_WIDTH-1 downto 0);
        last_word_in_r                      : std_logic;
        sample_info_r                       : SAMPLE_INFO_TYPE;
        sample_info_valid_r                 : std_logic;
        sample_info_ack_r                   : std_logic;
        eoc_r                               : std_logic;
        status_r                            : std_logic_vector(STATUS_KIND_WIDTH-1 downto 0);
        start_w                             : std_logic;
        ack_w                               : std_logic;
        opcode_w                            : DDS_WRITER_OPCODE_TYPE;
        instance_handle_out_w               : INSTANCE_HANDLE_TYPE;
        source_ts_w                         : TIME_TYPE;
        max_wait_w                          : DURATION_TYPE;
        done_w                              : std_logic;
        return_code_w                       : std_logic_vector(RETURN_CODE_WIDTH-1 downto 0);
        instance_handle_in_w                : INSTANCE_HANDLE_TYPE;
        valid_out_w                         : std_logic;
        ready_out_w                         : std_logic;
        data_out_w                          : std_logic_vector(WORD_WIDTH-1 downto 0);
        last_word_out_w                     : std_logic;
        valid_in_w                          : std_logic;
        ready_in_w                          : std_logic;
        data_in_w                           : std_logic_vector(WORD_WIDTH-1 downto 0);
        last_word_in_w                      : std_logic;
        status_w                            : std_logic_vector(STATUS_KIND_WIDTH-1 downto 0);
    end record;
    
    type SERVICE_INTERFACE_ARRAY_TYPE is array (natural range <>) of SERVICE_INTERFACE_TYPE;
    
    type ACTION_INTERFACE_TYPE is record
        g_start_r                   : std_logic;
        g_ack_r                     : std_logic;
        g_opcode_r                  : DDS_READER_OPCODE_TYPE;
        g_instance_state_r          : std_logic_vector(INSTANCE_STATE_KIND_WIDTH-1 downto 0);
        g_view_state_r              : std_logic_vector(VIEW_STATE_KIND_WIDTH-1 downto 0);
        g_sample_state_r            : std_logic_vector(SAMPLE_STATE_KIND_WIDTH-1 downto 0);
        g_instance_handle_r         : INSTANCE_HANDLE_TYPE;
        g_max_samples_r             : std_logic_vector(MAX_SAMPLES_WIDTH-1 downto 0);
        g_get_data_r                : std_logic;
        g_done_r                    : std_logic;
        g_return_code_r             : std_logic_vector(RETURN_CODE_WIDTH-1 downto 0);
        g_valid_in_r                : std_logic;
        g_ready_in_r                : std_logic;
        g_data_in_r                 : std_logic_vector(WORD_WIDTH-1 downto 0);
        g_last_word_in_r            : std_logic;
        g_sample_info_r             : SAMPLE_INFO_TYPE;
        g_sample_info_valid_r       : std_logic;
        g_sample_info_ack_r         : std_logic;
        g_eoc_r                     : std_logic;
        g_status_r                  : std_logic_vector(STATUS_KIND_WIDTH-1 downto 0);
        g_start_w                   : std_logic;
        g_ack_w                     : std_logic;
        g_opcode_w                  : DDS_WRITER_OPCODE_TYPE;
        g_instance_handle_out_w     : INSTANCE_HANDLE_TYPE;
        g_source_ts_w               : TIME_TYPE;
        g_max_wait_w                : DURATION_TYPE;
        g_done_w                    : std_logic;
        g_return_code_w             : std_logic_vector(RETURN_CODE_WIDTH-1 downto 0);
        g_instance_handle_in_w      : INSTANCE_HANDLE_TYPE;
        g_valid_out_w               : std_logic;
        g_ready_out_w               : std_logic;
        g_data_out_w                : std_logic_vector(WORD_WIDTH-1 downto 0);
        g_last_word_out_w           : std_logic;
        g_valid_in_w                : std_logic;
        g_ready_in_w                : std_logic;
        g_data_in_w                 : std_logic_vector(WORD_WIDTH-1 downto 0);
        g_last_word_in_w            : std_logic;
        g_status_w                  : std_logic_vector(STATUS_KIND_WIDTH-1 downto 0);
        r_start_r                   : std_logic;
        r_ack_r                     : std_logic;
        r_opcode_r                  : DDS_READER_OPCODE_TYPE;
        r_instance_state_r          : std_logic_vector(INSTANCE_STATE_KIND_WIDTH-1 downto 0);
        r_view_state_r              : std_logic_vector(VIEW_STATE_KIND_WIDTH-1 downto 0);
        r_sample_state_r            : std_logic_vector(SAMPLE_STATE_KIND_WIDTH-1 downto 0);
        r_instance_handle_r         : INSTANCE_HANDLE_TYPE;
        r_max_samples_r             : std_logic_vector(MAX_SAMPLES_WIDTH-1 downto 0);
        r_get_data_r                : std_logic;
        r_return_code_r             : std_logic_vector(RETURN_CODE_WIDTH-1 downto 0);
        r_done_r                    : std_logic;
        r_valid_in_r                : std_logic;
        r_ready_in_r                : std_logic;
        r_data_in_r                 : std_logic_vector(WORD_WIDTH-1 downto 0);
        r_last_word_in_r            : std_logic;
        r_sample_info_r             : SAMPLE_INFO_TYPE;
        r_sample_info_valid_r       : std_logic;
        r_sample_info_ack_r         : std_logic;
        r_eoc_r                     : std_logic;
        r_status_r                  : std_logic_vector(STATUS_KIND_WIDTH-1 downto 0);
        r_start_w                   : std_logic;
        r_ack_w                     : std_logic;
        r_opcode_w                  : DDS_WRITER_OPCODE_TYPE;
        r_instance_handle_out_w     : INSTANCE_HANDLE_TYPE;
        r_source_ts_w               : TIME_TYPE;
        r_max_wait_w                : DURATION_TYPE;
        r_done_w                    : std_logic;
        r_return_code_w             : std_logic_vector(RETURN_CODE_WIDTH-1 downto 0);
        r_instance_handle_in_w      : INSTANCE_HANDLE_TYPE;
        r_valid_out_w               : std_logic;
        r_ready_out_w               : std_logic;
        r_data_out_w                : std_logic_vector(WORD_WIDTH-1 downto 0);
        r_last_word_out_w           : std_logic;
        r_valid_in_w                : std_logic;
        r_ready_in_w                : std_logic;
        r_data_in_w                 : std_logic_vector(WORD_WIDTH-1 downto 0);
        r_last_word_in_w            : std_logic;
        r_status_w                  : std_logic_vector(STATUS_KIND_WIDTH-1 downto 0);
        c_start_r                   : std_logic;
        c_ack_r                     : std_logic;
        c_opcode_r                  : DDS_READER_OPCODE_TYPE;
        c_instance_state_r          : std_logic_vector(INSTANCE_STATE_KIND_WIDTH-1 downto 0);
        c_view_state_r              : std_logic_vector(VIEW_STATE_KIND_WIDTH-1 downto 0);
        c_sample_state_r            : std_logic_vector(SAMPLE_STATE_KIND_WIDTH-1 downto 0);
        c_instance_handle_r         : INSTANCE_HANDLE_TYPE;
        c_max_samples_r             : std_logic_vector(MAX_SAMPLES_WIDTH-1 downto 0);
        c_get_data_r                : std_logic;
        c_done_r                    : std_logic;
        c_return_code_r             : std_logic_vector(RETURN_CODE_WIDTH-1 downto 0);
        c_valid_in_r                : std_logic;
        c_ready_in_r                : std_logic;
        c_data_in_r                 : std_logic_vector(WORD_WIDTH-1 downto 0);
        c_last_word_in_r            : std_logic;
        c_sample_info_r             : SAMPLE_INFO_TYPE;
        c_sample_info_valid_r       : std_logic;
        c_sample_info_ack_r         : std_logic;
        c_eoc_r                     : std_logic;
        c_status_r                  : std_logic_vector(STATUS_KIND_WIDTH-1 downto 0);
        c_start_w                   : std_logic;
        c_ack_w                     : std_logic;
        c_opcode_w                  : DDS_WRITER_OPCODE_TYPE;
        c_instance_handle_out_w     : INSTANCE_HANDLE_TYPE;
        c_source_ts_w               : TIME_TYPE;
        c_max_wait_w                : DURATION_TYPE;
        c_done_w                    : std_logic;
        c_return_code_w             : std_logic_vector(RETURN_CODE_WIDTH-1 downto 0);
        c_instance_handle_in_w      : INSTANCE_HANDLE_TYPE;
        c_valid_out_w               : std_logic;
        c_ready_out_w               : std_logic;
        c_data_out_w                : std_logic_vector(WORD_WIDTH-1 downto 0);
        c_last_word_out_w           : std_logic;
        c_valid_in_w                : std_logic;
        c_ready_in_w                : std_logic;
        c_data_in_w                 : std_logic_vector(WORD_WIDTH-1 downto 0);
        c_last_word_in_w            : std_logic;
        c_status_w                  : std_logic_vector(STATUS_KIND_WIDTH-1 downto 0);
    end record;
    
    type ACTION_INTERFACE_ARRAY_TYPE is array (natural range <>) of ACTION_INTERFACE_TYPE;
    
    function ">" (L,R: ROS_TIME_TYPE) return boolean;
    function "<" (L,R: ROS_TIME_TYPE) return boolean;
    function ">=" (L,R: ROS_TIME_TYPE) return boolean;
    function "<=" (L,R: ROS_TIME_TYPE) return boolean;
    function "=" (L,R: ROS_TIME_TYPE) return boolean;
    function "/=" (L,R: ROS_TIME_TYPE) return boolean;
    
end package;

package body ros_package is
    
    function gen_duration(s,ns : integer) return ROS_DURATION_TYPE is
        variable ret : ROS_DURATION_TYPE := ROS_TIME_ZERO;
        constant sec  : natural := 10**9;
    begin
        assert (ns < sec) report "ns argument has to be less than a second" severity failure;
        
        ret.sec     := std_logic_vector(to_unsigned(s,ret.sec'length));
        ret.nanosec := std_logic_vector(to_unsigned(ns,ret.nanosec'length));
        
        return ret;
    end function;
    
    function gen_duration(t : time) return ROS_DURATION_TYPE is
        variable seconds : natural;
        variable nanoseconds : natural;
    begin
        -- Extract Seconds
        seconds := t / sec;
        -- Extract Nanoseconds
        nanoseconds := (t - (seconds * sec)) / ns;
        
        return gen_duration(seconds, nanoseconds);
    end function;
    
    function to_gid(guid : GUID_TYPE) return GID_TYPE is
        variable ret : GID_TYPE;
    begin
        ret := (others => (others => '0'));
        for i in 0 to guid'length-1 loop
            ret(i)  := guid(i);
        end loop;
        
        return ret;
    end function;
    
    function to_unsigned(input : UUID_TYPE) return unsigned is
        variable ret : std_logic_vector(UUID_WIDTH-1 downto 0);
    begin
        ret := (others => '0');
        for i in 0 to input'length-1 loop
            ret := write_sub_vector(ret, input(i), i, TRUE);
        end loop;
        return unsigned(ret);
    end function;
    
    function to_UUID(input : std_logic_vector) return UUID_TYPE is
        variable ret : UUID_TYPE;
    begin
        assert (input'length = UUID_WIDTH) report "SLV Length missmatch" severity FAILURE;
        ret := UUID_UNKNOWN;
        for i in 0 to ret'length-1 loop
            ret(i) := get_sub_vector(input, i, ret(i)'length, TRUE);
        end loop;
        return ret;
    end function;
    
    function to_unsigned(input : ROS_TIME_TYPE) return unsigned is
        variable ret : std_logic_vector(ROS_TIME_WIDTH-1 downto 0);
    begin
        ret := (others => '0');
        ret := write_sub_vector(ret, input.sec, 0, TRUE);
        ret := write_sub_vector(ret, input.nanosec, 1, TRUE);
        return unsigned(ret);
    end function;
    
    function to_ROS_TIME(input : std_logic_vector) return ROS_TIME_TYPE is
        variable ret : ROS_TIME_TYPE;
    begin
        assert (input'length = ROS_TIME_WIDTH) report "SLV Length missmatch" severity FAILURE;
        ret.sec     := get_sub_vector(input, 0, ret.sec'length, TRUE);
        ret.nanosec := get_sub_vector(input, 1, ret.nanosec'length, TRUE);
        return ret;
    end function;
    
    constant EMPTY_MESSAGE_INFO : MESSAGE_INFO_TYPE := (source_timestamp => TIME_INVALID, received_timestamp => TIME_INVALID, publisher_gid => to_gid(GUID_UNKNOWN), from_intra_process => FALSE);
    
    function get_num_pubs(nodes : ROS_NODE_ARRAY_TYPE) return natural is
        variable ret : natural;
    begin
        ret := 0;
        for i in 0 to nodes'length-1 loop
            ret := ret + nodes(i).NUM_PUBS;
        end loop;
        return ret;
    end function;
    
    function get_num_subs(nodes : ROS_NODE_ARRAY_TYPE) return natural is
        variable ret : natural;
    begin
        ret := 0;
        for i in 0 to nodes'length-1 loop
            ret := ret + nodes(i).NUM_SUBS;
        end loop;
        return ret;
    end function;
    
    function get_num_services(nodes : ROS_NODE_ARRAY_TYPE) return natural is
        variable ret : natural;
    begin
        ret := 0;
        for i in 0 to nodes'length-1 loop
            ret := ret + nodes(i).NUM_SERVICES;
        end loop;
        return ret;
    end function;
    
    function get_num_actions(nodes : ROS_NODE_ARRAY_TYPE) return natural is
        variable ret : natural;
    begin
        ret := 0;
        for i in 0 to nodes'length-1 loop
            ret := ret + nodes(i).NUM_ACTIONS;
        end loop;
        return ret;
    end function;
    
    function gen_num_readers(subs : ROS_TOPIC_ARRAY_TYPE; services : ROS_SERVICE_ARRAY_TYPE; actions : ROS_ACTION_ARRAY_TYPE) return natural is
        variable ret : natural := 0;
    begin
        ret := subs'length + services'length + (actions'length * 3);
        for i in 0 to actions'length-1 loop
            -- FEEDBACK SUBSCRIBER
            if (actions(i).is_client and actions(i).enable_feedback) then
                ret := ret + 1;
            end if;
        end loop;
        return ret;
    end function;
    
    function gen_num_writers(pubs : ROS_TOPIC_ARRAY_TYPE; services : ROS_SERVICE_ARRAY_TYPE; actions : ROS_ACTION_ARRAY_TYPE) return natural is
        variable ret : natural := 0;
    begin
        ret := pubs'length + services'length + (actions'length * 3);
        for i in 0 to actions'length-1 loop
            -- FEEDBACK PUBLISHER
            if ((not actions(i).is_client) and actions(i).enable_feedback) then
                ret := ret + 1;
            end if;
            -- STATUS PUBLISHER
            if (not actions(i).is_client) then
                ret := ret + 1;
            end if;
        end loop;
        -- ROS DISCOVERY PUBLISHER
        ret := ret + 1;
        return ret;
    end function;
    
    function is_numeric_char (char : character) return boolean is
        variable ret : boolean;
    begin
        if (character'POS(char) >= character'POS('0') and character'POS(char) <= character'POS('9')) then
            return TRUE;
        else
            return FALSE;
        end if;
    end function;
    
    function is_valid_char (char : character) return boolean is
        variable ret : boolean;
    begin
        -- Whitelist
        -- Name may contain alphanumeric characters ([0-9|a-z|A-Z]), underscores (_), or forward slashes (/)
        if is_numeric_char(char) then
            return TRUE;
        elsif (character'POS(char) >= character'POS('A') and character'POS(char) <= character'POS('Z')) then
            return TRUE;
        elsif (character'POS(char) >= character'POS('a') and character'POS(char) <= character'POS('z')) then
            return TRUE;
        elsif (char = '_' or char = '/') then
            return TRUE;
        end if;
        
        return FALSE;
    end function;
    
    procedure is_valid_name (name : in string) is
        variable start : natural;
        variable prev : character;
    begin
        assert (string_len(name) > 0) report "Name must not be empty" severity FAILURE;
        assert (not is_numeric_char(name(1))) report "Name must not start with a numeric character" severity FAILURE;
        assert (name(string_len(name)) /= '/') report "Name must not end with a forward slash (/)" severity FAILURE;
        
        start := 1;
        
        if (name(1) = '~') then
            if (string_len(name) > 1) then
                assert (name(2) = '/') report "Name must separate a tilde (~) from the rest of the name with a forward slash (/), i.e. ~/foo not ~foo" severity FAILURE;
            end if;
            start := 2; -- Skip valid char checking for '~'
        end if;
        
        prev := ' '; 
        for i in start to string_len(name) loop
            assert (name(i) /= '{') report "Substitution is not supported" severity FAILURE;
            assert (not (name(i) = '/' and name(i) = prev)) report "Name must not contain any number of repeated forward slashes (/)" severity FAILURE;
            assert (not (name(i) = '_' and name(i) = prev)) report "Name must not contain any number of repeated underscores (_)" severity FAILURE;
            assert (is_valid_char(name(i))) report "Name may contain alphanumeric characters ([0-9|a-z|A-Z]), underscores (_), or forward slashes (/)" severity FAILURE;
            prev := name(i);
        end loop;
    end procedure;
    
    
    function get_domain_id(nodes : ROS_NODE_ARRAY_TYPE) return natural is
        variable ret : natural;
    begin
        ret := nodes(0).domain_id;
        for i in 1 to nodes'length-1 loop
            assert (nodes(i).domain_id = ret) report "No support for multiple domain IDs in same ROS context" severity FAILURE;
        end loop;
        return ret;
    end function;
    
    procedure check_node_mapping (nodes : in ROS_NODE_ARRAY_TYPE; pubs : in ROS_TOPIC_ARRAY_TYPE; subs : in ROS_TOPIC_ARRAY_TYPE; services : in ROS_SERVICE_ARRAY_TYPE; actions : in ROS_ACTION_ARRAY_TYPE) is
        variable tmp : natural;
    begin
        for i in 0 to nodes'length-1 loop
            -- Check PUB Mapping
            if (nodes(i).NUM_PUBS > 0) then
                tmp := 0;
                for j in 0 to pubs'length-1 loop
                    if (pubs(j).node_id = i) then
                        tmp := tmp + 1;
                    end if;
                end loop;
                assert (tmp = nodes(i).NUM_PUBS) report "Missing publication definition for Node " & integer'image(i) severity FAILURE;
            end if;
            
            -- Check SUB Mapping
            if (nodes(i).NUM_SUBS > 0) then
                tmp := 0;
                for j in 0 to subs'length-1 loop
                    if (subs(j).node_id = i) then
                        tmp := tmp + 1;
                    end if;
                end loop;
                assert (tmp = nodes(i).NUM_SUBS) report "Missing subscription definition for Node " & integer'image(i) severity FAILURE;
            end if;
            
            -- Check SERVICE Mapping
            if (nodes(i).NUM_SERVICES > 0) then
                tmp := 0;
                for j in 0 to services'length-1 loop
                    if (services(j).node_id = i) then
                        tmp := tmp + 1;
                    end if;
                end loop;
                assert (tmp = nodes(i).NUM_SERVICES) report "Missing service definition for Node " & integer'image(i) severity FAILURE;
            end if;
            
            -- Check ACTION Mapping
            if (nodes(i).NUM_ACTIONS > 0) then
                tmp := 0;
                for j in 0 to actions'length-1 loop
                    if (actions(j).node_id = i) then
                        tmp := tmp + 1;
                    end if;
                end loop;
                assert (tmp = nodes(i).NUM_ACTIONS) report "Missing action definition for Node " & integer'image(i) severity FAILURE;
            end if;
        end loop;
    end procedure;
    
    procedure set_from_qos_profile(profile : in ROS_QOS_PROFILE_TYPE; config : inout CONFIG_TYPE) is
    begin
        config.RELIABILITY_QOS          := profile.RELIABILITY_QOS;
        config.DURABILITY_QOS           := profile.DURABILITY_QOS;
        config.HISTORY_QOS              := profile.HISTORY_QOS;
        config.HISTORY_DEPTH            := profile.HISTORY_DEPTH;
        config.DEADLINE_QOS             := profile.DEADLINE_QOS;
        config.LIFESPAN_QOS             := profile.LIFESPAN_QOS;
        config.LIVELINESS_QOS           := profile.LIVELINESS_QOS;
        config.LEASE_DURATION           := profile.LEASE_DURATION;
        -- Since ROS does not use Keyed Topics, we can effectively limit the Resources to the History Depth
        config.MAX_SAMPLES              := profile.HISTORY_DEPTH;
        config.MAX_INSTANCES            := profile.HISTORY_DEPTH;
        config.MAX_SAMPLES_PER_INSTANCE := profile.HISTORY_DEPTH;
        config.PUSH_MODE                := FALSE; -- CycloneDDS compatibility
    end procedure;
    
    function gen_fqn(ns : string; node : string; name : string) return USER_STRING_TYPE is
        variable ret : USER_STRING_TYPE;
    begin
        ret := EMPTY_USER_STRING;
        
        is_valid_name(name);
        
        -- Private Namespace
        if (name(1) = '~') then
            
            -- NAMESPACE
            if (string_len(ns) > 0) then
                assert(ns(1) = '/') report "Namespace has to be absolute" severity FAILURE;
                is_valid_name(ns);
                ret := concat(ns,"/");
            else
                ret := gen_user_string("/");
            end if;
            -- NODENAME
            assert(node(1) /= '/') report "Nodename cannot start with '/'" severity FAILURE;
            is_valid_name(node);
            ret := concat(ret,node);
            -- NAME
            if (string_len(name) > 1) then
                -- Remove First Character ('~')
                ret := concat(ret,substr(2,string_len(name),name));
            end if;
        -- Absolute Name
        elsif (name(1) = '/') then
            ret := name;
        -- Relative Name
        else
            -- NAMESPACE
            if (string_len(ns) > 0) then
                assert(ns(1) = '/') report "Namespace has to be absolute" severity FAILURE;
                is_valid_name(ns);
                ret := concat(ns,"/");
            else
                ret := gen_user_string("/");
            end if;
            -- NAME
            ret := concat(ret,name);
        end if;
        
        return ret;
    end function;
    
    function ">" (L,R: ROS_TIME_TYPE) return boolean is
    begin
        return to_unsigned(L) > to_unsigned(R);
    end function;
    
    function "<" (L,R: ROS_TIME_TYPE) return boolean is
    begin
        return to_unsigned(L) < to_unsigned(R);
    end function;
    
    function ">=" (L,R: ROS_TIME_TYPE) return boolean is
    begin
        return to_unsigned(L) >= to_unsigned(R);
    end function;
    
    function "<=" (L,R: ROS_TIME_TYPE) return boolean is
    begin
        return to_unsigned(L) <= to_unsigned(R);
    end function;
    
    function "=" (L,R: ROS_TIME_TYPE) return boolean is
    begin
        return to_unsigned(L) = to_unsigned(R);
    end function;
    
    function "/=" (L,R: ROS_TIME_TYPE) return boolean is
    begin
        return to_unsigned(L) /= to_unsigned(R);
    end function;
    
end package body;
