diff --git a/launch/fulltest.launch b/launch/fulltest.launch
new file mode 100644
index 0000000..6d7042f
--- /dev/null
+++ b/launch/fulltest.launch
@@ -0,0 +1,16 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/launch/teleoperation.launch b/launch/teleoperation.launch
index 73bfaa2..30c745d 100644
--- a/launch/teleoperation.launch
+++ b/launch/teleoperation.launch
@@ -20,12 +20,15 @@
-
+
-
+
-
+
diff --git a/script/controller.py b/script/controller.py
index a3afb2e..4a51536 100755
--- a/script/controller.py
+++ b/script/controller.py
@@ -46,8 +46,7 @@ def handle_request(r):
elif module == 'imitator':
if message == 'imitate':
- if STATE in ('imitate', 'idle'):
- STATE = 'imitate'
+ if STATE == 'imitate':
permission = True
else:
permission = False
@@ -66,13 +65,15 @@ def handle_request(r):
else:
permission = False
- print 'Got request from %s to %s. Permission: %s. State is now: %s.' % (
- module, message, permission, STATE
+ rospy.loginfo(
+ 'Got request from %s to %s. Permission: %s. State is now: %s.' % (
+ module, message, permission, STATE
+ )
)
return InformControllerResponse(permission)
if __name__ == '__main__':
- rospy.init_node('controller')
+ rospy.init_node('controller', log_level=rospy.INFO)
ic = rospy.Service('inform_controller', InformController, handle_request)
rospy.spin()
diff --git a/script/imitator.py b/script/imitator.py
index 3f918d0..47683fc 100755
--- a/script/imitator.py
+++ b/script/imitator.py
@@ -27,6 +27,7 @@ if __name__ == '__main__':
sleep(0.1)
if not _inform_controller('imitate'):
continue
+ rospy.loginfo('IMITATOR: ACTIVE')
for i, eff in enumerate(['L',
'R'], 1):
try:
@@ -36,18 +37,14 @@ if __name__ == '__main__':
rospy.Time(0)
)
except Exception as e:
- print e
continue
+
sign = 1 if eff == 'L' else -1
roll = asin(trans[1] /
sqrt(trans[0]**2 + trans[1]**2 + trans[2]**2))
roll -= sign * radians(25)
pitch = atan(-trans[2] / abs(trans[0]))
- # sign = 1 if roll > 0 else -1
- # roll -= sign * radians(10)
- # print degrees(roll)
-
mp.setAngles([
'{}ShoulderRoll'.format(eff),
'{}ShoulderPitch'.format(eff)
diff --git a/script/walker.py b/script/walker.py
index bd6f93b..f9bf4db 100755
--- a/script/walker.py
+++ b/script/walker.py
@@ -8,15 +8,32 @@ from naoqi import ALProxy
from controller import inform_controller_factory
-FW = -0.32
-BK = -0.55
-LT = -0.55
-RT = 0.0
+
+ #min #max
+FW = 1.65, 1.45
+BK = 2.20, 2.40
+LT = -0.35, -0.53
+RT = 0.35, 0.53
+
+VMIN = 0.3
+VMAX = 1.0
_inform_controller = inform_controller_factory('walker')
+def _speed(pos, interval):
+ int_dir = 1 if interval[1] > interval[0] else -1
+ if int_dir * (pos - interval[0]) < 0:
+ return 0.0
+ elif int_dir * (pos - interval[1]) > 0:
+ return 1.0
+ else:
+ return (VMAX - VMIN) * abs(pos - interval[0]) / (
+ abs(interval[1] - interval[0])
+ ) + VMIN
+
+
if __name__ == '__main__':
rospy.init_node('walker')
rospy.wait_for_service('inform_controller')
@@ -30,37 +47,39 @@ if __name__ == '__main__':
sleep(0.3)
try:
trans, rot = ll.lookupTransform('Aruco_0_frame',
- 'CameraTop_optical_frame',
+ 'odom',
rospy.Time(0))
except Exception as e:
mp.stopMove()
_inform_controller('stop')
continue
- print trans, rot
+ # print trans
# continue
- if (
- BK < trans[2] < FW and
- LT < trans[0] < RT
- # CW < trans[1] < CC
- ):
- _inform_controller('stop')
- mp.move(0, 0, 0)
- continue
-
permission = _inform_controller('move')
+
if not permission:
mp.stopMove()
continue
- if trans[2] < BK: # backwards
- mp.move(-1, 0, 0)
- elif FW < trans[2]: # forwards
- mp.move(1, 0, 0)
- elif RT < trans[0]: # right
- mp.move(0, -1, 0)
- elif trans[0] < LT: # left
- mp.move(0, 1, 0)
+ movement = [0, 0, 0]
+ #-1 1 -1 1
+ for i, dr in enumerate((BK, FW, RT, LT)):
+ idx = i // 2
+ sign = 1 if (i % 2) else -1
+ speed = _speed(trans[idx], dr)
+ if speed:
+ movement[idx] = sign * speed
+ break
+
+ if not any(movement):
+ rospy.loginfo('WALKER: STOP')
+ _inform_controller('stop')
+ mp.move(0, 0, 0)
+ else:
+ rospy.loginfo('WALKER: TRANS: {}'.format(trans))
+ rospy.loginfo('WALKER: MOVMT: {}'.format(movement))
+ mp.moveToward(*movement)
mp.rest()
diff --git a/src/speech.cpp b/src/speech.cpp
index 4e6fc8c..c2cfcc0 100644
--- a/src/speech.cpp
+++ b/src/speech.cpp
@@ -98,7 +98,7 @@ public:
}
//set pause duration
- double f_pause = 1;
+ double f_pause = 2;
if (recog_stop_srv.call(srv) && ((msg->words.size())> 0)) {
@@ -110,7 +110,7 @@ public:
if (msg->confidence_values[0] > 0.35) {
ROS_INFO("SPEECH STARTING");
- std::string say = "Ok I understood";
+ std::string say = "Ok I understood " + msg->words[0];
naoqi_bridge_msgs::SpeechWithFeedbackActionGoal s_msg;
s_msg.goal_id.id = int_to_str(this->speech_id_ctr);
@@ -121,11 +121,13 @@ public:
ic_msg.request.module = "speech_recognition";
if (msg->words[0] == "imitate") {
ic_msg.request.message = "imitate";
+ ROS_INFO("SPEECH: REQUESTING IMITATION");
if (this->ic.call(ic_msg) && ic_msg.response.permission) {
this->imitating = true;
}
}
else if (msg->words[0] == "stop") {
+ ROS_INFO("SPEECH: REQUESTING STOP IMITATION");
ic_msg.request.message = "stop";
if (this->ic.call(ic_msg) && ic_msg.response.permission) {
this->imitating = false;