Go to the documentation of this file.00001
00002
00003 import rospy
00004 from flexbe_msgs.msg import *
00005 from rospkg import RosPack, ResourceNotFound
00006
00007 from flexbe_core import Logger, BehaviorLibrary
00008 from std_msgs.msg import String
00009
00010 import pickle
00011 import zlib
00012 import difflib
00013 import os
00014 import yaml
00015 import xml.etree.ElementTree as ET
00016
00017 class BehaviorLauncher(object):
00018
00019 MIN_VERSION = '2.2.0'
00020
00021 def __init__(self):
00022 Logger.initialize()
00023
00024 self._sub = rospy.Subscriber("flexbe/request_behavior", BehaviorRequest, self._callback)
00025 self._version_sub = rospy.Subscriber("flexbe/ui_version", String, self._version_callback)
00026
00027 self._pub = rospy.Publisher("flexbe/start_behavior", BehaviorSelection, queue_size=100)
00028 self._status_pub = rospy.Publisher("flexbe/status", BEStatus, queue_size=100)
00029 self._mirror_pub = rospy.Publisher("flexbe/mirror/structure", ContainerStructure, queue_size=100)
00030
00031 self._rp = RosPack()
00032 self._behavior_lib = BehaviorLibrary()
00033
00034 rospy.loginfo("%d behaviors available, ready for start request." % self._behavior_lib.count_behaviors())
00035
00036
00037 def _callback(self, msg):
00038 be_id, behavior = self._behavior_lib.find_behavior(msg.behavior_name)
00039 if be_id is None:
00040 Logger.logerr("Did not find behavior with requested name: %s" % msg.behavior_name)
00041 self._status_pub.publish(BEStatus(code=BEStatus.ERROR))
00042 return
00043
00044 rospy.loginfo("Request for behavior " + behavior["name"])
00045
00046 be_selection = BehaviorSelection()
00047 be_selection.behavior_id = be_id
00048 be_selection.autonomy_level = msg.autonomy_level
00049 try:
00050 for k, v in zip(msg.arg_keys, msg.arg_values):
00051 if k.startswith('/YAML:'):
00052 key = k.replace('/YAML:', '/', 1)
00053 path = v.split(':')[0]
00054 ns = v.split(':')[1]
00055 if path.startswith('~') or path.startswith('/'):
00056 yamlpath = os.path.expanduser(path)
00057 else:
00058 yamlpath = os.path.join(self._rp.get_path(path.split('/')[0]), '/'.join(path.split('/')[1:]))
00059 with open(yamlpath, 'r') as f:
00060 content = yaml.load(f)
00061 if ns != '' and ns in content:
00062 content = content[ns]
00063 be_selection.arg_keys.append(key)
00064 be_selection.arg_values.append(yaml.dump(content))
00065 else:
00066 be_selection.arg_keys.append(k)
00067 be_selection.arg_values.append(v)
00068 except Exception as e:
00069 rospy.logwarn('Failed to parse and substitute behavior arguments, will use direct input.\n%s' % str(e))
00070 be_selection.arg_keys = msg.arg_keys
00071 be_selection.arg_values = msg.arg_values
00072
00073 be_structure = ContainerStructure()
00074 be_structure.containers = msg.structure
00075
00076 try:
00077 be_filepath_new = os.path.join(self._rp.get_path(behavior["package"]), 'src/' + behavior["package"] + '/' + behavior["file"] + '.py')
00078 except ResourceNotFound:
00079 rospy.logerr("Could not find behavior package '%s'" % (behavior["package"]))
00080 rospy.loginfo("Have you updated your ROS_PACKAGE_PATH after creating the behavior?")
00081 return
00082
00083 with open(be_filepath_new, "r") as f:
00084 be_content_new = f.read()
00085
00086 be_filepath_old = os.path.join(self._rp.get_path(behavior["package"]), 'src/' + behavior["package"] + '/' + behavior["file"] + '_tmp.py')
00087 if not os.path.isfile(be_filepath_old):
00088 be_selection.behavior_checksum = zlib.adler32(be_content_new)
00089 if msg.autonomy_level != 255:
00090 be_structure.behavior_id = be_selection.behavior_checksum
00091 self._mirror_pub.publish(be_structure)
00092 self._pub.publish(be_selection)
00093 rospy.loginfo("No changes to behavior version.")
00094 return
00095
00096 with open(be_filepath_old, "r") as f:
00097 be_content_old = f.read()
00098
00099 sqm = difflib.SequenceMatcher(a=be_content_old, b=be_content_new)
00100 diffs = [x[1] for x in sqm.get_grouped_opcodes(0)]
00101 for opcode, a0, a1, b0, b1 in diffs:
00102 content = be_content_new[b0:b1]
00103 be_selection.modifications.append(BehaviorModification(a0, a1, content))
00104
00105 be_selection.behavior_checksum = zlib.adler32(be_content_new)
00106 if msg.autonomy_level != 255:
00107 be_structure.behavior_id = be_selection.behavior_checksum
00108 self._mirror_pub.publish(be_structure)
00109
00110 self._pub.publish(be_selection)
00111
00112 def _version_callback(self, msg):
00113 vui = self._parse_version(msg.data)
00114 vex = self._parse_version(BehaviorLauncher.MIN_VERSION)
00115 if vui < vex:
00116 Logger.logwarn('FlexBE App needs to be updated!\n' \
00117 + 'Require at least version %s, but have %s\n' % (BehaviorLauncher.MIN_VERSION, msg.data) \
00118 + 'Please run a "git pull" in "roscd flexbe_app".')
00119
00120 def _parse_version(self, v):
00121 result = 0
00122 offset = 1
00123 for n in reversed(v.split('.')):
00124 result += int(n) * offset
00125 offset *= 100
00126 return result