614 lines
33 KiB
VHDL
614 lines
33 KiB
VHDL
-- 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;
|
|
|
|
subtype ROS_DURATION_TYPE is ROS_TIME_TYPE;
|
|
|
|
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;
|
|
|
|
|
|
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
|
|
);
|
|
|
|
|
|
|
|
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;
|
|
-- TODO
|
|
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, SEND_GOAL_REQUEST, TAKE_GOAL_REQUEST, SEND_GOAL_RESPONSE, TAKE_GOAL_RESPONSE, SEND_RESULT_REQUEST, TAKE_RESULT_REQUEST, SEND_RESULT_RESPONSE, TAKE_RESULT_RESPONSE, SEND_CANCEL_REQUEST, TAKE_CANCEL_REQUEST, SEND_CANCEL_RESPONSE, TAKE_CANCEL_RESPONSE, PUBLISH_FEEDBACK, ACCEPT_GOAL, UPDATE_GOAL, EXPIRE_GOAL);
|
|
|
|
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;
|
|
|
|
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 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;
|
|
si_sample_state_r : std_logic_vector(SAMPLE_STATE_KIND_WIDTH-1 downto 0);
|
|
si_view_state_r : std_logic_vector(VIEW_STATE_KIND_WIDTH-1 downto 0);
|
|
si_instance_state_r : std_logic_vector(INSTANCE_STATE_KIND_WIDTH-1 downto 0);
|
|
si_source_timestamp_r : TIME_TYPE;
|
|
si_instance_handle_r : INSTANCE_HANDLE_TYPE;
|
|
si_publication_handle_r : INSTANCE_HANDLE_TYPE;
|
|
si_disposed_generation_count_r : std_logic_vector(DISPOSED_GENERATION_COUNT_WIDTH-1 downto 0);
|
|
si_no_writers_generation_count_r : std_logic_vector(NO_WRITERS_GENERATION_COUNT_WIDTH-1 downto 0);
|
|
si_sample_rank_r : std_logic_vector(SAMPLE_RANK_WIDTH-1 downto 0);
|
|
si_generation_rank_r : std_logic_vector(GENERATION_RANK_WIDTH-1 downto 0);
|
|
si_absolute_generation_rank_r : std_logic_vector(ABSOLUTE_GENERATION_COUNT_WIDTH-1 downto 0);
|
|
si_valid_data_r : std_logic;
|
|
si_valid_r : std_logic;
|
|
si_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;
|
|
|
|
end package;
|
|
|
|
package body ros_package is
|
|
|
|
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 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;
|
|
|
|
end package body;
|