def nav_loop():
robot = UgvModel()
state = RobotState()
path_tracker = PurePursuit()
wp = WayPointProvider() # use whatever WaypointProvider
mpu_valid = -1
print "waiting for GPS fix"
gps_service = GpsService()
current_pos = gps_service.getPosition()
while not current_pos.valid:
time.sleep(1)
current_pos = gps_service.getPosition()
LocalCoordinates
while mpu_valid < 0:
time.sleep(1.0)
mpu_valid = mpu9150.mpuInit(1, 50, 4)
print mpu_valid
mpu9150.setMagCal('./magcal.txt')
mpu9150.setAccCal('./accelcal.txt')
for i in range(1000): # flushing a bit of sensor fifo to stabilize sensor
time.sleep(1/CONTROL_RATE)
mpu9150.mpuRead()
robot.setSteeringAngle(0.0)
robot.setSpeed(0)
while True:
#robot.resetWatchdog()
target_pos= wp.getCurrentWayPoint()
if target_pos == None: # done with the path
break
origin_pos = wp.getPreviousWaypoint()
sensors = populateSensorMap(robot, gps_service)
if sensor[IController.imu_key][0] < 0 : #invalid imu sample, wait a bit ot read next
time.sleep(0.01)
continue
if new_gps_fix :
state_estimate_service.computeEKF(sensor[IController.imu_key][1][2], sensor[IController.odometry_key], , , DT)
else:
state_estimate_service.computeEKF(sensor[IController.imu_key][2], sensor[IController.odometry_key], None, None, DT)
# need to convert gps positions to XY positions
if tracker_counter == TRACKER_RATE:
path_curvature = path_tracker.computeSteering(EuclidianPoint(), EuclidianPoint(), robot.getXYPos(), robot.getEuler()[2])
tracker_counter = 0
else:
tracker_counter = tracker_counter + 1
#compute command from path_curvature
#steering can be extracted from curvature while speed must be computed from curvature and max_speed
steering = sinh(WHEEL_BASE * path_curvature)
#command needs to be computed for speed using PID control or direct P control
speed = speedFromSteering(steering))
robot.setSpeed(0.0)
robot.setSteeringAngle(0.0)
robot.setSteeringAngle(0.0)
robot.setSpeed(0)
print "Shutdown ESC then quit programm (for safety reason)"
while True:
time.sleep(1)
def nav_loop():
robot = UgvPlatform()
#robot.setColorLut('lut_file.lut')
#robot.initImu('accelcal.txt', 'magcal.txt')
mpu_valid = -1
print "waiting for GPS fix"
gps_service = GpsService()
current_pos = gps_service.getPosition()
while not current_pos.valid:
time.sleep(1)
current_pos = gps_service.getPosition()
print current_pos.valid
while mpu_valid < 0:
time.sleep(1.0)
mpu_valid = mpu9150.mpuInit(1, 10, 4)
print mpu_valid
mpu9150.setMagCal('./magcal.txt')
mpu9150.setAccCal('./accelcal.txt')
#time.sleep(4)
wp = StaticWayPointProvider()
controller = Controller()
threads.append(controller)
#gps_service.start()
robot.setSteeringFailSafe(0.0)
robot.setSpeedFailSafe(ESC_ARM_ANGLE)
robot.resetWatchdog()
robot.setSteeringAngle(0.0)
robot.setSpeed(0)
while True:
robot.resetWatchdog()
target_pos = wp.getCurrentWayPoint()
sensors = populateSensorMap(robot, gps_service)
current_pos = sensors[Controller.gps_key]
imu_sample = sensors[Controller.imu_key]
if imu_sample[0] < 0:
time.sleep(0.05)
continue
#print "lat : "+str(current_pos.lat)+" lon :"+str(current_pos.lon)+"imu:"+str(imu_sample)
# continue
cmd = controller.getCommand(sensors)
if cmd == None:
break
print str(cmd)
if cmd.has_key(Controller.next_waypoint_key
) and cmd[Controller.next_waypoint_key] != None and cmd[
Controller.next_waypoint_key] == 1:
wp.getNextWaypoint()
else:
robot.setSteeringAngle(cmd[Controller.steering_key])
robot.setSpeed(cmd[Controller.speed_key])
time.sleep(0.100)
robot.setSteeringAngle(0.0)
robot.setSpeed(0)
print "Shutdown ESC then quit programm (for safety reason)"
while True:
time.sleep(1)
def nav_loop():
robot = UgvPlatform()
#robot.setColorLut('lut_file.lut')
#robot.initImu('accelcal.txt', 'magcal.txt')
mpu_valid = -1
print "waiting for GPS fix"
gps_service = GpsService()
current_pos = gps_service.getPosition()
while not current_pos.valid:
time.sleep(1)
current_pos = gps_service.getPosition()
print current_pos.valid
while mpu_valid < 0:
time.sleep(1.0)
mpu_valid = mpu9150.mpuInit(1, 10, 4)
print mpu_valid
mpu9150.setMagCal('./magcal.txt')
mpu9150.setAccCal('./accelcal.txt')
#time.sleep(4)
wp = StaticWayPointProvider()
controller = Controller()
threads.append(controller)
#gps_service.start()
robot.setSteeringFailSafe(0.0)
robot.setSpeedFailSafe(ESC_ARM_ANGLE)
robot.resetWatchdog()
robot.setSteeringAngle(0.0)
robot.setSpeed(0)
while True:
robot.resetWatchdog()
target_pos= wp.getCurrentWayPoint()
sensors = populateSensorMap(robot, gps_service)
current_pos = sensors[Controller.gps_key]
imu_sample = sensors[Controller.imu_key]
if imu_sample[0] < 0 :
time.sleep(0.05)
continue
#print "lat : "+str(current_pos.lat)+" lon :"+str(current_pos.lon)+"imu:"+str(imu_sample)
# continue
cmd = controller.getCommand(sensors)
if cmd == None:
break
print str(cmd)
if cmd.has_key(Controller.next_waypoint_key) and cmd[Controller.next_waypoint_key] != None and cmd[Controller.next_waypoint_key] == 1:
wp.getNextWaypoint()
else:
robot.setSteeringAngle(cmd[Controller.steering_key])
robot.setSpeed(cmd[Controller.speed_key])
time.sleep(0.100)
robot.setSteeringAngle(0.0)
robot.setSpeed(0)
print "Shutdown ESC then quit programm (for safety reason)"
while True:
time.sleep(1)
