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)
def nav_loop():
robot = Robot()
#initializing actuators and failsafe
robot.setSteeringAngle(0.0)
robot.setSpeed(0)
robot.setSpeedFailSafe(0)
robot.setSteeringFailSafe(0.0)
#initializing state estimate service
state = RobotState()
# initializing path tracker
path_tracker = TrackingService()
#initializing waypoint provider
wp = PlannerWayPointProvider(PLANNER_WAYPOINT_FILE_PATH) # use whatever WaypointProvider
mpu_valid = -1
print "waiting for GPS fix"
gps_service = GpsService()
current_pos = gps_service.getPosition()
# wainting to get a valid GPS fix. Maybe should also wait to get a good fix before starting
while not current_pos.valid:
time.sleep(1)
print "Waiting for GPS fix"
current_pos = gps_service.getPosition()
#initializing local coordinates system to start on current spot
coordinates_system = LocalCoordinates(current_pos)
old_gps = current_pos
#initiliaze mpu system
imu_service = ImuService(CONTROL_RATE) # initialize imu at given rate, wait until the IMU is initialized
#setting mpu calibration files (calibration should be re-run before every run)
imu_service.setCalibrationFiles('./magcal.txt', 'accelcal.txt')
for i in range(1000): # flushing a bit of sensor fifo to stabilize sensor
time.sleep(1/CONTROL_RATE)
imu_service.getAttitude()
# initalize speed service that reads encoders
speed_service = SpeedService()
#initialize PID variables
target_speed = 0.0
old_error = 0.0
integral = 0.0
start_script = 0
start_script_old = 0x01
run = False
tracker_counter = 0
path_curvature = 0.0
steering = 0.0
#reseting watchdog before we start
robot.resetWatchdog()
while True:
# need to read start button
start_script = robot.getPushButtons() & 0x01
# detecting rising edge to start the script (pb are active low)
if run == False and start_script_old == 0x00 and start_script == 0x01:
print "starting trajectory !!!!!"
run = True
elif run == True and start_script_old == 0x00 and start_script == 0x01:
print "stoping vehicle !!!!!"
run = False
start_script_old = start_script
#waypoint service provide the current waypoint
xy_target_pos= wp.getCurrentWayPointXY()
#no target point means path is done
if xy_target_pos == None: # done with the path
break
#get previous waypoint to draw the path between the two
xy_origin_pos = wp.getPreviousWayPointXY()
# populate the sensor structure to make sure we read all sensors only once per loop
sensors = populateSensorMap(robot, gps_service, speed_service, imu_service)
#imu has no valid sample, IMU helps keep the control loop rate as it delivers values at the right pace
if sensors[IController.imu_key][0] < 0 : #invalid imu sample, wait a bit to read next
time.sleep(0.01)
continue
#converting gps pos to the local coordinates system
xy_pos = coordinates_system.convertGpstoEuclidian(sensors[IController.gps_key])
new_gps_fix = (sensors[IController.gps_key].time != old_gps.time) and sensors[IController.gps_key].valid and old_gps.valid
old_gps = sensors[IController.gps_key]
# we have a new fix, integrate it to the kalman filter
robot_heading = sensors[IController.imu_key][1][2]
if new_gps_fix :
robot_state = state.computeEKF(robot_heading, sensors[IController.odometry_key], xy_pos.x, xy_pos.y, DT)
else:
robot_state = state.computeEKF(robot_heading, sensors[IController.odometry_key], None, None, DT)
# updating xy_pos to estimated pos
xy_pos = EuclidianPoint(robot_state[0], robot_state[1], xy_pos.time, True)
robot_heading = robot_state[2]
# execute tracker at a fraction of the control rate update
if tracker_counter >= (CONTROL_RATE/TRACKER_RATE):
path_curvature = path_tracker.computeSteering(xy_origin_pos, xy_target_pos, xy_pos, robot_heading)
#steering can be extracted from curvature while speed must be computed from curvature and max_speed
steering = -1 * math.sinh(path_curvature)*(180.0/math.pi) #should take into account wheel base, curvature is reversed compared to steering
print "Old Pos : "+str(xy_origin_pos.x)+", "+str(xy_origin_pos.y)
print "Next Pos : "+str(xy_target_pos.x)+", "+str(xy_target_pos.y)
print "Current Pos : "+str(xy_pos.x)+", "+str(xy_pos.y)
print "Heading : "+str(robot_heading)
print "New steering : "+str(steering)
tracker_counter = 0
else:
tracker_counter = tracker_counter + 1
# if we reached target, move to the next
distance_to_target = xy_pos.distanceTo(xy_target_pos)
if distance_to_target < DIST_TOLERANCE:
wp.getNextWayPoint()
# for tracker to be executed on next iteration
tracker_counter = (CONTROL_RATE/TRACKER_RATE)
#command needs to be computed for speed using PID control or direct P control
target_speed = speedFromSteeringAndDistance(steering, distance_to_target)
# trying to detect falling edge to start driving
if run == True :
# doing the PID math, PID term needs to be adjusted
error = (target_speed - sensors[IController.odometry_key])
derivative = error - old_error
old_error = error
cmd = error * P + derivative * D + integral * I
integral = integral + error
if cmd < 0.0 :
cmd = 0
if cmd > 127.0:
cmd = 127
# end of PID math
robot.setSpeed(cmd)
robot.setSteeringAngle(steering)
#reseting watchdog before for next iteration
robot.resetWatchdog()
else:
robot.setSpeed(0)
robot.setSteeringAngle(0.0)
# no need to sleep, we run at full speed !
#time.sleep(0.020)
# if ever we quit the loop, we need to set the speed to 0 and steering to 0
robot.setSteeringAngle(0.0)
robot.setSpeed(0)
print "Shutdown ESC then quit programm (for safety reason)"
while True:
time.sleep(1)
