class PredictionNode:
def __init__(self):
self.lastLocations = Locations()
self.home1 = HomeRobot()
self.sampling = False
def pendSamplingSemaphore(self):
while self.sampling:
pass
self.sampling = True
def postSamplingSemaphore(self):
self.sampling = False
def callback(self, data):
# parse message
measuredLocations = Locations()
measuredLocations.setLocationsFromMeasurement(data)
self.pendSamplingSemaphore()
self.lastLocations = measuredLocations
self.home1.updateLocation(self.lastLocations.home1)
self.postSamplingSemaphore()
def getPredictionsData(self, req):
return predictionsResponse(*(self.lastSample.getDiscreteSample()))
def getLocations(self, req):
newLocations = copy.deepcopy(self.lastLocations)
newLocations.home1 = self.home1.getLocation()
return curlocsResponse(pickle.dumps(newLocations))
# def getBallLoc(self,req):
# return balllocResponse(getTime(), self.ball.point.x, self.ball.point.y)
def recieveCommand(self, req):
command = RobotCommand(req.timestamp, req.vel_x, req.vel_y, req.omega)
self.home1.addCommand(command)
return commandsentResponse()
def server(self):
rospy.Service('predictionsService', predictions,
self.getPredictionsData)
rospy.Service('locations', curlocs, self.getLocations)
# rospy.Service('ballloc', ballloc, self.getBallLoc)
rospy.Service('commandSent', commandsent, self.recieveCommand)
def listener(self):
rospy.init_node('kalmanFilterNode')
# This subscribes to the velTopic topic expecting the 'velocities' message
rospy.Subscriber("locTopic", locations, self.callback)
def run(self):
print "Welcome to the motion prediction node:\nProgram starting"
self.listener()
print "Starting services."
self.server()
i = 0
rospy.spin()
class PredictionNode:
def __init__(self):
self.lastLocations = Locations()
self.home1 = HomeRobot()
self.sampling = False
def pendSamplingSemaphore(self):
while self.sampling:
pass
self.sampling = True
def postSamplingSemaphore(self):
self.sampling = False
def callback(self, data):
# parse message
measuredLocations = Locations()
measuredLocations.setLocationsFromMeasurement(data)
self.pendSamplingSemaphore()
self.lastLocations = measuredLocations
self.home1.updateLocation(self.lastLocations.home1)
self.postSamplingSemaphore()
def getPredictionsData(self, req):
return predictionsResponse(*(self.lastSample.getDiscreteSample()))
def getLocations(self, req):
newLocations = copy.deepcopy(self.lastLocations)
newLocations.home1 = self.home1.getLocation()
return curlocsResponse(pickle.dumps(newLocations))
# def getBallLoc(self,req):
# return balllocResponse(getTime(), self.ball.point.x, self.ball.point.y)
def recieveCommand(self, req):
command = RobotCommand(req.timestamp, req.vel_x, req.vel_y, req.omega)
self.home1.addCommand(command)
return commandsentResponse()
def server(self):
rospy.Service("predictionsService", predictions, self.getPredictionsData)
rospy.Service("locations", curlocs, self.getLocations)
# rospy.Service('ballloc', ballloc, self.getBallLoc)
rospy.Service("commandSent", commandsent, self.recieveCommand)
def listener(self):
rospy.init_node("kalmanFilterNode")
# This subscribes to the velTopic topic expecting the 'velocities' message
rospy.Subscriber("locTopic", locations, self.callback)
def run(self):
print "Welcome to the motion prediction node:\nProgram starting"
self.listener()
print "Starting services."
self.server()
i = 0
rospy.spin()
def __init__(self):
self.lastLocations = Locations()
self.home1 = HomeRobot()
self.sampling = False
def __init__(self):
self.lastLocations = Locations()
self.home1 = HomeRobot()
self.sampling = False
