From a064a44dc4178a0a9f8ec7989247accae3bc3d00 Mon Sep 17 00:00:00 2001 From: Greek64 Date: Sat, 5 Mar 2022 11:47:58 +0100 Subject: [PATCH] Add Fibonacci ROS Action Implementation A complete ROS action server and client implementation of the example_interfaces Fibonacci action is done, along with accompaning testbenches. --- sim/L1_Fibonacci_ros_action_test1.do | 111 +++ .../Level_1/L1_Fibonacci_ros_action_test1.vhd | 745 ++++++++++++++ .../Level_1/L1_Fibonacci_ros_action_test2.vhd | 716 ++++++++++++++ src/ros2/Tests/ros_testbench.pro | 8 +- .../Fibonacci_ros_action_client.vhd | 769 +++++++++++++++ .../Fibonacci_ros_action_server.vhd | 922 ++++++++++++++++++ src/ros2/ros_package.vhd | 2 +- 7 files changed, 3271 insertions(+), 2 deletions(-) create mode 100644 sim/L1_Fibonacci_ros_action_test1.do create mode 100644 src/ros2/Tests/Level_1/L1_Fibonacci_ros_action_test1.vhd create mode 100644 src/ros2/Tests/Level_1/L1_Fibonacci_ros_action_test2.vhd create mode 100644 src/ros2/example_interfaces/Fibonacci_ros_action_client.vhd create mode 100644 src/ros2/example_interfaces/Fibonacci_ros_action_server.vhd diff --git a/sim/L1_Fibonacci_ros_action_test1.do b/sim/L1_Fibonacci_ros_action_test1.do new file mode 100644 index 0000000..da15167 --- /dev/null +++ b/sim/L1_Fibonacci_ros_action_test1.do @@ -0,0 +1,111 @@ +onerror {resume} +radix define ROS_RETCODE { + "10#0#" "ROS_RET_OK", + "10#1#" "ROS_RET_ERROR", + "10#2#" "ROS_RET_TIMEOUT", + "10#3#" "ROS_RET_UNSUPPORTED", + "10#10#" "ROS_RET_BAD_ALLOC", + "10#11#" "ROS_RET_INVALID_ARGUMENT", + "10#12#" "ROS_RET_INCORRECT_RMW_IMPLEMENTATION", + "10#100#" "ROS_RET_ALREADY_INIT", + "10#101#" "ROS_RET_NOT_INIT", + "10#102#" "ROS_RET_MISMATCHED_RMW", + "10#103#" "ROS_RET_TOPIC_NAME_INVALID", + "10#104#" "ROS_RET_SERVICE_NAME_INVALID", + "10#105#" "ROS_RET_UNKNOWN_SUBSTITUTION", + "10#106#" "ROS_RET_ALREADY_SHUTDOWN", + "10#200#" "ROS_RET_NODE_INVALID", + "10#201#" "ROS_RET_NODE_INVALID_NAME", + "10#202#" "ROS_RET_NODE_INVALID_NAMESPACE", + "10#203#" "ROS_RET_NODE_NAME_NON_EXISTENT", + "10#400#" "ROS_RET_SUBSCRIPTION_INVALID", + "10#401#" "ROS_RET_SUBSCRIPTION_TAKE_FAILED", + "10#500#" "ROS_RET_CLIENT_INVALID", + "10#501#" "ROS_RET_CLIENT_TAKE_FAILED", + "10#600#" "ROS_RET_SERVICE_INVALID", + "10#601#" "ROS_RET_SERVICE_TAKE_FAILED", + "10#800#" "ROS_RET_TIMER_INVALID", + "10#801#" "ROS_RET_TIMER_CANCELED", + "10#900#" "ROS_RET_WAIT_SET_INVALID", + "10#901#" "ROS_RET_WAIT_SET_EMPTY", + "10#902#" "ROS_RET_WAIT_SET_FULL", + "10#1001#" "ROS_RET_INVALID_REMAP_RULE", + "10#1002#" "ROS_RET_WRONG_LEXEME", + "10#1003#" "ROS_RET_INVALID_ROS_ARGS", + "10#1010#" "ROS_RET_INVALID_PARAM_RULE", + "10#1020#" "ROS_RET_INVALID_LOG_LEVEL_RULE", + "10#2001#" "ROS_RET_EVENT_TAKE_FAILED", + "10#2000#" "ROS_RET_ACTION_NAME_INVALID", + "10#2100#" "ROS_RET_ACTION_GOAL_ACCEPTED", + "10#2101#" "ROS_RET_ACTION_GOAL_REJECTED", + "10#2102#" "ROS_RET_ACTION_CLIENT_INVALID", + "10#2103#" "ROS_RET_ACTION_CLIENT_TAKE_FAILED", + "10#2200#" "ROS_RET_ACTION_SERVER_INVALID", + "10#2201#" "ROS_RET_ACTION_SERVER_TAKE_FAILED", + "10#2300#" "ROS_RET_ACTION_GOAL_HANDLE_INVALID", + "10#2301#" "ROS_RET_ACTION_GOAL_EVENT_INVALID", + "10#3000#" "ROS_RET_LIFECYCLE_STATE_REGISTERED", + "10#3001#" "ROS_RET_LIFECYCLE_STATE_NOT_REGISTERED", + -default unsigned +} +quietly WaveActivateNextPane {} 0 +add wave -noupdate -divider SYSTEM +add wave -noupdate /l1_fibonacci_ros_action_test1/clk +add wave -noupdate /l1_fibonacci_ros_action_test1/reset +add wave -noupdate -divider CLIENT +add wave -noupdate /l1_fibonacci_ros_action_test1/uut_c/start +add wave -noupdate /l1_fibonacci_ros_action_test1/uut_c/opcode +add wave -noupdate /l1_fibonacci_ros_action_test1/uut_c/ack +add wave -noupdate -radix hexadecimal /l1_fibonacci_ros_action_test1/uut_c/sequence_id +add wave -noupdate -radix hexadecimal /l1_fibonacci_ros_action_test1/uut_c/service_info +add wave -noupdate /l1_fibonacci_ros_action_test1/uut_c/done +add wave -noupdate -radix ROS_RETCODE /l1_fibonacci_ros_action_test1/uut_c/return_code +add wave -noupdate -group GOAL_C /l1_fibonacci_ros_action_test1/uut_c/goal_srv_client_inst/stage +add wave -noupdate -group GOAL_C /l1_fibonacci_ros_action_test1/uut_c/goal_srv_client_inst/cnt +add wave -noupdate -expand -group RESULT_C /l1_fibonacci_ros_action_test1/uut_c/result_srv_client_inst/stage +add wave -noupdate -expand -group RESULT_C /l1_fibonacci_ros_action_test1/uut_c/result_srv_client_inst/cnt +add wave -noupdate -expand -group CANCEL_C /l1_fibonacci_ros_action_test1/uut_c/cancel_srv_client_inst/stage +add wave -noupdate -expand -group CANCEL_C /l1_fibonacci_ros_action_test1/uut_c/cancel_srv_client_inst/cnt +add wave -noupdate -group FEEDBACK_C /l1_fibonacci_ros_action_test1/uut_c/stage +add wave -noupdate -group FEEDBACK_C /l1_fibonacci_ros_action_test1/uut_c/cnt +add wave -noupdate /l1_fibonacci_ros_action_test1/uut_c/stage +add wave -noupdate /l1_fibonacci_ros_action_test1/uut_c/cnt +add wave -noupdate -divider SERVER +add wave -noupdate /l1_fibonacci_ros_action_test1/uut_s/start +add wave -noupdate /l1_fibonacci_ros_action_test1/uut_s/opcode +add wave -noupdate /l1_fibonacci_ros_action_test1/uut_s/ack +add wave -noupdate /l1_fibonacci_ros_action_test1/uut_s/done +add wave -noupdate -radix ROS_RETCODE /l1_fibonacci_ros_action_test1/uut_s/return_code +add wave -noupdate -group GOAL_S /l1_fibonacci_ros_action_test1/uut_s/goal_srv_server_inst/stage +add wave -noupdate -group GOAL_S /l1_fibonacci_ros_action_test1/uut_s/goal_srv_server_inst/cnt +add wave -noupdate -expand -group RESULT_S /l1_fibonacci_ros_action_test1/uut_s/result_srv_server_inst/stage +add wave -noupdate -expand -group RESULT_S /l1_fibonacci_ros_action_test1/uut_s/result_srv_server_inst/cnt +add wave -noupdate -group CANCEL_S /l1_fibonacci_ros_action_test1/uut_s/cancel_srv_server_inst/stage +add wave -noupdate -group CANCEL_S /l1_fibonacci_ros_action_test1/uut_s/cancel_srv_server_inst/cnt +add wave -noupdate -group FEEDBACK_S /l1_fibonacci_ros_action_test1/uut_s/feedback_gen/feedback_pub_inst/stage +add wave -noupdate -group FEEDBACK_S /l1_fibonacci_ros_action_test1/uut_s/feedback_gen/feedback_pub_inst/cnt +add wave -noupdate -group STATUS_S /l1_fibonacci_ros_action_test1/uut_s/status_pub_inst/stage +add wave -noupdate -group STATUS_S /l1_fibonacci_ros_action_test1/uut_s/status_pub_inst/cnt +add wave -noupdate -group ACTION_SERVER /l1_fibonacci_ros_action_test1/uut_s/action_server_inst/stage +add wave -noupdate -group ACTION_SERVER /l1_fibonacci_ros_action_test1/uut_s/action_server_inst/cnt +add wave -noupdate /l1_fibonacci_ros_action_test1/uut_s/stage +add wave -noupdate /l1_fibonacci_ros_action_test1/uut_s/cnt +add wave -noupdate -divider MISC +TreeUpdate [SetDefaultTree] +WaveRestoreCursors {{Cursor 1} {1890595 ps} 0} +quietly wave cursor active 1 +configure wave -namecolwidth 150 +configure wave -valuecolwidth 100 +configure wave -justifyvalue left +configure wave -signalnamewidth 1 +configure wave -snapdistance 10 +configure wave -datasetprefix 0 +configure wave -rowmargin 4 +configure wave -childrowmargin 2 +configure wave -gridoffset 0 +configure wave -gridperiod 1 +configure wave -griddelta 40 +configure wave -timeline 0 +configure wave -timelineunits ns +update +WaveRestoreZoom {1314200 ps} {2338200 ps} diff --git a/src/ros2/Tests/Level_1/L1_Fibonacci_ros_action_test1.vhd b/src/ros2/Tests/Level_1/L1_Fibonacci_ros_action_test1.vhd new file mode 100644 index 0000000..60c96b7 --- /dev/null +++ b/src/ros2/Tests/Level_1/L1_Fibonacci_ros_action_test1.vhd @@ -0,0 +1,745 @@ +library ieee; +use ieee.std_logic_1164.all; +use ieee.numeric_std.all; + +library osvvm; -- Utility Library +context osvvm.OsvvmContext; + +use work.rtps_package.all; +use work.ros_package.all; +use work.rtps_test_package.all; +use work.Fibonacci_package.all; +use work.GoalStatus_package; + +-- This testbench tests the General Behaviour of ROS Action. +-- More specifically following tests are done: +-- * Test Unsuported Opcode Operations +-- * Test Take Failure on Client TAKE_* Operations +-- * Test multiple concurrent Goals +-- * Test Encoding/Decoding of Action Messages + +entity L1_Fibonacci_ros_action_test1 is +end entity; + +architecture testbench of L1_Fibonacci_ros_action_test1 is + + constant MAX_GOALS : natural := 2; + + type GOAL_ID_ARRAY_TYPE is array (0 to MAX_GOALS-1) of std_logic_vector(UUID_WIDTH-1 downto 0); + type GOAL_HANDLE_ARRAY_TYPE is array (0 to MAX_GOALS-1) of std_logic_vector(GOAL_HANDLE_WIDTH-1 downto 0); + type RESULT_INDEX_ARRAY_TYPE is array (0 to MAX_GOALS-1) of natural; + type SEQUENCE_ID_ARRAY_TYPE is array (0 to MAX_GOALS-1) of std_logic_vector(ROS_SEQUENCE_ID_WIDTH-1 downto 0); + + constant DEFAULT_SAMPLE_INFO : SAMPLE_INFO_TYPE := ( + sample_state => ANY_SAMPLE_STATE, + view_state => ANY_VIEW_STATE, + instance_state => ANY_INSTANCE_STATE, + source_timestamp => TIME_ZERO, + instance_handle => HANDLE_NIL, + publication_handle => HANDLE_NIL, + disposed_generation_count => (others => '0'), + no_writers_generation_count => (others => '0'), + sample_rank => (others => '0'), + generation_rank => (others => '0'), + absolute_generation_rank => (others => '0'), + valid_data => '1' + ); + + signal clk, reset : std_logic := '0'; + signal start_g, start_r : std_logic := '0'; + signal da_g, da_r : std_logic_vector(STATUS_KIND_WIDTH-1 downto 0) := (others => '0'); + signal ret_sg, ret_sr, ret_cg, ret_cr : std_logic_vector(RETURN_CODE_WIDTH-1 downto 0) := RETCODE_OK; + signal valid_gr, valid_gw, valid_rr, valid_rw, valid_cr, valid_cw, valid_f : std_logic := '0'; + signal ready_gr, ready_gw, ready_rr, ready_rw, ready_cr, ready_cw, ready_f : std_logic := '0'; + signal last_word_gr, last_word_gw, last_word_rr, last_word_rw, last_word_cr, last_word_cw, last_word_f : std_logic := '0'; + signal data_gr, data_gw, data_rr, data_rw, data_cr, data_cw, data_f : std_logic_vector(WORD_WIDTH-1 downto 0) := (others => '0'); + + signal start_c, start_s, ack_c, ack_s, done_c, done_s : std_logic := '0'; + signal opcode_c, opcode_s : ROS_ACTION_OPCODE_TYPE := NOP; + signal return_code_c, return_code_s : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := (others => '0'); + -- ###GENERATED START### + signal goal_order_c, new_goal_order_s : std_logic_vector(CDR_LONG_WIDTH-1 downto 0) := (others => '0'); + signal result_seq_len_c, result_seq_len_s, result_seq_addr_c, result_seq_addr_s : std_logic_vector(R_RR_SEQ_ADDR_WIDTH-1 downto 0) := (others => '0'); + signal result_seq_ready_c, result_seq_ready_s, result_seq_ren_c, result_seq_ren_s, result_seq_wen_s, result_seq_valid_c, result_seq_valid_s, result_seq_ack_c, result_seq_ack_s : std_logic := '0'; + signal result_seq_c, result_seq_r_s, result_seq_w_s : std_logic_vector(CDR_LONG_WIDTH-1 downto 0) := (others => '0'); + signal feedback_seq_len_c, feedback_seq_len_s, feedback_seq_addr_c, feedback_seq_addr_s : std_logic_vector(F_SEQ_ADDR_WIDTH-1 downto 0) := (others => '0'); + signal feedback_seq_ready_c, feedback_seq_ready_s, feedback_seq_ren_c, feedback_seq_ren_s, feedback_seq_wen_s, feedback_seq_valid_c, feedback_seq_valid_s, feedback_seq_ack_c, feedback_seq_ack_s : std_logic := '0'; + signal feedback_seq_c, feedback_seq_r_s, feedback_seq_w_s : std_logic_vector(CDR_LONG_WIDTH-1 downto 0) := (others => '0'); + -- ###GENERATED END### + signal goal_id_c, goal_id_s, result_goal_id_c, feedback_goal_id_c : std_logic_vector(UUID_WIDTH-1 downto 0) := (others => '0'); + signal goal_accepted_c, new_goal_accepted_s : std_logic := '0'; + signal goal_stamp_c, goal_stamp_s : ROS_TIME_TYPE := ROS_TIME_ZERO; + signal result_status_c : std_logic_vector(CDR_INT8_WIDTH-1 downto 0) := (others => '0'); + signal service_info_c : SERVICE_INFO_TYPE := EMPTY_SERVICE_INFO; + signal sequence_id_c : std_logic_vector(ROS_SEQUENCE_ID_WIDTH-1 downto 0) := (others => '0'); + signal goal_handle_in_s, goal_handle_out_s, new_goal_handle_s : std_logic_vector(GOAL_HANDLE_WIDTH-1 downto 0) := (others => '0'); + signal goal_state_in_s, goal_state_out_s : std_logic_vector(CDR_INT8_WIDTH-1 downto 0) := (others => '0'); + signal goal_result_index_s, new_goal_result_index_s, index_result_s : std_logic_vector(WORD_WIDTH-1 downto 0) := (others => '0'); + signal new_goal_request_s, new_goal_response_s, index_result_wen_s, index_result_ready_s : std_logic := '0'; +begin + + uut_c : entity work.Fibonacci_ros_action_client(arch) + port map ( + clk => clk, + reset => reset, + time => ROS_TIME_ZERO, + g_start_r => open, + g_ack_r => '1', + g_opcode_r => open, + g_instance_state_r => open, + g_view_state_r => open, + g_sample_state_r => open, + g_instance_handle_r => open, + g_max_samples_r => open, + g_get_data_r => open, + g_done_r => '1', + g_return_code_r => ret_cg, + g_valid_in_r => valid_gr, + g_ready_in_r => ready_gr, + g_data_in_r => data_gr, + g_last_word_in_r => last_word_gr, + g_sample_info_r => DEFAULT_SAMPLE_INFO, + g_sample_info_valid_r => '1', + g_sample_info_ack_r => open, + g_eoc_r => '1', + g_status_r => (others => '0'), + g_start_w => open, + g_ack_w => '1', + g_opcode_w => open, + g_instance_handle_out_w => open, + g_source_ts_w => open, + g_max_wait_w => open, + g_done_w => '1', + g_return_code_w => RETCODE_OK, + g_instance_handle_in_w => HANDLE_NIL, + g_valid_out_w => valid_gw, + g_ready_out_w => ready_gw, + g_data_out_w => data_gw, + g_last_word_out_w => last_word_gw, + g_valid_in_w => '0', + g_ready_in_w => open, + g_data_in_w => (others => '0'), + g_last_word_in_w => '0', + g_status_w => (others => '0'), + r_start_r => open, + r_ack_r => '1', + r_opcode_r => open, + r_instance_state_r => open, + r_view_state_r => open, + r_sample_state_r => open, + r_instance_handle_r => open, + r_max_samples_r => open, + r_get_data_r => open, + r_done_r => '1', + r_return_code_r => ret_cr, + r_valid_in_r => valid_rr, + r_ready_in_r => ready_rr, + r_data_in_r => data_rr, + r_last_word_in_r => last_word_rr, + r_sample_info_r => DEFAULT_SAMPLE_INFO, + r_sample_info_valid_r => '1', + r_sample_info_ack_r => open, + r_eoc_r => '1', + r_status_r => (others => '0'), + r_start_w => open, + r_ack_w => '1', + r_opcode_w => open, + r_instance_handle_out_w => open, + r_source_ts_w => open, + r_max_wait_w => open, + r_done_w => '1', + r_return_code_w => RETCODE_OK, + r_instance_handle_in_w => HANDLE_NIL, + r_valid_out_w => valid_rw, + r_ready_out_w => ready_rw, + r_data_out_w => data_rw, + r_last_word_out_w => last_word_rw, + r_valid_in_w => '0', + r_ready_in_w => open, + r_data_in_w => (others => '0'), + r_last_word_in_w => '0', + r_status_w => (others => '0'), + c_start_r => open, + c_ack_r => '1', + c_opcode_r => open, + c_instance_state_r => open, + c_view_state_r => open, + c_sample_state_r => open, + c_instance_handle_r => open, + c_max_samples_r => open, + c_get_data_r => open, + c_done_r => '1', + c_return_code_r => RETCODE_NO_DATA, + c_valid_in_r => valid_cr, + c_ready_in_r => ready_cr, + c_data_in_r => data_cr, + c_last_word_in_r => last_word_cr, + c_sample_info_r => DEFAULT_SAMPLE_INFO, + c_sample_info_valid_r => '1', + c_sample_info_ack_r => open, + c_eoc_r => '1', + c_status_r => (others => '0'), + c_start_w => open, + c_ack_w => '1', + c_opcode_w => open, + c_instance_handle_out_w => open, + c_source_ts_w => open, + c_max_wait_w => open, + c_done_w => '1', + c_return_code_w => RETCODE_OK, + c_instance_handle_in_w => HANDLE_NIL, + c_valid_out_w => valid_cw, + c_ready_out_w => ready_cw, + c_data_out_w => data_cw, + c_last_word_out_w => last_word_cw, + c_valid_in_w => '0', + c_ready_in_w => open, + c_data_in_w => (others => '0'), + c_last_word_in_w => '0', + c_status_w => (others => '0'), + f_start_dds => open, + f_ack_dds => '1', + f_opcode_dds => open, + f_instance_state_dds => open, + f_view_state_dds => open, + f_sample_state_dds => open, + f_instance_handle_dds => open, + f_max_samples_dds => open, + f_get_data_dds => open, + f_done_dds => '1', + f_return_code_dds => RETCODE_OK, + f_valid_in_dds => valid_f, + f_ready_in_dds => ready_f, + f_data_in_dds => data_f, + f_last_word_in_dds => last_word_f, + f_sample_info_dds => DEFAULT_SAMPLE_INFO, + f_sample_info_valid_dds => '1', + f_sample_info_ack_dds => open, + f_eoc_dds => '1', + f_status_dds => (others => '0'), + start => start_c, + opcode => opcode_c, + ack => ack_c, + sequence_id => sequence_id_c, + service_info => service_info_c, + done => done_c, + return_code => return_code_c, + goal_id => goal_id_c, + -- ###GENERATED START### + goal_order => goal_order_c, + -- ###GENERATED END### + goal_accepted => goal_accepted_c, + goal_stamp => goal_stamp_c, + result_goal_id => result_goal_id_c, + result_status => result_status_c, + -- ###GENERATED START### + result_seq_len => result_seq_len_c, + result_seq_addr => result_seq_addr_c, + result_seq_ready => result_seq_ready_c, + result_seq_ren => result_seq_ren_c, + result_seq_valid => result_seq_valid_c, + result_seq_ack => result_seq_ack_c, + result_seq => result_seq_c, + -- ###GENERATED END### + cancel_goal_id => (others => '0'), + cancel_stamp => ROS_TIME_ZERO, + cancel_return_code => open, + cancel_goals_canceling_len => open, + cancel_goals_canceling_addr => (others => '0'), + cancel_goals_canceling_ready => open, + cancel_goals_canceling_ren => '0', + cancel_goals_canceling_valid => open, + cancel_goals_canceling_ack => '0', + cancel_goals_canceling_goal_id => open, + cancel_goals_canceling_stamp => open, + -- FEEDBACK SIGNALS + feedback_goal_id => feedback_goal_id_c, + -- ###GENERATED START### + feedback_seq_len => feedback_seq_len_c, + feedback_seq_addr => feedback_seq_addr_c, + feedback_seq_ready => feedback_seq_ready_c, + feedback_seq_ren => feedback_seq_ren_c, + feedback_seq_valid => feedback_seq_valid_c, + feedback_seq_ack => feedback_seq_ack_c, + feedback_seq => feedback_seq_c + -- ###GENERATED END### + ); + + uut_s : entity work.Fibonacci_ros_action_server(arch) + generic map ( + TIMEOUT_DURATION => ROS_DURATION_INFINITE, + MAX_GOALS => MAX_GOALS, + MAX_RESULT_REQUESTS => MAX_GOALS + ) + port map ( + clk => clk, + reset => reset, + time => ROS_TIME_ZERO, + g_start_r => start_g, + g_ack_r => '1', + g_opcode_r => open, + g_instance_state_r => open, + g_view_state_r => open, + g_sample_state_r => open, + g_instance_handle_r => open, + g_max_samples_r => open, + g_get_data_r => open, + g_done_r => '1', + g_return_code_r => ret_sg, + g_valid_in_r => valid_gw, + g_ready_in_r => ready_gw, + g_data_in_r => data_gw, + g_last_word_in_r => last_word_gw, + g_sample_info_r => DEFAULT_SAMPLE_INFO, + g_sample_info_valid_r => '1', + g_sample_info_ack_r => open, + g_eoc_r => '1', + g_status_r => da_g, + g_start_w => open, + g_ack_w => '1', + g_opcode_w => open, + g_instance_handle_out_w => open, + g_source_ts_w => open, + g_max_wait_w => open, + g_done_w => '1', + g_return_code_w => RETCODE_OK, + g_instance_handle_in_w => HANDLE_NIL, + g_valid_out_w => valid_gr, + g_ready_out_w => ready_gr, + g_data_out_w => data_gr, + g_last_word_out_w => last_word_gr, + g_valid_in_w => '0', + g_ready_in_w => open, + g_data_in_w => (others => '0'), + g_last_word_in_w => '0', + g_status_w => (others => '0'), + r_start_r => start_r, + r_ack_r => '1', + r_opcode_r => open, + r_instance_state_r => open, + r_view_state_r => open, + r_sample_state_r => open, + r_instance_handle_r => open, + r_max_samples_r => open, + r_get_data_r => open, + r_done_r => '1', + r_return_code_r => ret_sr, + r_valid_in_r => valid_rw, + r_ready_in_r => ready_rw, + r_data_in_r => data_rw, + r_last_word_in_r => last_word_rw, + r_sample_info_r => DEFAULT_SAMPLE_INFO, + r_sample_info_valid_r => '1', + r_sample_info_ack_r => open, + r_eoc_r => '1', + r_status_r => da_r, + r_start_w => open, + r_ack_w => '1', + r_opcode_w => open, + r_instance_handle_out_w => open, + r_source_ts_w => open, + r_max_wait_w => open, + r_done_w => '1', + r_return_code_w => RETCODE_OK, + r_instance_handle_in_w => HANDLE_NIL, + r_valid_out_w => valid_rr, + r_ready_out_w => ready_rr, + r_data_out_w => data_rr, + r_last_word_out_w => last_word_rr, + r_valid_in_w => '0', + r_ready_in_w => open, + r_data_in_w => (others => '0'), + r_last_word_in_w => '0', + r_status_w => (others => '0'), + c_start_r => open, + c_ack_r => '1', + c_opcode_r => open, + c_instance_state_r => open, + c_view_state_r => open, + c_sample_state_r => open, + c_instance_handle_r => open, + c_max_samples_r => open, + c_get_data_r => open, + c_done_r => '1', + c_return_code_r => RETCODE_OK, + c_valid_in_r => valid_cw, + c_ready_in_r => ready_cw, + c_data_in_r => data_cw, + c_last_word_in_r => last_word_cw, + c_sample_info_r => DEFAULT_SAMPLE_INFO, + c_sample_info_valid_r => '1', + c_sample_info_ack_r => open, + c_eoc_r => '1', + c_status_r => (others => '0'), + c_start_w => open, + c_ack_w => '1', + c_opcode_w => open, + c_instance_handle_out_w => open, + c_source_ts_w => open, + c_max_wait_w => open, + c_done_w => '1', + c_return_code_w => RETCODE_OK, + c_instance_handle_in_w => HANDLE_NIL, + c_valid_out_w => valid_cr, + c_ready_out_w => ready_cr, + c_data_out_w => data_cr, + c_last_word_out_w => last_word_cr, + c_valid_in_w => '0', + c_ready_in_w => open, + c_data_in_w => (others => '0'), + c_last_word_in_w => '0', + c_status_w => (others => '0'), + f_start_dds => open, + f_ack_dds => '1', + f_opcode_dds => open, + f_instance_handle_out_dds => open, + f_source_ts_dds => open, + f_max_wait_dds => open, + f_done_dds => '1', + f_return_code_dds => RETCODE_OK, + f_instance_handle_in_dds => HANDLE_NIL, + f_valid_out_dds => valid_f, + f_ready_out_dds => ready_f, + f_data_out_dds => data_f, + f_last_word_out_dds => last_word_f, + f_valid_in_dds => '0', + f_ready_in_dds => open, + f_data_in_dds => (others => '0'), + f_last_word_in_dds => '0', + f_status_dds => (others => '0'), + s_start_dds => open, + s_ack_dds => '1', + s_opcode_dds => open, + s_instance_handle_out_dds => open, + s_source_ts_dds => open, + s_max_wait_dds => open, + s_done_dds => '1', + s_return_code_dds => RETCODE_OK, + s_instance_handle_in_dds => HANDLE_NIL, + s_valid_out_dds => open, + s_ready_out_dds => '1', + s_data_out_dds => open, + s_last_word_out_dds => open, + s_valid_in_dds => '0', + s_ready_in_dds => open, + s_data_in_dds => (others => '0'), + s_last_word_in_dds => '0', + s_status_dds => (others => '0'), + start => start_s, + opcode => opcode_s, + ack => ack_s, + done => done_s, + return_code => return_code_s, + goal_handle_in => goal_handle_in_s, + goal_handle_out => goal_handle_out_s, + goal_state_in => goal_state_in_s, + goal_state_out => goal_state_out_s, + goal_id => goal_id_s, + goal_result_index => goal_result_index_s, + goal_stamp => goal_stamp_s, + new_goal_request => new_goal_request_s, + new_goal_handle => new_goal_handle_s, + new_goal_result_index => new_goal_result_index_s, + -- ###GENERATED START### + new_goal_order => new_goal_order_s, + -- ###GENERATED END### + new_goal_accepted => new_goal_accepted_s, + new_goal_response => new_goal_response_s, + cancel_request => open, + cancel_request_handle => open, + cancel_accepted => '1', + cancel_response => '1', + index_result => index_result_s, + index_result_wen => index_result_wen_s, + index_result_ready => index_result_ready_s, + -- ###GENERATED START### + result_seq_len => result_seq_len_s, + result_seq_addr => result_seq_addr_s, + result_seq_ready => result_seq_ready_s, + result_seq_ren => result_seq_ren_s, + result_seq_wen => result_seq_wen_s, + result_seq_valid => result_seq_valid_s, + result_seq_ack => result_seq_ack_s, + result_seq_r => result_seq_r_s, + result_seq_w => result_seq_w_s, + -- ###GENERATED END### + -- ###GENERATED START### + feedback_seq_len => feedback_seq_len_s, + feedback_seq_addr => feedback_seq_addr_s, + feedback_seq_ready => feedback_seq_ready_s, + feedback_seq_ren => feedback_seq_ren_s, + feedback_seq_wen => feedback_seq_wen_s, + feedback_seq_valid => feedback_seq_valid_s, + feedback_seq_ack => feedback_seq_ack_s, + feedback_seq_r => feedback_seq_r_s, + feedback_seq_w => feedback_seq_w_s + -- ###GENERATED END### + ); + + stimulus_prc : process + variable RV : RandomPType; + variable GOAL, RESULT, FEEDBACK : AlertLogIDType; + variable sid : SEQUENCE_ID_ARRAY_TYPE := (others => (others => '0')); + variable test_gid : GOAL_ID_ARRAY_TYPE := (others => (others => '0')); + variable test_handle : GOAL_HANDLE_ARRAY_TYPE := (others => (others => '0')); + variable test_ind : RESULT_INDEX_ARRAY_TYPE := (others => 0); + + impure function gen_goal_id return std_logic_vector is + begin + return RV.RandSlv(UUID_WIDTH); + end function; + + procedure wait_on_sig(signal sig : std_logic) is + begin + if (sig /= '1') then + wait on sig until sig = '1'; + end if; + end procedure; + + procedure client_op(op : in ROS_ACTION_OPCODE_TYPE; ret : in std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0)) is + begin + start_c <= '1'; + opcode_c <= op; + wait_on_sig(ack_c); + wait until rising_edge(clk); + start_c <= '0'; + case (op) is + when SEND_GOAL_REQUEST => + ret_sg <= RETCODE_OK; + da_g <= DATA_AVAILABLE_STATUS; + wait_on_sig(start_g); + da_g <= (others => '0'); + when SEND_RESULT_REQUEST => + ret_sr <= RETCODE_OK; + da_r <= DATA_AVAILABLE_STATUS; + wait_on_sig(start_r); + da_r <= (others => '0'); + when others => + null; + end case; + wait_on_sig(done_c); + case (op) is + when SEND_GOAL_REQUEST => + ret_sg <= RETCODE_NO_DATA; + when SEND_RESULT_REQUEST => + ret_sr <= RETCODE_NO_DATA; + when others => + null; + end case; + wait for 1 ps; -- Make sure all signals stable + AlertIf(return_code_c /= ret, "Unexpected Client Response", FAILURE); + end procedure; + + procedure server_op(op : in ROS_ACTION_OPCODE_TYPE; ret : in std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0)) is + begin + start_s <= '1'; + opcode_s <= op; + wait_on_sig(ack_s); + wait until rising_edge(clk); + start_s <= '0'; + wait_on_sig(done_s); + wait for 1 ps; -- Make sure all signals stable + AlertIf(return_code_s /= ret, "Unexpected Client Response", FAILURE); + end procedure; + begin + + SetAlertLogName("L1_Fibonacci_ros_action_test1 - (With FEEDBACK) - General"); + SetAlertEnable(FAILURE, TRUE); + SetAlertEnable(ERROR, TRUE); + SetAlertEnable(WARNING, TRUE); + SetLogEnable(DEBUG, FALSE); + SetLogEnable(PASSED, FALSE); + SetLogEnable(INFO, TRUE); + RV.InitSeed(RV'instance_name); + GOAL := GetAlertLogID("GOAL", ALERTLOG_BASE_ID); + RESULT := GetAlertLogID("RESULT", ALERTLOG_BASE_ID); + FEEDBACK := GetAlertLogID("FEEDBACK", ALERTLOG_BASE_ID); + + Log("Initial Reset", INFO); + ret_cg <= RETCODE_NO_DATA; + ret_cr <= RETCODE_NO_DATA; + ret_sg <= RETCODE_NO_DATA; + ret_sr <= RETCODE_NO_DATA; + start_c <= '0'; + start_s <= '0'; + reset <= '1'; + wait until rising_edge(clk); + wait until rising_edge(clk); + reset <= '0'; + + Log("CLIENT: Test Unsupported Opcode", INFO); + client_op(UPDATE_GOAL_STATE,ROS_RET_UNSUPPORTED); + wait until rising_edge(clk); + + Log("SERVER: Test Unsupported Opcode", INFO); + server_op(SEND_GOAL_REQUEST,ROS_RET_UNSUPPORTED); + wait until rising_edge(clk); + + Log("CLIENT: Take Goal Response (No Take)", INFO); + client_op(TAKE_GOAL_RESPONSE,ROS_RET_ACTION_CLIENT_TAKE_FAILED); + wait until rising_edge(clk); + + Log("CLIENT: Take Result Response (No Take)", INFO); + client_op(TAKE_RESULT_RESPONSE,ROS_RET_ACTION_CLIENT_TAKE_FAILED); + wait until rising_edge(clk); + + Log("CLIENT: Take Cancel Response (No Take)", INFO); + client_op(TAKE_CANCEL_RESPONSE,ROS_RET_ACTION_CLIENT_TAKE_FAILED); + wait until rising_edge(clk); + + ret_cg <= RETCODE_OK; + ret_cr <= RETCODE_OK; + + for i in 0 to MAX_GOALS-1 loop + Log("CLIENT: Send GOAL " & integer'image(i) & " Request", INFO); + test_gid(i) := gen_goal_id; + goal_id_c <= test_gid(i); + goal_order_c <= RV.RandSlv(goal_order_c'length); + client_op(SEND_GOAL_REQUEST,ROS_RET_OK); + sid(i) := sequence_id_c; + wait until rising_edge(clk); + + Log("SERVER: Check GOAL " & integer'image(i) & " Request", INFO); + wait until new_goal_request_s = '1'; + AffirmIfEqual(GOAL, new_goal_order_s, goal_order_c); + test_handle(i) := new_goal_handle_s; + test_ind(i) := to_integer(unsigned(new_goal_result_index_s)); + goal_handle_in_s <= test_handle(i); + new_goal_accepted_s <= '1'; + new_goal_response_s <= '1'; + wait until rising_edge(clk); + new_goal_accepted_s <= '0'; + new_goal_response_s <= '0'; + + Log("CLIENT: Take GOAL " & integer'image(i) & " Request", INFO); + client_op(TAKE_GOAL_RESPONSE,ROS_RET_OK); + AlertIf(to_unsigned(SEQUENCENUMBER_TYPE(service_info_c.request_id.sequence_number)) /= unsigned(sid(i)), "Request ID incorrect", FAILURE); + wait until rising_edge(clk); + + goal_handle_in_s <= test_handle(i); + goal_state_in_s <= GoalStatus_package.STATUS_EXECUTING; + server_op(UPDATE_GOAL_STATE,ROS_RET_OK); + wait until rising_edge(clk); + end loop; + + Log("SERVER: Send FEEDBACK", INFO); + for i in 0 to RV.RandInt(1,10) loop + feedback_seq_len_s <= int(i+1,feedback_seq_len_s'length); + feedback_seq_addr_s <= int(i,feedback_seq_addr_s'length); + feedback_seq_w_s <= RV.RandSlv(feedback_seq_w_s'length); + wait_on_sig(feedback_seq_ready_s); + feedback_seq_wen_s <= '1'; + wait until rising_edge(clk); + feedback_seq_wen_s <= '0'; + end loop; + goal_handle_in_s <= test_handle(0); + start_s <= '1'; + opcode_s <= PUBLISH_FEEDBACK; + wait_on_sig(ack_s); + wait until rising_edge(clk); + start_s <= '0'; + + Log("CLIENT: Take FEEDBACK", INFO); + client_op(TAKE_FEEDBACK,ROS_RET_OK); + AffirmIfEqual(FEEDBACK, feedback_goal_id_c, test_gid(0)); + AffirmIfEqual(FEEDBACK, feedback_seq_len_c, feedback_seq_len_s); + for i in 0 to to_integer(unsigned(feedback_seq_len_c))-1 loop + feedback_seq_addr_s <= int(i,feedback_seq_addr_s'length); + feedback_seq_addr_c <= int(i,feedback_seq_addr_c'length); + wait_on_sig(feedback_seq_ready_s); + wait_on_sig(feedback_seq_ready_c); + feedback_seq_ren_s <= '1'; + feedback_seq_ren_c <= '1'; + wait until rising_edge(clk); + feedback_seq_ren_s <= '0'; + feedback_seq_ren_c <= '0'; + wait_on_sig(feedback_seq_valid_s); + wait_on_sig(feedback_seq_valid_c); + AffirmIfEqual(FEEDBACK, feedback_seq_c, feedback_seq_r_s); + feedback_seq_ack_s <= '1'; + feedback_seq_ack_c <= '1'; + wait until rising_edge(clk); + feedback_seq_ack_s <= '0'; + feedback_seq_ack_c <= '0'; + end loop; + + for i in 0 to MAX_GOALS-1 loop + Log("CLIENT: Send RESULT Request (Goal " & integer'image(i) & ")", INFO); + result_goal_id_c <= test_gid(i); + client_op(SEND_RESULT_REQUEST,ROS_RET_OK); + sid(i) := sequence_id_c; + wait until rising_edge(clk); + end loop; + + for j in 0 to MAX_GOALS-1 loop + Log("SERVER: Set RESULT (Goal " & integer'image(j) & ")", INFO); + index_result_s <= int(j,index_result_s'length); + for i in 0 to RV.RandInt(1,10) loop + result_seq_len_s <= int(i+1,result_seq_len_s'length); + result_seq_addr_s <= int(i,result_seq_addr_s'length); + result_seq_w_s <= RV.RandSlv(result_seq_w_s'length); + wait_on_sig(result_seq_ready_s); + result_seq_wen_s <= '1'; + wait until rising_edge(clk); + result_seq_wen_s <= '0'; + end loop; + wait_on_sig(index_result_ready_s); + index_result_wen_s <= '1'; + wait until rising_edge(clk); + index_result_wen_s <= '0'; + goal_handle_in_s <= test_handle(j); + goal_state_in_s <= GoalStatus_package.STATUS_SUCCEEDED; + server_op(UPDATE_GOAL_STATE,ROS_RET_OK); + wait until rising_edge(clk); + + Log("SERVER: Take RESULT Response (Goal " & integer'image(j) & ")", INFO); + goal_id_c <= test_gid(j); + client_op(TAKE_RESULT_RESPONSE,ROS_RET_OK); + AlertIf(to_unsigned(SEQUENCENUMBER_TYPE(service_info_c.request_id.sequence_number)) /= unsigned(sid(j)), "Request ID incorrect", FAILURE); + AffirmIfEqual(RESULT, result_status_c, GoalStatus_package.STATUS_SUCCEEDED); + AffirmIfEqual(RESULT, result_seq_len_c, result_seq_len_s); + for i in 0 to to_integer(unsigned(result_seq_len_c))-1 loop + result_seq_addr_s <= int(i,result_seq_addr_s'length); + result_seq_addr_c <= int(i,result_seq_addr_c'length); + wait_on_sig(result_seq_ready_s); + wait_on_sig(result_seq_ready_c); + result_seq_ren_s <= '1'; + result_seq_ren_c <= '1'; + wait until rising_edge(clk); + result_seq_ren_s <= '0'; + result_seq_ren_c <= '0'; + wait_on_sig(result_seq_valid_s); + wait_on_sig(result_seq_valid_c); + AffirmIfEqual(RESULT, result_seq_c, result_seq_r_s); + result_seq_ack_s <= '1'; + result_seq_ack_c <= '1'; + wait until rising_edge(clk); + result_seq_ack_s <= '0'; + result_seq_ack_c <= '0'; + end loop; + end loop; + + TranscriptOpen(RESULTS_FILE, APPEND_MODE); + SetTranscriptMirror; + ReportAlerts; + TranscriptClose; + std.env.stop; + wait; + end process; + + clock_prc : process + begin + clk <= '0'; + wait for TEST_CLOCK_PERIOD/2; + clk <= '1'; + wait for TEST_CLOCK_PERIOD/2; + end process; + + watchdog : process + begin + wait for 1 ms; + Alert("Test timeout", FAILURE); + std.env.stop; + end process; + +end architecture; \ No newline at end of file diff --git a/src/ros2/Tests/Level_1/L1_Fibonacci_ros_action_test2.vhd b/src/ros2/Tests/Level_1/L1_Fibonacci_ros_action_test2.vhd new file mode 100644 index 0000000..5542802 --- /dev/null +++ b/src/ros2/Tests/Level_1/L1_Fibonacci_ros_action_test2.vhd @@ -0,0 +1,716 @@ +library ieee; +use ieee.std_logic_1164.all; +use ieee.numeric_std.all; + +library osvvm; -- Utility Library +context osvvm.OsvvmContext; + +use work.rtps_package.all; +use work.ros_package.all; +use work.rtps_test_package.all; +use work.Fibonacci_package.all; +use work.GoalStatus_package; + +-- This testbench tests the General Behaviour of ROS Action. +-- More specifically following tests are done: +-- * Test Unsuported Opcode Operations +-- * Test Take Failure on Client TAKE_* Operations +-- * Test multiple concurrent Goals +-- * Test Encoding/Decoding of Action Messages + +entity L1_Fibonacci_ros_action_test2 is +end entity; + +architecture testbench of L1_Fibonacci_ros_action_test2 is + + constant MAX_GOALS : natural := 2; + + type GOAL_ID_ARRAY_TYPE is array (0 to MAX_GOALS-1) of std_logic_vector(UUID_WIDTH-1 downto 0); + type GOAL_HANDLE_ARRAY_TYPE is array (0 to MAX_GOALS-1) of std_logic_vector(GOAL_HANDLE_WIDTH-1 downto 0); + type RESULT_INDEX_ARRAY_TYPE is array (0 to MAX_GOALS-1) of natural; + type SEQUENCE_ID_ARRAY_TYPE is array (0 to MAX_GOALS-1) of std_logic_vector(ROS_SEQUENCE_ID_WIDTH-1 downto 0); + + constant DEFAULT_SAMPLE_INFO : SAMPLE_INFO_TYPE := ( + sample_state => ANY_SAMPLE_STATE, + view_state => ANY_VIEW_STATE, + instance_state => ANY_INSTANCE_STATE, + source_timestamp => TIME_ZERO, + instance_handle => HANDLE_NIL, + publication_handle => HANDLE_NIL, + disposed_generation_count => (others => '0'), + no_writers_generation_count => (others => '0'), + sample_rank => (others => '0'), + generation_rank => (others => '0'), + absolute_generation_rank => (others => '0'), + valid_data => '1' + ); + + signal clk, reset : std_logic := '0'; + signal start_g, start_r : std_logic := '0'; + signal da_g, da_r : std_logic_vector(STATUS_KIND_WIDTH-1 downto 0) := (others => '0'); + signal ret_sg, ret_sr, ret_cg, ret_cr : std_logic_vector(RETURN_CODE_WIDTH-1 downto 0) := RETCODE_OK; + signal valid_gr, valid_gw, valid_rr, valid_rw, valid_cr, valid_cw, valid_f : std_logic := '0'; + signal ready_gr, ready_gw, ready_rr, ready_rw, ready_cr, ready_cw, ready_f : std_logic := '0'; + signal last_word_gr, last_word_gw, last_word_rr, last_word_rw, last_word_cr, last_word_cw, last_word_f : std_logic := '0'; + signal data_gr, data_gw, data_rr, data_rw, data_cr, data_cw, data_f : std_logic_vector(WORD_WIDTH-1 downto 0) := (others => '0'); + + signal start_c, start_s, ack_c, ack_s, done_c, done_s : std_logic := '0'; + signal opcode_c, opcode_s : ROS_ACTION_OPCODE_TYPE := NOP; + signal return_code_c, return_code_s : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := (others => '0'); + -- ###GENERATED START### + signal goal_order_c, new_goal_order_s : std_logic_vector(CDR_LONG_WIDTH-1 downto 0) := (others => '0'); + signal result_seq_len_c, result_seq_len_s, result_seq_addr_c, result_seq_addr_s : std_logic_vector(R_RR_SEQ_ADDR_WIDTH-1 downto 0) := (others => '0'); + signal result_seq_ready_c, result_seq_ready_s, result_seq_ren_c, result_seq_ren_s, result_seq_wen_s, result_seq_valid_c, result_seq_valid_s, result_seq_ack_c, result_seq_ack_s : std_logic := '0'; + signal result_seq_c, result_seq_r_s, result_seq_w_s : std_logic_vector(CDR_LONG_WIDTH-1 downto 0) := (others => '0'); + signal feedback_seq_len_c, feedback_seq_len_s, feedback_seq_addr_c, feedback_seq_addr_s : std_logic_vector(F_SEQ_ADDR_WIDTH-1 downto 0) := (others => '0'); + signal feedback_seq_ready_c, feedback_seq_ready_s, feedback_seq_ren_c, feedback_seq_ren_s, feedback_seq_wen_s, feedback_seq_valid_c, feedback_seq_valid_s, feedback_seq_ack_c, feedback_seq_ack_s : std_logic := '0'; + signal feedback_seq_c, feedback_seq_r_s, feedback_seq_w_s : std_logic_vector(CDR_LONG_WIDTH-1 downto 0) := (others => '0'); + -- ###GENERATED END### + signal goal_id_c, goal_id_s, result_goal_id_c, feedback_goal_id_c : std_logic_vector(UUID_WIDTH-1 downto 0) := (others => '0'); + signal goal_accepted_c, new_goal_accepted_s : std_logic := '0'; + signal goal_stamp_c, goal_stamp_s : ROS_TIME_TYPE := ROS_TIME_ZERO; + signal result_status_c : std_logic_vector(CDR_INT8_WIDTH-1 downto 0) := (others => '0'); + signal service_info_c : SERVICE_INFO_TYPE := EMPTY_SERVICE_INFO; + signal sequence_id_c : std_logic_vector(ROS_SEQUENCE_ID_WIDTH-1 downto 0) := (others => '0'); + signal goal_handle_in_s, goal_handle_out_s, new_goal_handle_s : std_logic_vector(GOAL_HANDLE_WIDTH-1 downto 0) := (others => '0'); + signal goal_state_in_s, goal_state_out_s : std_logic_vector(CDR_INT8_WIDTH-1 downto 0) := (others => '0'); + signal goal_result_index_s, new_goal_result_index_s, index_result_s : std_logic_vector(WORD_WIDTH-1 downto 0) := (others => '0'); + signal new_goal_request_s, new_goal_response_s, index_result_wen_s, index_result_ready_s : std_logic := '0'; +begin + + uut_c : entity work.Fibonacci_ros_action_client(arch) + generic map ( + ENABLE_FEEDBACK => '0' + ) + port map ( + clk => clk, + reset => reset, + time => ROS_TIME_ZERO, + g_start_r => open, + g_ack_r => '1', + g_opcode_r => open, + g_instance_state_r => open, + g_view_state_r => open, + g_sample_state_r => open, + g_instance_handle_r => open, + g_max_samples_r => open, + g_get_data_r => open, + g_done_r => '1', + g_return_code_r => ret_cg, + g_valid_in_r => valid_gr, + g_ready_in_r => ready_gr, + g_data_in_r => data_gr, + g_last_word_in_r => last_word_gr, + g_sample_info_r => DEFAULT_SAMPLE_INFO, + g_sample_info_valid_r => '1', + g_sample_info_ack_r => open, + g_eoc_r => '1', + g_status_r => (others => '0'), + g_start_w => open, + g_ack_w => '1', + g_opcode_w => open, + g_instance_handle_out_w => open, + g_source_ts_w => open, + g_max_wait_w => open, + g_done_w => '1', + g_return_code_w => RETCODE_OK, + g_instance_handle_in_w => HANDLE_NIL, + g_valid_out_w => valid_gw, + g_ready_out_w => ready_gw, + g_data_out_w => data_gw, + g_last_word_out_w => last_word_gw, + g_valid_in_w => '0', + g_ready_in_w => open, + g_data_in_w => (others => '0'), + g_last_word_in_w => '0', + g_status_w => (others => '0'), + r_start_r => open, + r_ack_r => '1', + r_opcode_r => open, + r_instance_state_r => open, + r_view_state_r => open, + r_sample_state_r => open, + r_instance_handle_r => open, + r_max_samples_r => open, + r_get_data_r => open, + r_done_r => '1', + r_return_code_r => ret_cr, + r_valid_in_r => valid_rr, + r_ready_in_r => ready_rr, + r_data_in_r => data_rr, + r_last_word_in_r => last_word_rr, + r_sample_info_r => DEFAULT_SAMPLE_INFO, + r_sample_info_valid_r => '1', + r_sample_info_ack_r => open, + r_eoc_r => '1', + r_status_r => (others => '0'), + r_start_w => open, + r_ack_w => '1', + r_opcode_w => open, + r_instance_handle_out_w => open, + r_source_ts_w => open, + r_max_wait_w => open, + r_done_w => '1', + r_return_code_w => RETCODE_OK, + r_instance_handle_in_w => HANDLE_NIL, + r_valid_out_w => valid_rw, + r_ready_out_w => ready_rw, + r_data_out_w => data_rw, + r_last_word_out_w => last_word_rw, + r_valid_in_w => '0', + r_ready_in_w => open, + r_data_in_w => (others => '0'), + r_last_word_in_w => '0', + r_status_w => (others => '0'), + c_start_r => open, + c_ack_r => '1', + c_opcode_r => open, + c_instance_state_r => open, + c_view_state_r => open, + c_sample_state_r => open, + c_instance_handle_r => open, + c_max_samples_r => open, + c_get_data_r => open, + c_done_r => '1', + c_return_code_r => RETCODE_NO_DATA, + c_valid_in_r => valid_cr, + c_ready_in_r => ready_cr, + c_data_in_r => data_cr, + c_last_word_in_r => last_word_cr, + c_sample_info_r => DEFAULT_SAMPLE_INFO, + c_sample_info_valid_r => '1', + c_sample_info_ack_r => open, + c_eoc_r => '1', + c_status_r => (others => '0'), + c_start_w => open, + c_ack_w => '1', + c_opcode_w => open, + c_instance_handle_out_w => open, + c_source_ts_w => open, + c_max_wait_w => open, + c_done_w => '1', + c_return_code_w => RETCODE_OK, + c_instance_handle_in_w => HANDLE_NIL, + c_valid_out_w => valid_cw, + c_ready_out_w => ready_cw, + c_data_out_w => data_cw, + c_last_word_out_w => last_word_cw, + c_valid_in_w => '0', + c_ready_in_w => open, + c_data_in_w => (others => '0'), + c_last_word_in_w => '0', + c_status_w => (others => '0'), + f_start_dds => open, + f_ack_dds => '1', + f_opcode_dds => open, + f_instance_state_dds => open, + f_view_state_dds => open, + f_sample_state_dds => open, + f_instance_handle_dds => open, + f_max_samples_dds => open, + f_get_data_dds => open, + f_done_dds => '1', + f_return_code_dds => RETCODE_OK, + f_valid_in_dds => valid_f, + f_ready_in_dds => ready_f, + f_data_in_dds => data_f, + f_last_word_in_dds => last_word_f, + f_sample_info_dds => DEFAULT_SAMPLE_INFO, + f_sample_info_valid_dds => '1', + f_sample_info_ack_dds => open, + f_eoc_dds => '1', + f_status_dds => (others => '0'), + start => start_c, + opcode => opcode_c, + ack => ack_c, + sequence_id => sequence_id_c, + service_info => service_info_c, + done => done_c, + return_code => return_code_c, + goal_id => goal_id_c, + -- ###GENERATED START### + goal_order => goal_order_c, + -- ###GENERATED END### + goal_accepted => goal_accepted_c, + goal_stamp => goal_stamp_c, + result_goal_id => result_goal_id_c, + result_status => result_status_c, + -- ###GENERATED START### + result_seq_len => result_seq_len_c, + result_seq_addr => result_seq_addr_c, + result_seq_ready => result_seq_ready_c, + result_seq_ren => result_seq_ren_c, + result_seq_valid => result_seq_valid_c, + result_seq_ack => result_seq_ack_c, + result_seq => result_seq_c, + -- ###GENERATED END### + cancel_goal_id => (others => '0'), + cancel_stamp => ROS_TIME_ZERO, + cancel_return_code => open, + cancel_goals_canceling_len => open, + cancel_goals_canceling_addr => (others => '0'), + cancel_goals_canceling_ready => open, + cancel_goals_canceling_ren => '0', + cancel_goals_canceling_valid => open, + cancel_goals_canceling_ack => '0', + cancel_goals_canceling_goal_id => open, + cancel_goals_canceling_stamp => open, + -- FEEDBACK SIGNALS + feedback_goal_id => feedback_goal_id_c, + -- ###GENERATED START### + feedback_seq_len => feedback_seq_len_c, + feedback_seq_addr => feedback_seq_addr_c, + feedback_seq_ready => feedback_seq_ready_c, + feedback_seq_ren => feedback_seq_ren_c, + feedback_seq_valid => feedback_seq_valid_c, + feedback_seq_ack => feedback_seq_ack_c, + feedback_seq => feedback_seq_c + -- ###GENERATED END### + ); + + uut_s : entity work.Fibonacci_ros_action_server(arch) + generic map ( + TIMEOUT_DURATION => ROS_DURATION_INFINITE, + MAX_GOALS => MAX_GOALS, + MAX_RESULT_REQUESTS => MAX_GOALS, + ENABLE_FEEDBACK => '0' + ) + port map ( + clk => clk, + reset => reset, + time => ROS_TIME_ZERO, + g_start_r => start_g, + g_ack_r => '1', + g_opcode_r => open, + g_instance_state_r => open, + g_view_state_r => open, + g_sample_state_r => open, + g_instance_handle_r => open, + g_max_samples_r => open, + g_get_data_r => open, + g_done_r => '1', + g_return_code_r => ret_sg, + g_valid_in_r => valid_gw, + g_ready_in_r => ready_gw, + g_data_in_r => data_gw, + g_last_word_in_r => last_word_gw, + g_sample_info_r => DEFAULT_SAMPLE_INFO, + g_sample_info_valid_r => '1', + g_sample_info_ack_r => open, + g_eoc_r => '1', + g_status_r => da_g, + g_start_w => open, + g_ack_w => '1', + g_opcode_w => open, + g_instance_handle_out_w => open, + g_source_ts_w => open, + g_max_wait_w => open, + g_done_w => '1', + g_return_code_w => RETCODE_OK, + g_instance_handle_in_w => HANDLE_NIL, + g_valid_out_w => valid_gr, + g_ready_out_w => ready_gr, + g_data_out_w => data_gr, + g_last_word_out_w => last_word_gr, + g_valid_in_w => '0', + g_ready_in_w => open, + g_data_in_w => (others => '0'), + g_last_word_in_w => '0', + g_status_w => (others => '0'), + r_start_r => start_r, + r_ack_r => '1', + r_opcode_r => open, + r_instance_state_r => open, + r_view_state_r => open, + r_sample_state_r => open, + r_instance_handle_r => open, + r_max_samples_r => open, + r_get_data_r => open, + r_done_r => '1', + r_return_code_r => ret_sr, + r_valid_in_r => valid_rw, + r_ready_in_r => ready_rw, + r_data_in_r => data_rw, + r_last_word_in_r => last_word_rw, + r_sample_info_r => DEFAULT_SAMPLE_INFO, + r_sample_info_valid_r => '1', + r_sample_info_ack_r => open, + r_eoc_r => '1', + r_status_r => da_r, + r_start_w => open, + r_ack_w => '1', + r_opcode_w => open, + r_instance_handle_out_w => open, + r_source_ts_w => open, + r_max_wait_w => open, + r_done_w => '1', + r_return_code_w => RETCODE_OK, + r_instance_handle_in_w => HANDLE_NIL, + r_valid_out_w => valid_rr, + r_ready_out_w => ready_rr, + r_data_out_w => data_rr, + r_last_word_out_w => last_word_rr, + r_valid_in_w => '0', + r_ready_in_w => open, + r_data_in_w => (others => '0'), + r_last_word_in_w => '0', + r_status_w => (others => '0'), + c_start_r => open, + c_ack_r => '1', + c_opcode_r => open, + c_instance_state_r => open, + c_view_state_r => open, + c_sample_state_r => open, + c_instance_handle_r => open, + c_max_samples_r => open, + c_get_data_r => open, + c_done_r => '1', + c_return_code_r => RETCODE_OK, + c_valid_in_r => valid_cw, + c_ready_in_r => ready_cw, + c_data_in_r => data_cw, + c_last_word_in_r => last_word_cw, + c_sample_info_r => DEFAULT_SAMPLE_INFO, + c_sample_info_valid_r => '1', + c_sample_info_ack_r => open, + c_eoc_r => '1', + c_status_r => (others => '0'), + c_start_w => open, + c_ack_w => '1', + c_opcode_w => open, + c_instance_handle_out_w => open, + c_source_ts_w => open, + c_max_wait_w => open, + c_done_w => '1', + c_return_code_w => RETCODE_OK, + c_instance_handle_in_w => HANDLE_NIL, + c_valid_out_w => valid_cr, + c_ready_out_w => ready_cr, + c_data_out_w => data_cr, + c_last_word_out_w => last_word_cr, + c_valid_in_w => '0', + c_ready_in_w => open, + c_data_in_w => (others => '0'), + c_last_word_in_w => '0', + c_status_w => (others => '0'), + f_start_dds => open, + f_ack_dds => '1', + f_opcode_dds => open, + f_instance_handle_out_dds => open, + f_source_ts_dds => open, + f_max_wait_dds => open, + f_done_dds => '1', + f_return_code_dds => RETCODE_OK, + f_instance_handle_in_dds => HANDLE_NIL, + f_valid_out_dds => valid_f, + f_ready_out_dds => ready_f, + f_data_out_dds => data_f, + f_last_word_out_dds => last_word_f, + f_valid_in_dds => '0', + f_ready_in_dds => open, + f_data_in_dds => (others => '0'), + f_last_word_in_dds => '0', + f_status_dds => (others => '0'), + s_start_dds => open, + s_ack_dds => '1', + s_opcode_dds => open, + s_instance_handle_out_dds => open, + s_source_ts_dds => open, + s_max_wait_dds => open, + s_done_dds => '1', + s_return_code_dds => RETCODE_OK, + s_instance_handle_in_dds => HANDLE_NIL, + s_valid_out_dds => open, + s_ready_out_dds => '1', + s_data_out_dds => open, + s_last_word_out_dds => open, + s_valid_in_dds => '0', + s_ready_in_dds => open, + s_data_in_dds => (others => '0'), + s_last_word_in_dds => '0', + s_status_dds => (others => '0'), + start => start_s, + opcode => opcode_s, + ack => ack_s, + done => done_s, + return_code => return_code_s, + goal_handle_in => goal_handle_in_s, + goal_handle_out => goal_handle_out_s, + goal_state_in => goal_state_in_s, + goal_state_out => goal_state_out_s, + goal_id => goal_id_s, + goal_result_index => goal_result_index_s, + goal_stamp => goal_stamp_s, + new_goal_request => new_goal_request_s, + new_goal_handle => new_goal_handle_s, + new_goal_result_index => new_goal_result_index_s, + -- ###GENERATED START### + new_goal_order => new_goal_order_s, + -- ###GENERATED END### + new_goal_accepted => new_goal_accepted_s, + new_goal_response => new_goal_response_s, + cancel_request => open, + cancel_request_handle => open, + cancel_accepted => '1', + cancel_response => '1', + index_result => index_result_s, + index_result_wen => index_result_wen_s, + index_result_ready => index_result_ready_s, + -- ###GENERATED START### + result_seq_len => result_seq_len_s, + result_seq_addr => result_seq_addr_s, + result_seq_ready => result_seq_ready_s, + result_seq_ren => result_seq_ren_s, + result_seq_wen => result_seq_wen_s, + result_seq_valid => result_seq_valid_s, + result_seq_ack => result_seq_ack_s, + result_seq_r => result_seq_r_s, + result_seq_w => result_seq_w_s, + -- ###GENERATED END### + -- ###GENERATED START### + feedback_seq_len => feedback_seq_len_s, + feedback_seq_addr => feedback_seq_addr_s, + feedback_seq_ready => feedback_seq_ready_s, + feedback_seq_ren => feedback_seq_ren_s, + feedback_seq_wen => feedback_seq_wen_s, + feedback_seq_valid => feedback_seq_valid_s, + feedback_seq_ack => feedback_seq_ack_s, + feedback_seq_r => feedback_seq_r_s, + feedback_seq_w => feedback_seq_w_s + -- ###GENERATED END### + ); + + stimulus_prc : process + variable RV : RandomPType; + variable GOAL, RESULT, FEEDBACK : AlertLogIDType; + variable sid : SEQUENCE_ID_ARRAY_TYPE := (others => (others => '0')); + variable test_gid : GOAL_ID_ARRAY_TYPE := (others => (others => '0')); + variable test_handle : GOAL_HANDLE_ARRAY_TYPE := (others => (others => '0')); + variable test_ind : RESULT_INDEX_ARRAY_TYPE := (others => 0); + + impure function gen_goal_id return std_logic_vector is + begin + return RV.RandSlv(UUID_WIDTH); + end function; + + procedure wait_on_sig(signal sig : std_logic) is + begin + if (sig /= '1') then + wait on sig until sig = '1'; + end if; + end procedure; + + procedure client_op(op : in ROS_ACTION_OPCODE_TYPE; ret : in std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0)) is + begin + start_c <= '1'; + opcode_c <= op; + wait_on_sig(ack_c); + wait until rising_edge(clk); + start_c <= '0'; + case (op) is + when SEND_GOAL_REQUEST => + ret_sg <= RETCODE_OK; + da_g <= DATA_AVAILABLE_STATUS; + wait_on_sig(start_g); + da_g <= (others => '0'); + when SEND_RESULT_REQUEST => + ret_sr <= RETCODE_OK; + da_r <= DATA_AVAILABLE_STATUS; + wait_on_sig(start_r); + da_r <= (others => '0'); + when others => + null; + end case; + wait_on_sig(done_c); + case (op) is + when SEND_GOAL_REQUEST => + ret_sg <= RETCODE_NO_DATA; + when SEND_RESULT_REQUEST => + ret_sr <= RETCODE_NO_DATA; + when others => + null; + end case; + wait for 1 ps; -- Make sure all signals stable + AlertIf(return_code_c /= ret, "Unexpected Client Response", FAILURE); + end procedure; + + procedure server_op(op : in ROS_ACTION_OPCODE_TYPE; ret : in std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0)) is + begin + start_s <= '1'; + opcode_s <= op; + wait_on_sig(ack_s); + wait until rising_edge(clk); + start_s <= '0'; + wait_on_sig(done_s); + wait for 1 ps; -- Make sure all signals stable + AlertIf(return_code_s /= ret, "Unexpected Client Response", FAILURE); + end procedure; + begin + + SetAlertLogName("L1_Fibonacci_ros_action_test2 - (Without FEEDBACK) - General"); + SetAlertEnable(FAILURE, TRUE); + SetAlertEnable(ERROR, TRUE); + SetAlertEnable(WARNING, TRUE); + SetLogEnable(DEBUG, FALSE); + SetLogEnable(PASSED, FALSE); + SetLogEnable(INFO, TRUE); + RV.InitSeed(RV'instance_name); + GOAL := GetAlertLogID("GOAL", ALERTLOG_BASE_ID); + RESULT := GetAlertLogID("RESULT", ALERTLOG_BASE_ID); + FEEDBACK := GetAlertLogID("FEEDBACK", ALERTLOG_BASE_ID); + + Log("Initial Reset", INFO); + ret_cg <= RETCODE_NO_DATA; + ret_cr <= RETCODE_NO_DATA; + ret_sg <= RETCODE_NO_DATA; + ret_sr <= RETCODE_NO_DATA; + start_c <= '0'; + start_s <= '0'; + reset <= '1'; + wait until rising_edge(clk); + wait until rising_edge(clk); + reset <= '0'; + + Log("CLIENT: Test Unsupported Opcode", INFO); + client_op(UPDATE_GOAL_STATE,ROS_RET_UNSUPPORTED); + wait until rising_edge(clk); + + Log("SERVER: Test Unsupported Opcode", INFO); + server_op(SEND_GOAL_REQUEST,ROS_RET_UNSUPPORTED); + wait until rising_edge(clk); + + Log("CLIENT: Take Goal Response (No Take)", INFO); + client_op(TAKE_GOAL_RESPONSE,ROS_RET_ACTION_CLIENT_TAKE_FAILED); + wait until rising_edge(clk); + + Log("CLIENT: Take Result Response (No Take)", INFO); + client_op(TAKE_RESULT_RESPONSE,ROS_RET_ACTION_CLIENT_TAKE_FAILED); + wait until rising_edge(clk); + + Log("CLIENT: Take Cancel Response (No Take)", INFO); + client_op(TAKE_CANCEL_RESPONSE,ROS_RET_ACTION_CLIENT_TAKE_FAILED); + wait until rising_edge(clk); + + ret_cg <= RETCODE_OK; + ret_cr <= RETCODE_OK; + + for i in 0 to MAX_GOALS-1 loop + Log("CLIENT: Send GOAL " & integer'image(i) & " Request", INFO); + test_gid(i) := gen_goal_id; + goal_id_c <= test_gid(i); + goal_order_c <= RV.RandSlv(goal_order_c'length); + client_op(SEND_GOAL_REQUEST,ROS_RET_OK); + sid(i) := sequence_id_c; + wait until rising_edge(clk); + + Log("SERVER: Check GOAL " & integer'image(i) & " Request", INFO); + wait until new_goal_request_s = '1'; + AffirmIfEqual(GOAL, new_goal_order_s, goal_order_c); + test_handle(i) := new_goal_handle_s; + test_ind(i) := to_integer(unsigned(new_goal_result_index_s)); + goal_handle_in_s <= test_handle(i); + new_goal_accepted_s <= '1'; + new_goal_response_s <= '1'; + wait until rising_edge(clk); + new_goal_accepted_s <= '0'; + new_goal_response_s <= '0'; + + Log("CLIENT: Take GOAL " & integer'image(i) & " Request", INFO); + client_op(TAKE_GOAL_RESPONSE,ROS_RET_OK); + AlertIf(to_unsigned(SEQUENCENUMBER_TYPE(service_info_c.request_id.sequence_number)) /= unsigned(sid(i)), "Request ID incorrect", FAILURE); + wait until rising_edge(clk); + + goal_handle_in_s <= test_handle(i); + goal_state_in_s <= GoalStatus_package.STATUS_EXECUTING; + server_op(UPDATE_GOAL_STATE,ROS_RET_OK); + wait until rising_edge(clk); + end loop; + + Log("SERVER: Send FEEDBACK (Fail)", INFO); + server_op(PUBLISH_FEEDBACK,ROS_RET_UNSUPPORTED); + wait until rising_edge(clk); + + Log("CLIENT: Take FEEDBACK (Fail)", INFO); + client_op(TAKE_FEEDBACK,ROS_RET_UNSUPPORTED); + wait until rising_edge(clk); + + for i in 0 to MAX_GOALS-1 loop + Log("CLIENT: Send RESULT Request (Goal " & integer'image(i) & ")", INFO); + result_goal_id_c <= test_gid(i); + client_op(SEND_RESULT_REQUEST,ROS_RET_OK); + sid(i) := sequence_id_c; + wait until rising_edge(clk); + end loop; + + for j in 0 to MAX_GOALS-1 loop + Log("SERVER: Set RESULT (Goal " & integer'image(j) & ")", INFO); + index_result_s <= int(j,index_result_s'length); + for i in 0 to RV.RandInt(1,10) loop + result_seq_len_s <= int(i+1,result_seq_len_s'length); + result_seq_addr_s <= int(i,result_seq_addr_s'length); + result_seq_w_s <= RV.RandSlv(result_seq_w_s'length); + wait_on_sig(result_seq_ready_s); + result_seq_wen_s <= '1'; + wait until rising_edge(clk); + result_seq_wen_s <= '0'; + end loop; + wait_on_sig(index_result_ready_s); + index_result_wen_s <= '1'; + wait until rising_edge(clk); + index_result_wen_s <= '0'; + goal_handle_in_s <= test_handle(j); + goal_state_in_s <= GoalStatus_package.STATUS_SUCCEEDED; + server_op(UPDATE_GOAL_STATE,ROS_RET_OK); + wait until rising_edge(clk); + + Log("SERVER: Take RESULT Response (Goal " & integer'image(j) & ")", INFO); + goal_id_c <= test_gid(j); + client_op(TAKE_RESULT_RESPONSE,ROS_RET_OK); + AlertIf(to_unsigned(SEQUENCENUMBER_TYPE(service_info_c.request_id.sequence_number)) /= unsigned(sid(j)), "Request ID incorrect", FAILURE); + AffirmIfEqual(RESULT, result_status_c, GoalStatus_package.STATUS_SUCCEEDED); + AffirmIfEqual(RESULT, result_seq_len_c, result_seq_len_s); + for i in 0 to to_integer(unsigned(result_seq_len_c))-1 loop + result_seq_addr_s <= int(i,result_seq_addr_s'length); + result_seq_addr_c <= int(i,result_seq_addr_c'length); + wait_on_sig(result_seq_ready_s); + wait_on_sig(result_seq_ready_c); + result_seq_ren_s <= '1'; + result_seq_ren_c <= '1'; + wait until rising_edge(clk); + result_seq_ren_s <= '0'; + result_seq_ren_c <= '0'; + wait_on_sig(result_seq_valid_s); + wait_on_sig(result_seq_valid_c); + AffirmIfEqual(RESULT, result_seq_c, result_seq_r_s); + result_seq_ack_s <= '1'; + result_seq_ack_c <= '1'; + wait until rising_edge(clk); + result_seq_ack_s <= '0'; + result_seq_ack_c <= '0'; + end loop; + end loop; + + TranscriptOpen(RESULTS_FILE, APPEND_MODE); + SetTranscriptMirror; + ReportAlerts; + TranscriptClose; + std.env.stop; + wait; + end process; + + clock_prc : process + begin + clk <= '0'; + wait for TEST_CLOCK_PERIOD/2; + clk <= '1'; + wait for TEST_CLOCK_PERIOD/2; + end process; + + watchdog : process + begin + wait for 1 ms; + Alert("Test timeout", FAILURE); + std.env.stop; + end process; + +end architecture; \ No newline at end of file diff --git a/src/ros2/Tests/ros_testbench.pro b/src/ros2/Tests/ros_testbench.pro index 753226d..6fb2bbf 100644 --- a/src/ros2/Tests/ros_testbench.pro +++ b/src/ros2/Tests/ros_testbench.pro @@ -119,8 +119,12 @@ analyze ../example_interfaces/Fibonacci_ros_action_result_srv_client.vhd analyze Level_1/L1_Fibonacci_ros_action_result_srv_test1.vhd analyze Level_1/L1_Fibonacci_ros_action_result_srv_test2.vhd analyze ../ros_action_server.vhd +analyze ../example_interfaces/Fibonacci_ros_action_server.vhd +analyze ../example_interfaces/Fibonacci_ros_action_client.vhd analyze Level_0/L0_ros_action_server_test1.vhd analyze Level_0/L0_ros_action_server_test2.vhd +analyze Level_1/L1_Fibonacci_ros_action_test1.vhd +analyze Level_1/L1_Fibonacci_ros_action_test2.vhd simulate L1_AddTwoInts_ros_srv_test1 @@ -137,4 +141,6 @@ simulate L1_Fibonacci_ros_action_goal_srv_test2 simulate L1_Fibonacci_ros_action_result_srv_test1 simulate L1_Fibonacci_ros_action_result_srv_test2 simulate L0_ros_action_server_test1 -simulate L0_ros_action_server_test2 \ No newline at end of file +simulate L0_ros_action_server_test2 +simulate L1_Fibonacci_ros_action_test1 +simulate L1_Fibonacci_ros_action_test2 \ No newline at end of file diff --git a/src/ros2/example_interfaces/Fibonacci_ros_action_client.vhd b/src/ros2/example_interfaces/Fibonacci_ros_action_client.vhd new file mode 100644 index 0000000..13ae11a --- /dev/null +++ b/src/ros2/example_interfaces/Fibonacci_ros_action_client.vhd @@ -0,0 +1,769 @@ +-- 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.math_pkg.all; +use work.rtps_package.all; +use work.ros_package.all; +use work.Fibonacci_package.all; +use work.GoalStatus_package; +use work.GoalStatusArray_package; +use work.CancelGoal_package; + +entity Fibonacci_ros_action_client is + generic ( + ENABLE_FEEDBACK : std_logic := '1'; + LITTLE_ENDIAN : std_logic := '0' + ); + port ( + -- SYSTEM + clk : in std_logic; + reset : in std_logic; + time : in ROS_TIME_TYPE; + -- *GOAL SERVICE* + -- DDS READER + g_start_r : out std_logic; + g_ack_r : in std_logic; + g_opcode_r : out DDS_READER_OPCODE_TYPE; + g_instance_state_r : out std_logic_vector(INSTANCE_STATE_KIND_WIDTH-1 downto 0); + g_view_state_r : out std_logic_vector(VIEW_STATE_KIND_WIDTH-1 downto 0); + g_sample_state_r : out std_logic_vector(SAMPLE_STATE_KIND_WIDTH-1 downto 0); + g_instance_handle_r : out INSTANCE_HANDLE_TYPE; + g_max_samples_r : out std_logic_vector(MAX_SAMPLES_WIDTH-1 downto 0); + g_get_data_r : out std_logic; + g_done_r : in std_logic; + g_return_code_r : in std_logic_vector(RETURN_CODE_WIDTH-1 downto 0); + g_valid_in_r : in std_logic; + g_ready_in_r : out std_logic; + g_data_in_r : in std_logic_vector(WORD_WIDTH-1 downto 0); + g_last_word_in_r : in std_logic; + g_sample_info_r : in SAMPLE_INFO_TYPE; + g_sample_info_valid_r : in std_logic; + g_sample_info_ack_r : out std_logic; + g_eoc_r : in std_logic; + g_status_r : in std_logic_vector(STATUS_KIND_WIDTH-1 downto 0); + -- DDS WRITER + g_start_w : out std_logic; + g_ack_w : in std_logic; + g_opcode_w : out DDS_WRITER_OPCODE_TYPE; + g_instance_handle_out_w : out INSTANCE_HANDLE_TYPE; + g_source_ts_w : out TIME_TYPE; + g_max_wait_w : out DURATION_TYPE; + g_done_w : in std_logic; + g_return_code_w : in std_logic_vector(RETURN_CODE_WIDTH-1 downto 0); + g_instance_handle_in_w : in INSTANCE_HANDLE_TYPE; + g_valid_out_w : out std_logic; + g_ready_out_w : in std_logic; + g_data_out_w : out std_logic_vector(WORD_WIDTH-1 downto 0); + g_last_word_out_w : out std_logic; + g_valid_in_w : in std_logic; + g_ready_in_w : out std_logic; + g_data_in_w : in std_logic_vector(WORD_WIDTH-1 downto 0); + g_last_word_in_w : in std_logic; + g_status_w : in std_logic_vector(STATUS_KIND_WIDTH-1 downto 0); + -- *RESULT SERVICE* + -- DDS READER + r_start_r : out std_logic; + r_ack_r : in std_logic; + r_opcode_r : out DDS_READER_OPCODE_TYPE; + r_instance_state_r : out std_logic_vector(INSTANCE_STATE_KIND_WIDTH-1 downto 0); + r_view_state_r : out std_logic_vector(VIEW_STATE_KIND_WIDTH-1 downto 0); + r_sample_state_r : out std_logic_vector(SAMPLE_STATE_KIND_WIDTH-1 downto 0); + r_instance_handle_r : out INSTANCE_HANDLE_TYPE; + r_max_samples_r : out std_logic_vector(MAX_SAMPLES_WIDTH-1 downto 0); + r_get_data_r : out std_logic; + r_done_r : in std_logic; + r_return_code_r : in std_logic_vector(RETURN_CODE_WIDTH-1 downto 0); + r_valid_in_r : in std_logic; + r_ready_in_r : out std_logic; + r_data_in_r : in std_logic_vector(WORD_WIDTH-1 downto 0); + r_last_word_in_r : in std_logic; + r_sample_info_r : in SAMPLE_INFO_TYPE; + r_sample_info_valid_r : in std_logic; + r_sample_info_ack_r : out std_logic; + r_eoc_r : in std_logic; + r_status_r : in std_logic_vector(STATUS_KIND_WIDTH-1 downto 0); + -- DDS WRITER + r_start_w : out std_logic; + r_ack_w : in std_logic; + r_opcode_w : out DDS_WRITER_OPCODE_TYPE; + r_instance_handle_out_w : out INSTANCE_HANDLE_TYPE; + r_source_ts_w : out TIME_TYPE; + r_max_wait_w : out DURATION_TYPE; + r_done_w : in std_logic; + r_return_code_w : in std_logic_vector(RETURN_CODE_WIDTH-1 downto 0); + r_instance_handle_in_w : in INSTANCE_HANDLE_TYPE; + r_valid_out_w : out std_logic; + r_ready_out_w : in std_logic; + r_data_out_w : out std_logic_vector(WORD_WIDTH-1 downto 0); + r_last_word_out_w : out std_logic; + r_valid_in_w : in std_logic; + r_ready_in_w : out std_logic; + r_data_in_w : in std_logic_vector(WORD_WIDTH-1 downto 0); + r_last_word_in_w : in std_logic; + r_status_w : in std_logic_vector(STATUS_KIND_WIDTH-1 downto 0); + -- *CANCEL SERVICE* + -- DDS READER + c_start_r : out std_logic; + c_ack_r : in std_logic; + c_opcode_r : out DDS_READER_OPCODE_TYPE; + c_instance_state_r : out std_logic_vector(INSTANCE_STATE_KIND_WIDTH-1 downto 0); + c_view_state_r : out std_logic_vector(VIEW_STATE_KIND_WIDTH-1 downto 0); + c_sample_state_r : out std_logic_vector(SAMPLE_STATE_KIND_WIDTH-1 downto 0); + c_instance_handle_r : out INSTANCE_HANDLE_TYPE; + c_max_samples_r : out std_logic_vector(MAX_SAMPLES_WIDTH-1 downto 0); + c_get_data_r : out std_logic; + c_done_r : in std_logic; + c_return_code_r : in std_logic_vector(RETURN_CODE_WIDTH-1 downto 0); + c_valid_in_r : in std_logic; + c_ready_in_r : out std_logic; + c_data_in_r : in std_logic_vector(WORD_WIDTH-1 downto 0); + c_last_word_in_r : in std_logic; + c_sample_info_r : in SAMPLE_INFO_TYPE; + c_sample_info_valid_r : in std_logic; + c_sample_info_ack_r : out std_logic; + c_eoc_r : in std_logic; + c_status_r : in std_logic_vector(STATUS_KIND_WIDTH-1 downto 0); + -- DDS WRITER + c_start_w : out std_logic; + c_ack_w : in std_logic; + c_opcode_w : out DDS_WRITER_OPCODE_TYPE; + c_instance_handle_out_w : out INSTANCE_HANDLE_TYPE; + c_source_ts_w : out TIME_TYPE; + c_max_wait_w : out DURATION_TYPE; + c_done_w : in std_logic; + c_return_code_w : in std_logic_vector(RETURN_CODE_WIDTH-1 downto 0); + c_instance_handle_in_w : in INSTANCE_HANDLE_TYPE; + c_valid_out_w : out std_logic; + c_ready_out_w : in std_logic; + c_data_out_w : out std_logic_vector(WORD_WIDTH-1 downto 0); + c_last_word_out_w : out std_logic; + c_valid_in_w : in std_logic; + c_ready_in_w : out std_logic; + c_data_in_w : in std_logic_vector(WORD_WIDTH-1 downto 0); + c_last_word_in_w : in std_logic; + c_status_w : in std_logic_vector(STATUS_KIND_WIDTH-1 downto 0); + -- *FEEDBACK SUBSCRIBER* + f_start_dds : out std_logic; + f_ack_dds : in std_logic; + f_opcode_dds : out DDS_READER_OPCODE_TYPE; + f_instance_state_dds : out std_logic_vector(INSTANCE_STATE_KIND_WIDTH-1 downto 0); + f_view_state_dds : out std_logic_vector(VIEW_STATE_KIND_WIDTH-1 downto 0); + f_sample_state_dds : out std_logic_vector(SAMPLE_STATE_KIND_WIDTH-1 downto 0); + f_instance_handle_dds : out INSTANCE_HANDLE_TYPE; + f_max_samples_dds : out std_logic_vector(MAX_SAMPLES_WIDTH-1 downto 0); + f_get_data_dds : out std_logic; + f_done_dds : in std_logic; + f_return_code_dds : in std_logic_vector(RETURN_CODE_WIDTH-1 downto 0); + f_valid_in_dds : in std_logic; + f_ready_in_dds : out std_logic; + f_data_in_dds : in std_logic_vector(WORD_WIDTH-1 downto 0); + f_last_word_in_dds : in std_logic; + f_sample_info_dds : in SAMPLE_INFO_TYPE; + f_sample_info_valid_dds : in std_logic; + f_sample_info_ack_dds : out std_logic; + f_eoc_dds : in std_logic; + f_status_dds : in std_logic_vector(STATUS_KIND_WIDTH-1 downto 0); + + -- *USER* + start : in std_logic; + opcode : in ROS_ACTION_OPCODE_TYPE; + ack : out std_logic; + sequence_id : out std_logic_vector(ROS_SEQUENCE_ID_WIDTH-1 downto 0); + service_info : out SERVICE_INFO_TYPE; + done : out std_logic; + return_code : out std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0); + -- GOAL SIGNALS + goal_id : in std_logic_vector(UUID_WIDTH-1 downto 0); + -- ###GENERATED START### + goal_order : in std_logic_vector(CDR_LONG_WIDTH-1 downto 0); + -- ###GENERATED END### + goal_accepted : out std_logic; + goal_stamp : out ROS_TIME_TYPE; + -- RESULT SIGNALS + result_goal_id : in std_logic_vector(UUID_WIDTH-1 downto 0); + result_status : out std_logic_vector(CDR_INT8_WIDTH-1 downto 0); + -- ###GENERATED START### + result_seq_len : out std_logic_vector(R_RR_SEQ_ADDR_WIDTH-1 downto 0); + result_seq_addr : in std_logic_vector(R_RR_SEQ_ADDR_WIDTH-1 downto 0); + result_seq_ready : out std_logic; + result_seq_ren : in std_logic; + result_seq_valid : out std_logic; + result_seq_ack : in std_logic; + result_seq : out std_logic_vector(CDR_LONG_WIDTH-1 downto 0); + -- ###GENERATED END### + -- CANCEL + cancel_goal_id : in std_logic_vector(UUID_WIDTH-1 downto 0); + cancel_stamp : in ROS_TIME_TYPE; + cancel_return_code : out std_logic_vector(CDR_INT8_WIDTH-1 downto 0); + cancel_goals_canceling_len : out std_logic_vector(CancelGoal_package.RR_GOALS_CANCELING_ADDR_WIDTH-1 downto 0); + cancel_goals_canceling_addr : in std_logic_vector(CancelGoal_package.RR_GOALS_CANCELING_ADDR_WIDTH-1 downto 0); + cancel_goals_canceling_ready : out std_logic; + cancel_goals_canceling_ren : in std_logic; + cancel_goals_canceling_valid : out std_logic; + cancel_goals_canceling_ack : in std_logic; + cancel_goals_canceling_goal_id : out std_logic_vector(UUID_WIDTH-1 downto 0); + cancel_goals_canceling_stamp : out std_logic_vector(ROS_TIME_WIDTH-1 downto 0); + -- FEEDBACK SIGNALS + feedback_goal_id : out std_logic_vector(UUID_WIDTH-1 downto 0); + -- ###GENERATED START### + feedback_seq_len : out std_logic_vector(F_SEQ_ADDR_WIDTH-1 downto 0); + feedback_seq_addr : in std_logic_vector(F_SEQ_ADDR_WIDTH-1 downto 0); + feedback_seq_ready : out std_logic; + feedback_seq_ren : in std_logic; + feedback_seq_valid : out std_logic; + feedback_seq_ack : in std_logic; + feedback_seq : out std_logic_vector(CDR_LONG_WIDTH-1 downto 0) + -- ###GENERATED END### + ); +end entity; + +architecture arch of Fibonacci_ros_action_client is + + --*****TYPE DECLARATION***** + type STAGE_TYPE is (IDLE,SEND_GOAL_REQUEST,TAKE_GOAL_RESPONSE,SEND_RESULT_REQUEST,TAKE_RESULT_RESPONSE,SEND_CANCEL_REQUEST,TAKE_CANCEL_RESPONSE,TAKE_FEEDBACK,RETURN_ROS); + + --*****SIGNAL DECLARATION***** + signal stage, stage_next : STAGE_TYPE; + signal cnt, cnt_next : natural range 0 to 5; + signal g_start_user, r_start_user, c_start_user, g_ack_user, r_ack_user, c_ack_user, g_data_available_user, r_data_available_user, c_data_available_user, g_taken_user, r_taken_user, c_taken_user, g_done_user, r_done_user, c_done_user : std_logic; + signal g_opcode_user, r_opcode_user, c_opcode_user : ROS_SERVICE_OPCODE_TYPE; + signal g_service_info_user, r_service_info_user, c_service_info_user : SERVICE_INFO_TYPE; + signal g_sequence_id_user, r_sequence_id_user, c_sequence_id_user : std_logic_vector(ROS_SEQUENCE_ID_WIDTH-1 downto 0); + signal g_return_code_user, r_return_code_user, c_return_code_user : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0); + + signal f_start_user, f_ack_user, f_done_user, f_taken_user : std_logic; + signal f_opcode_user : ROS_TOPIC_OPCODE_TYPE; + signal f_return_code_user : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0); + signal f_message_info_user : MESSAGE_INFO_TYPE; + + signal return_code_latch, return_code_latch_next : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0); + + signal goal_stamp_sig : std_logic_vector(ROS_TIME_WIDTH-1 downto 0); + +begin + + goal_srv_client_inst : entity work.Fibonacci_ros_action_goal_srv_client(arch) + generic map ( + LITTLE_ENDIAN => LITTLE_ENDIAN + ) + port map ( + clk => clk, + reset => reset, + start_r => g_start_r, + ack_r => g_ack_r, + opcode_r => g_opcode_r, + instance_state_r => g_instance_state_r, + view_state_r => g_view_state_r, + sample_state_r => g_sample_state_r, + instance_handle_r => g_instance_handle_r, + max_samples_r => g_max_samples_r, + get_data_r => g_get_data_r, + done_r => g_done_r, + return_code_r => g_return_code_r, + valid_in_r => g_valid_in_r, + ready_in_r => g_ready_in_r, + data_in_r => g_data_in_r, + last_word_in_r => g_last_word_in_r, + sample_info_r => g_sample_info_r, + sample_info_valid_r => g_sample_info_valid_r, + sample_info_ack_r => g_sample_info_ack_r, + eoc_r => g_eoc_r, + status_r => g_status_r, + start_w => g_start_w, + ack_w => g_ack_w, + opcode_w => g_opcode_w, + instance_handle_out_w => g_instance_handle_out_w, + source_ts_w => g_source_ts_w, + max_wait_w => g_max_wait_w, + done_w => g_done_w, + return_code_w => g_return_code_w, + instance_handle_in_w => g_instance_handle_in_w, + valid_out_w => g_valid_out_w, + ready_out_w => g_ready_out_w, + data_out_w => g_data_out_w, + last_word_out_w => g_last_word_out_w, + valid_in_w => g_valid_in_w, + ready_in_w => g_ready_in_w, + data_in_w => g_data_in_w, + last_word_in_w => g_last_word_in_w, + status_w => g_status_w, + start_user => g_start_user, + ack_user => g_ack_user, + opcode_user => g_opcode_user, + done_user => g_done_user, + return_code_user => g_return_code_user, + service_info_user => g_service_info_user, + sequence_id_user => g_sequence_id_user, + data_available_user => g_data_available_user, + taken_user => g_taken_user, + goal_id => goal_id, + -- ###GENERATED START### + order => goal_order, + -- ###GENERATED END### + accepted => goal_accepted, + stamp => goal_stamp_sig + ); + + goal_stamp <= to_ROS_TIME(goal_stamp_sig); + + result_srv_client_inst : entity work.Fibonacci_ros_action_result_srv_client(arch) + generic map ( + LITTLE_ENDIAN => LITTLE_ENDIAN + ) + port map ( + clk => clk, + reset => reset, + start_r => r_start_r, + ack_r => r_ack_r, + opcode_r => r_opcode_r, + instance_state_r => r_instance_state_r, + view_state_r => r_view_state_r, + sample_state_r => r_sample_state_r, + instance_handle_r => r_instance_handle_r, + max_samples_r => r_max_samples_r, + get_data_r => r_get_data_r, + done_r => r_done_r, + return_code_r => r_return_code_r, + valid_in_r => r_valid_in_r, + ready_in_r => r_ready_in_r, + data_in_r => r_data_in_r, + last_word_in_r => r_last_word_in_r, + sample_info_r => r_sample_info_r, + sample_info_valid_r => r_sample_info_valid_r, + sample_info_ack_r => r_sample_info_ack_r, + eoc_r => r_eoc_r, + status_r => r_status_r, + start_w => r_start_w, + ack_w => r_ack_w, + opcode_w => r_opcode_w, + instance_handle_out_w => r_instance_handle_out_w, + source_ts_w => r_source_ts_w, + max_wait_w => r_max_wait_w, + done_w => r_done_w, + return_code_w => r_return_code_w, + instance_handle_in_w => r_instance_handle_in_w, + valid_out_w => r_valid_out_w, + ready_out_w => r_ready_out_w, + data_out_w => r_data_out_w, + last_word_out_w => r_last_word_out_w, + valid_in_w => r_valid_in_w, + ready_in_w => r_ready_in_w, + data_in_w => r_data_in_w, + last_word_in_w => r_last_word_in_w, + status_w => r_status_w, + start_user => r_start_user, + ack_user => r_ack_user, + opcode_user => r_opcode_user, + done_user => r_done_user, + return_code_user => r_return_code_user, + service_info_user => r_service_info_user, + sequence_id_user => r_sequence_id_user, + data_available_user => r_data_available_user, + taken_user => r_taken_user, + goal_id => result_goal_id, + status => result_status, + -- ###GENERATED START### + seq_len => result_seq_len, + seq_addr => result_seq_addr, + seq_ready => result_seq_ready, + seq_ren => result_seq_ren, + seq_valid => result_seq_valid, + seq_ack => result_seq_ack, + seq => result_seq + -- ###GENERATED END### + ); + + cancel_srv_client_inst : entity work.CancelGoal_ros_srv_client(arch) + generic map ( + LITTLE_ENDIAN => LITTLE_ENDIAN + ) + port map ( + clk => clk, + reset => reset, + start_r => c_start_r, + ack_r => c_ack_r, + opcode_r => c_opcode_r, + instance_state_r => c_instance_state_r, + view_state_r => c_view_state_r, + sample_state_r => c_sample_state_r, + instance_handle_r => c_instance_handle_r, + max_samples_r => c_max_samples_r, + get_data_r => c_get_data_r, + done_r => c_done_r, + return_code_r => c_return_code_r, + valid_in_r => c_valid_in_r, + ready_in_r => c_ready_in_r, + data_in_r => c_data_in_r, + last_word_in_r => c_last_word_in_r, + sample_info_r => c_sample_info_r, + sample_info_valid_r => c_sample_info_valid_r, + sample_info_ack_r => c_sample_info_ack_r, + eoc_r => c_eoc_r, + status_r => c_status_r, + start_w => c_start_w, + ack_w => c_ack_w, + opcode_w => c_opcode_w, + instance_handle_out_w => c_instance_handle_out_w, + source_ts_w => c_source_ts_w, + max_wait_w => c_max_wait_w, + done_w => c_done_w, + return_code_w => c_return_code_w, + instance_handle_in_w => c_instance_handle_in_w, + valid_out_w => c_valid_out_w, + ready_out_w => c_ready_out_w, + data_out_w => c_data_out_w, + last_word_out_w => c_last_word_out_w, + valid_in_w => c_valid_in_w, + ready_in_w => c_ready_in_w, + data_in_w => c_data_in_w, + last_word_in_w => c_last_word_in_w, + status_w => c_status_w, + start_user => c_start_user, + ack_user => c_ack_user, + opcode_user => c_opcode_user, + done_user => c_done_user, + return_code_user => c_return_code_user, + service_info_user => c_service_info_user, + sequence_id_user => c_sequence_id_user, + data_available_user => c_data_available_user, + taken_user => c_taken_user, + goal_info_goal_id => cancel_goal_id, + goal_info_stamp => std_logic_vector(to_unsigned(cancel_stamp)), + return_code => cancel_return_code, + goals_canceling_len => cancel_goals_canceling_len, + goals_canceling_addr => cancel_goals_canceling_addr, + goals_canceling_ready => cancel_goals_canceling_ready, + goals_canceling_ren => cancel_goals_canceling_ren, + goals_canceling_valid => cancel_goals_canceling_valid, + goals_canceling_ack => cancel_goals_canceling_ack, + goals_canceling_goal_id => cancel_goals_canceling_goal_id, + goals_canceling_stamp => cancel_goals_canceling_stamp + ); + + feedback_gen : if ENABLE_FEEDBACK = '1' generate + feedback_sub_inst : entity work.Fibonacci_ros_action_feedback_sub(arch) + port map ( + clk => clk, + reset => reset, + start_dds => f_start_dds, + ack_dds => f_ack_dds, + opcode_dds => f_opcode_dds, + instance_state_dds => f_instance_state_dds, + view_state_dds => f_view_state_dds, + sample_state_dds => f_sample_state_dds, + instance_handle_dds => f_instance_handle_dds, + max_samples_dds => f_max_samples_dds, + get_data_dds => f_get_data_dds, + done_dds => f_done_dds, + return_code_dds => f_return_code_dds, + valid_in_dds => f_valid_in_dds, + ready_in_dds => f_ready_in_dds, + data_in_dds => f_data_in_dds, + last_word_in_dds => f_last_word_in_dds, + sample_info_dds => f_sample_info_dds, + sample_info_valid_dds => f_sample_info_valid_dds, + sample_info_ack_dds => f_sample_info_ack_dds, + eoc_dds => f_eoc_dds, + status_dds => f_status_dds, + start_user => f_start_user, + opcode_user => f_opcode_user, + ack_user => f_ack_user, + done_user => f_done_user, + return_code_user => f_return_code_user, + message_info_user => f_message_info_user, + taken_user => f_taken_user, + -- ###GENERATED START### + goal_id => feedback_goal_id, + seq_len => feedback_seq_len, + seq_addr => feedback_seq_addr, + seq_ready => feedback_seq_ready, + seq_ren => feedback_seq_ren, + seq_valid => feedback_seq_valid, + seq_ack => feedback_seq_ack, + seq => feedback_seq + -- ###GENERATED END### + ); + else generate + f_start_dds <= '0'; + f_opcode_dds <= NOP; + f_instance_state_dds <= ANY_INSTANCE_STATE; + f_view_state_dds <= ANY_VIEW_STATE; + f_sample_state_dds <= ANY_SAMPLE_STATE; + f_instance_handle_dds <= HANDLE_NIL; + f_max_samples_dds <= (others => '0'); + f_get_data_dds <= '0'; + f_ready_in_dds <= '0'; + f_sample_info_ack_dds <= '0'; + feedback_goal_id <= (others => '0'); + -- ###GENERATED START### + feedback_seq_len <= (others => '0'); + feedback_seq_ready <= '0'; + feedback_seq_valid <= '0'; + feedback_seq <= (others => '0'); + -- ###GENERATED END### + end generate; + + + main_prc : process(all) + begin + -- DEFAULT + stage_next <= stage; + cnt_next <= cnt; + return_code_latch_next <= return_code_latch; + -- DEFAULT Unregistered + f_start_user <= '0'; + f_opcode_user <= NOP; + g_start_user <= '0'; + r_opcode_user <= NOP; + r_start_user <= '0'; + g_opcode_user <= NOP; + c_start_user <= '0'; + c_opcode_user <= NOP; + sequence_id <= (others => '0'); + service_info <= EMPTY_SERVICE_INFO; + ack <= '0'; + done <= '0'; + return_code <= ROS_RET_OK; + + case (stage) is + when IDLE => + if (start = '1') then + ack <= '1'; + case (opcode) is + when SEND_GOAL_REQUEST => + stage_next <= SEND_GOAL_REQUEST; + cnt_next <= 0; + when TAKE_GOAL_RESPONSE => + stage_next <= TAKE_GOAL_RESPONSE; + cnt_next <= 0; + when SEND_RESULT_REQUEST => + stage_next <= SEND_RESULT_REQUEST; + cnt_next <= 0; + when TAKE_RESULT_RESPONSE => + stage_next <= TAKE_RESULT_RESPONSE; + cnt_next <= 0; + when SEND_CANCEL_REQUEST => + stage_next <= SEND_CANCEL_REQUEST; + cnt_next <= 0; + when TAKE_CANCEL_RESPONSE => + stage_next <= TAKE_CANCEL_RESPONSE; + cnt_next <= 0; + when TAKE_FEEDBACK => + -- SYNTHESIS GUARD + if (ENABLE_FEEDBACK = '1') then + stage_next <= TAKE_FEEDBACK; + cnt_next <= 0; + else + return_code_latch_next <= ROS_RET_UNSUPPORTED; + stage_next <= RETURN_ROS; + end if; + when others => + return_code_latch_next <= ROS_RET_UNSUPPORTED; + stage_next <= RETURN_ROS; + end case; + end if; + when SEND_GOAL_REQUEST => + case (cnt) is + -- Take + when 0 => + g_start_user <= '1'; + g_opcode_user <= SEND_REQUEST; + + if (g_ack_user = '1') then + cnt_next <= cnt + 1; + end if; + -- Wait for Subscriber + when 1 => + sequence_id <= g_sequence_id_user; + done <= g_done_user; + return_code <= g_return_code_user; + + if (g_done_user = '1') then + -- DONE + stage_next <= IDLE; + end if; + when others => + null; + end case; + when TAKE_GOAL_RESPONSE => + case (cnt) is + -- Take + when 0 => + g_start_user <= '1'; + g_opcode_user <= TAKE_RESPONSE; + + if (g_ack_user = '1') then + cnt_next <= cnt + 1; + end if; + -- Wait for Subscriber + when 1 => + service_info <= g_service_info_user; + return_code <= g_return_code_user; + + if (g_done_user = '1') then + if (g_taken_user = '1') then + done <= g_done_user; + stage_next <= IDLE; + else + return_code_latch_next <= ROS_RET_ACTION_CLIENT_TAKE_FAILED; + stage_next <= RETURN_ROS; + end if; + end if; + when others => + null; + end case; + when SEND_RESULT_REQUEST => + case (cnt) is + -- Take + when 0 => + r_start_user <= '1'; + r_opcode_user <= SEND_REQUEST; + + if (r_ack_user = '1') then + cnt_next <= cnt + 1; + end if; + -- Wait for Subscriber + when 1 => + sequence_id <= r_sequence_id_user; + done <= r_done_user; + return_code <= r_return_code_user; + + if (r_done_user = '1') then + -- DONE + stage_next <= IDLE; + end if; + when others => + null; + end case; + when TAKE_RESULT_RESPONSE => + case (cnt) is + -- Take + when 0 => + r_start_user <= '1'; + r_opcode_user <= TAKE_RESPONSE; + + if (r_ack_user = '1') then + cnt_next <= cnt + 1; + end if; + -- Wait for Subscriber + when 1 => + service_info <= r_service_info_user; + return_code <= r_return_code_user; + + if (r_done_user = '1') then + if (r_taken_user = '1') then + done <= r_done_user; + stage_next <= IDLE; + else + return_code_latch_next <= ROS_RET_ACTION_CLIENT_TAKE_FAILED; + stage_next <= RETURN_ROS; + end if; + end if; + when others => + null; + end case; + when SEND_CANCEL_REQUEST => + case (cnt) is + -- Take + when 0 => + c_start_user <= '1'; + c_opcode_user <= SEND_REQUEST; + + if (c_ack_user = '1') then + cnt_next <= cnt + 1; + end if; + -- Wait for Subscriber + when 1 => + sequence_id <= c_sequence_id_user; + done <= c_done_user; + return_code <= c_return_code_user; + + if (c_done_user = '1') then + -- DONE + stage_next <= IDLE; + end if; + when others => + null; + end case; + when TAKE_CANCEL_RESPONSE => + case (cnt) is + -- Take + when 0 => + c_start_user <= '1'; + c_opcode_user <= TAKE_RESPONSE; + + if (c_ack_user = '1') then + cnt_next <= cnt + 1; + end if; + -- Wait for Subscriber + when 1 => + service_info <= c_service_info_user; + return_code <= c_return_code_user; + + if (c_done_user = '1') then + if (c_taken_user = '1') then + done <= c_done_user; + stage_next <= IDLE; + else + return_code_latch_next <= ROS_RET_ACTION_CLIENT_TAKE_FAILED; + stage_next <= RETURN_ROS; + end if; + end if; + when others => + null; + end case; + when TAKE_FEEDBACK => + case (cnt) is + -- Take + when 0 => + f_start_user <= '1'; + f_opcode_user <= TAKE; + + if (f_ack_user = '1') then + cnt_next <= cnt + 1; + end if; + -- Wait for Subscriber + when 1 => + if (f_done_user = '1') then + return_code <= f_return_code_user; + + if (f_taken_user = '1') then + done <= f_done_user; + -- DONE + stage_next <= IDLE; + else + return_code_latch_next <= ROS_RET_ACTION_CLIENT_TAKE_FAILED; + stage_next <= RETURN_ROS; + end if; + end if; + when others => + null; + end case; + when RETURN_ROS => + done <= '1'; + return_code <= return_code_latch; + -- DONE + stage_next <= IDLE; + end case; + end process; + + sync_prc : process(clk) + begin + if rising_edge(clk) then + if (reset = '1') then + stage <= IDLE; + cnt <= 0; + return_code_latch <= ROS_RET_OK; + else + stage <= stage_next; + cnt <= cnt_next; + return_code_latch <= return_code_latch_next; + end if; + end if; + end process; + +end architecture; diff --git a/src/ros2/example_interfaces/Fibonacci_ros_action_server.vhd b/src/ros2/example_interfaces/Fibonacci_ros_action_server.vhd new file mode 100644 index 0000000..15a63a3 --- /dev/null +++ b/src/ros2/example_interfaces/Fibonacci_ros_action_server.vhd @@ -0,0 +1,922 @@ +-- 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.math_pkg.all; +use work.rtps_package.all; +use work.ros_package.all; +use work.Fibonacci_package.all; +use work.GoalStatus_package; +use work.GoalStatusArray_package; +use work.CancelGoal_package; + +entity Fibonacci_ros_action_server is + generic ( + TIMEOUT_DURATION : ROS_DURATION_TYPE; + MAX_GOALS : natural; + MAX_RESULT_REQUESTS : natural; + ENABLE_FEEDBACK : std_logic := '1'; + LITTLE_ENDIAN : std_logic := '0' + ); + port ( + -- SYSTEM + clk : in std_logic; + reset : in std_logic; + time : in ROS_TIME_TYPE; + -- *GOAL SERVICE* + -- DDS READER + g_start_r : out std_logic; + g_ack_r : in std_logic; + g_opcode_r : out DDS_READER_OPCODE_TYPE; + g_instance_state_r : out std_logic_vector(INSTANCE_STATE_KIND_WIDTH-1 downto 0); + g_view_state_r : out std_logic_vector(VIEW_STATE_KIND_WIDTH-1 downto 0); + g_sample_state_r : out std_logic_vector(SAMPLE_STATE_KIND_WIDTH-1 downto 0); + g_instance_handle_r : out INSTANCE_HANDLE_TYPE; + g_max_samples_r : out std_logic_vector(MAX_SAMPLES_WIDTH-1 downto 0); + g_get_data_r : out std_logic; + g_done_r : in std_logic; + g_return_code_r : in std_logic_vector(RETURN_CODE_WIDTH-1 downto 0); + g_valid_in_r : in std_logic; + g_ready_in_r : out std_logic; + g_data_in_r : in std_logic_vector(WORD_WIDTH-1 downto 0); + g_last_word_in_r : in std_logic; + g_sample_info_r : in SAMPLE_INFO_TYPE; + g_sample_info_valid_r : in std_logic; + g_sample_info_ack_r : out std_logic; + g_eoc_r : in std_logic; + g_status_r : in std_logic_vector(STATUS_KIND_WIDTH-1 downto 0); + -- DDS WRITER + g_start_w : out std_logic; + g_ack_w : in std_logic; + g_opcode_w : out DDS_WRITER_OPCODE_TYPE; + g_instance_handle_out_w : out INSTANCE_HANDLE_TYPE; + g_source_ts_w : out TIME_TYPE; + g_max_wait_w : out DURATION_TYPE; + g_done_w : in std_logic; + g_return_code_w : in std_logic_vector(RETURN_CODE_WIDTH-1 downto 0); + g_instance_handle_in_w : in INSTANCE_HANDLE_TYPE; + g_valid_out_w : out std_logic; + g_ready_out_w : in std_logic; + g_data_out_w : out std_logic_vector(WORD_WIDTH-1 downto 0); + g_last_word_out_w : out std_logic; + g_valid_in_w : in std_logic; + g_ready_in_w : out std_logic; + g_data_in_w : in std_logic_vector(WORD_WIDTH-1 downto 0); + g_last_word_in_w : in std_logic; + g_status_w : in std_logic_vector(STATUS_KIND_WIDTH-1 downto 0); + -- *RESULT SERVICE* + -- DDS READER + r_start_r : out std_logic; + r_ack_r : in std_logic; + r_opcode_r : out DDS_READER_OPCODE_TYPE; + r_instance_state_r : out std_logic_vector(INSTANCE_STATE_KIND_WIDTH-1 downto 0); + r_view_state_r : out std_logic_vector(VIEW_STATE_KIND_WIDTH-1 downto 0); + r_sample_state_r : out std_logic_vector(SAMPLE_STATE_KIND_WIDTH-1 downto 0); + r_instance_handle_r : out INSTANCE_HANDLE_TYPE; + r_max_samples_r : out std_logic_vector(MAX_SAMPLES_WIDTH-1 downto 0); + r_get_data_r : out std_logic; + r_done_r : in std_logic; + r_return_code_r : in std_logic_vector(RETURN_CODE_WIDTH-1 downto 0); + r_valid_in_r : in std_logic; + r_ready_in_r : out std_logic; + r_data_in_r : in std_logic_vector(WORD_WIDTH-1 downto 0); + r_last_word_in_r : in std_logic; + r_sample_info_r : in SAMPLE_INFO_TYPE; + r_sample_info_valid_r : in std_logic; + r_sample_info_ack_r : out std_logic; + r_eoc_r : in std_logic; + r_status_r : in std_logic_vector(STATUS_KIND_WIDTH-1 downto 0); + -- DDS WRITER + r_start_w : out std_logic; + r_ack_w : in std_logic; + r_opcode_w : out DDS_WRITER_OPCODE_TYPE; + r_instance_handle_out_w : out INSTANCE_HANDLE_TYPE; + r_source_ts_w : out TIME_TYPE; + r_max_wait_w : out DURATION_TYPE; + r_done_w : in std_logic; + r_return_code_w : in std_logic_vector(RETURN_CODE_WIDTH-1 downto 0); + r_instance_handle_in_w : in INSTANCE_HANDLE_TYPE; + r_valid_out_w : out std_logic; + r_ready_out_w : in std_logic; + r_data_out_w : out std_logic_vector(WORD_WIDTH-1 downto 0); + r_last_word_out_w : out std_logic; + r_valid_in_w : in std_logic; + r_ready_in_w : out std_logic; + r_data_in_w : in std_logic_vector(WORD_WIDTH-1 downto 0); + r_last_word_in_w : in std_logic; + r_status_w : in std_logic_vector(STATUS_KIND_WIDTH-1 downto 0); + -- *CANCEL SERVICE* + -- DDS READER + c_start_r : out std_logic; + c_ack_r : in std_logic; + c_opcode_r : out DDS_READER_OPCODE_TYPE; + c_instance_state_r : out std_logic_vector(INSTANCE_STATE_KIND_WIDTH-1 downto 0); + c_view_state_r : out std_logic_vector(VIEW_STATE_KIND_WIDTH-1 downto 0); + c_sample_state_r : out std_logic_vector(SAMPLE_STATE_KIND_WIDTH-1 downto 0); + c_instance_handle_r : out INSTANCE_HANDLE_TYPE; + c_max_samples_r : out std_logic_vector(MAX_SAMPLES_WIDTH-1 downto 0); + c_get_data_r : out std_logic; + c_done_r : in std_logic; + c_return_code_r : in std_logic_vector(RETURN_CODE_WIDTH-1 downto 0); + c_valid_in_r : in std_logic; + c_ready_in_r : out std_logic; + c_data_in_r : in std_logic_vector(WORD_WIDTH-1 downto 0); + c_last_word_in_r : in std_logic; + c_sample_info_r : in SAMPLE_INFO_TYPE; + c_sample_info_valid_r : in std_logic; + c_sample_info_ack_r : out std_logic; + c_eoc_r : in std_logic; + c_status_r : in std_logic_vector(STATUS_KIND_WIDTH-1 downto 0); + -- DDS WRITER + c_start_w : out std_logic; + c_ack_w : in std_logic; + c_opcode_w : out DDS_WRITER_OPCODE_TYPE; + c_instance_handle_out_w : out INSTANCE_HANDLE_TYPE; + c_source_ts_w : out TIME_TYPE; + c_max_wait_w : out DURATION_TYPE; + c_done_w : in std_logic; + c_return_code_w : in std_logic_vector(RETURN_CODE_WIDTH-1 downto 0); + c_instance_handle_in_w : in INSTANCE_HANDLE_TYPE; + c_valid_out_w : out std_logic; + c_ready_out_w : in std_logic; + c_data_out_w : out std_logic_vector(WORD_WIDTH-1 downto 0); + c_last_word_out_w : out std_logic; + c_valid_in_w : in std_logic; + c_ready_in_w : out std_logic; + c_data_in_w : in std_logic_vector(WORD_WIDTH-1 downto 0); + c_last_word_in_w : in std_logic; + c_status_w : in std_logic_vector(STATUS_KIND_WIDTH-1 downto 0); + -- *FEEDBACK PUBLISHER* + f_start_dds : out std_logic; + f_ack_dds : in std_logic; + f_opcode_dds : out DDS_WRITER_OPCODE_TYPE; + f_instance_handle_out_dds : out INSTANCE_HANDLE_TYPE; + f_source_ts_dds : out TIME_TYPE; + f_max_wait_dds : out DURATION_TYPE; + f_done_dds : in std_logic; + f_return_code_dds : in std_logic_vector(RETURN_CODE_WIDTH-1 downto 0); + f_instance_handle_in_dds : in INSTANCE_HANDLE_TYPE; + f_valid_out_dds : out std_logic; + f_ready_out_dds : in std_logic; + f_data_out_dds : out std_logic_vector(WORD_WIDTH-1 downto 0); + f_last_word_out_dds : out std_logic; + f_valid_in_dds : in std_logic; + f_ready_in_dds : out std_logic; + f_data_in_dds : in std_logic_vector(WORD_WIDTH-1 downto 0); + f_last_word_in_dds : in std_logic; + f_status_dds : in std_logic_vector(STATUS_KIND_WIDTH-1 downto 0); + -- *STATUS PUBLISHER* + s_start_dds : out std_logic; + s_ack_dds : in std_logic; + s_opcode_dds : out DDS_WRITER_OPCODE_TYPE; + s_instance_handle_out_dds : out INSTANCE_HANDLE_TYPE; + s_source_ts_dds : out TIME_TYPE; + s_max_wait_dds : out DURATION_TYPE; + s_done_dds : in std_logic; + s_return_code_dds : in std_logic_vector(RETURN_CODE_WIDTH-1 downto 0); + s_instance_handle_in_dds : in INSTANCE_HANDLE_TYPE; + s_valid_out_dds : out std_logic; + s_ready_out_dds : in std_logic; + s_data_out_dds : out std_logic_vector(WORD_WIDTH-1 downto 0); + s_last_word_out_dds : out std_logic; + s_valid_in_dds : in std_logic; + s_ready_in_dds : out std_logic; + s_data_in_dds : in std_logic_vector(WORD_WIDTH-1 downto 0); + s_last_word_in_dds : in std_logic; + s_status_dds : in std_logic_vector(STATUS_KIND_WIDTH-1 downto 0); + -- *USER* + start : in std_logic; + opcode : in ROS_ACTION_OPCODE_TYPE; + ack : out std_logic; + done : out std_logic; + return_code : out std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0); + goal_handle_in : in std_logic_vector(GOAL_HANDLE_WIDTH-1 downto 0); + goal_handle_out : out std_logic_vector(GOAL_HANDLE_WIDTH-1 downto 0); + goal_state_in : in std_logic_vector(CDR_INT8_WIDTH-1 downto 0); + goal_state_out : out std_logic_vector(CDR_INT8_WIDTH-1 downto 0); + goal_id : out std_logic_vector(UUID_WIDTH-1 downto 0); + goal_result_index : out std_logic_vector(WORD_WIDTH-1 downto 0); + goal_stamp : out ROS_TIME_TYPE; + -- GOAL + new_goal_request : out std_logic; + new_goal_handle : out std_logic_vector(GOAL_HANDLE_WIDTH-1 downto 0); + new_goal_result_index : out std_logic_vector(WORD_WIDTH-1 downto 0); + -- ###GENERATED START### + new_goal_order : out std_logic_vector(CDR_LONG_WIDTH-1 downto 0); + -- ###GENERATED END### + new_goal_accepted : in std_logic; + new_goal_response : in std_logic; + -- CANCEL + cancel_request : out std_logic; + cancel_request_handle : out std_logic_vector(GOAL_HANDLE_WIDTH-1 downto 0); + cancel_accepted : in std_logic; + cancel_response : in std_logic; + -- RESULT SIGNALS + index_result : in std_logic_vector(WORD_WIDTH-1 downto 0); + index_result_wen : in std_logic; + index_result_ready : out std_logic; + -- ###GENERATED START### + result_seq_len : in std_logic_vector(R_RR_SEQ_ADDR_WIDTH-1 downto 0); + result_seq_addr : in std_logic_vector(R_RR_SEQ_ADDR_WIDTH-1 downto 0); + result_seq_ready : out std_logic; + result_seq_ren : in std_logic; + result_seq_wen : in std_logic; + result_seq_valid : out std_logic; + result_seq_ack : in std_logic; + result_seq_r : out std_logic_vector(CDR_LONG_WIDTH-1 downto 0); + result_seq_w : in std_logic_vector(CDR_LONG_WIDTH-1 downto 0); + -- ###GENERATED END### + -- FEEDBACK SIGNALS + -- ###GENERATED START### + feedback_seq_len : in std_logic_vector(F_SEQ_ADDR_WIDTH-1 downto 0); + feedback_seq_addr : in std_logic_vector(F_SEQ_ADDR_WIDTH-1 downto 0); + feedback_seq_ready : out std_logic; + feedback_seq_ren : in std_logic; + feedback_seq_wen : in std_logic; + feedback_seq_valid : out std_logic; + feedback_seq_ack : in std_logic; + feedback_seq_r : out std_logic_vector(CDR_LONG_WIDTH-1 downto 0); + feedback_seq_w : in std_logic_vector(CDR_LONG_WIDTH-1 downto 0) + -- ###GENERATED END### + ); +end entity; + +architecture arch of Fibonacci_ros_action_server is + + --*****CONSTANT DECLARATION***** + constant RESULT_INDEX_WIDTH : natural := log2c(MAX_GOALS); + + --*****TYPE DECLARATION***** + type STAGE_TYPE is (IDLE,GET_RESULT,READ_RESULT,PASSTHROUGH,PUSH_SEQUENCE); + -- ###GENERATED START### + type R_SEQ_ADDR_TYPE is array (0 to MAX_GOALS-1) of std_logic_vector(R_RR_SEQ_ADDR_WIDTH-1 downto 0); + type R_SEQ_DATA_TYPE is array (0 to MAX_GOALS-1) of std_logic_vector(CDR_LONG_WIDTH-1 downto 0); + -- ###GENERATED END### + + --*****SIGNAL DECLARATION***** + signal abort_mem : std_logic; + signal stage, stage_next : STAGE_TYPE; + signal cnt, cnt_next : natural range 0 to 5; + signal g_start_user, r_start_user, c_start_user, g_ack_user, r_ack_user, c_ack_user, g_data_available_user, r_data_available_user, c_data_available_user, g_taken_user, r_taken_user, c_taken_user, g_done_user, r_done_user, c_done_user : std_logic; + signal g_opcode_user, r_opcode_user, c_opcode_user : ROS_SERVICE_OPCODE_TYPE; + signal g_service_info_user, r_service_info_user, c_service_info_user : SERVICE_INFO_TYPE; + signal g_request_id_user, r_request_id_user, c_request_id_user : REQUEST_ID_TYPE; + signal g_return_code_user, r_return_code_user, c_return_code_user : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0); + signal g_goal_id, r_goal_id, f_goal_id : std_logic_vector(UUID_WIDTH-1 downto 0); + signal g_accepted : std_logic; + signal g_stamp : std_logic_vector(ROS_TIME_WIDTH-1 downto 0); + signal r_status : std_logic_vector(CDR_INT8_WIDTH-1 downto 0); + signal r_index : std_logic_vector(WORD_WIDTH-1 downto 0); + signal r_sel, r_sel_ack :std_logic; + signal c_goal_info_goal_id : std_logic_vector(UUID_WIDTH-1 downto 0); + signal c_goal_info_stamp : std_logic_vector(ROS_TIME_WIDTH-1 downto 0); + signal c_return_code : std_logic_vector(CDR_INT8_WIDTH-1 downto 0); + signal c_goals_canceling_len : std_logic_vector(CancelGoal_package.RR_GOALS_CANCELING_ADDR_WIDTH-1 downto 0); + signal c_goals_canceling_addr : std_logic_vector(CancelGoal_package.RR_GOALS_CANCELING_ADDR_WIDTH-1 downto 0); + signal c_goals_canceling_ready, c_goals_canceling_ren, c_goals_canceling_wen, c_goals_canceling_valid, c_goals_canceling_ack : std_logic; + signal c_goals_canceling_goal_id_r, c_goals_canceling_goal_id_w : std_logic_vector(UUID_WIDTH-1 downto 0); + signal c_goals_canceling_stamp_r, c_goals_canceling_stamp_w : std_logic_vector(ROS_TIME_WIDTH-1 downto 0); + signal s_start_user, f_start_user, s_ack_user, f_ack_user, s_done_user, f_done_user : std_logic; + signal s_opcode_user, f_opcode_user : ROS_TOPIC_OPCODE_TYPE; + signal s_return_code_user, f_return_code_user : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0); + signal s_status_list_len, s_status_list_addr : std_logic_vector(GoalStatusArray_package.STATUS_LIST_ADDR_WIDTH-1 downto 0); + signal s_status_list_ready, s_status_list_ren, s_status_list_wen, s_status_list_valid, s_status_list_ack : std_logic; + signal s_status_list_goal_info_goal_id_r, s_status_list_goal_info_goal_id_w : std_logic_vector(UUID_WIDTH-1 downto 0); + signal s_status_list_goal_info_stamp_r, s_status_list_goal_info_stamp_w : std_logic_vector(ROS_TIME_WIDTH-1 downto 0); + signal s_status_list_status_r, s_status_list_status_w : std_logic_vector(CDR_INT8_WIDTH-1 downto 0); + signal result_index_latch, result_index_latch_next : std_logic_vector(RESULT_INDEX_WIDTH-1 downto 0); + -- ###GENERATED START### + signal seq_len_sig, seq_addr_sig : std_logic_vector(R_RR_SEQ_ADDR_WIDTH-1 downto 0); + signal seq_ready_sig, seq_wen_sig : std_logic; + signal seq_w_sig : std_logic_vector(CDR_LONG_WIDTH-1 downto 0); + + signal seq_cnt, seq_cnt_next : natural range 0 to R_RR_SEQ_MAX_DEPTH-1; + -- r_seq_mem Signals + signal r_seq_mem_addr : R_SEQ_ADDR_TYPE; + signal r_seq_mem_read, r_seq_mem_ready_in, r_seq_mem_ready_out, r_seq_mem_valid_in, r_seq_mem_valid_out : std_logic_vector(0 to MAX_GOALS-1); + signal r_seq_mem_data_in, r_seq_mem_data_out : R_SEQ_DATA_TYPE; + -- r_seq_len_mem Signals + signal r_seq_len_mem_addr : std_logic_vector(RESULT_INDEX_WIDTH-1 downto 0); + signal r_seq_len_mem_read, r_seq_len_mem_ready_in, r_seq_len_mem_ready_out, r_seq_len_mem_valid_in, r_seq_len_mem_valid_out : std_logic; + signal r_seq_len_mem_data_in, r_seq_len_mem_data_out : std_logic_vector(R_RR_SEQ_ADDR_WIDTH-1 downto 0); + -- ###GENERATED END### + +begin + + goal_srv_server_inst : entity work.Fibonacci_ros_action_goal_srv_server(arch) + generic map ( + LITTLE_ENDIAN => LITTLE_ENDIAN + ) + port map ( + clk => clk, + reset => reset, + start_r => g_start_r, + ack_r => g_ack_r, + opcode_r => g_opcode_r, + instance_state_r => g_instance_state_r, + view_state_r => g_view_state_r, + sample_state_r => g_sample_state_r, + instance_handle_r => g_instance_handle_r, + max_samples_r => g_max_samples_r, + get_data_r => g_get_data_r, + done_r => g_done_r, + return_code_r => g_return_code_r, + valid_in_r => g_valid_in_r, + ready_in_r => g_ready_in_r, + data_in_r => g_data_in_r, + last_word_in_r => g_last_word_in_r, + sample_info_r => g_sample_info_r, + sample_info_valid_r => g_sample_info_valid_r, + sample_info_ack_r => g_sample_info_ack_r, + eoc_r => g_eoc_r, + status_r => g_status_r, + start_w => g_start_w, + ack_w => g_ack_w, + opcode_w => g_opcode_w, + instance_handle_out_w => g_instance_handle_out_w, + source_ts_w => g_source_ts_w, + max_wait_w => g_max_wait_w, + done_w => g_done_w, + return_code_w => g_return_code_w, + instance_handle_in_w => g_instance_handle_in_w, + valid_out_w => g_valid_out_w, + ready_out_w => g_ready_out_w, + data_out_w => g_data_out_w, + last_word_out_w => g_last_word_out_w, + valid_in_w => g_valid_in_w, + ready_in_w => g_ready_in_w, + data_in_w => g_data_in_w, + last_word_in_w => g_last_word_in_w, + status_w => g_status_w, + start_user => g_start_user, + ack_user => g_ack_user, + opcode_user => g_opcode_user, + service_info_user => g_service_info_user, + request_id_user => g_request_id_user, + data_available_user => g_data_available_user, + taken_user => g_taken_user, + goal_id => g_goal_id, + -- ###GENERATED START### + order => new_goal_order, + -- ###GENERATED END### + accepted => g_accepted, + stamp => g_stamp, + done_user => g_done_user, + return_code_user => g_return_code_user + ); + + result_srv_server_inst : entity work.Fibonacci_ros_action_result_srv_server(arch) + generic map ( + LITTLE_ENDIAN => LITTLE_ENDIAN + ) + port map ( + clk => clk, + reset => reset, + start_r => r_start_r, + ack_r => r_ack_r, + opcode_r => r_opcode_r, + instance_state_r => r_instance_state_r, + view_state_r => r_view_state_r, + sample_state_r => r_sample_state_r, + instance_handle_r => r_instance_handle_r, + max_samples_r => r_max_samples_r, + get_data_r => r_get_data_r, + done_r => r_done_r, + return_code_r => r_return_code_r, + valid_in_r => r_valid_in_r, + ready_in_r => r_ready_in_r, + data_in_r => r_data_in_r, + last_word_in_r => r_last_word_in_r, + sample_info_r => r_sample_info_r, + sample_info_valid_r => r_sample_info_valid_r, + sample_info_ack_r => r_sample_info_ack_r, + eoc_r => r_eoc_r, + status_r => r_status_r, + start_w => r_start_w, + ack_w => r_ack_w, + opcode_w => r_opcode_w, + instance_handle_out_w => r_instance_handle_out_w, + source_ts_w => r_source_ts_w, + max_wait_w => r_max_wait_w, + done_w => r_done_w, + return_code_w => r_return_code_w, + instance_handle_in_w => r_instance_handle_in_w, + valid_out_w => r_valid_out_w, + ready_out_w => r_ready_out_w, + data_out_w => r_data_out_w, + last_word_out_w => r_last_word_out_w, + valid_in_w => r_valid_in_w, + ready_in_w => r_ready_in_w, + data_in_w => r_data_in_w, + last_word_in_w => r_last_word_in_w, + status_w => r_status_w, + start_user => r_start_user, + ack_user => r_ack_user, + opcode_user => r_opcode_user, + service_info_user => r_service_info_user, + request_id_user => r_request_id_user, + data_available_user => r_data_available_user, + taken_user => r_taken_user, + goal_id => r_goal_id, + status => r_status, + -- ###GENERATED START### + seq_len => seq_len_sig, + seq_addr => seq_addr_sig, + seq_ready => seq_ready_sig, + seq_ren => '0', + seq_wen => seq_wen_sig, + seq_valid => open, + seq_ack => '1', + seq_r => open, + seq_w => seq_w_sig, + -- ###GENERATED END### + done_user => r_done_user, + return_code_user => r_return_code_user + ); + + cancel_srv_server_inst : entity work.CancelGoal_ros_srv_server(arch) + generic map ( + LITTLE_ENDIAN => LITTLE_ENDIAN + ) + port map ( + clk => clk, + reset => reset, + start_r => c_start_r, + ack_r => c_ack_r, + opcode_r => c_opcode_r, + instance_state_r => c_instance_state_r, + view_state_r => c_view_state_r, + sample_state_r => c_sample_state_r, + instance_handle_r => c_instance_handle_r, + max_samples_r => c_max_samples_r, + get_data_r => c_get_data_r, + done_r => c_done_r, + return_code_r => c_return_code_r, + valid_in_r => c_valid_in_r, + ready_in_r => c_ready_in_r, + data_in_r => c_data_in_r, + last_word_in_r => c_last_word_in_r, + sample_info_r => c_sample_info_r, + sample_info_valid_r => c_sample_info_valid_r, + sample_info_ack_r => c_sample_info_ack_r, + eoc_r => c_eoc_r, + status_r => c_status_r, + start_w => c_start_w, + ack_w => c_ack_w, + opcode_w => c_opcode_w, + instance_handle_out_w => c_instance_handle_out_w, + source_ts_w => c_source_ts_w, + max_wait_w => c_max_wait_w, + done_w => c_done_w, + return_code_w => c_return_code_w, + instance_handle_in_w => c_instance_handle_in_w, + valid_out_w => c_valid_out_w, + ready_out_w => c_ready_out_w, + data_out_w => c_data_out_w, + last_word_out_w => c_last_word_out_w, + valid_in_w => c_valid_in_w, + ready_in_w => c_ready_in_w, + data_in_w => c_data_in_w, + last_word_in_w => c_last_word_in_w, + status_w => c_status_w, + start_user => c_start_user, + ack_user => c_ack_user, + opcode_user => c_opcode_user, + service_info_user => c_service_info_user, + request_id_user => c_request_id_user, + data_available_user => c_data_available_user, + taken_user => c_taken_user, + goal_info_goal_id => c_goal_info_goal_id, + goal_info_stamp => c_goal_info_stamp, + return_code => c_return_code, + goals_canceling_len => c_goals_canceling_len, + goals_canceling_addr => c_goals_canceling_addr, + goals_canceling_ready => c_goals_canceling_ready, + goals_canceling_ren => c_goals_canceling_ren, + goals_canceling_wen => c_goals_canceling_wen, + goals_canceling_valid => c_goals_canceling_valid, + goals_canceling_ack => c_goals_canceling_ack, + goals_canceling_goal_id_r => c_goals_canceling_goal_id_r, + goals_canceling_goal_id_w => c_goals_canceling_goal_id_w, + goals_canceling_stamp_r => c_goals_canceling_stamp_r, + goals_canceling_stamp_w => c_goals_canceling_stamp_w, + done_user => c_done_user, + return_code_user => c_return_code_user + ); + + feedback_gen : if ENABLE_FEEDBACK = '1' generate + feedback_pub_inst : entity work.Fibonacci_ros_action_feedback_pub(arch) + generic map ( + LITTLE_ENDIAN => LITTLE_ENDIAN + ) + port map ( + clk => clk, + reset => reset, + start_dds => f_start_dds, + ack_dds => f_ack_dds, + opcode_dds => f_opcode_dds, + instance_handle_out_dds => f_instance_handle_out_dds, + source_ts_dds => f_source_ts_dds, + max_wait_dds => f_max_wait_dds, + done_dds => f_done_dds, + return_code_dds => f_return_code_dds, + instance_handle_in_dds => f_instance_handle_in_dds, + valid_out_dds => f_valid_out_dds, + ready_out_dds => f_ready_out_dds, + data_out_dds => f_data_out_dds, + last_word_out_dds => f_last_word_out_dds, + valid_in_dds => f_valid_in_dds, + ready_in_dds => f_ready_in_dds, + data_in_dds => f_data_in_dds, + last_word_in_dds => f_last_word_in_dds, + status_dds => f_status_dds, + start_user => f_start_user, + opcode_user => f_opcode_user, + ack_user => f_ack_user, + -- ###GENERATED START### + goal_id => f_goal_id, + seq_len => feedback_seq_len, + seq_addr => feedback_seq_addr, + seq_ready => feedback_seq_ready, + seq_ren => feedback_seq_ren, + seq_wen => feedback_seq_wen, + seq_valid => feedback_seq_valid, + seq_ack => feedback_seq_ack, + seq_r => feedback_seq_r, + seq_w => feedback_seq_w, + -- ###GENERATED END### + done_user => f_done_user, + return_code_user => f_return_code_user + ); + else generate + f_start_dds <= '0'; + f_opcode_dds <= NOP; + f_instance_handle_out_dds <= HANDLE_NIL; + f_source_ts_dds <= TIME_INVALID; + f_max_wait_dds <= DURATION_ZERO; + f_valid_out_dds <= '0'; + f_data_out_dds <= (others => '0'); + f_last_word_out_dds <= '0'; + f_ready_in_dds <= '0'; + -- ###GENERATED START### + feedback_seq_ready <= '0'; + feedback_seq_valid <= '0'; + feedback_seq_r <= (others => '0'); + -- ###GENERATED END### + end generate; + + status_pub_inst : entity work.GoalStatusArray_ros_pub(arch) + generic map ( + LITTLE_ENDIAN => LITTLE_ENDIAN + ) + port map ( + clk => clk, + reset => reset, + start_dds => s_start_dds, + ack_dds => s_ack_dds, + opcode_dds => s_opcode_dds, + instance_handle_out_dds => s_instance_handle_out_dds, + source_ts_dds => s_source_ts_dds, + max_wait_dds => s_max_wait_dds, + done_dds => s_done_dds, + return_code_dds => s_return_code_dds, + instance_handle_in_dds => s_instance_handle_in_dds, + valid_out_dds => s_valid_out_dds, + ready_out_dds => s_ready_out_dds, + data_out_dds => s_data_out_dds, + last_word_out_dds => s_last_word_out_dds, + valid_in_dds => s_valid_in_dds, + ready_in_dds => s_ready_in_dds, + data_in_dds => s_data_in_dds, + last_word_in_dds => s_last_word_in_dds, + status_dds => s_status_dds, + start_user => s_start_user, + opcode_user => s_opcode_user, + ack_user => s_ack_user, + status_list_len => s_status_list_len, + status_list_addr => s_status_list_addr, + status_list_ready => s_status_list_ready, + status_list_ren => s_status_list_ren, + status_list_wen => s_status_list_wen, + status_list_valid => s_status_list_valid, + status_list_ack => s_status_list_ack, + status_list_goal_info_goal_id_r => s_status_list_goal_info_goal_id_r, + status_list_goal_info_goal_id_w => s_status_list_goal_info_goal_id_w, + status_list_goal_info_stamp_r => s_status_list_goal_info_stamp_r, + status_list_goal_info_stamp_w => s_status_list_goal_info_stamp_w, + status_list_status_r => s_status_list_status_r, + status_list_status_w => s_status_list_status_w, + done_user => s_done_user, + return_code_user => s_return_code_user + ); + + action_server_inst : entity work.ros_action_server(arch) + generic map ( + TIMEOUT_DURATION => TIMEOUT_DURATION, + MAX_GOALS => MAX_GOALS, + MAX_RESULT_REQUESTS => MAX_RESULT_REQUESTS, + ENABLE_FEEDBACK => ENABLE_FEEDBACK + ) + port map ( + clk => clk, + reset => reset, + time => time, + -- *GOAL SERVICE* + start_g => g_start_user, + ack_g => g_ack_user, + opcode_g => g_opcode_user, + service_info_g => g_service_info_user, + request_id_g => g_request_id_user, + data_available_g => g_data_available_user, + taken_g => g_taken_user, + goal_id_g => g_goal_id, + accepted_g => g_accepted, + stamp_g => g_stamp, + done_g => g_done_user, + return_code_g => g_return_code_user, + -- *RESULT SERVICE* + start_r => r_start_user, + ack_r => r_ack_user, + opcode_r => r_opcode_user, + service_info_r => r_service_info_user, + request_id_r => r_request_id_user, + data_available_r => r_data_available_user, + taken_r => r_taken_user, + result_index => r_index, + result_sel => r_sel, + result_sel_ack => r_sel_ack, + goal_id_r => r_goal_id, + status_r => r_status, + done_r => r_done_user, + return_code_r => r_return_code_user, + -- *CANCEL SERVICE* + start_c => c_start_user, + ack_c => c_ack_user, + opcode_c => c_opcode_user, + service_info_c => c_service_info_user, + request_id_c => c_request_id_user, + data_available_c => c_data_available_user, + taken_c => c_taken_user, + goal_info_goal_id_c => c_goal_info_goal_id, + goal_info_stamp_c => c_goal_info_stamp, + cancel_return_code_c => c_return_code, + goals_canceling_len_c => c_goals_canceling_len, + goals_canceling_addr_c => c_goals_canceling_addr, + goals_canceling_ready_c => c_goals_canceling_ready, + goals_canceling_ren_c => c_goals_canceling_ren, + goals_canceling_wen_c => c_goals_canceling_wen, + goals_canceling_valid_c => c_goals_canceling_valid, + goals_canceling_ack_c => c_goals_canceling_ack, + goals_canceling_goal_id_r_c => c_goals_canceling_goal_id_r, + goals_canceling_goal_id_w_c => c_goals_canceling_goal_id_w, + goals_canceling_stamp_r_c => c_goals_canceling_stamp_r, + goals_canceling_stamp_w_c => c_goals_canceling_stamp_w, + done_c => c_done_user, + return_code_c => c_return_code_user, + -- *FEEDBACK TOPIC* + start_fb => f_start_user, + opcode_fb => f_opcode_user, + ack_fb => f_ack_user, + goal_id_fb => f_goal_id, + done_fb => f_done_user, + return_code_fb => f_return_code_user, + -- *STATUS TOPIC* + start_s => s_start_user, + opcode_s => s_opcode_user, + ack_s => s_ack_user, + status_list_len_s => s_status_list_len, + status_list_addr_s => s_status_list_addr, + status_list_ready_s => s_status_list_ready, + status_list_ren_s => s_status_list_ren, + status_list_wen_s => s_status_list_wen, + status_list_valid_s => s_status_list_valid, + status_list_ack_s => s_status_list_ack, + status_list_goal_info_goal_id_r_s => s_status_list_goal_info_goal_id_r, + status_list_goal_info_goal_id_w_s => s_status_list_goal_info_goal_id_w, + status_list_goal_info_stamp_r_s => s_status_list_goal_info_stamp_r, + status_list_goal_info_stamp_w_s => s_status_list_goal_info_stamp_w, + status_list_status_r_s => s_status_list_status_r, + status_list_status_w_s => s_status_list_status_w, + done_s => s_done_user, + return_code_s => s_return_code_user, + -- USER + start => start, + opcode => opcode, + ack => ack, + goal_handle_in => goal_handle_in, + goal_handle_out => goal_handle_out, + goal_state_in => goal_state_in, + goal_state_out => goal_state_out, + goal_id => goal_id, + goal_result_index => goal_result_index, + goal_stamp => goal_stamp, + new_goal_request => new_goal_request, + new_goal_handle => new_goal_handle, + new_goal_result_index => new_goal_result_index, + new_goal_accepted => new_goal_accepted, + new_goal_response => new_goal_response, + cancel_request => cancel_request, + cancel_request_handle => cancel_request_handle, + cancel_accepted => cancel_accepted, + cancel_response => cancel_response, + done => done, + return_code => return_code + ); + + -- ###GENERATED START### + r_seq_len_mem : entity work.mem_ctrl(arch) + generic map ( + ADDR_WIDTH => RESULT_INDEX_WIDTH, + DATA_WIDTH => R_RR_SEQ_ADDR_WIDTH, + MEMORY_DEPTH => MAX_GOALS, + MAX_BURST_LENGTH => 1 + ) + port map ( + clk => clk, + reset => reset or abort_mem, + addr => r_seq_len_mem_addr, + read => r_seq_len_mem_read, + ready_in => r_seq_len_mem_ready_in, + valid_in => r_seq_len_mem_valid_in, + data_in => r_seq_len_mem_data_in, + ready_out => r_seq_len_mem_ready_out, + valid_out => r_seq_len_mem_valid_out, + data_out => r_seq_len_mem_data_out + ); + + r_seq_gen : for i in 0 to MAX_GOALS-1 generate + r_seq_mem : entity work.mem_ctrl(arch) + generic map ( + ADDR_WIDTH => R_RR_SEQ_ADDR_WIDTH, + DATA_WIDTH => CDR_LONG_WIDTH, + MEMORY_DEPTH => R_RR_SEQ_MAX_DEPTH, + MAX_BURST_LENGTH => 1 + ) + port map ( + clk => clk, + reset => reset or abort_mem, + addr => r_seq_mem_addr(i), + read => r_seq_mem_read(i), + ready_in => r_seq_mem_ready_in(i), + valid_in => r_seq_mem_valid_in(i), + data_in => r_seq_mem_data_in(i), + ready_out => r_seq_mem_ready_out(i), + valid_out => r_seq_mem_valid_out(i), + data_out => r_seq_mem_data_out(i) + ); + end generate; + -- ###GENERATED END### + + result_mux : process(all) + begin + -- DEFAULT + stage_next <= stage; + cnt_next <= cnt; + result_index_latch_next <= result_index_latch; + -- DEFAULT Unregistered + r_sel_ack <= '0'; + abort_mem <= '0'; + -- ###GENERATED START### + seq_cnt_next <= seq_cnt; + r_seq_len_mem_addr <= (others => '0'); + r_seq_len_mem_read <= '0'; + r_seq_len_mem_valid_in <= '0'; + r_seq_len_mem_data_in <= (others => '0'); + r_seq_len_mem_ready_out <= '0'; + r_seq_mem_addr <= (others => (others => '0')); + r_seq_mem_read <= (others => '0'); + r_seq_mem_valid_in <= (others => '0'); + r_seq_mem_data_in <= (others => (others => '0')); + r_seq_mem_ready_out <= (others => '0'); + seq_len_sig <= (others => '0'); + seq_addr_sig <= (others => '0'); + seq_wen_sig <= '0'; + seq_w_sig <= (others => '0'); + -- ###GENERATED END### + + case (stage) is + when IDLE => + if (r_sel = '1') then + stage_next <= GET_RESULT; + -- Reset + abort_mem <= '1'; + else + index_result_ready <= r_seq_len_mem_ready_in; -- AND all memories + + -- ###GENERATED START### + r_seq_len_mem_data_in <= result_seq_len; + r_seq_len_mem_addr <= std_logic_vector(resize(unsigned(index_result), RESULT_INDEX_WIDTH)); + r_seq_len_mem_valid_in <= index_result_wen; + + result_seq_r <= r_seq_mem_data_out(to_integer(unsigned(result_index_latch))); + result_seq_valid <= r_seq_mem_valid_out(to_integer(unsigned(result_index_latch))); + r_seq_mem_data_in <= (others => result_seq_w); + result_seq_ready <= r_seq_mem_ready_in(to_integer(unsigned(index_result))); + r_seq_mem_addr(to_integer(unsigned(index_result))) <= result_seq_addr; + r_seq_mem_read(to_integer(unsigned(index_result))) <= result_seq_ren; + r_seq_mem_valid_in(to_integer(unsigned(index_result))) <= result_seq_ren or result_seq_wen; + r_seq_mem_ready_out <= (others => result_seq_ack); + -- Latch Address for output + if (result_seq_ren = '1') then + result_index_latch_next <= std_logic_vector(resize(unsigned(index_result), RESULT_INDEX_WIDTH)); + end if; + -- ###GENERATED END### + end if; + when GET_RESULT => + assert (r_sel = '1') severity FAILURE; + + -- ###GENERATED START### + r_seq_len_mem_addr <= std_logic_vector(resize(unsigned(r_index), RESULT_INDEX_WIDTH)); + r_seq_len_mem_read <= '1'; + -- Memory Operation Guard + if (r_seq_len_mem_ready_in = '1') then -- AND all 'ready_in' signals + -- NOTE: We are toggling the 'valid_in' signal only after we make sure all memories are ready + r_seq_len_mem_valid_in <= '1'; + stage_next <= READ_RESULT; + end if; + -- ###GENERATED END### + when READ_RESULT => + assert (r_sel = '1') severity FAILURE; + + -- ###GENERATED START### + if (r_seq_len_mem_valid_out = '1') then -- AND all 'valid_out' Signals + stage_next <= PUSH_SEQUENCE; + cnt_next <= 0; + seq_cnt_next <= 0; + end if; + -- ###GENERATED END### + -- ###GENERATED START### + when PUSH_SEQUENCE => + assert (r_sel = '1') severity FAILURE; + + case (cnt) is + -- GET + when 0 => + r_seq_mem_addr(to_integer(unsigned(r_index))) <= std_logic_vector(to_unsigned(seq_cnt,R_RR_SEQ_ADDR_WIDTH)); + r_seq_mem_valid_in(to_integer(unsigned(r_index))) <= '1'; + r_seq_mem_read(to_integer(unsigned(r_index))) <= '1'; + -- Memory Operation Guard + if (r_seq_mem_ready_in(to_integer(unsigned(r_index))) = '1') then + cnt_next <= cnt + 1; + end if; + -- WRITE + when 1 => + -- Memory Operation Guard + if (result_seq_ready = '1' and r_seq_mem_valid_out(to_integer(unsigned(r_index))) = '1') then + seq_addr_sig <= std_logic_vector(to_unsigned(seq_cnt,R_RR_SEQ_ADDR_WIDTH)); + seq_wen_sig <= '1'; + seq_w_sig <= r_seq_mem_data_out(to_integer(unsigned(r_index))); + r_seq_mem_ready_out(to_integer(unsigned(r_index))) <= '1'; + + -- All Elements processed + if (seq_cnt = unsigned(r_seq_len_mem_data_out)-1) then + -- DONE + stage_next <= PASSTHROUGH; + else + seq_cnt_next <= seq_cnt + 1; + cnt_next <= 0; + end if; + end if; + when others => + end case; + -- ###GENERATED END### + when PASSTHROUGH => + r_sel_ack <= '1'; + + -- ###GENERATED START### + if (unsigned(r_index) = MAX_GOALS) then + seq_len_sig <= (others => '0'); + else + seq_len_sig <= r_seq_len_mem_data_out; + end if; + -- ###GENERATED END### + + if (r_sel = '0') then + stage_next <= IDLE; + -- Reset + abort_mem <= '1'; + end if; + end case; + end process; + + sync_prc : process(clk) + begin + if rising_edge(clk) then + if (reset = '1') then + stage <= IDLE; + cnt <= 0; + seq_cnt <= 0; + result_index_latch <= (others => '0'); + else + stage <= stage_next; + cnt <= cnt_next; + seq_cnt <= seq_cnt_next; + result_index_latch <= result_index_latch_next; + end if; + end if; + end process; + +end architecture; diff --git a/src/ros2/ros_package.vhd b/src/ros2/ros_package.vhd index 997dc14..d656933 100644 --- a/src/ros2/ros_package.vhd +++ b/src/ros2/ros_package.vhd @@ -273,7 +273,7 @@ package ros_package is 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_PREVIOUS_GOAL, UPDATE_GOAL_STATE, PUBLISH_FEEDBACK); + type ROS_ACTION_OPCODE_TYPE is (NOP, GET_GOAL, GET_LAST_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;