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

everything seems to work together as expected

Browse Source 
(if not perfectly)
This commit is contained in:
Pavel Lutskov
2019-01-29 12:07:17 +01:00
parent 2d0441306a
commit b6e7ab8aa4
7 changed files with 71 additions and 43 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
include/teleoperation/utils.hpp
View File

@@ -4,7 +4,7 @@
#include <sstream> #include <sstream>
namespace { namespace {
inline std::string int_to_str(int i) { template<typename T> inline std::string stuff_to_str(T i) {
std::ostringstream ss; std::ostringstream ss;
ss << i; ss << i;
return ss.str(); return ss.str();

18
launch/fulltest.launch
View File

@@ -3,14 +3,20 @@
<!--Launch file for Test of teleoperating NAO project--> <!--Launch file for Test of teleoperating NAO project-->
<launch> <launch>
<include file="$(find nao_apps)/launch/speech.launch"/>
<!-- speech recognition and response node--> <!--
<node name="usb_cam_node" pkg="usb_cam" type="usb_cam_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="controller" pkg="teleoperation" type="controller.py"
<node name="aruco_detector" pkg="teleoperation" type="aruco_detector"/> output="screen"/>
<node name="speech" pkg="teleoperation" type="speech" output="screen"/>
<node name="aruco_detector" pkg="teleoperation" type="aruco_detector"
output="screen"/>
<!--
<node name="walker" pkg="teleoperation" type="walker.py"/> <node name="walker" pkg="teleoperation" type="walker.py"/>
<node name="imitator" pkg="teleoperation" type="imitator.py"/> -->
<node name="imitator" pkg="teleoperation" type="imitator.py"
output="screen"/>
<node name="fall_detector" pkg="teleoperation" type="fall_detector.py"/> <node name="fall_detector" pkg="teleoperation" type="fall_detector.py"/>
</launch> </launch>

25
script/controller.py
View File

@@ -23,6 +23,8 @@ def inform_controller_factory(who):
def handle_request(r): def handle_request(r):
module = r.module module = r.module
message = r.message message = r.message
_state_old = STATE
permission = False
global STATE global STATE
if module == 'walker': if module == 'walker':
@@ -30,12 +32,10 @@ def handle_request(r):
if STATE in ('idle', 'walk'): if STATE in ('idle', 'walk'):
STATE = 'walk' STATE = 'walk'
permission = True permission = True
else:
permission = False
elif message == 'stop': elif message == 'stop':
if STATE == 'walk': if STATE == 'walk':
STATE = 'idle' STATE = 'idle'
permission = True permission = True
elif module == 'fall_detector': elif module == 'fall_detector':
permission = True permission = True
@@ -48,28 +48,27 @@ def handle_request(r):
if message == 'imitate': if message == 'imitate':
if STATE == 'imitate': if STATE == 'imitate':
permission = True permission = True
else:
permission = False
elif module == 'speech_recognition': elif module == 'speech_recognition':
if message == 'imitate': if message == 'recognize':
if STATE in ('idle', 'imitate'):
permission = True
elif message == 'imitate':
if STATE == 'idle': if STATE == 'idle':
STATE = 'imitate' STATE = 'imitate'
permission = True permission = True
else: elif message == 'stop':
permission = False
if message == 'stop':
if STATE == 'imitate': if STATE == 'imitate':
STATE = 'idle' STATE = 'idle'
permission = True permission = True
else:
permission = False
rospy.loginfo( rospy.logdebug(
'Got request from %s to %s. Permission: %s. State is now: %s.' % ( 'GOT REQUEST FROM %s TO %s.\nPERMISSION: %s.\nSTATE IS NOW: %s.' % (
module, message, permission, STATE module, message, permission, STATE
) )
) )
if _state_old != STATE:
rospy.loginfo('{} -> {}'.format(_state_old, STATE))
return InformControllerResponse(permission) return InformControllerResponse(permission)

5
script/imitator.py
View File

@@ -18,6 +18,8 @@ if __name__ == '__main__':
rospy.init_node('imitator') rospy.init_node('imitator')
rospy.wait_for_service('inform_controller') rospy.wait_for_service('inform_controller')
ll = tf.TransformListener() ll = tf.TransformListener()
am = ALProxy('ALAutonomousMoves', os.environ['NAO_IP'], 9559)
am.setExpressiveListeningEnabled(False)
mp = ALProxy('ALMotion', os.environ['NAO_IP'], 9559) mp = ALProxy('ALMotion', os.environ['NAO_IP'], 9559)
mp.wakeUp() mp.wakeUp()
mp.setAngles(['LElbowRoll', 'RElbowRoll', 'LElbowYaw', 'RElbowYaw'], mp.setAngles(['LElbowRoll', 'RElbowRoll', 'LElbowYaw', 'RElbowYaw'],
@@ -27,7 +29,7 @@ if __name__ == '__main__':
sleep(0.1) sleep(0.1)
if not _inform_controller('imitate'): if not _inform_controller('imitate'):
continue continue
rospy.loginfo('IMITATOR: ACTIVE') rospy.logdebug('IMITATOR: ACTIVE')
for i, eff in enumerate(['L', for i, eff in enumerate(['L',
'R'], 1): 'R'], 1):
try: try:
@@ -37,6 +39,7 @@ if __name__ == '__main__':
rospy.Time(0) rospy.Time(0)
) )
except Exception as e: except Exception as e:
rospy.logwarn(e)
continue continue
sign = 1 if eff == 'L' else -1 sign = 1 if eff == 'L' else -1

23
script/walker.py
View File

@@ -57,12 +57,6 @@ if __name__ == '__main__':
# print trans # print trans
# continue # continue
permission = _inform_controller('move')
if not permission:
mp.stopMove()
continue
movement = [0, 0, 0] movement = [0, 0, 0]
#-1 1 -1 1 #-1 1 -1 1
for i, dr in enumerate((BK, FW, RT, LT)): for i, dr in enumerate((BK, FW, RT, LT)):
@@ -74,12 +68,19 @@ if __name__ == '__main__':
break break
if not any(movement): if not any(movement):
rospy.loginfo('WALKER: STOP') rospy.logdebug('WALKER: STOP')
_inform_controller('stop') _inform_controller('stop')
mp.move(0, 0, 0) mp.move(0, 0, 0)
else: continue
rospy.loginfo('WALKER: TRANS: {}'.format(trans))
rospy.loginfo('WALKER: MOVMT: {}'.format(movement)) permission = _inform_controller('move')
mp.moveToward(*movement)
if not permission:
mp.stopMove()
continue
rospy.loginfo('WALKER: TRANS: {}'.format(trans))
rospy.loginfo('WALKER: MOVMT: {}'.format(movement))
mp.moveToward(*movement)
mp.rest() mp.rest()

7
src/aruco_detector.cpp
View File

@@ -25,12 +25,16 @@ class ImageConverter {
ImageConverter() : ImageConverter() :
it_(nh_) it_(nh_)
{ {
// Subscrive to input video feed ros::Rate loop_rate(0.1);
loop_rate.sleep();
// Subscribe to input video feed
image_sub_ = it_.subscribe("/usb_cam/image_raw", 1, image_sub_ = it_.subscribe("/usb_cam/image_raw", 1,
&ImageConverter::imageCb, this); &ImageConverter::imageCb, this);
// Create output windows // Create output windows
cv::namedWindow(ARUCO_WINDOW); cv::namedWindow(ARUCO_WINDOW);
ROS_INFO("ARUCO: UP");
} }
~ImageConverter() { ~ImageConverter() {
@@ -115,6 +119,7 @@ class ImageConverter {
int main(int argc, char** argv) { int main(int argc, char** argv) {
ros::init(argc, argv, "aruco_detector"); ros::init(argc, argv, "aruco_detector");
ROS_INFO("ARUCO: STARTING");
ImageConverter ic; ImageConverter ic;
ros::spin(); ros::spin();
return 0; return 0;

34
src/speech.cpp
View File

@@ -66,13 +66,12 @@ public:
&Nao_control::speechRecognitionCallback, this); &Nao_control::speechRecognitionCallback, this);
ic = nh_.serviceClient<teleoperation::InformController>( ic = nh_.serviceClient<teleoperation::InformController>(
"/inform_controller",5); "/inform_controller", false);
} }
~Nao_control() { ~Nao_control() {
ROS_INFO("Destructor"); ROS_INFO("SPEECH: DESTRUCT");
//stopping recognition
std_srvs::Empty srv; std_srvs::Empty srv;
@@ -90,8 +89,7 @@ public:
ROS_INFO("A WORD WAS RECOGNIZED"); ROS_INFO("A WORD WAS RECOGNIZED");
std_srvs::Empty srv; std_srvs::Empty srv;
ROS_INFO("Confidence Value:"); ROS_INFO("CONFIDENCE: %lf", msg->confidence_values[0]);
std::cout << int_to_str(msg->confidence_values[0]) << std::endl;
for (int i = 0; i < msg->words.size(); i++) { for (int i = 0; i < msg->words.size(); i++) {
std::cout << msg->words[i] << std::endl; std::cout << msg->words[i] << std::endl;
@@ -113,7 +111,7 @@ public:
std::string say = "Ok I understood " + msg->words[0]; std::string say = "Ok I understood " + msg->words[0];
naoqi_bridge_msgs::SpeechWithFeedbackActionGoal s_msg; naoqi_bridge_msgs::SpeechWithFeedbackActionGoal s_msg;
s_msg.goal_id.id = int_to_str(this->speech_id_ctr); s_msg.goal_id.id = stuff_to_str(this->speech_id_ctr);
this->speech_id_ctr += 1; this->speech_id_ctr += 1;
s_msg.goal.say = say; s_msg.goal.say = say;
this->speech_pub.publish(s_msg); this->speech_pub.publish(s_msg);
@@ -125,6 +123,9 @@ public:
if (this->ic.call(ic_msg) && ic_msg.response.permission) { if (this->ic.call(ic_msg) && ic_msg.response.permission) {
this->imitating = true; this->imitating = true;
} }
else {
ROS_ERROR("SPEECH: CONTROLLER UNREACHABLE");
}
} }
else if (msg->words[0] == "stop") { else if (msg->words[0] == "stop") {
ROS_INFO("SPEECH: REQUESTING STOP IMITATION"); ROS_INFO("SPEECH: REQUESTING STOP IMITATION");
@@ -132,6 +133,9 @@ public:
if (this->ic.call(ic_msg) && ic_msg.response.permission) { if (this->ic.call(ic_msg) && ic_msg.response.permission) {
this->imitating = false; this->imitating = false;
} }
else {
ROS_ERROR("SPEECH: CONTROLLER UNREACHABLE");
}
} }
} }
@@ -141,7 +145,7 @@ public:
std::string say = "I did not understand. Could you repeat that please"; std::string say = "I did not understand. Could you repeat that please";
naoqi_bridge_msgs::SpeechWithFeedbackActionGoal s_msg; naoqi_bridge_msgs::SpeechWithFeedbackActionGoal s_msg;
s_msg.goal_id.id = int_to_str(this->speech_id_ctr); s_msg.goal_id.id = stuff_to_str(this->speech_id_ctr);
this->speech_id_ctr += 1; this->speech_id_ctr += 1;
s_msg.goal.say = say; s_msg.goal.say = say;
this->speech_pub.publish(s_msg); this->speech_pub.publish(s_msg);
@@ -171,11 +175,12 @@ public:
else vocabulary.push_back("stop"); else vocabulary.push_back("stop");
vocabulary.push_back("open"); vocabulary.push_back("open");
vocabulary.push_back("close"); vocabulary.push_back("close");
// vocabulary.push_back("kill");
naoqi_bridge_msgs::SetSpeechVocabularyActionGoal msg; naoqi_bridge_msgs::SetSpeechVocabularyActionGoal msg;
msg.goal.words = vocabulary; msg.goal.words = vocabulary;
msg.goal_id.id = int_to_str(speech_id_ctr); msg.goal_id.id = stuff_to_str(speech_id_ctr);
std::cout << msg << std::endl; std::cout << msg.goal << std::endl;
speech_id_ctr += 1; speech_id_ctr += 1;
voc_params_pub.publish(msg); voc_params_pub.publish(msg);
ROS_INFO("VOCABULARY INITIALIZED"); ROS_INFO("VOCABULARY INITIALIZED");
@@ -184,6 +189,16 @@ public:
void commandRecognition() void commandRecognition()
{ {
//recognition has to be started and ended once a valid command was found //recognition has to be started and ended once a valid command was found
while (true) {
ros::Rate loop_rate(4);
loop_rate.sleep();
teleoperation::InformController ic_msg;
ic_msg.request.module = "speech_recognition";
ic_msg.request.message = "recognize";
if (this->ic.call(ic_msg) && ic_msg.response.permission) {
break;
}
}
this->initializeVocabulary(); this->initializeVocabulary();
ros::Rate loop_rate(1); ros::Rate loop_rate(1);
loop_rate.sleep(); loop_rate.sleep();
@@ -202,7 +217,6 @@ int main(int argc, char** argv) {
ros::init(argc, argv, "speech"); ros::init(argc, argv, "speech");
Nao_control TermiNAOtor; Nao_control TermiNAOtor;
//TermiNAOtor.initializeVocabulary();
ros::Rate loop_rate(1); ros::Rate loop_rate(1);
loop_rate.sleep(); loop_rate.sleep();
TermiNAOtor.commandRecognition(); TermiNAOtor.commandRecognition();