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>
namespace {
inline std::string int_to_str(int i) {
template<typename T> inline std::string stuff_to_str(T i) {
std::ostringstream ss;
ss << i;
return ss.str();

18
launch/fulltest.launch
View File

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

23
script/controller.py
View File

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

5
script/imitator.py
View File

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

17
script/walker.py
View File

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

7
src/aruco_detector.cpp
View File

@@ -25,12 +25,16 @@ class ImageConverter {
ImageConverter() :
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,
&ImageConverter::imageCb, this);
// Create output windows
cv::namedWindow(ARUCO_WINDOW);
ROS_INFO("ARUCO: UP");
}
~ImageConverter() {
@@ -115,6 +119,7 @@ class ImageConverter {
int main(int argc, char** argv) {
ros::init(argc, argv, "aruco_detector");
ROS_INFO("ARUCO: STARTING");
ImageConverter ic;
ros::spin();
return 0;

34
src/speech.cpp
View File

@@ -66,13 +66,12 @@ public:
&Nao_control::speechRecognitionCallback, this);
ic = nh_.serviceClient<teleoperation::InformController>(
"/inform_controller",5);
"/inform_controller", false);
}
~Nao_control() {
ROS_INFO("Destructor");
//stopping recognition
ROS_INFO("SPEECH: DESTRUCT");
std_srvs::Empty srv;
@@ -90,8 +89,7 @@ public:
ROS_INFO("A WORD WAS RECOGNIZED");
std_srvs::Empty srv;
ROS_INFO("Confidence Value:");
std::cout << int_to_str(msg->confidence_values[0]) << std::endl;
ROS_INFO("CONFIDENCE: %lf", msg->confidence_values[0]);
for (int i = 0; i < msg->words.size(); i++) {
std::cout << msg->words[i] << std::endl;
@@ -113,7 +111,7 @@ public:
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);
s_msg.goal_id.id = stuff_to_str(this->speech_id_ctr);
this->speech_id_ctr += 1;
s_msg.goal.say = say;
this->speech_pub.publish(s_msg);
@@ -125,6 +123,9 @@ public:
if (this->ic.call(ic_msg) && ic_msg.response.permission) {
this->imitating = true;
}
else {
ROS_ERROR("SPEECH: CONTROLLER UNREACHABLE");
}
}
else if (msg->words[0] == "stop") {
ROS_INFO("SPEECH: REQUESTING STOP IMITATION");
@@ -132,6 +133,9 @@ public:
if (this->ic.call(ic_msg) && ic_msg.response.permission) {
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";
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;
s_msg.goal.say = say;
this->speech_pub.publish(s_msg);
@@ -171,11 +175,12 @@ public:
else vocabulary.push_back("stop");
vocabulary.push_back("open");
vocabulary.push_back("close");
// vocabulary.push_back("kill");
naoqi_bridge_msgs::SetSpeechVocabularyActionGoal msg;
msg.goal.words = vocabulary;
msg.goal_id.id = int_to_str(speech_id_ctr);
std::cout << msg << std::endl;
msg.goal_id.id = stuff_to_str(speech_id_ctr);
std::cout << msg.goal << std::endl;
speech_id_ctr += 1;
voc_params_pub.publish(msg);
ROS_INFO("VOCABULARY INITIALIZED");
@@ -184,6 +189,16 @@ public:
void commandRecognition()
{
//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();
ros::Rate loop_rate(1);
loop_rate.sleep();
@@ -202,7 +217,6 @@ int main(int argc, char** argv) {
ros::init(argc, argv, "speech");
Nao_control TermiNAOtor;
//TermiNAOtor.initializeVocabulary();
ros::Rate loop_rate(1);
loop_rate.sleep();
TermiNAOtor.commandRecognition();