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