Skip to content

Commit

Permalink
Merge branch 'line-pred' of https://github.com/sergionr2/RacingRobot
Browse files Browse the repository at this point in the history
…into line-pred
  • Loading branch information
Enstar committed Jun 17, 2018
2 parents dc64c93 + 4e42dcb commit ad366d4
Show file tree
Hide file tree
Showing 7 changed files with 51 additions and 18 deletions.
2 changes: 1 addition & 1 deletion README.md
Original file line number Diff line number Diff line change
Expand Up @@ -222,7 +222,7 @@ pip install -r requirements.txt
```

In short:
- PySerial
- PySerial
- TQDM (for progressbar)
- [PyGame](http://www.pygame.org/wiki/CompileUbuntu#Installing%20pygame%20with%20pip) (for teleoperation)
- Enum support (for Python 2)
Expand Down
8 changes: 3 additions & 5 deletions constants.py
Original file line number Diff line number Diff line change
Expand Up @@ -5,7 +5,7 @@

import numpy as np

# Main Constants
# Camera Constants
CAMERA_RESOLUTION = (640 // 2, 480 // 2)
# https://picamera.readthedocs.io/en/release-1.13/fov.html#sensor-modes
# mode 4, larger FoV but FPS <= 40
Expand Down Expand Up @@ -37,16 +37,14 @@
MIN_SPEED = 25

# Path Planning
# TODO: compute curvature of bezier path to regulate speed
# see https://stackoverflow.com/questions/46762955
TARGET_POINT = 0.5 # between [0, 1] position on the bezier curve

# Stanley Steering Control params
# Control gain (compromise between smooth control
# and precise track following)
K_STANLEY_CONTROL = 1
CAR_LENGTH = 1.5 # [m] Wheel base of vehicle
MAX_STEERING_ANGLE = np.radians(25.0) # [rad]
CAR_LENGTH = 300 # Wheel base of vehicle (in the warped pixel space)
MAX_STEERING_ANGLE = np.radians(30.0) # [rad]
# Radius of curvature
MIN_RADIUS = 5 # Below this value, we consider to be in a sharp turn
MAX_RADIUS = 40 # Above this value, we consider to be in a straight line
Expand Down
1 change: 1 addition & 0 deletions image_processing/warp_image.py
Original file line number Diff line number Diff line change
Expand Up @@ -44,6 +44,7 @@ def showTransform(image): # pragma: no cover
cv2.circle(im, (int(cx), int(cy)), 8, (0, 255, 0), -1)
imshow(im, name="transform")


def transformPoints(x, y):
points = []
for i in range(len(x)):
Expand Down
16 changes: 8 additions & 8 deletions path_planning/stanley_controller.py
Original file line number Diff line number Diff line change
Expand Up @@ -17,8 +17,8 @@
from constants import K_STANLEY_CONTROL, CAR_LENGTH, MAX_STEERING_ANGLE,\
MAX_SPEED_STRAIGHT_LINE, MIN_SPEED, MIN_RADIUS, MAX_RADIUS

Kp_speed = 5 # speed propotional gain
dt = 0.05 # [s] time difference
Kp_speed = 0.1 # speed propotional gain
dt = 1 # [s] time difference


class State(object):
Expand Down Expand Up @@ -108,14 +108,14 @@ def calcTargetIndex(state, cx, cy):


def main(show_animation):
cp = demo_cp
cp = demo_cp * 100
cx, cy, cyaw, ck = calcTrajectory(cp, n_points=200)

target_speed = MAX_SPEED_STRAIGHT_LINE / 3.6 # [m/s]
target_speed = MAX_SPEED_STRAIGHT_LINE
max_simulation_time = 100.0

# initial state
state = State(x=10, y=5.0, yaw=np.radians(-180.0), v=0)
state = State(x=100, y=50, yaw=np.radians(-180.0), v=0)

last_idx = len(cx) - 1
current_t = 0.0
Expand Down Expand Up @@ -143,7 +143,7 @@ def main(show_animation):
current_radius = np.inf

h = 1 - (np.clip(current_radius, MIN_RADIUS, MAX_RADIUS) - MIN_RADIUS) / (MAX_RADIUS - MIN_RADIUS)
target_speed = h * MIN_SPEED / 3.6 + (1 - h) * MAX_SPEED_STRAIGHT_LINE / 3.6
target_speed = h * MIN_SPEED + (1 - h) * MAX_SPEED_STRAIGHT_LINE

curvatures.append(current_radius)

Expand All @@ -164,7 +164,7 @@ def main(show_animation):
plt.plot(cx[target_idx], cy[target_idx], "xg", label="target")
plt.axis("equal")
plt.grid(True)
plt.title("Speed[km/h]:" + str(state.v * 3.6)[:4])
plt.title("Speed[km/h]:" + str(state.v)[:4])
plt.pause(0.001)

if show_animation: # pragma: no cover
Expand All @@ -177,7 +177,7 @@ def main(show_animation):
plt.grid(True)

_, ax1 = plt.subplots(1)
plt.plot(t, [iv * 3.6 for iv in v], "-r")
plt.plot(t, [iv for iv in v], "-r")
plt.xlabel("Time[s]")
plt.ylabel("Speed[km/h]")
plt.grid(True)
Expand Down
7 changes: 7 additions & 0 deletions tests/test_image_processing.py
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,7 @@
from .common import *
from constants import *
from image_processing.image_processing import processImage
from image_processing.warp_image import transformPoints, warpImage

np.random.seed(0)
test_image = np.random.randint(0, 255, size=(MAX_WIDTH, MAX_HEIGHT, 3), dtype=np.uint8)
Expand All @@ -18,3 +19,9 @@ def testWarpDemo():
args = ['-i', TEST_IMAGE_PATH, '--no-display']
ok = subprocess.call(['python', '-m', 'image_processing.warp_image'] + args)
assertEq(ok, 0)


def testWarping():
x, y = processImage(test_image, debug=True)
transformPoints(x, y)
warpImage(test_image)
9 changes: 8 additions & 1 deletion train/split_video.py
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,7 @@

import cv2
import numpy as np
from tqdm import tqdm

from constants import RIGHT_KEY, LEFT_KEY, ENTER_KEY, EXIT_KEYS

Expand Down Expand Up @@ -36,6 +37,9 @@
n_frames = int(cap.get(frame_count))
print("{} frames".format(n_frames))

# Progress Bar
pbar = tqdm(total=n_frames)

while True:
# Read next frame
while True:
Expand Down Expand Up @@ -66,6 +70,9 @@
# Save image
path = '{}/{}.jpg'.format(output_folder, int(current_idx))
cv2.imwrite(path, original_img)
print("Saved {}".format(int(current_idx)))
# print("Saved {}".format(int(current_idx)))
pbar.update(1)

cap.set(image_zero_index, current_idx)

pbar.close()
26 changes: 23 additions & 3 deletions train/test.py
Original file line number Diff line number Diff line change
Expand Up @@ -11,7 +11,9 @@
import numpy as np

from constants import RIGHT_KEY, LEFT_KEY, EXIT_KEYS, ROI, NUM_OUTPUT, TARGET_POINT, MODEL_TYPE, WEIGHTS_PTH
from path_planning.bezier_curve import calcBezierPath, computeControlPoints, bezier
from constants import MAX_WIDTH, MAX_HEIGHT
from path_planning.bezier_curve import calcBezierPath, computeControlPoints, bezier, calcTrajectory
from path_planning.stanley_controller import stanleyControl, State, calcTargetIndex
from image_processing.warp_image import warpImage, transformPoints
from .utils import loadLabels, loadNetwork, predict, computeMSE

Expand Down Expand Up @@ -105,7 +107,7 @@

# print(current_idx)
# Compute bezier path
control_points = computeControlPoints(x, y, add_current_pos=False)
control_points = computeControlPoints(x, y, add_current_pos=True)
target = bezier(TARGET_POINT, control_points).astype(np.int32)
path = calcBezierPath(control_points).astype(np.int32)

Expand Down Expand Up @@ -141,7 +143,25 @@
cv2.line(warped_image, (points[i, 0], points[i, 1]), (points[i + 1, 0], points[i + 1, 1]), color=(176, 114, 76),
thickness=10)

path = calcBezierPath(points).astype(np.int32)

path = calcBezierPath(points, n_points=10).astype(np.int32)

cp = transformPoints(x, y)
cp[:, 1] *= -1
current_pos = transformPoints([MAX_WIDTH/2], [MAX_HEIGHT])[0]
# TODO: compute v in the pixel space
state = State(x=current_pos[0], y=-current_pos[1], yaw=np.radians(90.0), v=10)
cx, cy, cyaw, ck = calcTrajectory(cp, n_points=10)
target_idx, _ = calcTargetIndex(state, cx, cy)
target = (int(cx[target_idx]), - int(cy[target_idx]))

delta, _, cross_track_error = stanleyControl(state, cx, cy, cyaw, target_idx)
# print(delta, cross_track_error)

# Target
cv2.circle(warped_image, target, radius=50, color=(0, 0, int(0.9 * 255)),
thickness=10, lineType=8, shift=0)

# Draw bezier curve
for i in range(len(path) - 1):
cv2.line(warped_image, (path[i, 0], path[i, 1]), (path[i + 1, 0], path[i + 1, 1]),
Expand Down

0 comments on commit ad366d4

Please sign in to comment.