From a92bd9ece183c2daf8d8a23407be84d4c6092c9c Mon Sep 17 00:00:00 2001 From: Greek64 Date: Sun, 17 Apr 2022 13:49:15 +0200 Subject: [PATCH] BUG FIX: ros_action_server was not using latched values --- src/TODO.txt | 9 +++++++++ src/ros2/Tests/Level_2/L2_Testbench_ROS_Lib4.vhd | 2 +- src/ros2/ros_action_server.vhd | 16 ++++++++-------- 3 files changed, 18 insertions(+), 9 deletions(-) diff --git a/src/TODO.txt b/src/TODO.txt index 47907e2..9744cc8 100644 --- a/src/TODO.txt +++ b/src/TODO.txt @@ -476,6 +476,15 @@ DESIGN DECISIONS addition of a new instance. That means that all Instances are checked at the smae time, non depending on when they were added. +* Because the DATA_AVAILABLE bit of the status of the DDS Reader is reset on first read, the DATA_AVAILABLE + bit can be zero even if the DDS Reader still contains unread data. For this reason the ROS entities are + showing data_available until the first empty read after the DATA_AVAILABLE bit is set. + This has the inverse effect, that even if the DDS Reader has no more data it is showing DATA_AVAILABLE + (since a empty read has to be done to reset the bit). This trait has to be taken into account. + The ros_action_server, for instance, will for instance remove a goal entering terminal state, if the + MAX_GOALS limit is reached + + BRAINSTORMING ------------- diff --git a/src/ros2/Tests/Level_2/L2_Testbench_ROS_Lib4.vhd b/src/ros2/Tests/Level_2/L2_Testbench_ROS_Lib4.vhd index 0ef32c6..ab445b9 100644 --- a/src/ros2/Tests/Level_2/L2_Testbench_ROS_Lib4.vhd +++ b/src/ros2/Tests/Level_2/L2_Testbench_ROS_Lib4.vhd @@ -700,7 +700,7 @@ begin Fibonacci_ros_action_server_inst : entity work.Fibonacci_ros_action_server(arch) generic map ( TIMEOUT_DURATION => ROS_DURATION_INFINITE, - MAX_GOALS => 1, + MAX_GOALS => 2, MAX_RESULT_REQUESTS => 1, ENABLE_FEEDBACK => '1' ) diff --git a/src/ros2/ros_action_server.vhd b/src/ros2/ros_action_server.vhd index 0145fbf..501eab7 100644 --- a/src/ros2/ros_action_server.vhd +++ b/src/ros2/ros_action_server.vhd @@ -795,7 +795,7 @@ begin case (goal_state_latch) is when GoalStatus_package.STATUS_EXECUTING => -- Synthesis Guard - if (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and is_terminal(goal_state_in)) then + if (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and is_terminal(goal_state_latch)) then cnt_next <= cnt + 1; else cnt_next <= cnt + 3; -- UPDATE Goal State @@ -808,14 +808,14 @@ begin case (goal_state_latch) is when GoalStatus_package.STATUS_SUCCEEDED => -- Synthesis Guard - if (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and is_terminal(goal_state_in)) then + if (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and is_terminal(goal_state_latch)) then cnt_next <= cnt + 1; else cnt_next <= cnt + 3; -- UPDATE Goal State end if; when GoalStatus_package.STATUS_ABORTED => -- Synthesis Guard - if (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and is_terminal(goal_state_in)) then + if (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and is_terminal(goal_state_latch)) then cnt_next <= cnt + 1; else cnt_next <= cnt + 3; -- UPDATE Goal State @@ -828,21 +828,21 @@ begin case (goal_state_latch) is when GoalStatus_package.STATUS_SUCCEEDED => -- Synthesis Guard - if (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and is_terminal(goal_state_in)) then + if (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and is_terminal(goal_state_latch)) then cnt_next <= cnt + 1; else cnt_next <= cnt + 3; -- UPDATE Goal State end if; when GoalStatus_package.STATUS_ABORTED => -- Synthesis Guard - if (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and is_terminal(goal_state_in)) then + if (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and is_terminal(goal_state_latch)) then cnt_next <= cnt + 1; else cnt_next <= cnt + 3; -- UPDATE Goal State end if; when GoalStatus_package.STATUS_CANCELED => -- Synthesis Guard - if (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and is_terminal(goal_state_in)) then + if (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and is_terminal(goal_state_latch)) then cnt_next <= cnt + 1; else cnt_next <= cnt + 3; -- UPDATE Goal State @@ -889,7 +889,7 @@ begin mem_r.state <= goal_state_latch; -- Synthesis Guard - if (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and is_terminal(goal_state_in)) then + if (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and is_terminal(goal_state_latch)) then mem_r.field_flags <= GMF_STATE_FLAG or GMF_DEADLINE_FLAG; mem_r.deadline <= time_latch; @@ -899,7 +899,7 @@ begin end if; end if; - if (is_terminal(goal_state_in)) then + if (is_terminal(goal_state_latch)) then trigger_result_next <= '1'; -- Update Terminal Goal Counter terminal_cnt_next <= terminal_cnt + 1;