diff --git a/src/robot_behaviour/config/movements/p34.yaml b/src/robot_behaviour/config/movements/p34.yaml
index 10464cce626a24f4024f82f95e9cac59a0991907..5bad726184cad0fe48cbb68a387fbf7aca63af18 100644
--- a/src/robot_behaviour/config/movements/p34.yaml
+++ b/src/robot_behaviour/config/movements/p34.yaml
@@ -8,10 +8,8 @@ play_motion:
- time_from_start: 3
positions: [0.6386, 0.543, -1.1834, 1.3668, -1.6315, 0.5706, -1.5775]
- time_from_start: 5
- positions: [0.6274, 0.3311, -1.1834, 1.3083, -1.6315, 0.5706, -1.5775]
- - time_from_start: 7
positions: [0.6274, 0.3213, -1.1835, 1.3082, -1.6314, 0.5706, -1.5776]
meta:
name: p34
usage: p34
- description: pick a token in position p34 with intermediate point
\ No newline at end of file
+ description: pick a token in position p34 with intermediate point
diff --git a/src/robot_behaviour/config/movements/p35.yaml b/src/robot_behaviour/config/movements/p35.yaml
index a08e36f08603ea5641d097700bcbdb1f4fcf831a..fd26e59249932617e853a02074fe1a9c27c272e0 100644
--- a/src/robot_behaviour/config/movements/p35.yaml
+++ b/src/robot_behaviour/config/movements/p35.yaml
@@ -8,10 +8,8 @@ play_motion:
- time_from_start: 3
positions: [0.5399, 0.4803, -1.1818, 1.298, -1.6315, 0.5706, -1.5775]
- time_from_start: 5
- positions: [0.5417, 0.2994, -1.1818, 1.2352, -1.6315, 0.5706, -1.5775]
- - time_from_start: 7
positions: [0.5418, 0.2894, -1.1826, 1.2352, -1.6314, 0.5706, -1.5776]
meta:
name: p35
usage: p35
- description: pick a token in position p35 with intermediate point
\ No newline at end of file
+ description: pick a token in position p35 with intermediate point
diff --git a/src/robot_behaviour/config/movements/s22.yaml b/src/robot_behaviour/config/movements/s22.yaml
index 21c4c5411e2c1ff1184a37cb98a822876741d6c7..8c7b5173c39cb52751d9ea167e2324eb7dd77c1e 100644
--- a/src/robot_behaviour/config/movements/s22.yaml
+++ b/src/robot_behaviour/config/movements/s22.yaml
@@ -6,18 +6,18 @@ play_motion:
- time_from_start: 0
positions: [0.68, 0.2706, -1.3499, 1.3701, -1.6295, 0.5699, -1.5799]
#go to first step
- - time_from_start: 3
+ - time_from_start: 2
positions: [0.939, 0.223, -1.3074, 1.0479, -1.6315, 0.5706, -1.5775]
#go to second step
- - time_from_start: 5
+ - time_from_start: 4
positions: [0.939, 0.183, -1.3074, 1.0479, -1.6315, 0.5706, -1.5775]
#go to third step
- - time_from_start: 7
+ - time_from_start: 6
positions: [0.939, 0.223, -1.3074, 1.0479, -1.6315, 0.5706, -1.5775]
#come back
- - time_from_start: 10
+ - time_from_start: 8
positions: [0.2269, 0.3316, -1.6138, 2.2556, -1.6316, 0.5703, -1.5773]
meta:
name: s22
usage: s22
- description: suggest the solution for p22
\ No newline at end of file
+ description: suggest the solution for p22
diff --git a/src/robot_behaviour/config/movements/s31.yaml b/src/robot_behaviour/config/movements/s31.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..851fc78cf745495d87417400a9eb6d38e3679a0d
--- /dev/null
+++ b/src/robot_behaviour/config/movements/s31.yaml
@@ -0,0 +1,25 @@
+play_motion:
+ motions:
+ s31:
+ joints: [arm_1_joint, arm_2_joint, arm_3_joint, arm_4_joint, arm_5_joint, arm_6_joint, arm_7_joint]
+ points:
+ - time_from_start: 0
+ positions: [0.5516, 0.3872, -1.4816, 1.4506, -1.7999, 0.5705, -1.5773]
+ #go to step1
+ - time_from_start: 3
+ positions: [0.982, 0.4805, -1.2072, 1.3062, -1.6315, 0.5606, -1.5775]
+ #go to step2
+ - time_from_start: 4
+ positions: [0.982, 0.3805, -1.2072, 1.3062, -1.6315, 0.5606, -1.5775]
+ #go to step3
+ - time_from_start: 5
+ positions: [0.982, 0.4805, -1.2072, 1.3062, -1.6315, 0.5606, -1.5775]
+ #come back
+ - time_from_start: 7
+ positions: [0.2269, 0.3316, -1.6138, 2.2556, -1.6316, 0.5703, -1.5773]
+
+
+ meta:
+ name: s31
+ usage: s31
+ description: suggest the solution for p31
diff --git a/src/robot_behaviour/config/movements/s32.yaml b/src/robot_behaviour/config/movements/s32.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..0173dff579a9735b5a5d493751118bdc9599ec47
--- /dev/null
+++ b/src/robot_behaviour/config/movements/s32.yaml
@@ -0,0 +1,25 @@
+play_motion:
+ motions:
+ s32:
+ joints: [arm_1_joint, arm_2_joint, arm_3_joint, arm_4_joint, arm_5_joint, arm_6_joint, arm_7_joint]
+ points:
+ - time_from_start: 0
+ positions: [0.5516, 0.3872, -1.4816, 1.4506, -1.7999, 0.5705, -1.5773]
+ #go to step1
+ - time_from_start: 3
+ positions: [0.8871, 0.5781, -1.1497, 1.3456, -1.6315, 0.5706, -1.5775]
+ #go to step2
+ - time_from_start: 4
+ positions: [0.8871, 0.4781, -1.1497, 1.3456, -1.6315, 0.5706, -1.5775]
+ #go to step3
+ - time_from_start: 5
+ positions: [0.8871, 0.5781, -1.1497, 1.3456, -1.6315, 0.5706, -1.5775]
+ #come back
+ - time_from_start: 7
+ positions: [0.2269, 0.3316, -1.6138, 2.2556, -1.6316, 0.5703, -1.5773]
+
+
+ meta:
+ name: s32
+ usage: s32
+ description: suggest the solution for p32
diff --git a/src/robot_behaviour/config/movements/s33.yaml b/src/robot_behaviour/config/movements/s33.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..8d31abfd7b158d8911a808a5888132cf947eec40
--- /dev/null
+++ b/src/robot_behaviour/config/movements/s33.yaml
@@ -0,0 +1,25 @@
+play_motion:
+ motions:
+ s33:
+ joints: [arm_1_joint, arm_2_joint, arm_3_joint, arm_4_joint, arm_5_joint, arm_6_joint, arm_7_joint]
+ points:
+ - time_from_start: 0
+ positions: [0.5516, 0.3872, -1.4816, 1.4506, -1.7999, 0.5705, -1.5773]
+ #go to step1
+ - time_from_start: 3
+ positions: [0.7396, 0.4637, -1.1922, 1.3392, -1.6315, 0.5706, -1.5775]
+ #go to step2
+ - time_from_start: 4
+ positions: [0.7396, 0.3637, -1.1922, 1.3392, -1.6315, 0.5706, -1.5775]
+ #go to step3
+ - time_from_start: 5
+ positions: [0.7396, 0.4637, -1.1922, 1.3392, -1.6315, 0.5706, -1.5775]
+ #come back
+ - time_from_start: 7
+ positions: [0.2269, 0.3316, -1.6138, 2.2556, -1.6316, 0.5703, -1.5773]
+
+
+ meta:
+ name: s33
+ usage: s33
+ description: suggest the solution for p33
diff --git a/src/robot_behaviour/config/movements/s34.yaml b/src/robot_behaviour/config/movements/s34.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..4f19f81ea43b5fe167281984abfd7f5bc8fc9cbc
--- /dev/null
+++ b/src/robot_behaviour/config/movements/s34.yaml
@@ -0,0 +1,25 @@
+play_motion:
+ motions:
+ s34:
+ joints: [arm_1_joint, arm_2_joint, arm_3_joint, arm_4_joint, arm_5_joint, arm_6_joint, arm_7_joint]
+ points:
+ - time_from_start: 0
+ positions: [0.5516, 0.3872, -1.4816, 1.4506, -1.7999, 0.5705, -1.5773]
+ #go to step1
+ - time_from_start: 3
+ positions: [0.6274, 0.4813, -1.1835, 1.3082, -1.6314, 0.5706, -1.5776]
+ #go to step2
+ - time_from_start: 4
+ positions: [0.6274, 0.3813, -1.1835, 1.3082, -1.6314, 0.5706, -1.5776]
+ #go to step3
+ - time_from_start: 5
+ positions: [0.6274, 0.4813, -1.1835, 1.3082, -1.6314, 0.5706, -1.5776]
+ #come back
+ - time_from_start: 7
+ positions: [0.2269, 0.3316, -1.6138, 2.2556, -1.6316, 0.5703, -1.5773]
+
+
+ meta:
+ name: s34
+ usage: s34
+ description: suggest the solution for p34
diff --git a/src/robot_behaviour/config/movements/s35.yaml b/src/robot_behaviour/config/movements/s35.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..d3b0353bad00f9ac834c9e2eb764c62bad1f2e5e
--- /dev/null
+++ b/src/robot_behaviour/config/movements/s35.yaml
@@ -0,0 +1,25 @@
+play_motion:
+ motions:
+ s35:
+ joints: [arm_1_joint, arm_2_joint, arm_3_joint, arm_4_joint, arm_5_joint, arm_6_joint, arm_7_joint]
+ points:
+ - time_from_start: 0
+ positions: [0.5516, 0.3872, -1.4816, 1.4506, -1.7999, 0.5705, -1.5773]
+ #go to step1
+ - time_from_start: 3
+ positions: [0.5418, 0.4894, -1.1826, 1.2352, -1.6314, 0.5706, -1.5776]
+ #go to step2
+ - time_from_start: 4
+ positions: [0.5418, 0.3894, -1.1826, 1.2352, -1.6314, 0.5706, -1.5776]
+ #go to step3
+ - time_from_start: 5
+ positions: [0.5418, 0.4894, -1.1826, 1.2352, -1.6314, 0.5706, -1.5776]
+ #come back
+ - time_from_start: 7
+ positions: [0.2269, 0.3316, -1.6138, 2.2556, -1.6316, 0.5703, -1.5773]
+
+
+ meta:
+ name: s35
+ usage: s35
+ description: suggest the solution for p35
diff --git a/src/robot_behaviour/config/movements/sr3.yaml b/src/robot_behaviour/config/movements/sr3.yaml
index b13a385dc4292dead093d2b371cd0494ae7a77f7..74ba7cbb7ecac84ab56c2316aee1cdd28c8c0210 100644
--- a/src/robot_behaviour/config/movements/sr3.yaml
+++ b/src/robot_behaviour/config/movements/sr3.yaml
@@ -6,12 +6,12 @@ play_motion:
- time_from_start: 0
positions: [0.68, 0.2706, -1.3499, 1.3701, -1.6295, 0.5699, -1.5799]
#go to p13
- - time_from_start: 3
+ - time_from_start: 2
positions: [1.07, 0.2206, -1.2998, 1.0401, -1.6298, 0.5703, -1.5803]
- - time_from_start: 7
+ - time_from_start: 4.5
positions: [0.64, 0.22, -1.2499, 1.0199, -1.6, 0.5399, -1.5499]
#come back
- - time_from_start: 10
+ - time_from_start: 7
positions: [0.2269, 0.3316, -1.6138, 2.2556, -1.6316, 0.5703, -1.5773]
meta:
name: sr3
diff --git a/src/robot_behaviour/config/movements/sr4.yaml b/src/robot_behaviour/config/movements/sr4.yaml
index 6e75ab53cf0a2b7a2d3d2a5f24d0d3ada3431567..4bdddaf2c8723def3ebe8e12f0d3883dcd86837e 100644
--- a/src/robot_behaviour/config/movements/sr4.yaml
+++ b/src/robot_behaviour/config/movements/sr4.yaml
@@ -6,12 +6,12 @@ play_motion:
- time_from_start: 0
positions: [0.68, 0.2706, -1.3499, 1.3701, -1.6295, 0.5699, -1.5799]
#go to p13
- - time_from_start: 3
+ - time_from_start: 2
positions: [1.2786, 0.1891, -1.1414, 0.6463, -1.5298, 0.5698, -1.5781]
- - time_from_start: 7
+ - time_from_start: 4.5
positions: [0.9676, 0.1, -1.1193, 0.4847, -1.5498, 0.57, -1.5801]
#come back
- - time_from_start: 10
+ - time_from_start: 7
positions: [0.2269, 0.3316, -1.6138, 2.2556, -1.6316, 0.5703, -1.5773]
meta:
name: sr4
diff --git a/src/robot_behaviour/config/movements/ss31.yaml b/src/robot_behaviour/config/movements/ss31.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..ce9a8d889c1402eb69fd450ab74b0f06b342c7ca
--- /dev/null
+++ b/src/robot_behaviour/config/movements/ss31.yaml
@@ -0,0 +1,24 @@
+play_motion:
+ motions:
+ ss31:
+ joints: [arm_1_joint, arm_2_joint, arm_3_joint, arm_4_joint, arm_5_joint, arm_6_joint, arm_7_joint]
+ points:
+ - time_from_start: 0
+ positions: [0.5516, 0.3872, -1.4816, 1.4506, -1.7999, 0.5705, -1.5773]
+ #go to p33
+ - time_from_start: 3
+ positions: [0.6256, 0.3918, -1.4047, 1.4211, -1.7882, 0.5703, -1.5774]
+ #go to p31
+ - time_from_start: 5
+ positions: [0.9706, 0.3627, -1.269, 1.3155, -1.6315, 0.5706, -1.5775]
+ #go to p33
+ - time_from_start: 7
+ positions: [0.6256, 0.3918, -1.4047, 1.4211, -1.7882, 0.5703, -1.5774]
+ #come back
+ - time_from_start: 10
+ positions: [0.2269, 0.3316, -1.6138, 2.2556, -1.6316, 0.5703, -1.5773]
+ meta:
+ name: ss31
+ usage: ss31
+ description: suggest a subset of solution from p31
+
diff --git a/src/robot_behaviour/config/movements/ss32.yaml b/src/robot_behaviour/config/movements/ss32.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..4608a2f0d0fd82c30fd4da19c0bb03b806609c30
--- /dev/null
+++ b/src/robot_behaviour/config/movements/ss32.yaml
@@ -0,0 +1,24 @@
+play_motion:
+ motions:
+ ss32:
+ joints: [arm_1_joint, arm_2_joint, arm_3_joint, arm_4_joint, arm_5_joint, arm_6_joint, arm_7_joint]
+ points:
+ - time_from_start: 0
+ positions: [0.5516, 0.3872, -1.4816, 1.4506, -1.7999, 0.5705, -1.5773]
+ #go to p33
+ - time_from_start: 3
+ positions: [0.6256, 0.3918, -1.4047, 1.4211, -1.7882, 0.5703, -1.5774]
+ #go to p31
+ - time_from_start: 5
+ positions: [0.9706, 0.3627, -1.269, 1.3155, -1.6315, 0.5706, -1.5775]
+ #go to p33
+ - time_from_start: 7
+ positions: [0.6256, 0.3918, -1.4047, 1.4211, -1.7882, 0.5703, -1.5774]
+ #come back
+ - time_from_start: 10
+ positions: [0.2269, 0.3316, -1.6138, 2.2556, -1.6316, 0.5703, -1.5773]
+ meta:
+ name: ss32
+ usage: ss32
+ description: suggest a subset of solution from p32
+
diff --git a/src/robot_behaviour/config/movements/ss33.yaml b/src/robot_behaviour/config/movements/ss33.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..ae9d64361e82d3627741741c31b19ec2c0c38b1a
--- /dev/null
+++ b/src/robot_behaviour/config/movements/ss33.yaml
@@ -0,0 +1,24 @@
+play_motion:
+ motions:
+ ss33:
+ joints: [arm_1_joint, arm_2_joint, arm_3_joint, arm_4_joint, arm_5_joint, arm_6_joint, arm_7_joint]
+ points:
+ - time_from_start: 0
+ positions: [0.5516, 0.3872, -1.4816, 1.4506, -1.7999, 0.5705, -1.5773]
+ #go to p32
+ - time_from_start: 3
+ positions: [0.8808, 0.5741, -1.1959, 1.3947, -1.6315, 0.5706, -1.5775]
+ #go to p34
+ - time_from_start: 5
+ positions: [0.6386, 0.543, -1.1834, 1.3668, -1.6315, 0.5706, -1.5775]
+ #go to p32
+ - time_from_start: 7
+ positions: [0.8808, 0.5741, -1.1959, 1.3947, -1.6315, 0.5706, -1.5775]
+ #come back
+ - time_from_start: 10
+ positions: [0.2269, 0.3316, -1.6138, 2.2556, -1.6316, 0.5703, -1.5773]
+ meta:
+ name: ss33
+ usage: ss33
+ description: suggest a subset of solution from p33
+
diff --git a/src/robot_behaviour/config/movements/ss34.yaml b/src/robot_behaviour/config/movements/ss34.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..2c33af7973768a782389643d8fd8a50daa9c784e
--- /dev/null
+++ b/src/robot_behaviour/config/movements/ss34.yaml
@@ -0,0 +1,24 @@
+play_motion:
+ motions:
+ ss34:
+ joints: [arm_1_joint, arm_2_joint, arm_3_joint, arm_4_joint, arm_5_joint, arm_6_joint, arm_7_joint]
+ points:
+ - time_from_start: 0
+ positions: [0.5516, 0.3872, -1.4816, 1.4506, -1.7999, 0.5705, -1.5773]
+ #go to p33
+ - time_from_start: 3
+ positions: [0.7119, 0.4834, -1.2491, 1.3931, -1.6315, 0.5706, -1.5775]
+ #go to p35
+ - time_from_start: 5
+ positions: [0.5399, 0.4803, -1.1818, 1.298, -1.6315, 0.5706, -1.5775]
+ #go to p33
+ - time_from_start: 7
+ positions: [0.7119, 0.4834, -1.2491, 1.3931, -1.6315, 0.5706, -1.5775]
+ #come back
+ - time_from_start: 10
+ positions: [0.2269, 0.3316, -1.6138, 2.2556, -1.6316, 0.5703, -1.5773]
+ meta:
+ name: ss34
+ usage: ss34
+ description: suggest a subset of solution from p34
+
diff --git a/src/robot_behaviour/config/movements/ss35.yaml b/src/robot_behaviour/config/movements/ss35.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..bd11003b15907ac4d788f6361d8092b7d9effb55
--- /dev/null
+++ b/src/robot_behaviour/config/movements/ss35.yaml
@@ -0,0 +1,24 @@
+play_motion:
+ motions:
+ ss35:
+ joints: [arm_1_joint, arm_2_joint, arm_3_joint, arm_4_joint, arm_5_joint, arm_6_joint, arm_7_joint]
+ points:
+ - time_from_start: 0
+ positions: [0.5516, 0.3872, -1.4816, 1.4506, -1.7999, 0.5705, -1.5773]
+ #go to p33
+ - time_from_start: 3
+ positions: [0.7119, 0.4834, -1.2491, 1.3931, -1.6315, 0.5706, -1.5775]
+ #go to p35
+ - time_from_start: 5
+ positions: [0.5399, 0.4803, -1.1818, 1.298, -1.6315, 0.5706, -1.5775]
+ #go to p33
+ - time_from_start: 7
+ positions: [0.7119, 0.4834, -1.2491, 1.3931, -1.6315, 0.5706, -1.5775]
+ #come back
+ - time_from_start: 10
+ positions: [0.2269, 0.3316, -1.6138, 2.2556, -1.6316, 0.5703, -1.5773]
+ meta:
+ name: ss35
+ usage: ss35
+ description: suggest a subset of solution from p35
+
diff --git a/src/robot_behaviour/config/sentences/sentences_en_GB b/src/robot_behaviour/config/sentences/sentences_en_GB
index e50519d4fd3421869e896bd010588018da730b93..c06b7e494e9af1267d4bebe2bdc2e9661762744b 100644
--- a/src/robot_behaviour/config/sentences/sentences_en_GB
+++ b/src/robot_behaviour/config/sentences/sentences_en_GB
@@ -17,12 +17,12 @@
"Dont be afraid to make a mistake. Do it!! "],
'lev_2':["The solution is on row ",
"Look at row "],
-'lev_3':["I'm here to help you The solution can be one of those ",
+'lev_3':["I'm here to help you; The solution can be one of those ",
"Why do not try with one of those;"],
'lev_4':["The right token is this one ; ; ; :",
"The right token to move is ; ; ; :",
"It's not so difficult. the right token to move is ; ; ; :"],
-'lev_5':["The token to move is this one.",
+'lev_5':["I will give you the token to move:","The token to move is this one.",
"Take it . and move in the right location"],
'pick_ok':["Yep", "Ok", "Great"],
'pick_no':["Really???", "Are you sure?", "No"]}
\ No newline at end of file
diff --git a/src/robot_behaviour/gesture_reproducer.py b/src/robot_behaviour/gesture_reproducer.py
index 4afb071955971ccda25e69956742ad6632188aac..518148d5743bdfa11d333fca287c2a776d61526d 100644
--- a/src/robot_behaviour/gesture_reproducer.py
+++ b/src/robot_behaviour/gesture_reproducer.py
@@ -283,6 +283,11 @@ class Gesture:
state = self.client.get_state()
if action_ok:
rospy.loginfo("Action finished succesfully with state: " + str(self.get_status_string(state)))
+ if str(self.get_status_string(state)) == "REJECTED":
+ self.client.cancel_goal()
+ self.client.send_goal(goal)
+ rospy.loginfo("Waiting for result...")
+ action_ok = self.client.wait_for_result(rospy.Duration(30.0))
if action_ok:
activate = self.activate_magnet()
if activate == False:
@@ -477,11 +482,13 @@ class Gesture:
rospy.loginfo("Sending goal with motion: " + "p" + str(conv_cell))
self.client.send_goal(goal)
rospy.loginfo("Waiting for result...")
+ print(self.client.get_state())
action_ok = self.client.wait_for_result(rospy.Duration(30.0))
state = self.client.get_state()
reproduce_text(text_0)
-
+ print(self.client.get_state())
+ #replace action_ok with get_state ==3
if action_ok:
rospy.loginfo("Action finished succesfully with state: " + str(self.get_status_string(state)))
if action_ok:
diff --git a/src/robot_behaviour/robot_reproducer.py b/src/robot_behaviour/robot_reproducer.py
index 0ea76033aae4c84481d87873302e2c8ef4abfcdb..48358f1aa6c7c9a00d03c752985676093c60d8f0 100644
--- a/src/robot_behaviour/robot_reproducer.py
+++ b/src/robot_behaviour/robot_reproducer.py
@@ -45,6 +45,9 @@ class Robot:
"lev_5": self.offer_solution
}
+ def send_to_rest(self):
+ self.gesture.initial_pos()
+
def load_sentences(self, file):
file = open(file, "r")
@@ -141,7 +144,7 @@ class Robot:
self.speech.text_to_speech(self.sentences['user_move_back'][counter])
rospy.sleep(0.1)
self.face.reproduce_face_expression(facial_expression)
- success = self.gesture.pick_and_place(token_from, token_to)
+ success = self.gesture.pick_and_place(token_to, token_from)
self.gesture.initial_pos()
b_executed = True
@@ -185,17 +188,10 @@ class Robot:
'''
print("Lev_0")
b_executed = False
- if self.face!=None and self.gesture==None:
- self.speech.text_to_speech(self.sentences['lev_0'][counter])
- rospy.sleep(0.1)
- self.face.reproduce_face_expression(facial_expression)
- b_executed = True
- elif self.face == None and self.gesture != None:
- '''N.B For this kind of action we did not plan to move the robot'''
- assert "Error Lev 0 does not contemplate any gesture"
- else:
- '''N.B For this kind of action we did not plan to move the robot'''
- assert "Error Lev 0 does not contemplate any gesture"
+ self.speech.text_to_speech(self.sentences['lev_0'][counter])
+ rospy.sleep(0.1)
+ self.face.reproduce_face_expression(facial_expression)
+ b_executed = True
return b_executed
def encouragement(self, counter, facial_expression, delay_for_speech):
@@ -208,15 +204,9 @@ class Robot:
'''
print("Lev_1")
b_executed = False
- if self.face!=None and self.gesture==None:
- self.speech.text_to_speech(self.sentences['lev_1'][counter]+self.sentences['lev_0'][0])
- rospy.sleep(0.1)
- self.face.reproduce_face_expression(facial_expression)
- b_executed = True
- elif self.face==None and self.gesture!=None:
- assert "Error Lev 1 does not contemplate any gesture"
- else:
- assert "Error Lev 1 does not contemplate any gesture"
+ self.speech.text_to_speech(self.sentences['lev_1'][counter]+";;;;"+self.sentences['lev_0'][0])
+ rospy.sleep(0.1)
+ b_executed = True
return b_executed
@@ -357,6 +347,7 @@ class Robot:
self.speech.text_to_speech(self.sentences['lev_5'][0])
self.face.reproduce_face_expression(facial_expression)
self.gesture.offer_token(token_sol_from, self.reproduce_sentence, self.sentences['lev_5'][1], self.sentences['lev_5'][2])
+ self.gesture.initial_pos()
b_executed = True
return b_executed
@@ -384,6 +375,7 @@ class Robot:
self.speech.text_to_speech(self.sentences['max_attempt'][counter])
rospy.sleep(0.1)
self.gesture.pick_and_place(token_sol_from, token_sol_to)
+ self.gesture.initial_pos()
b_executed = True
else:
# TODO: test on the robot
@@ -391,6 +383,7 @@ class Robot:
rospy.sleep(0.1)
self.face.reproduce_face_expression(facial_expression)
self.gesture.pick_and_place(token_sol_from, token_sol_to)
+ self.gesture.initial_pos()
b_executed = True
def pick(self, positive, counter, facial_expression):
@@ -498,8 +491,8 @@ def main():
token_to = ""
who = "robot"
row = 3
- tokens = [("111",13), ("256", 15), ("341", 18)]
- token = ("111", 14, 13)
+ tokens = [("111",8), ("256", 9), ("341", 10)]
+ token = ("111", 8, 2)
positive = False
lev_id = 3
#robot.fake_function(robot.play_sentence)
@@ -510,8 +503,9 @@ def main():
#robot.action["move_back"].__call__(who, token)
#robot.action["lev_0"].__call__()
#rospy.sleep(3.0)
- robot.assistance(lev_id=5, row=2, counter=4, token=token, facial_expression=face, tokens=tokens, delay_for_speech=3)
-
+ #robot.assistance(lev_id=5, row=2, counter=0, token=token, facial_expression=face, tokens=tokens, delay_for_speech=1)
+ #robot.move_onbehalf(token, counter, face)
+ robot.move_token_back(who=robot, token=token, counter=counter, facial_expression=face)
#robot.action["assistance"].__call__(lev_id, row, counter, token, *tokens)
#robot.action["lev_2"].__call__(row, counter)
#robot.action["lev_3"].__call__(counter, token, *tokens)
diff --git a/src/robot_behaviour/speech_reproducer.py b/src/robot_behaviour/speech_reproducer.py
index fdc3180c80b574bf72837bd277afa8538334f910..f929b2c71d6ef7768590e08dae70533af2a9b9f2 100755
--- a/src/robot_behaviour/speech_reproducer.py
+++ b/src/robot_behaviour/speech_reproducer.py
@@ -62,11 +62,17 @@ class Speech():
if __name__ == "__main__":
speech = Speech("en_GB")
text = "The solution is 112"
- speech.text_to_speech(text, False, 0)
- t0 = time.time()
- elapsed_time = 0
- while(elapsed_time<5):
- elapsed_time = time.time()-t0
- print("Reproduction has ended? ",speech.reproduction_has_ended)
-
+ speech.text_to_speech(text, True)
+ print(speech.reproduction_has_ended)
+ speech.reproduction_has_ended = False
+ print(speech.reproduction_has_ended)
+ if speech.reproduction_has_ended == False:
+ speech.text_to_speech(text, True)
+ print(speech.reproduction_has_ended)
+ #
+ # t0 = time.time()
+ # elapsed_time = 0
+ # while(elapsed_time<5):
+ # elapsed_time = time.time()-t0
+ # print("Reproduction has ended? ",speech.reproduction_has_ended)