def execute(self, Rover):
"""Execute the Park state action."""
# Transform home coordinates to rover frame
home_pixpts_rf = world_to_rover(self.home_pixpts_wf, Rover.pos,
Rover.yaw)
home_distances, home_headings = to_polar_coords(home_pixpts_rf)
# Update Rover home polar coordinates
Rover.home_heading = np.mean(home_headings)
# Brake if still moving
if Rover.vel > self.MIN_VEL:
Rover.throttle = 0
Rover.brake = self.BRAKE_SET
Rover.steer = 0
# Otherwise orient to within +/- 10 deg of heading at start time
elif Rover.vel <= self.MIN_VEL:
# Yaw left if rock sample to left more than 10 deg
if Rover.home_heading >= 10:
Rover.throttle = 0
Rover.brake = 0
Rover.steer = self.YAW_LEFT_SET
# Yaw right if rock sample to right more than 10 deg
elif Rover.home_heading <= -10:
Rover.throttle = 0
Rover.brake = 0
Rover.steer = self.YAW_RIGHT_SET
# Otherwise stop
elif -10 < Rover.home_heading < 10:
Rover.steer = 0
Rover.brake = self.BRAKE_SET
def nearest_rock_angle(Rover): # in radians
min_dist = 1000.
min_dist_idx = -1
for idx in range(len(Rover.samples_pos[0]) - 1):
test_rock_x = Rover.samples_pos[0][idx]
test_rock_y = Rover.samples_pos[1][idx]
rock_sample_dists = distance([test_rock_x, test_rock_y], Rover.pos)
if rock_sample_dists < min_dist:
min_dist = rock_sample_dists
min_dist_idx = idx
dists, angles = to_polar_coords(Rover.samples_pos[0][min_dist_idx],
Rover.samples_pos[1][min_dist_idx])
if min_dist_idx >= 0:
return angles
else:
return 0
def decision_step(Rover):
# Implement conditionals to decide what to do given perception data
# Here you're all set up with some basic functionality but you'll need to
# improve on this decision tree to do a good job of navigating autonomously!
# Example:
# Check if we have vision data to make decisions with
if Rover.nav_angles is not None:
# Check for Rover.mode status
if Rover.mode == 'forward':
# Check the extent of navigable terrain
if len(Rover.nav_angles) >= Rover.stop_forward:
# If mode is forward, navigable terrain looks good
# and velocity is below max, then throttle
if Rover.vel < Rover.max_vel:
# Set throttle value to throttle setting
Rover.throttle = Rover.throttle_set
else: # Else coast
Rover.throttle = 0
Rover.brake = 0
# Steering proportional to the deviation results in
# small offsets on straight lines and
# large values in corners and open areas, 0.2 value is trial and error
offset = 0.2 * np.std(Rover.nav_angles)
# Set steering to average angle+offset clipped to the range +/- 15
Rover.steer = np.clip(
np.mean((Rover.nav_angles + offset) * 180 / np.pi), -15,
15)
### The code below tries to find the pixels on worldmap that the rover has visited, so the rover
# does not explore those pixels. However, there may be bugs that only very few pixels are actually
# non-zero, although visually those areas are already blue.
# reverse the pixels back to x and y
xpix = Rover.nav_dists * np.cos(Rover.nav_angles)
ypix = Rover.nav_dists * np.sin(Rover.nav_angles)
xpix_world, ypix_world = perception.pix_to_world(
xpix, ypix, Rover.pos[0], Rover.pos[1], Rover.yaw,
Rover.worldmap.shape[0], Rover.scale)
# those are visited should have blue pixel > 0, so this mask return the unvisited area
mask = (Rover.worldmap[xpix_world, ypix_world, 2] == 0)
xpix_world_zero = xpix_world[mask]
ypix_world_zero = ypix_world[mask]
# after reducing the xpix_world and ypix_world to only unvisited area, use self-defined world_to_pix
# to map back to rover centric coord.
xpix_zero, ypix_zero = perception.world_to_pix(xpix_world_zero, ypix_world_zero, \
Rover.pos[0], Rover.pos[1], Rover.yaw, Rover.worldmap.shape[0], Rover.scale)
# print("mask shape is: ", np.sum(mask))
# print("xpix_world shape is: ", xpix_world.shape, "xpix_world_zero shape is: ", xpix_world_zero.shape)
# print("xpix shape is: ", xpix.shape, "xpix_zero shape is: ", xpix_zero.shape)
# get the reduced nav_dist and nav_angles
fresh_nav_dist, fresh_nav_angles = perception.to_polar_coords(
xpix_zero, ypix_zero)
if len(fresh_nav_angles) / len(
Rover.nav_angles
) > 0.7: # 70% of the area is unexplored
print("!!!!!!!!!!!!Explore new area!!!!!!!!!!!!!!!!!!")
Rover.steer = np.clip(
np.mean((fresh_nav_angles + offset) * 180 / np.pi),
-15, 15)
else:
Rover.steer = np.clip(
np.mean((Rover.nav_angles + offset) * 180 / np.pi),
-15, 15)
### below is another way of doing this, does not seem working as well
# furthest_dist = np.max(Rover.nav_dists)
# angle_at_fur_dist = Rover.nav_angles[np.argmax(Rover.nav_dists)]
# # print("furthest_dist is: ", furthest_dist, "angle_at_fur_dist is: ", angle_at_fur_dist, "np.argmax: ", np.argmax(Rover.nav_dists))
# in_range_pix = np.nonzero(np.logical_and(Rover.nav_angles>=angle_at_fur_dist-2, Rover.nav_angles<=angle_at_fur_dist+2))
# area_around_furthest_dist = np.sum(Rover.nav_dists[in_range_pix])
# # print("area_around_furthest_dist is: ", area_around_furthest_dist)
# # Now let's also find the global x, y value in Rover.worldmap
# xpix = furthest_dist * np.cos(angle_at_fur_dist)
# ypix = furthest_dist * np.sin(angle_at_fur_dist)
# xpix_world, ypix_world = perception.pix_to_world(xpix, ypix, Rover.pos[0], Rover.pos[1], Rover.yaw, Rover.worldmap.shape[0], Rover.scale)
# print("xpix_world and ypix_world are:", xpix_world, ypix_world)
# if furthest_dist > Rover.nav_dists_thres and np.abs(Rover.mean_angles - angle_at_fur_dist*180/np.pi) < 20 and \
# area_around_furthest_dist > Rover.area_thres and Rover.worldmap[xpix_world, ypix_world, :].all() == 0:
# print("!!!!!!!!!!!!Explore new map!!!!!!!!!!!!!!!!!!")
# Rover.steer = np.clip(angle_at_fur_dist * 180/np.pi, -15, 15)
# else:
# Rover.steer = np.clip(np.mean(Rover.nav_angles * 180/np.pi), -15, 15)
### Now we want to check if the rover gets stuck
# fill in the history list first, if not, update the list. This includes the vel, yaw and pos list
if None in Rover.vel_hist:
ind_None = next(i for i, j in enumerate(Rover.vel_hist)
if j is None)
Rover.vel_hist[ind_None] = Rover.vel
else:
Rover.vel_hist = Rover.vel_hist[1:] + [Rover.vel]
if None in Rover.yaw_hist:
ind_None = next(i for i, j in enumerate(Rover.yaw_hist)
if j is None)
Rover.yaw_hist[ind_None] = Rover.yaw
else:
Rover.yaw_hist = Rover.yaw_hist[1:] + [Rover.yaw]
if None in Rover.pos_hist:
ind_None = next(i for i, j in enumerate(Rover.pos_hist)
if j is (None, None))
Rover.pos_hist[ind_None] = Rover.pos
else:
Rover.pos_hist = Rover.pos_hist[1:] + [Rover.pos]
# check if it gets stuck, 4 criterias:
# 1) vel is around 0. 2) pos does not change much. 3) yaw does not change much (so it's not in stop mode)
# 4) time elapses more than 3 seconds, so when switching back from 'back' mode, it stays in forward mode
# for at least 3 seconds.
if None not in Rover.vel_hist and None not in Rover.yaw_hist and None not in Rover.pos_hist:
if Rover.vel_hist[0] < 0.1 and Rover.vel_hist[-1] < 0.1 and \
np.sqrt((Rover.pos_hist[-1][0] - Rover.pos_hist[0][0])**2 + (Rover.pos_hist[-1][1] - Rover.pos_hist[0][1])**2) < 2 \
and np.abs(Rover.yaw_hist[-1] - Rover.yaw_hist[0]) < 10 \
and (Rover.total_time - Rover.start_forward_time) > 3:
Rover.throttle = 0
Rover.brake = 0
Rover.steer = 0
Rover.start_back_time = Rover.total_time
Rover.mode = 'back'
### If there's a lack of navigable terrain pixels then go to 'stop' mode
elif len(Rover.nav_angles) < Rover.stop_forward:
# Set mode to "stop" and hit the brakes!
Rover.throttle = 0
# Set brake to stored brake value
Rover.brake = Rover.brake_set
Rover.steer = 0
Rover.mode = 'stop'
# If we're already in "stop" mode then make different decisions
elif Rover.mode == 'stop':
# If we're in stop mode but still moving keep braking
if Rover.vel > 0.2:
Rover.throttle = 0
Rover.brake = Rover.brake_set
Rover.steer = 0
# If we're not moving (vel < 0.2) then do something else
elif Rover.vel <= 0.2:
# Now we're stopped and we have vision data to see if there's a path forward
if len(Rover.nav_angles) < Rover.go_forward:
Rover.throttle = 0
# Release the brake to allow turning
Rover.brake = 0
# Turn range is +/- 15 degrees, when stopped the next line will induce 4-wheel turning
Rover.steer = -15 # Could be more clever here about which way to turn
# If we're stopped but see sufficient navigable terrain in front then go!
if len(Rover.nav_angles) >= Rover.go_forward:
# Set throttle back to stored value
Rover.throttle = Rover.throttle_set
# Release the brake
Rover.brake = 0
# Set steer to mean angle
offset = 0.2 * np.std(Rover.nav_angles)
Rover.steer = offset + np.clip(
np.mean((Rover.nav_angles + offset) * 180 / np.pi),
-15, 15)
# Rover.steer = np.clip(np.mean(Rover.nav_angles * 180/np.pi), -15, 15)
Rover.mode = 'forward'
# If we're in 'back' mode then make different decisions
elif Rover.mode == 'back':
# if stuck in back mode for 5 seconds, maybe something is in the back. We either forward or stop.
if Rover.total_time - Rover.start_back_time > 5:
if len(Rover.nav_angles) >= Rover.stop_forward:
Rover.start_forward_time = Rover.total_time
Rover.mode = 'forward'
elif len(Rover.nav_angles) < Rover.stop_forward:
Rover.mode = 'stop'
# below situation is when the rover has not been stuck for 5 seconds
else:
# we have gain enough speed back
if Rover.vel < -0.5:
Rover.throttle = 0
Rover.brake = Rover.brake_set
Rover.steer = 0
Rover.mode = 'stop'
# we have not gained enough speed, keep backing
elif Rover.vel >= -0.5:
Rover.throttle = -Rover.throttle_set
Rover.brake = 0
Rover.steer = 0
# Just to make the rover do something
# even if no modifications have been made to the code
else:
Rover.throttle = Rover.throttle_set
Rover.steer = 0
Rover.brake = 0
# If in a state where want to pickup a rock send pickup command
if Rover.near_sample and Rover.vel == 0 and not Rover.picking_up:
Rover.send_pickup = True
return Rover
def execute(self, Rover):
"""Execute the ReturnHome state action."""
# Transform home coordinates to rover frame
home_pixpts_rf = world_to_rover(self.home_pixpts_wf, Rover.pos,
Rover.yaw)
home_distances, home_headings = to_polar_coords(home_pixpts_rf)
# Update Rover home polar coordinates
Rover.home_distance = np.mean(home_distances)
Rover.home_heading = np.mean(home_headings)
# Drive at a weighted average of home and nav headings with a 3:7 ratio
nav_heading = np.mean(Rover.nav_angles)
homenav_heading = 0.3 * Rover.home_heading + (1 - 0.3) * nav_heading
# Keep within max velocity
if Rover.vel < self.MAX_VEL:
Rover.throttle = self.MAX_THROTTLE_SET
else:
Rover.throttle = 0
# Approach at pure nav heading
if Rover.home_distance > 450:
Rover.brake = 0
Rover.steer = np.clip(nav_heading, self.YAW_RIGHT_SET,
self.YAW_LEFT_SET)
# Approach at the weighted average home and nav headings
elif 200 < Rover.home_distance <= 450:
Rover.brake = 0
Rover.steer = np.clip(homenav_heading, self.YAW_RIGHT_SET,
self.YAW_LEFT_SET)
# Slow down while keeping current heading
elif 100 < Rover.home_distance <= 200:
if Rover.vel < self.SLOW_VEL:
Rover.throttle = self.SLOW_THROTTLE_SET
else:
Rover.throttle = 0
Rover.brake = 0
Rover.steer = np.clip(homenav_heading, self.YAW_RIGHT_SET,
self.YAW_LEFT_SET)
# Precisely approach at pure home heading and slow down for parking
elif Rover.home_distance <= 100:
if Rover.vel > self.PARK_VEL:
Rover.throttle = 0
Rover.brake = self.BRAKE_SET
Rover.steer = 0
elif Rover.vel <= self.PARK_VEL:
Rover.brake = 0
# yaw left if home to the left more than 23 deg
if Rover.home_heading >= 23:
Rover.throttle = 0
Rover.steer = self.YAW_LEFT_SET
# yaw right if home to the right more than 23 deg
elif Rover.home_heading <= -23:
Rover.throttle = 0
Rover.steer = self.YAW_RIGHT_SET
# otherwise tread slowly at pure home heading
elif -23 < Rover.home_heading < 23:
Rover.throttle = self.PARK_THROTTLE_SET
Rover.steer = np.clip(Rover.home_heading,
self.YAW_RIGHT_SET,
self.YAW_LEFT_SET)
def decision_step(Rover):
# Implement conditionals to decide what to do given perception data
# Here you're all set up with some basic functionality but you'll need to
# improve on this decision tree to do a good job of navigating autonomously!
# exit if we are home
if Rover.back_home == True:
return Rover
# stop if we are near a sample
if ((Rover.near_sample == 1) & (Rover.pickup_started == 0)):
Rover.throttle = 0
Rover.brake = Rover.brake_set
Rover.mode = 'stop'
#print(timestampstring(),'stopping to pickup')
# restart if a pickup finished
if (Rover.pickup_started == 1) & (Rover.near_sample != 1):
Rover.mode = 'forward'
Rover.brake = 0
Rover.throttle = Rover.throttle_set
Rover.pickup_started = 0
Rover.rocks_picked_up += 1
Rover.pickup_location = Rover.pos
#print(timestampstring(),'restarting')
# Example:
# Check if we have vision data to make decisions with
if Rover.nav_angles is not None:
# Check for Rover.mode status
if Rover.mode == 'forward':
# Check the extent of navigable terrain
if len(Rover.nav_angles) >= Rover.stop_forward:
# If mode is forward, navigable terrain looks good
# and velocity is below max, then throttle
if Rover.vel < Rover.max_vel:
# Set throttle value to throttle setting
Rover.throttle = Rover.throttle_set
else: # Else coast
Rover.throttle = 0
Rover.brake = 0
# At the goal?
if (len(Rover.goal) > 0) & (Rover.success == True):
dists, angles = to_polar_coords(Rover.goal[0],
Rover.goal[1])
np.append(Rover.nav_angles, [angles])
#print('steering perferentially towards goal dist=', dist)
if (distance(Rover.pos, Rover.goal) < 3.0):
Rover.throttle = 0
# Set brake to stored brake value
Rover.brake = Rover.brake_set
Rover.steer = 0
Rover.mode = 'stop'
Rover.back_home = True
print(timestampstring(), " Goal reached.... stopping.")
# stuck?
if im_stuck(Rover) | (Rover.near_sample == 1) | (
Rover.success == True
): # don't add the steering bias when stuck or near a rock or coming home
totalmean = np.mean(radians_to_degrees(Rover.nav_angles))
else:
totalmean = np.median(radians_to_degrees(Rover.nav_angles))
if totalmean < 0.5:
totalmean = np.mean(
radians_to_degrees(Rover.nav_angles) +
Rover.steering_bias)
nearest_rock = radians_to_degrees(
nearest_rock_angle(Rover))
#totalmean = np.mean([totalmean,nearest_rock/10])
#print('nearest rock angle in degrees',nearest_rock)
# steer preferentially towards a rock
if Rover.rock_angles is not None:
if len(Rover.rock_angles) > 0:
totalmean = np.mean([
totalmean / 20,
radians_to_degrees(Rover.rock_angles[0])
])
#print(timestampstring(),'steering towards rock 1', Rover.rock_dists[0])
Rover.steer = np.clip(totalmean, -15, 15)
#Rover.pid.setPoint(np.clip(totalmean, -Rover.max_steer, Rover.max_steer))
#Rover.steer = Rover.pid.update(Rover.steer)
# check for stuck
if Rover.pickup_started == 0:
if Rover.near_sample == 0:
if (Rover.vel < 0.2) & (Rover.vel >= 0.0):
if distance(Rover.pickup_location,
Rover.pos) > 1.0:
Rover.stuck_count += 1
if Rover.stuck_count == 1:
Rover.stuck_time = time.time()
if im_stuck(Rover):
Rover.mode = 'backward'
print(timestampstring(), "Stuck",
Rover.stuck_count, Rover.steer,
Rover.brake, Rover.throttle,
Rover.vel)
# are we in stuck mode but moving? then we got unstuck
if im_stuck(Rover):
if Rover.vel >= 0.2:
Rover.stuck_count = 0
Rover.mode = 'forward'
print(timestampstring(), "UnStuck", Rover.stuck_count,
Rover.steer, Rover.brake, Rover.vel)
# If there's a lack of navigable terrain pixels then go to 'stop' mode
elif len(Rover.nav_angles) < Rover.stop_forward:
# Set mode to "stop" and hit the brakes!
Rover.throttle = 0
# Set brake to stored brake value
Rover.brake = Rover.brake_set
#Rover.steer = 0
Rover.mode = 'stop'
# we're stuck, back out...
if Rover.mode == 'backward':
if (Rover.backup_count > Rover.backup_threshold) | (
(Rover.backup_count >
(Rover.backup_threshold / 2)) & closeto(Rover.vel, 0.0, 0.2)):
Rover.backup_count = 0
Rover.stuck_count = 0
Rover.steer = -Rover.max_steer
if balance_to_right(Rover):
Rover.steer = -Rover.max_steer # turn right
Rover.mode = 'stop'
Rover.throttle = 0
print(timestampstring(), "Stopping", Rover.backup_count,
Rover.steer, Rover.brake, Rover.throttle, Rover.vel)
else:
Rover.steer = Rover.max_steer
if balance_to_right(Rover):
Rover.steer = Rover.max_steer # turn left
Rover.brake = 0
Rover.throttle = -Rover.throttle_set
Rover.backup_count += 1
if Rover.backup_count == Rover.backup_threshold:
print(timestampstring(), "Backing up", Rover.backup_count,
Rover.steer, Rover.brake, Rover.throttle, Rover.vel)
# If we're already in "stop" mode then make different decisions
if Rover.mode == 'stop':
# If we're in stop mode but still moving keep braking
if Rover.vel > 0.2:
Rover.throttle = 0
Rover.brake = Rover.brake_set
#???Rover.steer = 0
# If we're not moving (vel < 0.2) then do something else
elif Rover.vel <= 0.2:
# Now we're stopped and we have vision data to see if there's a path forward
if len(Rover.nav_angles) < Rover.go_forward:
Rover.throttle = 0
# Release the brake to allow turning
Rover.brake = 0
# Turn range is +/- 15 degrees, when stopped the next line will induce 4-wheel turning
if (Rover.steer != -Rover.max_steer) & (Rover.steer < 0.):
Rover.steer = Rover.max_steer
elif (Rover.steer != Rover.max_steer) & (Rover.steer > 0.):
Rover.steer = -Rover.max_steer
else:
Rover.steer = -Rover.max_steer
# If we're stopped but see sufficient navigable terrain in front then go!
if len(Rover.nav_angles) >= Rover.go_forward:
# Set throttle back to stored value
Rover.throttle = Rover.throttle_set
# Release the brake
Rover.brake = 0
Rover.mode = 'forward'
# Just to make the rover do something
# even if no modifications have been made to the code
else:
Rover.throttle = Rover.throttle_set
Rover.steer = 0
Rover.brake = 0
print('just do something')
# pickup sample if nearby
if ((Rover.near_sample == 1) & (Rover.pickup_started == 0)):
Rover.pick_up = 1
Rover.pickup_started = 1
return Rover
def decision_step(Rover):
# Implement conditionals to decide what to do given perception data
# Here you're all set up with some basic functionality but you'll need to
# improve on this decision tree to do a good job of navigating autonomously!
# For PICKING UP
if Rover.picking_up:
rock_x = int(round(Rover.pos[0]))
rock_y = int(round(Rover.pos[1]))
print('picking_up:', rock_x, rock_y)
cv2.circle(Rover.worldmap3, (rock_x, rock_y), 4, (255, 0, 255))
return Rover
# If in a state where want to pickup a rock send pickup command
elif Rover.near_sample:
if Rover.vel != 0:
print('near_sample: will stop', Rover.vel)
Rover.throttle = 0
Rover.brake = Rover.brake_set
else:
print('near_sample: will pickup')
Rover.send_pickup = True
Rover.mode = 'forward'
return Rover
# For RETURN
if Rover.samples_collected >= Rover.samples_to_find - 1:
ret_dist = np.linalg.norm(
np.int_(Rover.pos) - np.int_(Rover.start_pos))
if ret_dist < 2:
print('return: arrived, dist={:.1f}'.format(ret_dist))
Rover.throttle = 0
Rover.brake = Rover.brake_set
Rover.arrived = True
return Rover
init = [int(Rover.pos[1]), int(Rover.pos[0])]
goal = [int(Rover.start_pos[1]), int(Rover.start_pos[0])]
world_vis = resize_point(Rover.world_vis, size=4, intensity=1)
grid = Rover.worldmap[:, :, 2] + world_vis
Rover.world_ret = a_star_search(grid > 0, init, goal)
y_ret, x_ret = Rover.world_ret.nonzero()
if len(y_ret) == 0:
print('return: no path to', Rover.start_pos, 'from', Rover.pos)
return Rover
xr_rover, yr_rover = world_to_rover(x_ret, y_ret, Rover.pos[0],
Rover.pos[1], Rover.yaw,
Rover.worldmap.shape[0], 10)
vision_ret = np.zeros_like(Rover.vision_image[:, :, 2], dtype=np.uint8)
rover_coords_to_image(xr_rover, yr_rover, vision_ret)
vision_ret = resize_point(vision_ret, size=30, intensity=255)
vision_ret[:, :] = (
(vision_ret - Rover.vision_image[:, :, 0]) > 0) * Rover.vision_mask
yy, xx = vision_ret.nonzero()
Rover.vision_image2[:, :, :] = 0
Rover.vision_image2[yy, xx, 1:] = 255
xr_rover, yr_rover = rover_coords(vision_ret)
Rover.ret_dists, Rover.ret_angles = to_polar_coords(xr_rover, yr_rover)
Rover.steer_bias = 0
go_forward = 0
if len(Rover.ret_dists) < 5:
print('return: zero ret_dists')
Rover.brake = 0
Rover.throttle = 0
Rover.steer = -15
Rover.mode = 'forward'
stop_forward = len(Rover.go_dirs)
else:
Rover.go_dirs = Rover.ret_angles
stop_forward = len(Rover.go_dirs)
Rover.mode = 'forward'
# For SEARCH
else:
if Rover.rock_angles is not None and len(Rover.rock_angles) != 0:
print('search: for the rock_dir')
Rover.throttle = 0.05
Rover.steer_bias = 0
Rover.go_dirs = Rover.rock_angles
go_forward = 0
stop_forward = len(Rover.go_dirs)
else:
Rover.steer_bias = 10
Rover.go_dirs = Rover.nav_angles
stop_forward = Rover.stop_forward
go_forward = Rover.go_forward
# Check if we have vision data to make decisions with
if Rover.go_dirs is not None:
# Check for Rover.mode status
if Rover.mode == 'forward':
# Check the extent of navigable terrain
if len(Rover.go_dirs) >= stop_forward:
if Rover.vel < 0.001:
if Rover.prev_yaw and abs(Rover.prev_yaw - Rover.yaw) < 5:
print('forward: stuck, yaw={}<>{}'.format(
Rover.prev_yaw, Rover.yaw))
Rover.steer = -15
Rover.throttle = 0
Rover.brake = 0
else:
print('forward: not moving, yaw=', Rover.yaw)
Rover.steer = 0
Rover.throttle = Rover.throttle_set
Rover.brake = 0
Rover.prev_yaw = Rover.yaw
return Rover
# If mode is forward, navigable terrain looks good
# and velocity is below max, then throttle
if Rover.vel < Rover.max_vel:
# Set throttle value to throttle setting
Rover.throttle = Rover.throttle_set
else: # Else coast
Rover.throttle = 0
Rover.brake = 0
Rover.steer = np.clip(
np.mean(Rover.go_dirs * 180 / np.pi) + Rover.steer_bias,
-15, 15)
# Set steering to average angle clipped to the range +/- 15
print(
'forward: left steer_bias={}, steer={:.1f}, throttle={:.1f}, vel={:.1f}'
.format(Rover.steer_bias, Rover.steer, Rover.throttle,
Rover.vel))
# If there's a lack of navigable terrain pixels then go to 'stop' mode
elif len(Rover.go_dirs) < stop_forward:
print('forward: will stop, for angles:', len(Rover.go_dirs),
'< stop_forward:', stop_forward)
# Set mode to "stop" and hit the brakes!
Rover.throttle = 0
# Set brake to stored brake value
Rover.brake = Rover.brake_set
Rover.steer = 0
Rover.mode = 'stop'
# If we're already in "stop" mode then make different decisions
elif Rover.mode == 'stop':
# If we're in stop mode but still moving keep braking
if Rover.vel > 0.2:
print('stop: will brake, for vel:', Rover.vel, '> 0.2')
Rover.throttle = 0
Rover.brake = Rover.brake_set
Rover.steer = 0
# If we're not moving (vel < 0.2) then do something else
elif Rover.vel <= 0.2:
# Now we're stopped and we have vision data to see if there's a path forward
if len(Rover.go_dirs) < go_forward:
print('stop: turn, for short nav_dirs={} at yaw={}'.format(
len(Rover.go_dirs), Rover.yaw))
Rover.throttle = 0
# Release the brake to allow turning
Rover.brake = 0
# Turn range is +/- 15 degrees, when stopped the next line will induce 4-wheel turning
Rover.steer = -15 # Could be more clever here about which way to turn
# If we're stopped but see sufficient navigable terrain in front then go!
elif len(Rover.go_dirs) >= go_forward:
print('stop: now go, for large nav_dirs{} at trottle={}'.
format(len(Rover.go_dirs), Rover.throttle))
# Set throttle back to stored value
Rover.throttle = Rover.throttle_set
# Release the brake
Rover.brake = 0
# Set steer to mean angle
Rover.steer = np.clip(
np.mean(Rover.go_dirs * 180 / np.pi) +
Rover.steer_bias, -15, 15)
Rover.mode = 'forward'
# Just to make the rover do something
# even if no modifications have been made to the code
else:
print('Rover.go_dirs zero')
Rover.throttle = Rover.throttle_set
Rover.steer = 0
Rover.brake = 0
return Rover