From e4e702ebc4b422ab6654a1af53e0d77fa6439f25 Mon Sep 17 00:00:00 2001 From: Greek64 Date: Sat, 5 Mar 2022 16:45:01 +0100 Subject: [PATCH] Add GEN_FIRST_GOAL and GET_NEXT_GOAL opcodes to ROS action server The functionality was already there, so we just extended the opcodes to allow the user to also make use of them. --- .../Level_0/L0_ros_action_server_test1.vhd | 183 +++++++++++------- .../Level_0/L0_ros_action_server_test2.vhd | 183 +++++++++++------- .../Tests/Level_0/ros_action_server_tests.txt | 5 + src/ros2/ros_action_server.vhd | 32 +++ src/ros2/ros_package.vhd | 2 +- 5 files changed, 272 insertions(+), 133 deletions(-) diff --git a/src/ros2/Tests/Level_0/L0_ros_action_server_test1.vhd b/src/ros2/Tests/Level_0/L0_ros_action_server_test1.vhd index 45e742e..06f1af5 100644 --- a/src/ros2/Tests/Level_0/L0_ros_action_server_test1.vhd +++ b/src/ros2/Tests/Level_0/L0_ros_action_server_test1.vhd @@ -22,8 +22,13 @@ use work.GoalStatusArray_package; -- TEST: NEW GOAL [FULL GOAL MEMORY, TIMEOUT/=INFINITE] -- TEST: GET_GOAL -- TEST: GET_GOAL [INVALID HANDLE] +-- TEST: GET_FIRST_GOAL [MEMORY NOT EMPTY] +-- TEST: GET_FIRST_GOAL [MEMORY EMPTY] -- TEST: GET_LAST_GOAL [MEMORY NOT EMPTY] -- TEST: GET_LAST_GOAL [MEMORY EMPTY] +-- TEST: GET_NEXT_GOAL [NEXT GOAL EXISTS] +-- TEST: GET_NEXT_GOAL [NEXT GOAL DOES NOT EXISTS] +-- TEST: GET_NEXT_GOAL [INVALID HANDLE] -- TEST: GET_PREVIOUS_GOAL [PREVIOUS GOAL EXISTS] -- TEST: GET_PREVIOUS_GOAL [PREVIOUS GOAL DOES NOT EXISTS] -- TEST: GET_PREVIOUS_GOAL [INVALID HANDLE] @@ -713,10 +718,20 @@ begin wait until rising_edge(clk); reset <= '0'; + -- TEST: GET_FIRST_GOAL [MEMORY EMPTY] + + Log("GET_FIRST_GOAL [No Goal]", INFO); + t <= gen_duration(1 sec); + user_op(GET_FIRST_GOAL); + wait for 1 ps; -- Make sure all signals stable + AffirmIfEqual(STATUS, goal_handle_out, GOAL_HANDLE_UNKNOWN); + AlertIf(return_code /= ROS_RET_OK, "User operation return code incorrect", FAILURE); + wait_on_idle; + -- TEST: GET_LAST_GOAL [MEMORY EMPTY] Log("GET_LAST_GOAL [No Goal]", INFO); - t <= gen_duration(1 sec); + t <= gen_duration(2 sec); user_op(GET_LAST_GOAL); wait for 1 ps; -- Make sure all signals stable AffirmIfEqual(STATUS, goal_handle_out, GOAL_HANDLE_UNKNOWN); @@ -726,17 +741,27 @@ begin -- TEST: GET_GOAL [INVALID HANDLE] Log("GET_GOAL [Invalid Handle]", INFO); - t <= gen_duration(2 sec); + t <= gen_duration(3 sec); goal_handle_in <= (others => '0'); user_op(GET_GOAL); wait for 1 ps; -- Make sure all signals stable AlertIf(return_code /= ROS_RET_ACTION_GOAL_HANDLE_INVALID, "User operation return code incorrect", FAILURE); wait_on_idle; + -- TEST: GET_NEXT_GOAL [INVALID HANDLE] + + Log("GET_NEXT_GOAL [Invalid Handle]", INFO); + t <= gen_duration(4 sec); + goal_handle_in <= (others => '0'); + user_op(GET_NEXT_GOAL); + wait for 1 ps; -- Make sure all signals stable + AlertIf(return_code /= ROS_RET_ACTION_GOAL_HANDLE_INVALID, "User operation return code incorrect", FAILURE); + wait_on_idle; + -- TEST: GET_PREVIOUS_GOAL [INVALID HANDLE] Log("GET_PREVIOUS_GOAL [Invalid Handle]", INFO); - t <= gen_duration(3 sec); + t <= gen_duration(5 sec); goal_handle_in <= (others => '0'); user_op(GET_PREVIOUS_GOAL); wait for 1 ps; -- Make sure all signals stable @@ -746,7 +771,7 @@ begin -- TEST: UPDATE_GOAL_STATE [INVALID HANDLE] Log("UPDATE_GOAL_STATE [Invalid Handle]", INFO); - t <= gen_duration(4 sec); + t <= gen_duration(6 sec); goal_handle_in <= (others => '0'); goal_state_in <= STATUS_EXECUTING; user_op(UPDATE_GOAL_STATE); @@ -757,7 +782,7 @@ begin -- TEST: NEW RESULT REQUEST [MEMORY EMPTY] Log("New Result Request [Goal 1]", INFO); - t <= gen_duration(5 sec); + t <= gen_duration(7 sec); get_result_request(2, gen_goal_id); get_result_response(5,2); wait_on_idle; @@ -766,7 +791,7 @@ begin -- TEST: CANCEL REQUEST [NO GOAL ID SET, NO TIME SET, NO GOALS EXIST] Log("Cancel Request [ALL]", INFO); - t <= gen_duration(6 sec); + t <= gen_duration(8 sec); test_cancel := (others => MAX_GOALS); get_cancel_request((others => '0'), (others => '0')); get_cancel_response(CancelGoal_package.RR_ERROR_REJECTED); @@ -777,7 +802,7 @@ begin -- TEST: NEW GOAL [MEMORY EMPTY] Log("New Goal [Goal 1, Accept]", INFO); - t <= gen_duration(7 sec); + t <= gen_duration(9 sec); get_goal_request(0, goal_id_r); user_input_goal(0, TRUE); get_goal_response(0); @@ -786,28 +811,28 @@ begin -- TEST: NEW GOAL [REJECT] Log("New Goal [Goal 2, Reject]", INFO); - t <= gen_duration(8 sec); + t <= gen_duration(10 sec); get_goal_request(5, gen_goal_id); user_input_goal(5,FALSE); get_goal_response(5); wait_on_idle; Log("New Goal [Goal 3, Accept]", INFO); - t <= gen_duration(9 sec); + t <= gen_duration(11 sec); get_goal_request(1, gen_goal_id); user_input_goal(1,TRUE); get_goal_response(1); wait_on_idle; Log("New Goal [Goal 4, Accept]", INFO); - t <= gen_duration(10 sec); + t <= gen_duration(12 sec); get_goal_request(2, gen_goal_id); user_input_goal(2,TRUE); get_goal_response(2); wait_on_idle; Log("New Goal [Goal 5, Accept]", INFO); - t <= gen_duration(11 sec); + t <= gen_duration(13 sec); get_goal_request(3, gen_goal_id); user_input_goal(3,TRUE); get_goal_response(3); @@ -816,7 +841,7 @@ begin -- TEST: NEW GOAL [FULL GOAL MEMORY, TIMEOUT/=INFINITE] data_available_g <= '1'; - t <= gen_duration(12 sec); + t <= gen_duration(14 sec); wait until rising_edge(clk); wait until rising_edge(clk); AlertIf(start_r = '1', "Goal service operation on full memory", FAILURE); @@ -825,7 +850,7 @@ begin -- TEST: STATUS UPDATE [GOAL UPDATE] Log("UPDATE_GOAL_STATE [Goal 3, Executing]", INFO); - t <= gen_duration(13 sec); + t <= gen_duration(15 sec); goal_handle_in <= test_goals(1).handle; goal_state_in <= STATUS_EXECUTING; user_op(UPDATE_GOAL_STATE); @@ -835,7 +860,7 @@ begin wait_on_idle; Log("UPDATE_GOAL_STATE [Goal 5, Executing]", INFO); - t <= gen_duration(14 sec); + t <= gen_duration(16 sec); goal_handle_in <= test_goals(3).handle; goal_state_in <= STATUS_EXECUTING; user_op(UPDATE_GOAL_STATE); @@ -844,13 +869,13 @@ begin AlertIf(return_code /= ROS_RET_OK, "User operation return code incorrect", FAILURE); wait_on_idle; - -- TEST: GET_GOAL + -- TEST: GET_FIRST_GOAL [MEMORY NOT EMPTY] - Log("GET_GOAL [Goal 1]", INFO); - t <= gen_duration(15 sec); - goal_handle_in <= test_goals(0).handle; - user_op(GET_GOAL); + Log("GET_FIRST_GOAL [Goal 5]", INFO); + t <= gen_duration(17 sec); + user_op(GET_FIRST_GOAL); wait for 1 ps; -- Make sure all signals stable + AffirmIfEqual(STATUS, goal_handle_out, test_goals(0).handle); AffirmIfEqual(STATUS, goal_id, test_goals(0).goal_id); AffirmIfEqual(STATUS, std_logic_vector(to_unsigned(goal_stamp)), test_goals(0).stamp); AffirmIfEqual(STATUS, goal_state_out, test_goals(0).state); @@ -860,7 +885,7 @@ begin -- TEST: GET_LAST_GOAL [MEMORY NOT EMPTY] Log("GET_LAST_GOAL [Goal 5]", INFO); - t <= gen_duration(16 sec); + t <= gen_duration(18 sec); user_op(GET_LAST_GOAL); wait for 1 ps; -- Make sure all signals stable AffirmIfEqual(STATUS, goal_handle_out, test_goals(3).handle); @@ -870,32 +895,6 @@ begin AlertIf(return_code /= ROS_RET_OK, "User operation return code incorrect", FAILURE); wait_on_idle; - -- TEST: GET_PREVIOUS_GOAL [PREVIOUS GOAL EXISTS] - - Log("GET_PREVIOUS_GOAL [Goal 4]", INFO); - t <= gen_duration(17 sec); - goal_handle_in <= test_goals(3).handle; - user_op(GET_PREVIOUS_GOAL); - wait for 1 ps; -- Make sure all signals stable - AffirmIfEqual(STATUS, goal_handle_out, test_goals(2).handle); - AffirmIfEqual(STATUS, goal_id, test_goals(2).goal_id); - AffirmIfEqual(STATUS, std_logic_vector(to_unsigned(goal_stamp)), test_goals(2).stamp); - AffirmIfEqual(STATUS, goal_state_out, test_goals(2).state); - AlertIf(return_code /= ROS_RET_OK, "User operation return code incorrect", FAILURE); - wait_on_idle; - - Log("GET_PREVIOUS_GOAL [Goal 3]", INFO); - t <= gen_duration(18 sec); - goal_handle_in <= test_goals(2).handle; - user_op(GET_PREVIOUS_GOAL); - wait for 1 ps; -- Make sure all signals stable - AffirmIfEqual(STATUS, goal_handle_out, test_goals(1).handle); - AffirmIfEqual(STATUS, goal_id, test_goals(1).goal_id); - AffirmIfEqual(STATUS, std_logic_vector(to_unsigned(goal_stamp)), test_goals(1).stamp); - AffirmIfEqual(STATUS, goal_state_out, test_goals(1).state); - AlertIf(return_code /= ROS_RET_OK, "User operation return code incorrect", FAILURE); - wait_on_idle; - -- TEST: GET_PREVIOUS_GOAL [PREVIOUS GOAL DOES NOT EXISTS] Log("GET_PREVIOUS_GOAL [No Goal]", INFO); @@ -907,10 +906,49 @@ begin AlertIf(return_code /= ROS_RET_OK, "User operation return code incorrect", FAILURE); wait_on_idle; + -- TEST: GET_PREVIOUS_GOAL [PREVIOUS GOAL EXISTS] + + Log("GET_PREVIOUS_GOAL [Goal 4]", INFO); + t <= gen_duration(20 sec); + goal_handle_in <= test_goals(3).handle; + user_op(GET_PREVIOUS_GOAL); + wait for 1 ps; -- Make sure all signals stable + AffirmIfEqual(STATUS, goal_handle_out, test_goals(2).handle); + AffirmIfEqual(STATUS, goal_id, test_goals(2).goal_id); + AffirmIfEqual(STATUS, std_logic_vector(to_unsigned(goal_stamp)), test_goals(2).stamp); + AffirmIfEqual(STATUS, goal_state_out, test_goals(2).state); + AlertIf(return_code /= ROS_RET_OK, "User operation return code incorrect", FAILURE); + wait_on_idle; + + -- TEST: GET_NEXT_GOAL [NEXT GOAL DOES NOT EXISTS] + + Log("GET_NEXT_GOAL [No Goal]", INFO); + t <= gen_duration(21 sec); + goal_handle_in <= test_goals(3).handle; + user_op(GET_NEXT_GOAL); + wait for 1 ps; -- Make sure all signals stable + AffirmIfEqual(STATUS, goal_handle_out, GOAL_HANDLE_UNKNOWN); + AlertIf(return_code /= ROS_RET_OK, "User operation return code incorrect", FAILURE); + wait_on_idle; + + -- TEST: GET_NEXT_GOAL [NEXT GOAL EXISTS] + + Log("GET_NEXT_GOAL [Goal 0]", INFO); + t <= gen_duration(22 sec); + goal_handle_in <= test_goals(0).handle; + user_op(GET_NEXT_GOAL); + wait for 1 ps; -- Make sure all signals stable + AffirmIfEqual(STATUS, goal_handle_out, test_goals(1).handle); + AffirmIfEqual(STATUS, goal_id, test_goals(1).goal_id); + AffirmIfEqual(STATUS, std_logic_vector(to_unsigned(goal_stamp)), test_goals(1).stamp); + AffirmIfEqual(STATUS, goal_state_out, test_goals(1).state); + AlertIf(return_code /= ROS_RET_OK, "User operation return code incorrect", FAILURE); + wait_on_idle; + -- TEST: NEW RESULT REQUEST [GOAL DOES NOT EXIST] Log("New Result Request [Unknown Goal]", INFO); - t <= gen_duration(20 sec); + t <= gen_duration(23 sec); get_result_request(2, gen_goal_id); get_result_response(5,2); wait_on_idle; @@ -918,14 +956,14 @@ begin -- TEST: NEW RESULT REQUEST [GOAL NOT TERMINAL] Log("New Result Request [Goal 1, Store]", INFO); - t <= gen_duration(21 sec); + t <= gen_duration(24 sec); get_result_request(0, test_goals(0).goal_id); wait_on_idle; -- TEST: STORED RESULT REQUEST [GOAL STATE UPDATES] Log("UPDATE_GOAL_STATE [Goal 1, Executing]", INFO); - t <= gen_duration(22 sec); + t <= gen_duration(25 sec); goal_handle_in <= test_goals(0).handle; goal_state_in <= STATUS_EXECUTING; user_op(UPDATE_GOAL_STATE); @@ -935,7 +973,7 @@ begin wait_on_idle; Log("UPDATE_GOAL_STATE [Goal 5, Succeeded]", INFO); - t <= gen_duration(23 sec); + t <= gen_duration(26 sec); goal_handle_in <= test_goals(3).handle; goal_state_in <= STATUS_SUCCEEDED; user_op(UPDATE_GOAL_STATE); @@ -947,20 +985,20 @@ begin -- TEST: NEW RESULT REQUEST [GOAL TERMINAL] Log("New Result Request [Goal 5]", INFO); - t <= gen_duration(24 sec); + t <= gen_duration(27 sec); get_result_request(2, test_goals(3).goal_id); get_result_response(3,2); wait_on_idle; Log("New Result Request [Goal 3, Store]", INFO); - t <= gen_duration(25 sec); + t <= gen_duration(28 sec); get_result_request(1, test_goals(1).goal_id); wait_on_idle; -- TEST: NEW RESULT REQUEST [FULL RESULT MEMORY] data_available_r <= '1'; - t <= gen_duration(26 sec); + t <= gen_duration(29 sec); wait until rising_edge(clk); wait until rising_edge(clk); AlertIf(start_r = '1', "Result service operation on full memory", FAILURE); @@ -968,7 +1006,7 @@ begin -- TEST: STORED RESULT REQUEST [GOAL STATE CHANGES TO TERMINAL] Log("UPDATE_GOAL_STATE [Goal 1, Aborted]", INFO); - t <= gen_duration(27 sec); + t <= gen_duration(30 sec); goal_handle_in <= test_goals(0).handle; goal_state_in <= STATUS_ABORTED; user_op(UPDATE_GOAL_STATE); @@ -980,14 +1018,14 @@ begin AlertIf(start_r /= '1', "No Result service operation on available memory", FAILURE); Log("New Result Request [Goal 4, Store]", INFO); - t <= gen_duration(28 sec); + t <= gen_duration(31 sec); get_result_request(0, test_goals(2).goal_id); wait_on_idle; -- TEST: UPDATE_GOAL_STATE [INVALID STATE] Log("UPDATE_GOAL_STATE [Goal 1, Invalid State Transition]", INFO); - t <= gen_duration(29 sec); + t <= gen_duration(32 sec); goal_handle_in <= test_goals(0).handle; goal_state_in <= STATUS_SUCCEEDED; user_op(UPDATE_GOAL_STATE); @@ -998,7 +1036,7 @@ begin -- TEST: CANCEL REQUEST [GOAL ID SET, GOAL DOES NOT EXISTS] Log("Cancel Request [Unknown Goal] (ERROR_REJECTED)", INFO); - t <= gen_duration(30 sec); + t <= gen_duration(33 sec); test_cancel := (others => MAX_GOALS); get_cancel_request(gen_goal_id, (others => '0')); get_cancel_response(CancelGoal_package.RR_ERROR_UNKNOWN_GOAL_ID); @@ -1007,7 +1045,7 @@ begin -- TEST: CANCEL REQUEST [GOAL ID SET, GOAL EXISTS, GOAL TERMINAL] Log("Cancel Request [Goal 1] (ERROR_GOAL_TERMINATED)", INFO); - t <= gen_duration(31 sec); + t <= gen_duration(34 sec); test_cancel := (others => MAX_GOALS); get_cancel_request(test_goals(0).goal_id, (others => '0')); get_cancel_response(CancelGoal_package.RR_ERROR_GOAL_TERMINATED); @@ -1017,7 +1055,7 @@ begin -- TEST: STATUS UPDATE [GOAL CANCELED] Log("Cancel Request [Goal 3] (Goal 3 Accept)", INFO); - t <= gen_duration(32 sec); + t <= gen_duration(35 sec); test_cancel := (0 => 1, others => MAX_GOALS); get_cancel_request(test_goals(1).goal_id, (others => '0')); user_input_cancel(1,TRUE); @@ -1027,7 +1065,7 @@ begin -- TEST: CANCEL REQUEST [NO GOAL ID SET, NO TIME SET, GOALS EXIST] Log("Cancel Request [ALL] (Goal 4 Accept)", INFO); - t <= gen_duration(33 sec); + t <= gen_duration(36 sec); test_cancel := (0 => 2, 1 => 1, others => MAX_GOALS); get_cancel_request((others => '0'), (others => '0')); user_input_cancel(2,TRUE); @@ -1035,7 +1073,7 @@ begin wait_on_idle; Log("UPDATE_GOAL_STATE [Goal 3, Canceled]", INFO); - t <= gen_duration(34 sec); + t <= gen_duration(37 sec); goal_handle_in <= test_goals(1).handle; goal_state_in <= STATUS_CANCELED; user_op(UPDATE_GOAL_STATE); @@ -1046,7 +1084,7 @@ begin wait_on_idle; Log("UPDATE_GOAL_STATE [Goal 4, Abort]", INFO); - t <= gen_duration(35 sec); + t <= gen_duration(38 sec); goal_handle_in <= test_goals(2).handle; goal_state_in <= STATUS_ABORTED; user_op(UPDATE_GOAL_STATE); @@ -1060,7 +1098,7 @@ begin -- TEST: STATUS UPDATE [GOAL REMOVED] Log("Expire Goal [Goal 5]", INFO); - t <= gen_duration(43 sec); + t <= gen_duration(46 sec); test_goal_cnt := test_goal_cnt - 1; wait_on_idle; AlertIf(start_g /= '1', "No Goal service operation on available memory", FAILURE); @@ -1072,12 +1110,25 @@ begin get_goal_response(3); wait_on_idle; + -- TEST: GET_GOAL + + Log("GET_GOAL [Goal 6]", INFO); + t <= gen_duration(47 sec); + goal_handle_in <= test_goals(3).handle; + user_op(GET_GOAL); + wait for 1 ps; -- Make sure all signals stable + AffirmIfEqual(STATUS, goal_id, test_goals(3).goal_id); + AffirmIfEqual(STATUS, std_logic_vector(to_unsigned(goal_stamp)), test_goals(3).stamp); + AffirmIfEqual(STATUS, goal_state_out, test_goals(3).state); + AlertIf(return_code /= ROS_RET_OK, "User operation return code incorrect", FAILURE); + wait_on_idle; + -- TEST: CANCEL REQUEST [TIME SET, NO RELEVANT GOALS] Log("Cancel Request [Time Set] (NO GOALS)", INFO); - t <= gen_duration(44 sec); + t <= gen_duration(48 sec); test_cancel := (others => MAX_GOALS); - get_cancel_request((others => '0'), ros_gen_time(40 sec)); + get_cancel_request((others => '0'), ros_gen_time(45 sec)); get_cancel_response(CancelGoal_package.RR_ERROR_REJECTED); wait_on_idle; @@ -1094,7 +1145,7 @@ begin Log("Cancel Request [Time Set] (Goal 6 Accept)", INFO); t <= gen_duration(61 sec); test_cancel := (0 => 0, others => MAX_GOALS); - get_cancel_request((others => '0'), ros_gen_time(43 sec)); + get_cancel_request((others => '0'), ros_gen_time(46 sec)); user_input_cancel(0,TRUE); get_cancel_response(CancelGoal_package.RR_ERROR_NONE); wait_on_idle; diff --git a/src/ros2/Tests/Level_0/L0_ros_action_server_test2.vhd b/src/ros2/Tests/Level_0/L0_ros_action_server_test2.vhd index 58f1fef..7f451df 100644 --- a/src/ros2/Tests/Level_0/L0_ros_action_server_test2.vhd +++ b/src/ros2/Tests/Level_0/L0_ros_action_server_test2.vhd @@ -23,8 +23,13 @@ use work.GoalStatusArray_package; -- TEST: NEW GOAL [FULL GOAL MEMORY, TIMEOUT=INFINITE, TERMINAL STATE GOAL EXISTS] -- TEST: GET_GOAL -- TEST: GET_GOAL [INVALID HANDLE] +-- TEST: GET_FIRST_GOAL [MEMORY NOT EMPTY] +-- TEST: GET_FIRST_GOAL [MEMORY EMPTY] -- TEST: GET_LAST_GOAL [MEMORY NOT EMPTY] -- TEST: GET_LAST_GOAL [MEMORY EMPTY] +-- TEST: GET_NEXT_GOAL [NEXT GOAL EXISTS] +-- TEST: GET_NEXT_GOAL [NEXT GOAL DOES NOT EXISTS] +-- TEST: GET_NEXT_GOAL [INVALID HANDLE] -- TEST: GET_PREVIOUS_GOAL [PREVIOUS GOAL EXISTS] -- TEST: GET_PREVIOUS_GOAL [PREVIOUS GOAL DOES NOT EXISTS] -- TEST: GET_PREVIOUS_GOAL [INVALID HANDLE] @@ -710,10 +715,20 @@ begin wait until rising_edge(clk); reset <= '0'; + -- TEST: GET_FIRST_GOAL [MEMORY EMPTY] + + Log("GET_FIRST_GOAL [No Goal]", INFO); + t <= gen_duration(1 sec); + user_op(GET_FIRST_GOAL); + wait for 1 ps; -- Make sure all signals stable + AffirmIfEqual(STATUS, goal_handle_out, GOAL_HANDLE_UNKNOWN); + AlertIf(return_code /= ROS_RET_OK, "User operation return code incorrect", FAILURE); + wait_on_idle; + -- TEST: GET_LAST_GOAL [MEMORY EMPTY] Log("GET_LAST_GOAL [No Goal]", INFO); - t <= gen_duration(1 sec); + t <= gen_duration(2 sec); user_op(GET_LAST_GOAL); wait for 1 ps; -- Make sure all signals stable AffirmIfEqual(STATUS, goal_handle_out, GOAL_HANDLE_UNKNOWN); @@ -723,17 +738,27 @@ begin -- TEST: GET_GOAL [INVALID HANDLE] Log("GET_GOAL [Invalid Handle]", INFO); - t <= gen_duration(2 sec); + t <= gen_duration(3 sec); goal_handle_in <= (others => '0'); user_op(GET_GOAL); wait for 1 ps; -- Make sure all signals stable AlertIf(return_code /= ROS_RET_ACTION_GOAL_HANDLE_INVALID, "User operation return code incorrect", FAILURE); wait_on_idle; + -- TEST: GET_NEXT_GOAL [INVALID HANDLE] + + Log("GET_NEXT_GOAL [Invalid Handle]", INFO); + t <= gen_duration(4 sec); + goal_handle_in <= (others => '0'); + user_op(GET_NEXT_GOAL); + wait for 1 ps; -- Make sure all signals stable + AlertIf(return_code /= ROS_RET_ACTION_GOAL_HANDLE_INVALID, "User operation return code incorrect", FAILURE); + wait_on_idle; + -- TEST: GET_PREVIOUS_GOAL [INVALID HANDLE] Log("GET_PREVIOUS_GOAL [Invalid Handle]", INFO); - t <= gen_duration(3 sec); + t <= gen_duration(5 sec); goal_handle_in <= (others => '0'); user_op(GET_PREVIOUS_GOAL); wait for 1 ps; -- Make sure all signals stable @@ -743,7 +768,7 @@ begin -- TEST: UPDATE_GOAL_STATE [INVALID HANDLE] Log("UPDATE_GOAL_STATE [Invalid Handle]", INFO); - t <= gen_duration(4 sec); + t <= gen_duration(6 sec); goal_handle_in <= (others => '0'); goal_state_in <= STATUS_EXECUTING; user_op(UPDATE_GOAL_STATE); @@ -754,7 +779,7 @@ begin -- TEST: NEW RESULT REQUEST [MEMORY EMPTY] Log("New Result Request [Goal 1]", INFO); - t <= gen_duration(5 sec); + t <= gen_duration(7 sec); get_result_request(2, gen_goal_id); get_result_response(5,2); wait_on_idle; @@ -763,7 +788,7 @@ begin -- TEST: CANCEL REQUEST [NO GOAL ID SET, NO TIME SET, NO GOALS EXIST] Log("Cancel Request [ALL]", INFO); - t <= gen_duration(6 sec); + t <= gen_duration(8 sec); test_cancel := (others => MAX_GOALS); get_cancel_request((others => '0'), (others => '0')); get_cancel_response(CancelGoal_package.RR_ERROR_REJECTED); @@ -774,7 +799,7 @@ begin -- TEST: NEW GOAL [MEMORY EMPTY] Log("New Goal [Goal 1, Accept]", INFO); - t <= gen_duration(7 sec); + t <= gen_duration(9 sec); get_goal_request(0, goal_id_r); user_input_goal(0, TRUE); get_goal_response(0); @@ -783,28 +808,28 @@ begin -- TEST: NEW GOAL [REJECT] Log("New Goal [Goal 2, Reject]", INFO); - t <= gen_duration(8 sec); + t <= gen_duration(10 sec); get_goal_request(5, gen_goal_id); user_input_goal(5,FALSE); get_goal_response(5); wait_on_idle; Log("New Goal [Goal 3, Accept]", INFO); - t <= gen_duration(9 sec); + t <= gen_duration(11 sec); get_goal_request(1, gen_goal_id); user_input_goal(1,TRUE); get_goal_response(1); wait_on_idle; Log("New Goal [Goal 4, Accept]", INFO); - t <= gen_duration(10 sec); + t <= gen_duration(12 sec); get_goal_request(2, gen_goal_id); user_input_goal(2,TRUE); get_goal_response(2); wait_on_idle; Log("New Goal [Goal 5, Accept]", INFO); - t <= gen_duration(11 sec); + t <= gen_duration(13 sec); get_goal_request(3, gen_goal_id); user_input_goal(3,TRUE); get_goal_response(3); @@ -813,7 +838,7 @@ begin -- TEST: NEW GOAL [FULL GOAL MEMORY, TIMEOUT=INFINITE, NO TERMINAL STATE GOAL EXISTS] data_available_g <= '1'; - t <= gen_duration(12 sec); + t <= gen_duration(14 sec); wait until rising_edge(clk); wait until rising_edge(clk); AlertIf(start_r = '1', "Goal service operation on full memory", FAILURE); @@ -822,7 +847,7 @@ begin -- TEST: STATUS UPDATE [GOAL UPDATE] Log("UPDATE_GOAL_STATE [Goal 3, Executing]", INFO); - t <= gen_duration(13 sec); + t <= gen_duration(15 sec); goal_handle_in <= test_goals(1).handle; goal_state_in <= STATUS_EXECUTING; user_op(UPDATE_GOAL_STATE); @@ -832,7 +857,7 @@ begin wait_on_idle; Log("UPDATE_GOAL_STATE [Goal 5, Executing]", INFO); - t <= gen_duration(14 sec); + t <= gen_duration(16 sec); goal_handle_in <= test_goals(3).handle; goal_state_in <= STATUS_EXECUTING; user_op(UPDATE_GOAL_STATE); @@ -841,13 +866,13 @@ begin AlertIf(return_code /= ROS_RET_OK, "User operation return code incorrect", FAILURE); wait_on_idle; - -- TEST: GET_GOAL + -- TEST: GET_FIRST_GOAL [MEMORY NOT EMPTY] - Log("GET_GOAL [Goal 1]", INFO); - t <= gen_duration(15 sec); - goal_handle_in <= test_goals(0).handle; - user_op(GET_GOAL); + Log("GET_FIRST_GOAL [Goal 5]", INFO); + t <= gen_duration(17 sec); + user_op(GET_FIRST_GOAL); wait for 1 ps; -- Make sure all signals stable + AffirmIfEqual(STATUS, goal_handle_out, test_goals(0).handle); AffirmIfEqual(STATUS, goal_id, test_goals(0).goal_id); AffirmIfEqual(STATUS, std_logic_vector(to_unsigned(goal_stamp)), test_goals(0).stamp); AffirmIfEqual(STATUS, goal_state_out, test_goals(0).state); @@ -857,7 +882,7 @@ begin -- TEST: GET_LAST_GOAL [MEMORY NOT EMPTY] Log("GET_LAST_GOAL [Goal 5]", INFO); - t <= gen_duration(16 sec); + t <= gen_duration(18 sec); user_op(GET_LAST_GOAL); wait for 1 ps; -- Make sure all signals stable AffirmIfEqual(STATUS, goal_handle_out, test_goals(3).handle); @@ -867,32 +892,6 @@ begin AlertIf(return_code /= ROS_RET_OK, "User operation return code incorrect", FAILURE); wait_on_idle; - -- TEST: GET_PREVIOUS_GOAL [PREVIOUS GOAL EXISTS] - - Log("GET_PREVIOUS_GOAL [Goal 4]", INFO); - t <= gen_duration(17 sec); - goal_handle_in <= test_goals(3).handle; - user_op(GET_PREVIOUS_GOAL); - wait for 1 ps; -- Make sure all signals stable - AffirmIfEqual(STATUS, goal_handle_out, test_goals(2).handle); - AffirmIfEqual(STATUS, goal_id, test_goals(2).goal_id); - AffirmIfEqual(STATUS, std_logic_vector(to_unsigned(goal_stamp)), test_goals(2).stamp); - AffirmIfEqual(STATUS, goal_state_out, test_goals(2).state); - AlertIf(return_code /= ROS_RET_OK, "User operation return code incorrect", FAILURE); - wait_on_idle; - - Log("GET_PREVIOUS_GOAL [Goal 3]", INFO); - t <= gen_duration(18 sec); - goal_handle_in <= test_goals(2).handle; - user_op(GET_PREVIOUS_GOAL); - wait for 1 ps; -- Make sure all signals stable - AffirmIfEqual(STATUS, goal_handle_out, test_goals(1).handle); - AffirmIfEqual(STATUS, goal_id, test_goals(1).goal_id); - AffirmIfEqual(STATUS, std_logic_vector(to_unsigned(goal_stamp)), test_goals(1).stamp); - AffirmIfEqual(STATUS, goal_state_out, test_goals(1).state); - AlertIf(return_code /= ROS_RET_OK, "User operation return code incorrect", FAILURE); - wait_on_idle; - -- TEST: GET_PREVIOUS_GOAL [PREVIOUS GOAL DOES NOT EXISTS] Log("GET_PREVIOUS_GOAL [No Goal]", INFO); @@ -904,10 +903,49 @@ begin AlertIf(return_code /= ROS_RET_OK, "User operation return code incorrect", FAILURE); wait_on_idle; + -- TEST: GET_PREVIOUS_GOAL [PREVIOUS GOAL EXISTS] + + Log("GET_PREVIOUS_GOAL [Goal 4]", INFO); + t <= gen_duration(20 sec); + goal_handle_in <= test_goals(3).handle; + user_op(GET_PREVIOUS_GOAL); + wait for 1 ps; -- Make sure all signals stable + AffirmIfEqual(STATUS, goal_handle_out, test_goals(2).handle); + AffirmIfEqual(STATUS, goal_id, test_goals(2).goal_id); + AffirmIfEqual(STATUS, std_logic_vector(to_unsigned(goal_stamp)), test_goals(2).stamp); + AffirmIfEqual(STATUS, goal_state_out, test_goals(2).state); + AlertIf(return_code /= ROS_RET_OK, "User operation return code incorrect", FAILURE); + wait_on_idle; + + -- TEST: GET_NEXT_GOAL [NEXT GOAL DOES NOT EXISTS] + + Log("GET_NEXT_GOAL [No Goal]", INFO); + t <= gen_duration(21 sec); + goal_handle_in <= test_goals(3).handle; + user_op(GET_NEXT_GOAL); + wait for 1 ps; -- Make sure all signals stable + AffirmIfEqual(STATUS, goal_handle_out, GOAL_HANDLE_UNKNOWN); + AlertIf(return_code /= ROS_RET_OK, "User operation return code incorrect", FAILURE); + wait_on_idle; + + -- TEST: GET_NEXT_GOAL [NEXT GOAL EXISTS] + + Log("GET_NEXT_GOAL [Goal 0]", INFO); + t <= gen_duration(22 sec); + goal_handle_in <= test_goals(0).handle; + user_op(GET_NEXT_GOAL); + wait for 1 ps; -- Make sure all signals stable + AffirmIfEqual(STATUS, goal_handle_out, test_goals(1).handle); + AffirmIfEqual(STATUS, goal_id, test_goals(1).goal_id); + AffirmIfEqual(STATUS, std_logic_vector(to_unsigned(goal_stamp)), test_goals(1).stamp); + AffirmIfEqual(STATUS, goal_state_out, test_goals(1).state); + AlertIf(return_code /= ROS_RET_OK, "User operation return code incorrect", FAILURE); + wait_on_idle; + -- TEST: NEW RESULT REQUEST [GOAL DOES NOT EXIST] Log("New Result Request [Unknown Goal]", INFO); - t <= gen_duration(20 sec); + t <= gen_duration(23 sec); get_result_request(2, gen_goal_id); get_result_response(5,2); wait_on_idle; @@ -915,14 +953,14 @@ begin -- TEST: NEW RESULT REQUEST [GOAL NOT TERMINAL] Log("New Result Request [Goal 1, Store]", INFO); - t <= gen_duration(21 sec); + t <= gen_duration(24 sec); get_result_request(0, test_goals(0).goal_id); wait_on_idle; -- TEST: STORED RESULT REQUEST [GOAL STATE UPDATES] Log("UPDATE_GOAL_STATE [Goal 1, Executing]", INFO); - t <= gen_duration(22 sec); + t <= gen_duration(25 sec); goal_handle_in <= test_goals(0).handle; goal_state_in <= STATUS_EXECUTING; user_op(UPDATE_GOAL_STATE); @@ -934,7 +972,7 @@ begin data_available_g <= '0'; Log("UPDATE_GOAL_STATE [Goal 5, Succeeded]", INFO); - t <= gen_duration(23 sec); + t <= gen_duration(26 sec); goal_handle_in <= test_goals(3).handle; goal_state_in <= STATUS_SUCCEEDED; user_op(UPDATE_GOAL_STATE); @@ -946,20 +984,20 @@ begin -- TEST: NEW RESULT REQUEST [GOAL TERMINAL] Log("New Result Request [Goal 5]", INFO); - t <= gen_duration(24 sec); + t <= gen_duration(27 sec); get_result_request(2, test_goals(3).goal_id); get_result_response(3,2); wait_on_idle; Log("New Result Request [Goal 3, Store]", INFO); - t <= gen_duration(25 sec); + t <= gen_duration(28 sec); get_result_request(1, test_goals(1).goal_id); wait_on_idle; -- TEST: NEW RESULT REQUEST [FULL RESULT MEMORY] data_available_r <= '1'; - t <= gen_duration(26 sec); + t <= gen_duration(29 sec); wait until rising_edge(clk); wait until rising_edge(clk); AlertIf(start_r = '1', "Result service operation on full memory", FAILURE); @@ -967,7 +1005,7 @@ begin -- TEST: STORED RESULT REQUEST [GOAL STATE CHANGES TO TERMINAL] Log("UPDATE_GOAL_STATE [Goal 1, Aborted]", INFO); - t <= gen_duration(27 sec); + t <= gen_duration(30 sec); goal_handle_in <= test_goals(0).handle; goal_state_in <= STATUS_ABORTED; user_op(UPDATE_GOAL_STATE); @@ -979,14 +1017,14 @@ begin AlertIf(start_r /= '1', "No Result service operation on available memory", FAILURE); Log("New Result Request [Goal 4, Store]", INFO); - t <= gen_duration(28 sec); + t <= gen_duration(31 sec); get_result_request(0, test_goals(2).goal_id); wait_on_idle; -- TEST: UPDATE_GOAL_STATE [INVALID STATE] Log("UPDATE_GOAL_STATE [Goal 1, Invalid State Transition]", INFO); - t <= gen_duration(29 sec); + t <= gen_duration(32 sec); goal_handle_in <= test_goals(0).handle; goal_state_in <= STATUS_SUCCEEDED; user_op(UPDATE_GOAL_STATE); @@ -997,7 +1035,7 @@ begin -- TEST: CANCEL REQUEST [GOAL ID SET, GOAL DOES NOT EXISTS] Log("Cancel Request [Unknown Goal] (ERROR_REJECTED)", INFO); - t <= gen_duration(30 sec); + t <= gen_duration(33 sec); test_cancel := (others => MAX_GOALS); get_cancel_request(gen_goal_id, (others => '0')); get_cancel_response(CancelGoal_package.RR_ERROR_UNKNOWN_GOAL_ID); @@ -1006,7 +1044,7 @@ begin -- TEST: CANCEL REQUEST [GOAL ID SET, GOAL EXISTS, GOAL TERMINAL] Log("Cancel Request [Goal 1] (ERROR_GOAL_TERMINATED)", INFO); - t <= gen_duration(31 sec); + t <= gen_duration(34 sec); test_cancel := (others => MAX_GOALS); get_cancel_request(test_goals(0).goal_id, (others => '0')); get_cancel_response(CancelGoal_package.RR_ERROR_GOAL_TERMINATED); @@ -1016,7 +1054,7 @@ begin -- TEST: STATUS UPDATE [GOAL CANCELED] Log("Cancel Request [Goal 3] (Goal 3 Accept)", INFO); - t <= gen_duration(32 sec); + t <= gen_duration(35 sec); test_cancel := (0 => 1, others => MAX_GOALS); get_cancel_request(test_goals(1).goal_id, (others => '0')); user_input_cancel(1,TRUE); @@ -1026,7 +1064,7 @@ begin -- TEST: CANCEL REQUEST [NO GOAL ID SET, NO TIME SET, GOALS EXIST] Log("Cancel Request [ALL] (Goal 4 Accept)", INFO); - t <= gen_duration(33 sec); + t <= gen_duration(36 sec); test_cancel := (0 => 2, 1 => 1, others => MAX_GOALS); get_cancel_request((others => '0'), (others => '0')); user_input_cancel(2,TRUE); @@ -1034,7 +1072,7 @@ begin wait_on_idle; Log("UPDATE_GOAL_STATE [Goal 3, Canceled]", INFO); - t <= gen_duration(34 sec); + t <= gen_duration(37 sec); goal_handle_in <= test_goals(1).handle; goal_state_in <= STATUS_CANCELED; user_op(UPDATE_GOAL_STATE); @@ -1045,7 +1083,7 @@ begin wait_on_idle; Log("UPDATE_GOAL_STATE [Goal 4, Abort]", INFO); - t <= gen_duration(35 sec); + t <= gen_duration(38 sec); goal_handle_in <= test_goals(2).handle; goal_state_in <= STATUS_ABORTED; user_op(UPDATE_GOAL_STATE); @@ -1059,7 +1097,7 @@ begin -- TEST: STATUS UPDATE [GOAL REMOVED] Log("New Goal [Goal 6, Accept]", INFO); - t <= gen_duration(43 sec); + t <= gen_duration(46 sec); test_goals(0) := test_goals(1); test_goals(1) := test_goals(2); test_goals(2) := test_goals(3); @@ -1069,12 +1107,25 @@ begin get_goal_response(3); wait_on_idle; + -- TEST: GET_GOAL + + Log("GET_GOAL [Goal 6]", INFO); + t <= gen_duration(47 sec); + goal_handle_in <= test_goals(3).handle; + user_op(GET_GOAL); + wait for 1 ps; -- Make sure all signals stable + AffirmIfEqual(STATUS, goal_id, test_goals(3).goal_id); + AffirmIfEqual(STATUS, std_logic_vector(to_unsigned(goal_stamp)), test_goals(3).stamp); + AffirmIfEqual(STATUS, goal_state_out, test_goals(3).state); + AlertIf(return_code /= ROS_RET_OK, "User operation return code incorrect", FAILURE); + wait_on_idle; + -- TEST: CANCEL REQUEST [TIME SET, NO RELEVANT GOALS] Log("Cancel Request [Time Set] (NO GOALS)", INFO); - t <= gen_duration(44 sec); + t <= gen_duration(48 sec); test_cancel := (others => MAX_GOALS); - get_cancel_request((others => '0'), ros_gen_time(40 sec)); + get_cancel_request((others => '0'), ros_gen_time(45 sec)); get_cancel_response(CancelGoal_package.RR_ERROR_REJECTED); wait_on_idle; @@ -1083,7 +1134,7 @@ begin Log("Cancel Request [Time Set] (Goal 6 Accept)", INFO); t <= gen_duration(61 sec); test_cancel := (0 => 3, others => MAX_GOALS); - get_cancel_request((others => '0'), ros_gen_time(43 sec)); + get_cancel_request((others => '0'), ros_gen_time(46 sec)); user_input_cancel(3,TRUE); get_cancel_response(CancelGoal_package.RR_ERROR_NONE); wait_on_idle; diff --git a/src/ros2/Tests/Level_0/ros_action_server_tests.txt b/src/ros2/Tests/Level_0/ros_action_server_tests.txt index 6b76a33..5498c51 100644 --- a/src/ros2/Tests/Level_0/ros_action_server_tests.txt +++ b/src/ros2/Tests/Level_0/ros_action_server_tests.txt @@ -7,8 +7,13 @@ -- TEST: GET_GOAL -- TEST: GET_GOAL [INVALID HANDLE] +-- TEST: GET_FIRST_GOAL [MEMORY NOT EMPTY] +-- TEST: GET_FIRST_GOAL [MEMORY EMPTY] -- TEST: GET_LAST_GOAL [MEMORY NOT EMPTY] -- TEST: GET_LAST_GOAL [MEMORY EMPTY] +-- TEST: GET_NEXT_GOAL [NEXT GOAL EXISTS] +-- TEST: GET_NEXT_GOAL [NEXT GOAL DOES NOT EXISTS] +-- TEST: GET_NEXT_GOAL [INVALID HANDLE] -- TEST: GET_PREVIOUS_GOAL [PREVIOUS GOAL EXISTS] -- TEST: GET_PREVIOUS_GOAL [PREVIOUS GOAL DOES NOT EXISTS] -- TEST: GET_PREVIOUS_GOAL [INVALID HANDLE] diff --git a/src/ros2/ros_action_server.vhd b/src/ros2/ros_action_server.vhd index c5acbad..e6a9694 100644 --- a/src/ros2/ros_action_server.vhd +++ b/src/ros2/ros_action_server.vhd @@ -593,6 +593,38 @@ begin return_code_latch_next <= ROS_RET_OK; end if; end if; + when GET_FIRST_GOAL => + -- No Goals Available + if (mem_occupied_tail = GOAL_MEMORY_MAX_ADDRESS) then + mem_data_r_next.addr <= GOAL_MEMORY_MAX_ADDRESS; + ack <= '1'; + return_code_latch_next <= ROS_RET_OK; + stage_next <= RETURN_USER; + else + -- Memory Operation Guard + if (mem_done = '1') then + mem_start <= '1'; + mem_opcode <= GET; + mem_r.field_flags <= GMF_GOAL_ID_FLAG or GMF_STAMP_FLAG or GMF_RESULT_INDEX_FLAG or GMF_STATE_FLAG; + mem_r.addr <= mem_occupied_tail; + + ack <= '1'; + stage_next <= WAIT_FOR_U_MEM; + return_code_latch_next <= ROS_RET_OK; + end if; + end if; + when GET_NEXT_GOAL => + -- Memory Operation Guard + if (mem_done = '1') then + mem_start <= '1'; + mem_opcode <= GET_PREV; + mem_r.field_flags <= GMF_GOAL_ID_FLAG or GMF_STAMP_FLAG or GMF_RESULT_INDEX_FLAG or GMF_STATE_FLAG; + mem_r.addr <= resize(unsigned(goal_handle_in),GOAL_MEMORY_ADDR_WIDTH); + + ack <= '1'; + stage_next <= WAIT_FOR_U_MEM; + return_code_latch_next <= ROS_RET_OK; + end if; when GET_PREVIOUS_GOAL => -- Memory Operation Guard if (mem_done = '1') then diff --git a/src/ros2/ros_package.vhd b/src/ros2/ros_package.vhd index d656933..bd8075e 100644 --- a/src/ros2/ros_package.vhd +++ b/src/ros2/ros_package.vhd @@ -273,7 +273,7 @@ package ros_package is type ROS_TOPIC_OPCODE_TYPE is (NOP, PUBLISH, TAKE); type ROS_SERVICE_OPCODE_TYPE is (NOP, SEND_REQUEST, TAKE_REQUEST, SEND_RESPONSE, TAKE_RESPONSE); - type ROS_ACTION_OPCODE_TYPE is (NOP, GET_GOAL, GET_LAST_GOAL, GET_PREVIOUS_GOAL, UPDATE_GOAL_STATE, PUBLISH_FEEDBACK, SEND_GOAL_REQUEST, TAKE_GOAL_RESPONSE, SEND_RESULT_REQUEST, TAKE_RESULT_RESPONSE, SEND_CANCEL_REQUEST, TAKE_CANCEL_RESPONSE, TAKE_FEEDBACK); + type ROS_ACTION_OPCODE_TYPE is (NOP, GET_GOAL, GET_LAST_GOAL, GET_FIRST_GOAL, GET_NEXT_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;