diff --git a/src/ros2/example_interfaces/Fibonacci_ros_action_server.vhd b/src/ros2/example_interfaces/Fibonacci_ros_action_server.vhd
index 4287f43..98b313c 100644
--- a/src/ros2/example_interfaces/Fibonacci_ros_action_server.vhd
+++ b/src/ros2/example_interfaces/Fibonacci_ros_action_server.vhd
@@ -804,7 +804,11 @@ begin
         case (stage) is
             when IDLE =>
                 if (r_sel = '1') then
-                    stage_next  <= GET_RESULT;
+                    if (unsigned(r_index) = MAX_GOALS) then
+                        stage_next  <= PASSTHROUGH;
+                    else
+                        stage_next  <= GET_RESULT;
+                    end if;
                     -- Reset
                     abort_mem   <= '1';
                 else