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