diff --git a/presets.json b/presets.json
index 0c292d056f2837d8aa25db0664afcef8d8b11169..f2eef492e115273d91b12dc9a5ccc8f4c60d4fc4 100644
--- a/presets.json
+++ b/presets.json
@@ -34,17 +34,18 @@
{
"container" : "lhw_qi",
- "command" : "ros2 run lhw_qi tablet -i $PEPPER_IP"
+ "command" : "ros2 run lhw_qi tablet -i $PEPPER_IP",
+ "verbose" : true
},
{
"container" : "lhw_qi",
"command" : "ros2 run lhw_qi microphone -i $PEPPER_IP",
- "verbose" : false
+ "verbose" : true
},
{
"container" : "lhw_qi",
"command" : "ros2 run lhw_qi animated_speech -i $PEPPER_IP",
- "verbose" : false
+ "verbose" : true
},
{
"container" : "lhw_tablet",
@@ -54,6 +55,12 @@
{
"container" : "lhw_intelligence",
"command" : "ros2 launch lhw_intelligence safety_check.launch.py",
+ "verbose" : true,
+ "delay" : 1
+ },
+ {
+ "container" : "lhw_intelligence",
+ "command" : "py-trees-tree-watcher -a",
"verbose" : true
}
]
diff --git a/src/lhw_intelligence/activate b/src/lhw_intelligence/activate
index db966db836cc921fac837c528d40b118b83a719e..4d49787672bdc377eac2468e9a99894408a74f7c 100644
--- a/src/lhw_intelligence/activate
+++ b/src/lhw_intelligence/activate
@@ -15,7 +15,7 @@ if [ -f $install_setup ]; then
else
echo -e "${ORANGE}could not source colcon workspace. Probably not yet built.${NC}"
fi
-export PEPPER_IP=$(arp-scan --localnet | grep -i "48:a9:d2:8c:6f:91" | cut -f1)
+#export PEPPER_IP=$(arp-scan --localnet | grep -i "48:a9:d2:8c:6f:91" | cut -f1)
# For starting up all required nodes for intelligence
if [ $ENV_LAUNCH ]; then
# TODO: Why is both intelligence.launch.py and py_tree.launch.py needed????
diff --git a/src/lhw_intelligence/lhw_intelligence/behaviours/motion_behaviours.py b/src/lhw_intelligence/lhw_intelligence/behaviours/motion_behaviours.py
index 463b0ca152e0ccc0e9515e44944f6f295b4424ca..a3f5fac91f2d00898b8e1770eb43e6445103c067 100644
--- a/src/lhw_intelligence/lhw_intelligence/behaviours/motion_behaviours.py
+++ b/src/lhw_intelligence/lhw_intelligence/behaviours/motion_behaviours.py
@@ -99,9 +99,11 @@ class MoveToPoint(py_trees.behaviour.Behaviour):
msg = Bool()
msg.data = True
+ self.accepted = False
self.fix_head_pub.publish(msg)
self.move_base_status = 0
- self.move_status_subscription = self.node.create_subscription(GoalStatusArray, "/lhw/nav/move_base/status", self.goal_sucess_callback, 10)
+ self.prior_status = 0
+
#Orientation
goal_orientation = '{{x: {0}, y: {1}, z: {2}, w: {3}}}'
@@ -110,9 +112,10 @@ class MoveToPoint(py_trees.behaviour.Behaviour):
print(g_o)
#Position
goal_position = '{{x: {0}, y: {1}, z: {2}}}'
+ self.move_status_subscription = self.node.create_subscription(GoalStatusArray, "/lhw/nav/move_base/status", self.goal_feedback_callback, 10)
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"
+ self.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}")
@@ -120,12 +123,19 @@ class MoveToPoint(py_trees.behaviour.Behaviour):
self.ssh_client.invoke_shell()
- stdin, stdout, stderr = self.ssh_client.exec_command("source ~/.bash_profile && " + goal_cmd)
+
+ stdin, stdout, stderr = self.ssh_client.exec_command("source ~/.bash_profile && " + self.goal_cmd)
def update(self):
print("Listening for callback...")
+
+ if self.move_base_status == 2 and self.prior_status == 2:
+ return py_trees.common.Status.SUCCESS
+ if not self.accepted:
+ return py_trees.common.Status.RUNNING
+
if self.move_base_status == 3:
return py_trees.common.Status.SUCCESS
if self.move_base_status >= 4:
@@ -136,17 +146,24 @@ class MoveToPoint(py_trees.behaviour.Behaviour):
def terminate(self, new_status: py_trees.common.Status):
self.node.destroy_subscription(self.move_status_subscription)
- def goal_sucess_callback(self, msg):
+ def goal_feedback_callback(self, msg):
try:
print(msg.status_list[0].status, "Current Status")
except IndexError:
return
+ if msg.status_list[0].status == 1:
+ self.accepted = True
+
+ # if self.move_base_status == 1 and self.prior_status == 2:
+ # self.resend = True
+ # stdin, stdout, stderr = self.ssh_client.exec_command("source ~/.bash_profile && " + self.goal_cmd)
+
#May Add more Statuses if necessary
- if (msg.status_list[0].status == 3 or msg.status_list[0].status == 2):
- self.move_base_status = 3
- if msg.status_list[0].status == 4:
- self.move_base_status = 4
+
+ self.prior_status = self.move_base_status
+ self.move_base_status = msg.status_list[0].status
+
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
index f64b069bf134d5c54931027c373a472da05028a8..be4fcecd2580342b3df0e6751adb5286eae88585 100644
--- a/src/lhw_intelligence/lhw_intelligence/behaviours/wait_for_touch_behaviour.py
+++ b/src/lhw_intelligence/lhw_intelligence/behaviours/wait_for_touch_behaviour.py
@@ -61,9 +61,9 @@ class WaitForHeadTouch(py_trees.behaviour.Behaviour):
"""
self.logger.debug("%s.initialise()" % self.__class__.__name__)
- self.timer = self.node.create_timer(1.0, self._toggle_leds_callback)
+ #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)
-
+ self.blackboard.scenario_started = False
goal = TabletAction.Goal()
goal.type = "text"
goal.text = "Please tap on my head"
@@ -96,7 +96,7 @@ class WaitForHeadTouch(py_trees.behaviour.Behaviour):
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.timer.destroy()
self.node.destroy_subscription(self.head_touch_sub)
if not self.led_is_on:
msg = std_msgs.msg.Bool()
diff --git a/src/lhw_intelligence/lhw_intelligence/config.json b/src/lhw_intelligence/lhw_intelligence/config.json
index 7fa34a955ae66a3d1453176dba51afa73508dc90..db4bf2c79336777277fc2eff291beb93a7b70b4c 100644
--- a/src/lhw_intelligence/lhw_intelligence/config.json
+++ b/src/lhw_intelligence/lhw_intelligence/config.json
@@ -39,7 +39,7 @@
"x": 1.0
},
"safety_point": {
- "x": -1.913,
+ "x": -1.713,
"y": 1.483,
"z": 0.0,
"x_o": 0.0,
@@ -47,9 +47,18 @@
"z_o": 0.7837259930777551,
"w_o": 0.6211067281669765
},
+ "middle_point": {
+ "x" : 0.895,
+ "y" : 1.483,
+ "z" : 0.0,
+ "x_o": 0.0,
+ "y_o": 0.0,
+ "z_o": -0.707595531016635,
+ "w_o": 0.7066176933004766
+ },
"inside_door_point": {
"x": 0.9849369525909424,
- "y": 3.346065883636475,
+ "y": 3.146065883636475,
"z": 0.0,
"x_o": 0.0,
"y_o": 0.0,
diff --git a/src/lhw_intelligence/lhw_intelligence/config_old.json b/src/lhw_intelligence/lhw_intelligence/config_old.json
new file mode 100644
index 0000000000000000000000000000000000000000..db4bf2c79336777277fc2eff291beb93a7b70b4c
--- /dev/null
+++ b/src/lhw_intelligence/lhw_intelligence/config_old.json
@@ -0,0 +1,95 @@
+{
+ "waiting_point": {
+ "x": 6.3,
+ "y": 1.5,
+ "z": 0.0,
+ "x_o":0.0,
+ "y_o":0.0,
+ "z_o": -0.686935745454,
+ "w_o":0.72671815831
+ },
+ "exit": {
+ "x": -1.9913077354431152,
+ "y": 4.441965579986572,
+ "z": 0.0,
+ "x_o": 0.0,
+ "y_o": 0.0,
+ "z_o": 0.7084559755124268,
+ "w_o": 0.7057550076058516
+ },
+ "door": {
+ "x": 7.8,
+ "y": 1.8,
+ "z": 0.0,
+ "x_o":0.0,
+ "y_o":0.0,
+ "z_o": 0.2835846957,
+ "w_o":0.958947193731
+ },
+ "sofa": {
+ "x": 7.16,
+ "y": 3.35,
+ "z": 0.0,
+ "x_o":0.0,
+ "y_o":0.0,
+ "z_o":0.703396051335,
+ "w_o":0.710798139394
+ },
+ "door_dist": {
+ "x": 1.0
+ },
+ "safety_point": {
+ "x": -1.713,
+ "y": 1.483,
+ "z": 0.0,
+ "x_o": 0.0,
+ "y_o": 0.0,
+ "z_o": 0.7837259930777551,
+ "w_o": 0.6211067281669765
+ },
+ "middle_point": {
+ "x" : 0.895,
+ "y" : 1.483,
+ "z" : 0.0,
+ "x_o": 0.0,
+ "y_o": 0.0,
+ "z_o": -0.707595531016635,
+ "w_o": 0.7066176933004766
+ },
+ "inside_door_point": {
+ "x": 0.9849369525909424,
+ "y": 3.146065883636475,
+ "z": 0.0,
+ "x_o": 0.0,
+ "y_o": 0.0,
+ "z_o": -0.7052083656403756,
+ "w_o": 0.7090001135619305
+ },
+ "before_safety_point_door": {
+ "x": -0.316 ,
+ "y": 1.483,
+ "z": 0.0,
+ "x_o": 0.0,
+ "y_o": 0.0,
+ "z_o": -0.9998805173052926,
+ "w_o": 0.015458043644024547
+ },
+ "starting_point": {
+ "x": 0.992931604385376,
+ "y": 4.604823589324951,
+ "z": 0.0,
+ "x_o": 0.0,
+ "y_o": 0.0,
+ "z_o": -0.707595531016635,
+ "w_o": 0.7066176933004766
+ },
+ "receptionist_starting_point": {
+ "x": 5.5,
+ "y": 1.8,
+ "z": 0.0,
+ "x_o":0.0,
+ "y_o":0.0,
+ "z_o": 0.339630850466,
+ "w_o":0.940558815498
+ }
+}
\ No newline at end of file
diff --git a/src/lhw_intelligence/lhw_intelligence/trees/scenario_trees/safety_check_tree.py b/src/lhw_intelligence/lhw_intelligence/trees/scenario_trees/safety_check_tree.py
index 6c0f282d3065c9d3600d1110a6181ae050853702..18ac46728f2e176a4260271eea9088c195b4b451 100644
--- a/src/lhw_intelligence/lhw_intelligence/trees/scenario_trees/safety_check_tree.py
+++ b/src/lhw_intelligence/lhw_intelligence/trees/scenario_trees/safety_check_tree.py
@@ -35,17 +35,13 @@ def position_to_pose(position: dict) -> Pose:
def show_speech_tree() -> py_trees.behaviour.Behaviour:
guard = GuardBehaviour('show_speech_guard', 'has_shown_speech')
- show_speech_beginning = TabletShowFIALogo(name="show_speech_beginning")
- show_speech_end = TabletShowFIALogo(name="show_speech_end")
+ show_speech_end = Say(name="safe say", message="My head was pressed")
- wait = py_trees.timers.Timer(name="Wait", duration=2)
- say_ready_for_safety_check = Say(name="safety check say", message="I am ready for the safety check. Press the button when it's done.")
+ say_ready_for_safety_check = Say(name="safety check say", message="I am ready for the safety check. Press my head when it's done.")
head_touch = WaitForHeadTouch(name="wait_for_head_touch")
return py_trees.composites.Sequence(
name = "Show speech",
- children = [
- show_speech_beginning,
- wait,
+ children = [
say_ready_for_safety_check,
head_touch,
show_speech_end
@@ -83,13 +79,15 @@ def create_root(ssh) -> py_trees.behaviour.Behaviour:
starting_point = position_to_pose(config["starting_point"])
inside_door = position_to_pose(config["inside_door_point"])
before_safety = position_to_pose(config["before_safety_point_door"])
+ middle_point = position_to_pose(config["middle_point"])
safety_point = position_to_pose(config["safety_point"])
exit_pose = position_to_pose(config["exit"])
set_pose = SetPose(name="Set starting pose", pose=starting_point, ssh_client=ssh)
move_inside_door = MoveToPoint(name="Move inside door", pose=inside_door, ssh_client=ssh)
- move_before_safety = MoveToPoint(name="Move inside door", pose=before_safety, ssh_client=ssh)
- move_to_safety_point = MoveToPoint(name="Move to Point", pose=safety_point, ssh_client=ssh)
+ move_middle = MoveToPoint(name="Move to middle", pose=middle_point, ssh_client=ssh)
+ move_before_safety = MoveToPoint(name="Move before safety", pose=before_safety, ssh_client=ssh)
+ move_to_safety_point = MoveToPoint(name="Move to safety", pose=safety_point, ssh_client=ssh)
move_to_exit = MoveToPoint(name="Move to Point",pose=exit_pose, ssh_client=ssh)
#standby = Standby(name="Standby")
@@ -97,7 +95,7 @@ def create_root(ssh) -> py_trees.behaviour.Behaviour:
say_door_open = Say(name="test say", message="Checking that door is open")
wait_for_door = WaitForDoorOpen(name="wait_for_door")
speech_tree = show_speech_tree()
-
+ idle = py_trees.behaviours.Running("idle")
safety_check_sequence = py_trees.composites.Sequence(
name="Safety Check Sequence",
children=[
@@ -106,12 +104,19 @@ def create_root(ssh) -> py_trees.behaviour.Behaviour:
say_door_open,
wait_for_door,
move_inside_door,
+ py_trees.timers.Timer(name="Wait", duration=1),
+ move_middle,
+ py_trees.timers.Timer(name="Wait", duration=1),
move_before_safety,
+ py_trees.timers.Timer(name="Wait", duration=1),
move_to_safety_point,
+ py_trees.timers.Timer(name="Wait", duration=8),
speech_tree,
- move_to_exit
- ]
- )
+ move_to_exit,
+ idle
+ ]
+ )
+
return safety_check_sequence
@@ -123,10 +128,11 @@ def main():
ssh_client.set_missing_host_key_policy(paramiko.AutoAddPolicy)
pepper_ip = os.getenv("PEPPER_IP")
if not pepper_ip:
- pepper_ip = "192.168.1.240"
+ pepper_ip = "169.254.3.76"
ssh_client.connect(hostname = pepper_ip, username = "nao", password = "ellis")
ssh_client.invoke_shell()
root = create_root(ssh_client)
+
tree = py_trees_ros.trees.BehaviourTree(
root=root,
unicode_tree_debug=True
diff --git a/src/lhw_qi/activate b/src/lhw_qi/activate
index a888b3293169e20d944baafbbcf28b3f8b1bb388..2987a047331e8227afb8a97dac56b73fcfd1ef56 100644
--- a/src/lhw_qi/activate
+++ b/src/lhw_qi/activate
@@ -22,8 +22,8 @@ ldconfig
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:68:0c\|00:13:95:1d:4c:43" | cut -f1)
-export PEPPER_IP=192.168.1.240
+#export PEPPER_IP=$(arp-scan --localnet | grep -i "48:a9:d2:8c:68:0c\|00:13:95:1d:4c:43" | cut -f1)
+export PEPPER_IP=169.254.3.76
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_tablet/activate b/src/lhw_tablet/activate
index f1abbb8d2d115f52ac729251babf5b55057ed865..2b9a761fd68f5d538c90b42e953de1db7a83e965 100644
--- a/src/lhw_tablet/activate
+++ b/src/lhw_tablet/activate
@@ -20,8 +20,8 @@ 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)
-export PEPPER_IP=192.168.1.240
+#export EPPER_IP=$(arp-scan --localnet | grep -i "48:a9:d2:8c:6a:0c\|00:13:95:1d:4c:43" | cut -f1)
+export PEPPER_IP=169.254.3.76
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_tablet/lhw_tablet/socket_server.py b/src/lhw_tablet/lhw_tablet/socket_server.py
index b6e42109038d5c5405ede7b3ada41defde25ce2d..ef840980a9258f74ac4a1935e6a48ad30690e863 100755
--- a/src/lhw_tablet/lhw_tablet/socket_server.py
+++ b/src/lhw_tablet/lhw_tablet/socket_server.py
@@ -10,7 +10,7 @@ class SocketServer():
self.PORT = 6969
# Change this IP-address if the network configuration changes
- self.FETDATORN_IP = "192.168.1.190"
+ self.FETDATORN_IP = "169.254.59.107"
self.SHARED_MEMORY_SIZE = 20 # Also dictates max size for ws msgs & option strings