diff --git a/compose_scripts/lhw_intelligence.sh b/compose_scripts/lhw_intelligence.sh
deleted file mode 100755
index 76d0ebfdbe03ee68adcc4f64b6b6c4f12f4893cf..0000000000000000000000000000000000000000
--- a/compose_scripts/lhw_intelligence.sh
+++ /dev/null
@@ -1,8 +0,0 @@
-#!/bin/bash
-
-echo "Starting Intelligence"
-
-docker-compse run lhw_intelligence zsh
-source install/setup.zsh && ros2 run lhw_intelligence py_tree
-
-echo "Status 200 - Intelligence"
\ No newline at end of file
diff --git a/compose_scripts/lhw_nlp.sh b/compose_scripts/lhw_nlp.sh
deleted file mode 100755
index 9b0f5c3bff732d662c9238f6d33390476111d0c1..0000000000000000000000000000000000000000
--- a/compose_scripts/lhw_nlp.sh
+++ /dev/null
@@ -1,8 +0,0 @@
-#!/bin/bash
-
-echo "Starting dialogflow"
-
-docker-compse run lhw_nlp zsh
-source install/setup.zsh && ros2 run lhw_nlp dialogflow
-
-echo "Status 200 - dialogflow"
\ No newline at end of file
diff --git a/compose_scripts/lhw_qi.sh b/compose_scripts/lhw_qi.sh
deleted file mode 100755
index 2c65165a118d5fae77b918b9d97ae7c5b2aa1f22..0000000000000000000000000000000000000000
--- a/compose_scripts/lhw_qi.sh
+++ /dev/null
@@ -1,15 +0,0 @@
-#!/bin/bash
-
-echo "Starting qi"
-
-docker-compose run lhw_qi zsh
-
-colcon build --symlink-install && source $SOURCE_FILE && echo "Motion" && ros2 run lhw_qi motion -i $PEPPER_IP & \
-echo "Animated speech" && ros2 run lhw_qi animated_speech -i $PEPPER_IP & \
-echo "Microphone" && ros2 run lhw_qi microphone -i $PEPPER_IP & \
-echo "Camera" && ros2 run lhw_qi camera -i $PEPPER_IP & \
-echo "Tablet" && ros2 run lhw_qi tablet -i $PEPPER_IP & \
-echo "Depth laser" && ros2 launch lhw_qi depth_laser.launch.py & \
-echo "Simple listener" && ros2 run lhw_qi speech_recognition -i $PEPPER_IP
-
-echo "Status 200 - qi"
\ No newline at end of file
diff --git a/compose_scripts/lhw_ros1_bridge.sh b/compose_scripts/lhw_ros1_bridge.sh
deleted file mode 100644
index a4b1ee5a41d8768525ad0346cd363e3d29651d7c..0000000000000000000000000000000000000000
--- a/compose_scripts/lhw_ros1_bridge.sh
+++ /dev/null
@@ -1,9 +0,0 @@
-#!/bin/bash
-
-echo "Starting ros1 bridge"
-
-docker-compse up lhw_ros1_bridge
-
-source /opt/ros/noetic/setup.zsh && source /opt/ros/foxy/setup.zsh && source /ros1_bridge_ws/install/setup.zsh && sleep 5 && ros2 run ros1_bridge parameter_bridge
-
-echo "Status 200 - ros1 bridge"
diff --git a/compose_scripts/lhw_stop.sh b/compose_scripts/lhw_stop.sh
deleted file mode 100644
index 0f3ca9bb2ba57e93c9bddd9a1f15f086d7b2df1e..0000000000000000000000000000000000000000
--- a/compose_scripts/lhw_stop.sh
+++ /dev/null
@@ -1,3 +0,0 @@
-#!/bin/bash
-
-docker-compose stop
diff --git a/compose_scripts/lhw_tablet.sh b/compose_scripts/lhw_tablet.sh
deleted file mode 100755
index c7192b4dcc28987d24d6e3ee622cb8803ef8e0b8..0000000000000000000000000000000000000000
--- a/compose_scripts/lhw_tablet.sh
+++ /dev/null
@@ -1,9 +0,0 @@
-#!/bin/bash
-
-echo "Starting tablet"
-
-docker-compose run lhw_tablet zsh
-
-source /install/setup.zsh && ros2 run lhw_tablet action_server
-
-echo "Status 200 - tablet"
\ No newline at end of file
diff --git a/compose_scripts/lhw_visage.sh b/compose_scripts/lhw_visage.sh
deleted file mode 100755
index 84a01416f0c390772d82099f4f923a341a60192a..0000000000000000000000000000000000000000
--- a/compose_scripts/lhw_visage.sh
+++ /dev/null
@@ -1,10 +0,0 @@
-#!/bin/bash
-
-echo "Starting Visage server and visage_analysis node"
-
-docker-compose run lhw_visage zsh
-
-./build/lhw_visage/visage_server & \
-ros2 run lhw_visage visage_analysis
-
-echo "Status 200 - Visage server and visage_analysis node"
\ No newline at end of file
diff --git a/compose_scripts/lhw_vision.sh b/compose_scripts/lhw_vision.sh
deleted file mode 100755
index 977c68309a6284d773a0760c7feb52453393a20f..0000000000000000000000000000000000000000
--- a/compose_scripts/lhw_vision.sh
+++ /dev/null
@@ -1,8 +0,0 @@
-#!/bin/bash
-
-echo "Starting depth_node"
-
-docker-compose run lhw_vision zsh
-ros2 run lhw_vision depth_node
-
-echo "Status 200 - depth_nodes"
\ No newline at end of file
diff --git a/compose_scripts/ssh_pepper.sh b/compose_scripts/ssh_pepper.sh
deleted file mode 100755
index e8ee94cff9239230d3011e30eab76e61894dbc53..0000000000000000000000000000000000000000
--- a/compose_scripts/ssh_pepper.sh
+++ /dev/null
@@ -1 +0,0 @@
-ssh-keyscan $PEPPER_IP > /root/.ssh/known_hosts && ssh -i /id_rsa nao@$PEPPER_IP 'qicli call ALAudioDevice.setOutputVolume 100 && qicli call ALTextToSpeech.say "Hello Axel from Docker"'
\ No newline at end of file
diff --git a/launch.py b/launch.py
new file mode 100755
index 0000000000000000000000000000000000000000..b6d2bd41aea76031df293bb3532d9d3b1743d5da
--- /dev/null
+++ b/launch.py
@@ -0,0 +1,93 @@
+#!/usr/bin/python3
+import sys
+import subprocess
+import time
+import datetime
+import json
+
+GET_PID_STR = "docker exec {} zsh -c \"ps\" | grep -v 'ros2\|zsh\|ps\|PID' | awk '{{print $1}}'"
+
+class Command:
+ def __init__(self, container, val, timeout, verbose):
+ self.container = container
+ self.val = val
+ self.timeout = timeout
+ self.verbose = verbose
+
+class Launch:
+ def __init__(self, json_preset):
+ self.containers = [container for container in json_preset["containers"]]
+ self.commands = []
+ for command in json_preset["commands"]:
+ container = command["container"]
+ val = command["command"]
+ timeout = command.get("timeout", 3)
+ verbose = command.get("verbose", False)
+ self.commands.append(Command(container, val, timeout, verbose))
+ if len(self.commands) > 0:
+ self.commands[len(self.commands) - 1].timeout = 0
+
+ def run(self):
+ created = []
+ try:
+ for container in self.containers:
+ name = container + "_launch_" + str(datetime.datetime.now()).replace(" ", "_").replace(":", ".")
+ if subprocess.run(["docker-compose run -d --name {} --rm {} zsh".format(name, container)], shell=True).returncode != 0:
+ print("Failed to start " + container + ", aborting")
+ break
+ created.append(name)
+ print("Starting: " + container)
+
+ children = []
+ if len(created) == len(self.containers):
+ for command in self.commands:
+ name = created[self.containers.index(command.container)]
+ output = None if command.verbose else subprocess.DEVNULL
+ print("Running: " + command.val)
+ children.append(subprocess.Popen(
+ ["/usr/bin/docker", "exec", name, "zsh", "-c", "source /etc/zsh/zshrc && " + command.val],
+ stdout=output,
+ stderr=output
+ ))
+ time.sleep(command.timeout)
+ for child in children:
+ child.wait()
+
+ except KeyboardInterrupt:
+ for container in reversed(created):
+ res = subprocess.run([GET_PID_STR.format(container)], shell=True, stdout = subprocess.PIPE)
+ for pid in reversed(res.stdout.decode("utf-8").split("\n")):
+ if pid == '':
+ continue
+ try:
+ subprocess.run(["docker exec {} zsh -c \"kill -2 {}\"".format(container, pid)], shell=True, timeout=5)
+ except TimeoutError:
+ pass
+ print("Exited")
+
+ for container in created:
+ subprocess.run(["docker kill {} && docker container rm {}".format(container, container)], shell=True)
+
+def main():
+ if "--help" in sys.argv:
+ print("Usage: launch.py <PRESET_NAME>")
+ print("Available presets in presets.json")
+ return
+
+ with open("presets.json", "r") as file:
+ data = file.read()
+ json_data = json.JSONDecoder().decode(data)
+
+ if len(sys.argv) < 2:
+ print("No preset name given")
+ return
+
+ name = sys.argv[1]
+ if not name in json_data:
+ print("Invalid preset name")
+ return
+ preset = Launch(json_data[name])
+ preset.run()
+
+if __name__ == "__main__":
+ main()
diff --git a/presets.json b/presets.json
new file mode 100644
index 0000000000000000000000000000000000000000..d5e1996a8560209f19856bfef70a0e802baacc28
--- /dev/null
+++ b/presets.json
@@ -0,0 +1,77 @@
+{
+ "head_touched" : {
+ "containers" : ["lhw_qi", "lhw_tablet", "lhw_intelligence"],
+ "commands" : [
+ {
+ "container" : "lhw_qi",
+ "command" : "ros2 launch naoqi_driver naoqi_driver.launch.py nao_ip:=$PEPPER_IP",
+ "timeout" : 8,
+ "verbose" : true
+ },
+ {
+ "container" : "lhw_qi",
+ "command" : "ros2 run lhw_qi tablet -i $PEPPER_IP"
+ },
+ {
+ "container" : "lhw_qi",
+ "command" : "ros2 run lhw_qi leds -i $PEPPER_IP"
+ },
+ {
+ "container" : "lhw_tablet",
+ "command" : "ros2 run lhw_tablet action_server",
+ "verbose" : false
+ },
+ {
+ "container" : "lhw_intelligence",
+ "command" : "ros2 run lhw_intelligence head_touch_tree",
+ "verbose" : true
+ }
+ ]
+ },
+ "greet_person" : {
+ "containers" : ["lhw_qi", "lhw_nlp", "lhw_intelligence"],
+ "commands" : [
+ {
+ "container" : "lhw_qi",
+ "command" : "ros2 run lhw_qi microphone -i $PEPPER_IP",
+ "verbose" : true
+ },
+ {
+ "container" : "lhw_qi",
+ "command" : "ros2 run lhw_qi animated_speech -i $PEPPER_IP",
+ "verbose" : true
+ },
+ {
+ "container" : "lhw_nlp",
+ "command" : "ros2 run lhw_nlp dialogflow",
+ "verbose" : true
+ },
+ {
+ "container" : "lhw_intelligence",
+ "command" : "ros2 run lhw_intelligence greet_person_tree",
+ "verbose" : true
+ }
+ ]
+ },
+ "introduce_person" : {
+ "containers" : ["lhw_qi", "lhw_intelligence"],
+ "commands" : [
+ {
+ "container" : "lhw_qi",
+ "command" : "ros2 launch naoqi_driver naoqi_driver.launch.py nao_ip:=$PEPPER_IP",
+ "timeout" : 8,
+ "verbose" : true
+ },
+ {
+ "container" : "lhw_qi",
+ "command" : "ros2 run lhw_qi motion -i $PEPPER_IP"
+ },
+ {
+ "container" : "lhw_intelligence",
+ "command" : "ros2 run lhw_intelligence introduce_person_tree",
+ "verbose" : true
+ }
+ ]
+ }
+}
+
diff --git a/src/lhw_intelligence/activate b/src/lhw_intelligence/activate
index e2ad4d3b52518daba0c939971155a5b8ba217a98..db966db836cc921fac837c528d40b118b83a719e 100644
--- a/src/lhw_intelligence/activate
+++ b/src/lhw_intelligence/activate
@@ -24,6 +24,4 @@ if [ $ENV_LAUNCH ]; then
echo -e "${GREEN}Intelligence started${NC}"
fi
-export AMENT_PREFIX_PATH=$AMENT_PREFIX_PATH:/workspace/liu-home-wreckers/install/lhw_intelligence
-
ldconfig
diff --git a/src/lhw_intelligence/lhw_intelligence/FredrikDemo.py b/src/lhw_intelligence/lhw_intelligence/OldCode/FredrikDemo.py
similarity index 100%
rename from src/lhw_intelligence/lhw_intelligence/FredrikDemo.py
rename to src/lhw_intelligence/lhw_intelligence/OldCode/FredrikDemo.py
diff --git a/src/lhw_intelligence/lhw_intelligence/help_functions/__init__.py b/src/lhw_intelligence/lhw_intelligence/OldCode/__init__.py
similarity index 100%
rename from src/lhw_intelligence/lhw_intelligence/help_functions/__init__.py
rename to src/lhw_intelligence/lhw_intelligence/OldCode/__init__.py
diff --git a/src/lhw_intelligence/lhw_intelligence/action_clients.py b/src/lhw_intelligence/lhw_intelligence/OldCode/action_clients.py
similarity index 100%
rename from src/lhw_intelligence/lhw_intelligence/action_clients.py
rename to src/lhw_intelligence/lhw_intelligence/OldCode/action_clients.py
diff --git a/src/lhw_intelligence/lhw_intelligence/action_clients/rotate_robot_action_client.py b/src/lhw_intelligence/lhw_intelligence/OldCode/action_clients/rotate_robot_action_client.py
similarity index 98%
rename from src/lhw_intelligence/lhw_intelligence/action_clients/rotate_robot_action_client.py
rename to src/lhw_intelligence/lhw_intelligence/OldCode/action_clients/rotate_robot_action_client.py
index 82f876ab5d0f44e1f5cf2581e5185f78b9bd757c..661d2a58ce5941ee4960e0d6cef28cfb93ecd93c 100644
--- a/src/lhw_intelligence/lhw_intelligence/action_clients/rotate_robot_action_client.py
+++ b/src/lhw_intelligence/lhw_intelligence/OldCode/action_clients/rotate_robot_action_client.py
@@ -10,7 +10,7 @@ class RotateRobotActionClient(Node):
def __init__(self):
super().__init__('turn_in_direction_action_client')
- self._action_client = ActionClient(self, RotateRobot, 'rotate_robot')
+ self._action_client = ActionClient(self, RotateRobot, 'robot_action')
def send_goal(self, angle):
diff --git a/src/lhw_intelligence/lhw_intelligence/action_clients_stage_two.py b/src/lhw_intelligence/lhw_intelligence/OldCode/action_clients_stage_two.py
similarity index 100%
rename from src/lhw_intelligence/lhw_intelligence/action_clients_stage_two.py
rename to src/lhw_intelligence/lhw_intelligence/OldCode/action_clients_stage_two.py
diff --git a/src/lhw_intelligence/lhw_intelligence/action_servers/rotate_robot_action_server.py b/src/lhw_intelligence/lhw_intelligence/OldCode/action_servers/rotate_robot_action_server.py
similarity index 100%
rename from src/lhw_intelligence/lhw_intelligence/action_servers/rotate_robot_action_server.py
rename to src/lhw_intelligence/lhw_intelligence/OldCode/action_servers/rotate_robot_action_server.py
diff --git a/src/lhw_intelligence/lhw_intelligence/actions.py b/src/lhw_intelligence/lhw_intelligence/OldCode/actions.py
similarity index 100%
rename from src/lhw_intelligence/lhw_intelligence/actions.py
rename to src/lhw_intelligence/lhw_intelligence/OldCode/actions.py
diff --git a/src/lhw_intelligence/lhw_intelligence/ai_core.py b/src/lhw_intelligence/lhw_intelligence/OldCode/ai_core.py
similarity index 100%
rename from src/lhw_intelligence/lhw_intelligence/ai_core.py
rename to src/lhw_intelligence/lhw_intelligence/OldCode/ai_core.py
diff --git a/src/lhw_intelligence/lhw_intelligence/behaviour.py b/src/lhw_intelligence/lhw_intelligence/OldCode/behaviour.py
similarity index 100%
rename from src/lhw_intelligence/lhw_intelligence/behaviour.py
rename to src/lhw_intelligence/lhw_intelligence/OldCode/behaviour.py
diff --git a/src/lhw_intelligence/lhw_intelligence/behaviour_copy.py b/src/lhw_intelligence/lhw_intelligence/OldCode/behaviour_copy.py
similarity index 99%
rename from src/lhw_intelligence/lhw_intelligence/behaviour_copy.py
rename to src/lhw_intelligence/lhw_intelligence/OldCode/behaviour_copy.py
index 1a5632d9f2b13b88c524e54e275ce42c5b9f310e..90031013591688cd1c532088e10c81d1160e8075 100644
--- a/src/lhw_intelligence/lhw_intelligence/behaviour_copy.py
+++ b/src/lhw_intelligence/lhw_intelligence/OldCode/behaviour_copy.py
@@ -13,7 +13,7 @@ class Behaviour:
"""
def __init__(self, node):
self.node = node
- #self.start_time = self.node.get_clock().now()
+ #self.start_time self.node.get_clock().now()
#self.behaviour_timeout_in_sec = args.behaviour_timeout_in_sec
def __del__(self):
diff --git a/src/lhw_intelligence/lhw_intelligence/behaviours.py b/src/lhw_intelligence/lhw_intelligence/OldCode/behaviours.py
similarity index 100%
rename from src/lhw_intelligence/lhw_intelligence/behaviours.py
rename to src/lhw_intelligence/lhw_intelligence/OldCode/behaviours.py
diff --git a/src/lhw_intelligence/lhw_intelligence/behaviours/behaviour.py b/src/lhw_intelligence/lhw_intelligence/OldCode/behaviours/behaviour.py
similarity index 100%
rename from src/lhw_intelligence/lhw_intelligence/behaviours/behaviour.py
rename to src/lhw_intelligence/lhw_intelligence/OldCode/behaviours/behaviour.py
diff --git a/src/lhw_intelligence/lhw_intelligence/behaviours/categorize.py b/src/lhw_intelligence/lhw_intelligence/OldCode/behaviours/categorize.py
similarity index 100%
rename from src/lhw_intelligence/lhw_intelligence/behaviours/categorize.py
rename to src/lhw_intelligence/lhw_intelligence/OldCode/behaviours/categorize.py
diff --git a/src/lhw_intelligence/lhw_intelligence/behaviours/explore_room.py b/src/lhw_intelligence/lhw_intelligence/OldCode/behaviours/explore_room.py
similarity index 100%
rename from src/lhw_intelligence/lhw_intelligence/behaviours/explore_room.py
rename to src/lhw_intelligence/lhw_intelligence/OldCode/behaviours/explore_room.py
diff --git a/src/lhw_intelligence/lhw_intelligence/behaviours/find_person.py b/src/lhw_intelligence/lhw_intelligence/OldCode/behaviours/find_person.py
similarity index 100%
rename from src/lhw_intelligence/lhw_intelligence/behaviours/find_person.py
rename to src/lhw_intelligence/lhw_intelligence/OldCode/behaviours/find_person.py
diff --git a/src/lhw_intelligence/lhw_intelligence/behaviours/follow_person.py b/src/lhw_intelligence/lhw_intelligence/OldCode/behaviours/follow_person.py
similarity index 100%
rename from src/lhw_intelligence/lhw_intelligence/behaviours/follow_person.py
rename to src/lhw_intelligence/lhw_intelligence/OldCode/behaviours/follow_person.py
diff --git a/src/lhw_intelligence/lhw_intelligence/behaviours/general_state.py b/src/lhw_intelligence/lhw_intelligence/OldCode/behaviours/general_state.py
similarity index 100%
rename from src/lhw_intelligence/lhw_intelligence/behaviours/general_state.py
rename to src/lhw_intelligence/lhw_intelligence/OldCode/behaviours/general_state.py
diff --git a/src/lhw_intelligence/lhw_intelligence/behaviours/go_to_entity.py b/src/lhw_intelligence/lhw_intelligence/OldCode/behaviours/go_to_entity.py
similarity index 100%
rename from src/lhw_intelligence/lhw_intelligence/behaviours/go_to_entity.py
rename to src/lhw_intelligence/lhw_intelligence/OldCode/behaviours/go_to_entity.py
diff --git a/src/lhw_intelligence/lhw_intelligence/behaviours/have_a_conversation.py b/src/lhw_intelligence/lhw_intelligence/OldCode/behaviours/have_a_conversation.py
similarity index 100%
rename from src/lhw_intelligence/lhw_intelligence/behaviours/have_a_conversation.py
rename to src/lhw_intelligence/lhw_intelligence/OldCode/behaviours/have_a_conversation.py
diff --git a/src/lhw_intelligence/lhw_intelligence/behaviours/rotate_behaviour/test.tree b/src/lhw_intelligence/lhw_intelligence/OldCode/behaviours/rotate_behaviour/test.tree
similarity index 100%
rename from src/lhw_intelligence/lhw_intelligence/behaviours/rotate_behaviour/test.tree
rename to src/lhw_intelligence/lhw_intelligence/OldCode/behaviours/rotate_behaviour/test.tree
diff --git a/src/lhw_intelligence/lhw_intelligence/behaviours/speak_core.py b/src/lhw_intelligence/lhw_intelligence/OldCode/behaviours/speak_core.py
similarity index 100%
rename from src/lhw_intelligence/lhw_intelligence/behaviours/speak_core.py
rename to src/lhw_intelligence/lhw_intelligence/OldCode/behaviours/speak_core.py
diff --git a/src/lhw_intelligence/lhw_intelligence/behaviours_stage_two.py b/src/lhw_intelligence/lhw_intelligence/OldCode/behaviours_stage_two.py
similarity index 94%
rename from src/lhw_intelligence/lhw_intelligence/behaviours_stage_two.py
rename to src/lhw_intelligence/lhw_intelligence/OldCode/behaviours_stage_two.py
index 04e5580f84f24cd684e344a2e4967476c5a186a9..48765f43196412947401a7615e552f9209a63e75 100644
--- a/src/lhw_intelligence/lhw_intelligence/behaviours_stage_two.py
+++ b/src/lhw_intelligence/lhw_intelligence/OldCode/behaviours_stage_two.py
@@ -381,8 +381,6 @@ class DiscoverObject(py_trees.behaviour.Behaviour):
):
super(DiscoverObject, self).__init__(name=name)
self.blackboard = self.attach_blackboard_client(name=self.name)
- self.blackboard.register_key(key="colors", access=py_trees.common.Access.READ)
- self.blackboard.register_key(key="text", access=py_trees.common.Access.READ)
self.blackboard.register_key(key="data", access=py_trees.common.Access.READ)
self.blackboard.register_key(key="object", access=py_trees.common.Access.WRITE)
@@ -428,7 +426,7 @@ class DiscoverObject(py_trees.behaviour.Behaviour):
def _send_discover_object_request(self):
self.node.get_logger().info("added goal to blackboard")
request = lhw_srvs.DiscoverObject.Request()
- request.data = self.blackboard.colors + ". " + self.blackboard.text + ". " + self.blackboard.data
+ request.data = self.blackboard.data
self.discover_object_future = self.discover_object_client.call_async(request)
def _process_discover_object_response(self):
@@ -448,8 +446,7 @@ class Say(py_trees.behaviour.Behaviour):
self,
name,
message: str,
- from_blackboard: bool,
- check_if_True: bool
+ from_blackboard: bool
) -> None:
super(Say, self).__init__(name)
self.message = message
@@ -469,10 +466,11 @@ class Say(py_trees.behaviour.Behaviour):
error_message = "didn't find 'node' in setup's kwargs [{}]".format(self.qualified_name)
raise KeyError(error_message) from e # 'direct cause' traceability
- self.say_pub = self.node.say_publisher
+ self.say_pub = self.node.create_publisher(std_msgs.msg.String, 'say', 100)
+
def initialise(self):
- msg = std_msgs.String()
+ msg = std_msgs.msg.String()
if (self.from_blackboard):
msg.data = self.message + self.blackboard.object
else:
@@ -481,25 +479,38 @@ class Say(py_trees.behaviour.Behaviour):
self.blackboard.is_talking = True
def update(self):
- if self.blackboard.is_talking:
- return py_trees.common.Status.RUNNING
return py_trees.common.Status.SUCCESS
class Listen(py_trees.behaviour.Behaviour):
- def __init__(self, name, key: str) -> None:
- super(Listen, self).__init__(name)
+ def __init__(self, name, key: str, message: str, check_if_true: bool) -> None:
+ super(Listen, self).__init__(name=name)
self.key = key
+ self.message = message
+ self.check_if_true = check_if_true
self.blackboard = py_trees.blackboard.Client(name=self.name)
+ self.blackboard.register_key(
+ key="answer",
+ access=py_trees.common.Access.WRITE,
+ remap_to=py_trees.blackboard.Blackboard.absolute_name("/", key)
+ )
self.blackboard.register_key(
key="answer",
access=py_trees.common.Access.READ,
remap_to=py_trees.blackboard.Blackboard.absolute_name("/", key)
)
self.blackboard.register_key(
- name="object",
+ key="object",
+ access=py_trees.common.Access.READ
+ )
+ self.blackboard.register_key(
+ key="finished",
access=py_trees.common.Access.READ
)
+ self.blackboard.register_key(
+ key="finished",
+ access=py_trees.common.Access.WRITE
+ )
def setup(self, **kwargs):
self.logger.debug("%s.setup()" % self.__class__.__name__)
@@ -519,12 +530,16 @@ class Listen(py_trees.behaviour.Behaviour):
)
self.finished = False
self.answer = None
+ self.blackboard.answer = ""
+ self.blackboard.finished = False
def update(self):
- if self.finished:
+ if self.blackboard.finished:
+ self.blackboard.finished = False
return py_trees.common.Status.SUCCESS
if (self.check_if_true):
if (self.blackboard.object in self.answer):
+ self.blackboard.finished = False
return py_trees.common.Status.SUCCESS
else:
return py_trees.common.Status.FAILURE
@@ -532,5 +547,5 @@ class Listen(py_trees.behaviour.Behaviour):
def answer_callback(self, msg):
self.answer = msg.recognized_text
- self.blackboard.answer += ". " + msg.recognized_text
- self.finished = True
+ self.blackboard.answer += ". " + self.message + msg.recognized_text
+ self.blackboard.finished = True
diff --git a/src/lhw_intelligence/lhw_intelligence/behaviours_tree/behaviour.py b/src/lhw_intelligence/lhw_intelligence/OldCode/behaviours_tree/behaviour.py
similarity index 100%
rename from src/lhw_intelligence/lhw_intelligence/behaviours_tree/behaviour.py
rename to src/lhw_intelligence/lhw_intelligence/OldCode/behaviours_tree/behaviour.py
diff --git a/src/lhw_intelligence/lhw_intelligence/behaviours_tree/py-trees-demo-behaviour-lifecycle.py b/src/lhw_intelligence/lhw_intelligence/OldCode/behaviours_tree/py-trees-demo-behaviour-lifecycle.py
similarity index 100%
rename from src/lhw_intelligence/lhw_intelligence/behaviours_tree/py-trees-demo-behaviour-lifecycle.py
rename to src/lhw_intelligence/lhw_intelligence/OldCode/behaviours_tree/py-trees-demo-behaviour-lifecycle.py
diff --git a/src/lhw_intelligence/lhw_intelligence/behaviours_tree/safety_check_bh.py b/src/lhw_intelligence/lhw_intelligence/OldCode/behaviours_tree/safety_check_bh.py
similarity index 100%
rename from src/lhw_intelligence/lhw_intelligence/behaviours_tree/safety_check_bh.py
rename to src/lhw_intelligence/lhw_intelligence/OldCode/behaviours_tree/safety_check_bh.py
diff --git a/src/lhw_intelligence/lhw_intelligence/example_py_tree_behaviour.py b/src/lhw_intelligence/lhw_intelligence/OldCode/example_py_tree_behaviour.py
similarity index 100%
rename from src/lhw_intelligence/lhw_intelligence/example_py_tree_behaviour.py
rename to src/lhw_intelligence/lhw_intelligence/OldCode/example_py_tree_behaviour.py
diff --git a/src/lhw_intelligence/lhw_intelligence/fake_dialogflow.py b/src/lhw_intelligence/lhw_intelligence/OldCode/fake_dialogflow.py
similarity index 100%
rename from src/lhw_intelligence/lhw_intelligence/fake_dialogflow.py
rename to src/lhw_intelligence/lhw_intelligence/OldCode/fake_dialogflow.py
diff --git a/src/lhw_intelligence/lhw_intelligence/fake_gpt.py b/src/lhw_intelligence/lhw_intelligence/OldCode/fake_gpt.py
similarity index 100%
rename from src/lhw_intelligence/lhw_intelligence/fake_gpt.py
rename to src/lhw_intelligence/lhw_intelligence/OldCode/fake_gpt.py
diff --git a/src/lhw_intelligence/lhw_intelligence/help_functions/DataBase.py b/src/lhw_intelligence/lhw_intelligence/OldCode/help_functions/DataBase.py
similarity index 100%
rename from src/lhw_intelligence/lhw_intelligence/help_functions/DataBase.py
rename to src/lhw_intelligence/lhw_intelligence/OldCode/help_functions/DataBase.py
diff --git a/src/lhw_intelligence/lhw_intelligence/help_functions/Map.py b/src/lhw_intelligence/lhw_intelligence/OldCode/help_functions/Map.py
similarity index 100%
rename from src/lhw_intelligence/lhw_intelligence/help_functions/Map.py
rename to src/lhw_intelligence/lhw_intelligence/OldCode/help_functions/Map.py
diff --git a/src/lhw_intelligence/lhw_intelligence/OldCode/help_functions/__init__.py b/src/lhw_intelligence/lhw_intelligence/OldCode/help_functions/__init__.py
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/src/lhw_intelligence/lhw_intelligence/maps/simple_map.png b/src/lhw_intelligence/lhw_intelligence/OldCode/maps/simple_map.png
similarity index 100%
rename from src/lhw_intelligence/lhw_intelligence/maps/simple_map.png
rename to src/lhw_intelligence/lhw_intelligence/OldCode/maps/simple_map.png
diff --git a/src/lhw_intelligence/lhw_intelligence/maps/simple_map_rooms.png b/src/lhw_intelligence/lhw_intelligence/OldCode/maps/simple_map_rooms.png
similarity index 100%
rename from src/lhw_intelligence/lhw_intelligence/maps/simple_map_rooms.png
rename to src/lhw_intelligence/lhw_intelligence/OldCode/maps/simple_map_rooms.png
diff --git a/src/lhw_intelligence/lhw_intelligence/maps/simple_map_with_goals.png b/src/lhw_intelligence/lhw_intelligence/OldCode/maps/simple_map_with_goals.png
similarity index 100%
rename from src/lhw_intelligence/lhw_intelligence/maps/simple_map_with_goals.png
rename to src/lhw_intelligence/lhw_intelligence/OldCode/maps/simple_map_with_goals.png
diff --git a/src/lhw_intelligence/lhw_intelligence/motion_behaviours.py b/src/lhw_intelligence/lhw_intelligence/OldCode/motion_behaviours.py
similarity index 100%
rename from src/lhw_intelligence/lhw_intelligence/motion_behaviours.py
rename to src/lhw_intelligence/lhw_intelligence/OldCode/motion_behaviours.py
diff --git a/src/lhw_intelligence/lhw_intelligence/movies.json b/src/lhw_intelligence/lhw_intelligence/OldCode/movies.json
similarity index 100%
rename from src/lhw_intelligence/lhw_intelligence/movies.json
rename to src/lhw_intelligence/lhw_intelligence/OldCode/movies.json
diff --git a/src/lhw_intelligence/lhw_intelligence/OldCode/new_behaviours/action_client.py b/src/lhw_intelligence/lhw_intelligence/OldCode/new_behaviours/action_client.py
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/src/lhw_intelligence/lhw_intelligence/OldCode/new_behaviours/service_client.py b/src/lhw_intelligence/lhw_intelligence/OldCode/new_behaviours/service_client.py
new file mode 100644
index 0000000000000000000000000000000000000000..dda5f730b3a30219258ccc817945aaa45d04daa0
--- /dev/null
+++ b/src/lhw_intelligence/lhw_intelligence/OldCode/new_behaviours/service_client.py
@@ -0,0 +1,165 @@
+
+
+
+
+
+##############################################################################
+# Imports
+##############################################################################
+
+from os import access
+import typing
+import py_trees
+import lhw_interfaces.msg as lhw_msgs
+import lhw_interfaces.srv as lhw_srvs
+from py_trees_ros import exceptions
+import rclpy
+
+##############################################################################
+# Behaviours
+##############################################################################
+
+class ServiceFromBlackboard(py_trees.behaviour.Behaviour):
+ """
+ """
+ def __init__(
+ self,
+ name: str,
+ key: str,
+ service_name: str,
+ service_type: typing.Any,
+ generate_feedback_message: typing.Callable[[typing.Any], str]=None,
+ wait_for_service_timeout_sec: float=-3.0
+ ):
+ super(ServiceFromBlackboard, self).__init__(name=name)
+ self.service_name = service_name
+ self.service_type = service_type
+ self.wait_for_service_timeout_sec = wait_for_service_timeout_sec
+ self.generate_feedback_message = generate_feedback_message
+
+ self.blackboard = self.attach_blackboard_client(name=self.name)
+ self.blackboard.register_key(
+ key="request",
+ access=py_trees.common.Access.READ,
+ remap_to=py_trees.blackboard.Blackboard.absolute_name("/%s_request", key)
+ )
+ self.blackboard.register_key(
+ key="result",
+ access=py_trees.common.Access.WRITE,
+ remap_to=py_trees.blackboard.Blackboard.absolute_name("/%s_result", key)
+ )
+
+
+ def setup(self, **kwargs):
+ """
+ Setup the service client.
+ Args:
+ **kwargs (:obj:`dict`): distribute arguments to this
+ behaviour and in turn, all of it's children
+ Raises:
+ :class:`KeyError`: if a ros2 node isn't passed under the key 'node' in kwargs
+ :class:`~py_trees_ros.exceptions.TimedOutError`: if the action server could not be found
+ """
+ self.logger.debug("{}.setup()".format(self.qualified_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.service_client = self.node.create_client(self.service_type, self.service_name)
+ result = None
+ if self.wait_for_service_timeout_sec > 0.0:
+ result = self.service_client.wait_for_server(timeout_sec=self.wait_for_service_timeout_sec)
+ else:
+ iterations = 0
+ period_sec = -1.0*self.wait_for_server_timeout_sec
+ while not result:
+ iterations += 1
+ result = self.service_client.wait_for_server(timeout_sec=period_sec)
+ if not result:
+ self.node.get_logger().warning(
+ "waiting for action server ... [{}s][{}][{}]".format(
+ iterations * period_sec,
+ self.action_name,
+ self.qualified_name
+ )
+ )
+ if not result:
+ self.feedback_message = "timed out waiting for the server [{}]".format(self.service_name)
+ self.node.get_logger().error("{}[{}]".format(self.feedback_message, self.qualified_name))
+ raise exceptions.TimedOutError(self.feedback_message)
+ else:
+ self.feedback_message = "... connected to action server [{}]".format(self.service_name)
+ self.node.get_logger().info("{}[{}]".format(self.feedback_message, self.qualified_name))
+
+ def initialise(self):
+ """
+ Reset the internal variables and kick off a new request.
+ """
+ self.logger.debug("%s.initialise()" % self.__class__.__name__)
+
+ # initialise some temporary variables
+ self.request_handle = None
+ self.get_result_future = None
+
+ self.result_message = None
+
+ self.blackboard.feedback = None
+ self.blackboard.result = None
+
+ try:
+ self._send_request(self.blackboard.request)
+ self.feedback_message = "sent request"
+ except KeyError:
+ pass # self.send_request_future will be None, check on that
+
+ def update(self):
+ """
+ Check only to see whether the underlying service server has
+ succeeded, is running, or has cancelled/aborted for som reason and
+ map these to the usual behaviour return states.
+ Returns:
+ :class:`py_trees.common.Status`
+ """
+ self.logger.debug("{}.update()".format(self.qualified_name))
+
+ if self.get_result_future is None:
+ self.feedback_message = "request failed :[ [{}]\n{!r}".format(
+ self.qualified_name,
+ self.get_result_future.exception()
+ )
+ self.node.get_logger().debug('... {}'.format(self.feedback_message))
+ return py_trees.common.Status.FAILURE
+ self.request_handle = self.get_result_future.result()
+ if not self.request_handle.accepted:
+ self.feedback_message = "request rejected :( [{}]".format(self.qualified_name)
+ self.node.get_logger().debug('... {}'.format(self.feedback_message))
+ return py_trees.common.Status.FAILURE
+ # CONTINUE HERE!
+
+
+
+
+ def _send_request(self, request: typing.Any):
+ """
+ Send the request, get a future back and start lining up the
+ chain of callbacks that will lead to a result.
+ """
+ self.feedback_message = "sending goal ..."
+ self.node.get_logger().debug("{} [{}]".format(
+ self.feedback_message,
+ self.qualified_name
+ ))
+ self.get_result_future = self.service_client.call_async(request)
+ self.get_result_future.add_done_callback(self._get_result_callback)
+
+ def _get_result_callback(self, future: rclpy.task.Future):
+ """
+ Immediate callback for the result, saves data into local variables so
+ the update method can react accordingly.
+ Args:
+ future: incoming request result delivered from the service server
+ """
+ self.blackboard.result = future.result()
+ self.result_message = future.result()
\ No newline at end of file
diff --git a/src/lhw_intelligence/lhw_intelligence/py_tree.py b/src/lhw_intelligence/lhw_intelligence/OldCode/py_tree.py
similarity index 100%
rename from src/lhw_intelligence/lhw_intelligence/py_tree.py
rename to src/lhw_intelligence/lhw_intelligence/OldCode/py_tree.py
diff --git a/src/lhw_intelligence/lhw_intelligence/py_tree_behaviours.py b/src/lhw_intelligence/lhw_intelligence/OldCode/py_tree_behaviours.py
similarity index 100%
rename from src/lhw_intelligence/lhw_intelligence/py_tree_behaviours.py
rename to src/lhw_intelligence/lhw_intelligence/OldCode/py_tree_behaviours.py
diff --git a/src/lhw_intelligence/lhw_intelligence/py_tree_script/bigger_py_tree.tree b/src/lhw_intelligence/lhw_intelligence/OldCode/py_tree_script/bigger_py_tree.tree
similarity index 100%
rename from src/lhw_intelligence/lhw_intelligence/py_tree_script/bigger_py_tree.tree
rename to src/lhw_intelligence/lhw_intelligence/OldCode/py_tree_script/bigger_py_tree.tree
diff --git a/src/lhw_intelligence/lhw_intelligence/py_tree_script/bt_converted_output.py b/src/lhw_intelligence/lhw_intelligence/OldCode/py_tree_script/bt_converted_output.py
similarity index 100%
rename from src/lhw_intelligence/lhw_intelligence/py_tree_script/bt_converted_output.py
rename to src/lhw_intelligence/lhw_intelligence/OldCode/py_tree_script/bt_converted_output.py
diff --git a/src/lhw_intelligence/lhw_intelligence/py_tree_script/bt_to_py_tree_script.py b/src/lhw_intelligence/lhw_intelligence/OldCode/py_tree_script/bt_to_py_tree_script.py
similarity index 100%
rename from src/lhw_intelligence/lhw_intelligence/py_tree_script/bt_to_py_tree_script.py
rename to src/lhw_intelligence/lhw_intelligence/OldCode/py_tree_script/bt_to_py_tree_script.py
diff --git a/src/lhw_intelligence/lhw_intelligence/py_tree_script/example.tree b/src/lhw_intelligence/lhw_intelligence/OldCode/py_tree_script/example.tree
similarity index 100%
rename from src/lhw_intelligence/lhw_intelligence/py_tree_script/example.tree
rename to src/lhw_intelligence/lhw_intelligence/OldCode/py_tree_script/example.tree
diff --git a/src/lhw_intelligence/lhw_intelligence/py_tree_script/output.py b/src/lhw_intelligence/lhw_intelligence/OldCode/py_tree_script/output.py
similarity index 100%
rename from src/lhw_intelligence/lhw_intelligence/py_tree_script/output.py
rename to src/lhw_intelligence/lhw_intelligence/OldCode/py_tree_script/output.py
diff --git a/src/lhw_intelligence/lhw_intelligence/py_tree_script/pac_man_example.tree b/src/lhw_intelligence/lhw_intelligence/OldCode/py_tree_script/pac_man_example.tree
similarity index 100%
rename from src/lhw_intelligence/lhw_intelligence/py_tree_script/pac_man_example.tree
rename to src/lhw_intelligence/lhw_intelligence/OldCode/py_tree_script/pac_man_example.tree
diff --git a/src/lhw_intelligence/lhw_intelligence/py_tree_test_functions.py b/src/lhw_intelligence/lhw_intelligence/OldCode/py_tree_test_functions.py
similarity index 100%
rename from src/lhw_intelligence/lhw_intelligence/py_tree_test_functions.py
rename to src/lhw_intelligence/lhw_intelligence/OldCode/py_tree_test_functions.py
diff --git a/src/lhw_intelligence/lhw_intelligence/scenario_trees.py b/src/lhw_intelligence/lhw_intelligence/OldCode/scenario_trees.py
similarity index 100%
rename from src/lhw_intelligence/lhw_intelligence/scenario_trees.py
rename to src/lhw_intelligence/lhw_intelligence/OldCode/scenario_trees.py
diff --git a/src/lhw_intelligence/lhw_intelligence/scenario_trees/receptionist/greet_person.tree b/src/lhw_intelligence/lhw_intelligence/OldCode/scenario_trees/receptionist/greet_person.tree
similarity index 100%
rename from src/lhw_intelligence/lhw_intelligence/scenario_trees/receptionist/greet_person.tree
rename to src/lhw_intelligence/lhw_intelligence/OldCode/scenario_trees/receptionist/greet_person.tree
diff --git a/src/lhw_intelligence/lhw_intelligence/scenario_trees/receptionist/receptionist.tree b/src/lhw_intelligence/lhw_intelligence/OldCode/scenario_trees/receptionist/receptionist.tree
similarity index 100%
rename from src/lhw_intelligence/lhw_intelligence/scenario_trees/receptionist/receptionist.tree
rename to src/lhw_intelligence/lhw_intelligence/OldCode/scenario_trees/receptionist/receptionist.tree
diff --git a/src/lhw_intelligence/lhw_intelligence/scenario_trees/receptionist_OLD.tree b/src/lhw_intelligence/lhw_intelligence/OldCode/scenario_trees/receptionist_OLD.tree
similarity index 100%
rename from src/lhw_intelligence/lhw_intelligence/scenario_trees/receptionist_OLD.tree
rename to src/lhw_intelligence/lhw_intelligence/OldCode/scenario_trees/receptionist_OLD.tree
diff --git a/src/lhw_intelligence/lhw_intelligence/scenario_trees/root.tree b/src/lhw_intelligence/lhw_intelligence/OldCode/scenario_trees/root.tree
similarity index 100%
rename from src/lhw_intelligence/lhw_intelligence/scenario_trees/root.tree
rename to src/lhw_intelligence/lhw_intelligence/OldCode/scenario_trees/root.tree
diff --git a/src/lhw_intelligence/lhw_intelligence/scenario_trees/safety_check.tree b/src/lhw_intelligence/lhw_intelligence/OldCode/scenario_trees/safety_check.tree
similarity index 100%
rename from src/lhw_intelligence/lhw_intelligence/scenario_trees/safety_check.tree
rename to src/lhw_intelligence/lhw_intelligence/OldCode/scenario_trees/safety_check.tree
diff --git a/src/lhw_intelligence/lhw_intelligence/scenario_trees/test.tree b/src/lhw_intelligence/lhw_intelligence/OldCode/scenario_trees/test.tree
similarity index 100%
rename from src/lhw_intelligence/lhw_intelligence/scenario_trees/test.tree
rename to src/lhw_intelligence/lhw_intelligence/OldCode/scenario_trees/test.tree
diff --git a/src/lhw_intelligence/lhw_intelligence/states/emotional_pepper.py b/src/lhw_intelligence/lhw_intelligence/OldCode/states/emotional_pepper.py
similarity index 100%
rename from src/lhw_intelligence/lhw_intelligence/states/emotional_pepper.py
rename to src/lhw_intelligence/lhw_intelligence/OldCode/states/emotional_pepper.py
diff --git a/src/lhw_intelligence/lhw_intelligence/states/state.py b/src/lhw_intelligence/lhw_intelligence/OldCode/states/state.py
similarity index 100%
rename from src/lhw_intelligence/lhw_intelligence/states/state.py
rename to src/lhw_intelligence/lhw_intelligence/OldCode/states/state.py
diff --git a/src/lhw_intelligence/lhw_intelligence/tablet_behaviours.py b/src/lhw_intelligence/lhw_intelligence/OldCode/tablet_behaviours.py
similarity index 100%
rename from src/lhw_intelligence/lhw_intelligence/tablet_behaviours.py
rename to src/lhw_intelligence/lhw_intelligence/OldCode/tablet_behaviours.py
diff --git a/src/lhw_intelligence/lhw_intelligence/OldCode/templates/behaviour.py b/src/lhw_intelligence/lhw_intelligence/OldCode/templates/behaviour.py
new file mode 100644
index 0000000000000000000000000000000000000000..18fcad50f2a4795fb479844299890a615a86ff8b
--- /dev/null
+++ b/src/lhw_intelligence/lhw_intelligence/OldCode/templates/behaviour.py
@@ -0,0 +1,150 @@
+#
+# Assignee: Markus Handstedt (@marha066)
+# Issue: #158 Standardise Py-Trees
+# Branch: 158-standardise-py-trees
+#
+
+##############################################################################
+# Documentation
+##############################################################################
+
+"""
+Template behaviours for implementing new behaviours
+"""
+
+##############################################################################
+# Imports
+##############################################################################
+
+import typing
+
+from requests import request
+import py_trees
+import lhw_interfaces.msg as lhw_msgs
+import lhw_interfaces.srv as lhw_srvs
+import lhw_interfaces.action as lhw_action
+
+##############################################################################
+# Behaviours
+##############################################################################
+
+class GeneralBehaviour(py_trees.behaviour.Behaviour):
+ """
+ General behaviour template.
+ """
+ def __init__(
+ self,
+ name: str
+ ):
+ super(GeneralBehaviour, self).__init__(name=name)
+
+ def setup(self, **kwargs):
+ """
+ """
+ self.logger.debug("%s.setup()" % self.__class__.__name__)
+
+ # Node
+ 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):
+ """
+ """
+ self.logger.debug("%s.initialise()" % self.__class__.__name__)
+
+ def update(self) -> py_trees.common.Status:
+ """
+ """
+ self.logger.debug("%s.update()" % self.__class__.__name__)
+
+ def terminate(self, new_status: py_trees.common.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)
+ )
+
+
+class ServiceBehaviour(py_trees.behaviour.Behaviour):
+ """
+ Service behaviour template.
+ """
+ def __init__(
+ self,
+ name: str
+ ):
+ super(ServiceBehaviour, self).__init__(name=name)
+ self.blackboard = self.attach_blackboard_client(name=self.name)
+ self.blackboard.register_key(key="my_write_key", access=py_trees.common.Access.Write)
+ self.blackboard.register_key(key="my_read_key", access=py_trees.common.Access.READ)
+
+ def setup(self, **kwargs):
+ """
+ """
+ self.logger.debug("%s.setup()" % self.__class__.__name__)
+
+ # Node
+ 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
+
+ # Service client
+ self.service_client = self.node.create_client(lhw_srvs.TemplateService, '/template_service')
+ if not self.service_client.wait_for_service(timeout_sec=3.0):
+ raise RuntimeError("client timed out waiting for server <some_server>")
+
+ def initialise(self):
+ """
+ """
+ self.logger.debug("%s.initialise()" % self.__class__.__name__)
+ self._send_service_request()
+
+ def update(self) -> py_trees.common.Status:
+ """
+ """
+ self.logger.debug("%s.update()" % self.__class__.__name__)
+
+ # wait for server to respond
+ if self._process_service_response():
+ self.add_to_blackboard()
+ return py_trees.common.Status.SUCCESS
+ else:
+ return py_trees.common.Status.RUNNING
+
+ def terminate(self, new_status: py_trees.common.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 _send_service_request(self):
+ self.feedback_message = "sent service request"
+ self.node.get_logger().info(self.feedback_message)
+ request = lhw_srvs.TemplateService.Request()
+ request.variable = self.blackboard.my_read_key
+ self.service_future = self.service_client.call_async(request)
+
+ def _process_service_response(self) -> bool:
+ if not self.service_future.done():
+ return False
+ if self.service_future.result() is None:
+ self.feedback_message = "Service call to server <some_server> failed %r" % (self.future.exception(),)
+ self.node.get_logger().error(self.feedback_message)
+ return True
+
+ def _add_to_blackboard(self):
+ self.blackboard.my_write_key = self.service_future.result().variable
\ No newline at end of file
diff --git a/src/lhw_intelligence/lhw_intelligence/OldCode/tree_point_action.py b/src/lhw_intelligence/lhw_intelligence/OldCode/tree_point_action.py
new file mode 100644
index 0000000000000000000000000000000000000000..05cf6b0fc0345428ff39396529e5236c5e64923e
--- /dev/null
+++ b/src/lhw_intelligence/lhw_intelligence/OldCode/tree_point_action.py
@@ -0,0 +1,176 @@
+#!/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_stage_two, motion_behaviours
+
+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"
+ # )
+
+ goal = lhw_action.RobotAction.Goal()
+ goal.action.type = goal.action.POINT_AT
+ goal.action.xyz = [1.0, 0.0, 0.2]
+ goal.action.arm = goal.action.ARM_BOTH
+
+ prepare_point_goal = behaviours_stage_two.ToBlackboard(
+ name="PreparePointGoal",
+ key="point_to_goal",
+ variable=goal
+ )
+
+ point_at_action = action_clients.FromBlackboard(
+ name="PointAtAction",
+ action_name="robot_action",
+ action_type=lhw_action.RobotAction,
+ key="point_to_goal"
+ )
+
+ pause5 = py_trees.timers.Timer("Pause5", duration=5.0)
+ pause10 = py_trees.timers.Timer("Pause10", duration=10.0)
+ pause30 = py_trees.timers.Timer("Pause30", duration=30.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_action, pause10, 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 = create_root()
+ 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
diff --git a/src/lhw_intelligence/lhw_intelligence/tree_stage_two.py b/src/lhw_intelligence/lhw_intelligence/OldCode/tree_stage_two.py
similarity index 78%
rename from src/lhw_intelligence/lhw_intelligence/tree_stage_two.py
rename to src/lhw_intelligence/lhw_intelligence/OldCode/tree_stage_two.py
index 4604952369cbcb70dbe7c9e58db2879d05ec3d85..423ffdc764f1d0ae751883b381c4c46409337bb5 100644
--- a/src/lhw_intelligence/lhw_intelligence/tree_stage_two.py
+++ b/src/lhw_intelligence/lhw_intelligence/OldCode/tree_stage_two.py
@@ -16,6 +16,8 @@ def create_root() -> py_trees.behaviour.Behaviour:
name="Root"
)
+ root.add_child(discover(5))
+
return root
def check(number) -> py_trees.behaviour.Behaviour:
@@ -23,31 +25,32 @@ def check(number) -> py_trees.behaviour.Behaviour:
name="Check"
)
- right_answer = py_trees.composites.Selection(
+ right_answer = py_trees.composites.Selector(
name="Right Answer?"
)
- scan = behaviours_stage_two.Scan(
- name="Scan"
- )
+ # 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)
+ reveal_object = behaviours_stage_two.Say(name="Reveal Object", message="", from_blackboard=True)
answer_from_person = behaviours_stage_two.Listen(
name="Listen",
key="object",
+ message="",
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])
+ number -= 1
+ root.add_children([discover_object, reveal_object, right_answer])
right_answer.add_children([answer_from_person, check(number)])
return root
@@ -60,59 +63,69 @@ def discover(number) -> py_trees.behaviour.Behaviour:
name="Ask Questions"
)
- check = check(5)
+ check_ = check(1)
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"
+ message="What is the release date of the movie",
+ from_blackboard=False
)
answer_release_date = behaviours_stage_two.Listen(
name="Answer Release Date",
message="Release date is ",
+ check_if_true=False,
key="data"
)
about = behaviours_stage_two.Say(
name="About",
- message="What is the movie about"
+ message="What is the movie about",
+ from_blackboard=False
)
answer_about = behaviours_stage_two.Listen(
name="Answer About",
message="The movie is about ",
+ check_if_true=False,
key="data"
)
actors = behaviours_stage_two.Say(
name="Actors",
- message="Which actors is acting in the movie"
+ message="Which actors is acting in the movie",
+ from_blackboard=False
)
answer_actors = behaviours_stage_two.Listen(
name="Answer Actor",
message="Actors in the movie are ",
+ check_if_true=False,
key="data"
)
misc = behaviours_stage_two.Say(
name="Misc",
- message="Do you have any other information about the movie"
+ message="Do you have any other information about the movie",
+ from_blackboard=False
)
answer_misc = behaviours_stage_two.Listen(
name="Answer Misc",
message="Additional information is that ",
+ check_if_true=False,
key="data"
)
+ pause5 = py_trees.timers.Timer("Pause5", duration=5.0)
+
number -= 1
if (number == 0):
root = done
else:
- root.add_children([ask_questions, check, discover(number)])
+ root.add_children([ask_questions, check_, discover(number), pause5])
ask_questions.add_children([release_date, answer_release_date, about, answer_about, actors, answer_actors, misc, answer_misc])
return root
@@ -121,7 +134,9 @@ def main():
"""
"""
rclpy.init(args=None)
- root = create_root(10)
+
+
+ root = create_root()
tree = py_trees_ros.trees.BehaviourTree(
root=root,
unicode_tree_debug=True
diff --git a/src/lhw_intelligence/lhw_intelligence/behaviours/__init__.py b/src/lhw_intelligence/lhw_intelligence/behaviours/__init__.py
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/src/lhw_intelligence/lhw_intelligence/behaviours/actions.py b/src/lhw_intelligence/lhw_intelligence/behaviours/actions.py
new file mode 100644
index 0000000000000000000000000000000000000000..a5a07db8df3fabd022ce6ed6ba0ebed33192c1d7
--- /dev/null
+++ b/src/lhw_intelligence/lhw_intelligence/behaviours/actions.py
@@ -0,0 +1,178 @@
+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
diff --git a/src/lhw_intelligence/lhw_intelligence/behaviours/greet_person_behaviours.py b/src/lhw_intelligence/lhw_intelligence/behaviours/greet_person_behaviours.py
new file mode 100644
index 0000000000000000000000000000000000000000..4a8f30775a34cdbcc76da27181d4eebf300635d9
--- /dev/null
+++ b/src/lhw_intelligence/lhw_intelligence/behaviours/greet_person_behaviours.py
@@ -0,0 +1,357 @@
+##############################################################################
+# 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.actions as lhw_actions
+import std_msgs
+import time
+
+
+##############################################################################
+# Behaviours
+##############################################################################
+
+class BlackboardUpdated(py_trees.behaviour.Behaviour):
+ """
+ """
+ def __init__(
+ self,
+ name: str,
+ category: str
+ ):
+ super(BlackboardUpdated, self).__init__(name=name)
+ self.category = category
+ 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.
+ try:
+ self.blackboard.register_key(
+ key="recognized_people",
+ access=py_trees.common.Access.WRITE
+ )
+
+ self.blackboard.register_key(
+ key="current_person_id",
+ access=py_trees.common.Access.READ
+ )
+
+ self.blackboard.recognized_people
+ 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(
+ 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}], 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:
+ """
+ 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__)
+
+ # value = self.blackboard.recognized_people[self.blackboard.current_person_id][self.key]
+
+ # 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"]
+ except:
+ return py_trees.common.Status.FAILURE
+
+ # Return succes if value for key exists in blackboard and it is confirmed
+ if value and confirmed:
+ return py_trees.common.Status.SUCCESS
+ # Otherwise return false
+ return py_trees.common.Status.FAILURE
+
+ 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
+
+
+class ListenConfirmation(py_trees.behaviour.Behaviour):
+ """
+ """
+ def __init__(
+ self,
+ name: str,
+ category: str
+ ):
+ super(ListenConfirmation, self).__init__(name=name)
+
+ self.category = category
+ self.blackboard = py_trees.blackboard.Client(name=self.name)
+
+ self.blackboard.register_key(
+ key="recognized_people",
+ access=py_trees.common.Access.WRITE
+ )
+
+ self.blackboard.register_key(
+ key="current_person_id",
+ 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
+
+ # WRITE CLIENT SETUP CODE HERE
+
+ def initialise(self):
+ """
+ This function is first to be executed when behaviour is runned.
+ """
+ self.logger.debug("%s.initialise()" % self.__class__.__name__)
+
+ self.time_start = time.time()
+
+ self.dialog_sub = self.node.create_subscription(
+ lhw_msgs.Response,
+ 'dialogflow_response',
+ self._answer_callback,
+ 10
+ )
+
+ self.confirmed = ""
+
+ 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__)
+
+ # TODO: change to node timer
+ time_duration_sec = time.gmtime(time.time() - self.time_start).tm_sec
+
+ if self.confirmed == "True":
+ self.blackboard.recognized_people[self.blackboard.current_person_id][f"{self.category}_confirmed"] \
+ = True
+ return py_trees.common.Status.SUCCESS
+ elif self.confirmed == "False":
+ self.blackboard.recognized_people[self.blackboard.current_person_id][f"{self.category}_confirmed"] \
+ = False
+ return py_trees.common.Status.FAILURE
+
+ if time_duration_sec > 10.0: # If more than 10 sec --> fail
+ return py_trees.common.Status.FAILURE
+
+ return py_trees.common.Status.RUNNING
+
+
+ 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))
+
+ # Destroy subscription if still exists
+ try:
+ self.node.destroy_subscription(self.dialog_sub)
+ except:
+ pass
+
+
+ def _answer_callback(self, msg):
+ if "yes" in msg.recognized_text:
+ self.confirmed = "True"
+
+ elif "no" in msg.recognized_text:
+ self.confirmed = "False"
+
+ # ADD PRIVATE FUNCTIONS HERE "_function_name"
+
+class Idle(py_trees.behaviour.Behaviour):
+ """
+ """
+ def __init__(
+ self,
+ name:str
+ ):
+ super(Idle, self).__init__(name=name)
+
+
+
+ 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__)
+
+ return py_trees.common.Status.RUNNING
+
+
+class AskForConfirmation(py_trees.behaviour.Behaviour):
+ """
+ """
+ def __init__(
+ self,
+ name: str,
+ variable: str
+ ):
+ super(AskForConfirmation, self).__init__(name=name)
+ self.variable = variable
+
+ self.blackboard = py_trees.blackboard.Client(name=name)
+
+ self.blackboard.register_key(
+ key="current_person_id",
+ access=py_trees.common.Access.READ
+ )
+
+ 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.say_pub = self.node.create_publisher(std_msgs.msg.String, 'say', 100)
+
+ self.is_talking_sub = self.node.create_subscription(
+ std_msgs.msg.Bool, 'is_talking', self._is_talking_callback, 10
+ )
+
+ self.is_talking = False
+
+ def initialise(self):
+ """
+ This function is first to be executed when behaviour is runned.
+ """
+ self.logger.debug("%s.initialise()" % self.__class__.__name__)
+
+ current_person_id = self.blackboard.current_person_id
+
+ # Get information about recognized people from blackboard
+ blackboard_variable = self.blackboard.recognized_people[current_person_id][self.variable]
+
+ print(blackboard_variable)
+
+ msg = std_msgs.msg.String()
+
+ if self.variable == "actors":
+ msg.data = "Is your name " + blackboard_variable + "?"
+ elif self.variable == "drinks":
+ msg.data = "Is your favorite drink " + blackboard_variable + "?"
+
+
+ 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))
+
+ # WRITE CODE HERE
+
+ def _is_talking_callback(self, msg):
+ self.is_talking = msg.data
+
diff --git a/src/lhw_intelligence/lhw_intelligence/behaviours/guard_behaviour.py b/src/lhw_intelligence/lhw_intelligence/behaviours/guard_behaviour.py
new file mode 100644
index 0000000000000000000000000000000000000000..ce6a9f745b620c20440dcd42c507bb7d2c200313
--- /dev/null
+++ b/src/lhw_intelligence/lhw_intelligence/behaviours/guard_behaviour.py
@@ -0,0 +1,27 @@
+import py_trees
+
+class GuardBehaviour(py_trees.behaviour.Behaviour):
+ """
+ General behaviour template.
+ """
+ def __init__(
+ self,
+ name: str,
+ blackboard_key: str
+ ):
+ super(GuardBehaviour, self).__init__(name=name)
+ self.blackboard = self.attach_blackboard_client(name = self.name)
+ self.blackboard.register_key(
+ key="guard",
+ access=py_trees.common.Access.WRITE,
+ remap_to=py_trees.blackboard.Blackboard.absolute_name("/", blackboard_key)
+ )
+
+ def update(self) -> py_trees.common.Status:
+ """
+ """
+ self.logger.debug("%s.update()" % self.__class__.__name__)
+ if self.blackboard.guard:
+ return py_trees.common.Status.SUCCESS
+ else:
+ return py_trees.common.Status.FAILURE
diff --git a/src/lhw_intelligence/lhw_intelligence/behaviours/introduce_person_behaviours.py b/src/lhw_intelligence/lhw_intelligence/behaviours/introduce_person_behaviours.py
new file mode 100644
index 0000000000000000000000000000000000000000..5e8780230c4e87a4f9f9b459b48f13b3fabf0bad
--- /dev/null
+++ b/src/lhw_intelligence/lhw_intelligence/behaviours/introduce_person_behaviours.py
@@ -0,0 +1,392 @@
+##############################################################################
+# 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
+##############################################################################
+
+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
+
+
+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
+
+
+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):
+ # 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._action_client.wait_for_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()
+
+
+ def update(self) -> py_trees.common.Status:
+
+ # Done with introducing the guests
+ if self.all_introduced():
+ return py_trees.common.Status.SUCCESS
+
+ if self.state == "looking for people":
+ # Find new person
+ self.to_introduce_id = get_person_in_frame()
+
+ # Lift arm and introduce if not introduced
+ if not self.already_introduced(self.to_introduce_id):
+ self.state = "introducing"
+ 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)
+
+ 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)
+ msg.data = f"This is {person}. {person} likes to drink {drink}."
+
+ 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
diff --git a/src/lhw_intelligence/lhw_intelligence/behaviours/motion_behaviours.py b/src/lhw_intelligence/lhw_intelligence/behaviours/motion_behaviours.py
new file mode 100644
index 0000000000000000000000000000000000000000..e51660d6bcec1e1ef62f56518300f133ef74e327
--- /dev/null
+++ b/src/lhw_intelligence/lhw_intelligence/behaviours/motion_behaviours.py
@@ -0,0 +1,402 @@
+##############################################################################
+# Documentation
+##############################################################################
+
+"""
+Behaviours for basic motions
+"""
+
+##############################################################################
+# Imports
+##############################################################################
+
+import py_trees
+import time
+import lhw_interfaces.msg as lhw_msgs
+import lhw_interfaces.srv as lhw_srvs
+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, String
+# from lhw_interfaces.action import RobotAction
+
+##############################################################################
+# Behaviours
+##############################################################################
+
+class MoveForward(py_trees.behaviour.Behaviour):
+ """
+ """
+ def __init__(
+ self,
+ name: str
+ ):
+ super(MoveForward, self).__init__(name=name)
+
+ def setup(self, **kwargs):
+ """
+ """
+ self.logger.debug("%s.setup()" % self.__class__.__name__)
+
+ # ros2 node
+ 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):
+ """
+ """
+ self.logger.debug("%s.initialise()" % self.__class__.__name__)
+
+ def update(self) -> py_trees.common.Status:
+ """
+ """
+ self.logger.debug("%s.update()" % self.__class__.__name__)
+
+ # Do something
+
+ return py_trees.common.Status.SUCCESS
+
+ def terminate(self, new_status: py_trees.common.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))
+
+class MoveToPoint(py_trees.behaviour.Behaviour):
+ def __init__(self, name, pose: Pose, ssh_client) -> None:
+ super(MoveToPoint, self).__init__(name)
+ self.pose = pose
+ self.time = 0
+ 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
+
+ self.fix_head_pub = self.node.fix_head_pub
+ self.blackboard = py_trees.blackboard.Client(name="bb")
+ self.blackboard.register_key(key="move_base_status", access=py_trees.common.Access.READ)
+ self.blackboard.register_key(key="move_base_status", access=py_trees.common.Access.WRITE)
+ self.blackboard.register_key(key="updated", access=py_trees.common.Access.READ)
+ self.blackboard.register_key(key="updated", access=py_trees.common.Access.WRITE)
+
+ self.blackboard.move_base_status = "Default Status"
+
+ def initialise(self):
+ print("Initialising MoveToPoint")
+
+ msg = Bool()
+ msg.data = True
+ self.fix_head_pub.publish(msg)
+
+ #Orientation
+ goal_orientation = '{{x: {0}, y: {1}, z: {2}, w: {3}}}'
+ g_o = goal_orientation.format(self.pose.orientation.x, self.pose.orientation.y, self.pose.orientation.z, self.pose.orientation.w) # Unpack the list as laid out args
+
+ print(g_o)
+ #Position
+ goal_position = '{{x: {0}, y: {1}, z: {2}}}'
+ g_p = goal_position.format(self.pose.position.x, self.pose.position.y, self.pose.position.z)
+
+ goal_cmd = f"rostopic pub /lhw/nav/goal_server/goal geometry_msgs/PoseStamped \"{{header: {{frame_id: 'map'}}, pose: {{position: {g_p}, orientation: {g_o} }}}}\" --once"
+
+ #print(goal_cmd)
+ #print("Goal cmd: {goal_cmd}")
+ #print(goal_cmd)
+ self.ssh_client.invoke_shell()
+
+
+ stdin, stdout, stderr = self.ssh_client.exec_command("source ~/.bash_profile && " + goal_cmd)
+
+
+ def update(self):
+ print("Listening for callback...")
+
+ #self.node.log.info(str(self.blackboard.move_base_status))
+
+ if self.blackboard.move_base_status == 3:
+ self.blackboard.updated = False
+ self.blackboard.move_base_status = 0
+ return py_trees.common.Status.SUCCESS
+ if self.blackboard.move_base_status == 4:
+ return py_trees.common.Status.FAILED
+ else:
+ return py_trees.common.Status.RUNNING
+
+
+
+""" Move the robot to a specified location
+"""
+class SetPose(py_trees.behaviour.Behaviour):
+ def __init__(self, name, pose: Pose, ssh_client) -> None:
+ super(SetPose, self).__init__(name)
+ self.pose = pose
+ self.ssh_client = ssh_client
+
+
+ def initialise(self):
+ print("Initialising SetPose")
+ position = '{{x: {0}, y: {1}, z: {2}}}'
+ p = position.format(self.pose.position.x, self.pose.position.y, self.pose.position.z)
+
+ orientation = '{{x: {0}, y: {1}, z: {2}, w: {3}}}'
+
+ o = orientation.format(self.pose.orientation.x, self.pose.orientation.y, self.pose.orientation.z, self.pose.orientation.w ) # Unpack the list as laid out args
+
+ pose_cmd = f"""
+ rostopic pub /lhw/nav/initialpose geometry_msgs/PoseWithCovarianceStamped \"header:
+ frame_id: 'map'
+pose:
+ pose:
+ position: {p}
+ orientation: {o}\" --once"""
+
+ self.ssh_client.invoke_shell()
+ try:
+ print("")
+ stdin, stdout, stderr = self.ssh_client.exec_command("source ~/.bash_profile && " + pose_cmd, timeout = 1)#while not self.action_client.wait_for_server():
+ except:
+ print()
+
+ def update(self):
+ print("Setting pose...")
+
+ #Timer for next step in tree -> SetPose Doesn't need a callback (as long as stack works)
+
+
+ return py_trees.common.Status.SUCCESS
+
+""" Rotates the robot counter (?) clockwise.
+"""
+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 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
\ No newline at end of file
diff --git a/src/lhw_intelligence/lhw_intelligence/behaviours/speech_behaviours.py b/src/lhw_intelligence/lhw_intelligence/behaviours/speech_behaviours.py
new file mode 100644
index 0000000000000000000000000000000000000000..91ae7c3100d78c82adc8b3f1d981f8e1535bb7cf
--- /dev/null
+++ b/src/lhw_intelligence/lhw_intelligence/behaviours/speech_behaviours.py
@@ -0,0 +1,197 @@
+import py_trees
+import std_msgs
+import lhw_interfaces.msg as lhw_msgs
+import time
+
+""" Pepper says the message"""
+
+class Say(py_trees.behaviour.Behaviour):
+ """
+ """
+ def __init__(
+ self,
+ name: str,
+ message:str
+ ):
+ super(Say, self).__init__(name=name)
+ self.message = message
+
+ 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.say_pub = self.node.create_publisher(std_msgs.msg.String, 'say', 100)
+
+ self.is_talking_sub = self.node.create_subscription(
+ std_msgs.msg.Bool, 'is_talking', self._is_talking_callback, 10
+ )
+
+ self.is_talking = False
+
+ def initialise(self):
+ """
+ This function is first to be executed when behaviour is runned.
+ """
+ self.logger.debug("%s.initialise()" % self.__class__.__name__)
+
+ msg = std_msgs.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))
+
+ # WRITE CODE HERE
+
+ def _is_talking_callback(self, msg):
+ self.is_talking = msg.data
+
+
+class Listen(py_trees.behaviour.Behaviour):
+ """
+ """
+ def __init__(
+ self,
+ name: str,
+ category: str
+ ):
+ super(Listen, self).__init__(name=name)
+ self.category = category
+ self.blackboard = py_trees.blackboard.Client(name=self.name)
+
+ self.blackboard.register_key(
+ key="recognized_people",
+ access=py_trees.common.Access.WRITE
+ )
+
+
+ self.blackboard.register_key(
+ key="current_person_id",
+ access=py_trees.common.Access.WRITE
+ )
+
+
+
+ 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
+
+
+
+ def initialise(self):
+ """
+ This function is first to be executed when behaviour is runned.
+ """
+ self.logger.debug("%s.initialise()" % self.__class__.__name__)
+
+ self.time_start = time.time()
+
+ self.callback_executed = False
+
+ self.dialog_sub = self.node.create_subscription(
+ lhw_msgs.Response,
+ 'dialogflow_response',
+ self._answer_callback,
+ 10
+ )
+
+ 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("testing -----------------------------------------------------------")
+ self.logger.debug("%s.update()" % self.__class__.__name__)
+
+ # TODO: change to node timer
+ time_duration_sec = time.gmtime(time.time() - self.time_start).tm_sec
+ self.node.get_logger().info(str(time_duration_sec))
+
+ if self.callback_executed:
+ return py_trees.common.Status.SUCCESS
+ if time_duration_sec > 10.0: # If more than 10 sec --> fail
+ return py_trees.common.Status.FAILURE
+
+ return py_trees.common.Status.RUNNING
+
+ 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))
+
+ # Destroy subscription if still exists
+ if self.dialog_sub:
+ self.node.destroy_subscription(self.dialog_sub)
+
+ # WRITE CODE HERE
+
+ def _answer_callback(self, msg):
+
+ # self.blackboard.recognized_people = False
+ print(self.blackboard)
+
+ self.logger.debug("aa parametes")
+ print(msg.parameters)
+ # self.get_logger().info(msg.parameters)
+ for i in range(len(msg.parameters)):
+ # Check if correct category was found
+
+ self.logger.debug(msg.parameters[i].key)
+ print(msg.parameters[i].key)
+
+ if msg.parameters[i].key == self.category:
+ self.callback_executed = True
+ self.answer = msg.parameters[i].value
+
+ self.blackboard.recognized_people[self.blackboard.current_person_id][self.category] \
+ = self.answer
diff --git a/src/lhw_intelligence/lhw_intelligence/behaviours/tablet_behaviors.py b/src/lhw_intelligence/lhw_intelligence/behaviours/tablet_behaviors.py
new file mode 100644
index 0000000000000000000000000000000000000000..1f04ea99a9df038210ea3249d8aeff5014d1cb09
--- /dev/null
+++ b/src/lhw_intelligence/lhw_intelligence/behaviours/tablet_behaviors.py
@@ -0,0 +1,36 @@
+import py_trees
+from lhw_interfaces.action import TabletAction
+
+FIA_LOGO_URL = 'assets/fia_logo.png'
+
+def while_wait_for_server(behaviour:py_trees.behaviour.Behaviour, node):
+
+ while not behaviour.action_client.wait_for_server(timeout_sec=1):
+ node.log.info(f"waiting for tablet server connection")
+
+
+class TabletShowFIALogo(py_trees.behaviour.Behaviour):
+ def __init__(self, name) -> None:
+ super(TabletShowFIALogo, self).__init__(name)
+ self.action_client = self.node.tablet_action_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):
+
+ while_wait_for_server(self, self.node)
+
+ goal = TabletAction.Goal()
+ goal.type = "image"
+ goal.url = FIA_LOGO_URL
+
+ self.future = self.action_client.send_goal_async(goal)
+
+ def update(self):
+ self.node.log.info(f"UPDATE")
+ return py_trees.common.Status.SUCCESS
\ No newline at end of file
diff --git a/src/lhw_intelligence/lhw_intelligence/behaviours/template_behaviour.py b/src/lhw_intelligence/lhw_intelligence/behaviours/template_behaviour.py
new file mode 100644
index 0000000000000000000000000000000000000000..a7bafa24afdad8a1954773de8b52ebfcc0fd6436
--- /dev/null
+++ b/src/lhw_intelligence/lhw_intelligence/behaviours/template_behaviour.py
@@ -0,0 +1,81 @@
+##############################################################################
+# 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)
+
+ 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
+
+ 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:
+ """
+ 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
diff --git a/src/lhw_intelligence/lhw_intelligence/behaviours/wait_for_touch_behaviour.py b/src/lhw_intelligence/lhw_intelligence/behaviours/wait_for_touch_behaviour.py
new file mode 100644
index 0000000000000000000000000000000000000000..cc191f610b2e2beb1a37dd3b9bfb0b61bc3affb7
--- /dev/null
+++ b/src/lhw_intelligence/lhw_intelligence/behaviours/wait_for_touch_behaviour.py
@@ -0,0 +1,140 @@
+import typing
+import py_trees
+import rclpy
+from lhw_interfaces.action import TabletAction
+
+from naoqi_bridge_msgs.msg import HeadTouch
+import typing
+import std_msgs
+"""
+Behaviour to wait until head is touched, and blinking lights while waiting
+"""
+class WaitForHeadTouch(py_trees.behaviour.Behaviour):
+ """
+ """
+ def __init__(
+ self,
+ name: str
+ ):
+ super(WaitForHeadTouch, self).__init__(name=name)
+ self.blackboard = self.attach_blackboard_client(name = self.name)
+ self.blackboard.register_key(
+ key = "scenario_started",
+ access=py_trees.common.Access.WRITE
+ )
+ self.blackboard.scenario_started = False
+ self.led_is_on = False
+
+ 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.led_pub = self.node.create_publisher(std_msgs.msg.Bool, "turn_on_off", 10)
+ self.tablet_action_client = rclpy.action.ActionClient(
+ node=self.node,
+ action_type=TabletAction,
+ action_name="tablet_action_server"
+ )
+
+ if not self.tablet_action_client.wait_for_server(timeout_sec=3.0):
+ raise RuntimeError("client timed out waiting for server tablet_action_server")
+
+ self.logger.debug("Action client successfully created")
+
+
+ def initialise(self):
+ """
+ This function is first to be executed when behaviour is runned.
+ Creates a timer that toggles all LED:s every second.
+ """
+ self.logger.debug("%s.initialise()" % self.__class__.__name__)
+
+ self.timer = self.node.create_timer(1.0, self._toggle_leds_callback)
+ self.head_touch_sub = self.node.create_subscription(HeadTouch, 'head_touch', self._head_touch_callback, 10)
+
+ goal = TabletAction.Goal()
+ goal.type = "text"
+ goal.text = "Slap my head, boi"
+
+ self.send_goal_future = self.tablet_action_client.send_goal_async(
+ goal,
+ feedback_callback=None
+ )
+ self.logger.debug("Action goal sent")
+
+
+ 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.blackboard.scenario_started:
+ return py_trees.common.Status.SUCCESS
+
+ return py_trees.common.Status.RUNNING
+
+ 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))
+ self.timer.destroy()
+ self.node.destroy_subscription(self.head_touch_sub)
+ if not self.led_is_on:
+ msg = std_msgs.msg.Bool()
+ msg.data = True
+ self.led_pub.publish(msg)
+
+ goal = TabletAction.Goal()
+ goal.type = "text"
+ goal.text = "Scenario Started"
+
+ self.send_goal_future = self.tablet_action_client.send_goal_async(
+ goal,
+ feedback_callback=None
+ )
+
+
+ def shutdown(self):
+ """
+ This function is executed when the tree is destroyed. Destroy the publisher created in setup.
+ """
+ goal = TabletAction.Goal()
+ goal.type = "image"
+ goal.url = "assets/fia_logo.png"
+
+ self.send_goal_future = self.tablet_action_client.send_goal_async(
+ goal,
+ feedback_callback=None
+ )
+ self.led_pub.destroy()
+ self.tablet_action_client.destroy()
+
+
+ def _toggle_leds_callback(self):
+ msg = std_msgs.msg.Bool()
+ msg.data = not self.led_is_on
+ self.led_is_on = not self.led_is_on
+ self.led_pub.publish(msg)
+
+ def _head_touch_callback(self, msg):
+ self.blackboard.scenario_started = True
diff --git a/src/lhw_intelligence/lhw_intelligence/tree_main.py b/src/lhw_intelligence/lhw_intelligence/tree_main.py
new file mode 100644
index 0000000000000000000000000000000000000000..df63d9db6af858469db24a72522cb6cf41de9e5a
--- /dev/null
+++ b/src/lhw_intelligence/lhw_intelligence/tree_main.py
@@ -0,0 +1,65 @@
+#!/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()
diff --git a/src/lhw_intelligence/lhw_intelligence/tree_point_action.py b/src/lhw_intelligence/lhw_intelligence/tree_point_action.py
deleted file mode 100644
index c4e8524d85ff34d42e2f816db8d1564e8bb95d16..0000000000000000000000000000000000000000
--- a/src/lhw_intelligence/lhw_intelligence/tree_point_action.py
+++ /dev/null
@@ -1,379 +0,0 @@
-#!/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
diff --git a/src/lhw_intelligence/lhw_intelligence/trees/__init__.py b/src/lhw_intelligence/lhw_intelligence/trees/__init__.py
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/src/lhw_intelligence/lhw_intelligence/trees/safety_check_tree.py b/src/lhw_intelligence/lhw_intelligence/trees/safety_check_tree.py
new file mode 100644
index 0000000000000000000000000000000000000000..a192e617dae7061a1f5f43653524d6bf08bc782f
--- /dev/null
+++ b/src/lhw_intelligence/lhw_intelligence/trees/safety_check_tree.py
@@ -0,0 +1,8 @@
+
+
+
+import py_trees
+
+def create_root() -> py_trees.behaviour.Behaviour:
+ """
+ """
\ No newline at end of file
diff --git a/src/lhw_intelligence/lhw_intelligence/trees/scenario_sketches/greet_person.tree b/src/lhw_intelligence/lhw_intelligence/trees/scenario_sketches/greet_person.tree
new file mode 100644
index 0000000000000000000000000000000000000000..e00b3efa6c6fba84f0c9c73e1a37820e5eefdbb0
--- /dev/null
+++ b/src/lhw_intelligence/lhw_intelligence/trees/scenario_sketches/greet_person.tree
@@ -0,0 +1,15 @@
+->;;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
diff --git a/src/lhw_intelligence/lhw_intelligence/trees/scenario_sketches/introduce_person_tree.tree b/src/lhw_intelligence/lhw_intelligence/trees/scenario_sketches/introduce_person_tree.tree
new file mode 100644
index 0000000000000000000000000000000000000000..093b5fd6f14bf65fbbb09faf25829d3294501390
--- /dev/null
+++ b/src/lhw_intelligence/lhw_intelligence/trees/scenario_sketches/introduce_person_tree.tree
@@ -0,0 +1,18 @@
+?;;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
diff --git a/src/lhw_intelligence/lhw_intelligence/trees/scenario_sketches/receptionist.tree b/src/lhw_intelligence/lhw_intelligence/trees/scenario_sketches/receptionist.tree
new file mode 100644
index 0000000000000000000000000000000000000000..37afec979c622e0d4b5a797529a333439561e3b0
--- /dev/null
+++ b/src/lhw_intelligence/lhw_intelligence/trees/scenario_sketches/receptionist.tree
@@ -0,0 +1,26 @@
+->;;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
diff --git a/src/lhw_intelligence/lhw_intelligence/trees/scenario_sketches/receptionist_v2.tree b/src/lhw_intelligence/lhw_intelligence/trees/scenario_sketches/receptionist_v2.tree
new file mode 100644
index 0000000000000000000000000000000000000000..3c467cf830f48d32559ff9a7337a83db598acb28
--- /dev/null
+++ b/src/lhw_intelligence/lhw_intelligence/trees/scenario_sketches/receptionist_v2.tree
@@ -0,0 +1,27 @@
+->;;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
diff --git a/src/lhw_intelligence/lhw_intelligence/trees/scenario_trees/__init__.py b/src/lhw_intelligence/lhw_intelligence/trees/scenario_trees/__init__.py
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/src/lhw_intelligence/lhw_intelligence/trees/scenario_trees/greet_person_tree.py b/src/lhw_intelligence/lhw_intelligence/trees/scenario_trees/greet_person_tree.py
new file mode 100644
index 0000000000000000000000000000000000000000..457a6d8e5573c0db4c502119356ffe6521e685f7
--- /dev/null
+++ b/src/lhw_intelligence/lhw_intelligence/trees/scenario_trees/greet_person_tree.py
@@ -0,0 +1,153 @@
+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
diff --git a/src/lhw_intelligence/lhw_intelligence/trees/scenario_trees/head_touch_tree.py b/src/lhw_intelligence/lhw_intelligence/trees/scenario_trees/head_touch_tree.py
new file mode 100644
index 0000000000000000000000000000000000000000..fb50c5dcb6b29552c12348a00c085750ff78f0c3
--- /dev/null
+++ b/src/lhw_intelligence/lhw_intelligence/trees/scenario_trees/head_touch_tree.py
@@ -0,0 +1,57 @@
+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()
diff --git a/src/lhw_intelligence/lhw_intelligence/trees/scenario_trees/introduce_person_tree.py b/src/lhw_intelligence/lhw_intelligence/trees/scenario_trees/introduce_person_tree.py
new file mode 100644
index 0000000000000000000000000000000000000000..d41a6c3ea99038331b54515a6a5ca977fe24ec48
--- /dev/null
+++ b/src/lhw_intelligence/lhw_intelligence/trees/scenario_trees/introduce_person_tree.py
@@ -0,0 +1,138 @@
+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 |
+ # -------------------------------------------
+
+ 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"
+ )
+
+ # 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([
+ 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
+
+"""
diff --git a/src/lhw_intelligence/lhw_intelligence/trees/testing/bt_to_py_tree_script.py b/src/lhw_intelligence/lhw_intelligence/trees/testing/bt_to_py_tree_script.py
new file mode 100644
index 0000000000000000000000000000000000000000..39a899b9bfe1576c5cb8ca5052c4cd9ca01c2e5d
--- /dev/null
+++ b/src/lhw_intelligence/lhw_intelligence/trees/testing/bt_to_py_tree_script.py
@@ -0,0 +1,697 @@
+'''
+ Very rudimentary way of converting a .tree file to py_trees code.
+ It will not write all code for you, but it will fix the structure
+ and write all the initializations & tidy upp the adding of children to parents.
+
+ See https://github.com/jan-dolejsi/vscode-btree) for instruction on the syntax for the plugin
+
+ Creation of behaviour classes and minor code tweaking will still be needed
+
+ (<> are not included when writing, they are just used as clear dividers in this docstring)
+
+ See the example.tree file for examples of how everything should be written
+
+
+ USAGES:
+ # Disclaimer:
+ # ANY USAGE OF PY_TREES_ROS HAVE TO BE MANUALLY WRITTEN (for now)
+ # Please do not use numbers at the start of names :)
+
+ # Any <> with [Opt] inside means it is optional, though skipping it decreases the amount this script auto-completes for you
+ # Not giving a [Opt] name will cause py_trees to auto generate a name
+
+ Added syntax for .tree files to convert to py_trees code:
+ # Anything that start with <behaviour> will have it's name set as whatever is inside the behaviour clamps, unless specified otherwise
+ # i.e the behaviour "[Close Door]" will be named "Close Door" and it's variable will be "close_door"
+
+ # Any <> with [Opt] means it is optional, though it lessens the amount this script auto-completes for you
+
+ # Comments in the .tree file are allowed to be written on lines with no characters other than " " or "|" before ";;". See example.tree for clarification
+
+ ------ Composite based implementations ------
+ - Composites: <composite>;;<composite name>
+ - Optional Parallel "*" implementations:
+ - SuccessOnAll policy (default): =1;;<parallel name>
+ - SuccessOnOne policy: =2;;<parallel name>
+ - SuccessOnSelected policy: =3;;<parallel name>;;<child, child, ..., child>
+ - Idioms:
+ - <composite>;;<idiom name>;;<idiom> (Looks weird in tree preview but it was the only way to implement it)
+ ---------------------------------------------
+
+ ------ Behaviour based implementations ------
+ - Behaviour (Separate class name and variable name)
+ - <behaviour>;;<class name>
+ behaviour => variable name
+ name => class name
+ - Constant Behaviour Status:
+ - <behaviour>;;<behaviour status> (i.e. "(Constant Success);;Success" or "[Close door];;FAILURE or [Constant running];;running")
+ - Blackboard Behaviours:
+ - TODO: ADD THIS
+ - Meta:
+ <behaviour>;;Meta;;<function name [Opt]>
+ (Not giving a function name will leave it undefined and will need to be changed)
+ - Timers:
+ <behaviour>;;Timer;;<int [Opt]>
+ - Decorators:
+ Normal decorator syntax:
+ - <behaviour>;;<decorator>;;<decorator name [Opt]>
+
+ Special decorator syntax:
+ - Timeout: <behaviour>;;Timeout;;<timeout name [Opt]>;;<int [Opt]>
+
+ - Condition: <behaviour>;;Condition;;<condition name [Opt]>;;<behaviour status [Opt]>
+
+ - Inverter: !<behaviour>;;<inverter name [Opt]>
+ ---------------------------------------------
+
+ IMPLEMENTED:
+ Composites:
+ - Selector
+ - Sequence
+ - Parallel
+ - Parallel policies (policies)
+
+ Idioms:
+ - EitherOr
+ - EternalGuard
+ - OneShot
+ - PickUpWhereYouLeftOff / PUWYLO
+
+ Behaviour status:
+ - Success (constant)
+ - Running (constant)
+ - Failure (constant)
+
+ Meta:
+ - Meta (Create behaviour from function)
+
+ Timers:
+ - Timer
+
+ Decorators:
+ - Condition
+ - EternalGuard
+ - OneShot
+ - Inverter
+ - Timeout
+ - FailureIsRunning
+ - FailureIsSuccess
+ - RunningIsFailure
+ - RunningIsSuccess
+ - SuccessIsFailure
+ - SuccessIsRunning
+'''
+
+from io import TextIOWrapper
+import sys
+from typing import Iterable, List
+
+def to_variable(s: str) -> str:
+ return s.lower().strip().replace(' ', '_').replace('?','_QUESTION').replace("'", "").replace('-', '')
+
+def to_class(s: str) -> str:
+ return s.replace(' ', '').replace("'", " ").replace('?','_QUESTION').replace("'", "")
+
+def to_behaviour(s: str) -> str:
+ return s.strip().replace('!', '')[1:-1] # Remove ! from inverter and then ( ) or [ ]
+
+def get_name_from_composite(s: str) -> str:
+ return s.split(';;')[-1] # Always want the last index
+
+def get_name_from_dict_elem(d: str) -> str:
+ return get_name_from_composite(list(d.keys())[0])
+
+def dict_insert_or_append(adict: dict, key, val) -> None:
+ '''Insert a value in dict at key if one does not exist
+ Otherwise, convert value to list and append
+ '''
+ if key in adict:
+ if type(adict[key]) != list:
+ adict[key] = [adict[key]]
+ adict[key].append(val)
+ else:
+ adict[key] = val
+
+def refactor_input(file_to_read: str) -> List[dict]:
+ '''
+ Takes a file as input and returns a list of dictionaries, one dictionary for each line in the file with its contents and "level" of indentation
+ {
+ "name": Content of the line (excluding " " and "|"),
+ "value": Empty list for now, might be used later, #TODO: Decide if this is needed
+ "level": Level of indentation
+ }
+ '''
+ result = []
+ with open(file_to_read, 'r') as input:
+ for line in input:
+ curr_line = line.strip()
+ indents = 0
+ for i in range(0,len(curr_line)):
+ if curr_line[i] == '|':
+ indents += 1
+ elif curr_line[i] == ' ':
+ continue
+ elif curr_line[i] == ';': # Allows comments to be written in the .tree file
+ break
+ else:
+ result.append({'name': curr_line[i:], 'value': [], 'level': indents})
+ break
+ input.close
+ return result
+
+def convert_to_tree(ttree: dict, current_level: int =0) -> dict:
+ '''
+ Based on Tomcat's answer here https://www.py4u.net/discuss/22067
+ Returns a tree based structure of dictionaries
+ '''
+ result = {}
+ for i in range(0,len(ttree)):
+ current_dict = ttree[i]
+ try:
+ next_dict = ttree[i+1]
+ except:
+ next_dict = {'level':-1}
+
+ # Edge cases
+ if current_dict['level']>current_level:
+ continue
+ if current_dict['level']<current_level:
+ return result
+
+ # Recursion
+ # Next in line is on the same level, should add to current dict
+ if next_dict['level']==current_level:
+ dict_insert_or_append(result,current_dict['name'],current_dict['value'])
+
+ # Next in line is on a deeper level, create sub-dict and add to current dict
+ elif next_dict['level']>current_level:
+ rr = convert_to_tree(ttree[i+1:], current_level=next_dict['level'])
+ dict_insert_or_append(result,current_dict['name'],rr)
+ # Next in line is one level higher, add current dict to result and finish this recursion
+ else:
+ dict_insert_or_append(result,current_dict['name'],current_dict['value'])
+ return result
+ return result
+
+
+def write_formatted_elems_as_list(writer: TextIOWrapper, formatted_elements: Iterable) -> None:
+ '''
+ Enumerates through all elements of the iterable and writes them as a list
+ '''
+ writer.write('[')
+ for i, e in enumerate(formatted_elements):
+
+ writer.write(f'{to_variable(e)}')
+ if i+1 != len(formatted_elements):
+ writer.write(', ')
+ writer.write(']')
+
+def write_unformatted_elems_as_list(writer: TextIOWrapper, unformatted_elements: Iterable) -> None:
+ '''
+ Enumerates through all elements of the iterable, formats them and writes them as a list
+ '''
+ writer.write('[')
+ for i, e in enumerate(unformatted_elements):
+ elem = get_name_from_composite(e)
+ if elem[0] in BEHAVIOURS:
+ elem = elem[1:-1]
+ writer.write(f'{to_variable(elem)}')
+ if i+1 != len(unformatted_elements):
+ writer.write(', ')
+ writer.write(']')
+
+def write_tree(writer: TextIOWrapper, tree: list, parent: str ='') -> None:
+ '''
+ Main function performing the brute of the conversion of the .tree file
+ into py_trees code through recursion
+ '''
+
+ def write_idiom() -> None:
+ #TODO: Add functionality to specify condition behaviours
+
+ # Continue recursion so children of these idioms get initialized before they are needed in the declaration of the idiom
+ write_tree(writer, tree[elem])
+
+ if composite_extension == 'OneShot':
+ subtree_behaviour = to_variable(get_name_from_dict_elem(tree[elem]))
+ writer.write(f'{varr_name} = py_trees.idioms.oneshot(behaviour={subtree_behaviour}, name="{name}", variable_name="{varr_name}") # variable_name parameter might need to be changed\n')
+
+ elif composite_extension == 'EternalGuard':
+ writer.write(f'{varr_name} = py_trees.idioms.eternal_guard(subtree=')
+ write_unformatted_elems_as_list(writer, tree[elem])
+
+ writer.write(f', name="{name}", conditions=[ADD_CONDITION_BEHAVIOURS], blackboard_namespace=None) # Default blackboard_namespace variable\n')
+
+ elif composite_extension == 'PickUpWhereYouLeftOff' or composite_extension == 'PUWYLO':
+ writer.write(f'{varr_name} = py_trees.idioms.pick_up_where_you_left_off(behaviours=')
+ write_unformatted_elems_as_list(writer, tree[elem])
+ writer.write(', name="{name}") # Make sure behaviours are in correct order\n')
+
+ elif composite_extension == 'EitherOr':
+ writer.write(f'{varr_name} = py_trees.idioms.either_or(conditions=[ADD_CONDITION_BEHAVIOURS], subtrees=')
+ write_unformatted_elems_as_list(writer, tree[elem])
+ writer.write(f', name={varr_name}, namespace=None) # Default namespace\n')
+
+ def write_composite() -> None:
+ is_success_on_selected_policy = False
+ # Parallel
+ if c_key[0] == '=':
+ # Add different policies
+ if c_key[1] == '3': # SuccessOnSelected (Edge case)
+ # Recurse the children first since they will be needed for the 'children' parameter
+ write_tree(writer, tree[elem], varr_name)
+
+ is_success_on_selected_policy = True
+ writer.write(f'{varr_name} = py_trees.composites.Parallel(name="{name}", policy=py_trees.common.ParallelPolicy.SuccessOnSelected(children=')
+ parallel_extension = composite_extension
+ if parallel_extension is not None:
+ unformatted_children = parallel_extension.split(',')
+ formatted_children = []
+ for child in unformatted_children:
+ formatted_children.append(to_variable(child))
+ write_formatted_elems_as_list(writer, formatted_children)
+ else:
+ writer.write('[INPUT_OWN_CHILDREN_HERE]')
+
+ writer.write(', synchronize=True))\n')
+ else:
+ writer.write(f'{varr_name} = py_trees.composites.Parallel(name="{name}", ')
+ if c_key[1] == '2': # SuccessOnOne
+ writer.write('policy=py_trees.common.ParallelPolicy.SuccessOnOne)\n')
+
+ else: # SuccessOnAll (Default)
+ writer.write('policy=py_trees.common.ParallelPolicy.SuccessOnAll(synchronize=True))\n')
+ else:
+ # Selector
+ if c_key == '?':
+ writer.write(f'{varr_name} = py_trees.composites.Selector(name="{name}")\n')
+
+ # Sequence
+ elif c_key == '->':
+ writer.write(f'{varr_name} = py_trees.composites.Sequence(name="{name}")\n')
+
+ # Normally we want to continue recursion after initialization of the composite
+ if not is_success_on_selected_policy:
+ write_tree(writer, tree[elem], varr_name)
+
+ def write_behaviour() -> None:
+
+ desired_behaviour = None
+ class_name = None
+ try:
+ if key_name_list[1].lower() in BEHAVIOUR_EXTENSIONS:
+ desired_behaviour = key_name_list[1].lower().capitalize()
+
+ else:
+ # If a specific behaviour type is not specified then assume the name of the class comes after the ';;'
+ class_name = key_name_list[1]
+ except:
+ pass
+
+ varr_name = to_variable(name)
+
+ ### Extra implementations
+ if desired_behaviour is not None:
+ if desired_behaviour == 'Meta':
+ writer.write(f'{varr_name} = py_trees.meta.create_behaviour_from_function')
+ if len(key_name_list) == 3: # A desired function name has also been given
+ func_name = key_name_list[2]
+ writer.write(f'({func_name})\n')
+ else:
+ writer.write('(ADD_A_FUNCTION)\n')
+
+ elif desired_behaviour == 'Timer':
+ writer.write(f'{varr_name} = py_trees.timers.Timer(name="{name}", duration={key_name_list[2]})\n')
+
+ elif desired_behaviour == 'Subtree':
+
+ writer.write(f'{varr_name} = {varr_name.split("_subtree")[0]}({first_node})\n')
+ # Const behaviours (Success, Running, Fail)
+ else:
+ writer.write(f'{varr_name} = py_trees.behaviours.{desired_behaviour}(name="{name}")\n')
+ #TODO: Implement more kinds of behaviours
+
+ # Standard behaviour
+ else:
+ if class_name is not None:
+ behaviour = to_class(class_name)
+ else:
+ behaviour = to_class(name)
+ writer.write(f'{varr_name} = {behaviour}(name="{name}"{node})\n')
+ if parent:
+ writer.write(f'{parent}.add_child({varr_name})\n')
+
+ def write_decorator(decorator_name: str) -> None:
+
+ decorator_line = ''
+ tree_to_recurse = None
+
+ if c_key[0] == '!':
+ try:
+ decorator_name = key_name_list[1]
+ except:
+ decorator_name = None
+
+ # Remove the '!' from the name
+ tree_to_recurse = {name[1:]: tree[elem]}
+ if decorator_name is not None:
+ varr_name = to_variable(decorator_name)
+ decorator_line = f'{varr_name} = py_trees.decorators.Inverter(child={decorator_child}, name="{decorator_name}")\n'
+ else:
+ #TODO: If number of inverters cause any issues, add a counter
+ varr_name = decorator_child + '_INVERTER'
+ decorator_line = f'{varr_name} = py_trees.decorators.Inverter(child={decorator_child})\n'
+
+ elif decorator == 'Condition':
+ '''
+ 4 possible cases:
+ - <behaviour>;;Condition;;<condition name>;;<status>
+ - <behaviour>;;Condition;;<condition name>
+ - <behaviour>;;Condition;;<status>
+ - <behaviour>;;Condition
+ '''
+ # Remove the decorator from the name
+ tree_to_recurse = {name: tree[elem]}
+
+ # Check if there is a desired status
+ try:
+ status = key_name_list[3]
+ except:
+ try: # The status's already exist in BEHAVIOUR_EXTENSIONS
+ if key_name_list[2] in BEHAVIOUR_EXTENSIONS:
+ status = key_name_list[2]
+ else:
+ status = None
+ except:
+ status = None
+
+ if decorator_name is not None:
+ varr_name = to_variable(decorator_name)
+ decorator_line = f'{varr_name} = py_trees.decorators.Condition(child={decorator_child}, name="{decorator_name}"'
+ else:
+ varr_name = decorator_child + '_CONDITION'
+ decorator_line = f'{varr_name} = py_trees.decorators.Condition(child={decorator_child}'
+
+ if status is not None: # Add the desired status parameter
+ decorator_line += f', status=py_trees.common.Status.{status.upper()})\n'
+ else:
+ decorator_line += ')\n'
+
+ elif decorator == 'EternalGuard':
+ # Remove the decorator from the name
+ tree_to_recurse = {name: tree[elem]}
+ if decorator_name is not None:
+ varr_name = to_variable(decorator_name)
+ decorator_line = f'{varr_name} = py_trees.decorators.EternalGuard(child={decorator_child}, name="{decorator_name}", condition=ADD_CONDITION, blackboard_keys=[ADD_BLACKBOARD_KEYS])\n'
+ else:
+ varr_name = decorator_child + '_ETERNAL_GUARD'
+ decorator_line = f'{varr_name} = py_trees.decorators.EternalGuard(child={decorator_child}, condition=ADD_CONDITION, blackboard_keys=[ADD_BLACKBOARD_KEYS])\n'
+
+ elif decorator == 'OneShot':
+ # Remove the decorator from the name
+ tree_to_recurse = {name: tree[elem]}
+
+ if decorator_name is not None:
+ varr_name = to_variable(decorator_name)
+ decorator_line = f'{varr_name} = py_trees.decorators.OneShot(child={decorator_child}, name="{decorator_name}")\n'
+ else:
+ varr_name = decorator_child + '_ONESHOT'
+ decorator_line = f'{varr_name} = py_trees.decorator.OneShot(child={decorator_child})\n'
+
+ elif decorator == 'Timeout':
+ duration = None
+ try: # Check for the duration
+ duration = key_name_list[3]
+ except:
+ if isinstance(key_name_list[2], int):
+ duration = key_name_list[2]
+ tree_to_recurse = {name: tree[elem]}
+
+ # Write line with name if given
+ if decorator_name is not None:
+ varr_name = to_variable(decorator_name)
+ decorator_line = f'{varr_name} = py_trees.decorators.Timeout(child={decorator_child}, name="{decorator_name}"'
+ else:
+ varr_name = decorator_child + '_TIMEOUT'
+ decorator_line = f'{varr_name} = py_trees.decorator.Timeout(child={decorator_child}'
+
+ # Add duration if given
+ if decorator is not None:
+ decorator_line += f', duration={duration})\n'
+ else:
+ decorator_line += ')\n'
+ # X is Y cases
+ else:
+ # Remove the decorators from the name
+ tree_to_recurse = {name: tree[elem]}
+ if decorator_name is not None:
+ varr_name = to_variable(decorator_name)
+ decorator_line = f'{varr_name} = py_trees.decorators.{decorator}(child={decorator_child}, name="{decorator_name}")\n'
+ else:
+ varr_name = decorator_child + f'_{decorator.upper()}'
+ decorator_line = f'{varr_name} = py_trees.decorators.{decorator}(child={decorator_child})\n'
+
+ # Recurse the child first so it's initialized before parent (decorator) is written
+ write_tree(writer, tree_to_recurse)
+ writer.write(decorator_line)
+ writer.write(f'{parent}.add_child({varr_name})\n')
+
+ for elem in tree:
+ key_name_list = elem.split(';;')
+ c_key = key_name_list[0] # Guaranteed to always be one element
+
+ if c_key[0] in COMPOSITES:
+
+ try:
+ name = key_name_list[1] # If its a composite, there is a second element which is the desired name
+ except:
+ raise RuntimeError('Composite missing name: ' + key_name_list) # In case someone forgets to give a name
+ varr_name = to_variable(name)
+
+ composite_extension = None
+ try:
+ composite_extension = key_name_list[2] # If there is a third element, there is a desired idiom or parallel policy to be used
+ except:
+ # No idiom desired
+ pass
+
+ # ------ IDIOMS ------
+ # Idioms are decorators that handle entire sub-trees
+ if composite_extension in IDIOMS:
+ write_idiom()
+
+ # ------ COMPOSITES ------
+ else:
+ write_composite()
+
+ # Used to not write a parent to the root
+ if parent:
+ writer.write(f'{parent}.add_child({varr_name})\n')
+
+ else:
+ decorator = None
+ decorator_name = None
+ try: # Look if there is a decorator and a desired name for it
+ if isinstance(key_name_list[2], int): # If an int exist here, the decorator is Timeout and no name was given
+ decorator = key_name_list[1]
+ else:
+ if key_name_list[1] in DECORATORS:
+ decorator = key_name_list[1]
+ decorator_name = key_name_list[2]
+ except:
+ try: # Look if there is only a decorator
+ if key_name_list[1] in DECORATORS and key_name_list[1] not in BEHAVIOUR_EXTENSIONS:
+ decorator = key_name_list[1]
+ except: # None of the above => A behaviour was given
+ pass
+
+ # ------ BEHAVIOURS ------
+ if c_key[0] in BEHAVIOURS and decorator is None:
+ name = c_key[1:-1]
+
+ write_behaviour()
+
+ # ------ DECORATORS ------
+ # Decorators handle a SINGLE child
+ elif c_key[0] in DECORATORS or decorator in DECORATORS:
+ name = c_key
+ decorator_child = to_behaviour(to_variable(name))
+
+ write_decorator(decorator_name)
+
+ else: # Code was given something it can't handle, needs to be implemented
+ raise RuntimeError(f'Undefined behaviour for: "{elem}"')
+ writer.write("\n")
+
+
+def tidy_file(output_file_name: str, is_method: bool):
+ '''
+ Goes through the output file and reformats it to look better.
+
+ Groups the file into two sections:
+ 1. Initialization of all variables
+ 2. Structuring the tree (adds children to behaviours)
+
+ Gathers .add_child() calls to the same behaviour into one .add_children() call instead (still retains correct order of children)
+ Also adds new initialization of behaviour if the same behaviour is used several times in the tree (due to that a behaviour only can have a single parent)
+ '''
+ root = None
+ regular_lines = []
+ children_to_add = {}
+ seen = {} # K: Variable name ; V: List[Unused initialized variable (should swap between 1 and 0), Whole line]
+ special_case_parallels = {}
+ with open(output_file_name, 'r') as input:
+ first_line = True
+ for line in input:
+ if first_line: # Root should always be on the first line of the file
+ root = line.split(' = ')[0]
+ first_line = False
+
+ if 'add_child' in line:
+ s = line.split('.')
+ name = s[0]
+ rest = s[1]
+ dict_insert_or_append(children_to_add, name, rest[rest.find('(')+1:rest.find(')')]) # Adds the child within the () of the method call to the dict
+
+ else:
+ if 'SuccessOnSelected' in line:
+ children = line[line.find('[')+1: line.find(']')].split(',')
+ form_children = [child.strip() for child in children]
+ if form_children[0].isupper(): # Cheap way of checking if no children were specified
+ form_children = []
+ dict_insert_or_append(special_case_parallels, line, form_children)
+ regular_lines.append(line)
+
+ input.close
+ with open(output_file_name, 'w') as output:
+ output.write('import py_trees\n\n')
+ for line in regular_lines:
+ if line == '\n':
+ output.write(line)
+ continue
+
+ ls_rs = line.split(' = ')
+
+ name = ls_rs[0]
+ rest = ls_rs[1]
+
+ # Fill the dictionary with all variables
+ if name not in seen:
+ seen[name] = [1, line]
+ # Do not print the line yet, need to make sure it's children are initialized first
+ if line not in special_case_parallels:
+ output.write(line)
+
+ # Make sure all children for SuccessOnSelected policy is initialized before initializing the parallel
+ for special_parallel in special_case_parallels:
+ for child in special_case_parallels[special_parallel]:
+ # For each child found, remove it from the list of children for the policy
+ if child == name or child in seen:
+ special_case_parallels[special_parallel].remove(child)
+
+ if len(special_case_parallels[special_parallel]) == 0:
+ output.write(special_parallel)
+ special_case_parallels.pop(special_parallel)
+ break
+
+ for k, v in sorted(children_to_add.items()):
+
+ # Check if there are unused initialized variables, write one if not
+ # Checks if any of the elements in the dictionary match with a variable on the right side of the =
+ # If so, remove from the amount of unused variables of this kind
+
+ if isinstance(v, list):
+ for elem in v:
+ # If this crashes something is wrong with write_tree() function
+ if seen[elem][0] == 1:
+ seen[elem][0] -= 1
+ else:
+ output.write(seen[elem][1])
+
+ output.write(f'{k}.add_children([')
+ for i, elem in enumerate(v):
+ output.write(elem)
+ if i != len(v)-1:
+ output.write(', ')
+ output.write('])\n\n')
+ else:
+ # If this crashes something is wrong with write_tree() function
+ if seen[v][0] == 1:
+ seen[v][0] -= 1
+ else:
+ output.write(seen[v][1])
+
+ output.write(f'{k}.add_child({v})\n')
+
+ # Write the behaviour tree initialisation
+ if not is_method:
+ output.write(f'\n\nbehaviour_tree = py_trees.trees.BehaviourTree(root={root})\n')
+ output.write('behaviour_tree.setup(timeout=15)\n')
+ output.write('try:\n behaviour_tree.tick_tock(\n period_ms=500,\n number_of_iterations=py_trees.trees.CONTINOUS_TICK_TOCK,\n pre_tick_handler=None,\n post_tick_handler=None\n )\n')
+ output.write('except KeyboardInterrupt:\n behaviour_tree.interrupt()\n')
+ output.close
+
+def create_method() -> None:
+ method_name = file_to_read.split('/')[-1].split('.')[0]
+ root = ''
+ with open(output_file_name, 'r') as inp:
+ lines = inp.readlines()
+ inp.close()
+ with open(output_file_name, 'w') as out:
+ out.write(f'{lines[0]}\n') # "Import py_trees"
+ root = lines[2].split(" = ")[0] # Get the root for return
+ out.write(f'def {method_name}():')
+
+ for line in lines[1:]:
+ out.write(f' {line}')
+ out.write(f' return {root}')
+
+
+if __name__ == '__main__':
+
+ COMPOSITES = ['?', '-', '=']
+ IDIOMS = ['OneShot', 'EternalGuard', 'PickUpWhereYouLeftOff', 'PUWYLO', 'EitherOr']
+ BEHAVIOURS = ['[', '(']
+ BEHAVIOUR_EXTENSIONS = ['success', 'running', 'failure', 'meta', 'timer', "subtree"]
+ DECORATORS = ['!', 'Condition', 'EternalGuard', 'OneShot', 'Timeout', 'FailureIsRunning', 'FailureIsSuccess',
+ 'RunningIsFailure', 'RunningIsSuccess', 'SuccessIsFailure', 'SuccessIsRunning']
+
+ # This is used to add the 'node=self.node' parameter to behaviours
+ node = ""
+ first_node = ""
+ add_node = True
+ add_self_node = False
+ if add_node:
+ first_node = "node=node"
+ if add_self_node:
+ node = ", node=self.node"
+ else:
+ node = ", node=node"
+ if len(sys.argv) > 1:
+ file_to_read = sys.argv[1]
+ output_file_name = sys.argv[2]
+ else:
+ 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)
+
+ with open(output_file_name, 'w') as output:
+ write_tree(output, tree)
+ output.close
+
+ # TODO: Correctly implement this
+ #ans = input('Do you wish this tree to be created as a method? (y/n)\n If no, a class will be created instead')
+
+ tidy_file(output_file_name, True)
+ create_method()
+ """
+ if ans == 'y':
+ else:
+ pass
+ """
+ print('-----------------------------------------------\n')
+ print(f' Conversion finished!\n')
+ print(f' Result can be found in:\n')
+ print(f'{output_file_name}\n')
+ print('-----------------------------------------------')
\ No newline at end of file
diff --git a/src/lhw_intelligence/lhw_intelligence/trees/testing/headTouched.py b/src/lhw_intelligence/lhw_intelligence/trees/testing/headTouched.py
new file mode 100644
index 0000000000000000000000000000000000000000..96e28accc0f00275457e324ed53ccec2fc95f00e
--- /dev/null
+++ b/src/lhw_intelligence/lhw_intelligence/trees/testing/headTouched.py
@@ -0,0 +1,124 @@
+
+
+#!/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()
diff --git a/src/lhw_intelligence/lhw_intelligence/trees/testing/output.py b/src/lhw_intelligence/lhw_intelligence/trees/testing/output.py
new file mode 100644
index 0000000000000000000000000000000000000000..509563dabeab88bed762e8469dadd7946a197410
--- /dev/null
+++ b/src/lhw_intelligence/lhw_intelligence/trees/testing/output.py
@@ -0,0 +1,61 @@
+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
diff --git a/src/lhw_intelligence/lhw_intelligence/trees/testing/receptionist_v2.tree b/src/lhw_intelligence/lhw_intelligence/trees/testing/receptionist_v2.tree
new file mode 100644
index 0000000000000000000000000000000000000000..3c467cf830f48d32559ff9a7337a83db598acb28
--- /dev/null
+++ b/src/lhw_intelligence/lhw_intelligence/trees/testing/receptionist_v2.tree
@@ -0,0 +1,27 @@
+->;;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
diff --git a/src/lhw_intelligence/package.xml b/src/lhw_intelligence/package.xml
index 59460b1820fb733b1b75c0917900bfc51c9d704b..5e6df410dcb938b3a05261399252826f733fa516 100644
--- a/src/lhw_intelligence/package.xml
+++ b/src/lhw_intelligence/package.xml
@@ -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
diff --git a/src/lhw_intelligence/setup.py b/src/lhw_intelligence/setup.py
index 76198b0d1bfc446ed12696b089039e8148b5ced4..602bb9fa0ecf947e515c0a9c6a5556e584c1adce 100644
--- a/src/lhw_intelligence/setup.py
+++ b/src/lhw_intelligence/setup.py
@@ -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,20 @@ 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'
+
+ #'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',
+ 'introduce_person_tree = lhw_intelligence.trees.scenario_trees.introduce_person_tree:main'
+
],
},
)
diff --git a/src/lhw_qi/activate b/src/lhw_qi/activate
index 9a9289a9f2691d7e38d3c36c0720665a88cd7f1b..cd00e44606046d75036f38dcda06cdcfaf063c50 100644
--- a/src/lhw_qi/activate
+++ b/src/lhw_qi/activate
@@ -21,8 +21,7 @@ ldconfig
# Search for pepper on the local network
echo "Searching for the IP of Pepper"
#export PEPPER_IP=$(arp-scan --localnet | grep -i "48:a9:d2:8c:6a:0c\|00:13:95:1d:4c:43" | cut -f1) - pepper linköping MAC
-export PEPPER_IP=$(arp-scan --localnet | grep -i "48:a9:d2:8c:6f:91" | cut -f1)
-48:a9:d2:8c:6f:91
+export PEPPER_IP=$(arp-scan --localnet | grep -i "48:a9:d2:8c:6a:0c\|00:13:95:1d:4c:43" | cut -f1)
if [[ -z "${PEPPER_IP}" ]]; then
echo "${ORANGE}Pepper was not found on the current network, please set PEPPER_IP manually!${NC}"
else
diff --git a/src/lhw_qi/src/animated_speech_component.cpp b/src/lhw_qi/src/animated_speech_component.cpp
index 27ff27cc78b7a2ea163f04b6cd3603ac3bade198..f9ec11189dea3ac2b1a853e078dffc2d405e6d29 100644
--- a/src/lhw_qi/src/animated_speech_component.cpp
+++ b/src/lhw_qi/src/animated_speech_component.cpp
@@ -17,7 +17,7 @@ namespace lhw_qi
this->declare_parameter("speed", 0.8);
this->declare_parameter("pitch", 1.2);
this->declare_parameter("volume", 0.8);
- this->declare_parameter("use_arms", true);
+ this->declare_parameter("use_arms", false);
// Create a callback function for when messages are received.
diff --git a/src/lhw_qi/src/motion_component.cpp b/src/lhw_qi/src/motion_component.cpp
index fcea3f67a9f053bdab87fadacbed711a0480cd15..1ded3afb77649dc6bfbcd840c7701b9b10f954c2 100644
--- a/src/lhw_qi/src/motion_component.cpp
+++ b/src/lhw_qi/src/motion_component.cpp
@@ -147,7 +147,7 @@ namespace lhw_qi
else if (action.type == action.POINT_ABS_STRAIGHT) {
if (action.arm == action.ARM_RIGHT) {
// Right arm
- std::vector<float> position = {0.0f, -1.0f, 0.0f};
+ std::vector<float> position = {1.0f, 0.0f, 0.0f};
tracker_service_.call<void>("pointAt", "RArm", position, 0, 0.3);
}
else if (action.arm == action.ARM_LEFT) {