diff --git a/test/jani_generator/_test_data/grid_robot_blackboard/bt_update_goal_and_current_position.scxml b/test/jani_generator/_test_data/grid_robot_blackboard/bt_update_goal_and_current_position.scxml
index 0f797429..474fe424 100644
--- a/test/jani_generator/_test_data/grid_robot_blackboard/bt_update_goal_and_current_position.scxml
+++ b/test/jani_generator/_test_data/grid_robot_blackboard/bt_update_goal_and_current_position.scxml
@@ -14,7 +14,8 @@
     <bt_declare_port_in key="curr_topic" type="string" />
 
     <datamodel>
-        <data id="is_init" expr="false" type="bool" />
+        <data id="is_goal_init" expr="false" type="bool" />
+        <data id="is_pose_init" expr="false" type="bool" />
         <data id="goal_x" expr="0" type="int32" />
         <data id="goal_y" expr="0" type="int32" />
         <data id="curr_x" expr="0" type="int32" />
@@ -38,15 +39,17 @@
         <ros_topic_callback name="goal_sub" target="running">
             <assign location="goal_x" expr="_msg.x" />
             <assign location="goal_y" expr="_msg.y" />
+            <assign location="is_goal_init" expr="true" />
         </ros_topic_callback>
         <!-- New robot pose -->
         <ros_topic_callback name="curr_sub" target="running">
             <assign location="curr_x" expr="_msg.x" />
             <assign location="curr_y" expr="_msg.y" />
+            <assign location="is_pose_init" expr="true" />
         </ros_topic_callback>
         <!-- Leaf node tick -->
         <bt_tick target="running">
-            <if cond="is_init">
+            <if cond="is_goal_init &amp;&amp; is_pose_init">
                 <bt_set_output key="goal_x" expr="goal_x" />
                 <bt_set_output key="goal_y" expr="goal_y" />
                 <bt_set_output key="curr_x" expr="curr_x" />
diff --git a/test/jani_generator/_test_data/grid_robot_blackboard/world.scxml b/test/jani_generator/_test_data/grid_robot_blackboard/world.scxml
index 30b457a5..21bd48cc 100644
--- a/test/jani_generator/_test_data/grid_robot_blackboard/world.scxml
+++ b/test/jani_generator/_test_data/grid_robot_blackboard/world.scxml
@@ -9,8 +9,8 @@
         <data id="n" type="int32" expr="10" />  <!-- grid size -->
         <data id="pose_x" type="int32" expr="0" /> <!-- TODO: Math.floor(Math.random() * n) -->
         <data id="pose_y" type="int32" expr="0" /> <!-- TODO: Math.floor(Math.random() * n) -->
-        <data id="goal_x" type="int32" expr="4" />
-        <data id="goal_y" type="int32" expr="9" />
+        <data id="goal_x" type="int32" expr="0" />
+        <data id="goal_y" type="int32" expr="0" />
     </datamodel>
 
     <ros_topic_subscriber topic="move" type="std_msgs/Int16" />
@@ -25,17 +25,21 @@
 
     <state id="init">
         <onentry>
-            <!-- if at goal -->
-            <if cond="pose_x == goal_x &amp;&amp; pose_y == goal_y">
-                <assign location="goal_x" expr="goal_x + 1" /> <!-- TODO: Math.floor(Math.random() * n) -->
-                <assign location="goal_y" expr="goal_y - 1" /> <!-- TODO: Math.floor(Math.random() * n) -->
-                <ros_topic_publish name="goal">
-                    <field name="x" expr="goal_x" />
-                    <field name="y" expr="goal_y" />
-                </ros_topic_publish>
-            </if>
+            <assign location="goal_x" expr="4" /> <!-- TODO: Math.floor(Math.random() * n) -->
+            <assign location="goal_y" expr="9" /> <!-- TODO: Math.floor(Math.random() * n) -->
+            <ros_topic_publish name="goal">
+                <field name="x" expr="goal_x" />
+                <field name="y" expr="goal_y" />
+            </ros_topic_publish>
+            <ros_topic_publish name="pose">
+                <field name="x" expr="pose_x" />
+                <field name="y" expr="pose_y" />
+            </ros_topic_publish>
         </onentry>
+        <transition target="running" />
+    </state>
 
+    <state id="running">
         <ros_topic_callback name="move" target="init">
             <if cond="_msg.data == 0 &amp;&amp; pose_x &lt; n - 1">
                 <!-- right -->
@@ -55,6 +59,5 @@
                 <field name="y" expr="pose_y" />
             </ros_topic_publish>
         </ros_topic_callback>
-
     </state>
 </scxml>