From 1efeeef27945ff2d22bbff6d14ccd74d6832bf96 Mon Sep 17 00:00:00 2001 From: Greek64 Date: Sat, 16 Apr 2022 20:01:37 +0200 Subject: [PATCH] CYCLONE DDS COMPATIBILITY: Add start delay in ROS action Server Even with the previous commits, there still exist race conditions in which if the Cyclone DDS implementation receives the initial HEARTBEAT after the message has been sent, it is silently dropped (Volatile behaviour). And since the Cyclone DDS implementation is ignoring HEARTBEATs of yet unmatched endpoints (which is what happens to our init HEARTBEAT), the best way to counter this is to just wait after the reception of the first Cyclone DDS message (which signifies that it has matched all our endpoints) until the HEARTBEAT timeout has also sent the respective initial HEARTBEATs. --- src/ros2/Tests/Fibonacci.vhd | 44 ++++++++++++++++++++++++------------ 1 file changed, 30 insertions(+), 14 deletions(-) diff --git a/src/ros2/Tests/Fibonacci.vhd b/src/ros2/Tests/Fibonacci.vhd index be09ea4..cfca531 100644 --- a/src/ros2/Tests/Fibonacci.vhd +++ b/src/ros2/Tests/Fibonacci.vhd @@ -71,7 +71,7 @@ end entity; architecture arch of Fibonacci is --*****TYPE DECLARATION***** - type STAGE_TYPE is (IDLE,START_EXECUTION,INITIALIZE_EXECUTION,EXECUTE,FEEDBACK,TIMER,FINALIZE,CANCEL); + type STAGE_TYPE is (IDLE,DELAY_START,START_EXECUTION,INITIALIZE_EXECUTION,EXECUTE,FEEDBACK,TIMER,FINALIZE,CANCEL); --*****SIGNAL DECLARATION***** signal stage, stage_next : STAGE_TYPE; @@ -123,19 +123,35 @@ begin case (stage) is when IDLE => - if (new_goal_request = '1') then - new_goal_accepted <= '1'; - new_goal_response <= '1'; - - -- Latch DATA - order_next <= unsigned(new_goal_order); - handle_next <= new_goal_handle; - ind_next <= new_goal_result_index; - - stage_next <= START_EXECUTION; - cnt_next <= 0; - elsif (cancel_request = '1') then - cancel_response <= '1'; + -- NOTE: Because of race conditions that can occur with the Cyclone DDS implementation, we wait + -- 2 seconds to make sure that all remote readers have received an initial HEARTBEAT. + if (new_goal_request = '1' or cancel_request = '1') then + -- NOTE: During simulation we essentially skip the timer + if (ros_config.SIMULATION_TIMING) then + deadline_next <= time; + else + deadline_next.sec <= std_logic_vector(unsigned(time.sec) + 2); + deadline_next.nanosec <= time.nanosec; + end if; + stage_next <= DELAY_START; + end if; + when DELAY_START => + if (deadline <= time) then + if (new_goal_request = '1') then + new_goal_accepted <= '1'; + new_goal_response <= '1'; + + -- Latch DATA + order_next <= unsigned(new_goal_order); + handle_next <= new_goal_handle; + ind_next <= new_goal_result_index; + + stage_next <= START_EXECUTION; + cnt_next <= 0; + elsif (cancel_request = '1') then + cancel_response <= '1'; + stage_next <= IDLE; + end if; end if; when START_EXECUTION => case (cnt) is