def getEstimatedLinearAcceleration(self):
if self.robot is not None:
baseAccel = convertToVector3(math.Vector3,
self.robot._main_part.acceleration)
return baseAccel + math.Vector3(0, 0, 0)
else:
return math.Vector3.ZERO
def testObstacle(self):
estimator = MockStateEstimator({})
obstacle = estimation.Obstacle()
desiredLoc = math.Vector3(5, -5, 2)
obstacle.setLocation(desiredLoc)
estimator.addObstacle(estimation.Obstacle.RED_BUOY, obstacle)
self.assertEquals(
desiredLoc,
estimator.getObstacleLocation(estimation.Obstacle.RED_BUOY))
def _angleroll(self, event):
"""
When we want to roll the robot, send a command to the controller
to rotate the robot around the body x axis proportional to the
event command. When we receive a zero rate command, we should
only pass along the command to the controller ONCE.
"""
rate = event.number
if rate == 0 and self._lastRollCommand == 0:
# dont sent ZERO command twice
# note that we do want to send commands repeatedly if
# the speed is not zero
pass
else:
ori = self._estimator.getEstimatedOrientation()
self._controller.rotate(ori, math.Vector3(rate, 0, 0))
self._lastRollCommand = rate
def enter(self, timeout = 4):
self._timeout = self._config.get('timeout', timeout)
state.FindAttempt.enter(self, self._timeout)
ensureBinTracking(self.queuedEventHub, self.ai)
# Will be in the next commit
x = self.ai.data['lastBinX']
y = self.ai.data['lastBinY']
ahead = math.Vector3(x, y, 0)
quat = math.Vector3.UNIT_Y.getRotationTo(ahead)
self._direction = quat.getYaw(True)
self._speed = self._config.get('speed', 0.5)
self._delay = None
searchMotion = motion.basic.MoveDirection(self._direction, self._speed)
self.motionManager.setMotion(searchMotion)
def _setSpeeds(self, orientation):
"""
Steps the speeds to vehicle based on the given orientation
"""
# Direction of desired vehicle motion
desiredDirection = self._direction.getYaw(True).valueDegrees()
# Vehicle heading in degrees
vehicleHeading = orientation.getYaw(True).valueDegrees()
yawTransform = vehicleHeading - desiredDirection
# Find speed in vehicle cordinates
baseSpeed = math.Vector3(self._speed, 0, 0)
toVehicleFrame = math.Quaternion(math.Degree(yawTransform),
math.Vector3.UNIT_Z)
vehicleSpeed = toVehicleFrame * baseSpeed
# Finally set the speeds
self._controller.setSpeed(vehicleSpeed.x)
self._controller.setSidewaysSpeed(vehicleSpeed.y)
def getEstimatedAngularRate(self):
return math.Vector3(0, 0, 0)
def getEstimatedLinearAcceleration(self):
return math.Vector3(0, 0, 0)
def _roll_right(self, event):
ori = self._estimator.getEstimatedOrientation()
rate = event.number
self._controller.rotate(ori, math.Vector3(rate, 0, 0))
def _pitch_down(self, event):
ori = self._estimator.getEstimatedOrientation()
rate = event.number
self._controller.rotate(ori, math.Vector3(0, rate, 0))
def _yaw_left(self, event):
ori = self._estimator.getEstimatedOrientation()
rate = event.number
self._controller.rotate(ori, math.Vector3(0, 0, rate))
def getMagnetometer(self):
return self._getActualOrientation() * math.Vector3(0.5, 0, -1);
def getLinearAcceleration(self):
baseAccel = convertToVector3(math.Vector3,
self.robot._main_part.acceleration)
# Add in gravity
return baseAccel + math.Vector3(0, 0, -9.8)
def getLocation(self):
return math.Vector3(0, 0, 0)
def sendPing(x=0, y=0, z=0, pingerID=0):
event = ext.vehicle.SonarEvent()
event.direction = math.Vector3(x, y, z)
event.pingerID = pingerID
queuedEventHub.publish(ext.vehicle.device.ISonar.UPDATE, event)