From 3f2e4e82d1e40fa03d07746997c5004674aedb22 Mon Sep 17 00:00:00 2001 From: John Daktylidis Date: Thu, 15 Jun 2023 19:12:07 +0200 Subject: [PATCH] Fix Bug in TEMPLATE_ros_action_server, that could lead to deadlocks. The server could at any time reset and sever the connection of the user to the result memory, that could lead to the user indefenitely waiting for a droped memory read request. The server now only sever the connection when the result memory has no pending requests. L1_Fibanacci_ros_ation_server_test1 and L1_Fibanacci_ros_ation_server_test2 are exetnded to test the fix. --- src/ros2/TEMPLATE_ros_action_server.vhd | 7 +- .../Level_1/L1_Fibonacci_ros_action_test1.vhd | 199 ++++++++++++----- .../Level_1/L1_Fibonacci_ros_action_test2.vhd | 200 +++++++++++++----- .../Fibonacci_ros_action_server.vhd | 5 +- 4 files changed, 302 insertions(+), 109 deletions(-) diff --git a/src/ros2/TEMPLATE_ros_action_server.vhd b/src/ros2/TEMPLATE_ros_action_server.vhd index 9d3b6df..59ecfbf 100644 --- a/src/ros2/TEMPLATE_ros_action_server.vhd +++ b/src/ros2/TEMPLATE_ros_action_server.vhd @@ -272,6 +272,7 @@ architecture arch of TEMPLATE_ros_action_server is 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 idle_sig : std_logic := '0'; -- Test signal used in testbenches -- ###GENERATED START### -- SIGNAL DECLARATIONS -- ###GENERATED END### @@ -697,13 +698,17 @@ begin result_ready <= '0'; r_sel_ack <= '0'; abort_mem <= '0'; + idle_sig <= '0'; -- ###GENERATED START### -- DEFAULT SIGNAL ASSIGNMENTS -- ###GENERATED END### case (stage) is when IDLE => - if (r_sel = '1') then + idle_sig <= '1'; + -- ###GENERATED START### + if (r_sel = '1' and TODO_result_ready = '1' and result_ren = '0') then + -- ###GENERATED END### if (unsigned(r_index) = MAX_GOALS) then stage_next <= PASSTHROUGH; else diff --git a/src/ros2/Tests/Level_1/L1_Fibonacci_ros_action_test1.vhd b/src/ros2/Tests/Level_1/L1_Fibonacci_ros_action_test1.vhd index 2ab85a4..738f2de 100644 --- a/src/ros2/Tests/Level_1/L1_Fibonacci_ros_action_test1.vhd +++ b/src/ros2/Tests/Level_1/L1_Fibonacci_ros_action_test1.vhd @@ -493,6 +493,9 @@ begin variable test_handle : GOAL_HANDLE_ARRAY_TYPE := (others => (others => '0')); variable test_ind : RESULT_INDEX_ARRAY_TYPE := (others => 0); + alias idle_sig is <>; + alias r_sel is <>; + impure function gen_goal_id return std_logic_vector is begin return RV.RandSlv(UUID_WIDTH); @@ -605,25 +608,24 @@ begin 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); + sid(i) := sequence_id_c; - Log("SERVER: Check GOAL " & integer'image(i) & " Request", INFO); + Log("SERVER: Accept 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); + Log("CLIENT: Take GOAL " & integer'image(i) & " Response", 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); + AlertIf(to_unsigned(SEQUENCENUMBER_TYPE(service_info_c.request_id.sequence_number)) /= unsigned(sid(i)), "Request ID incorrect", FAILURE); goal_handle_in_s <= test_handle(i); goal_state_in_s <= GoalStatus_package.STATUS_EXECUTING; @@ -650,6 +652,7 @@ begin Log("CLIENT: Take FEEDBACK", INFO); client_op(TAKE_FEEDBACK,ROS_RET_OK); + wait until rising_edge(clk); 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 @@ -676,57 +679,147 @@ begin 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); + sid(i) := sequence_id_c; end loop; - for j in 0 to MAX_GOALS-1 loop - Log("SERVER: Set RESULT (Goal " & integer'image(j) & ")", INFO); - result_addr_s <= int(j,result_addr_s'length); - for i in 0 to RV.RandInt(1,10) loop - result_seq_len_w_s <= int(i+1,result_seq_len_w_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(result_ready_s); - result_wen_s <= '1'; + Log("SERVER: Set RESULT (Goal 0)", INFO); + result_addr_s <= int(test_ind(0),result_addr_s'length); + for i in 0 to RV.RandInt(1,10) loop + result_seq_len_w_s <= int(i+1,result_seq_len_w_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_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_w_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; + result_seq_wen_s <= '0'; end loop; + wait_on_sig(result_ready_s); + result_wen_s <= '1'; + wait until rising_edge(clk); + result_wen_s <= '0'; + + Log("SERVER: Request Result (Goal 0)", INFO); + result_addr_s <= int(test_ind(0),result_addr_s'length); + wait_on_sig(result_ready_s); + result_ren_s <= '1'; + wait until rising_edge(clk); + result_ren_s <= '0'; + wait until rising_edge(clk); + AlertIf(result_ready_s /= '0', "Result memory not in expected state", FAILURE); + + Log("SERVER: Update Goal State (Goal 0)", INFO); + goal_handle_in_s <= test_handle(0); + goal_state_in_s <= GoalStatus_package.STATUS_SUCCEEDED; + server_op(UPDATE_GOAL_STATE,ROS_RET_OK); + wait until rising_edge(clk); + + wait_on_sig(r_sel); + wait until rising_edge(clk); + wait until rising_edge(clk); + AlertIf(idle_sig /= '1', "Server interrupted User Result Request", FAILURE); + + result_ack_s <= '1'; + wait until rising_edge(clk); + result_ack_s <= '0'; + + Log("SERVER: Take RESULT Response (Goal 0)", INFO); + goal_id_c <= test_gid(0); + client_op(TAKE_RESULT_RESPONSE,ROS_RET_OK); + wait until rising_edge(clk); + result_addr_s <= int(test_ind(0),result_addr_s'length); + wait_on_sig(result_ready_s); + result_ren_s <= '1'; + wait until rising_edge(clk); + result_ren_s <= '0'; + wait_on_sig(result_valid_s); + AlertIf(to_unsigned(SEQUENCENUMBER_TYPE(service_info_c.request_id.sequence_number)) /= unsigned(sid(0)), "Request ID incorrect", FAILURE); + AffirmIfEqual(RESULT, result_status_c, GoalStatus_package.STATUS_SUCCEEDED); + AffirmIfEqual(RESULT, result_seq_len_c, result_seq_len_w_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; + result_ack_s <= '1'; + wait until rising_edge(clk); + result_ack_s <= '0'; + + if MAX_GOALS > 1 then + for j in 1 to MAX_GOALS-1 loop + Log("SERVER: Set RESULT (Goal " & integer'image(j) & ")", INFO); + result_addr_s <= int(test_ind(j),result_addr_s'length); + for i in 0 to RV.RandInt(1,10) loop + result_seq_len_w_s <= int(i+1,result_seq_len_w_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(result_ready_s); + result_wen_s <= '1'; + wait until rising_edge(clk); + result_wen_s <= '0'; + + Log("SERVER: Update Goal State (Goal " & integer'image(j) & ")", INFO); + 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); + wait until rising_edge(clk); + result_addr_s <= int(test_ind(j),result_addr_s'length); + wait_on_sig(result_ready_s); + result_ren_s <= '1'; + wait until rising_edge(clk); + result_ren_s <= '0'; + wait_on_sig(result_valid_s); + 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_w_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; + result_ack_s <= '1'; + wait until rising_edge(clk); + result_ack_s <= '0'; + end loop; + end if; TranscriptOpen(RESULTS_FILE, APPEND_MODE); SetTranscriptMirror; @@ -738,10 +831,10 @@ begin clock_prc : process begin - clk <= '0'; - wait for TEST_CLOCK_PERIOD/2; clk <= '1'; wait for TEST_CLOCK_PERIOD/2; + clk <= '0'; + wait for TEST_CLOCK_PERIOD/2; end process; watchdog : process diff --git a/src/ros2/Tests/Level_1/L1_Fibonacci_ros_action_test2.vhd b/src/ros2/Tests/Level_1/L1_Fibonacci_ros_action_test2.vhd index 6758006..8e83148 100644 --- a/src/ros2/Tests/Level_1/L1_Fibonacci_ros_action_test2.vhd +++ b/src/ros2/Tests/Level_1/L1_Fibonacci_ros_action_test2.vhd @@ -59,7 +59,7 @@ architecture testbench of L1_Fibonacci_ros_action_test2 is 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_r_s, result_seq_len_w_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_len_c, result_seq_len_w_s, result_seq_len_r_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'); @@ -497,6 +497,9 @@ begin variable test_handle : GOAL_HANDLE_ARRAY_TYPE := (others => (others => '0')); variable test_ind : RESULT_INDEX_ARRAY_TYPE := (others => 0); + alias idle_sig is <>; + alias r_sel is <>; + impure function gen_goal_id return std_logic_vector is begin return RV.RandSlv(UUID_WIDTH); @@ -609,25 +612,24 @@ begin 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); + sid(i) := sequence_id_c; - Log("SERVER: Check GOAL " & integer'image(i) & " Request", INFO); + Log("SERVER: Accept 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); + Log("CLIENT: Take GOAL " & integer'image(i) & " Response", 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); + AlertIf(to_unsigned(SEQUENCENUMBER_TYPE(service_info_c.request_id.sequence_number)) /= unsigned(sid(i)), "Request ID incorrect", FAILURE); goal_handle_in_s <= test_handle(i); goal_state_in_s <= GoalStatus_package.STATUS_EXECUTING; @@ -647,57 +649,147 @@ begin 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); + sid(i) := sequence_id_c; end loop; - for j in 0 to MAX_GOALS-1 loop - Log("SERVER: Set RESULT (Goal " & integer'image(j) & ")", INFO); - result_addr_s <= int(j,result_addr_s'length); - for i in 0 to RV.RandInt(1,10) loop - result_seq_len_w_s <= int(i+1,result_seq_len_w_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(result_ready_s); - result_wen_s <= '1'; + Log("SERVER: Set RESULT (Goal 0)", INFO); + result_addr_s <= int(test_ind(0),result_addr_s'length); + for i in 0 to RV.RandInt(1,10) loop + result_seq_len_w_s <= int(i+1,result_seq_len_w_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_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_w_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; + result_seq_wen_s <= '0'; end loop; + wait_on_sig(result_ready_s); + result_wen_s <= '1'; + wait until rising_edge(clk); + result_wen_s <= '0'; + + Log("SERVER: Request Result (Goal 0)", INFO); + result_addr_s <= int(test_ind(0),result_addr_s'length); + wait_on_sig(result_ready_s); + result_ren_s <= '1'; + wait until rising_edge(clk); + result_ren_s <= '0'; + wait until rising_edge(clk); + AlertIf(result_ready_s /= '0', "Result memory not in expected state", FAILURE); + + Log("SERVER: Update Goal State (Goal 0)", INFO); + goal_handle_in_s <= test_handle(0); + goal_state_in_s <= GoalStatus_package.STATUS_SUCCEEDED; + server_op(UPDATE_GOAL_STATE,ROS_RET_OK); + wait until rising_edge(clk); + + wait_on_sig(r_sel); + wait until rising_edge(clk); + wait until rising_edge(clk); + AlertIf(idle_sig /= '1', "Server interrupted User Result Request", FAILURE); + + result_ack_s <= '1'; + wait until rising_edge(clk); + result_ack_s <= '0'; + + Log("SERVER: Take RESULT Response (Goal 0)", INFO); + goal_id_c <= test_gid(0); + client_op(TAKE_RESULT_RESPONSE,ROS_RET_OK); + wait until rising_edge(clk); + result_addr_s <= int(test_ind(0),result_addr_s'length); + wait_on_sig(result_ready_s); + result_ren_s <= '1'; + wait until rising_edge(clk); + result_ren_s <= '0'; + wait_on_sig(result_valid_s); + AlertIf(to_unsigned(SEQUENCENUMBER_TYPE(service_info_c.request_id.sequence_number)) /= unsigned(sid(0)), "Request ID incorrect", FAILURE); + AffirmIfEqual(RESULT, result_status_c, GoalStatus_package.STATUS_SUCCEEDED); + AffirmIfEqual(RESULT, result_seq_len_c, result_seq_len_w_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; + result_ack_s <= '1'; + wait until rising_edge(clk); + result_ack_s <= '0'; + + if MAX_GOALS > 1 then + for j in 1 to MAX_GOALS-1 loop + Log("SERVER: Set RESULT (Goal " & integer'image(j) & ")", INFO); + result_addr_s <= int(test_ind(j),result_addr_s'length); + for i in 0 to RV.RandInt(1,10) loop + result_seq_len_w_s <= int(i+1,result_seq_len_w_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(result_ready_s); + result_wen_s <= '1'; + wait until rising_edge(clk); + result_wen_s <= '0'; + + Log("SERVER: Update Goal State (Goal " & integer'image(j) & ")", INFO); + 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); + wait until rising_edge(clk); + result_addr_s <= int(test_ind(j),result_addr_s'length); + wait_on_sig(result_ready_s); + result_ren_s <= '1'; + wait until rising_edge(clk); + result_ren_s <= '0'; + wait_on_sig(result_valid_s); + 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_w_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; + result_ack_s <= '1'; + wait until rising_edge(clk); + result_ack_s <= '0'; + end loop; + end if; TranscriptOpen(RESULTS_FILE, APPEND_MODE); SetTranscriptMirror; @@ -709,10 +801,10 @@ begin clock_prc : process begin - clk <= '0'; - wait for TEST_CLOCK_PERIOD/2; clk <= '1'; wait for TEST_CLOCK_PERIOD/2; + clk <= '0'; + wait for TEST_CLOCK_PERIOD/2; end process; watchdog : process diff --git a/src/ros2/example_interfaces/Fibonacci_ros_action_server.vhd b/src/ros2/example_interfaces/Fibonacci_ros_action_server.vhd index 1e408a7..8ecc8df 100644 --- a/src/ros2/example_interfaces/Fibonacci_ros_action_server.vhd +++ b/src/ros2/example_interfaces/Fibonacci_ros_action_server.vhd @@ -291,6 +291,7 @@ architecture arch of Fibonacci_ros_action_server is 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 idle_sig : std_logic := '0'; -- Test signal used in testbenches -- ###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; @@ -791,6 +792,7 @@ begin result_ready <= '0'; r_sel_ack <= '0'; abort_mem <= '0'; + idle_sig <= '0'; -- ###GENERATED START### seq_cnt_next <= seq_cnt; r_seq_len_mem_addr <= (others => '0'); @@ -810,7 +812,8 @@ begin case (stage) is when IDLE => - if (r_sel = '1') then + idle_sig <= '1'; + if (r_sel = '1' and r_seq_len_mem_ready_in = '1' and result_ren = '0') then if (unsigned(r_index) = MAX_GOALS) then stage_next <= PASSTHROUGH; else