6: Add simple states for finding people

src/lhw_intelligence/lhw_intelligence/states/emotional_pepper.py

 from .state import State
 from rclpy.node import Node
+from sensor_msgs.msg import Image
+from std_msgs.msg import String
+from lhw_interfaces.msg import Result, Entity, Entities
 import transitions as sm
 import rclpy
 
@@ -7,28 +10,87 @@ class EmotionalPepper(State):
     states = ["look_for_persons", "greet_person", "recognize_person",
         "remember_person", "free_talk", "analyse_emotion", "emotion_verdict"]
 
-    # self.machine.add_transition(trigger='wake_up', source='asleep', dest='hanging out')
-    transitions = [
-        {"trigger": 0, "source": 0, "dest": 0}
-    ]
-
     def __init__(self, node, args=[]):
         super().__init__(node, args)
 
         self.name = node.get_name()
         self.node = node
-        self.machine = sm.Machine(model=self, states = EmotionalPepper.states, initial="look_for_persons")
-
+        self.log = self.node.get_logger()
+        self.machine = sm.Machine(model=self, states = EmotionalPepper.states, initial="dummy")
+        self.to_look_for_persons()
+        self.person_in_frame = False
+        self.machine.add_transition("trigger_found_person", "look_for_persons", "greet_person")
+        self.machine.add_transition("trigger_recognize_person", "greet_person", "recognize_person")
+    
+        self.entities_sub = self.node.create_subscription(Entities, "yolov5_entities", self.__entities_cb, 1)
+        
         self.run_timer = self.node.create_timer(2, self.__run) 
 
+    """
+    enter/exit callbacks
+    """
+    def on_enter_greet_person(self):
+        self.node.get_logger().info("Entering greet person")
+        self.say("Hello Person")
+
+    def on_exit_greet_person(self):
+        self.node.get_logger().info("Exiting greet person")
+
+    def on_enter_look_for_persons(self):
+        self.log.info("Entering look_for_persons")
+        self.say("Looking for persons")
+
+    def on_exit_look_for_persons(self):
+        self.log.info("Exiting look_for_persons")
+
+    def on_enter_recognize_person(self):
+        self.log.info("Entering recognize_person")
+        self.say("Trying to recognize you...")
+
+    """
+    end callbacks
+    """
+
     def __run(self):
-        self.node.get_logger().info("Running State Machine Loop :)")
+        #self.node.get_logger().info("Running State Machine Loop :)")
+        self.node.get_logger().info(f"In state: {self.state}")
+
+        if self.state == "greet_person":
+            self.trigger_recognize_person()
+        elif self.state == "look_for_persons":
+            pass
+        elif self.state == "recognize_person":
+            pass
+        else:
+            self.say("Unknown state")
+
+    def __entities_cb(self, entities):
+        #self.node.get_logger().info("Recevied entities! :D")
+        ents = entities.entities
+        found_person = False
+        for e in ents:
+            # lhw_interfaces.msg.Entity(id=0, sources_types=lhw_interfaces.msg.VisualSources(),
+            # sources=[], sources_ids=[], bbox=array([305, 331, 535, 479], dtype=int32), xyz=array([0., 0., 0.], dtype=float32), type='tv', confidence=0.67578125)
+            if (e.type == "person"):
+                if not self.person_in_frame:
+                    self.node.get_logger().info("Found person!")
+                    self.person_in_frame = True
+                    self.trigger_found_person()
+                found_person = True
+                #self.node.get_logger().info(str(e))
+        if not found_person:
+            self.person_in_frame = False
+            self.to_look_for_persons()
+    
+
+
 
 
 class StateMachineNode(Node):
     def __init__(self):
         super().__init__("transitions_ros") # Node name
-
+        self.log = self.get_logger()
+        self.say_publisher = self.create_publisher(String, 'say', 100)
         self.machine = EmotionalPepper(self)