Skip to content
Snippets Groups Projects

Compare revisions

Changes are shown as if the source revision was being merged into the target revision. Learn more about comparing revisions.

Source

Select target project
No results found

Target

Select target project
  • liuhomewreckers/liu-home-wreckers
  • tdde19-2021-5/tdde-19-group-5-pepper
2 results
Show changes
Hide whitespace changes
Inline Side-by-side
Showing
with 654 additions and 1050 deletions
src/lhw_intelligence/lhw_intelligence/scenario_trees/test.tree deleted 100644 → 0
View file @ d4b266eb
->;;Greet Person Subtree
| [Greet];;Say
| ?;;Name Wait Sel
| | ->;;Get Name Seq
| | | [Ask For Name];;Say
| | | [Head Touched];;IsHeadTouched
| | | (Is Door Open);;IsDoorOpen
| | | [Complete];;Say
| | ->;;Ask Name Again Seq
| | | [Ask Name Again];;Say
| | | [Reset Head Touched];;ResetHeadTouched
| | | [Fail];;FAILURE
| | [Name Retry];;Standby
\ No newline at end of file
src/lhw_intelligence/lhw_intelligence/states/emotional_pepper.py deleted 100644 → 0
View file @ d4b266eb
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 Entities, DeepFaceEmotion, DeepFaceEmotions, EventT2E, Response
from datetime import datetime, timedelta
from naoqi_bridge_msgs.srv import SetString
import transitions as sm
import rclpy
import time
CB_TIMER_S = 4
def timer_passed(time, time_passed_s=CB_TIMER_S):
"""Checks time
Args:
now (datetime.time):
prev (datetime.time):
Returns:
bool: if time passed according to a timer constant
"""
return datetime.now() - time > timedelta(seconds=time_passed_s)
class Person:
def __init__(self, id, emotion, name="unknown") -> None:
self.id = id
self.last_emotion = None
self.current_emotion = emotion
self.name = name
self.timers = {} # E.g. use for when person was last greeted or when emotion was last spoken
self.seen()
@property
def new_emotion(self):
return self.last_emotion != self.current_emotion or timer_passed(self.timers["emotion"], 5)
@property
def is_forgotten(self):
# Is forgotten if not seen for 20 seconds
return timer_passed(self.timers["seen"], 20)
def seen(self):
self.last_seen = datetime.now()
self.set_timer("seen")
def feel(self, emotion="unknown"):
self.last_emotion = self.current_emotion
self.current_emotion = emotion
self.set_timer("emotion")
def time_since_or_create(self, key, time_passed_s=CB_TIMER_S):
"""Checks if a timer has passed and if the timer doesn't exists, creates it then returns True.
Used to avoid spam when more data is received than can be handled.
Args:
key (string): Key in a dictionary
time_passed_s (int, optional): Time in seconds. Defaults to CB_TIMER_S.
Returns:
Bool: True if time passed/timer didn't exist. False otherwise
"""
time = datetime.now()
passed = True
if key in self.timers.keys():
passed = timer_passed(self.timers[key], time_passed_s)
if passed:
self.timers[key] = time
else:
self.timers[key] = time
return passed
def set_timer(self, key):
self.timers[key] = datetime.now()
class EmotionalPepper(State):
states = ["look_for_persons", "greet_person", "recognize_person",
"free_talk", "analyze_emotion", "ask_for_name"]
def set_emotion(self, event):
self.emotion = event.kwargs.get("emotion", "unknown")
if self.emotion["id"] != -1:
self.new_face_emotion = True
# Could add a dictionary, e.g. 0 you look surprised. 12 you look happy.
def __init__(self, node, args=[]):
super().__init__(node, args)
self.name = node.get_name()
self.node = node
self.emotion = "unknown"
self.log = self.node.get_logger()
self.new_face_emotion = False
self.new_nlp_emotion = False
self.greeted = {}
self.person_in_frame = False
self.listening_for_entities = False
self.listening_for_emotion = False
self.timers = {}
self.people = {} # Id to Person dict. TODO: Put out in a db or in the general state class
# State machine
self.machine = sm.Machine(model=self, states = EmotionalPepper.states, initial="dummy", send_event=True)
self.to_look_for_persons()
self.machine.add_transition("trigger_found_person", "look_for_persons", "recognize_person")
self.machine.add_transition("trigger_greet_person", "recognize_person", "greet_person")
self.machine.add_transition("trigger_free_talk", "greet_person", "free_talk")
self.machine.add_transition("trigger_analyze_emotion", "*", "analyze_emotion", before="set_emotion")
self.machine.add_transition("trigger_ask_for_name", "*", "ask_for_name")
# ROS
self.entities_sub = self.node.create_subscription(Entities, "yolov5_entities", self.__entities_cb, 1)
self.emotions_sub = self.node.create_subscription(DeepFaceEmotion, "emotion", self.__emotion_cb, 1)
self.emotions_sub = self.node.create_subscription(DeepFaceEmotions, "emotions", self.__emotions_cb, 1)
self.nlp_emotion_sub = self.node.create_subscription(EventT2E, "nlp_emotion", self.__nlp_emotion_cb, 1)
self.run_timer = self.node.create_timer(2, self.__run)
self.tablet_cli_show_url = self.node.create_client(SetString, "tablet/show_url")
self.tablet_cli_show_image = self.node.create_client(SetString, "tablet/show_image")
def forget_people(self):
for id, person in self.people.items():
person.forget()
def show_url(self, url="http://google.se"):
req = SetString.Request()
req.data = url
self.node.get_logger().info(f"Showing url {url}")
return self.tablet_cli_show_url.call_async(req)
"""
enter/exit callbacks
"""
def on_enter_greet_person(self, event):
self.log.info("Entering greet person")
self.say("Hello Person")
def on_exit_greet_person(self, event):
self.log.info("Exiting greet person")
def on_enter_look_for_persons(self, event):
self.log.info("Entering look_for_persons")
self.say("Looking for persons")
def on_exit_look_for_persons(self, event):
self.log.info("Exiting look_for_persons")
def on_enter_recognize_person(self, event):
self.log.info("Entering recognize_person")
def on_exit_recognize_person(self, event):
pass
def on_enter_free_talk(self, event):
self.log.info("Entering free_talk")
def on_exit_free_talk(self, event):
self.log.info("Exiting free_talk")
def on_enter_analyze_emotion(self, event):
detection = event.kwargs.get("emotion", "unknown")
id = detection["id"]
emotion = detection["emotion"]
self.log.info(f"Analyzing emotion: {id}: {emotion}")
def on_exit_analyze_emotion(self, event):
pass
def on_enter_ask_for_name(self, event):
self.log.info("Entering ask_for_name")
self.say("What's your name?")
def on_exit_ask_for_name(self, event):
pass
"""
end callbacks
"""
def tablet_emotion(self, emotion):
self.log.info(f"Show emotion {emotion} on tablet")
if emotion == "neutral":
self.show_url("https://simonws-athome.s3.eu-north-1.amazonaws.com/neutral.png")
elif emotion == "happy":
self.show_url("https://simonws-athome.s3.eu-north-1.amazonaws.com/happy.png")
elif emotion == "angry":
self.show_url("https://simonws-athome.s3.eu-north-1.amazonaws.com/angry.png")
elif emotion == "surprise":
self.show_url("https://simonws-athome.s3.eu-north-1.amazonaws.com/surprise.png")
elif emotion == "sad":
self.show_url("https://simonws-athome.s3.eu-north-1.amazonaws.com/sad.png")
elif emotion == "disgust":
self.show_url("https://simonws-athome.s3.eu-north-1.amazonaws.com/disgust.png")
elif emotion == "fear":
self.show_url("https://simonws-athome.s3.eu-north-1.amazonaws.com/fear.png")
def __seen_people(self):
seen = []
for id, person in self.people.items():
if not person.is_forgotten:
seen.append(id)
return seen
def __run(self):
self.log.info(f"State: {self.state}")
self.log.info(f"Currently seeing: {self.__seen_people()}")
# TODO: Run an abstract method on current state, extend state class?
if self.state == "greet_person":
self.trigger_free_talk()
elif self.state == "look_for_persons":
# self.listening_for_entities = True
self.log.info(str(self.listening_for_entities))
elif self.state == "recognize_person":
self.trigger_greet_person()
elif self.state == "free_talk":
pass
# self.listening_for_emotion = True
elif self.state == "analyze_emotion":
if self.new_face_emotion and self.__seen_people():
self.new_face_emotion = False
self.say(f"You look {self.emotion['emotion']}")
self.tablet_emotion(self.emotion["emotion"])
if self.new_nlp_emotion:
self.new_nlp_emotion = False
self.say(f"You sound {self.nlp_emotion}")
else:
self.say("Unknown state")
def __entities_cb(self, entities):
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") and self.listening_for_entities:
# if not self.person_in_frame:
self.node.get_logger().info("Found person!")
self.person_in_frame = True
self.to_recognize_person()
found_person = True
if not found_person:
self.person_in_frame = False
# TODO: Maybe forget people here
def __emotion_cb(self, emotions):
# TODO: Use z-index. E.g. the person in the back looks angry
id = emotions.id
emotion = emotions.dominant_emotion
# New person!
if id not in self.people:
self.say(f"Oh wow, a new person. I'll remember you as {id}")
self.people[id] = Person(id, emotion)
self.trigger_ask_for_name()
else: # Known person!
time_to_say_hello = self.people[id].time_since_or_create("greet", 20)
self.log.info(f"Time to say hello: {time_to_say_hello}")
if self.people[id].is_forgotten or (time_to_say_hello and self.people[id].time_since_or_create("seen", 20)):
self.say(f"Nice to see you {id}")
self.people[id].feel(emotion)
self.people[id].seen()
# TODO: Move this to the enter of ask_for_name and put it in the else above
if self.people[id].new_emotion:
self.trigger_analyze_emotion(emotion={"id": emotions.id, "emotion": emotions.dominant_emotion})
def __emotions_cb(self, emotions):
self.log.info("Got emotion_s_ cb")
self.log.info(str(emotions))
def __nlp_emotion_cb(self, emotions):
emotion = emotions.dominant_emotion
# self.log.info(str(emotion))
angry = emotions.angry
disgust = emotions.disgust
fear = emotions.fear
happy = emotions.happy
neutral = emotions.neutral
sad = emotions.sad
surprise = emotions.surprised
lst = [time.time() - self.start_time, angry, disgust, fear, happy, neutral ,sad, surprise, emotion]
s_lst = [str(s) for s in lst]
self.log.info("\t".join(s_lst))
self.fh_nlp.write("\t".join(s_lst))
self.fh_nlp.write("\n")
self.new_nlp_emotion = True
self.nlp_emotion = dominant
# TODO: May not be a good idea to send unknown id
self.trigger_analyze_emotion(emotion={"id": -1, "emotion": dominant})
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)
def main(args=None):
rclpy.init(args=args)
node = StateMachineNode()
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
if __name__ == "__main__":
main()
\ No newline at end of file
src/lhw_intelligence/lhw_intelligence/states/state.py deleted 100644 → 0
View file @ d4b266eb
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
import transitions as sm
class State:
"""
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, args=[]):
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 enter(self):
pass
def run(self):
pass
def exit(self):
pass
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
src/lhw_intelligence/lhw_intelligence/tree_main.py 0 → 100644
View file @ 2f7d10cb
#!/usr/bin/env python3
#
# Assignee: Markus Handstedt (@marha066)
# Issue: #158: Standardise Py-Trees
# Branch: 158-standardise-py-trees
#
##############################################################################
# Documentation
##############################################################################
"""
"""
##############################################################################
# Imports
##############################################################################
import py_trees
import rclpy
import paramiko
import sys
import getopt
##############################################################################
# Tree
##############################################################################
def usage():
print("Help") # TODO: print help info
def main():
"""
"""
# Remove 1st argument from the
# list of command line arguments
argv = sys.argv[1:]
options = "f:d:t:h:"
long_options = ["file =", "debug =", "tick_tock_period = ", "help ="]
try:
opts, args = getopt.getopt(argv, options, long_options)
except getopt.GetoptErroras as err:
print (str(err))
file_name = ''
debug = False
tick_tock_period = 1000.0
for opt, arg in opts:
if opt in ['-f', '--file']:
file_name = arg
else:
print("You need to input a file with (-f or --file)")
sys.exit()
if opt in ['-d', '--debug']:
debug = True
if opt in ['-t', '--tick_tock_period']:
tick_tock_period = arg
if opt in ['-h', '--help']:
usage()
sys.exit()
src/lhw_intelligence/lhw_intelligence/tree_point_action.py deleted 100644 → 0
View file @ d4b266eb
#!/usr/bin/env python3
#
# Assignee: Markus Handstedt (@marha066)
# Issue: #115 Create service for using face recognition
# Branch: 115-create-service-for-using-face-recognition
#
##############################################################################
# Documentation
##############################################################################
"""
About
^^^^^
Tree for point action
Tree
^^^^
.. code-block:: bash
# Launch the face analysis tree
# In the lhw_qi container
$ export PEPPER_IP=<put ip address here>
$ ros2 run lhw_qi motion -i $PEPPER_IP
# In the lhw_intelligence container
$ ros2 run lhw_intelligence tree-point_action
# In a different lhw_intelligence container shell,
# introspect the entire blackboard
$ py-trees-blackboard-watcher
"""
##############################################################################
# Imports
##############################################################################
import py_trees
import py_trees_ros
import py_trees_ros.trees
import py_trees.console as console
from std_msgs.msg import Int32
import lhw_interfaces.msg as lhw_msg
import lhw_interfaces.srv as lhw_srv
import lhw_interfaces.action as lhw_action
import rclpy
import sys
import json
from paramiko.util import log_to_file
import py_trees
import operator
from .py_tree_behaviours import *
from .motion_behaviours import *
from lhw_intelligence.behaviour_copy import Behaviour
from .tablet_behaviours import *
from . import action_clients
from . import action_clients, behaviours, motion_behaviours
def greet_person(node, repeat_message="Okay, can you repeat that?") -> py_trees.behaviour.Behaviour:
start = Say(name="Start", node=node, message="Hello! Could you please look into my eyes so I can identify you?")
find_person = WaitForPerson(name="Find person", node=node)
#turn_off_camera = TurnCameraOnOrOff(name="Camera Off1", node=node, turn_on=False)
greet = Say(name="Greet", node=node, message="Welcome!")
ask_for_name = Say(name="Ask For Name", node=node, message="What's your name?")
wait_for_name = TabletInput(name="Tablet Name Input", node=node, text="Please input your name", type="name")# WaitForItem(name="Wait For Name", node=node, key="name")
say_name_confirm = Say(name="test1", node=node, message="If this is your name say yes. If you want to retype your answer say no.")
confirm_name = ConfirmSay(name="Confirm Name", node=node)
add_name = AddItemToPerson(name="Add Name", node=node)
ask_name_again = Say(name="Ask Name Again", node=node, message=repeat_message)
fail_name = py_trees.behaviours.Failure(name='Name Failure')
name_retry = Standby(name="Name Retry", node=node)
ask_for_favorite_drink = Say(name="Ask For Favorite Drink", node=node, message="What's your favorite drink?")
wait_for_drink = TabletInput(name="Tablet Name Input", node=node, text="Please input your favorite drink", type="drink")#WaitForItem(name="Wait For Drink", node=node, key="drinks")
say_drink_confirm = Say(name="test1", node=node, message="If this is your favourite drink say yes. If you want to retype your answer say no.")
confirm_drink = ConfirmSay(name="Confirm Drink", node=node)
add_drink = AddItemToPerson(name="Add Drink", node=node)
ask_drink_again = Say(name="Ask Drink Again", node=node,message=repeat_message)
fail_drink = py_trees.behaviours.Failure(name='Drink Failure')
drink_retry = Standby(name="Drink Retry", node=node)
completed_greet = Say(name="Complete", node=node, message="Thank you! That should be all I need to know.")
#turn_on_camera= TurnCameraOnOrOff(name="Camera On1", node=node, turn_on=True)
get_name_seq = py_trees.composites.Sequence(
name="Get Name Seq",
children=[ask_for_name, wait_for_name, add_name] #say_name_confirm, confirm_name,
)
ask_name_again_seq = py_trees.composites.Sequence(
name="Ask Name Again Seq",
children=[ask_name_again, fail_name]
)
name_wait_sel = py_trees.composites.Selector(
name="Name Wait Sel",
children=[get_name_seq, ask_name_again_seq, name_retry]
)
get_drink_seq = py_trees.composites.Sequence(
name="Get Drink Seq",
children=[ask_for_favorite_drink, wait_for_drink, add_drink] #say_drink_confirm, confirm_drink,
)
ask_drink_again_seq = py_trees.composites.Sequence(
name="Ask Drink Again Seq",
children=[ask_drink_again, fail_drink]
)
drink_wait_sel = py_trees.composites.Selector(
name="Drink Wait Sel",
children=[get_drink_seq, ask_drink_again_seq, drink_retry]
)
return py_trees.composites.Sequence(
name="Greet Person Subtree",
children=[start, find_person, greet, name_wait_sel, drink_wait_sel, completed_greet])
#children=[start, find_person, turn_off_camera, greet, name_wait_sel, drink_wait_sel, completed_greet, turn_on_camera])
def receptionist(node, config: dict, ssh) -> py_trees.behaviour.Behaviour:
behaviour = Behaviour(node)
sofa = behaviour.position_to_pose(config["sofa"])
door = behaviour.position_to_pose(config["door"])
starting_point = behaviour.position_to_pose(config["receptionist_starting_point"])
loading_tablet = TabletLoading(name="loading tablet", node=node)
set_initial_pose = SetPose(name = "Set Starting Point", pose = starting_point, ssh_client=ssh)
wait = py_trees.timers.Timer(name="Wait", duration=2)
is_button_clicked = TabletQuestion(name="Confirm continue", node=node, text="Press button to continue", options=["this is the button"])
show_speech = TabletSpeech(name="show_speech_end", node=node)
#start_move_to_door = MoveToPoint(name="Move To Door At Start", node=node, pose=door, ssh_client=ssh)
center_head = PepperCenterHead(name="Center Head", node=node)
tell_referee_to_open_door = Say(name="Tell Referee To Open Door", node=node, message="Please step through the door")
greet_person_subtree = greet_person(node=node)
tell_person_to_follow = Say(name="Tell Person To Follow", node=node, message="Please come with me")
#move_to_sofa = MoveToPoint(name="Move to Point",node=node, pose=sofa, ssh_client=ssh)
all_people_introduced = IsEveryoneIntroduced(name="All People Introduced", node=node)
get_face_pixel = GetFacePixelCoordinate(name="Get Face Pixel", node=node)
point_at_pixel = PointAt(name="Point At Pixel", key="point_at_person_goal")
# Motion Action
point_at_person = action_clients.FromBlackboard(
name="Point At Person",
action_name="robot_action",
action_type=RobotAction,
key="point_at_person_goal"
)
say_name_of_person = SayDynamic(name="Say Name Of Person", node=node)
point_quit_both_goal = PointQuit(
name="Point Quit Both Goal",
key="point_quit_both",
arm = RobotActionGoal.ARM_BOTH
)
point_quit_both_action = action_clients.FromBlackboard(
name="Point Quit Both Action",
action_name="robot_action",
action_type=RobotAction,
key="point_quit_both"
)
const_fail = py_trees.behaviours.Failure(name="Const Fail")
introduce_retry = Standby(name="Introduce Retry", node=node)
#get_couch_gap_pixel = CalculateGapPointLocation(name="Get Couch Gap Pixel", node=node)
place_person = PlacePerson(name="Place Person", node=node)
point_at_empty_seat = PointAt(name="Point At Empty Seat", key="point_at_empty_seat_goal")
# Motion Action
point_at_person = action_clients.FromBlackboard(
name="Point At Empty Seat",
action_name="robot_action",
action_type=RobotAction,
key="point_at_empty_seat_goal"
)
tell_person_to_sit = SayDynamic(name="Tell Person To Sit", node=node)
point_quit_both_goal_2 = PointQuit(
name="Point Quit Both Goal 2",
key="point_quit_both_2",
arm = RobotActionGoal.ARM_BOTH
)
point_quit_both_action_2 = action_clients.FromBlackboard(
name="Point Quit Both Action 2",
action_name="robot_action",
action_type=RobotAction,
key="point_quit_both_2"
)
#say_there_is_no_seat = Say(name="Say There Is No Seat", node=node, message="There is no seat for you")
move_to_door = MoveToPoint(name="Move To Door",node=node,pose=door, ssh_client=ssh)
"""receptionist_sel = py_trees.composites.Selector(
name="Receptionist pytree",
children=[tell_referee_to_open_door, greet_person_subtree, tell_person_to_follow, move_to_sofa, at_sofa_seq, move_to_door]
)"""
initialize_scenario = py_trees.composites.Sequence(
name="Initialize Scenario",
children=[set_initial_pose,loading_tablet, wait,is_button_clicked, show_speech]#, start_move_to_door,]
)
introduce_person = py_trees.composites.Sequence(
name=" Introduce person",
children=[get_face_pixel, point_at_pixel, point_at_person, say_name_of_person, point_quit_both_goal, point_quit_both_action, const_fail]
)
# unsuccessful_seating = py_trees.composites.Sequence(
# name="Unsuccessful Seating",
# children=[say_there_is_no_seat]
# )
timer = py_trees.timers.Timer(name="Wait", duration=5)
successful_seating = py_trees.composites.Sequence(
name="Successful Seating",
children=[place_person, point_at_empty_seat, tell_person_to_sit, point_quit_both_goal_2, point_quit_both_action_2, timer]#[get_couch_gap_pixel, point_at_empty_seat, tell_person_to_sit]
)
# empty_seat_sel = py_trees.composites.Selector(
# name="Empty Seat Sel",
# children=[successful_seating, unsuccessful_seating]
# )
intros_wait_sel = py_trees.composites.Selector(
name="Intros Wait Sel",
children=[all_people_introduced, introduce_person, introduce_retry]
)
at_sofa_seq = py_trees.composites.Sequence(
name="At Sofa Seq",
children=[intros_wait_sel, successful_seating]
)
"""
either_or = py_trees.idioms.either_or(
name="EitherOr",
conditions=[
py_trees.common.ComparisonExpression("head_touched", False, operator.eq),
py_trees.common.ComparisonExpression("head_touched", True, operator.eq)],
subtrees=[?, ?]
)"""
return py_trees.composites.Sequence(
name="receptionist tree",
children=[initialize_scenario, center_head, tell_referee_to_open_door, greet_person_subtree, tell_person_to_follow,at_sofa_seq]#, move_to_door]#, move_to_sofa, at_sofa_seq, move_to_door]
)
def create_root() -> py_trees.behaviour.Behaviour:
"""
"""
root = py_trees.composites.Sequence(
name="PointAction"
)
# Goal should be on blackboard already,
# this is only for testing purposes
# pixel = [1290, 479]
pixel = [0, 0]
prepare_point_goal = behaviours.ToBlackboard(
name="PreparePointGoal",
key="point_to_pixel",
variable=pixel
)
point_at_goal = motion_behaviours.PointAt(
name="Point At Goal",
key="point_at_goal"
)
point_at_action = action_clients.FromBlackboard(
name="PointAtAction",
action_name="robot_action",
action_type=lhw_action.RobotAction,
key="point_at_goal"
)
pause5 = py_trees.timers.Timer("Pause5", duration=5.0)
pause10 = py_trees.timers.Timer("Pause10", duration=10.0)
# Goal should be on blackboard already,
# this is only for testing purposes
point_quit_both_goal = motion_behaviours.PointQuit(
name="Point Quit Both Goal",
key="point_quit_both",
arm = motion_behaviours.RobotActionGoal.ARM_BOTH
)
point_quit_both_action = action_clients.FromBlackboard(
name="Point Quit Both Action",
action_name="robot_action",
action_type=lhw_action.RobotAction,
key="point_quit_both"
)
idle = py_trees.behaviours.Running(name="Idle")
root.add_children([prepare_point_goal, point_at_goal, point_at_action, pause5, point_quit_both_goal, point_quit_both_action, idle]) # point_quit_right_goal, point_quit_right_action, idle])
return root
def main():
"""
"""
rclpy.init(args=None)
config = json.load(open("src/lhw_intelligence/lhw_intelligence/config.json"))
pepper_ip = "192.168.0.108"#os.environ["PEPPER_IP"]
user = "nao"
pw = "nao"
ssh_client = paramiko.SSHClient()
ssh_client.set_missing_host_key_policy(paramiko.AutoAddPolicy)
print("start pytree")
try:
ssh_client.connect(hostname = pepper_ip, username = user, password = pw)
print(f"Successful Connection")
except Exception as e:
print(f"Failed to connect to {pepper_ip} vi Shh")
bullshit = py_trees.composites.Sequence(name="BS")
tree = py_trees_ros.trees.BehaviourTree(
root=bullshit,
unicode_tree_debug=True
)
root = receptionist(node=tree.node, config=config, ssh=ssh_client)
bullshit.add_child(root)
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()
\ No newline at end of file
src/lhw_intelligence/lhw_intelligence/tree_stage_two.py deleted 100644 → 0
View file @ d4b266eb
import py_trees
import py_trees_ros
import py_trees_ros.trees
import py_trees.console as console
from std_msgs.msg import Int32
import lhw_interfaces.srv as lhw_srv
import lhw_interfaces.action as lhw_action
import rclpy
import sys
from . import action_clients_stage_two, behaviours_stage_two
def create_root() -> py_trees.behaviour.Behaviour:
root = py_trees.composites.Sequence(
name="Root"
)
return root
def check(number) -> py_trees.behaviour.Behaviour:
root = py_trees.composites.Sequence(
name="Check"
)
right_answer = py_trees.composites.Selection(
name="Right Answer?"
)
scan = behaviours_stage_two.Scan(
name="Scan"
)
discover_object = behaviours_stage_two.DiscoverObject(
name="Discover Object"
)
reveal_object = behaviours_stage_two.Say(name="Reveal Object", message="Is the movie ", from_blackboard=True)
answer_from_person = behaviours_stage_two.Listen(
name="Listen",
key="object",
check_if_true=True
)
number -= 1
if (number == 0):
root = py_trees.behaviours.Success(name="Success")
else:
root.add_children([scan, discover_object, reveal_object, right_answer])
right_answer.add_children([answer_from_person, check(number)])
return root
def discover(number) -> py_trees.behaviour.Behaviour:
root = py_trees.composites.Sequence(
name="Discover"
)
ask_questions = py_trees.composites.Sequence(
name="Ask Questions"
)
check = check(5)
done = py_trees.behaviours.Running(name="Done")
release_date = behaviours_stage_two.Say(
name="Release Date",
message="What is the release date of the movie"
)
answer_release_date = behaviours_stage_two.Listen(
name="Answer Release Date",
message="Release date is ",
key="data"
)
about = behaviours_stage_two.Say(
name="About",
message="What is the movie about"
)
answer_about = behaviours_stage_two.Listen(
name="Answer About",
message="The movie is about ",
key="data"
)
actors = behaviours_stage_two.Say(
name="Actors",
message="Which actors is acting in the movie"
)
answer_actors = behaviours_stage_two.Listen(
name="Answer Actor",
message="Actors in the movie are ",
key="data"
)
misc = behaviours_stage_two.Say(
name="Misc",
message="Do you have any other information about the movie"
)
answer_misc = behaviours_stage_two.Listen(
name="Answer Misc",
message="Additional information is that ",
key="data"
)
number -= 1
if (number == 0):
root = done
else:
root.add_children([ask_questions, check, discover(number)])
ask_questions.add_children([release_date, answer_release_date, about, answer_about, actors, answer_actors, misc, answer_misc])
return root
def main():
"""
"""
rclpy.init(args=None)
root = create_root(10)
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()
src/lhw_intelligence/lhw_intelligence/trees/__init__.py 0 → 100644
View file @ 2f7d10cb
src/lhw_intelligence/lhw_intelligence/trees/scenario_sketches/greet_person.tree 0 → 100644
View file @ 2f7d10cb
->;;Root
| ?;;Find Name
| | (got name)
| | ->;;get name sequence
| | | [Ask: What is your name]
| | | [Listen for responce1]
| | | [ask: is your name *name*?]
| | | [Listen for yes/no responce1]
| ?;;find drink
| | (got drink)
| | ->;; get drink sequence
| | | [Ask: What is your favorite drink]
| | | [Listen for responce2]
| | | [ask: is your favorite drink *drink*?]
| | | [Listen for yes/no responce2]
\ No newline at end of file
src/lhw_intelligence/lhw_intelligence/trees/scenario_sketches/receptionist.tree 0 → 100644
View file @ 2f7d10cb
->;;Root
| ?;;TouchHead
| | (Is Head Touched)
| | [Wait and blink]
| ?;;FindPersonToIntroduce
| | (has person to introduce)
| | ->;;DoorGreeter
| | | [navigate door subtree]
| | | ?;;DoorChecker
| | | | (is door open)
| | | | ->;;DoorActions
| | | | | [say: open_door_promt]
| | | | | [check door]
| | | ?;;PersonChecker
| | | | (found person)
| | | | ->;;GreetActions
| | | | | [find person]
| | | | | [greet and ask subtree]
| [navigate: sofa subtree]
| ?;;IntroduceChecker
| | (all introduced)
| | ->;;IntroduceActions
| | | [Introduce person subtree]
| | | (all introduced)
| [find empty spot subtree]
| [reset conditions]
\ No newline at end of file
src/lhw_intelligence/lhw_intelligence/trees/scenario_sketches/receptionist_v2.tree 0 → 100644
View file @ 2f7d10cb
->;;Root
| ?;;TouchHead
| | (Is Head Touched)
| | [Wait and blink]
| ?;;FindPersonToIntroduce
| | (has person to introduce)
| | ->;;DoorGreeter
| | | [navigate door subtree]
| | | ?;;DoorChecker
| | | | (is door open)
| | | | ->;;DoorActions
| | | | | [say: open_door_promt]
| | | | | [check door]
| | | ?;;PersonChecker
| | | | (found person)
| | | | ->;;GreetActions
| | | | | [find person]
| | | | | [greet and ask subtree]
| [navigate: sofa subtree]
| ?;;IntroduceChecker
| | (all introduced)
| | ->;;IntroduceActions
| | | [Introduce person subtree]
| | | (all introduced)
| [find empty spot subtree]
| [reset conditions]
|
\ No newline at end of file
src/lhw_intelligence/lhw_intelligence/trees/scenario_trees/greet_person_tree.py 0 → 100644
View file @ 2f7d10cb
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 greet_person_behaviours
from lhw_intelligence.behaviours import speech_behaviours
def create_root() -> py_trees.behaviour.Behaviour:
root = py_trees.composites.Sequence(name="Root")
# -------------------------------------------
# | This is get name part |
# -------------------------------------------
find_name_selector = py_trees.composites.Selector(
name="FindNameSelector"
)
ask_for_name_sequence = py_trees.composites.Sequence(
name="AskForNameSequence"
)
ask_for_name = speech_behaviours.Say(
name="Ask For Name",
message="What is your name?"
)
answer_name = speech_behaviours.Listen(
name="Answer Name",
category="actors"
)
ask_for_confirmation_name = greet_person_behaviours.AskForConfirmation(
name="Ask For Confirmation",
variable="actors"
)
answer_confirmation_name = greet_person_behaviours.ListenConfirmation(
name="Answer confirmation",
category="actors"
)
already_have_name = greet_person_behaviours.BlackboardUpdated(
name="Already have name",
category="actors"
)
idle = greet_person_behaviours.Idle(name = "idle")
ask_for_name_sequence.add_children([
ask_for_name,
answer_name,
ask_for_confirmation_name,
answer_confirmation_name
])
find_name_selector.add_children([
already_have_name, ask_for_name_sequence
])
# -------------------------------------------
# | This is get drink part |
# -------------------------------------------
find_drink_selector = py_trees.composites.Selector(
name="FindDrinkSelector"
)
ask_for_drink_sequence = py_trees.composites.Sequence(
name="AskForDrinkSequence"
)
ask_for_drink = speech_behaviours.Say(
name="Ask For Drink",
message="What is your favorite drink?"
)
answer_drink = speech_behaviours.Listen(
name="Answer Drink",
category="drinks"
)
ask_for_confirmation_drink = greet_person_behaviours.AskForConfirmation(
name="Ask For Confirmation",
variable="drinks"
)
answer_confirmation_drink = greet_person_behaviours.ListenConfirmation(
name="Answer confirmation",
category="drinks"
)
already_have_drink = greet_person_behaviours.BlackboardUpdated(
name="Already have drink",
category="drinks"
)
ask_for_drink_sequence.add_children([
ask_for_drink,
answer_drink,
ask_for_confirmation_drink,
answer_confirmation_drink])
find_drink_selector.add_children([
already_have_drink,
ask_for_drink_sequence,
])
root.add_children([find_name_selector, find_drink_selector])
# root.add_children([find_drink_selector])
# root.add_children([find_name_selector])
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()
\ No newline at end of file
src/lhw_intelligence/lhw_intelligence/trees/scenario_trees/head_touch_tree.py 0 → 100644
View file @ 2f7d10cb
import py_trees
import py_trees_ros
import py_trees.console as console
import sys
from lhw_intelligence.behaviours.wait_for_touch_behaviour import WaitForHeadTouch
from lhw_intelligence.behaviours.guard_behaviour import GuardBehaviour
import rclpy
def create_root() -> py_trees.behaviour.Behaviour:
root = py_trees.composites.Selector(
name="Touch Head"
)
guard = GuardBehaviour(
name="Scenario Started Guard",
blackboard_key = "scenario_started"
)
wait_for_head_touch = WaitForHeadTouch(name="Wait For Head Touch")
root.add_children([guard, wait_for_head_touch])
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()
src/lhw_intelligence/lhw_intelligence/trees/scenario_trees/safety_check_tree.py 0 → 100644
View file @ 2f7d10cb
import py_trees
import py_trees_ros
import py_trees.console as console
from geometry_msgs.msg import Pose
import json
from lhw_intelligence.behaviours.tablet_behaviors import TabletShowFIALogo, TabletQuestion
from lhw_intelligence.behaviours.motion_behaviours import MoveForward, MoveToPoint, Standby
from lhw_intelligence.behaviours.speech_behaviours import Say
from lhw_intelligence.behaviours.door_open_behavior import IsDoorOpen
from lhw_intelligence.behaviours.guard_behaviour import GuardBehaviour, SetGuardBehaviour
import rclpy
import sys
def position_to_pose(self, position: dict) -> 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"]
pose.orientation.x = position["x_o"]
pose.orientation.y = position["y_o"]
pose.orientation.z = position["z_o"]
pose.orientation.w = position["w_o"]
def show_speech_tree(ssh, safety_point : Pose) -> py_trees.behaviour.Behaviour:
guard = GuardBehaviour('show_speech_guard', 'has_shown_speech')
show_speech_beginning = TabletShowFIALogo(name="show_speech_beginning")
say_door_open = Say(name="test say", message="Checking that door is open")
is_door_open = IsDoorOpen(name="Is Door Open")
move_forward = MoveForward(name="Forward", x=2.0)
move_to_safety_point = MoveToPoint(name="Move to Point",pose=safety_point, ssh_client=ssh)
wait = py_trees.timers.Timer(name="Wait", duration=2)
say_ready_for_safety_check = Say(name="safety check say", message="I am ready for the safety check. Press the button when it's done.")
is_button_clicked = TabletQuestion(name="Confirm continue", text="Press button to continue", options=["this is the button"])
button_clicked_retry = Standby(name="Head Touched Retry")
button_clicked_selector_wait = py_trees.composites.Selector(
name="Head Touched Selector Wait",
children=[is_button_clicked, button_clicked_retry]
)
show_fia_logo = TabletShowFIALogo(name="show_speech_end")
set_done = SetGuardBehaviour("Set_speech_guard", "has_shown_speech", True)
show_speech_seq = py_trees.composites.Sequenc(
name="Show speech sequence",
children=[show_speech_beginning, say_door_open, is_door_open, move_forward, move_to_safety_point, wait, say_ready_for_safety_check, button_clicked_selector_wait, show_fia_logo, set_done]
)
return py_trees.composites.Selector(
name="Show speech",
children=[guard, show_speech_seq]
)
def create_root(ssh) -> py_trees.behaviour.Behaviour:
"""
"""
with open("src/lhw_intelligence/lhw_intelligence/config.json") as file:
config = json.load(file)
starting_point = position_to_pose(config["starting_point"])
waiting_point = position_to_pose(config["waiting_point"])
safety_point = position_to_pose(config["safety_point"])
exit_pose = position_to_pose(config["exit"])
move_to_waiting_point = MoveToPoint(name="Move to Point", pose=waiting_point, ssh_client=ssh)
move_to_exit = MoveToPoint(name="Move to Point",pose=exit_pose, ssh_client=ssh)
standby = Standby(name="Standby")
speech_tree = show_speech_tree(ssh, safety_point)
safety_check_sequence = py_trees.composites.Sequence(
name="Safety Check Sequence",
children=[speech_tree, move_to_waiting_point, move_to_exit, standby]
)
return safety_check_sequence
src/lhw_intelligence/lhw_intelligence/py_tree_script/bt_to_py_tree_script.py → src/lhw_intelligence/lhw_intelligence/trees/testing/bt_to_py_tree_script.py
View file @ 2f7d10cb
......@@ -671,8 +671,8 @@ if __name__ == '__main__':
file_to_read = sys.argv[1]
output_file_name = sys.argv[2]
else:
file_to_read = 'src/lhw_intelligence/lhw_intelligence/scenario_trees/receptionist/receptionist.tree'
output_file_name = 'src/lhw_intelligence/lhw_intelligence/py_tree_script/output.py'
file_to_read = 'src/lhw_intelligence/lhw_intelligence/trees/testing/receptionist_v2.tree'
output_file_name = 'src/lhw_intelligence/lhw_intelligence/trees/testing/output.py'
inp = refactor_input(file_to_read)
tree = convert_to_tree(inp)
......
src/lhw_intelligence/lhw_intelligence/trees/testing/headTouched.py 0 → 100644
View file @ 2f7d10cb
#!/usr/bin/env python
#
# License: BSD
# https://raw.githubusercontent.com/splintered-reality/py_trees/devel/LICENSE
#
##############################################################################
# Documentation
##############################################################################
"""
.. argparse::
:module: py_trees.demos.selector
:func: command_line_argument_parser
:prog: py-trees-demo-selector
.. graphviz:: dot/demo-selector.dot
.. image:: images/selector.gif
"""
##############################################################################
# Imports
##############################################################################
import argparse
import py_trees
import sys
import time
import py_trees.console as console
##############################################################################
# Classes
##############################################################################
def description():
content = "Higher priority switching and interruption in the children of a selector.\n"
content += "\n"
content += "In this example the higher priority child is setup to fail initially,\n"
content += "falling back to the continually running second child. On the third\n"
content += "tick, the first child succeeds and cancels the hitherto running child.\n"
if py_trees.console.has_colours:
banner_line = console.green + "*" * 79 + "\n" + console.reset
s = "\n"
s += banner_line
s += console.bold_white + "Selectors".center(79) + "\n" + console.reset
s += banner_line
s += "\n"
s += content
s += "\n"
s += banner_line
else:
s = content
return s
def epilog():
if py_trees.console.has_colours:
return console.cyan + "And his noodly appendage reached forth to tickle the blessed...\n" + console.reset
else:
return None
def command_line_argument_parser():
parser = argparse.ArgumentParser(description=description(),
epilog=epilog(),
formatter_class=argparse.RawDescriptionHelpFormatter,
)
parser.add_argument('-r', '--render', action='store_true', help='render dot tree to file')
return parser
def create_root():
root = py_trees.composites.Selector("Sequende")
success_after_two = py_trees.behaviours.Count(name="founda persona",
fail_until=2,
running_until=2,
success_until=10)
always_running = py_trees.behaviours.Running(name="hola los personos")
root.add_children([success_after_two, always_running])
return root
##############################################################################
# Main
##############################################################################
def main():
"""
Entry point for the demo script.
"""
args = command_line_argument_parser().parse_args()
print(description())
py_trees.logging.level = py_trees.logging.Level.DEBUG
root = create_root()
####################
# Rendering
####################
if args.render:
py_trees.display.render_dot_tree(root)
sys.exit()
####################
# Execute
####################
root.setup_with_descendants()
for i in range(1, 11):
try:
print("\n--------- Tick {0} ---------\n".format(i))
root.tick_once()
print("\n")
print(py_trees.display.unicode_tree(root=root, show_status=True))
time.sleep(1.0)
except KeyboardInterrupt:
break
print("\n")
if __name__ == "__main__":
main()
src/lhw_intelligence/lhw_intelligence/trees/testing/output.py 0 → 100644
View file @ 2f7d10cb
import py_trees
def receptionist_v2():
root = py_trees.composites.Sequence(name="Root")
touchhead = py_trees.composites.Selector(name="TouchHead")
is_head_touched = IsHeadTouched(name="Is Head Touched", node=node)
wait_and_blink = Waitandblink(name="Wait and blink", node=node)
findpersontointroduce = py_trees.composites.Selector(name="FindPersonToIntroduce")
has_person_to_introduce = haspersontointroduce(name="has person to introduce", node=node)
doorgreeter = py_trees.composites.Sequence(name="DoorGreeter")
navigate_door_subtree = navigatedoorsubtree(name="navigate door subtree", node=node)
doorchecker = py_trees.composites.Selector(name="DoorChecker")
is_door_open = isdooropen(name="is door open", node=node)
dooractions = py_trees.composites.Sequence(name="DoorActions")
say:_open_door_promt = say:open_door_promt(name="say: open_door_promt", node=node)
check_door = checkdoor(name="check door", node=node)
personchecker = py_trees.composites.Selector(name="PersonChecker")
found_person = foundperson(name="found person", node=node)
greetactions = py_trees.composites.Sequence(name="GreetActions")
find_person = findperson(name="find person", node=node)
greet_and_ask_subtree = greetandasksubtree(name="greet and ask subtree", node=node)
navigate:_sofa_subtree = navigate:sofasubtree(name="navigate: sofa subtree", node=node)
introducechecker = py_trees.composites.Selector(name="IntroduceChecker")
all_introduced = allintroduced(name="all introduced", node=node)
introduceactions = py_trees.composites.Sequence(name="IntroduceActions")
introduce_person_subtree = Introducepersonsubtree(name="Introduce person subtree", node=node)
find_empty_spot_subtree = findemptyspotsubtree(name="find empty spot subtree", node=node)
reset_conditions = resetconditions(name="reset conditions", node=node)
dooractions.add_children([say:_open_door_promt, check_door])
doorchecker.add_children([is_door_open, dooractions])
doorgreeter.add_children([navigate_door_subtree, doorchecker, personchecker])
findpersontointroduce.add_children([has_person_to_introduce, doorgreeter])
greetactions.add_children([find_person, greet_and_ask_subtree])
introduceactions.add_children([introduce_person_subtree, all_introduced])
all_introduced = allintroduced(name="all introduced", node=node)
introducechecker.add_children([all_introduced, introduceactions])
personchecker.add_children([found_person, greetactions])
root.add_children([touchhead, findpersontointroduce, navigate:_sofa_subtree, introducechecker, find_empty_spot_subtree, reset_conditions])
touchhead.add_children([is_head_touched, wait_and_blink])
return root
\ No newline at end of file
src/lhw_intelligence/lhw_intelligence/trees/testing/receptionist_v2.tree 0 → 100644
View file @ 2f7d10cb
->;;Root
| ?;;TouchHead
| | (Is Head Touched)
| | [Wait and blink]
| ?;;FindPersonToIntroduce
| | (has person to introduce)
| | ->;;DoorGreeter
| | | [navigate door subtree]
| | | ?;;DoorChecker
| | | | (is door open)
| | | | ->;;DoorActions
| | | | | [say: open_door_promt]
| | | | | [check door]
| | | ?;;PersonChecker
| | | | (found person)
| | | | ->;;GreetActions
| | | | | [find person]
| | | | | [greet and ask subtree]
| [navigate: sofa subtree]
| ?;;IntroduceChecker
| | (all introduced)
| | ->;;IntroduceActions
| | | [Introduce person subtree]
| | | (all introduced)
| [find empty spot subtree]
| [reset conditions]
|
\ No newline at end of file
src/lhw_intelligence/package.xml
View file @ 2f7d10cb
......@@ -12,15 +12,12 @@
<test_depend>ament_pep257</test_depend>
<test_depend>python3-pytest</test_depend>
<build_depend>lhw_interfaces</build_depend>
<exec_depend>lhw_interfaces</exec_depend>
<depend>lhw_interfaces</depend>
<depend>std_msgs</depend>
<build_depend>std_msgs</build_depend>
<exec_depend>std_msgs</exec_exec>
<build_depend>rclpy</build_depend>
<build_depend>rclpy_action</build_depend>
<build_depend>rclpy_components</build_depend>
<depend>rclpy</depend>
<depend>rclpy_action</depend>
<depend>rclpy_components</depend>
<depend>py_trees</depend>
<depend>py_trees_ros</depend>
......@@ -29,4 +26,4 @@
<export>
<build_type>ament_python</build_type>
</export>
</package>
</package>
\ No newline at end of file
src/lhw_intelligence/setup.py
View file @ 2f7d10cb
......@@ -7,7 +7,12 @@ package_name = 'lhw_intelligence'
setup(
name=package_name,
version='0.0.0',
packages=[package_name],
packages=[package_name,
package_name + ".trees.scenario_trees",
package_name + ".behaviours"
],
data_files=[
('share/ament_index/resource_index/packages',
['resource/' + package_name]),
......@@ -23,15 +28,9 @@ setup(
tests_require=['pytest'],
entry_points={
'console_scripts': [
'ai_core = lhw_intelligence.ai_core:main',
'find_person = lhw_intelligence.find_person:main',
'fake_dialogflow = lhw_intelligence.fake_dialogflow:main',
'fake_gpt = lhw_intelligence.fake_gpt:main',
'transitions = lhw_intelligence.states.emotional_pepper:main',
'py_tree = lhw_intelligence.py_tree:main',
'battery_check_tree = lhw_intelligence.battery_check_tree:main',
'tree-point-action = lhw_intelligence.tree_point_action:main',
'tree-stage-two = lhw_intelligence.tree_stage_two:main'
'head_touch_tree = lhw_intelligence.trees.scenario_trees.head_touch_tree:main',
'greet-person-tree = lhw_intelligence.trees.scenario_trees.greet_person_tree:main'
],
},
)
......