def update(self, time):
"""
Sends out events using the current estimated values
"""
self.depthFilter.addValue(
-3.281 * self.robot._main_part._node.position.z)
self.posXFilter.addValue(
self.robot._main_part._node.position.x)
self.posYFilter.addValue(
-self.robot._main_part._node.position.y)
self.position = math.Vector2(
self.posXFilter.getValue(),
self.posYFilter.getValue())
self.velocity = math.Vector2(
self.posXFilter.getValue(1, time),
self.posYFilter.getValue(1, time))
self.depth = self.depthFilter.getValue()
self.depthRate = self.depthFilter.getValue(1, time)
# Package all events
devent = math.NumericEvent()
devent.number = self.getEstimatedDepth()
drevent = math.NumericEvent()
drevent.number = self.getEstimatedDepthRate()
pevent = math.Vector2Event()
pevent.vector2 = self.getEstimatedPosition()
vevent = math.Vector2Event()
vevent.vector2 = self.getEstimatedVelocity()
oevent = math.OrientationEvent()
oevent.orientation = self.getEstimatedOrientation()
# Send all events at the same time
self.publish(estimation.IStateEstimator.ESTIMATED_DEPTH_UPDATE, devent)
self.publish(estimation.IStateEstimator.ESTIMATED_DEPTHRATE_UPDATE,
drevent)
self.publish(estimation.IStateEstimator.ESTIMATED_POSITION_UPDATE,
pevent)
self.publish(estimation.IStateEstimator.ESTIMATED_VELOCITY_UPDATE,
vevent)
self.publish(estimation.IStateEstimator.ESTIMATED_ORIENTATION_UPDATE,
oevent)
def update(self, time):
# On each update, we'll find the velocity using the old position
currentPos = math.Vector2(self.robot._main_part._node.position.x,
-self.robot._main_part._node.position.y)
# Subtract by the old position and divide by time
self.velocity = (currentPos - self.oldPos) / time
# Update the position
self.oldPos = currentPos
# Publish an update event
event = math.Vector2Event()
event.vector2 = self.getVelocity()
self.publish(device.IVelocitySensor.UPDATE, event)
def update(self, time):
event = math.Vector2Event()
event.vector2 = self.getPosition()
self.publish(device.IPositionSensor.UPDATE, event)
def publishPositionUpdate(self, vehiclePosition):
event = math.Vector2Event()
event.vector2 = vehiclePosition
self.publish(estimation.IStateEstimator.ESTIMATED_POSITION_UPDATE,
event)
def publishVelocityUpdate(self, vehicleVelocity):
event = math.Vector2Event()
event.vector2 = vehicleVelocity
self.publish(estimation.IStateEstimator.ESTIMATED_VELOCITY_UPDATE,
event)