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