From a17d29d45c42081bedef947c5fa8dbe34be81f2c Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Thu, 1 Apr 2021 23:50:41 +0200 Subject: [PATCH 1/4] (ghost) convert back to normal TransformBroadcaster --- src/tf_ghost/tf_ghost.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/tf_ghost/tf_ghost.py b/src/tf_ghost/tf_ghost.py index 37643a6..ed115a8 100755 --- a/src/tf_ghost/tf_ghost.py +++ b/src/tf_ghost/tf_ghost.py @@ -55,9 +55,11 @@ def sendTransform(self, translation, rotation, time, child, parent): if __name__ == "__main__": rospy.init_node('tf_ghost_publisher') + rate = rospy.Rate(50) try: ghost = Ghost() - ghost.create_ghosts() - rospy.spin() + while not rospy.is_shutdown(): + ghost.create_ghosts() + rate.sleep() except rospy.ROSInterruptException: pass From 288623a56189894fc476099215737d5508793c6a Mon Sep 17 00:00:00 2001 From: Janno Lunenburg Date: Tue, 20 Apr 2021 20:41:57 +0200 Subject: [PATCH 2/4] cleanup()Removed lights bridge --- src/lights/light_bridge.py | 26 -------------------------- 1 file changed, 26 deletions(-) delete mode 100755 src/lights/light_bridge.py diff --git a/src/lights/light_bridge.py b/src/lights/light_bridge.py deleted file mode 100755 index 837db8e..0000000 --- a/src/lights/light_bridge.py +++ /dev/null @@ -1,26 +0,0 @@ -#!/usr/bin/env python -# -*- coding: utf-8 -*- - -import rospy - -from tue_msgs.msg import RGBLightCommand -from std_msgs.msg import ColorRGBA - - -class Shine(object): - def __init__(self): - - # topics - self.sub_lights = rospy.Subscriber('rgb_lights_manager/user_set_rgb_lights', RGBLightCommand, self.change_lights) - self.pub_lights = rospy.Publisher('command_status_led_rgb', ColorRGBA, queue_size=2) - - def change_lights(self, command): - color = command.color - self.pub_lights.publish(color) - - -if __name__ == "__main__": - rospy.init_node('lights_bridge') - shine = Shine() - - rospy.spin() From bb0ff45276146a00a151ce54b1d1c9de6ed7b2f8 Mon Sep 17 00:00:00 2001 From: Janno Lunenburg Date: Sat, 1 May 2021 10:00:27 +0200 Subject: [PATCH 3/4] feat(launch)Added launch filee containing all nodes --- launch/hero_bridge.launch | 13 +++++++++++++ 1 file changed, 13 insertions(+) create mode 100644 launch/hero_bridge.launch diff --git a/launch/hero_bridge.launch b/launch/hero_bridge.launch new file mode 100644 index 0000000..b85540c --- /dev/null +++ b/launch/hero_bridge.launch @@ -0,0 +1,13 @@ + + + + + + + + + + + + + From ded91b3aed5b4949f80619bba37c6ff6588d5caa Mon Sep 17 00:00:00 2001 From: Janno Lunenburg Date: Sat, 1 May 2021 11:25:15 +0200 Subject: [PATCH 4/4] cleanup(speech)Removed speech bridge --- launch/hero_bridge.launch | 1 - src/speech_node/speech.py | 170 -------------------------------------- 2 files changed, 171 deletions(-) delete mode 100755 src/speech_node/speech.py diff --git a/launch/hero_bridge.launch b/launch/hero_bridge.launch index b85540c..27e0b91 100644 --- a/launch/hero_bridge.launch +++ b/launch/hero_bridge.launch @@ -2,7 +2,6 @@ - diff --git a/src/speech_node/speech.py b/src/speech_node/speech.py deleted file mode 100755 index 0d6dc2a..0000000 --- a/src/speech_node/speech.py +++ /dev/null @@ -1,170 +0,0 @@ -#!/usr/bin/env python -# -*- coding: utf-8 -*- - -""" -This node listens to a service call and a topic for text to speech -requests. These will be processed by the festival or the philips tts module. -""" - -import rospy -import actionlib -import collections - -from std_srvs.srv import Empty, EmptyResponse -from std_msgs.msg import String -from tmc_msgs.msg import TalkRequestAction, TalkRequestGoal, Voice -from text_to_speech.srv import Speak, SpeakRequest - - -class bcolors: - HEADER = '\033[95m' - OKBLUE = '\033[94m' - OKGREEN = '\033[92m' - WARNING = '\033[93m' - FAIL = '\033[91m' - ENDC = '\033[0m' - BOLD = '\033[1m' - UNDERLINE = '\033[4m' - - -class TTS(object): - """ - Bridge from TTS service calls and topic messages to Toyota TTS, introducing a buffer to handle all requests in - receiving order - """ - def __init__(self, rate): - """ - Constructor - - :param rate: ROS parameter for spin-rate - """ - - self.samples_path = rospy.get_param("~samples_path", "~/MEGA/media/audio/soundboard") - self._rate = rate - self._active_req = None - - # buffer and goal state - self.buffer = collections.deque() - - # topics - self.sub_speak = rospy.Subscriber("~input", String, self.speak) - self.pub_sentence = rospy.Publisher('~output', String, queue_size=10) - - # services - self.srv_speak = rospy.Service('~speak', Speak, self.speak_srv) - self.srv_clear_buffer = rospy.Service('~clear_buffer', Empty, self.clear_buffer_srv) - - # clients - self.speech_client = actionlib.SimpleActionClient('/talk_request_action', TalkRequestAction) - self.speech_client.wait_for_server() - - @property - def block_queue(self): - """ - Property which checks whether the active request or remaining queue is blocking - :return: Boolean - """ - # First check if self._active_req is not None and if not, if it is blocking - active_req_blocking = self._active_req is not None and self._active_req.blocking_call - - # Also check if there is still a blocking call in the remaining queue - return active_req_blocking or any(requests.blocking_call for requests in self.buffer) - - def buffer_requests(self, req): - """ - Handle requests: put them in buffer and wait if there is a blocking TTS call in the buffer - :param req: server TTS request - :type req: SpeakRequest - """ - - # Extend the buffer queue on the right - self.buffer.append(req) - rospy.logdebug("Buffer size [{}]: Added the TTS request to the queue.".format(len(self.buffer))) - - # Wait with returning function call if there is a blocking element in the queue - rate = rospy.Rate(self._rate) - while not rospy.is_shutdown() and self.block_queue: - rate.sleep() - - def speak(self, sentence_msg): - """ - Receiving subscribed messages over the ~input topic - :param sentence_msg: topic TTS message - :type sentence_msg: String - """ - - # Change speak topic message to same type as service call - req = SpeakRequest() - req.sentence = sentence_msg.data - - self.buffer_requests(req) - - def speak_srv(self, req): - """ - Receiving service calls over the ~speak service - :param req: server TTS request - :type req: SpeakRequest - """ - - self.buffer_requests(req) - return "" - - def clear_buffer_srv(self, empty): - """ - Clearing the buffer on service call and setting the (now empty) queue to non-blocking to avoid dead-lock - :param empty: empty service call - """ - - self.buffer.clear() - return EmptyResponse() - - def _done_cb(self, empty1, empty2): - """ - Function which is called back if the speech client is done with the TTS of the active request - """ - self._active_req = None - - def spin(self): - """ - ROS spin-like function, should be ran after initializing the node. Sends new TTS goals to the Toyota TTS if - the buffer queue is non-empty and the robot isn't already talking and adjusts the buffer as necessary - """ - - rate = rospy.Rate(self._rate) - while not rospy.is_shutdown(): - - # Print the talking state of the robot (PENDING = 0, TALKING = 1, DONE = 2) to the ROS debugging level - rospy.logdebug(self.speech_client.simple_state) - - # If the buffer is non-empty and the robot is not currently talking, send a new TTS request - if self.buffer and self._active_req is None: - self._active_req = self.buffer[0] - - # Bridge between the req object and simple_action_client goal object - goal = TalkRequestGoal() - out = Voice() - out.interrupting = False - out.queueing = True - out.language = 1 - out.sentence = self._active_req.sentence - goal.data = out - - # Publish what the robot is going to say - self.pub_sentence.publish(self._active_req.sentence) - - # Send the left-most queue entry to Toyota TTS over the simple_action_client - self.speech_client.send_goal(goal, done_cb=self._done_cb) - - # Pop left-most queue entry - if self.buffer: - self.buffer.popleft() - rospy.logdebug("Buffer size [{}]: Send TTS request and removed from queue.".format(len(self.buffer))) - - rate.sleep() - - -if __name__ == "__main__": - rospy.init_node('text_to_speech') - - tts = TTS(rospy.get_param('~rate', 10)) - tts.spin()