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

Add GEN_FIRST_GOAL and GET_NEXT_GOAL opcodes to ROS action server

Browse Source 
The functionality was already there, so we just extended the opcodes to
allow the user to also make use of them.
This commit is contained in:
John Ring 2022-03-05 16:45:01 +01:00
parent a064a44dc4
commit e4e702ebc4
5 changed files with 272 additions and 133 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

183
src/ros2/Tests/Level_0/L0_ros_action_server_test1.vhd
View File

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

183
src/ros2/Tests/Level_0/L0_ros_action_server_test2.vhd
View File

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

5
src/ros2/Tests/Level_0/ros_action_server_tests.txt
View File

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

32
src/ros2/ros_action_server.vhd
View File

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

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