greek64/rtps-fpga
2
1
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Add Fibonacci ROS Action Implementation

Browse Source 
A complete ROS action server and client implementation of the
example_interfaces Fibonacci action is done, along with accompaning
testbenches.
This commit is contained in:
John Ring 2022-03-05 11:47:58 +01:00
parent b86ef8ec53
commit a064a44dc4
7 changed files with 3271 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

111
sim/L1_Fibonacci_ros_action_test1.do Normal file
View File

@ -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}

745
src/ros2/Tests/Level_1/L1_Fibonacci_ros_action_test1.vhd Normal file
View File

@ -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;

716
src/ros2/Tests/Level_1/L1_Fibonacci_ros_action_test2.vhd Normal file
View File

@ -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;

8
src/ros2/Tests/ros_testbench.pro
View File

@ -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
simulate L0_ros_action_server_test2
simulate L1_Fibonacci_ros_action_test1
simulate L1_Fibonacci_ros_action_test2

769
src/ros2/example_interfaces/Fibonacci_ros_action_client.vhd Normal file
View File

@ -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;

922
src/ros2/example_interfaces/Fibonacci_ros_action_server.vhd Normal file
View File

@ -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;

2
src/ros2/ros_package.vhd
View File

@ -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;