def main():
BB.Initialize(2001, fmap)
BB.Start()
BB.SetReady(True)
print 'Waiting for commands...'
BB.Wait()
def initialize():
BB.Initialize(2100, fmap)
BB.Start()
BB.SetReady(True)
BB.SubscribeToSharedVar('recognizedSpeech',mySubscriptionHandler)
BB.SubscribeToSharedVar('hypothesizedSpeech',myHandlerHypothesis)
def initialize():
BB.Initialize(2100, fmap)
BB.Start()
BB.CreateSharedVar(BB.SharedVarTypes.STRING, 'actuator')
BB.WriteSharedVar(BB.SharedVarTypes.STRING, 'actuator', 'initial_value')
BB.SetReady(True)
def main():
BB.Initialize(2003)
BB.Start()
BB.SetReady(True)
BB.SubscribeToSharedVar('test_string', printingFunc)
print 'Waiting for shared variable updates...'
BB.Wait()
def writeVar():
print 'What would you like to write?'
s = raw_input(':')
if BB.WriteSharedVar(SharedVarTypes.STRING, 'test_string', s):
print 'Written to var: ' + str(s)
else:
print 'Var NOT written'
def bridgeROSTopicData(self, topicData):
rospy.logdebug('ROS Topic "' + str(self.rosTopicName) +
'" updated from ROS. New value "' + str(topicData) +
'"')
#try to parse the ros topic data to a BB SV valid data
dicSuccess = False
try:
bbSharedVarValue = bridge_utils.ROS2BB_TYPE_MAP[
self.rosMsgType][1](topicData)
dicSuccess = True
except KeyError:
rospy.logfatal(
'Error ocurred when updating the Shared Var "' +
str(self.bbVarName) + '" from ROS. The "' +
str(self.rosMsgType) +
'" ROS MSG type doesn\'t have an associate function to parse a value. No value updated.'
)
except:
rospy.logfatal(
'An unexpected error ocurred when trying to update the Shared Var "'
+ str(self.bbVarName) + '" from ROS. No value updated.')
if dicSuccess:
if BB.WriteSharedVar(self.bbVarType, self.bbVarName,
bbSharedVarValue):
rospy.loginfo('BB shared Var "' + str(self.bbVarName) +
'" updated from ROS.')
else:
rospy.logwarn('BB Shared Var "' + str(self.bbVarName) +
'" not updated from ROS.')
def slowFunction(c):
print "slow function called..."
time.sleep(3)
r = BB.SendAndWait(Command('tst_testfunction', "This is another test."),
3000)
success = r and r.successful
return Response.FromCommandObject(c, success, 'Slow Command response')
def readVar():
r = BB.ReadSharedVar('test_string')
if r:
print 'Var read: ' + str(r)
else:
print 'Var NOT read'
def pass_var_value(self):
"""passVarValue
Get the value from the BB shared var and copy it to the ROS topic.
"""
#try to read the shared var maxAttempt times
maxAttempts = 3
sharedVarReaded = False
for currentAttempt in range(maxAttempts):
commandResponse = BB.SendAndWait(
Command('read_var', self.sharedVarName), 2000)
if commandResponse and commandResponse.successful:
if commandResponse.data != None:
#the variable was read, publish its value on his corresponding ROS topic
rosValue = str(commandResponse.data)
try:
rosValue = bridge_utils.BB2ROS_TYPE_MAP[
self.bbVarType][2](commandResponse.data)
self.rosPublisher.publish(rosValue)
rospy.loginfo('BB SharedVariable "' +
str(self.sharedVarName) +
'" value was published in the "' +
str(self.sharedVarName) + '" ROS Topic')
except KeyError:
rospy.logfatal(
'The "' + str(self.bbVarType) +
'" BB SV type doesn\'t have an associate function to parse a value. No value published.'
)
except:
rospy.logfatal(
'An unexpected error ocurrs when trying to parse a "'
+ str(self.bbVarType) +
'" BB SV type. No value published.')
sharedVarReaded = True
break
if not (sharedVarReaded):
#the variable was not read from BB, publish a default value in its corresponding ROS topic
try:
defaultValue = bridge_utils.BB2ROS_TYPE_MAP[
self.bbVarType][3]()
self.rosPublisher.publish(defaultValue)
rospy.logwarn('BB SharedVariable "' + str(self.sharedVarName) +
'" was not read. ROS Topic: "' +
str(self.sharedVarName) +
'" initialized with default: "' +
str(defaultValue + '"'))
except KeyError:
rospy.logfatal('No default ROS data defined for "' +
str(self.bbVarType) +
'" SV type. No data published on "' +
str(self.sharedVarName) + '" topic.')
except:
rospy.logfatal(
'An unexpected error ocurred during getting the ROS default value for '
+ str(self.bbVarType) +
' SV type. No data published on "' +
str(self.sharedVarName) + '" topic.')
return sharedVarReaded
def main():
initialize()
s = ''
while s != 'exit':
s = 'A1 motor on left 16:51:21'
BB.WriteSharedVar(BB.SharedVarTypes.STRING, 'actuator', s)
def main():
BB.Initialize(2000, fmap)
BB.Start()
BB.SetReady(True)
print 'Sending command say...'
print BB.SendAndWait(Command('spg_say', 'This is a test.'), 5000, 3)
print 'Sending Async command...'
ps = ParallelSender(
Command('othertst_slowfunction', 'This is another test.'), 5000, 3)
while ps.sending:
print 'sending...'
time.sleep(0.3)
print 'Response received...'
print ps.response
BB.Wait()
def main():
BB.Initialize(2002)
BB.Start()
BB.SetReady(True)
s = ''
while s != '4':
print '1. Create var'
print '2. Write var'
print '3. Read var'
print '4. exit'
s = raw_input(':')
if s == '1':
createVar()
elif s == '2':
writeVar()
elif s == '3':
readVar()
def initialize():
BB.Initialize(2100, fmap)
BB.Start()
BB.CreateSharedVar(BB.SharedVarTypes.STRING, 'actuator')
BB.WriteSharedVar(BB.SharedVarTypes.STRING, 'actuator', 'initial_value')
BB.SubscribeToSharedVar('actuator',ActuatorHandler, subscriptionType='writeothers', reportType='content')
BB.SetReady(True)
def Initialize():
BB.Initialize(2030, fmap)
BB.Start()
BB.CreateSharedVar(BB.SharedVarTypes.STRING, 'interface')
BB.CreateSharedVar(BB.SharedVarTypes.STRING, 'smarthause')
BB.SubscribeToSharedVar('smarthause',
LamparaHandler,
subscriptionType='writeothers',
reportType='content')
BB.SetReady()
def createBBSV(self, sharedVarName, sharedVarType):
#try to create a shared var
SVcreated = False
maxAttempts = 3
for i in range(maxAttempts):
if BB.CreateSharedVar(sharedVarType, sharedVarName):
SVcreated = True
break
#verify if the SV was created
if SVcreated:
rospy.logdebug('Shared var "' + str(sharedVarName) +
'" of type "' + str(sharedVarType) + '" created.')
else:
rospy.logfatal('Shared var "' + str(sharedVarName) +
'" of type "' + str(sharedVarType) +
'" cannot be created.')
return SVcreated
def commandCaller(self, req):
"""
Handle the ros service corresponding to the BB command to call
"""
#send the parameters to BB directly
rospy.logdebug('Sending parameters "' + str(req.parameters) + '" to command "' + str(self.commandName) + '" with timeout ' + str(req.timeout))
commandResponse = BB.SendAndWait(Command(self.commandName, req.parameters), req.timeout)
ret = Default_ROS_BB_BridgeResponse()
ret.success = False
ret.response = ''
if commandResponse and commandResponse.successful:
#return the command response
ret.success = True
ret.response = str(commandResponse.params)
rospy.logdebug('Response received from command "' + str(self.commandName) + '": ' + str(commandResponse.params))
return ret
def getBBSharedVars(inclussionList):
#initialize a dictionary and a list to parse and store the variables and its types
bbVarsDictionary = {}
sharedVarList = []
#try to read the shared var 'vars' maxAttempt times
maxAttempts = 3
sharedVarReaded = False
for currentAttempt in range(maxAttempts):
commandResponse = BB.SendAndWait(Command('read_var','vars'), 5000)
if commandResponse and commandResponse.successful:
#the variable list was read, create a list of var-vartype
sharedVarList = str(commandResponse.data).split(' ')
sharedVarReaded = True
break
#Parse the variable list, the format of the response is: varType varName varType varName ...
if sharedVarReaded:
#exclude, from the sv list, the non included svs in the inclussion ist
if len(inclussionList) > 0:
if inclussionList[0] != '*':
newSharedVarList = []
#sharedVarList = filter(lambda x: x not in inclussionList, sharedVarList)
index = 0
while index < len(sharedVarList) - 1:
if sharedVarList[index+1] in inclussionList:
newSharedVarList.append(sharedVarList[index])
newSharedVarList.append(sharedVarList[index+1])
index += 2
sharedVarList = newSharedVarList
index = 0
while index < len(sharedVarList) and index+1 < len(sharedVarList):
#add the varname-vartype relation to the dictionary
bbVarsDictionary[sharedVarList[index+1]] = sharedVarList[index]
index += 2
#Return the shared vars dictionary
return bbVarsDictionary
def __init__(self, sharedVarName, bbVarType):
"""Constructor
Creates a new BB2ROSPublisher Object.
Receives:
shredVarName: Name of the shared var to bridge.
bbVarType: The type of the var.
"""
self.sharedVarName = sharedVarName
try:
#get the msg data type for ros publisher and subscriber
self.bbVarType = bbVarType
self.rosMsgType = bridge_utils.BB2ROS_TYPE_MAP[self.bbVarType][1]
#create the corresponding ros publisher to update the ROS topic when updated the BB shared var
self.rosPublisher = rospy.Publisher(self.sharedVarName,
self.rosMsgType,
queue_size=1000)
rospy.logdebug('ROS publisher "' + str(self.rosMsgType) +
'" for "' + str(sharedVarName) + '" created.')
except:
rospy.logwarn('No ROS msg type associated with "' +
str(bbVarType) +
'" BB SV type. Using default association.')
#get the default msg data type for ros publisher and subscriber
self.bbVarType = 'default'
self.rosMsgType = bridge_utils.BB2ROS_TYPE_MAP[self.bbVarType][1]
#create the corresponding ros publisher to update the ROS topic when updated the BB shared var
self.rosPublisher = rospy.Publisher(self.sharedVarName,
self.rosMsgType,
queue_size=1000)
rospy.logdebug('ROS publisher "' + str(self.rosMsgType) +
'" for "' + str(sharedVarName) + '" created.')
#create a subscriber to read the values of the BB shared var
BB.SubscribeToSharedVar(self.sharedVarName, self.bridge_SV_data)
#read the value of the shared var from BB and publish it to ROS
self.pass_var_value()
def createVar():
if BB.CreateSharedVar(SharedVarTypes.STRING, 'test_string'):
print 'Var created'
else:
print 'Var NOT created'
def main():
bbConnectionPort = 2080;
topicsInclusionList = ["*"];
svInclusionList = ["*"];
srvInclusionList = [];
cmdInclusionList = [];
#load the config files
parser = OptionParser()
parser.add_option('-f', '--configfile', action='store', type='string', dest='configFile', help='Bridge rules')
(options, args) = parser.parse_args()
if options.configFile:
#parser.error('Configuration File not given')
#load the configuration options
with open(options.configFile) as config_file:
configOptions = json.load(config_file)
#print configOptions
try:
topicsInclusionList = configOptions['ros_topics']
except:
topicsInclusionList = []
try:
svInclusionList = configOptions['blackboard_sv']
except:
svInclusionList = []
try:
srvInclusionList = configOptions['ros_services']
except:
srvInclusionList = []
try:
cmdInclusionList = configOptions['blackboard_cmd']
except:
cmdInclusionList = []
try:
bbConnectionPort = configOptions["bb_port"]
except:
bbConnectionPort = 2080
#define the list of topics to be bridged and its msg types
callersMap = {
#'add_two_ints' : BB2ROS_ServiceCallers.add_two_ints_caller,
'default_server' : BB2ROS_DefaultServicesCallers.default_caller
}
#Manage BB connection
print 'Initializing BB connection'
BB.Initialize(bbConnectionPort, callersMap)
BB.Start()
BB.SetReady(True)
#Manage ROS connection
#rospy.init_node('bbros_bridge', log_level = rospy.DEBUG)
rospy.init_node('bbros_bridge')
#atend calls from ROS nodes to BB commands
for bbCommand in cmdInclusionList:
ROS2BB_CommandsCalls(bbCommand, Default_ROS_BB_Bridge)
#BRIDGE SHARED VARS AND TOPICS
#create a dictionary of bridges, for the BB shared vars and for the ROS topics
bb2rosPublishersDictionary = {}
ros2bbPublishersDictionary = {}
#initialize the dictionaries of shared vars and topics already bridged
bbVarsList = {}
rosTopicsDictionary = {}
#verify if new shared vars or ros topic where created
rate = rospy.Rate(10) # 1hz rate
while not rospy.is_shutdown():
#get the BB shared vars list, this list is a dictionary {varName:varType}
new_bbVarsList = getBBSharedVars(svInclusionList)
#get the ROS published topics list
new_rosTopicsDictionary = getROSTopicList(topicsInclusionList)
#verify if there are news bb sv not bridged yet
bbVarsToBridge = dictionaryDifference(new_bbVarsList, bbVarsList)
#verify if there are news ros topics not bridged yet
rosTopicsToBridge = dictionaryDifference(new_rosTopicsDictionary, rosTopicsDictionary)
if len(bbVarsToBridge) > 0:
rospy.logdebug('Current BB var-type dictionary: ' + str(bbVarsList))
rospy.logdebug('New BB var-type dictionary: ' + str(new_bbVarsList))
if len(rosTopicsToBridge) > 0:
rospy.logdebug('Current ROS topic-type dictionary: ' + str(rosTopicsDictionary))
rospy.logdebug('New ROS topic-type dictionary: ' + str(new_rosTopicsDictionary))
#bridge the bb shared vars to ros topics
bridge_BB2ROS_SharedVars(bbVarsToBridge, rosTopicsDictionary, bb2rosPublishersDictionary)
#bridge the ros topics to bb shared vars
bridge_ROS2BB_Topics(rosTopicsToBridge, bbVarsList, ros2bbPublishersDictionary)
#update the originals bb sv dictionary and ros topics dictionary
dictionaryAddition(bbVarsList, bbVarsToBridge)
dictionaryAddition(rosTopicsDictionary, rosTopicsToBridge)
rate.sleep()
def main():
BB.Initialize(2100, fmap)
BB.Start()
BB.SetReady(True)
BB.Wait()
def main():
global ban_ejec
global s
magnetTime = " "
movTime = " "
magnetStatus = " "
Initialize()
while True:
if ban_ejec == 1:
ban_ejec = 0
BB.WriteSharedVar(BB.SharedVarTypes.STRING, 'interface', s)
os.system('echo "st" | nc localhost 1099')
os.system('echo "st" | nc localhost 1099')
os.system('echo "st" | nc localhost 1099')
s = os.popen('echo "st" | nc localhost 1099').read(500)
i = 0
b_fin = 0
while b_fin == 0:
if s != "":
i = i + 1
if s[i] == "7":
i = i + 1
if s[i] == "7":
i = i + 1
if s[i] == "E":
i = i + 1
if s[i] == "D":
i = i + 1
if s[i] == "0":
i = i + 1
if s[i] == "0":
magnetTime = s[i + 8] + s[i + 9] + s[
i + 10] + s[i + 11] + s[i + 12]
print magnetTime
magnetStatus = s[i + 14] + s[
i +
15] + s[i + 16] + s[i + 17] + s[
i + 18] + s[i + 19] + s[
i + 20] + s[i + 21] + s[
i +
22] + s[i + 23] + s[
i + 24] + s[
i + 25] + s[
i +
26] + s[i +
27]
elif s[i] == "8":
i = i + 1
if s[i] == "8":
i = i + 1
if s[i] == "0":
i = i + 1
if s[i] == "2":
i = i + 1
if s[i] == "8":
i = i + 1
if s[i] == "0":
movTime = s[i + 8] + s[i + 9] + s[
i + 10] + s[i + 11] + s[i + 12]
print movTime
elif s[i] == "E":
i = i + 1
if s[i] == "n":
i = i + 1
if s[i] == "d":
b_fin = 1
s = magnetTime + " " + movTime + " " + magnetStatus
BB.WriteSharedVar(BB.SharedVarTypes.STRING, 'interface', s)
time.sleep(.5)