def Path_Planning_Planner(image=None, plan_type=1):
global planner, path, img_path
smooth = True # False
start = (np.int(pos[0]), np.int(pos[1]))
goal = (np.int(nav_pos[0]), np.int(nav_pos[1]))
if plan_type == 0:
from PathPlanning.Astra import AStar
planner = AStar(m_dilate)
path = planner.planning(start=start, goal=goal, inter=20, img=image)
#print("\rAStar path =")
#print(path)
cv2.circle(image, (start[0], start[1]), 5, (0, 0, 1), 3)
cv2.circle(image, (goal[0], goal[1]), 5, (0, 1, 0), 3)
# Extract Path
if not smooth:
for i in range(len(path) - 1):
cv2.line(image, path[i], path[i + 1], (0, 255, 0), 2)
else:
# from cubic_spline import *
path = np.array(cubic_spline_2d(path, interval=2))
for i in range(len(path) - 1):
cv2.line(image, pos_int(path[i]), pos_int(path[i + 1]),
(0, 255, 0), 1)
img_ = cv2.flip(image, 0)
# cv2.imshow("A* Test", img_)
# k = cv2.waitKey(0)
#print("\rAStar path 1=")
#print(path)
elif plan_type == 1:
from PathPlanning.rrt_star import RRTStar
planner = RRTStar(m_dilate)
path = planner.planning(start, goal, 30, img)
cv2.circle(image, (start[0], start[1]), 5, (0, 0, 1), 3)
cv2.circle(image, (goal[0], goal[1]), 5, (0, 1, 0), 3)
# Extract Path
if not smooth:
for i in range(len(path) - 1):
cv2.line(image, pos_int(path[i]), pos_int(path[i + 1]),
(0, 255, 0), 2)
else:
# from cubic_spline import *
path = np.array(cubic_spline_2d(path, interval=4))
for i in range(len(path) - 1):
cv2.line(image, pos_int(path[i]), pos_int(path[i + 1]),
(0, 255, 0), 1)
img_ = cv2.flip(image, 0)
from PathTracking.bicycle_pure_pursuit import PurePursuitControl
controller = PurePursuitControl(kp=0.7,Lfc=10)
elif control_type == 2:
from PathTracking.bicycle_stanley import StanleyControl
controller = StanleyControl(kp=0.5)
elif control_type == 3:
from PathTracking.bicycle_lqr import LQRControl
controller = LQRControl()
# Path Planning Planner
if plan_type == 0:
from PathPlanning.astar import AStar
planner = AStar(m_dilate)
elif plan_type == 1:
from PathPlanning.rrt_star import RRTStar
planner = RRTStar(m_dilate)
from PathPlanning.cubic_spline import *
##############################
# Util Function
##############################
# Mouse Click Callback
def mouse_click(event, x, y, flags, param):
global control_type, plan_type, nav_pos, pos, path, m_dilate, way_points, controller
if event == cv2.EVENT_LBUTTONUP:
nav_pos_new = (x, m.shape[0]-y)
if m_dilate[nav_pos_new[1], nav_pos_new[0]] > 0.5:
way_points = planner.planning((pos[0],pos[1]), nav_pos_new, 20)
if len(way_points) > 1:
nav_pos = nav_pos_new
path = np.array(cubic_spline_2d(way_points, interval=4))
def main(argv):
try:
opts, args = getopt.getopt(argv,"hp:c:",["Planning=","Control="])
except getopt.GetoptError:
print('<ERROR> main.py -p <PlanningMethod 0:AStar 1:RRTStar> -c <ControlMethod 0:PurePersuit 1:Stanley>')
sys.exit(2)
for opt, arg in opts:
if opt == '-h':
print('main.py -p <PlanningMethod 0:AStar 1:RRTStar> -c <ControlMethod 0:PurePersuit 1:Stanley>')
sys.exit()
elif opt in ("-p", "--Planning"):
plan_type = arg
elif opt in ("-c", "--Control"):
control_type = arg
# Path Tracking Controller
if control_type == '0':
from PathTracking.bicycle_pure_pursuit import PurePursuitControl
controller = PurePursuitControl(kp=0.7,Lfc=10)
elif control_type == '1':
from PathTracking.bicycle_stanley import StanleyControl
controller = StanleyControl(kp=0.75)
else:
print('ERROR')
sys.exit(2)
# Path Planning Planner
if plan_type == '0':
from PathPlanning.dijkstra import AStar
planner = AStar(m_dilate)
elif plan_type == '1':
from PathPlanning.rrt_star import RRTStar
planner = RRTStar(m_dilate)
#from PathPlanning.cubic_spline import *
global nav_pos, path, init_pos, pos, planFlag, smooth, last_nav, canControl
cv2.namedWindow(window_name)
cv2.setMouseCallback(window_name, mouse_click)
# Main Loop
while(True):
# Update State
car.update()
pos = (car.x, car.y, car.yaw)
print("\rState: "+car.state_str(), "| Goal:", nav_pos, end="\t")
img_ = img.copy()
#####################################
# Control and Path Planning
## Planning ##
if nav_pos != None and planFlag == False and last_nav != nav_pos:
path = planner.planning(start=(int(car.x),int(car.y)),goal=nav_pos,img=img)
last_nav = nav_pos
canControl = True
img_ = img.copy()
cv2.imshow(window_name,img_)
planFlag = True
# Extract Path
if not smooth:
for i in range(len(path)-1):
cv2.line(img, path[i], path[i+1], (1,0,0), 2)
else:
path = np.array(cubic_spline_2d(path, interval=1))
for i in range(len(path)-1):
cv2.line(img, pos_int(path[i]), pos_int(path[i+1]), (1,0,0), 1)
## Control ##
if canControl == True:
controller.set_path(path)
# ================= Control Algorithm =================
# PID Longitude Control
end_dist = np.hypot(path[-1,0]-car.x, path[-1,1]-car.y)
target_v = 20 if end_dist > 180 else 0
next_a = 0.1*(target_v - car.v)
# Lateral Control
state = {"x":car.x, "y":car.y, "yaw":car.yaw, "v":car.v, "l":car.l, "delta":car.delta}
next_delta, target = controller.feedback(state)
car.control(next_a, next_delta)
# =====================================================
#####################################
# Collision Simulation
if collision_detect(car, m):
car.redo()
car.v = -0.5*car.v
# Environment Rendering
if nav_pos is not None:
cv2.circle(img_,nav_pos,5,(0.5,0.5,1.0),3)
img_ = car.render(img_)
img_ = cv2.flip(img_, 0)
cv2.imshow(window_name ,img_)
# Remove Path
Path = None
planFlag = False
# Keyboard
k = cv2.waitKey(1)
if k == ord("a"):
car.delta += 5
elif k == ord("d"):
car.delta -= 5
elif k == ord("w"):
car.v += 4
elif k == ord("s"):
car.v -= 4
elif k == ord("r"):
car.init_state(init_pos)
nav_pos = None
path = None
print("Reset!!")
if k == 27:
print()
break
controller = PidControl()
elif control_type == 1:
controller = PurePursuitControl(kp=0.7, Lfc=10)
elif control_type == 2:
controller = StanleyControl(kp=0.5)
elif control_type == 3:
controller = LQRControl()
else:
controller = PurePursuitControl(kp=0.7, Lfc=10)
pos = (100, 200, 0)
car.x = 100
car.y = 200
car.yaw = 0
rrt = RRTStar(m_dilate)
astar = AStar(m_dilate)
gm = GridMap([0.5, -0.5, 5.0, -5.0], gsize=3)
def mouse_click(event, x, y, flags, param):
global nav_pos, pos, path, m_dilate, way_points, controller
if event == cv2.EVENT_LBUTTONUP:
nav_pos_new = (x, m.shape[0] - y)
if m_dilate[nav_pos_new[1], nav_pos_new[0]] > 0.5:
way_points = rrt.planning((pos[0], pos[1]), nav_pos_new, 20)
#way_points = astar.planning(pos_int((pos[0],pos[1])), pos_int(nav_pos_new),inter=20)
if len(way_points) > 1:
nav_pos = nav_pos_new
path = np.array(cubic_spline_2d(way_points, interval=4))
controller.set_path(path)