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)