diff --git a/src/ros2/Tests/Level_1/L1_Fibonacci_action_feedback_test1.vhd b/src/ros2/Tests/Level_1/L1_Fibonacci_action_feedback_test1.vhd index 20cd112..34ba5e5 100644 --- a/src/ros2/Tests/Level_1/L1_Fibonacci_action_feedback_test1.vhd +++ b/src/ros2/Tests/Level_1/L1_Fibonacci_action_feedback_test1.vhd @@ -35,9 +35,7 @@ architecture testbench of L1_Fibonacci_action_feedback_test1 is signal ready_w, selector : std_logic := '0'; signal message_info_sub : MESSAGE_INFO_TYPE := EMPTY_MESSAGE_INFO; -- - signal goal_id_addr_sub, goal_id_addr_pub : std_logic_vector(GOAL_ID_ADDR_WIDTH-1 downto 0) := (others => '0'); - signal goal_id_ready_sub, goal_id_ready_pub, goal_id_ren_sub, goal_id_ren_pub, goal_id_wen_pub, goal_id_valid_sub, goal_id_valid_pub, goal_id_ack_sub, goal_id_ack_pub : std_logic := '0'; - signal goal_id_sub, goal_id_r_pub, goal_id_w_pub : std_logic_vector(CDR_INT8_WIDTH-1 downto 0) := (others => '0'); + signal goal_id_sub, goal_id_pub : std_logic_vector(UUID_WIDTH-1 downto 0) := (others => '0'); signal seq_len_sub, seq_len_pub, seq_addr_sub, seq_addr_pub : std_logic_vector(SEQ_ADDR_WIDTH-1 downto 0) := (others => '0'); signal seq_ready_sub, seq_ready_pub, seq_ren_sub, seq_ren_pub, seq_wen_pub, seq_valid_sub, seq_valid_pub, seq_ack_sub, seq_ack_pub : std_logic := '0'; signal seq_sub, seq_r_pub, seq_w_pub : std_logic_vector(CDR_LONG_WIDTH-1 downto 0) := (others => '0'); @@ -84,11 +82,6 @@ begin done_user => done_sub, return_code_user => return_code_sub, data_available_user => open, - goal_id_addr => goal_id_addr_sub, - goal_id_ready => goal_id_ready_sub, - goal_id_ren => goal_id_ren_sub, - goal_id_valid => goal_id_valid_sub, - goal_id_ack => goal_id_ack_sub, goal_id => goal_id_sub, seq_len => seq_len_sub, seq_addr => seq_addr_sub, @@ -126,14 +119,7 @@ begin start_user => start_pub,-- ack_user => ack_pub,-- opcode_user => opcode_pub,-- - goal_id_addr => goal_id_addr_pub, - goal_id_ready => goal_id_ready_pub, - goal_id_ren => goal_id_ren_pub, - goal_id_wen => goal_id_wen_pub, - goal_id_valid => goal_id_valid_pub, - goal_id_ack => goal_id_ack_pub, - goal_id_r => goal_id_r_pub, - goal_id_w => goal_id_w_pub, + goal_id => goal_id_pub, seq_len => seq_len_pub, seq_addr => seq_addr_pub, seq_ready => seq_ready_pub, @@ -170,7 +156,7 @@ begin end procedure; begin - SetAlertLogName("AddTwoInts Service - Level 1 - (Big Endian) - General"); + SetAlertLogName("Fibonacci Action Feedback - Level 1 - (Big Endian) - General"); SetAlertEnable(FAILURE, TRUE); SetAlertEnable(ERROR, TRUE); SetAlertEnable(WARNING, TRUE); @@ -257,14 +243,7 @@ begin selector <= '0'; Log("Setting Setting Data in Publisher Side", INFO); - for i in 0 to GOAL_ID_MAX_DEPTH-1 loop - goal_id_addr_pub <= int(i,goal_id_addr_pub'length); - goal_id_w_pub <= RV.RandSlv(goal_id_w_pub'length); - wait_on_sig(goal_id_ready_pub); - goal_id_wen_pub <= '1'; - wait for TEST_CLOCK_PERIOD; - goal_id_wen_pub <= '0'; - end loop; + goal_id_pub <= RV.RandSlv(goal_id_pub'length); for i in 0 to 9 loop seq_len_pub <= int(10,seq_len_pub'length); seq_addr_pub <= int(i,seq_addr_pub'length); @@ -304,27 +283,9 @@ begin AlertIf(taken_sub /= '1', "Subscriber did not take Message", FAILURE); Log("Compare Messages", INFO); - for i in 0 to GOAL_ID_MAX_DEPTH-1 loop - goal_id_addr_pub <= int(i,goal_id_addr_pub'length); - goal_id_addr_sub <= int(i,goal_id_addr_sub'length); - wait_on_sig(goal_id_ready_pub); - wait_on_sig(goal_id_ready_sub); - goal_id_ren_pub <= '1'; - goal_id_ren_sub <= '1'; - wait for TEST_CLOCK_PERIOD; - goal_id_ren_pub <= '0'; - goal_id_ren_sub <= '0'; - wait_on_sig(goal_id_valid_pub); - wait_on_sig(goal_id_valid_sub); - AffirmIfEqual(SEQ, goal_id_sub, goal_id_r_pub); - goal_id_ack_pub <= '1'; - goal_id_ack_sub <= '1'; - wait for TEST_CLOCK_PERIOD; - goal_id_ack_pub <= '0'; - goal_id_ack_sub <= '0'; - end loop; - AffirmIfEqual(seq_len_sub, seq_len_pub); - for i in 0 to to_integer(unsigned(seq_len_pub)) loop + AffirmIfEqual(GOAL_ID, goal_id_sub, goal_id_pub); + AffirmIfEqual(SEQ, seq_len_sub, seq_len_pub); + for i in 0 to to_integer(unsigned(seq_len_pub))-1 loop seq_addr_pub <= int(i,seq_addr_pub'length); seq_addr_sub <= int(i,seq_addr_sub'length); wait_on_sig(seq_ready_pub); diff --git a/src/ros2/example_interfaces/Fibonacci_action_feedback_pub.vhd b/src/ros2/example_interfaces/Fibonacci_action_feedback_pub.vhd index 52d27dd..a3146b6 100644 --- a/src/ros2/example_interfaces/Fibonacci_action_feedback_pub.vhd +++ b/src/ros2/example_interfaces/Fibonacci_action_feedback_pub.vhd @@ -45,14 +45,7 @@ entity Fibonacci_action_feedback_pub is ack_user : out std_logic; -- ###GENERATED START### - goal_id_addr : in std_logic_vector(GOAL_ID_ADDR_WIDTH-1 downto 0); - goal_id_ready : out std_logic; - goal_id_ren : in std_logic; - goal_id_wen : in std_logic; - goal_id_valid : out std_logic; - goal_id_ack : in std_logic; - goal_id_r : out std_logic_vector(CDR_INT8_WIDTH-1 downto 0); - goal_id_w : in std_logic_vector(CDR_INT8_WIDTH-1 downto 0); + goal_id : in std_logic_vector(UUID_WIDTH-1 downto 0); seq_len : in std_logic_vector(SEQ_ADDR_WIDTH-1 downto 0); seq_addr : in std_logic_vector(SEQ_ADDR_WIDTH-1 downto 0); seq_ready : out std_logic; @@ -90,12 +83,8 @@ architecture arch of Fibonacci_action_feedback_pub is signal return_code_latch, return_code_latch_next : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0); signal encode_stage, encode_stage_next : ENCODE_STAGE_TYPE; -- ###GENERATED START### - signal goal_id_cnt, goal_id_cnt_next : natural range 0 to GOAL_ID_MAX_DEPTH-1; + signal uuid_cnt, uuid_cnt_next : natural range 0 to GOAL_ID_MAX_DEPTH-1; signal seq_cnt, seq_cnt_next : natural range 0 to SEQ_MAX_DEPTH-1; - -- goal_id_mem SIGNALS - signal goal_id_mem_addr : std_logic_vector(GOAL_ID_ADDR_WIDTH-1 downto 0); - signal goal_id_mem_read, goal_id_mem_ready_in, goal_id_mem_ready_out, goal_id_mem_valid_in, goal_id_mem_valid_out : std_logic; - signal goal_id_mem_data_in, goal_id_mem_data_out : std_logic_vector(CDR_INT8_WIDTH-1 downto 0); -- seq_mem SIGNALS signal seq_mem_addr : std_logic_vector(SEQ_ADDR_WIDTH-1 downto 0); signal seq_mem_read, seq_mem_ready_in, seq_mem_ready_out, seq_mem_valid_in, seq_mem_valid_out : std_logic; @@ -106,26 +95,6 @@ architecture arch of Fibonacci_action_feedback_pub is begin -- ###GENERATED START### - goal_id_mem : entity work.mem_ctrl(arch) - generic map ( - ADDR_WIDTH => GOAL_ID_ADDR_WIDTH, - DATA_WIDTH => CDR_INT8_WIDTH, - MEMORY_DEPTH => GOAL_ID_MAX_DEPTH, - MAX_BURST_LENGTH => 1 - ) - port map ( - clk => clk, - reset => reset or abort_mem, - addr => goal_id_mem_addr, - read => goal_id_mem_read, - ready_in => goal_id_mem_ready_in, - valid_in => goal_id_mem_valid_in, - data_in => goal_id_mem_data_in, - ready_out => goal_id_mem_ready_out, - valid_out => goal_id_mem_valid_out, - data_out => goal_id_mem_data_out - ); - seq_mem : entity work.mem_ctrl(arch) generic map ( ADDR_WIDTH => SEQ_ADDR_WIDTH, @@ -153,9 +122,6 @@ begin max_wait_dds <= DURATION_ZERO; ready_in_dds <= '0'; -- DDS Writer Input is unused -- ###GENERATED START### - goal_id_valid <= goal_id_mem_valid_out; - goal_id_r <= goal_id_mem_data_out; - goal_id_mem_data_in <= goal_id_w; seq_valid <= seq_mem_valid_out; seq_r <= seq_mem_data_out; seq_mem_data_in <= seq_w; @@ -183,12 +149,7 @@ begin return_code_user <= ROS_RET_OK; data_out_dds <= (others => '0'); -- ###GENERATED START### - goal_id_cnt_next <= goal_id_cnt; - goal_id_mem_addr <= (others => '0'); - goal_id_mem_read <= '0'; - goal_id_mem_valid_in <= '0'; - goal_id_mem_ready_out <= '0'; - goal_id_ready <= '0'; + uuid_cnt_next <= uuid_cnt; seq_cnt_next <= seq_cnt; seq_mem_addr <= (others => '0'); seq_mem_read <= '0'; @@ -212,11 +173,6 @@ begin abort_mem <= '1'; else -- ###GENERATED START### - goal_id_ready <= goal_id_mem_ready_in; - goal_id_mem_addr <= goal_id_addr; - goal_id_mem_read <= goal_id_ren; - goal_id_mem_valid_in <= goal_id_ren or goal_id_wen; - goal_id_mem_ready_out <= goal_id_ack; seq_ready <= seq_mem_ready_in; seq_mem_addr <= seq_addr; seq_mem_read <= seq_ren; @@ -253,7 +209,6 @@ begin data_out_latch_next <= (others => '0'); -- ###GENERATED START### encode_stage_next <= WRITE_GOAL_ID; - goal_id_cnt_next <= 0; -- ###GENERATED END### end if; when PUSH => @@ -298,43 +253,27 @@ begin case (encode_stage) is -- ###GENERATED START### when WRITE_GOAL_ID => + -- Special Encoding for effieciency (Prevent having to define memory for UUID) -- ALIGN GUARD if (not check_align(align_offset, ALIGN_1)) then target_align_next <= ALIGN_1; stage_next <= ALIGN_STREAM; - else - case (cnt) is - -- GET - when 0 => - goal_id_mem_addr <= std_logic_vector(to_unsigned(goal_id_cnt,GOAL_ID_ADDR_WIDTH)); - goal_id_mem_valid_in <= '1'; - goal_id_mem_read <= '1'; - -- Memory Operation Guard - if (goal_id_mem_ready_in = '1') then - cnt_next <= cnt + 1; - end if; - -- WRITE - when 1 => - goal_id_mem_ready_out <= '1'; - -- Memory Operation Guard - if (goal_id_mem_valid_out = '1') then - data_out_latch_next <= write_sub_vector(data_out_latch, endian_swap(LITTLE_ENDIAN, goal_id_mem_data_out), to_integer(align_offset(1 downto 0)), TRUE); - align_offset_next <= align_offset + 1; - cnt_next <= 0; - - if (goal_id_cnt = GOAL_ID_MAX_DEPTH-1) then - encode_stage_next <= WRITE_SEQ_LENGTH; - else - goal_id_cnt_next <= goal_id_cnt + 1; - end if; - - -- Need to fetch next Word - if(align_offset(1 downto 0) = "11") then - stage_next <= PUSH; - end if; - end if; - when others => - end case; + else + data_out_latch_next <= write_sub_vector(data_out_latch_next, get_sub_vector(goal_id,uuid_cnt,CDR_INT8_WIDTH,TRUE), to_integer(align_offset(1 downto 0)), TRUE); + align_offset_next <= align_offset + 1; + + if (uuid_cnt = GOAL_ID_MAX_DEPTH-1) then + encode_stage_next <= WRITE_SEQ_LENGTH; + cnt_next <= 0; + uuid_cnt_next <= 0; -- Post-Reset + else + uuid_cnt_next <= uuid_cnt + 1; + end if; + + -- Need to fetch next Word + if(align_offset(1 downto 0) = "11") then + stage_next <= PUSH; + end if; end if; when WRITE_SEQ_LENGTH => -- ALIGN GUARD @@ -427,7 +366,7 @@ begin align_offset <= (others => '0'); data_out_latch <= (others => '0'); -- ###GENERATED START### - goal_id_cnt <= 0; + uuid_cnt <= 0; seq_cnt <= 0; -- ###GENERATED END### else @@ -441,7 +380,7 @@ begin align_offset <= align_offset_next; data_out_latch <= data_out_latch_next; -- ###GENERATED START### - goal_id_cnt <= goal_id_cnt_next; + uuid_cnt <= uuid_cnt_next; seq_cnt <= seq_cnt_next; -- ###GENERATED END### end if; diff --git a/src/ros2/example_interfaces/Fibonacci_action_feedback_sub.vhd b/src/ros2/example_interfaces/Fibonacci_action_feedback_sub.vhd index ab20ca2..81d65b0 100644 --- a/src/ros2/example_interfaces/Fibonacci_action_feedback_sub.vhd +++ b/src/ros2/example_interfaces/Fibonacci_action_feedback_sub.vhd @@ -61,12 +61,7 @@ entity Fibonacci_action_feedback_sub is data_available_user : out std_logic; -- ###GENERATED START### - goal_id_addr : in std_logic_vector(GOAL_ID_ADDR_WIDTH-1 downto 0); - goal_id_ready : out std_logic; - goal_id_ren : in std_logic; - goal_id_valid : out std_logic; - goal_id_ack : in std_logic; - goal_id : out std_logic_vector(CDR_INT8_WIDTH-1 downto 0); + goal_id : out std_logic_vector(UUID_WIDTH-1 downto 0); seq_len : out std_logic_vector(SEQ_ADDR_WIDTH-1 downto 0); seq_addr : in std_logic_vector(SEQ_ADDR_WIDTH-1 downto 0); seq_ready : out std_logic; @@ -109,13 +104,10 @@ architecture arch of Fibonacci_action_feedback_sub is signal decode_stage, decode_stage_next : DECODE_STAGE_TYPE; signal return_stage, return_stage_next : DECODE_STAGE_TYPE; -- ###GENERATED START### - signal goal_id_cnt, goal_id_cnt_next : natural range 0 to GOAL_ID_MAX_DEPTH-1; + signal uuid_cnt, uuid_cnt_next : natural range 0 to GOAL_ID_MAX_DEPTH-1; signal seq_cnt, seq_cnt_next : natural range 0 to SEQ_MAX_DEPTH-1; signal seq_len_latch, seq_len_latch_next : unsigned(SEQ_ADDR_WIDTH-1 downto 0); - -- seq_mem SIGNALS - signal goal_id_mem_addr : std_logic_vector(GOAL_ID_ADDR_WIDTH-1 downto 0); - signal goal_id_mem_read, goal_id_mem_ready_in, goal_id_mem_ready_out, goal_id_mem_valid_in, goal_id_mem_valid_out : std_logic; - signal goal_id_mem_data_in, goal_id_mem_data_out : std_logic_vector(CDR_INT8_WIDTH-1 downto 0); + signal goal_id_latch, goal_id_latch_next : std_logic_vector(UUID_WIDTH-1 downto 0); -- seq_mem SIGNALS signal seq_mem_addr : std_logic_vector(SEQ_ADDR_WIDTH-1 downto 0); signal seq_mem_read, seq_mem_ready_in, seq_mem_ready_out, seq_mem_valid_in, seq_mem_valid_out : std_logic; @@ -130,26 +122,6 @@ architecture arch of Fibonacci_action_feedback_sub is begin -- ###GENERATED START### - goal_id_mem : entity work.mem_ctrl(arch) - generic map ( - ADDR_WIDTH => GOAL_ID_ADDR_WIDTH, - DATA_WIDTH => CDR_INT8_WIDTH, - MEMORY_DEPTH => GOAL_ID_MAX_DEPTH, - MAX_BURST_LENGTH => 1 - ) - port map ( - clk => clk, - reset => reset or abort_mem, - addr => goal_id_mem_addr, - read => goal_id_mem_read, - ready_in => goal_id_mem_ready_in, - valid_in => goal_id_mem_valid_in, - data_in => goal_id_mem_data_in, - ready_out => goal_id_mem_ready_out, - valid_out => goal_id_mem_valid_out, - data_out => goal_id_mem_data_out - ); - seq_mem : entity work.mem_ctrl(arch) generic map ( ADDR_WIDTH => SEQ_ADDR_WIDTH, @@ -182,8 +154,7 @@ begin instance_handle_dds <= HANDLE_NIL; max_samples_dds <= (others => '0'); -- ###GENERATED START### - goal_id_valid <= goal_id_mem_valid_out; - goal_id <= goal_id_mem_data_out; + goal_id <= goal_id_latch; seq_len <= std_logic_vector(seq_len_latch); seq_valid <= seq_mem_valid_out; seq <= seq_mem_data_out; @@ -220,13 +191,8 @@ begin done_user <= '0'; return_code_user <= ROS_RET_OK; -- ###GENERATED START### - goal_id_cnt_next <= goal_id_cnt; - goal_id_mem_addr <= (others => '0'); - goal_id_mem_read <= '0'; - goal_id_mem_valid_in <= '0'; - goal_id_mem_ready_out <= '0'; - goal_id_mem_data_in <= (others => '0'); - goal_id_ready <= '0'; + uuid_cnt_next <= uuid_cnt; + goal_id_latch_next <= goal_id_latch; seq_len_latch_next <= seq_len_latch; seq_cnt_next <= seq_cnt; seq_mem_addr <= (others => '0'); @@ -262,11 +228,6 @@ begin abort_mem <= '1'; else -- ###GENERATED START### - goal_id_ready <= goal_id_mem_ready_in; - goal_id_mem_addr <= goal_id_addr; - goal_id_mem_read <= goal_id_ren; - goal_id_mem_valid_in <= goal_id_ren; - goal_id_mem_ready_out <= goal_id_ack; seq_ready <= seq_mem_ready_in; seq_mem_addr <= seq_addr; seq_mem_read <= seq_ren; @@ -335,7 +296,6 @@ begin align_offset_next <= (others => '0'); -- ###GENERATED START### decode_stage_next <= GET_GOAL_ID; - goal_id_cnt_next <= 0; -- ###GENERATED END### -- Initial Fetch when CDR_LE => @@ -345,7 +305,6 @@ begin align_offset_next <= (others => '0'); -- ###GENERATED START### decode_stage_next <= GET_GOAL_ID; - goal_id_cnt_next <= 0; -- ###GENERATED END### when others => -- Unknown Payload Encoding @@ -384,28 +343,25 @@ begin case (decode_stage) is -- ###GENERATED START### when GET_GOAL_ID => + -- Special Decoding for effieciency (Prevent having to define memory for UUID) -- ALIGN GUARD if (not check_align(align_offset, ALIGN_1)) then target_align_next <= ALIGN_1; stage_next <= ALIGN_STREAM; else - goal_id_mem_addr <= std_logic_vector(to_unsigned(goal_id_cnt,GOAL_ID_ADDR_WIDTH)); - goal_id_mem_data_in <= endian_swap(endian_flag, get_sub_vector(data_in_latch, to_integer(align_offset(1 downto 0)), CDR_INT8_WIDTH, TRUE)); - goal_id_mem_valid_in <= '1'; - -- Memory Operation Guard - if (goal_id_mem_ready_in = '1') then - align_offset_next <= align_offset + 1; - - -- Need to fetch next Word - if(align_offset(1 downto 0) = "11") then - stage_next <= FETCH; - end if; - - if (goal_id_cnt = GOAL_ID_MAX_DEPTH-1) then - decode_stage_next <= GET_SEQ_LENGTH; - else - goal_id_cnt_next <= goal_id_cnt + 1; - end if; + goal_id_latch_next <= write_sub_vector(goal_id_latch_next, get_sub_vector(data_in_latch, to_integer(align_offset(1 downto 0)), CDR_INT8_WIDTH, TRUE), uuid_cnt, TRUE); + align_offset_next <= align_offset + 1; + + -- Need to fetch next Word + if(align_offset(1 downto 0) = "11") then + stage_next <= FETCH; + end if; + + if (uuid_cnt = GOAL_ID_MAX_DEPTH-1) then + decode_stage_next <= GET_SEQ_LENGTH; + uuid_cnt_next <= 0; -- Post-reset + else + uuid_cnt_next <= uuid_cnt + 1; end if; end if; when GET_SEQ_LENGTH => @@ -560,7 +516,8 @@ begin align_offset <= (others => '0'); data_in_latch <= (others => '0'); -- ###GENERATED START### - goal_id_cnt <= 0; + uuid_cnt <= 0; + goal_id_latch <= (others => '0'); seq_cnt <= 0; seq_len_latch <= (others => '0'); -- ###GENERATED END### @@ -582,7 +539,8 @@ begin align_offset <= align_offset_next; data_in_latch <= data_in_latch_next; -- ###GENERATED START### - goal_id_cnt <= goal_id_cnt_next; + uuid_cnt <= uuid_cnt_next; + goal_id_latch <= goal_id_latch_next; seq_cnt <= seq_cnt_next; seq_len_latch <= seq_len_latch_next; -- ###GENERATED END### diff --git a/src/ros2/ros_package.vhd b/src/ros2/ros_package.vhd index 1871af7..0ed11db 100644 --- a/src/ros2/ros_package.vhd +++ b/src/ros2/ros_package.vhd @@ -9,6 +9,30 @@ 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); @@ -157,9 +181,6 @@ package ros_package is constant EMPTY_SERVICE_INFO : SERVICE_INFO_TYPE := (source_timestamp => TIME_INVALID, received_timestamp => TIME_INVALID, request_id => EMPTY_REQUEST_ID); - 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); - function to_gid(guid : GUID_TYPE) return GID_TYPE; type MESSAGE_INFO_TYPE is record @@ -328,6 +349,45 @@ package body ros_package is 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