rosbags/microphone_buffer/rosbag2_microphone_talking/metadata.yaml

+rosbag2_bagfile_information:
+  version: 4
+  storage_identifier: sqlite3
+  relative_file_paths:
+    - rosbag2_2022_11_27-20_37_50_0.db3
+  duration:
+    nanoseconds: 20026637917
+  starting_time:
+    nanoseconds_since_epoch: 1669581472048293955
+  message_count: 5
+  topics_with_message_count:
+    - topic_metadata:
+        name: /microphone_buffer
+        type: lhw_interfaces/msg/Audio
+        serialization_format: cdr
+        offered_qos_profiles: "- history: 3\n  depth: 0\n  reliability: 1\n  durability: 2\n  deadline:\n    sec: 2147483647\n    nsec: 4294967295\n  lifespan:\n    sec: 2147483647\n    nsec: 4294967295\n  liveliness: 1\n  liveliness_lease_duration:\n    sec: 2147483647\n    nsec: 4294967295\n  avoid_ros_namespace_conventions: false"
+      message_count: 5
+  compression_format: ""
+  compression_mode: ""
\ No newline at end of file

src/lhw_intelligence/lhw_intelligence/behaviours/actions.py

+from typing import Any, Iterable, Union
+from std_msgs.msg import Bool, String, Empty, Int16
+from lhw_interfaces.msg import Entities, Response, Entity, Result
+from geometry_msgs.msg import Twist, Pose, PoseStamped
+import numpy as np
+
+class Actions:
+    """
+    Abstract class, used for all behaviours (in the behaviours folder)
+    The node is passed to the behaviour, so create new subscribers and
+    publishers is possible. This should only be done if the callbacks or
+    publish function isn't already defined in this class.
+    """
+    def __init__(self, node):
+        self.node = node
+        self.start_time = self.node.get_clock().now()
+
+    def __del__(self):
+        pass
+
+    # ------- Common methods -------
+
+    def behaviour_timeout(self) -> bool:
+        """ Checks if the behaviour has gone on for too long
+        Returns:
+            True if too long, False otherwise.
+        """
+        if self.behaviour_timeout_in_sec:
+            current_time = self.node.get_clock().now()
+            current_behaviour_time = current_time - self.start_time
+            current_behaviour_time_in_sec = round(current_behaviour_time.nanoseconds * 10**(-9))
+            return current_behaviour_time_in_sec >= self.behaviour_timeout_in_sec
+        else:
+            return False
+        
+    def add_to_db(self, collection: str, element: Union[Entity,Any]):
+        """ Adds an arbitrary element to the database TODO: Not sure of the format of the nlp_parameters
+        """
+        self.node.db.add_to_db(collection, element)
+
+    def get_categories_of_type(self, type):
+        return self.node.db.get_categories_of_type(type)
+
+    def get_entity(self, id: int) -> Entity:
+        """ Returns the entitity from the database corresponding to the supplied id
+        Args:
+            id: The identity of the object
+        Returns:
+            The entity
+        """
+        return self.node.db.get_entity(id)
+
+    def relative_xyz_to_relative_pose(self, xyz: Iterable) -> Pose:
+        """ Converts relative coordinates from the vision stystem to a Pose message
+        Note that this function assumes that the coordinates from vision are not from a rotated head for example. 
+        Args:
+            xyz: x - right , y - down, z - depth
+        Returns:
+            Pose where x - right, y - forward 
+        """
+        vector_in_2d = np.array((float(xyz[0]), float(xyz[2])))
+        direction_vector_in_2d = vector_in_2d / np.linalg.norm(vector_in_2d)
+        
+        pose = Pose()
+        pose.position.x = direction_vector_in_2d[0]
+        pose.position.y = direction_vector_in_2d[1]
+        pose.position.z = 0.0
+        return pose
+
+    def relative_xyz_to_global_position(self, xyz):
+        #DO SOME MAGIC HERE!
+        position = {}
+        position["x"] = float(xyz[0])
+        position["y"] = float(xyz[1])
+        position["z"] = float(xyz[2])
+        return position
+
+    def position_to_pose(self, position: Iterable) -> Pose:
+        """ Converts a position to a Pose message
+        Args:
+            position: The position
+        Returns:
+            The corresponding pose
+        """
+        pose = Pose()
+        pose.position.x = position["x"]
+        pose.position.y = position["y"]
+        pose.position.z = position["z"]
+        return pose
+        
+    
+    # ------- Publishers -----------
+
+    def rotate_robot(self):
+        """ Rotates the robot counter (?) clockwise.
+        """
+        self.node.log.info("Rotating robot")
+        msg = Twist()
+        msg.angular.z = -1.0
+        self.node.cmd_vel_publisher.publish(msg)
+
+    def stop_robot(self):
+        """ Stops the robot
+        """
+        self.node.log.info("Stop robot")
+        msg = Twist()
+        self.node.cmd_vel_publisher.publish(msg)
+        
+
+    def move_robot_to_relative_pose(self, pose: Pose):
+        """ Takes a pose and 
+        """
+        goal = PoseStamped()
+        goal.pose = pose
+        self.node.log.info("Navigate to relative pose " + str(goal))
+        self.node.relative_goal_pose_publisher.publish(goal)
+
+    def move_robot_to_pose(self, pose):
+        goal = PoseStamped()
+        goal.pose = pose
+        self.node.log.info("Navigate to pose " + str(goal))
+        self.node.goal_pose_publisher.publish(goal)
+        
+
+    def send_result(self, success=True, id=-1):
+        result = Result()
+        result.success = success
+        result.id = id
+        self.node.log.info("Result: " + str(result))
+        self.node.result_publisher.publish(result)
+
+    def say(self, line):
+        msg = String()
+        msg.data = line
+        self.node.log.info("Saying " + line)
+        self.node.say_publisher.publish(msg)
+
+    def send_event(self, event_name, parameters = {}):
+        msg = Event()
+        msg.event_name = event_name
+        msg_parameters = []
+        for key, value in parameters.items():
+            parameter = Parameter()
+            parameter.key = key
+            parameter.value = value
+            msg_parameters.append(parameter)
+            
+        msg.parameters = msg_parameters
+        self.node.log.info("Sending event " + event_name + " with parameters " + str(parameters) + "to Dialogflow")
+        self.node.event_publisher.publish(msg)
+
+    # ------- Subscribers ---------
+
+    def entities_callback(self, data):
+        return
+
+    def dialogflow_response_callback(self, data):
+        print("received data from dialogflow")
+        return
+
+    def gpt_response_callback(self, data):
+        print("received data from gpt")
+        return
+
+
+
+    
+
+
+    '''
+    def tracked_callback(self, msg):
+        """ Contains visually tracked objects with 3D positions inserted """
+        return
+
+    def publish_say(self, msg):
+        """ For using pepper text to speech """
+        self.node.say_publisher.publish(msg)
+    '''
\ No newline at end of file

src/lhw_intelligence/lhw_intelligence/behaviours/find_face.py

+##############################################################################
+# Documentation
+##############################################################################
+
+"""
+Template behaviour that is used for implementation of new behaviours.
+"""
+
+##############################################################################
+# Imports
+##############################################################################
+
+import py_trees
+import lhw_interfaces.msg as lhw_msgs
+import lhw_interfaces.srv as lhw_srvs
+import lhw_interfaces.actions as lhw_actions
+
+##############################################################################
+# Behaviours
+##############################################################################
+
+class Template(py_trees.behaviour.Behaviour):
+    """
+    """
+    def __init__(
+        self,
+        name: str
+    ):
+        super(Template, self).__init__(name=name)
+        self.blackboard = self.attach_blackboard_client(name=self.name)
+
+        ########################
+        # Blackboard variables # 
+        ########################
+
+        self.blackboard.register_key(
+            key="current_person_id",
+            access=py_trees.common.Access.WRITE
+        )
+
+        # A list of dictionaries
+        self.blackboard.register_key(
+            key="recognized_people",
+            access=py_trees.common.Access.WRITE
+        )
+
+        self.blackboard.recognized_people = {}
+
+
+    def setup(self, **kwargs):
+        """
+        This function is executed when tree is setup.
+        Args:
+            **kwargs (:obj:`dict`): look for the 'node' object being passed down from the tree
+        Raises:
+            :class:`KeyError`: if a ros2 node isn't passed under the key 'node' in kwargs
+        """
+        self.logger.debug("%s.setup()" % self.__class__.__name__)
+
+        # Fetch 'node' object passed down from the tree.
+        try:
+            self.node = kwargs['node']
+        except KeyError as e:
+            error_message = "didn't find 'node' in setup's kwargs [{}]".format(self.qualified_name)
+            raise KeyError(error_message) from e # 'direct cause' traceability
+
+        # WRITE CLIENT SETUP CODE HERE
+
+        # Creating the client
+        self.analyse_faces_client = self.node.create_client(lhw_srvs.AnalyseFaces, '/analyse_faces')
+        # Check if the server is up and running.
+        if not self.analyse_faces_client.wait_for_service(timeout_sec=3.0):
+            raise RuntimeError("client timed out waiting for the visage_face_analysis")
+
+
+    def initialise(self):
+        """
+        This function is first to be executed when behaviour is runned.
+        """
+        self.logger.debug("%s.initialise()" % self.__class__.__name__)
+
+        # WRITE CODE HERE
+        request = lhw_srvs.AnalyseFaces.Request()
+        self.analyse_faces_future = self.analyse_faces_client.call_async(request)
+
+    def update(self) -> py_trees.common.Status:
+        """
+        This function is second to be executed right after initialise
+        when behaviour is runned. The execution will continue as long as
+        return flag is RUNNING and will be stopped when return flag is
+        SUCCESS or FAILURE.
+        """
+        self.logger.debug("%s.update()" % self.__class__.__name__)
+
+        # WRITE CODE HERE
+
+        return py_trees.common.Status.SUCCESS
+
+    def terminate(self, new_status: py_trees.common.Status):
+        """
+        This function is executed when behaviour is terminated.
+        Args:
+            new_status: the behaviour is transitioning to this new status
+        """
+        self.logger.debug("%s.terminate(%s)" % (self.__class__.__name__, "%s->%s" % (self.status, new_status) if self.status != new_status else "%s" % new_status))
+
+        # WRITE CODE HERE
+
+    # ADD PRIVATE FUNCTIONS HERE "_function_name"
\ No newline at end of file

src/lhw_intelligence/lhw_intelligence/behaviours/greet_person_behaviours.py

@@ -67,7 +67,7 @@ class BlackboardUpdated(py_trees.behaviour.Behaviour):
             self.blackboard.current_person_id
             self.node.get_logger().info("In TRY")
         except: 
-            self.node.get_logger().info("In Except")
+            
             
             # Temporary write access
             self.blackboard.register_key(
@@ -76,7 +76,7 @@ class BlackboardUpdated(py_trees.behaviour.Behaviour):
             )
             # recognized people borde se ut såhär: [{name: "", drink: "", id: 0}]
             #self.blackboard.set(self.variable_name, self.variable_value, overwrite=True)
-            self.blackboard.set("recognized_people", [{"id": 0}], overwrite=True)
+            self.blackboard.set("recognized_people", [{"id": 0, "actors": "Robert", "drinks": "Milk"}], overwrite=True)
             self.blackboard.set("current_person_id", 0, overwrite=True)
         
 
@@ -99,16 +99,19 @@ class BlackboardUpdated(py_trees.behaviour.Behaviour):
         
         # value = self.blackboard.recognized_people[self.blackboard.current_person_id][self.key] 
 
+        print(self.blackboard.recognized_people)
         # Try to read a value from recognized people
         try:
-            value = self.blackboard.recognized_people[self.blackboard.current_person_id][self.category] 
-            confirmed = self.blackboard.recognized_people[self.blackboard.current_person_id][f"{self.category}_confirmed"] 
+            value = self.blackboard.recognized_people[self.blackboard.current_person_id][self.category]             
+            # confirmed = self.blackboard.recognized_people[self.blackboard.current_person_id][f"{self.category}_confirmed"] 
         except: 
+            self.node.get_logger().info("In Except")
             return py_trees.common.Status.FAILURE
             
         # Return succes if value for key exists in blackboard and it is confirmed
-        if value and confirmed:
+        if value: # and confirmed
             return py_trees.common.Status.SUCCESS
+                
         # Otherwise return false
         return py_trees.common.Status.FAILURE
 
@@ -120,6 +123,7 @@ class BlackboardUpdated(py_trees.behaviour.Behaviour):
         """
         self.logger.debug("%s.terminate(%s)" % (self.__class__.__name__, "%s->%s" % (self.status, new_status) if self.status != new_status else "%s" % new_status))
 
+        self.node.get_logger().info("terminate")
         # WRITE CODE HERE
 
 

src/lhw_intelligence/lhw_intelligence/behaviours/guard_behaviour.py

@@ -42,4 +42,4 @@ class SetGuardBehaviour(py_trees.behaviour.Behaviour):
         self.blackboard.guard = self.val
 
     def update(self) -> py_trees.common.Status:
-        return py_trees.common.Status.SUCCESS
\ No newline at end of file
+        return py_trees.common.Status.SUCCESS

src/lhw_intelligence/lhw_intelligence/behaviours/introduce_person_behaviours.py

+##############################################################################
+# Documentation
+##############################################################################
+
+"""
+Template behaviour that is used for implementation of new behaviours.
+"""
+
+##############################################################################
+# Imports
+##############################################################################
+
+import py_trees
+import lhw_interfaces.msg as lhw_msgs
+import lhw_interfaces.srv as lhw_srvs
+import lhw_interfaces.action as lhw_actions
+from geometry_msgs.msg import Twist
+from rclpy.action import ActionClient
+#import std_msgs
+from std_msgs.msg import Bool, String
+import time
+
+##############################################################################
+# Behaviours
+##############################################################################
+"""
+NOT USED ATM
+"""
+class IntroducePeople(py_trees.behaviour.Behaviour):
+    """
+    """
+    def __init__(
+        self,
+        name: str
+        # person_to_present_id: int
+    ):
+        super(IntroducePeople, self).__init__(name=name)
+        self.person_to_present_id = person_to_present_id
+
+        self.blackboard = py_trees.blackboard.Client(name=self.name)
+
+        self.blackboard.register_key(
+            key="recognized_people",
+            access=py_trees.common.Access.READ
+        )      
+
+    def setup(self, **kwargs):
+        """
+        This function is executed when tree is setup.
+        Args:
+            **kwargs (:obj:`dict`): look for the 'node' object being passed down from the tree
+        Raises:
+            :class:`KeyError`: if a ros2 node isn't passed under the key 'node' in kwargs
+        """
+        self.logger.debug("%s.setup()" % self.__class__.__name__)
+
+        # Fetch 'node' object passed down from the tree.
+        try:
+            self.node = kwargs['node']
+        except KeyError as e:
+            error_message = "didn't find 'node' in setup's kwargs [{}]".format(self.qualified_name)
+            raise KeyError(error_message) from e # 'direct cause' traceability
+
+        self.is_talking_sub = self.node.create_subscription(
+            Bool, 'is_talking', self._is_talking_callback, 10
+        )
+
+    def initialise(self):
+        """
+        This function is first to be executed when behaviour is runned.
+        """
+        self.logger.debug("%s.initialise()" % self.__class__.__name__)
+
+        name = self.blackboard.recognized_people[self.person_to_present_id]['actor']
+        drink = self.blackboard.recognized_people[self.person_to_present_id]['drinks']
+        self.message = "This is " + name + "he likes to drink " + drink + "."
+
+        msg = String()
+        msg.data = self.message
+        self.say_pub.publish(msg)
+
+    def update(self) -> py_trees.common.Status:
+        """
+        This function is second to be executed right after initialise
+        when behaviour is runned. The execution will continue as long as
+        return flag is RUNNING and will be stopped when return flag is
+        SUCCESS or FAILURE.
+        """
+        self.logger.debug("%s.update()" % self.__class__.__name__)
+
+        if self.is_talking:
+            return py_trees.common.Status.RUNNING
+
+        return py_trees.common.Status.SUCCESS
+
+    def terminate(self, new_status: py_trees.common.Status):
+        """
+        This function is executed when behaviour is terminated.
+        Args:
+            new_status: the behaviour is transitioning to this new status
+        """
+        self.logger.debug("%s.terminate(%s)" % (self.__class__.__name__, "%s->%s" % (self.status, new_status) if self.status != new_status else "%s" % new_status))
+
+
+    def _is_talking_callback(self, msg):
+        self.is_talking = msg.data
+
+"""
+NOT USED
+"""
+class TempPerson(py_trees.behaviour.Behaviour):
+    """
+    """
+    def __init__(
+        self,
+        name: str,         
+    ):
+        super(TempPerson, self).__init__(name=name)        
+        self.blackboard = py_trees.blackboard.Client(name=self.name)
+
+    def setup(self, **kwargs):
+        """
+        This function is executed when tree is setup.
+        Args:
+            **kwargs (:obj:`dict`): look for the 'node' object being passed down from the tree
+        Raises:
+            :class:`KeyError`: if a ros2 node isn't passed under the key 'node' in kwargs
+        """
+        self.logger.debug("%s.setup()" % self.__class__.__name__)
+
+        # Fetch 'node' object passed down from the tree.
+        try:
+            self.node = kwargs['node']
+        except KeyError as e:
+            error_message = "didn't find 'node' in setup's kwargs [{}]".format(self.qualified_name)
+            raise KeyError(error_message) from e # 'direct cause' traceability
+
+        # Tries to read person from blackboard. 
+        # If person not found -> creates a mock person for debugging.
+         
+        self.blackboard.register_key(
+            key="recognized_people",
+            access=py_trees.common.Access.WRITE
+        )
+        
+        # Temporary write access
+        self.blackboard.register_key(
+            key="current_person_id",
+            access=py_trees.common.Access.WRITE
+        )
+        # recognized people borde se ut såhär: [{name: "", drink: "", id: 0}]
+        #self.blackboard.set(self.variable_name, self.variable_value, overwrite=True)
+        self.blackboard.set("recognized_people", [{"id": 0, "actors": "Robert", "drinks": "milk"}], overwrite=True)
+        self.blackboard.set("current_person_id", 0, overwrite=True)
+        
+
+    def initialise(self):
+        """
+        This function is first to be executed when behaviour is runned.
+        """
+        self.logger.debug("%s.initialise()" % self.__class__.__name__)
+
+        # WRITE CODE HERE    
+
+    def update(self) -> py_trees.common.Status:
+        return py_trees.common.Status.SUCCESS
+
+    def terminate(self, new_status: py_trees.common.Status):
+        """
+        This function is executed when behaviour is terminated.
+        Args:
+            new_status: the behaviour is transitioning to this new status
+        """
+        self.logger.debug("%s.terminate(%s)" % (self.__class__.__name__, "%s->%s" % (self.status, new_status) if self.status != new_status else "%s" % new_status))
+
+        # WRITE CODE HERE
+
+"""
+NOT USED
+"""
+class DoneSearchingCheck(py_trees.behaviour.Behaviour):
+    # Rotate behavior. Rotates set number of deg
+    def __init__(
+        self,
+        name: str,        
+    ):
+        super(DoneSearchingCheck, self).__init__(name=name)   
+        self.rotate_started = False      
+
+        self.blackboard = py_trees.blackboard.Client(name=name)     
+        
+    def setup(self, **kwargs):    
+        self.logger.debug("%s.setup()" % self.__class__.__name__)
+        try:
+            self.node = kwargs['node']
+        except KeyError as e:
+            error_message = "didn't find 'node' in setup's kwargs [{}]".format(self.qualified_name)
+            raise KeyError(error_message) from e # 'direct cause' traceability
+
+        self.blackboard.register_key(key="start_time_rotate", access=py_trees.common.Access.WRITE) 
+
+        self.publisher_ = self.node.create_publisher(Twist, 'cmd_vel', 10)        
+
+
+    def update(self) -> py_trees.common.Status: 
+        time_duration_sec = time.gmtime(time.time() - self.time_start).tm_sec  
+
+        if not self.blackboard["start_rotate_timer"]: # in not yet initialized
+            return py_trees.common.Status.FAILURE
+           
+        elif time_duration_sec > 20.0:
+            msg = Twist()
+
+            msg.linear.x = 0.0
+            msg.linear.y = 0.0
+            msg.linear.z = 0.0
+            msg.angular.x = 0.0
+            msg.angular.y = 0.0
+            msg.angular.z = 0.0
+            
+            self.publisher_.publish(msg)   
+            return py_trees.common.Status.SUCCESS
+
+
+class FindAndIntroduce(py_trees.behaviour.Behaviour):
+    # Rotate behavior. Rotates set number of deg
+    def __init__(
+        self,   
+        name: str,
+    ):
+        super(FindAndIntroduce, self).__init__(name=name)        
+        self.blackboard = py_trees.blackboard.Client(name=self.name)
+        
+        self.introduced_persons = []
+        self.blackboard = py_trees.blackboard.Client(name=name)     
+        self.to_introduce_id = ""
+        self.rotate_timeout = time.time()
+        self.talk_finished = False    
+        self.raised_arm = False
+        self.state = "looking for people"
+
+        
+    def setup(self, **kwargs):
+        self.logger.debug("%s.setup()" % self.__class__.__name__)
+        try:
+            self.node = kwargs['node']
+        except KeyError as e:
+            error_message = "didn't find 'node' in setup's kwargs [{}]".format(self.qualified_name)
+            raise KeyError(error_message) from e # 'direct cause' traceability        
+
+        # Blackboard varibles
+        self.blackboard.register_key(
+            key="recognized_people",
+            access=py_trees.common.Access.READ
+        )
+           
+
+    def initialise(self):   
+        self.node.get_logger().info("In find & introduce")
+        
+        # Needed to lift arm pepper
+        self._action_client = ActionClient(self.node, lhw_actions.RobotAction, 'robot_action')                         
+
+        # Turn off idle posture
+        goal_msg = lhw_actions.RobotAction.Goal()  
+        goal_msg.action.type = goal_msg.action.IDLE_POSTURE_OFF 
+        self.node.get_logger().info("Before server")
+        self._action_client.wait_for_server()
+        self.node.get_logger().info("After server")
+        self._action_client.send_goal_async(goal_msg)        
+        
+        # For pepper to talk
+        self.say_pub = self.node.create_publisher(String, 'say', 100)  
+        self.is_talking_sub = self.node.create_subscription(Bool, 'is_talking', self._is_talking_callback, 10)    
+    
+        # Rotate pepper
+        self.publisher_ = self.node.create_publisher(Twist, 'cmd_vel', 10)        
+        self.start_rotate()
+        self.node.get_logger().info("After rotate")
+        
+
+    def update(self) -> py_trees.common.Status:
+        self.node.get_logger().info("In update")
+
+        # Done with introducing the guests
+        if self.all_introduced():
+            self.stop_rotate()
+            return py_trees.common.Status.SUCCESS            
+        
+        if self.state == "looking for people":            
+            # Find new person
+            # TODO change from 0 to get_person_in_frame
+            self.to_introduce_id = 0 # get_person_in_frame()
+            
+            # Lift arm and introduce if not introduced
+            if not self.already_introduced():
+                self.state = "introducing"
+                self.stop_rotate()
+                self.lift_arm()   
+
+        if self.state == "introducing":
+            
+            if self.talk_finished: # Done with person          
+                self.talk_finished = False
+                self.raised_arm = False
+                self.start_rotate()
+                self.state = "looking for people"
+                self.rotate_timeout = time.time()
+                self.introduced_persons.append(self.to_introduce_id)
+
+        # Timeout if no people are found
+        time_duration_sec = time.gmtime(time.time() - self.rotate_timeout).tm_sec  
+        if time_duration_sec > 8.0:
+            return py_trees.common.Status.FAILURE 
+        
+        return py_trees.common.Status.RUNNING
+
+
+    def terminate(self, new_status: py_trees.common.Status):
+        # Turn on idle posture
+        goal_msg = lhw_actions.RobotAction.Goal()     
+        goal_msg.action.type = goal_msg.action.IDLE_POSTURE_ON
+        self._action_client.wait_for_server() 
+        self._action_client.send_goal_async(goal_msg)
+
+
+    """
+    Function to check if active person is not
+    already introduced
+    """
+    def already_introduced(self):  
+        return (self.to_introduce_id in self.introduced_persons)
+            
+    def all_introduced(self):
+        # Kolla i blackboard hur många som sitter i soffan
+        # return len(self.blackboard.in_sofa) == len(self.introduced_persons)            
+        return 1 == len(self.introduced_persons)            
+
+    def stop_rotate(self):
+        msg = Twist()
+        msg.linear.x = 0.0
+        msg.linear.y = 0.0
+        msg.linear.z = 0.0
+        msg.angular.x = 0.0
+        msg.angular.y = 0.0
+        msg.angular.z = 0.0
+        
+        print(msg)
+        
+        self.publisher_.publish(msg)   
+
+    def start_rotate(self):
+        msg = Twist()
+        msg.linear.x = 0.0
+        msg.linear.y = 0.0
+        msg.linear.z = 0.0
+        msg.angular.x = 0.0
+        msg.angular.y = 0.0
+        msg.angular.z = 0.1
+        self.publisher_.publish(msg)   
+        
+
+    # ***************************
+    # *** Introduce functions ***
+    # ***************************
+    def lift_arm(self, coords = [1.0, 0.0, 0.0]):
+        print("in lift_arm")
+        # goal_msg = Fibonacci.Goal()
+        goal_msg = lhw_actions.RobotAction.Goal()        
+        goal_msg.action.type = goal_msg.action.POINT_AT
+        goal_msg.action.arm = goal_msg.action.ARM_RIGHT
+        goal_msg.action.xyz = coords
+
+        self._action_client.wait_for_server()
+
+        self._send_goal_future = self._action_client.send_goal_async(goal_msg)
+
+        self._send_goal_future.add_done_callback(self.lift_arm_callback)        
+
+        # return self._action_client.send_goal_async(goal_msg)
+
+    """
+    Callback for when arm is raised
+    """
+    def lift_arm_callback(self, future):
+        self._goal_handle = future.result()
+        # self.get_logger().info("Callback done")
+        self.raised_arm = True
+        self.introduce()  
+
+
+    def introduce(self):        
+        msg = String()
+        person = self.blackboard.recognized_people[self.to_introduce_id]['actors']
+        drink = self.blackboard.recognized_people[self.to_introduce_id]['drinks']
+        print(person)
+        print(drink)
+        
+        # ^mode(disabled) to not use arms when talking
+        msg.data = f"^mode(disabled) This is {person}. {person} likes to drink {drink}.^mode(contextual)"
+        
+        # ^startTag(yes) - 
+        # happy
+        # happy birthday
+        
+        self.say_pub.publish(msg)   
+        
+        self.to_introduce_id = -1
+        
+
+    def _is_talking_callback(self, msg):
+        self.is_talking = msg.data
+        if not self.is_talking:
+            self.talk_finished = True
\ No newline at end of file

src/lhw_intelligence/lhw_intelligence/behaviours/motion_behaviours.py

@@ -11,12 +11,16 @@ Behaviours for basic motions
 ##############################################################################
 
 import py_trees
+import time
 import lhw_interfaces.msg as lhw_msgs
 import lhw_interfaces.srv as lhw_srvs
-import lhw_interfaces.actions as lhw_actions
+from rclpy.action import ActionClient
+import lhw_interfaces.action as lhw_actions
+from geometry_msgs.msg import Twist, Pose, PoseStamped
 
 from geometry_msgs.msg import Pose
-from std_msgs.msg import Bool
+from std_msgs.msg import Bool, String
+# from lhw_interfaces.action import RobotAction
 
 ##############################################################################
 # Behaviours
@@ -69,8 +73,6 @@ class MoveToPoint(py_trees.behaviour.Behaviour):
         self.time = 0
         self.ssh_client = ssh_client
         
-
-    
     def setup(self, **kwargs):
         try:
             self.node = kwargs['node']
@@ -133,17 +135,10 @@ class MoveToPoint(py_trees.behaviour.Behaviour):
 """ Move the robot to a specified location
 """
 class SetPose(py_trees.behaviour.Behaviour):
-    def __init__(self, name: str, pose: Pose, ssh_client) -> None:
+    def __init__(self, name, pose: Pose, ssh_client) -> None:
         super(SetPose, self).__init__(name)
         self.pose = pose
         self.ssh_client = ssh_client
-
-    def setup(self, **kwargs):
-        try:
-            self.node = kwargs['node']
-        except KeyError as e:
-            error_message = "didn't find 'node' in setup's kwargs [{}]".format(self.qualified_name)
-            raise KeyError(error_message) from e # 'direct cause' traceability
         
 
     def initialise(self):
@@ -178,11 +173,230 @@ pose:
 
         return py_trees.common.Status.SUCCESS
 
-""" Standby 
+""" Rotates the robot counter (?) clockwise.
 """
-class Standby(py_trees.behaviour.Behaviour):
-    def __init__(self, name) -> None:
-        super(Standby, self).__init__(name)
+class RotateRobot(py_trees.behaviour.Behaviour):
+    # Rotate behavior. Rotates set number of deg
+    def __init__(
+        self,
+        name: str,
+        duration: float
+    ):
+        super(RotateRobot, self).__init__(name=name)
+        self.duration = duration            
+        
 
-    def update(self):
-        return py_trees.common.Status.RUNNING
\ No newline at end of file
+    def setup(self, **kwargs):    
+        self.logger.debug("%s.setup()" % self.__class__.__name__)
+        try:
+            self.node = kwargs['node']
+        except KeyError as e:
+            error_message = "didn't find 'node' in setup's kwargs [{}]".format(self.qualified_name)
+            raise KeyError(error_message) from e # 'direct cause' traceability
+
+        # self._action_client = ActionClient(self.node, Twist, 'robot_action')
+        self.publisher_ = self.node.create_publisher(Twist, 'cmd_vel', 10)
+
+    def initialise(self):     
+
+        self.time_start = time.time()
+        print("Start timer")
+
+        msg = Twist()
+        msg.linear.x = 0.0
+        msg.linear.y = 0.0
+        msg.linear.z = 0.0
+        msg.angular.x = 0.0
+        msg.angular.y = 0.0
+        msg.angular.z = 0.1
+        self.publisher_.publish(msg)   
+
+
+    def update(self) -> py_trees.common.Status: 
+        print("upadating")
+        
+        time_duration_sec = time.gmtime(time.time() - self.time_start).tm_sec   
+
+        print(f"Time duration: {time_duration_sec}")
+        # if time_duration_sec > self.duration:
+        if time_duration_sec > 5.0:
+            print("TIMER OUT")
+            return py_trees.common.Status.SUCCESS
+        
+        # if self.is_rotating:
+        #     return py_trees.common.Status.RUNNING
+
+        return py_trees.common.Status.RUNNING     
+
+    def terminate(self, new_status: py_trees.common.Status):        
+        print("terminate")
+        msg = Twist()
+        msg.linear.x = 0.0
+        msg.linear.y = 0.0
+        msg.linear.z = 0.0
+        msg.angular.x = 0.0
+        msg.angular.y = 0.0
+        msg.angular.z = 0.0
+        self.publisher_.publish(msg)   
+        
+        self.logger.debug("%s.terminate(%s)" % (self.__class__.__name__, "%s->%s" % (self.status, new_status) if self.status != new_status else "%s" % new_status))        
+
+    def goal_response_callback(self, future):
+        self._goal_handle = future.result()
+        self.get_logger().info("Callback done")
+
+
+""" Start the robot to rotate
+"""
+class StartRotate(py_trees.behaviour.Behaviour):
+    # Rotate behavior. Rotates set number of deg
+    def __init__(
+        self,
+        name: str,        
+    ):
+        super(StartRotate, self).__init__(name=name)   
+        self.rotate_started = False      
+
+        self.blackboard = py_trees.blackboard.Client(name=name)     
+        
+    def setup(self, **kwargs):    
+        self.logger.debug("%s.setup()" % self.__class__.__name__)
+        try:
+            self.node = kwargs['node']
+        except KeyError as e:
+            error_message = "didn't find 'node' in setup's kwargs [{}]".format(self.qualified_name)
+            raise KeyError(error_message) from e # 'direct cause' traceability
+
+        self.blackboard.register_key(key="start_time_rotate", access=py_trees.common.Access.WRITE) 
+
+        self.publisher_ = self.node.create_publisher(Twist, 'cmd_vel', 10)
+
+    def initialise(self):         
+        msg = Twist()
+        msg.linear.x = 0.0
+        msg.linear.y = 0.0
+        msg.linear.z = 0.0
+        msg.angular.x = 0.0
+        msg.angular.y = 0.0
+        msg.angular.z = 0.1
+        self.publisher_.publish(msg)   
+
+        if not self.blackboard["start_rotate_timer"]:
+            self.blackboard["start_rotate_timer"] = time.time()
+        
+
+    def update(self) -> py_trees.common.Status: 
+        return py_trees.common.Status.SUCCESS     
+
+
+
+class RaiseRightArm(py_trees.behaviour.Behaviour):
+    # Rotate behavior. Rotates set number of deg
+    def __init__(
+        self,
+        name: str,           
+    ):
+        super(RaiseRightArm, self).__init__(name=name)        
+        self.blackboard = py_trees.blackboard.Client(name=self.name)
+        
+    def setup(self, **kwargs):
+        self.logger.debug("%s.setup()" % self.__class__.__name__)
+        try:
+            self.node = kwargs['node']
+        except KeyError as e:
+            error_message = "didn't find 'node' in setup's kwargs [{}]".format(self.qualified_name)
+            raise KeyError(error_message) from e # 'direct cause' traceability
+
+        self._action_client = ActionClient(self.node, lhw_actions.RobotAction, 'robot_action')
+
+        self.is_talking_sub = self.node.create_subscription(
+            Bool, 'is_talking', self._is_talking_callback, 10
+        )
+
+        self.talk_finished = False    
+        self.raised_arm = False
+
+        self.blackboard.register_key(
+            key="recognized_people",
+            access=py_trees.common.Access.READ
+        )
+
+        self.blackboard.register_key(
+            key="current_person_id",
+            access=py_trees.common.Access.READ
+        )
+
+    def initialise(self):      
+        self.say_pub = self.node.create_publisher(String, 'say', 100)
+                
+        goal_msg = lhw_actions.RobotAction.Goal()     
+    
+        # Stäng av idle
+        goal_msg.action.type = goal_msg.action.IDLE_POSTURE_OFF
+
+        self._action_client.wait_for_server()
+        
+        self._action_client.send_goal_async(goal_msg)
+
+        # Raise arm
+        self.send_goal()
+       
+
+    def update(self) -> py_trees.common.Status:
+        print("in update")
+
+        if self.raised_arm and self.talk_finished:
+            return py_trees.common.Status.SUCCESS   
+
+        return py_trees.common.Status.RUNNING     
+
+    def terminate(self, new_status: py_trees.common.Status):
+        # Turn on idle
+        goal_msg = lhw_actions.RobotAction.Goal()     
+
+        # goal_msg.action.type = goal_msg.action.POINT_QUIT
+        # goal_msg.action.arm = goal_msg.action.ARM_RIGHT
+        goal_msg.action.type = goal_msg.action.IDLE_POSTURE_ON
+
+        self._action_client.wait_for_server()
+        
+        self._action_client.send_goal_async(goal_msg)
+
+
+    def send_goal(self):
+        print("in send_goal")
+        # goal_msg = Fibonacci.Goal()
+        goal_msg = lhw_actions.RobotAction.Goal()        
+        goal_msg.action.type = goal_msg.action.POINT_AT
+        goal_msg.action.arm = goal_msg.action.ARM_RIGHT
+        goal_msg.action.xyz = [1.0, 0.0, 0.0]
+
+        self._action_client.wait_for_server()
+
+        self._send_goal_future = self._action_client.send_goal_async(goal_msg)
+
+        self._send_goal_future.add_done_callback(self.goal_response_callback)        
+
+        # return self._action_client.send_goal_async(goal_msg)
+
+    def goal_response_callback(self, future):
+        self._goal_handle = future.result()
+        # self.get_logger().info("Callback done")
+        self.raised_arm = True
+        self.say()        
+
+        
+    def say(self):        
+        msg = String()
+        person = self.blackboard.recognized_people[self.blackboard.current_person_id]['actors']
+        drink = self.blackboard.recognized_people[self.blackboard.current_person_id]['drinks']
+        print(person)
+        print(drink)
+        msg.data = f"This is {person}. {person} likes to drink {drink}."
+        # msg.data = self.say_message 
+        self.say_pub.publish(msg)
+
+    def _is_talking_callback(self, msg):
+        self.is_talking = msg.data
+        if not self.is_talking:
+            self.talk_finished = True

src/lhw_intelligence/lhw_intelligence/behaviours/tablet_behaviors.py

@@ -148,7 +148,6 @@ class TabletShowFIALogo(py_trees.behaviour.Behaviour):
     def __init__(self, name) -> None:
         super(TabletShowFIALogo, self).__init__(name)
 
-
     def setup(self, **kwargs):
         try:
             self.node = kwargs['node']

src/lhw_intelligence/lhw_intelligence/trees/safety_check_tree.py

+
+
+
+import py_trees
+
+def create_root() -> py_trees.behaviour.Behaviour:
+    """
+    """
\ No newline at end of file

src/lhw_intelligence/lhw_intelligence/trees/scenario_sketches/introduce_person_tree.tree

+?;;Root
+    (search done)
+    ->;; look_for_new_people_sequence
+        [rotate X deg]
+        [look for people] ;; returns false if no new people
+        [lift arm]
+        [introduce found persons]
+
+?;;Root
+    (search done) ;; check start time and count persons of total --> stop rotate is true
+    ->;; look_for_new_people_sequence
+        [start rotate]
+        [find & introduce] 
+            ;; 1. find person in middle of frame else false
+            ;; 2. filter for new person else false
+            ;; 3. stop rotate
+            ;; 4. lift arm to 3D-cord.
+            ;; 5. introduce phrase        
\ No newline at end of file

src/lhw_intelligence/lhw_intelligence/trees/scenario_trees/introduce_person_tree.py

+import py_trees
+import py_trees_ros
+import py_trees_ros.trees
+import py_trees.console as console
+import rclpy
+import sys
+
+# from ../behaviours import greet_person_behaviours
+from lhw_intelligence.behaviours import introduce_person_behaviours
+from lhw_intelligence.behaviours import speech_behaviours
+from lhw_intelligence.behaviours import guard_behaviour
+from lhw_intelligence.behaviours import motion_behaviours 
+from lhw_intelligence.behaviours import greet_person_behaviours # idle for testing
+
+def create_root() -> py_trees.behaviour.Behaviour:
+    root = py_trees.composites.Selector(name="Root")
+
+    # -------------------------------------------
+    # |         Find and introduce poople       |
+    # -------------------------------------------
+
+    fill_blackboard = greet_person_behaviours.BlackboardUpdated(
+        name="FillBlackboard",
+        category="drinks"
+    )
+
+    look_for_new_people_sequence = py_trees.composites.Sequence(
+        name="FindNameSelector"
+    )
+
+    rotate_x_deg =  motion_behaviours.RotateRobot(
+        name="RotateRobot",
+        duration=10.0
+    )
+
+    raise_right_arm =  motion_behaviours.RaiseRightArm(
+        name="RaiseRightArm"
+    )
+
+    temp_person =  introduce_person_behaviours.TempPerson(
+        name="temp_person"
+    )
+
+    find_and_introduce = introduce_person_behaviours.FindAndIntroduce(
+        name="FindAndIntroduce"
+    )
+
+    # look_for_people = introduce_person_behaviours.LookForPeople(
+    #     name="Look For People"        
+    # )
+
+    # lift_arm = motion_behaviours.lift_arm(
+    #     name="Lift Arm",
+    # )
+
+    # introduce_people = introduce_person_behaviours.IntroducePeople(
+    #     name="Introduce People"
+    # )
+
+    # search_done_guard = guard_behaviour.GuardBehaviour(
+    #     name="Search Done Guard", 
+    # )
+
+    idle = greet_person_behaviours.Idle(name = "idle")
+
+
+    look_for_new_people_sequence.add_children([
+        fill_blackboard,
+        find_and_introduce
+        # temp_person, 
+        # rotate_x_deg,
+        # raise_right_arm,
+        # idle
+        # look_for_people, 
+        # lift_arm,
+        # introduce_people
+    ])
+
+
+    root.add_children([look_for_new_people_sequence])
+
+    return root
+
+def main():
+    """
+    """
+    rclpy.init(args=None)
+
+    root = create_root()
+    tree = py_trees_ros.trees.BehaviourTree(
+        root=root,
+        unicode_tree_debug=True
+    )
+    try:
+        tree.setup(timeout=15)
+    except py_trees_ros.exceptions.TimedOutError as e:
+        console.logerror(console.red + "failed to setup the tree, aborting [{}]".format(str(e)) + console.reset)
+        tree.shutdown()
+        rclpy.shutdown()
+        sys.exit(1)
+    except KeyboardInterrupt:
+        # not a warning, nor error, usually a user-initiated shutdown
+        console.logerror("tree setup interrupted")
+        tree.shutdown()
+        rclpy.shutdown()
+        sys.exit(1)
+
+    tree.tick_tock(period_ms=1000.0)
+
+    try:
+        rclpy.spin(tree.node)
+    except KeyboardInterrupt:
+        pass
+
+    tree.shutdown()
+    rclpy.shutdown()
+
+
+"""
+How to do motion things with pepper:
+1. ssh nao@10.133.5.209, psw: ellis
+2. go to catkin_ws/src/lhw_bringup/launch
+3. roslaunch drivers.launch
+4. roslaunch full.launch            -->navstack
+4. rostopic pub /cmd_vel geometry_msgs/Twist "linear:
+  x: 0.1
+  y: 0.0
+  z: 0.0
+angular:
+  x: 0.0
+  y: 0.0
+  z: 0.0"
+
+How to start pepper:
+1. ssh nao@10.133.5.209
+2: exit
+2. go to ~
+3. python turn_off_diagnosis_effect.py 
+4. qicli call ALMotion.wakeUp
+ 
+To stop autonomous movement
+ qicli call ALAutonomousLife.setState "disabled"
+Reenable with
+ qicli call ALAutonomousLife.setState "solitary"
+
+notes:
+changed to false from true on line 20 in src/lhw_qi/src/animated_spech_component.cpp
+
+"""

src/lhw_intelligence/lhw_intelligence/visage_client.py

+import rclpy
+from rclpy.action import ActionClient
+from rclpy.node import Node
+
+from lhw_interfaces.action import FaceAnalysis 
+
+
+class VisageAnalysisClient(Node):
+
+    def __init__(self, _done_callback):
+        super().__init__('face_analysis_client')
+        self._action_client = ActionClient(self, FaceAnalysis, 'face_analysis')
+        self._done_callback = _done_callback;
+
+    def send_goal(self, setting):
+        goal_msg = FaceAnalysis.Goal()
+        goal_msg.setting = 3
+
+        self._action_client.wait_for_server()
+        print("sending request") 
+        self._send_goal_future = self._action_client.send_goal_async(goal_msg)
+        self._send_goal_future.add_done_callback(self.goal_response_callback)
+
+        return self._action_client.send_goal_async(goal_msg)
+
+    def goal_response_callback(self, future):
+        goal_handle = future.result()
+        if not goal_handle.accepted:
+            self.get_logger().info('Goal rejected :(')
+            return
+
+        self.get_logger().info('Goal accepted :)')
+
+        self._get_result_future = goal_handle.get_result_async()
+        self._get_result_future.add_done_callback(self.get_result_callback)
+
+    def get_result_callback(self, future):
+        result = future.result().result
+        self.get_logger().info('Result: {0}'.format(result.faces))
+        self.get_logger().info('Result: {0}'.format(result.faces))
+        print(' ==========the id is ==========')
+        for i in range(0,len(result.faces.faces_detected)):
+            print(result.faces.faces_detected[i].id)
+        self._done_callback(result.faces.faces_detected[i].id); 
+        rclpy.shutdown()
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    def test(id):
+        print(id)
+
+    action_client = VisageAnalysisClient(test)
+
+    action_client.send_goal(3)
+    rclpy.spin(action_client)
+    #rclpy.add_callback()
+    #print(future)
+    #response = future.get_result()
+    #print(response)
+
+if __name__ == '__main__':
+    main()
\ No newline at end of file

src/lhw_intelligence/setup.py

@@ -30,6 +30,7 @@ setup(
         'console_scripts': [
             'head_touch_tree = lhw_intelligence.trees.scenario_trees.head_touch_tree:main',
             'greet-person-tree = lhw_intelligence.trees.scenario_trees.greet_person_tree:main'
+            'visage_client = lhw_intelligence.visage_client:main'
 
         ],
     },

src/lhw_interfaces/CMakeLists.txt

@@ -61,8 +61,11 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "msg_vision/DeepFaceEmotions.msg"
   "msg_vision/FaceDetected.msg"
   "msg_vision/FacesDetected.msg"
+  "msg_vision/VisageSettings.msg"
   "msg_vision/PixelToXYZ.srv"
   "msg_vision/TransformXYZ.srv"
+  "srv_visage/FaceAnalysis.srv"
+  "action_visage/FaceAnalysis.action"
   "srv_intelligence/Scan.srv"
   "srv_nlp/DiscoverObject.srv"
   "action_intelligence/RobotAction.action"

src/lhw_interfaces/action_visage/FaceAnalysis.action

+uint16 setting
+---
+FacesDetected faces
+---
+bool done
\ No newline at end of file

src/lhw_interfaces/msg_vision/FaceDetected.msg

-# Switch to int8 if continues to crash?
+# Different sources
+uint8 VISAGE = 0
+
+uint8 source
 
+# Switch to int8 if continues to crash?
 # id of the target
 uint16 id
 
-# z_index
 uint16 z_index
 
-# Age
 uint16 age
 
-# Gender
 string gender
 
-# Bounding box
+# Values to decide if gender or age are being estimated 
+bool estimate_age
+bool estimate_gender
+
+# Float value of how similar this person is to id
+# -1 => Failure
+float32 similarity
+
+# Center of this person's face in analyzed image
 int32[2] face_center_xy # [cx, cy]
+int32[4] face_bbox # [x1, y1, x2, y2]
 
 # Distance
-int32 dist 
\ No newline at end of file
+int32 dist

src/lhw_interfaces/msg_vision/VisageSettings.msg

+# Switch to int8 if continues to crash?
+
+# Detection settings
+uint8 SETTING_DETECT_AND_RECOGNIZE = 0
+uint8 SETTING_RECOGNIZE = 1
+uint8 SETTING_DETECT = 2
+uint8 SETTING_NOTHING = 3
+
+uint8 setting # Value of enums above
+
+# Estimation settings for turning On/Off
+bool estimate_age True
+bool estimate_gender True
+bool estimate_emotion False # TODO: Currently not implemented
+
+float32 recognition_threshold 0.72
+float32 detection_threshold_buffer 0.10
+uint8 max_faces 3
+# TODO: Add more settings?
+# Faces? Etc.
\ No newline at end of file