diff --git a/amigo_skills/src/amigo_skills/amigo.py b/amigo_skills/src/amigo_skills/amigo.py index 81f06406a0..d09f5ef84f 100644 --- a/amigo_skills/src/amigo_skills/amigo.py +++ b/amigo_skills/src/amigo_skills/amigo.py @@ -40,16 +40,16 @@ def __init__(self, wait_services=False): self.add_body_part('ssl', sound_source_localisation.SSL(self.robot_name, self.tf_buffer)) # Human Robot Interaction - self.add_body_part('lights', lights.Lights(self.robot_name, self.tf_buffer)) - self.add_body_part('speech', speech.Speech(self.robot_name, self.tf_buffer, - lambda: self.lights.set_color_colorRGBA(lights.SPEAKING), - lambda: self.lights.set_color_colorRGBA(lights.RESET))) + self.add_body_part('lights', lights.TueLights(self.robot_name, self.tf_buffer)) + self.add_body_part('speech', speech.TueSpeech(self.robot_name, self.tf_buffer, + lambda: self.lights.set_color_rgba_msg(lights.SPEAKING), + lambda: self.lights.set_color_rgba_msg(lights.RESET))) self.add_body_part('hmi', api.Api(self.robot_name, self.tf_buffer, - lambda: self.lights.set_color_colorRGBA(lights.LISTENING), - lambda: self.lights.set_color_colorRGBA(lights.RESET))) + lambda: self.lights.set_color_rgba_msg(lights.LISTENING), + lambda: self.lights.set_color_rgba_msg(lights.RESET))) self.add_body_part('ears', ears.Ears(self.robot_name, self.tf_buffer, - lambda: self.lights.set_color_colorRGBA(lights.LISTENING), - lambda: self.lights.set_color_colorRGBA(lights.RESET))) + lambda: self.lights.set_color_rgba_msg(lights.LISTENING), + lambda: self.lights.set_color_rgba_msg(lights.RESET))) ebutton_class = SimEButton if is_sim_mode() else ebutton.EButton self.add_body_part('ebutton', ebutton_class(self.robot_name, self.tf_buffer)) diff --git a/hero_skills/src/hero_skills/hero.py b/hero_skills/src/hero_skills/hero.py index a781c7da69..7dca8ed91f 100644 --- a/hero_skills/src/hero_skills/hero.py +++ b/hero_skills/src/hero_skills/hero.py @@ -9,6 +9,9 @@ from robot_skills.arm import arms, force_sensor, gripper, handover_detector from robot_skills.simulation import is_sim_mode, SimEButton +# Hero skills +from .tmc_speech import TmcSpeech + class Hero(robot.Robot): """Hero""" @@ -42,16 +45,25 @@ def __init__(self, wait_services=False): camera_base_ns='hero/head_rgbd_sensor')) # Human Robot Interaction - self.add_body_part('lights', lights.Lights(self.robot_name, self.tf_buffer)) - self.add_body_part('speech', speech.Speech(self.robot_name, self.tf_buffer, - lambda: self.lights.set_color_colorRGBA(lights.SPEAKING), - lambda: self.lights.set_color_colorRGBA(lights.RESET))) + self.add_body_part( + 'lights', lights.Lights( + self.robot_name, self.tf_buffer, '/' + self.robot_name + '/rgb_lights_manager/user_set_rgb_lights' + ) + ) + if is_sim_mode(): + self.add_body_part('speech', speech.TueSpeech(self.robot_name, self.tf_buffer, + lambda: self.lights.set_color_rgba_msg(lights.SPEAKING), + lambda: self.lights.set_color_rgba_msg(lights.RESET))) + else: + self.add_body_part('speech', TmcSpeech(self.robot_name, self.tf_buffer, + lambda: self.lights.set_color_rgba_msg(lights.SPEAKING), + lambda: self.lights.set_color_rgba_msg(lights.RESET))) self.add_body_part('hmi', api.Api(self.robot_name, self.tf_buffer, - lambda: self.lights.set_color_colorRGBA(lights.LISTENING), - lambda: self.lights.set_color_colorRGBA(lights.RESET))) + lambda: self.lights.set_color_rgba_msg(lights.LISTENING), + lambda: self.lights.set_color_rgba_msg(lights.RESET))) self.add_body_part('ears', ears.Ears(self.robot_name, self.tf_buffer, - lambda: self.lights.set_color_colorRGBA(lights.LISTENING), - lambda: self.lights.set_color_colorRGBA(lights.RESET))) + lambda: self.lights.set_color_rgba_msg(lights.LISTENING), + lambda: self.lights.set_color_rgba_msg(lights.RESET))) ebutton_class = SimEButton if is_sim_mode() else ebutton.EButton self.add_body_part('ebutton', ebutton_class(self.robot_name, self.tf_buffer, topic="/hero/runstop_button")) diff --git a/hero_skills/src/hero_skills/tmc_speech.py b/hero_skills/src/hero_skills/tmc_speech.py new file mode 100644 index 0000000000..7c079f2784 --- /dev/null +++ b/hero_skills/src/hero_skills/tmc_speech.py @@ -0,0 +1,58 @@ +# ROS +import actionlib +import rospy + +# TMC +from tmc_msgs.msg import TalkRequestAction, TalkRequestGoal, Voice + +# TU/e Robotics +from robot_skills.speech import SpeechInterface + + +class TmcSpeech(SpeechInterface): + def __init__(self, robot_name, tf_buffer, pre_hook=None, post_hook=None): + """ + Speech interface to the TMC text-to-speech node. + + :param robot_name: name of the robot + :param tf_buffer: tf buffer object + :param pre_hook: method that is executed before speaking + :param post_hook: method that is executed after speaking + """ + super(TmcSpeech, self).__init__( + robot_name=robot_name, tf_buffer=tf_buffer, pre_hook=pre_hook, post_hook=post_hook, + ) + + # Client + self.speech_client = self.create_simple_action_client("/talk_request_action", TalkRequestAction) + + # noinspection PyUnusedLocal + def speak_impl(self, sentence, language, personality, voice, mood, block): + # type: (string, string, string, string, string, bool) -> bool + """ + Send a sentence to the text to speech module. + + When block=False, this method returns immediately. + With the replace-dictionary, you can specify which characters to replace with what. By default, it replace + underscores with spaces. + + :param sentence: string with sentence to pronounce + :param language: string with language to speak; only 'us' can be used. + :param personality: string indicating personality. Not used. + :param voice: string indicating the voice to speak with. Not used. + :param mood: string indicating the emotion. Not used. + :param block: bool to indicate whether this function should return immediately or if it should block until the + sentence has been spoken + """ + request = TalkRequestGoal() + request.data.interrupting = False + request.data.queueing = True + if language not in ["us", "en"]: + rospy.logwarn("TmcSpeech can only handle English, not {}".format(language)) + request.data.language = Voice.kEnglish + request.sentence = sentence + self.speech_client.send_goal(request) + # ToDo: test if blocking works as desired + if block: + self.speech_client.wait_for_result() + return True diff --git a/robot_skills/src/robot_skills/lights.py b/robot_skills/src/robot_skills/lights.py index c030ff51da..b6f724a569 100644 --- a/robot_skills/src/robot_skills/lights.py +++ b/robot_skills/src/robot_skills/lights.py @@ -11,20 +11,15 @@ RESET = ColorRGBA(0, 0, 1, 1) -class Lights(RobotPart): - """ - Interface to amigo's lights. - """ +class LightsInterface(RobotPart): def __init__(self, robot_name, tf_buffer): """ - constructor + Interface to the robot's lights. To use this, a deriving class needs to be defined that implements _send_color_msg. :param robot_name: robot_name :param tf_buffer: tf2_ros.Buffer """ - super(Lights, self).__init__(robot_name=robot_name, tf_buffer=tf_buffer) - self._topic = rospy.Publisher('/'+robot_name+'/rgb_lights_manager/user_set_rgb_lights', RGBLightCommand, - queue_size=10) + super(LightsInterface, self).__init__(robot_name=robot_name, tf_buffer=tf_buffer) def close(self): pass @@ -39,18 +34,25 @@ def set_color(self, r, g, b, a=1.0): :param a: alpha value 0.0-1.0 :return: no return """ - self.set_color_colorRGBA(ColorRGBA(r, g, b, a)) + self.set_color_rgba_msg(ColorRGBA(r, g, b, a)) - def set_color_colorRGBA(self, rgba): + def set_color_rgba_msg(self, rgba): """ Set the color of the robot by a std_msgs.msg.ColorRGBA :param rgba: std_msgs.msg.ColorRGBA :return: no return """ - rgb_msg = RGBLightCommand(color=rgba) - rgb_msg.show_color.data = True - self._topic.publish(rgb_msg) + self._send_color_msg(rgba_msg=rgba) + + def _send_color_msg(self, rgba_msg): + """ + Sends the color message to the robot hardware. This function needs to be implemented by deriving classes. + + :param rgba_msg: message to send + """ + # ToDo: replace by fstring (after going to Python3) + raise NotImplementedError("_send_color_msg is not implemented for {}".format(self.__class__.__name__)) def selfreset(self): """ @@ -58,26 +60,32 @@ def selfreset(self): :return: no return """ - self.set_color_colorRGBA(RESET) + self.set_color_rgba_msg(RESET) return True - def on(self): + +class TueLights(LightsInterface): + def __init__(self, robot_name, tf_buffer): """ - Set the lights of the robot ON + Interface to the robot's lights. This uses the TU/e-specific RGBLightCommand message type. - :return: no return + :param robot_name: robot_name + :param tf_buffer: tf_server.TFClient() """ - rgb_msg = RGBLightCommand(show_color=True) - self._topic.publish(rgb_msg) + super(TueLights, self).__init__(robot_name=robot_name, tf_buffer=tf_buffer) + self._publisher = rospy.Publisher( + '/{}/rgb_lights_manager/user_set_rgb_lights'.format(robot_name), RGBLightCommand, queue_size=10 + ) - def off(self): + def _send_color_msg(self, rgba_msg): """ - Set the lights of the robot OFF + Sends the color message to the robot hardware. This uses the RGBLightCommand message - :return: no return + :param rgba_msg: message to send """ - rgb_msg = RGBLightCommand(show_color=False) - self._topic.publish(rgb_msg) + rgb_msg = RGBLightCommand(color=rgba) + rgb_msg.show_color.data = True + self._publisher.publish(rgb_msg) def taste_the_rainbow(self, duration=5.0): """ @@ -86,9 +94,9 @@ def taste_the_rainbow(self, duration=5.0): :param duration: (float) Indicates the total duration of the rainbow """ - # rood: \_ - # groen: /\ - # blauw: _/ + # red: \_ + # green: /\ + # blue: _/ def red(t): if t < duration / 2.0: @@ -118,3 +126,24 @@ def blue(t): r, g, b = (red(time_after_start), green(time_after_start), blue(time_after_start)) self.set_color(r, g, b) rate.sleep() + + +class Lights(LightsInterface): + def __init__(self, robot_name, tf_buffer, topic): + """ + Interface to the robot's lights. This uses the TU/e-specific RGBLightCommand message type. + + :param robot_name: robot_name + :param tf_buffer: tf_server.TFClient() + :param topic: topic where to publish the messages + """ + super(Lights, self).__init__(robot_name=robot_name, tf_buffer=tf_buffer) + self._publisher = rospy.Publisher(topic, ColorRGBA, queue_size=1) + + def _send_color_msg(self, rgba_msg): + """ + Sends the color message to the robot hardware. This uses the RGBLightCommand message + + :param rgba_msg: message to send + """ + self._publisher.publish(rgba_msg) diff --git a/robot_skills/src/robot_skills/mockbot.py b/robot_skills/src/robot_skills/mockbot.py index b0a846c4de..6b186f9f13 100755 --- a/robot_skills/src/robot_skills/mockbot.py +++ b/robot_skills/src/robot_skills/mockbot.py @@ -256,7 +256,7 @@ def __init__(self, robot_name, tf_buffer, *args, **kwargs): super(Lights, self).__init__(robot_name, tf_buffer) self.close = AlteredMagicMock() self.set_color = AlteredMagicMock() - self.set_color_colorRGBA = AlteredMagicMock() + self.set_color_rgba_msg = AlteredMagicMock() self.on = AlteredMagicMock() self.off = AlteredMagicMock() diff --git a/robot_skills/src/robot_skills/speech.py b/robot_skills/src/robot_skills/speech.py index beccc25efe..90bda01564 100644 --- a/robot_skills/src/robot_skills/speech.py +++ b/robot_skills/src/robot_skills/speech.py @@ -6,19 +6,24 @@ from robot_skills.robot_part import RobotPart -class Speech(RobotPart): - """Interface to TTS-module""" - +class SpeechInterface(RobotPart): def __init__(self, robot_name, tf_buffer, pre_hook=None, post_hook=None): - super(Speech, self).__init__(robot_name=robot_name, tf_buffer=tf_buffer) - self._speech_service = self.create_service_client('/%s/text_to_speech/speak' % robot_name, Speak) + """ + Interface to text-to-speech module of the robot + + :param robot_name: name of the robot + :param tf_buffer: tf listener object + :param pre_hook: method that is executed before speaking + :param post_hook: method that is executed after speaking + """ + super(SpeechInterface, self).__init__(robot_name=robot_name, tf_buffer=tf_buffer) self._pre_hook = pre_hook self._post_hook = post_hook - self._default_language = self.load_param('text_to_speech/language', 'us') - self._default_voice = self.load_param('text_to_speech/voice', 'kyle') + self._default_language = self.load_param('text_to_speech/language', 'us') + self._default_voice = self.load_param('text_to_speech/voice', 'kyle') self._default_character = self.load_param('text_to_speech/character', 'default') - self._default_emotion = self.load_param('text_to_speech/emotion', 'neutral') + self._default_emotion = self.load_param('text_to_speech/emotion', 'neutral') def close(self): pass @@ -44,24 +49,83 @@ def speak(self, sentence, language=None, personality=None, voice=None, mood=None :param replace: dictionary with replacement stuff """ # ToDo: replace personality by character and mood by emotion. Furthermore, change the order of the arguments. - if not language: - language = self._default_language - if not voice: - voice = self._default_voice - if not personality: - personality = self._default_character - if not mood: - mood = self._default_emotion - if replace is None: - replace = {"_": " "} - - if hasattr(self._pre_hook, '__call__'): + language = language or self._default_language + voice = voice or self._default_voice + personality = personality or self._default_character + mood = mood or self._default_emotion + replace = replace or {"_": " "} + + if callable(self._pre_hook): self._pre_hook() for orig, replacement in replace.items(): sentence = sentence.replace(orig, replacement) - result = False + result = self.speak_impl( + sentence=sentence, language=language, personality=personality, voice=voice, mood=mood, block=block + ) + + if callable(self._post_hook): + self._post_hook() + + return result + + def speak_impl(self, sentence, language, personality, voice, mood, block): + # type: (string, string, string, string, string, bool) -> bool + """ + Send a sentence to the text to speech module. + + When block=False, this method returns immediately. + With the replace-dictionary, you can specify which characters to replace with what. By default, it replace + underscores with spaces. + + :param sentence: string with sentence to pronounce + :param language: string with language to speak. + :param personality: string indicating the personality. + :param voice: string indicating the voice to speak with. + :param mood: string indicating the emotion. + :param block: bool to indicate whether this function should return immediately or if it should block until the + sentence has been spoken + """ + raise NotImplementedError("speak_impl not implemented for {}".format(self.__class__.__name__)) + + +class TueSpeech(SpeechInterface): + def __init__(self, robot_name, tf_buffer, pre_hook=None, post_hook=None): + """ + Speech interface to the TU/e text-to-speech node. If present, this uses Philips TTS; otherwise, it defaults to + espeak + + :param robot_name: name of the robot + :param tf_buffer: tf listener object + :param pre_hook: method that is executed before speaking + :param post_hook: method that is executed after speaking + """ + super(TueSpeech, self).__init__( + robot_name=robot_name, tf_buffer=tf_buffer, pre_hook=pre_hook, post_hook=post_hook, + ) + self._speech_service = self.create_service_client('/%s/text_to_speech/speak' % robot_name, Speak) + + def speak_impl(self, sentence, language, personality, voice, mood, block): + # type: (string, string, string, string, string, bool) -> bool + """ + Send a sentence to the text to speech module. + + When block=False, this method returns immediately. + With the replace-dictionary, you can specify which characters to replace with what. By default, it replace + underscores with spaces. + + :param sentence: string with sentence to pronounce + :param language: string with language to speak. Philips TTS supports English (us) Dutch (nl) + :param personality: string indicating the personality. Supported are Default, Man, OldMan, OldWoman, Boy, + YoungGirl, Robot, Giant, Dwarf, Alien + :param voice: string indicating the voice to speak with. In English, "kyle" (default), "gregory" (French + accent) and "carlos" (Spanish accent) are supported. The Dutch voices are "david" and "marjolijn" + :param mood: string indicating the emotion. Supported are: Neutral, Friendly, Angry, Furious, Drill, Scared, + Emotional, Weepy, Excited, Surprised, Sad, Disgusted, Whisper. + :param block: bool to indicate whether this function should return immediately or if it should block until the + sentence has been spoken + """ try: # ToDo: test this. This just seems utterly wrong if language == 'nl' and not (personality in ['david', 'marjolijn']): @@ -70,23 +134,18 @@ def speak(self, sentence, language=None, personality=None, voice=None, mood=None # The funny stuff around sentence is for coloring the output text in the console req = SpeakRequest() - req.language = language - req.voice = voice - req.character = personality - req.emotion = mood - req.sentence = sentence + req.language = language + req.voice = voice + req.character = personality + req.emotion = mood + req.sentence = sentence req.blocking_call = block resp1 = self._speech_service(req) - result = resp1.error_msg == "" + return resp1.error_msg == "" except rospy.ServiceException as e: rospy.logerr("Service call failed: {0}".format(e)) - result = False + return False except Exception as e: rospy.logerr("Something went seriously wrong: {}".format(e)) - result = False - - if hasattr(self._post_hook, '__call__'): - self._post_hook() - - return result + return False diff --git a/robot_skills/src/robot_skills/test_tools/test_robot.py b/robot_skills/src/robot_skills/test_tools/test_robot.py index 9fcf959f68..fef26f72d9 100644 --- a/robot_skills/src/robot_skills/test_tools/test_robot.py +++ b/robot_skills/src/robot_skills/test_tools/test_robot.py @@ -38,6 +38,33 @@ class TestAmigo(TestRobot): to be loaded on the parameter server. """ def test_robot(self): + """ + Top level test method. Calls other functions in this class + """ rospy.init_node("test_robot") - # noinspection PyUnresolvedReferences robot = self.ROBOT_CLASS() + self.lights(robot) + self.speech(robot) + + def part_commons(self, part): + self.assert_has_attr(part, "close") + self.assert_has_attr(part, "selfreset") + + def lights(self, robot): + self.assert_has_attr(robot, "lights") + self.part_commons(robot.lights) + self.assert_has_attr(robot.lights, "set_color") + self.assert_has_attr(robot.lights, "set_color_rgba_msg") + + def speech(self, robot): + self.assert_has_attr(robot, "speech") + self.part_commons(robot.speech) + self.assert_has_attr(robot.speech, "speak") + + def assert_has_attr(self, obj, intended_attr): + test_bool = hasattr(obj, intended_attr) + + # self.assertTrue(test_bool, msg=f"{obj} lacks attribute {intended_attr}") # Python3 + self.assertTrue(test_bool, msg="{} lacks attribute {}".format(obj, intended_attr)) + + diff --git a/sergio_skills/src/sergio_skills/sergio.py b/sergio_skills/src/sergio_skills/sergio.py index 18946ddf4c..c51370ccf6 100644 --- a/sergio_skills/src/sergio_skills/sergio.py +++ b/sergio_skills/src/sergio_skills/sergio.py @@ -30,16 +30,16 @@ def __init__(self, wait_services=False): self.add_body_part('ssl', sound_source_localisation.SSL(self.robot_name, self.tf_buffer)) # Human Robot Interaction - self.add_body_part('lights', lights.Lights(self.robot_name, self.tf_buffer)) - self.add_body_part('speech', speech.Speech(self.robot_name, self.tf_buffer, - lambda: self.lights.set_color_colorRGBA(lights.SPEAKING), - lambda: self.lights.set_color_colorRGBA(lights.RESET))) + self.add_body_part('lights', lights.TueLights(self.robot_name, self.tf_buffer)) + self.add_body_part('speech', speech.TueSpeech(self.robot_name, self.tf_buffer, + lambda: self.lights.set_color_rgba_msg(lights.SPEAKING), + lambda: self.lights.set_color_rgba_msg(lights.RESET))) self.add_body_part('hmi', api.Api(self.robot_name, self.tf_buffer, - lambda: self.lights.set_color_colorRGBA(lights.LISTENING), - lambda: self.lights.set_color_colorRGBA(lights.RESET))) + lambda: self.lights.set_color_rgba_msg(lights.LISTENING), + lambda: self.lights.set_color_rgba_msg(lights.RESET))) self.add_body_part('ears', ears.Ears(self.robot_name, self.tf_buffer, - lambda: self.lights.set_color_colorRGBA(lights.LISTENING), - lambda: self.lights.set_color_colorRGBA(lights.RESET))) + lambda: self.lights.set_color_rgba_msg(lights.LISTENING), + lambda: self.lights.set_color_rgba_msg(lights.RESET))) ebutton_class = SimEButton if is_sim_mode() else ebutton.EButton self.add_body_part('ebutton', ebutton_class(self.robot_name, self.tf_buffer))