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

BUG FIX: ros_action_server was not using latched values

Browse Source
This commit is contained in:
John Ring 2022-04-17 13:49:15 +02:00
parent 1e8b5464b8
commit a92bd9ece1
3 changed files with 18 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
src/TODO.txt
View File

@ -476,6 +476,15 @@ DESIGN DECISIONS
addition of a new instance. That means that all Instances are checked at the smae time, non depending addition of a new instance. That means that all Instances are checked at the smae time, non depending
on when they were added. 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 BRAINSTORMING
------------- -------------

2
src/ros2/Tests/Level_2/L2_Testbench_ROS_Lib4.vhd
View File

@ -700,7 +700,7 @@ begin
Fibonacci_ros_action_server_inst : entity work.Fibonacci_ros_action_server(arch) Fibonacci_ros_action_server_inst : entity work.Fibonacci_ros_action_server(arch)
generic map ( generic map (
TIMEOUT_DURATION => ROS_DURATION_INFINITE, TIMEOUT_DURATION => ROS_DURATION_INFINITE,
MAX_GOALS => 1, MAX_GOALS => 2,
MAX_RESULT_REQUESTS => 1, MAX_RESULT_REQUESTS => 1,
ENABLE_FEEDBACK => '1' ENABLE_FEEDBACK => '1'
) )

16
src/ros2/ros_action_server.vhd
View File

@ -795,7 +795,7 @@ begin
case (goal_state_latch) is case (goal_state_latch) is
when GoalStatus_package.STATUS_EXECUTING => when GoalStatus_package.STATUS_EXECUTING =>
-- Synthesis Guard -- 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; cnt_next <= cnt + 1;
else else
cnt_next <= cnt + 3; -- UPDATE Goal State cnt_next <= cnt + 3; -- UPDATE Goal State
@ -808,14 +808,14 @@ begin
case (goal_state_latch) is case (goal_state_latch) is
when GoalStatus_package.STATUS_SUCCEEDED => when GoalStatus_package.STATUS_SUCCEEDED =>
-- Synthesis Guard -- 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; cnt_next <= cnt + 1;
else else
cnt_next <= cnt + 3; -- UPDATE Goal State cnt_next <= cnt + 3; -- UPDATE Goal State
end if; end if;
when GoalStatus_package.STATUS_ABORTED => when GoalStatus_package.STATUS_ABORTED =>
-- Synthesis Guard -- 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; cnt_next <= cnt + 1;
else else
cnt_next <= cnt + 3; -- UPDATE Goal State cnt_next <= cnt + 3; -- UPDATE Goal State
@ -828,21 +828,21 @@ begin
case (goal_state_latch) is case (goal_state_latch) is
when GoalStatus_package.STATUS_SUCCEEDED => when GoalStatus_package.STATUS_SUCCEEDED =>
-- Synthesis Guard -- 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; cnt_next <= cnt + 1;
else else
cnt_next <= cnt + 3; -- UPDATE Goal State cnt_next <= cnt + 3; -- UPDATE Goal State
end if; end if;
when GoalStatus_package.STATUS_ABORTED => when GoalStatus_package.STATUS_ABORTED =>
-- Synthesis Guard -- 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; cnt_next <= cnt + 1;
else else
cnt_next <= cnt + 3; -- UPDATE Goal State cnt_next <= cnt + 3; -- UPDATE Goal State
end if; end if;
when GoalStatus_package.STATUS_CANCELED => when GoalStatus_package.STATUS_CANCELED =>
-- Synthesis Guard -- 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; cnt_next <= cnt + 1;
else else
cnt_next <= cnt + 3; -- UPDATE Goal State cnt_next <= cnt + 3; -- UPDATE Goal State
@ -889,7 +889,7 @@ begin
mem_r.state <= goal_state_latch; mem_r.state <= goal_state_latch;
-- Synthesis Guard -- 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.field_flags <= GMF_STATE_FLAG or GMF_DEADLINE_FLAG;
mem_r.deadline <= time_latch; mem_r.deadline <= time_latch;
@ -899,7 +899,7 @@ begin
end if; end if;
end if; end if;
if (is_terminal(goal_state_in)) then if (is_terminal(goal_state_latch)) then
trigger_result_next <= '1'; trigger_result_next <= '1';
-- Update Terminal Goal Counter -- Update Terminal Goal Counter
terminal_cnt_next <= terminal_cnt + 1; terminal_cnt_next <= terminal_cnt + 1;