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

before merge

Browse Source
This commit is contained in:
Pavel Lutskov
2019-01-28 14:59:10 +01:00
parent f10e9d3ca7
commit bf217b6ac6
7 changed files with 92 additions and 58 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
CMakeLists.txt
View File

@@ -22,10 +22,12 @@ add_service_files(
generate_messages(DEPENDENCIES std_msgs)
catkin_package(
CATKIN_DEPENDS message_runtime
INCLUDE_DIRS include
)
include_directories(
${catkin_INCLUDE_DIRS}
include
${catkin_INCLUDE_DIRS}
)
catkin_install_python(PROGRAMS
@@ -39,3 +41,4 @@ catkin_install_python(PROGRAMS
add_executable(aruco_detector src/aruco_detector.cpp)
add_executable(speech src/speech.cpp)
target_link_libraries(aruco_detector ${catkin_LIBRARIES} ${aruco_LIB})
target_link_libraries(speech ${catkin_LIBRARIES} ${aruco_LIB})

14
include/teleoperation/utils.hpp Normal file
View File

@@ -0,0 +1,14 @@
#ifndef _UTILS_HPP_
#define _UTILS_HPP_
#include <sstream>
namespace {
inline std::string int_to_str(int i) {
std::ostringstream ss;
ss << i;
return ss.str();
}
}
#endif

4
launch/teleoperation.launch
View File

@@ -7,14 +7,14 @@ Launch file for Teleoperating NAO project
<launch>
<!-- roscore is automatically started if none exists-->
<!-- condition for rviz to be started at launch-->
<?ignore
<arg name="gui" default="false" />
<param name="use_gui" value="$(arg gui)"/>
?>
?>
<!-- aruco detector node-->
<node pkg="teleoperation" type="aruco_detector" name="aruco_detector_node"/>

14
script/controller.py
View File

@@ -51,10 +51,20 @@ def handle_request(r):
permission = True
else:
permission = False
elif message == 'stop':
elif module == 'speech_recognition':
if message == 'imitate':
if STATE == 'idle':
STATE = 'imitate'
permission = True
else:
permission = False
if message == 'stop':
if STATE == 'imitate':
STATE = 'idle'
permission = True
permission = True
else:
permission = False
print 'Got request from %s to %s. Permission: %s. State is now: %s.' % (
module, message, permission, STATE

11
script/imitator.py
View File

@@ -20,11 +20,13 @@ if __name__ == '__main__':
ll = tf.TransformListener()
mp = ALProxy('ALMotion', os.environ['NAO_IP'], 9559)
mp.wakeUp()
mp.setAngles(['LElbowRoll', 'RElbowRoll'],
[radians(-50), radians(50)], 0.5)
mp.setAngles(['LElbowRoll', 'RElbowRoll', 'LElbowYaw', 'RElbowYaw'],
[radians(-70), radians(70), -radians(40), radians(40)], 0.5)
while not rospy.is_shutdown():
sleep(0.1)
if not _inform_controller('imitate'):
continue
for i, eff in enumerate(['L',
'R'], 1):
try:
@@ -36,9 +38,6 @@ if __name__ == '__main__':
except Exception as e:
print e
continue
permission = _inform_controller('imitate')
if not permission:
continue
sign = 1 if eff == 'L' else -1
roll = asin(trans[1] /
sqrt(trans[0]**2 + trans[1]**2 + trans[2]**2))
@@ -48,6 +47,7 @@ if __name__ == '__main__':
# sign = 1 if roll > 0 else -1
# roll -= sign * radians(10)
# print degrees(roll)
mp.setAngles([
'{}ShoulderRoll'.format(eff),
'{}ShoulderPitch'.format(eff)
@@ -55,6 +55,7 @@ if __name__ == '__main__':
roll,
pitch
], 0.3)
# targ = np.array(trans)
# targ = targ / np.linalg.norm(targ) * 0.3
# mp.setPositions(eff, _FRAME_TORSO,

1
script/walker.py
View File

@@ -24,6 +24,7 @@ if __name__ == '__main__':
mp = ALProxy('ALMotion', os.environ['NAO_IP'], 9559)
mp.wakeUp()
mp.moveInit()
mp.setMoveArmsEnabled(False, False)
while not rospy.is_shutdown():
sleep(0.3)

101
src/speech.cpp
View File

@@ -12,6 +12,9 @@
#include <std_msgs/Bool.h>
#include <std_msgs/String.h>
#include <teleoperation/InformController.h>
#include <teleoperation/utils.hpp>
using namespace std;
class Nao_control {
@@ -37,18 +40,18 @@ protected:
ros::Subscriber recog_sub;
// Publisher for recognized commands
ros::Publisher command_pub;
ros::ServiceClient ic;
int speech_id_ctr = 1;
std::vector<std::string> recognized_words;
bool commandfound = false;
bool imitating;
int speech_id_ctr;
public:
Nao_control(){
Nao_control() : imitating(false), speech_id_ctr(1) {
ROS_INFO("Constructor");
speech_pub = nh_.advertise<naoqi_bridge_msgs::SpeechWithFeedbackActionGoal>(
"/speech_action/goal", 1);
@@ -62,8 +65,8 @@ public:
recog_sub=nh_.subscribe("/word_recognized", 1,
&Nao_control::speechRecognitionCallback, this);
command_pub = nh_.advertise<std_msgs::String>("/recieved_commands",5);
ic = nh_.serviceClient<teleoperation::InformController>(
"/inform_controller",5);
}
~Nao_control() {
@@ -72,7 +75,7 @@ public:
//stopping recognition
std_srvs::Empty srv;
if (recog_stop_srv.call(srv)) {
ROS_INFO("SUCCESSFULLY STOPPED RECOGNITION");
}
@@ -81,93 +84,95 @@ public:
}
}
void speechRecognitionCallback(const naoqi_bridge_msgs::WordRecognized::ConstPtr& msg)
{
void speechRecognitionCallback(
const naoqi_bridge_msgs::WordRecognized::ConstPtr& msg) {
ROS_INFO("A WORD WAS RECOGNIZED");
std_srvs::Empty srv;
ROS_INFO("Confidence Value:");
std::cout << std::to_string(msg->confidence_values[0]) << std::endl;
std::cout << int_to_str(msg->confidence_values[0]) << std::endl;
for (int i = 0; i < msg->words.size(); i++) {
std::cout << msg->words[i] << std::endl;
}
//set pause duration
double f_pause = 1;
if (recog_stop_srv.call(srv) && ((msg->words.size())> 0)) {
ROS_INFO("SUCCESSFULLY STOPPED RECOGNITION");
// Use confidence level to decide wether the recognized word should be published
// Use confidence level to decide wether the recognized word
// should be published
if (msg->confidence_values[0] > 0.35) {
ROS_INFO("SPEECH STARTING");
std::string say = "Ok I understood";
naoqi_bridge_msgs::SpeechWithFeedbackActionGoal s_msg;
s_msg.goal_id.id = std::to_string(this->speech_id_ctr);
s_msg.goal_id.id = int_to_str(this->speech_id_ctr);
this->speech_id_ctr += 1;
s_msg.goal.say = say;
this->speech_pub.publish(s_msg);
this->recognized_words.clear();
teleoperation::InformController ic_msg;
ic_msg.request.module = "speech_recognition";
if (msg->words[0] == "imitate") {
ic_msg.request.message = "imitate";
if (this->ic.call(ic_msg) && ic_msg.response.permission) {
this->imitating = true;
}
}
else if (msg->words[0] == "stop") {
ic_msg.request.message = "stop";
if (this->ic.call(ic_msg) && ic_msg.response.permission) {
this->imitating = false;
}
}
// publish here
ROS_INFO("PUBLISH");
std_msgs::String c_msg;
c_msg.data = msg->words[0];
this->command_pub.publish(c_msg);
}
else if(msg->confidence_values[0] > 0.05) {
else if (msg->confidence_values[0] > 0.05) {
ROS_INFO("SPEECH STARTING");
std::string say = "I did not understand. Could you repeat that please";
naoqi_bridge_msgs::SpeechWithFeedbackActionGoal s_msg;
s_msg.goal_id.id = std::to_string(this->speech_id_ctr);
s_msg.goal_id.id = int_to_str(this->speech_id_ctr);
this->speech_id_ctr += 1;
s_msg.goal.say = say;
this->speech_pub.publish(s_msg);
this->recognized_words.clear();
// increase pause duration
f_pause = 0.4;
}
this->recognized_words.clear();
}
}
else {
ROS_ERROR("COULDN'T STOP RECOGNITION");
}
}
// pause until NAO stops talking
ros::Rate loop_rate(f_pause);
loop_rate.sleep();
// re-start recogntion
commandRecognition();
commandRecognition();
}
void initializeVocabulary()
{
std::vector<std::string> vocabulary;
vocabulary.push_back("Copy postion");
vocabulary.push_back("Stop");// Operation ");
//vocabulary.push_back("hello ");
vocabulary.push_back("Open your hands");
vocabulary.push_back("Close your hands");
if (!this->imitating) vocabulary.push_back("imitate");
else vocabulary.push_back("stop");
vocabulary.push_back("open");
vocabulary.push_back("close");
naoqi_bridge_msgs::SetSpeechVocabularyActionGoal msg;
msg.goal.words = vocabulary;
msg.goal_id.id = std::to_string(speech_id_ctr);
msg.goal_id.id = int_to_str(speech_id_ctr);
std::cout << msg << std::endl;
speech_id_ctr += 1;
voc_params_pub.publish(msg);
@@ -177,7 +182,7 @@ public:
void commandRecognition()
{
//recognition has to be started and ended once a valid command was found
initializeVocabulary();
std_srvs::Empty srv;
@@ -186,13 +191,13 @@ public:
}
else {
ROS_ERROR("COULDN'T START RECOGNITION");
}
}
}
};
int main(int argc, char** argv) {
ros::init(argc, argv, "speech");
Nao_control TermiNAOtor;
ros::Rate loop_rate(0.5);
loop_rate.sleep();