Feature/remove speech from bridge

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

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

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

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,45 +34,58 @@ 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):
         """
         Set the lights to blue
 
         :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)

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