Added target state selector logic, added battery logic

state/assets/battery.png.import

@@ -0,0 +1,34 @@
+[remap]
+
+importer="texture"
+type="CompressedTexture2D"
+uid="uid://buoiey8bh6i0e"
+path="res://.godot/imported/battery.png-0291d92f3cb8a965d4b9647e0eab9582.ctex"
+metadata={
+"vram_texture": false
+}
+
+[deps]
+
+source_file="res://assets/battery.png"
+dest_files=["res://.godot/imported/battery.png-0291d92f3cb8a965d4b9647e0eab9582.ctex"]
+
+[params]
+
+compress/mode=0
+compress/high_quality=false
+compress/lossy_quality=0.7
+compress/hdr_compression=1
+compress/normal_map=0
+compress/channel_pack=0
+mipmaps/generate=false
+mipmaps/limit=-1
+roughness/mode=0
+roughness/src_normal=""
+process/fix_alpha_border=true
+process/premult_alpha=false
+process/normal_map_invert_y=false
+process/hdr_as_srgb=false
+process/hdr_clamp_exposure=false
+process/size_limit=0
+detect_3d/compress_to=1

state/assets/character_state_machine.json

@@ -2,9 +2,31 @@
   "start": {
     "state": "Idle"
   },
+  "template_transitions": {
+    "battery_low": {
+      "target": "GoToBattery",
+      "conditions": [
+        {
+          "type": ">=",
+          "left": {
+            "value": 20
+          },
+          "right": {
+            "accessor": [
+              "character",
+              "battery_charge"
+            ]
+          }
+        }
+      ]
+    }
+  },
   "states": {
     "Idle": {
       "transitions": [
+        {
+          "template": "battery_low"
+        },
         {
           "target": "PickupTrash",
           "signal": "waste_detected",
@@ -51,6 +73,9 @@
     },
     "PickupTrash": {
       "transitions": [
+        {
+          "template": "battery_low"
+        },
         {
           "target": "ThrowTrashAway",
           "signal": "waste_detected",
@@ -97,6 +122,9 @@
     },
     "ThrowTrashAway": {
       "transitions": [
+        {
+          "template": "battery_low"
+        },
         {
           "target": "Idle",
           "conditions": [
@@ -129,6 +157,68 @@
           ]
         }
       ]
+    },
+    "GoToBattery": {
+      "transitions": [
+        {
+          "target": "RechargeBattery",
+          "conditions": [
+            {
+              "type": ">=",
+              "left": {
+                "value": 90
+              },
+              "right": {
+                "function": "distance",
+                "args": [
+                  {
+                    "accessor": [
+                      "character",
+                      "position"
+                    ]
+                  },
+                  {
+                    "accessor": [
+                      "root_nodes",
+                      "StateMachineWorld",
+                      "child_nodes",
+                      "Battery",
+                      "position"
+                    ]
+                  }
+                ]
+              }
+            }
+          ]
+        }
+      ]
+    },
+    "RechargeBattery": {
+      "transitions": [
+        {
+          "target": {
+            "type": "history_first",
+            "ignore": [
+              "RechargeBattery",
+              "GoToBattery"
+            ]
+          },
+          "conditions": [
+            {
+              "type": "<=",
+              "left": {
+                "value": 100
+              },
+              "right": {
+                "accessor": [
+                  "character",
+                  "battery_charge"
+                ]
+              }
+            }
+          ]
+        }
+      ]
     }
   }
 }

state/project.godot

@@ -26,3 +26,13 @@ spawn_trash={
 "events": [Object(InputEventMouseButton,"resource_local_to_scene":false,"resource_name":"","device":-1,"window_id":0,"alt_pressed":false,"shift_pressed":false,"ctrl_pressed":false,"meta_pressed":false,"button_mask":0,"position":Vector2(0, 0),"global_position":Vector2(0, 0),"factor":1.0,"button_index":2,"canceled":false,"pressed":false,"double_click":false,"script":null)
 ]
 }
+debug_vis_1={
+"deadzone": 0.5,
+"events": [Object(InputEventKey,"resource_local_to_scene":false,"resource_name":"","device":-1,"window_id":0,"alt_pressed":false,"shift_pressed":false,"ctrl_pressed":false,"meta_pressed":false,"pressed":false,"keycode":0,"physical_keycode":65,"key_label":0,"unicode":97,"location":0,"echo":false,"script":null)
+]
+}
+debug_vis_2={
+"deadzone": 0.5,
+"events": [Object(InputEventKey,"resource_local_to_scene":false,"resource_name":"","device":-1,"window_id":0,"alt_pressed":false,"shift_pressed":false,"ctrl_pressed":false,"meta_pressed":false,"pressed":false,"keycode":0,"physical_keycode":66,"key_label":0,"unicode":98,"location":0,"echo":false,"script":null)
+]
+}

state/scenes/state/Battery.tscn

@@ -0,0 +1,16 @@
+[gd_scene load_steps=3 format=3 uid="uid://blp3dyd38b8i2"]
+
+[ext_resource type="Texture2D" uid="uid://buoiey8bh6i0e" path="res://assets/battery.png" id="1_jitck"]
+
+[sub_resource type="CapsuleShape2D" id="CapsuleShape2D_bhmyx"]
+radius = 0.0
+height = 0.0
+
+[node name="TrashBin" type="StaticBody2D"]
+
+[node name="Sprite2D" type="Sprite2D" parent="."]
+scale = Vector2(0.0900715, 0.0900715)
+texture = ExtResource("1_jitck")
+
+[node name="CollisionShape2D" type="CollisionShape2D" parent="."]
+shape = SubResource("CapsuleShape2D_bhmyx")

state/scenes/state/SMCharacter.gd

@@ -12,6 +12,10 @@ const ROTATION_SPEED: float   = 5.0  # adians per second
 #
 @onready var detection_area: Area2D = $VisionCone
 signal waste_detected(waste)
+#
+var battery_charge: float                       = 100.0
+const battery_charge_drain_per_second: float    = 4.0
+const battery_charge_recharge_per_second: float = 27.0
 
 
 func _ready() -> void:
@@ -38,11 +42,17 @@ func _draw() -> void:
         draw_line(Vector2.ZERO, local_velocity, Color(1, 0, 0), 2)
 
 
+func _process(delta: float) -> void:
+    %StateMachineInfoPanel.values["battery_charge"] = battery_charge
+
+
 func _physics_process(delta: float) -> void:
     rotate_towards_velocity(delta)
     detect_waste()
     queue_redraw()
 
+    battery_charge -= battery_charge_drain_per_second * delta
+
 
 func detect_waste() -> void:
     var overlapping_bodies: Array[Node2D] = detection_area.get_overlapping_bodies()

state/scenes/state/StateMachine.gd

@@ -11,10 +11,13 @@ var received_signals: Array[Dictionary] = []
 # state change parameters ("transfer")
 var state_transfer_variables: Dictionary = {}
 
+var state_history: Array = []
+
 var current_state: State:
     set(value):
         if current_state:
             current_state.state_exit()
+        state_history.append(value)
         current_state = value
         current_state.state_enter()
 
@@ -39,7 +42,7 @@ func _ready() -> void:
     else:
         push_error("Failed to load JSON character_state_machine.json")
 
-    current_state = find_state_by_name(state_machine_data["start"]["state"])
+    current_state = find_state(state_machine_data["start"]["state"])
 
     call_deferred("defer_ready")
 
@@ -63,6 +66,7 @@ func _process(delta: float) -> void:
         %StateMachineInfoPanel.values["signal_" + sgl["signal"]] = sgl["args"]
     var transition_names: Array = []
     for transition in state_machine_data["states"][current_state.get_name()]["transitions"]:
+        transition = resolve_transition_template(transition)
         transition_names.append(transition["target"])
     %StateMachineInfoPanel.values["Transitions"] = transition_names
 
@@ -72,6 +76,7 @@ func _process(delta: float) -> void:
 func check_transitions() -> void:
     if current_state:
         for transition in state_machine_data["states"][current_state.get_name()]["transitions"]:
+            transition = resolve_transition_template(transition)
 
             if "signal" in transition and transition["signal"] and not was_signal_received(transition["signal"]):
                 continue
@@ -82,7 +87,7 @@ func check_transitions() -> void:
                     var condition_met: bool = transition_check_condition(condition)
                     if not condition_met:
                         all_conditions_met = false
-                        %StateMachineInfoPanel.values["condition failed for " + transition["target"]] = human_readable_condition(condition)
+                        %StateMachineInfoPanel.values["condition failed for " + str(transition["target"])] = human_readable_condition(condition)
                         break
                 if not all_conditions_met:
                     continue
@@ -100,7 +105,7 @@ func check_transitions() -> void:
             current_state.contribute_transfer_variables(state_transfer_variables)
 
             print("All criteria were met for switching to state ", transition["target"], " with transfer variables ", state_transfer_variables)
-            current_state = find_state_by_name(transition["target"])
+            current_state = find_state(transition["target"])
 
 
 func transition_check_condition(condition: Dictionary) -> bool:
@@ -226,6 +231,12 @@ func find_detected_signal(signal_name: String) -> Dictionary:
 
 
 # UTILITY FUNCTIONS
+func find_state(name) -> State:
+    if name is Dictionary:
+        return find_state_by_selector(name)
+    return find_state_by_name(name)
+
+
 func find_state_by_name(name: String) -> State:
     for child in self.get_children():
         if child is State:
@@ -234,6 +245,23 @@ func find_state_by_name(name: String) -> State:
     return null
 
 
+func find_state_by_selector(name: Dictionary) -> State:
+    # {
+    #   "type": "history_first",
+    #   "ignore": [
+    #     "RechargeBattery",
+    #     "GoToBattery"
+    #   ]
+    # }
+    if name["type"] == "history_first":
+        var index: int = state_history.size() - 1
+        while index >= 0:
+            if not state_history[index].get_name() in name["ignore"]:
+                return state_history[index]
+            index -= 1
+    return null
+
+
 func was_signal_received(signal_name: String) -> bool:
     for received_signal in received_signals:
         if received_signal["signal"] == signal_name:
@@ -259,20 +287,27 @@ func load_json(path: String) -> JSON:
     return json
 
 
+func resolve_transition_template(data: Dictionary) -> Dictionary:
+    if data.has("template"):
+        return state_machine_data["template_transitions"][data["template"]]
+    return data
+
+
 func objects_equal(a: Variant, b: Variant) -> bool:
     if typeof(a) != typeof(b):
         return false
     return a == b
 
 
-func human_readable_transition(condition: Dictionary) -> String:
+func human_readable_transition(transition: Dictionary) -> String:
+    transition = resolve_transition_template(transition)
     var parts: Array[Variant] = []
 
-    if condition.has("signal"):
+    if transition.has("signal"):
-        parts.append(condition["signal"])
+        parts.append(transition["signal"])
 
-    if condition.has("conditions"):
+    if transition.has("conditions"):
-        parts.append(human_readable_condition(condition))
+        parts.append(human_readable_condition(transition))
 
     return " & ".join(parts)
 

state/scenes/state/StateMachineWorld.tscn

@@ -1,4 +1,4 @@
-[gd_scene load_steps=14 format=3 uid="uid://cgdvptekjbpgq"]
+[gd_scene load_steps=17 format=3 uid="uid://cgdvptekjbpgq"]
 
 [ext_resource type="Script" path="res://scenes/state/SMCharacter.gd" id="1_84313"]
 [ext_resource type="Script" path="res://scenes/state/SceneManagement.gd" id="1_s1suw"]
@@ -6,11 +6,14 @@
 [ext_resource type="Texture2D" uid="uid://cx04xknqfdscp" path="res://assets/cleaning_robot.png" id="2_cwwab"]
 [ext_resource type="Script" path="res://scenes/state/StateMachine.gd" id="2_d1xqo"]
 [ext_resource type="Script" path="res://scenes/state/state_idle.gd" id="3_r1btx"]
+[ext_resource type="PackedScene" uid="uid://blp3dyd38b8i2" path="res://scenes/state/Battery.tscn" id="5_pml1y"]
 [ext_resource type="Texture2D" uid="uid://dkqw1wsjbvl0i" path="res://assets/floor.png" id="6_15pjb"]
 [ext_resource type="Script" path="res://scenes/state/state_PickupTrash.gd" id="7_1rfah"]
 [ext_resource type="Script" path="res://scenes/state/state_ThrowTrashAway.gd" id="8_677fi"]
 [ext_resource type="PackedScene" uid="uid://dng6a8i8kp4kw" path="res://scenes/state/TrashBin.tscn" id="9_sd6r3"]
 [ext_resource type="Script" path="res://scenes/state/visualization/state_machine_info_panel.gd" id="11_70vf4"]
+[ext_resource type="Script" path="res://scenes/state/state_GoToBattery.gd" id="12_e37qa"]
+[ext_resource type="Script" path="res://scenes/state/state_RechargeBattery.gd" id="13_qf7y1"]
 
 [sub_resource type="CapsuleShape2D" id="CapsuleShape2D_tr1gq"]
 radius = 60.0
@@ -35,6 +38,10 @@ texture = ExtResource("6_15pjb")
 position = Vector2(67, 78)
 scale = Vector2(4.42446, 4.42446)
 
+[node name="Battery" parent="." instance=ExtResource("5_pml1y")]
+position = Vector2(124, 578)
+scale = Vector2(6.12, 6.12)
+
 [node name="CleaningRobotCB2D" type="CharacterBody2D" parent="."]
 position = Vector2(546, 342)
 script = ExtResource("1_84313")
@@ -59,6 +66,12 @@ script = ExtResource("7_1rfah")
 [node name="ThrowTrashAway" type="Node" parent="CleaningRobotCB2D/StateMachine"]
 script = ExtResource("8_677fi")
 
+[node name="GoToBattery" type="Node" parent="CleaningRobotCB2D/StateMachine"]
+script = ExtResource("12_e37qa")
+
+[node name="RechargeBattery" type="Node" parent="CleaningRobotCB2D/StateMachine"]
+script = ExtResource("13_qf7y1")
+
 [node name="VisionCone" type="Area2D" parent="CleaningRobotCB2D"]
 
 [node name="CollisionShape2D" type="CollisionShape2D" parent="CleaningRobotCB2D/VisionCone"]

state/scenes/state/state_GoToBattery.gd

@@ -0,0 +1,13 @@
+extends State
+
+@onready var battery: Node2D = $"../../../Battery"
+
+
+func state_enter():
+    print("Go to ", battery.position, " to recharge battery")
+
+
+func state_process(delta: float) -> void:
+    var target_position: Vector2 = battery.position
+    character.move_towards(target_position, delta)
+    character.move_and_slide()

state/scenes/state/state_RechargeBattery.gd

@@ -0,0 +1,16 @@
+extends State
+
+@onready var battery: Node2D = $"../../../Battery"
+
+
+func state_enter():
+    print("Arrived at ", battery.position, " to recharge battery")
+
+
+func state_process(delta: float) -> void:
+    var target_position: Vector2 = battery.position
+    target_position += Vector2(rand_range(-10, 10), rand_range(-10, 10))
+    character.move_towards(target_position, delta)
+    character.move_and_slide()
+
+    character.battery_charge += character.battery_charge_recharge_per_second * delta

state/scenes/state/visualization/d_tree_node.gd

@@ -1,8 +1,8 @@
 class_name DTreeNode
 extends GraphNode
 
-var left_slots: Array[String]  = []
+var left_slots: Array[String]       = []
-var right_slots: Array[String] = []
+var right_slots: Array[String]      = []
 var color_highlighted: StyleBoxFlat = StyleBoxFlat.new()
 var color_normal: StyleBoxFlat      = StyleBoxFlat.new()
 
@@ -29,12 +29,11 @@ func add_label(label_text: String, left: bool, right: bool) -> Vector3i:
         self.set_slot_color_right(child_index, Color(0.9, 0.9, 0.9, 1))
         right_slots.append(label_text)
 
-    # the port index is counted separately from the left and right slots.
+    # the port index is counted separately from the left and right slots
     return Vector3i(child_index, left_slots.size() - 1, right_slots.size() - 1)
 
 
 func set_highlighted(highlight: bool) -> void:
-    # set the background color of the node
     if highlight:
         self.add_theme_stylebox_override("panel", color_highlighted)
     else:

state/scenes/state/visualization/state_machine_info_panel.gd

@@ -3,7 +3,11 @@ extends VBoxContainer
 
 var values: Dictionary      = {}
 var last_values: Dictionary = {}
-var value_order: Array      = [] # Keep track of the order of values
+var value_order: Array      = []
+
+
+func _ready() -> void:
+    hide()
 
 
 func to_str(value) -> String:
@@ -19,6 +23,12 @@ func to_str(value) -> String:
 
 
 func _process(delta: float) -> void:
+    if Input.is_action_just_pressed("debug_vis_2"):
+        if is_visible():
+            hide()
+        else:
+            show()
+
     for child in self.get_children():
         if child is Label:
             child.queue_free()
@@ -26,7 +36,8 @@ func _process(delta: float) -> void:
     # 1. Update and Track Order of Values
     for value in values.keys():
         if value in value_order:
-            value_order.erase(value)  # Move to the end (most recent)
+            # Move to the end (most recent)
+            value_order.erase(value)
         value_order.append(value)
 
     # 2. Display Current Values (Most Recent First)

state/scenes/state/visualization/tree_visualizer.gd

@@ -11,7 +11,7 @@ var all_node_names_to_nodes: Dictionary = {}
 
 
 func _process(delta: float) -> void:
-    if Input.is_action_just_pressed("ui_accept"):
+    if Input.is_action_just_pressed("debug_vis_1"):
         if is_visible():
             hide()
         else:
@@ -35,7 +35,6 @@ func build_tree():
     var node_positions: Array[DTreeNode] = []
     var y_spacing: int                   = 300
     var x_spacing: int                   = 600
-    var depths: Dictionary               = calculate_node_depths()
 
     var states = state_machine.state_machine_data["states"]
 
@@ -60,12 +59,9 @@ func build_tree():
         # Set up labels (input and output)
         populate_node_labels(graph_node, state_name, state_data, all_node_names_to_nodes)
 
-        # Calculate position based on tree depth
-        var depth: int      = depths[i]
-        var siblings: Array = get_nodes_at_depth(depth, depths)
         @warning_ignore("integer_division")
-        var y_pos: int = y_spacing * (siblings.find(i) - (siblings.size() - 1) / 2)
+        var y_pos: int = randi() % (y_spacing * 2) - y_spacing
-        var x_pos: int = x_spacing * depth
+        var x_pos: int = x_spacing * i
         y_pos += randi() % 200 - 100
         graph_node.position_offset = Vector2(x_pos, y_pos)
         node_positions.append(graph_node)
@@ -80,8 +76,18 @@ func populate_node_labels(node: DTreeNode, state_name: String, state_data: Dicti
     # start from the node passed in and only use the output connections to determine the input connections on the other nodes
     var output_transitions: Array = state_data.get("transitions", [])
     for transition in output_transitions:
+        transition = state_machine.resolve_transition_template(transition)
+
+        var port_out: Vector3i = node.add_label(state_machine.human_readable_transition(transition), false, true)
+
+        if not transition.has("target"):
+            continue
+        if not transition['target'] is String:
+            continue
+
         var target_state_name: String = transition['target']
-        var port_out: Vector3i        = node.add_label(state_machine.human_readable_transition(transition), false, true)
+        if not node_names_to_nodes.has(target_state_name):
+            continue
 
         var target_node: DTreeNode = node_names_to_nodes[target_state_name]
         if target_node:
@@ -94,28 +100,3 @@ func populate_node_labels(node: DTreeNode, state_name: String, state_data: Dicti
             )
             print("Connecting %s [%d] to %s [%d]" % [node.get_name(), port_out, target_node.get_name(), port_in])
 
-
-func calculate_node_depths() -> Dictionary:
-    var depths: Dictionary = {}
-    depths[0] = 0  # Root is at depth 0
-    var state_names        = state_machine.state_machine_data["states"].keys()
-    for i in range(state_names.size()):
-        var state_name = state_names[i]
-        var state_data = state_machine.state_machine_data["states"][state_name]
-        if state_data.has("transitions"):
-            for transition in state_data["transitions"]:
-                if transition.has("target"):
-                    var target_state_name  = transition["target"]
-                    var target_state_index = state_names.find(target_state_name)
-                    if target_state_index != -1:
-                        depths[target_state_index] = depths[i] + 1
-    return depths
-
-
-func get_nodes_at_depth(depth: int, depths: Dictionary) -> Array:
-    var nodes_at_depth: Array[Variant] = []
-    for i in depths.keys():
-        if depths[i] == depth:
-            nodes_at_depth.append(i)
-    return nodes_at_depth
-