From dc865fae2b1377d16213954ca7242408c93764df Mon Sep 17 00:00:00 2001 From: jakory Date: Fri, 28 Oct 2016 12:54:12 -0400 Subject: [PATCH] Clean up and format robot_translation_node - Fix line wrapping, whitespace, and comment style in the robot_translation_node. - Remove unused functions. - Add some comments explaining what's going on. - While cleaning up, reword a couple sentences in the README for clarity. --- README.md | 9 +- src/robot_translation_node.py | 380 ++++++++++++++++++---------------- 2 files changed, 206 insertions(+), 183 deletions(-) diff --git a/README.md b/README.md index 2c8a056..b761929 100644 --- a/README.md +++ b/README.md @@ -43,12 +43,15 @@ See [sar\_robot\_command\_msgs](https://github.com/personal-robots/sar_robot_command_msgs "sar_robot_command_msgs") for more info about these messages. + + ### Robot-specific messages -The program will publish messages to robot-specific topics depending on which -robot is set in the configuration file. +The program will publish and subscribe to robot-specific topics depending on +which robot is set in the configuration file. -TODO: Determine message types and ROS topic names for each robot platform. +TODO: Determine message types and ROS topic names for each robot platform, and +list here. ## Version and dependency notes diff --git a/src/robot_translation_node.py b/src/robot_translation_node.py index 6ccda12..ad81c27 100755 --- a/src/robot_translation_node.py +++ b/src/robot_translation_node.py @@ -31,27 +31,33 @@ from sar_robot_command_msgs.msg import RobotState # ROS msgs from geometry_msgs.msg import Vector3 from std_msgs.msg import Header # standard ROS Header -import re #regular expression +import re # regular expression from std_msgs.msg import String import os, sys from sar_jibo_command_msgs.msg import JiboSpeech, JiboAnimation, JiboLookat import Queue -# The SAR robot translation node subscribes to the robot_command topic and -# translates any robot commands received from the generic format to platform- -# specific commands that are sent to the specific robot being used (specified -# in the config file). class robot_translation(): - """ Robot translation node """ + """ The SAR robot translation node subscribes to the robot_command topic and + translates any robot commands received from the generic format to platform- + specific commands that are sent to the specific robot being used (specified + in the config file). + """ def __init__(self): """ Initialize anything that needs initialization """ - # parse config file to find out which robot to send to try: - __location__ = os.path.realpath(os.path.join(os.getcwd(), os.path.dirname(sys.argv[0]))) - config_file = os.path.join(__location__, 'robot_translation_config.json') + # If we are not running the robot_translation node from the node's + # src/ directory, we may need to get the full path to that directory + # so that we can find the config file (a relative path to the config + # file wouldn't work.) + __location__ = os.path.realpath(os.path.join(os.getcwd(), + os.path.dirname(sys.argv[0]))) + config_file = os.path.join(__location__, + 'robot_translation_config.json') rospy.loginfo('robot translation config = ' + config_file) + # Parse config file to find out which robot to send to. with open (config_file) as json_file: json_data = json.load(json_file) rospy.loginfo("Got config:\n" + str(json_data)) @@ -64,59 +70,71 @@ def __init__(self): + "Defaulting to SIMULATED robot.") self.which_robot = "SIMULATED" except ValueError as e: - rospy.logerr('Error! Could not open or parse json config file!' - + '\n Did you use valid json?\nError: %s' % e) + rospy.logerr("Error! Could not open or parse json config file!" + + "\n Did you use valid json?\nError: %s" % e) except IOError as e: - rospy.logerr('Error! Could not open or could not parse json ' - +'config file!\n Does the file exist in this directory?' - + '\nError: %s' % e) + rospy.logerr("Error! Could not open or could not parse json config " + + "file!\n Does the file exist in this directory?" + + "\nError: %s" % e) def run_robot_translation_node(self): - """ wait for robot commands and pass them on """ - # start ros node + """ Wait for robot commands and pass them on. """ + # Start ros node. rospy.init_node('robot_translation_node', anonymous=True) rospy.loginfo("Robot translation node starting up! Configured to send " "to " + self.which_robot + " robot.") - # obtain participant names + # Obtain participant names. self.child_name = rospy.get_param('/sar/global/_child_name') self.guardian_name = rospy.get_param('/sar/global/_guardian_name') - # subscribe to /robot_command topic to get command messages - # for the robot + # Subscribe to /sar/robot_command topic to get command messages for the + # robot. rospy.Subscriber('/sar/robot_command', RobotCommand, self.on_robot_command_msg) rospy.loginfo("Subscribed to 'robot_command' topic.") - # subscribe to /robot_state topic to get status messages from - # the robot + # Subscribe to /sar/robot_state topic to get status messages from the + # robot. rospy.Subscriber('/sar/robot_state', RobotState, self.on_robot_state_msg) rospy.loginfo("Subscribed to 'robot_state' topic.") - # publish to robot-specific topic to pass commands to robots - # if robot is jibo... + # Publish to robot-specific topic to pass commands to robots. + # If robot is Jibo... if (self.which_robot == 'JIBO'): rospy.loginfo('setting up jibo communication') - rospy.Subscriber('/sar/jibo/state', RobotState, - self.on_jibo_state) + + # Subscribe to jibo state messages. + rospy.Subscriber('/sar/jibo/state', RobotState, self.on_jibo_state) rospy.loginfo("Subscribed to '/sar/jibo/state' topic.") - rospy.Subscriber('/sar/perception/user_head_position_rf', Vector3, self.user_head_position_jibo_rf_callback) - #self.jibo_command_pub = rospy.Publisher('/sar/jibo/command', JiboCommand, queue_size = 10) - self.jibo_lookat_pub = rospy.Publisher('/sar/jibo/lookat', JiboLookat, queue_size=10) - self.jibo_speech_pub = rospy.Publisher('/sar/jibo/speech', JiboSpeech, queue_size=10) - self.jibo_animation_pub = rospy.Publisher('/sar/jibo/animation', JiboAnimation, queue_size=10) - self.robot_state_pub = rospy.Publisher('/sar/robot_state', RobotState, queue_size=10) + + # Subscribe to user head position messages (so we can do head + # tracking with Jibo). + rospy.Subscriber('/sar/perception/user_head_position_rf', Vector3, + self.user_head_position_jibo_rf_callback) + + # We will publish lookat, speech, and animation command messages to + # Jibo, and we will publish robot state messages as Jibo. + self.jibo_lookat_pub = rospy.Publisher('/sar/jibo/lookat', + JiboLookat, queue_size=10) + self.jibo_speech_pub = rospy.Publisher('/sar/jibo/speech', + JiboSpeech, queue_size=10) + self.jibo_animation_pub = rospy.Publisher('/sar/jibo/animation', + JiboAnimation, queue_size=10) + self.robot_state_pub = rospy.Publisher('/sar/robot_state', + RobotState, queue_size=10) + + # Initialize other Jibo-specific variables. self._is_jibo_ready = True self._is_robot_ready = True - # default position of the user head + # Set the default position of the user head. self._current_user_head_pos_jibo_rf = Vector3() self._current_user_head_pos_jibo_rf.x = 1 self._current_user_head_pos_jibo_rf.y = -0.4 self._current_user_head_pos_jibo_rf.z = 0.8 - rospy.loginfo("Will publish to 'jibo_command' topic.") - # if robot is a SPRITE robot... + # If robot is a SPRITE robot... elif (self.which_robot == 'SPRITE'): pass # TODO what topic and what message type for SPRITE robots? @@ -124,24 +142,24 @@ def run_robot_translation_node(self): #queue_size = 10) #rospy.loginfo("Will publish to 'sprite_command' topic.") - # if robot is a simulated robot... + # If robot is a simulated robot... elif (self.which_robot == 'SIMULATED'): self.robot_sim_pub = rospy.Publisher('robot_sim_command', RobotCommand, queue_size = 10) rospy.loginfo("Will publish to 'robot_sim_command' topic.") - # fill in for any other robots + # Fill in for any other robots! #elif (self.which_robot == 'OTHER_ROBOT'): # TODO what topic and what message type for other robot? #self.other_pub = rospy.Publisher('other_command', OtherMessage, #queue_size = 10) - # keep python from exiting until this node is stopped + # Keep python from exiting until this node is stopped. rospy.spin() def on_robot_state_msg(self, data): - """ Receive status messages from robots """ + """ Receive status messages from robots. """ #rospy.loginfo("Got message:\n" + str(data)) # TODO do something with robot status messages? pass @@ -150,44 +168,38 @@ def on_robot_state_msg(self, data): def on_robot_command_msg(self, data): """ Translate the robot command for the specific platform! """ rospy.loginfo("Got message:\n" + str(data)) + # TODO check that data is valid before trying to parse! - # TODO check that data is valid after we finalize command format! - - # send a \fake\ conceptual robot state signaling that the robot is busy - self._is_robot_ready = False - conceptual_robot_state = RobotState() - conceptual_robot_state.header = Header() - conceptual_robot_state.header.stamp = rospy.Time.now() - conceptual_robot_state.doing_action = True - conceptual_robot_state.is_playing_sound = True - self.robot_state_pub.publish(conceptual_robot_state) - - # pass command in platform-specific way - # send to Jibo... + # Pass command in platform-specific way. + # Send to Jibo... if (self.which_robot == 'JIBO'): + # Send a \fake\ conceptual robot state signaling that the robot is + # busy. + self._is_robot_ready = False + conceptual_robot_state = RobotState() + conceptual_robot_state.header = Header() + conceptual_robot_state.header.stamp = rospy.Time.now() + conceptual_robot_state.doing_action = True + conceptual_robot_state.is_playing_sound = True + self.robot_state_pub.publish(conceptual_robot_state) + # Parse and send the command to Jibo. self.send_to_jibo(data) - # send to SPRITE robot... + + # Send to SPRITE robot... elif (self.which_robot == 'SPRITE'): self.send_to_sprite(data) - # send to simulated robot... + + # Send to simulated robot... elif (self.which_robot == 'SIMULATED'): self.send_to_simulated(data) - # fill in for any other robots: + + # Fill in for any other robots: #elif (self.which_robot == 'OTHER_ROBOT'): #self.send_to_other_robot(data) - # send a conceptual robot state signaling that the robot is busy - self._is_robot_ready = False - conceptual_robot_state = RobotState() - conceptual_robot_state.header = Header() - conceptual_robot_state.header.stamp = rospy.Time.now() - conceptual_robot_state.doing_action = True - conceptual_robot_state.is_playing_sound = True - self.robot_state_pub.publish(conceptual_robot_state) - def send_to_sprite(self, data): - """ Translate robot command to format SPRITE robot uses """ + """ Translate robot command to format SPRITE robot uses. """ # TODO send command to SPRITE robot rospy.logwarn("TODO send to sprite robot!") @@ -234,39 +246,20 @@ def send_to_sprite(self, data): def on_jibo_state(self, data): + """ Receive Jibo status messages. """ self._is_jibo_ready = not (data.is_playing_sound or data.doing_action) - def extract_jibo_commands(self, properties): - # TODO: what is the tag for lookat? - jibo_speech = None - jibo_animation = None - jibo_lookat = None - - if "<" in properties: - regex = re.compile("<(.*)>") - match = regex.search(properties) - if match == None: - return None, None, None #cmhuang: test - jibo_animation = match.group(1) #TODO: assuming only one match - jibo_speech = re.sub("<(.*)>", "", properties) - else: #only speech - jibo_speech = properties - - # TODO: lookat: lookat-game, lookat-user - - return jibo_speech, jibo_animation, jibo_lookat - - - def command_to_behavior_queue(self, properties): + def command_to_jibo_behavior_queue(self, properties): + """ Translate a RobotCommand string to commands to send to Jibo. """ jibo_behavior_queue = Queue.Queue() speech_parameters = {} - # replace child-name and guardian-name + # Replace child-name and guardian-name with actual values. properties = properties.replace("[child-name]", self.child_name) properties = properties.replace("[guardian-name]", self.guardian_name) - # extract speech parameters + # Extract speech parameters. if "(" in properties: regex = re.compile("(\([\w\s,.:]*?\))") p = regex.match(properties) @@ -276,7 +269,7 @@ def command_to_behavior_queue(self, properties): if sp != None: _temp = properties.split(sp) properties = _temp[1].lstrip() - #parse sp to parameters + # Parse sp to parameters. sp = sp.replace('(',"") sp = sp.replace(')',"") sp_elements = sp.split(',') @@ -288,9 +281,10 @@ def command_to_behavior_queue(self, properties): _value = float(_elem[1]) speech_parameters[_key] = _value - # break the robot command into jibo unit commands + # Break the RobotCommand string into Jibo unit commands for speech and + # animations. Actions/animations are enclosed in angle brackets; all + # else is considered speech. while "<" in properties: - #regex = re.compile("(<[a-z\-]*?,b>)|(<[a-z\-]*?,nb>)|(<[a-z\-]*?>)") regex = re.compile("(<[a-z\-_]*?\s?,?\s?[b|nb]*>)") _first_match = None for matches in regex.finditer(properties): @@ -304,6 +298,7 @@ def command_to_behavior_queue(self, properties): jibo_behavior_queue.put(_speech) jibo_behavior_queue.put(_first_match) properties = _seg[1] + # If there are no actions/animations, everything is speech. if properties != "": jibo_behavior_queue.put(properties) @@ -316,34 +311,35 @@ def command_to_behavior_queue(self, properties): def send_to_jibo(self, data): - """ Translate robot command to format Jibo uses """ - #rospy.loginfo("sending jibo command") - - #msg = JiboCommand() - #msg.header = Header() - - # TODO: should spin out a thread for this.. + """ Translate robot command to format Jibo uses. """ + # TODO: should spin out a thread for this... if data.command == RobotCommand.SLEEP: #msg.signal = JiboCommand.SLEEP #self.jibo_command_pub.publish(msg) + # TODO: Deal with SLEEP commands. pass + elif data.command == RobotCommand.WAKEUP: #msg.signal = JiboCommand.WAKEUP #self.jibo_command_pub.publish(msg) + # TODO: Deal with WAKEUP commands. pass + elif data.command == RobotCommand.DO: #msg.signal = JiboCommand.DO # prepare sub-command queue - behavior_queue, speech_parameters = self.command_to_behavior_queue(data.properties) + behavior_queue, speech_parameters = \ + self.command_to_jibo_behavior_queue(data.properties) + # Send Jibo speech and action commands while we have some to send. while not behavior_queue.empty(): if self._is_jibo_ready: - #_msg = msg - #_msg.speech = "" _content = behavior_queue.get() if _content == None: continue rospy.loginfo('_content = ' + _content) + + # Send animation commands if we have them. if "<" in _content: _content = _content.replace("<", "") _content = _content.replace(">", "") @@ -357,70 +353,122 @@ def send_to_jibo(self, data): if _specified_blocking == "nb": _blocking = False rospy.loginfo('_blocking = ' + str(_blocking)) + + # The action/animation should block before we play more + # speech or do more actions/animations. if _blocking: rospy.loginfo('_anim_file = ' + _anim_file) - if (_anim_file == "lookat-game") or (_anim_file == "lookat-screen"): - #_msg.lookat_x = 0.15 # TODO: make these values globally available - #_msg.lookat_y = -0.65 - #_msg.lookat_z = 0.25 + + # Send a lookat game/screen message. + if (_anim_file == "lookat-game") or \ + (_anim_file == "lookat-screen"): + # TODO: make these values globally available self.send_jibo_lookat(0.15, -0.65, 0.25) - elif (_anim_file == "lookat_child") or (_anim_file == "return-front") or (_anim_file == "lookat-child"): - rospy.loginfo('user head: ' + str(abs(self._current_user_head_pos_jibo_rf.x)) + ' ' + str(self._current_user_head_pos_jibo_rf.y) + ' ' +str(self._current_user_head_pos_jibo_rf.z)) + + # Using our head tracking info, send a lookat child + # command. + elif (_anim_file == "lookat_child") or \ + (_anim_file == "return-front") or \ + (_anim_file == "lookat-child"): + rospy.loginfo('user head: ' + str(abs( + self._current_user_head_pos_jibo_rf.x)) + + ' ' + + str(self._current_user_head_pos_jibo_rf.y) + + ' ' + + str(self._current_user_head_pos_jibo_rf.z)) my_y = self._current_user_head_pos_jibo_rf.y if my_y >= 0.0: my_y = 0 if my_y < -0.4: my_y = 0 - self.send_jibo_lookat(abs(self._current_user_head_pos_jibo_rf.x), my_y, self._current_user_head_pos_jibo_rf.z) - #self.send_jibo_lookat(1,0,1) + self.send_jibo_lookat(abs( + self._current_user_head_pos_jibo_rf.x), + my_y, + self._current_user_head_pos_jibo_rf.z) + + # Send a lookat guardian command. elif _anim_file == "lookat_guardian": # TODO: need to update these values self.send_jibo_lookat(1, 0.5, 1) + + # Otherwise, it's a named animation to play back. else: - #_msg.animation = _anim_file self.send_jibo_animation(_anim_file+'-2.keys') + + # The action/animation should play at the same time as + # the next chunk of speech. + # TODO Much the code in this else-block duplicates the + # logic and functionality above -- need to consolidate + # and find a more consie way to write this! For example, + # everything up until sending speech along with the + # action could be put in its own function that we call + # here, and above... else: - if (_anim_file == "lookat-game") or (_anim_file == "lookat-screen"): - #_msg.lookat_x = 0.15 # TODO: make these values globally available - #_msg.lookat_y = -0.65 - #_msg.lookat_z = 0.25 + # Send a lookat game/screen message. + if (_anim_file == "lookat-game") or \ + (_anim_file == "lookat-screen"): + # TODO: make these values globally available self.send_jibo_lookat(0.15, -0.65, 0.25) - elif (_anim_file == "lookat_child") or (_anim_file == "return-front") or (_anim_file == "lookat-child"): - rospy.loginfo('user head: ' + str(abs(self._current_user_head_pos_jibo_rf.x)) + ' ' + str(self._current_user_head_pos_jibo_rf.y) + ' ' +str(self._current_user_head_pos_jibo_rf.z)) + + # Using our head tracking info, send a lookat child + # command. + elif (_anim_file == "lookat_child") or \ + (_anim_file == "return-front") or \ + (_anim_file == "lookat-child"): + rospy.loginfo('user head: ' + str(abs( + self._current_user_head_pos_jibo_rf.x)) + + ' ' + + str(self._current_user_head_pos_jibo_rf.y) + + ' ' + + str(self._current_user_head_pos_jibo_rf.z)) my_y = self._current_user_head_pos_jibo_rf.y if my_y >= 0.0: my_y = 0 if my_y < -0.4: my_y = 0 - self.send_jibo_lookat(abs(self._current_user_head_pos_jibo_rf.x), my_y, self._current_user_head_pos_jibo_rf.z) - #self.send_jibo_lookat(1,0,1) + self.send_jibo_lookat( + abs(self._current_user_head_pos_jibo_rf.x), + my_y, + self._current_user_head_pos_jibo_rf.z) + + # Send a lookat guardian command. elif _anim_file == "lookat_guardian": # TODO: need to update these values self.send_jibo_lookat(1, 0.5, 1) + + # Otherwise, it's a named animation to play back. else: - #_msg.animation = _anim_file self.send_jibo_animation(_anim_file+'-2.keys') + + # Now, if there is speech to play after this action/ + # animation, we also send a command to play the + # speech. if not behavior_queue.empty(): - _speech = behavior_queue.get() # assuming that no two animations are attached together - #_msg.speech = _speech + # We assume that no two animations are attached + # together. + _speech = behavior_queue.get() rospy.loginfo('_speech = ' + _speech) - self.send_jibo_speech(_speech, speech_parameters) + self.send_jibo_speech(_speech, + speech_parameters) + + # There are no animations to send; just send speech! else: #_msg.speech = _content rospy.loginfo('_speech/content = ' + _content) self.send_jibo_speech(_content, speech_parameters) - #_msg.header.stamp = rospy.Time.now() - #self.jibo_command_pub.publish(_msg) # not the best way, causing delays - #rospy.loginfo("Forwarding message to jibo robot:\n" + str(_msg)) + self._is_robot_ready = False self._is_jibo_ready = False - rospy.Rate(10).sleep() # sleep for 100 ms: do not use 50ms or below.. + # Sleep for 100 ms: do not use 50ms or below. + rospy.Rate(10).sleep() - # need to wait for jibo to finish the last command request + # Need to wait for jibo to finish the last command request. while self._is_jibo_ready == False: rospy.Rate(10).sleep() continue - # send a conceptual robot state signaling that the robot is ready again + + # Send a conceptual robot state signaling that the robot is ready + # again. self._is_robot_ready = True conceptual_robot_state = RobotState() conceptual_robot_state.header = Header() @@ -430,40 +478,8 @@ def send_to_jibo(self, data): self.robot_state_pub.publish(conceptual_robot_state) - def send_to_jibo(self, data): - """ Translate robot command to format Jibo uses """ - # TODO send command to jibo - #rospy.logwarn("TODO send to jibo!") - - # TODO create message to send - # TODO fill message with data from RobotCommand - # TODO send message - jibo_speech = None - jibo_animation = None - jibo_lookat = None - rospy.loginfo("sending jibo command") - - if data.command == RobotCommand.SLEEP: - pass - elif data.command == RobotCommand.WAKEUP: - pass - elif data.command == RobotCommand.DO: - # TODO: parsing the new format - jibo_speech, jibo_animation, jibo_lookat = self.extract_jibo_commands(data.properties) - - self.jibo_command_pub.publish(msg) - rospy.loginfo("Forwarding message to jibo robot:\n" + str(msg)) - - if jibo_speech != None: - rospy.loginfo("jibo_speech = " + jibo_speech) - self.jibo_speech_pub.publish(jibo_speech) - if jibo_animation != None: - self.jibo_animation_pub.publish(jibo_animation+'.keys') - rospy.loginfo("jibo_animation = " + jibo_animation) - - #rospy.loginfo("jibo_lookat = " + jibo_lookat) - def send_jibo_lookat(self, _x, _y, _z, _duration=-1): + """ Build and send Jibo lookat message. """ msg = JiboLookat() msg.header = Header() msg.header.stamp = rospy.Time.now() @@ -475,6 +491,7 @@ def send_jibo_lookat(self, _x, _y, _z, _duration=-1): def send_jibo_animation(self, _name, _n=1.0): + """ Build and send Jibo animation message. """ msg = JiboAnimation() msg.header = Header() msg.header.stamp = rospy.Time.now() @@ -484,51 +501,54 @@ def send_jibo_animation(self, _name, _n=1.0): def send_jibo_speech(self, _content, _parameters): + """ Build and send a Jibo speech message. """ msg = JiboSpeech() msg.header = Header() msg.header.stamp = rospy.Time.now() msg.speech_content = _content - # default paramters for jibo speech - _pitch = 7.6 - _bandwidth = 2.0 - _stretch = 1.07 - if _parameters: # dictionary is not empty - _pitch = _parameters['pitch'] - _bandwidth = _parameters['pitchBandwidth'] - _stretch = _parameters['duration_stretch'] - msg.pitch = _pitch - msg.pitch_bandwidth = _bandwidth - msg.duration_stretch = _stretch + # If we have Jibo speech parameters, use them. + # TODO Add error checking: what if these keys are not in the dictionary? + if _parameters: + msg.pitch = _parameters['pitch'] + msg.pitch_bandwidth = _parameters['pitchBandwidth'] + msg.duration_stretch = _parameters['duration_stretch'] + # Otherwise, use the default paramters for Jibo speech. + else: + msg.pitch = 7.6 + msg.pitch_bandwidth = 2.0 + msg.duration_stretch = 1.07 self.jibo_speech_pub.publish(msg) def user_head_position_jibo_rf_callback(self, data): + """ Receive user head position messages. """ self._current_user_head_pos_jibo_rf.x = data.x self._current_user_head_pos_jibo_rf.y = data.y self._current_user_head_pos_jibo_rf.z = data.z + def send_to_simulated(self, data): - """ Translate robot command to format simulated robot uses """ - # Simulated robot just gets standard RobotCommand messages + """ Translate robot command to format simulated robot uses. """ + # Simulated robot just gets standard RobotCommand messages. msg = RobotCommand() - # add header + # Add header. msg.header = Header() msg.header.stamp = rospy.Time.now() - # add message content + # Add message content. msg.command = data.command msg.properties = data.properties - # send message + # Send message. self.robot_sim_pub.publish(msg) rospy.loginfo("Forwarding message to simulated robot:\n" + str(msg)) def send_to_other_robot(self, data): - """ Translate robot command to format other robot uses """ - # fill in to send commands to other robot + """ Translate robot command to format other robot uses. """ + # Fill in to send commands to other robot. rospy.logwarn("TODO send to other robot!") if __name__ == '__main__': - # run the node! + # Run the node! node = robot_translation() node.run_robot_translation_node()