pavel/teleoperation
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

did some work to assemble it together

Browse Source
This commit is contained in:
Pavel Lutskov
2019-01-28 17:34:55 +01:00
parent dc88c5259b
commit 2d0441306a
6 changed files with 76 additions and 38 deletions
Download Patch File Download Diff File Expand all files Collapse all files

16
launch/fulltest.launch Normal file
View File

@@ -0,0 +1,16 @@
<?xml version="1.0"?>
<!--Launch file for Test of teleoperating NAO project-->
<launch>
<!-- speech recognition and response node-->
<node name="usb_cam_node" pkg="usb_cam" type="usb_cam_node"/>
<node name="controller" pkg="teleoperation" type="controller.py"/>
<node name="speech" pkg="teleoperation" type="speech"/>
<node name="aruco_detector" pkg="teleoperation" type="aruco_detector"/>
<node name="walker" pkg="teleoperation" type="walker.py"/>
<node name="imitator" pkg="teleoperation" type="imitator.py"/>
<node name="fall_detector" pkg="teleoperation" type="fall_detector.py"/>
</launch>

9
launch/teleoperation.launch
View File

@@ -20,12 +20,15 @@
<node name="speech" pkg="teleoperation" type="speech"/>
<!-- open rviz window showing nao -->
<node name="rviz_back" pkg="rviz" type="rviz" args = " -d $(find teleoperation)/launch/rviz_config/nao.rviz"/>
<node name="rviz_back" pkg="rviz" type="rviz" args =
" -d $(find teleoperation)/launch/rviz_config/nao.rviz"/>
<!-- open rviz window showing nao from its right side -->
<node name="rviz_right" pkg="rviz" type="rviz" args = " -d $(find teleoperation)/launch/rviz_config/nao_right.rviz"/>
<node name="rviz_right" pkg="rviz" type="rviz"
args=" -d $(find teleoperation)/launch/rviz_config/nao_right.rviz"/>
<!-- open rviz window showing human -->
<node name="rviz_human" pkg="rviz" type="rviz" args = " -d $(find teleoperation)/launch/rviz_config/human.rviz"/>
<node name="rviz_human" pkg="rviz" type="rviz"
args=" -d $(find teleoperation)/launch/rviz_config/human.rviz"/>
</launch>

11
script/controller.py
View File

@@ -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()

7
script/imitator.py
View File

@@ -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)

65
script/walker.py
View File

@@ -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()

6
src/speech.cpp
View File

@@ -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;