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