def get_angle(c_i):
global_path = mt.get_csv()
try:
pt1 = (global_path['x'][c_i + 1] - global_path['x'][c_i],
global_path['y'][c_i + 1] - global_path['y'][c_i])
pt2 = (global_path['x'][c_i + 15] - global_path['x'][c_i],
global_path['y'][c_i + 15] - global_path['y'][c_i])
ang1 = np.arctan2(*pt2[::-1])
ang2 = np.arctan2(*pt1[::-1])
res = np.rad2deg((ang1 - ang2) % (2 * np.pi))
res = (res + 360) % 360
except IndexError:
res = 0
if 300 < res < 355:
direction = 'left'
Lights_Indicator = 1
elif 10 < res < 60:
direction = 'right'
Lights_Indicator = -1
else:
direction = None
Lights_Indicator = 0
return direction, Lights_Indicator
def main():
global settings
settings = termios.tcgetattr(sys.stdin)
rospy.init_node('controller')
controller = Controller()
PID_controller = Pid_Controller()
cfg = Config()
udp = sub_udp()
states = States()
pose_dict = mt.get_csv()
cx = pose_dict['x']
cy = pose_dict['y']
target_course = TargetCourse(cx, cy)
udp.GearNo = 1
tar_vel_ = 0
controller.loop = 0
v_dt = 0
distance_old = 0
count = 0
# start -----------------------------------------------
while 1:
# cal dt
if controller.car_lasted_time == 0:
controller.car_lasted_time = controller.car_now_time
controller.car_dt_time = controller.car_now_time - controller.car_lasted_time
controller.car_lasted_time = controller.car_now_time
key = getKey()
try:
# use localization
state = State(x=controller.car_local_point_x,
y=controller.car_local_point_y,
yaw=controller.car_local_point_theta,
v=controller.cur_vel)
last_index = len(cx) - 1
target_ind, Lf, Ld = target_course.search_target_index(state, 1.6)
cur_vel_ = controller.cur_vel
cur_dt = controller.car_dt_time
cur_acc_ = controller.car_acc
except (AttributeError, IndexError) as set_err:
print(set_err)
print("*-*-*-L-O-D-I-N-G-*-*-*")
continue
# traffic light | red, yellow : stop, green : go
if controller.traffic_stat == 1:
udp.GearNo = -9
print("traffic light red!!")
# else:
# udp.GearNo = 1
print("tra : ", controller.traffic_stat)
# In global path
if last_index > target_ind:
# left_and_right.py
# if cfg.mode == "trun"
direction, LI = lar.get_ang(target_ind)
udp.Lights_Indicator = LI
if direction is None:
# 직진
print("직진")
else:
print(direction)
# 여기서 turn모드
# acc_controller.py
# if cfg.mode == "acc":
try:
distance_new = controller.Front_car_dis
E_state = controller.emergency_state_info
distance_old, cfg, Lights_Hazard = ac.accs(
distance_new, E_state, tar_vel_, cur_vel_, cur_dt, cfg,
distance_old)
udp.Lights_Hazard = Lights_Hazard
# pd controller
mov_vel = PID_controller.accel_control(cfg.acc_v, cur_vel_,
cur_dt, 1, 0, 1)
udp.Ax = mov_vel # m/s^2
except AttributeError as a_e:
mov_vel = PID_controller.accel_control(tar_vel_, cur_vel_,
cur_dt, 1, 0, 1)
udp.Ax = mov_vel # m/s^2
cfg.state = 0
#
# print(cfg.state, cfg.acc_v)
# lkas_controller.py
# if cfg.mode == "lkas":
steering_re = lc.lkas(controller.lane_center_pix)
if steering_re is None:
delta_gain = pure_pursuit_steer_control(
state, target_course, target_ind, 0.08)
udp.SteeringWheel = delta_gain
else:
print("############################################")
udp.SteeringWheel = steering_re
#
# local_path.py
# if cfg.mode == "local":
try:
# E_state = controller.emergency_state_info
local_x = controller.lo_x
local_y = controller.lo_y
local_state = controller.lo_state
local_course = TargetCourse(local_x, local_y)
local_last_index = len(cx) - 1
# 여기서 스테이트만 주지 말고 게인값 까지 다 줄 수 있게 수정할 것.
# 라스트 인덱스도 필요하면 주고 필요없으면 제거 하면 될 것 같음.
local_ind, local_Lf, local_Ld = local_course.search_target_index(
state, 1)
# here, 필요없는 변수 리턴 다 풀고 필요한것만 리턴하게 할것.
local_delta_gain = pure_pursuit_steer_control(
state, local_course, local_ind, 0.2)
udp.SteeringWheel = local_delta_gain
"""
오늘 할일
먼저 로컬 기본적인 틀은 짜놨지만 퓨어 제어기에서 들어가거나 나오는 값들의 수정이 필요하고 게인값을 넣을 수 있게 조정이 필요함.
그리고 방향 지시등에 따른 변경도 추가해야함
한다음에 모드로 움직이는 트리거를 만들어야함.
모드를 선택해 주는 셀렉 모드도 하나 만들고 이런 로컬이 끝나면 셀렉모드로 돌아와서 턴이면 턴하고 웨이면 웨이포인트 따라가는 트리거를
만든 다음 모드 추가를 진행하면 될 것 같음.
다만 게인값을 넣는다고 한들 그게 과연 튜닝없이 될까 라는 생각이긴함.
일단은 해보고 이쪽은 대강 값아니까 그거 넣어보면 되고
로컬이랑 글로벌은 완전히 다른변수로 돌아가야해서 혹시나 찝히거나 하는 부분이 있는지 잘 확인해야함.
글로벌변수는 완전하게 납둔 채로 로컬이 진행되어야함.
그 표지판 정민이형 컴에서만 돌아간다고 해서 완성한 다음 그부분 전달해서 추가받아야 할듯.
그리고 웨이포인트 만들어준 4개 테스트 해봐야함.
3번 4번을 아직 안해봤음..
차간거리제어도 공식 아까 다시 만들어 놓은거 적용해서 잘 작동하나 테스트 해볼 것.
"""
except AttributeError as local_err:
count += 1
local_x = []
local_y = []
local_state = None
v_dt += cur_dt
states.append(v_dt, state)
controller.loop += 1
# drawing plot
if cfg.show_animation: # pragma: no cover
plt.cla()
# for stopping simulation with the esc key.
plt.gcf().canvas.mpl_connect(
'key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
plot_arrow(state.x, state.y, state.yaw)
plt.plot(cx, cy, "-r", label="course")
plt.plot(states.x, states.y, "-b", label="trajectory")
plt.plot(cx[target_ind], cy[target_ind], "xg", label="target")
plt.axis("equal")
plt.grid(True)
plt.title("Speed[km/h]:" + str(state.v)[:4])
plt.pause(0.001)
if cfg.show_animation_gain:
plt.cla()
plt.gcf().canvas.mpl_connect(
'key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
plt.grid(True)
plt.plot(states.t, [iv for iv in states.v], "-r")
plt.legend()
plt.xlabel("Time[s]")
plt.ylabel("Speed[km/h]")
plt.axis("equal")
plt.title("Speed[km/h]:" + str(state.v)[:4])
plt.grid(True)
plt.pause(0.00001)
else:
print("lastIndex < target_ind")
print("*-*-*-*-*-*-END-*-*-*-*-*-*")
# ----------------------------------------------------
# set target V
if key == '0':
udp.VC_SwitchOn = 0
print('maneuver')
if key == '1':
udp.VC_SwitchOn = 1
udp.GearNo = 1
tar_vel_ = 10
if key == '2':
udp.VC_SwitchOn = 1
udp.GearNo = 1
tar_vel_ = 20
if key == '3':
udp.VC_SwitchOn = 1
udp.GearNo = 1
tar_vel_ = 30
if key == '4':
udp.VC_SwitchOn = 1
udp.GearNo = 1
tar_vel_ = 40
if key == '5':
udp.VC_SwitchOn = 1
udp.GearNo = 1
tar_vel_ = 50
# Brake
if key == 'c':
udp.VC_SwitchOn = 1
udp.GearNo = 0
udp.Ax = 0
udp.SteeringWheel = 0
print('stop - 0.5')
if key == 'x':
udp.VC_SwitchOn = 1
udp.GearNo = -9
udp.Ax = 0
udp.SteeringWheel = 0
print('stop - 1')
else:
if (key == '\x03'):
break
controller.car_pub.publish(udp)
# draw plot
try:
if controller.show_animation: # pragma: no cover
plt.cla()
plt.plot(cx, cy, ".r", label="course")
plt.plot(states.x, states.y, "-b", label="trajectory")
plt.legend()
plt.xlabel("x[m]")
plt.ylabel("y[m]")
plt.axis("equal")
plt.grid(True)
plt.subplots(1)
plt.plot(states.t, [iv for iv in states.v], "-r")
plt.xlabel("Time[s]")
plt.ylabel("Speed[km/h]")
plt.grid(True)
plt.show()
except AttributeError:
print("*-*-*-n-o-d-a-t-a-*-*-*")
def main():
global settings
settings = termios.tcgetattr(sys.stdin)
rospy.init_node('controller')
controller = Controller()
PID_controller = Pid_Controller()
cfg = Config()
udp = sub_udp()
states = States()
pose_dict = mt.get_csv()
cx = pose_dict['x']
cy = pose_dict['y']
target_course = TargetCourse(cx, cy)
udp.GearNo = 1
udp.VC_SwitchOn = 1
controller.loop = 0
v_dt = 0
distance_old = 0
count = 0
acc_mode = 0
pure_mode = 0
local_x = None
local_y = None
local_course = None
local_ind = None
local_now_ind = None
local_state = 2
local_stop = 0
y_line_state = 9
E_state = None
distance_new = None
tar_vel_ = 20
Lf_k = 1.6
listener = tf.TransformListener()
mc = Marker_Class()
# start -----------------------------------------------
while 1:
# cal dt
if controller.car_lasted_time == 0:
controller.car_lasted_time = controller.car_now_time
controller.car_dt_time = controller.car_now_time - controller.car_lasted_time
controller.car_lasted_time = controller.car_now_time
key = getKey()
try:
(rear_tf,
rot_r) = listener.lookupTransform('localization', 'Wheel_Rear',
rospy.Time(0))
(front_tf,
rot_f) = listener.lookupTransform('localization', 'Wheel_Front',
rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
continue
try:
# use localization
state = State(rear_tf,
front_tf,
x=controller.car_local_point_x,
y=controller.car_local_point_y,
yaw=controller.car_local_point_theta,
v=controller.cur_vel)
last_index = len(cx) - 1
target_ind, Ld, now_ind = target_course.search_target_index(
state, Lf_k)
# what matter here?
mc.init_markers([target_ind, now_ind])
cur_vel_ = controller.cur_vel
cur_dt = controller.car_dt_time
cur_acc_ = controller.car_acc
except (AttributeError, IndexError) as set_err:
print(set_err)
print("*-*-*-L-O-D-I-N-G-*-*-*")
continue
print("-------------------------------------------------------------")
print("tar_vel_set", tar_vel_)
if controller.speed_distance != 0:
if controller.speed_distance <= 30:
if controller.speed_limit >= 45:
tar_vel_ = controller.speed_limit - 10
elif controller.speed_limit >= 35:
tar_vel_ = controller.speed_limit - 5
else:
tar_vel_ = controller.speed_limit
print("tar_vel_limit", tar_vel_)
print("cul_vel", controller.cur_vel)
print("speed_distance", controller.speed_distance)
print("speed_limit", controller.speed_limit)
try:
E_state = controller.emergency_state_info
distance_new = controller.Front_car_dis
print("1 distance : ", distance_new)
# controller.Front_car_dis = None
except AttributeError as e_s_err:
print("No E_state, No distance")
print("distance_ in : ", distance_new)
print("mode : ", cfg.mode)
print("state : ", cfg.state)
print("E_state : ", E_state)
if controller.traffic_state.data == 0:
udp.GearNo = 1
print("traffic", controller.traffic_state)
# --------------------------------------------------------------------------------------
# In global path
if last_index > target_ind:
cfg.mode = "waypoint"
udp.VC_SwitchOn = 1
try:
local_x = controller.lo_x
local_y = controller.lo_y
local_state = controller.lo_state
if local_state is 1:
local_stop = 1
elif local_x is not None or local_state is 0:
cfg.mode = "local"
local_course = TargetCourse(controller.lo_x,
controller.lo_y)
local_last_index = len(local_x) - 1
local_ind, local_Ld, local_now_ind = local_course.search_target_index(
state, Lf_k)
print(local_x)
print(local_y)
if (local_last_index - 2) <= local_ind:
local_stop = 1
cfg.mode = "waypoint"
print("loca_next_waypoint", cfg.mode)
except (AttributeError, ValueError) as local_err:
cfg.mode = "waypoint"
print("No local")
# 신호등
# if cfg.mode is "local" and local_x is not None and local_ind is not None:
if cfg.mode is "local":
direction, LI = lar.local_get_angle(local_now_ind, local_x,
local_y)
print("local traffic")
else:
direction, LI = lar.get_angle(now_ind)
# print("LI : ", LI)
# print("direction : ", direction)
udp.Lights_Indicator = LI
# 직선 스티어링
steering_re = lc.lkas(controller.lane_center_pix)
# mode pick --------------------------------------------------------------------------------------
if steering_re is not None and cfg.mode is not "turn":
print("############################################")
cfg.mode = "line"
if direction is not None:
cfg.mode = "turn"
print("local_state : ", local_state)
if local_stop == 1:
print("local_state : ", local_state)
cfg.mode = "waypoint"
local_x = None
local_y = None
local_course = None
local_ind = None
local_state = 2
local_stop = 0
#
Ks = 0.3
Lf_k = 0.3
if cfg.mode == "waypoint":
acc_mode = 1
pure_mode = 0
if cfg.mode == "line":
acc_mode = 1
pure_mode = 1
if cfg.mode == "turn":
acc_mode = 0
pure_mode = 0
Ks = 0.6
Lf_k = 0.5
if cfg.mode == "local":
acc_mode = 0
pure_mode = 2
Ks = 0.6
Lf_k = 1
udp.VC_SwitchOn = 1
udp.GearNo = 1
# mode --------------------------------------------------------------------------------------
# acc mode
target_velocity = tar_vel_
if cfg.mode == "local":
target_velocity = 5
if cfg.mode == "turn":
target_velocity = 15
mov_vel = PID_controller.accel_control(target_velocity, cur_vel_,
cur_dt, 1, 0, 1)
print(distance_new)
if acc_mode == 1:
if distance_new is not None and E_state != 0:
distance_old, cfg, Lights_Hazard, GearNo = ac.accs(
distance_new, E_state, tar_vel_, cur_vel_, cur_dt, cfg,
distance_old)
udp.Lights_Hazard = Lights_Hazard
# if GearNo == -9:
# cfg.mode = "stop"
# elif GearNo == 1:
# udp.GearNo = 1
# udp.VC_SwitchOn = 1
print("front car velocity : ", cfg.acc_tar)
# if cfg.state == 4:
# if target_velocity < 10:
# target_velocity = target_velocity - 5
# else:
# target_velocity = target_velocity - 9
# if cfg.state == 3:
# if target_velocity > 10:
# target_velocity = 10
# else:
# target_velocity = 5
# if cfg.state < 3:
# # pd controller
target_velocity = cfg.acc_tar
mov_vel = PID_controller.accel_control(
target_velocity, cur_vel_, cur_dt, 1, 0, 0)
else:
acc_mode = 0
udp.Ax = mov_vel
#
# pure mode
delta_gain = pure_pursuit_steer_control(state, target_course,
target_ind, Ks)
steering_value = delta_gain
if pure_mode == 1:
steering_value = steering_re
if steering_re is None:
steering_value = 0
if pure_mode == 2:
local_delta_gain = pure_pursuit_steer_control(
state, local_course, local_ind, Ks)
steering_value = local_delta_gain
#
# print("controller.yellow_lane", controller.yellow_lane)
# print("controller.yellow_lane_dist", controller.yellow_lane_dist)
# if controller.yellow_lane == "left":
# if controller.yellow_lane_dist == "far":
# print("left-far")
# # 왼쪽으로 가야로함 / 차가 오른쪽으로 기울어져 있음
# y_line_state = 1
# elif controller.yellow_lane_dist == "close":
# print("left-close")
# # 오른쪽으로 가야함 / 차가 왼쪽으로 기울어져 있음
# y_line_state = -1
# elif controller.yellow_lane == "right":
# if controller.yellow_lane_dist == "far":
# print("right-far")
# # 오른쪽으로 가야함 / 차가 왼쪽으로 기울어져 있음
# y_line_state = -1
# elif controller.yellow_lane_dist == "close":
# print("right-close")
# # 왼쪽으로 가야로함 / 차가 오른쪽으로 기울어져 있음
# y_line_state = 1
# elif controller.yellow_lane == "no" and controller.yellow_lane_dist == "no":
# y_line_state = 9
# if controller.yellow_lane_dist == "ok":
# y_line_state = 0
# print("before Yellow", steering_value)
# if -90 <= controller.yellow_deg < -50:
# # "right"
# steering_value = controller.yellow_deg * (np.pi / 180)
# print("turn right", steering_value)
# elif 50 <= controller.yellow_deg < 90:
# # "left"
# steering_value = controller.yellow_deg * (np.pi / 180)
# print("turn left", steering_value)
# else:
# steering_value = steering_value
print("last steering_value : ", steering_value)
print("udp GearNo : ", udp.GearNo)
print("udp mode : ", cfg.mode)
udp.SteeringWheel = steering_value
# ----------------------------------------------------------------------------------
controller.loop += 1
# draw plot
v_dt += cur_dt
states.append(v_dt, state)
else:
print("lastIndex < target_ind")
print("*-*-*-*-*-*-END-*-*-*-*-*-*")
# ----------------------------------------------------
# traffic light | red, yellow : stop, green : go
if E_state == 0 and controller.cur_vel < 0:
cfg.mode = "waypoint"
distance_new = 100
tar_vel_ = 20
if controller.traffic_state.data == 1:
cfg.mode = "stop"
if distance_new is not None and cfg.mode is not "local":
if distance_new <= 5:
print(cfg.mode)
cfg.mode = "stop"
if E_state == 0 and distance_new is not None:
cfg.mode = "waypoint"
distance_new = None
if cfg.mode == "stop":
udp.GearNo = -9
# ----------------------------------------------------
# Brake
if key == 'x':
udp.VC_SwitchOn = 1
udp.GearNo = -9
udp.Ax = 0
udp.SteeringWheel = 0
print('stop - 1')
else:
if (key == '\x03'):
break
controller.car_pub.publish(udp)
def main():
global settings
settings = termios.tcgetattr(sys.stdin)
rospy.init_node('controller')
controller = Controller()
PID_controller = Pid_Controller()
cfg = Config()
udp = sub_udp()
states = States()
pose_dict = mt.get_csv()
cx = pose_dict['x']
cy = pose_dict['y']
target_course = TargetCourse(cx, cy)
udp.GearNo = 1
tar_vel_ = 0
controller.loop = 0
v_dt = 0
distance_old = 0
count = 0
acc_mode = 0
pure_mode = 0
E_stop = 0
local_x = None
local_y = None
local_course = None
local_ind = None
local_state = 2
local_stop = 0
# start -----------------------------------------------
while 1:
# cal dt
if controller.car_lasted_time == 0:
controller.car_lasted_time = controller.car_now_time
controller.car_dt_time = controller.car_now_time - controller.car_lasted_time
controller.car_lasted_time = controller.car_now_time
key = getKey()
try:
# use localization
state = State(x=controller.car_local_point_x,
y=controller.car_local_point_y,
yaw=controller.car_local_point_theta,
v=controller.cur_vel)
last_index = len(cx) - 1
target_ind, Ld = target_course.search_target_index(state, 1.6)
cur_vel_ = controller.cur_vel
cur_dt = controller.car_dt_time
cur_acc_ = controller.car_acc
except (AttributeError, IndexError) as set_err:
print(set_err)
print("*-*-*-L-O-D-I-N-G-*-*-*")
continue
try:
E_stop = controller.emergency_state_info
distance_new = controller.Front_car_dis
print("distance_new : ", distance_new)
print("E_stop : ", E_stop)
except AttributeError as a_e:
print("No E_stop")
print("mode : ", cfg.mode)
# traffic light | red, yellow : stop, green : go
if controller.traffic_stat == 1:
udp.GearNo = -9
cfg.mode = "stop"
else:
udp.GearNo = 1
# --------------------------------------------------------------------------------------
# In global path
if last_index > target_ind:
cfg.mode = "waypoint"
try:
local_x = controller.lo_x
local_y = controller.lo_y
local_state = controller.lo_state
print("local!")
print("local_x : ", local_x)
print("local_y : ", local_y)
if local_x is not None:
local_course = TargetCourse(local_x, local_y)
local_last_index = len(cx) - 1
local_ind, local_Ld = local_course.search_target_index(
state, 0.1)
if (local_last_index - 10) <= local_ind:
local_stop = 1
if local_state == 1:
local_stop = 1
except (AttributeError, ValueError) as local_err:
print("local err : ", local_err)
# 신호등
if cfg.mode is "local" and local_x is not None and local_ind is not None:
direction, LI = lar.local_get_angle(local_ind, local_x,
local_y)
print("local traffic")
else:
direction, LI = lar.get_angle(target_ind)
# print("LI : ", LI)
# print("direction : ", direction)
udp.Lights_Indicator = LI
# 직선 스티어링
steering_re = lc.lkas(controller.lane_center_pix)
# mode pick --------------------------------------------------------------------------------------
if steering_re is not None and cfg.mode is not "turn":
print("############################################")
cfg.mode = "line"
if direction is not None:
cfg.mode = "turn"
if local_state == 0:
cfg.mode = "local"
print("local_state : ", local_state)
if local_stop == 1:
print("local_state : ", local_state)
cfg.mode = "waypoint"
local_x = None
local_y = None
local_course = None
local_ind = None
local_state = 2
local_stop = 0
print("wayin", cfg.mode)
#
Ks = 0.09
if cfg.mode == "waypoint":
acc_mode = 1
pure_mode = 0
if cfg.mode == "line":
acc_mode = 1
pure_mode = 1
if cfg.mode == "turn":
acc_mode = 0
pure_mode = 0
Ks = 0.6
if cfg.mode == "local":
acc_mode = 0
pure_mode = 2
Ks = 0.09
# mode --------------------------------------------------------------------------------------
# acc mode
print("1", acc_mode)
target_velocity = tar_vel_
if acc_mode == 1:
try:
distance_new = controller.Front_car_dis
E_state = controller.emergency_state_info
acc_mode = 1
except AttributeError as a_e:
acc_mode = 0
if acc_mode == 1:
distance_new = controller.Front_car_dis
E_state = controller.emergency_state_info
distance_old, cfg, Lights_Hazard, GearNo = ac.accs(
distance_new, E_state, tar_vel_, cur_vel_, cur_dt, cfg,
distance_old)
udp.Lights_Hazard = Lights_Hazard
if GearNo == -9:
udp.GearNo = -9
else:
udp.GearNo = 1
# pd controller
target_velocity = cfg.acc_tar
if cfg.mode == "local":
target_velocity = 5
print("2", acc_mode)
mov_vel = PID_controller.accel_control(target_velocity, cur_vel_,
cur_dt, 1, 0, 1)
udp.Ax = mov_vel
#
# pure mode
delta_gain = pure_pursuit_steer_control(state, target_course,
target_ind, Ks)
steering_value = delta_gain
if pure_mode == 1:
steering_value = steering_re
if steering_re is None:
steering_value = 0
if pure_mode == 2:
local_delta_gain = pure_pursuit_steer_control(
state, local_course, local_ind, Ks)
steering_value = local_delta_gain
udp.SteeringWheel = steering_value
#
controller.loop += 1
# draw plot
v_dt += cur_dt
states.append(v_dt, state)
# drawing plot
if cfg.show_animation: # pragma: no cover
plt.cla()
# for stopping simulation with the esc key.
plt.gcf().canvas.mpl_connect(
'key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
plot_arrow(state.x, state.y, state.yaw)
plt.plot(cx, cy, "-r", label="course")
plt.plot(states.x, states.y, "-b", label="trajectory")
plt.plot(cx[target_ind], cy[target_ind], "xg", label="target")
plt.axis("equal")
plt.grid(True)
plt.title("Speed[km/h]:" + str(state.v)[:4])
plt.pause(0.001)
if cfg.show_animation_gain:
plt.cla()
plt.gcf().canvas.mpl_connect(
'key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
plt.grid(True)
plt.plot(states.t, [iv for iv in states.v], "-r")
plt.legend()
plt.xlabel("Time[s]")
plt.ylabel("Speed[km/h]")
plt.axis("equal")
plt.title("Speed[km/h]:" + str(state.v)[:4])
plt.grid(True)
plt.pause(0.00001)
#
else:
print("lastIndex < target_ind")
print("*-*-*-*-*-*-END-*-*-*-*-*-*")
# ----------------------------------------------------
# set target V
if key == '0':
udp.VC_SwitchOn = 0
print('maneuver')
if key == '1':
udp.VC_SwitchOn = 1
udp.GearNo = 1
tar_vel_ = 10
if key == '2':
udp.VC_SwitchOn = 1
udp.GearNo = 1
tar_vel_ = 20
if key == '3':
udp.VC_SwitchOn = 1
udp.GearNo = 1
tar_vel_ = 30
if key == '4':
udp.VC_SwitchOn = 1
udp.GearNo = 1
tar_vel_ = 40
if key == '5':
udp.VC_SwitchOn = 1
udp.GearNo = 1
tar_vel_ = 50
# Brake
if key == 'c':
udp.VC_SwitchOn = 1
udp.GearNo = 0
udp.Ax = 0
udp.SteeringWheel = 0
print('stop - 0.5')
if key == 'x':
udp.VC_SwitchOn = 1
udp.GearNo = -9
udp.Ax = 0
udp.SteeringWheel = 0
print('stop - 1')
else:
if (key == '\x03'):
break
controller.car_pub.publish(udp)
# draw plot
try:
if controller.show_animation: # pragma: no cover
plt.cla()
plt.plot(cx, cy, ".r", label="course")
plt.plot(states.x, states.y, "-b", label="trajectory")
plt.legend()
plt.xlabel("x[m]")
plt.ylabel("y[m]")
plt.axis("equal")
plt.grid(True)
plt.subplots(1)
plt.plot(states.t, [iv for iv in states.v], "-r")
plt.xlabel("Time[s]")
plt.ylabel("Speed[km/h]")
plt.grid(True)
plt.show()
except AttributeError:
print("*-*-*-n-o-d-a-t-a-*-*-*")
def main():
global settings
settings = termios.tcgetattr(sys.stdin)
rospy.init_node('controller')
controller = Controller()
PID_controller = Pid_Controller()
cfg = Config()
udp = sub_udp()
udp.GearNo = 1
tar_vel_ = 0
controller.loop = 0
pose_dict = mt.get_csv()
cx = pose_dict['x']
cy = pose_dict['y']
target_course = TargetCourse(cx, cy)
states = States()
v_dt = 0
distance_old = 0
count = 0
# start -----------------------------------------------
while 1:
key = getKey()
# cal dt
if controller.car_lasted_time == 0:
controller.car_lasted_time = controller.car_now_time
controller.car_dt_time = controller.car_now_time - controller.car_lasted_time
controller.car_lasted_time = controller.car_now_time
# set target target V
if key == 't':
udp.VC_SwitchOn = 1
udp.GearNo = 1
try:
tar_vel_ = float(input("target velue : "))
print("set", tar_vel_, "km/h")
except NameError:
print("try again")
except SyntaxError:
print("try again")
try:
# use localization
state = State(x=controller.car_local_point_x, y=controller.car_local_point_y, yaw=controller.car_local_point_theta, v=controller.cur_vel)
except (AttributeError, IndexError) as e:
print(e)
print("*-*-*-L-O-D-I-N-G-*-*-*")
continue
try:
last_index = len(cx) - 1
target_ind, Lf, Ld = target_course.search_target_index(state)
except (AttributeError, IndexError) as e:
print(e)
print("*-*-*-L-O-D-I-N-G-*-*-*")
continue
# print(target_ind)
# # Loop in index, untill last index
# ## 만약 Wapoint
# if cfg.mode == "waypoint":
#
## 만약 lane_tracking
## 만약 trun
## 만약 local
if last_index > target_ind:
cur_vel_ = controller.cur_vel
cur_dt = controller.car_dt_time
cur_acc_ = controller.car_acc
if controller.traffic_stat == 1:
udp.GearNo = -9
else:
udp.GearNo = 1
try:
distance_new = controller.Front_car_dis
E_state = controller.emergency_state_info
if cfg.state == 0:
cfg.acc_v = tar_vel_
mov_vel = PID_controller.accel_control(cfg.acc_v, cur_vel_, cur_dt)
if distance_new <= cfg.safe_distance / 2:
cfg.state = 5
# udp.GearNo = -9
print("Danger! Stop!")
udp.Lights_Hazard = 1
elif cfg.safe_distance / 2 < distance_new < cfg.safe_distance:
cfg.state = 4
print("Deceleration")
udp.Lights_Hazard = 0
elif cfg.safe_distance <= distance_new <= cfg.safe_distance + 5:
cfg.state = 3
print("Safe distance")
udp.Lights_Hazard = 0
elif cfg.safe_distance + 5 < distance_new < cfg.safe_distance + 15:
cfg.state = 2
print("Acceleration")
udp.Lights_Hazard = 0
# else:
# cfg.state = 1
# cfg.acc_v = tar_vel_
# print("Nothing front")
# cfg.acc_v = math.sqrt((cur_vel_ ** 2) + 2 * cur_acc_ * (cfg.safe_distance - distance))
try:
cfg.acc_v = cur_vel_ + (distance_new - distance_old) / cur_dt
except ZeroDivisionError as z:
print(z)
if controller.emergency_state_info == 0:
cfg.state = 1
cfg.acc_v = tar_vel_
print("Nothing front")
# pd controller
mov_vel = PID_controller.accel_control(cfg.acc_v, cur_vel_, cur_dt, "p")
udp.Ax = mov_vel # m/s^2
print("acc_vel : ", cfg.acc_v)
distance_old = distance_new
except AttributeError:
# Nothing front
# pd controller
mov_vel = PID_controller.accel_control(tar_vel_, cur_vel_, cur_dt)
udp.Ax = mov_vel # m/s^2
cfg.state = 0
print("Nothing--------------------------")
try:
top_pix, low_pix, mid_pix = controller.lane_center_pix
t_l_dx = top_pix - low_pix
t_l_dy = 0 - 800
t_l_alpha = math.atan2(t_l_dy, t_l_dx)
m_l_dx = mid_pix - low_pix
m_l_dy = 400 - 800
m_l_alpha = math.atan2(m_l_dy, m_l_dx)
# rad
pix_alpha = t_l_alpha - m_l_alpha
# deg
pix_alpha_d = pix_alpha * (180 / math.pi)
print("pix_alpha : ", pix_alpha)
print("pix_alpha_d : ", pix_alpha_d)
if -1.5 <= pix_alpha_d <= 1.5:
print("############################################")
# pix_theta = math.pi/2 - t_l_alpha
count += 1
if count >= 5:
if -0.05 <= t_l_dx <= 0.05:
udp.SteeringWheel = 0
else:
udp.SteeringWheel = t_l_dx / -1000
else:
# # steering wheel ang! = deg
delta, target_ind, delta_rad, delta_gain, alpha, alpha_m, car_alpha_m, delta_m, Ld_m = pure_pursuit_steer_control(
state, target_course, target_ind)
udp.SteeringWheel = delta_gain
count = 0
except AttributeError as k:
print(k)
v_dt += cur_dt
# state.update(mov_vel, delta_rad, cur_dt)
states.append(v_dt, state)
# drawing plot
if cfg.show_animation: # pragma: no cover
plt.cla()
# for stopping simulation with the esc key.
plt.gcf().canvas.mpl_connect(
'key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
plot_arrow(state.x, state.y, state.yaw)
plt.plot(cx, cy, "-r", label="course")
plt.plot(states.x, states.y, "-b", label="trajectory")
plt.plot(cx[target_ind], cy[target_ind], "xg", label="target")
plt.axis("equal")
plt.grid(True)
plt.title("Speed[km/h]:" + str(state.v)[:4])
plt.pause(0.001)
if cfg.show_animation_gain:
plt.cla()
plt.gcf().canvas.mpl_connect(
'key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
plt.grid(True)
plt.plot(states.t, [iv for iv in states.v], "-r")
plt.legend()
plt.xlabel("Time[s]")
plt.ylabel("Speed[km/h]")
plt.axis("equal")
plt.title("Speed[km/h]:" + str(state.v)[:4])
plt.grid(True)
plt.pause(0.00001)
# loop check
controller.loop += 1
else:
print("lastIndex < target_ind")
print("*-*-*-*-*-*-END-*-*-*-*-*-*")
# ----------------------------------------------------
# set target V
if key == '0':
udp.VC_SwitchOn = 0
print('maneuver')
if key == '1':
udp.VC_SwitchOn = 1
udp.GearNo = 1
tar_vel_ = 10
if key == '2':
udp.VC_SwitchOn = 1
udp.GearNo = 1
tar_vel_ = 20
if key == '3':
udp.VC_SwitchOn = 1
udp.GearNo = 1
tar_vel_ = 30
if key == '4':
udp.VC_SwitchOn = 1
udp.GearNo = 1
tar_vel_ = 40
if key == '5':
udp.VC_SwitchOn = 1
udp.GearNo = 1
tar_vel_ = 50
# Brake
if key == 'c':
udp.VC_SwitchOn = 1
udp.GearNo = 0
udp.Ax = 0
udp.SteeringWheel = 0
print('stop - 0.5')
if key == 'x':
udp.VC_SwitchOn = 1
udp.GearNo = -9
udp.Ax = 0
udp.SteeringWheel = 0
print('stop - 1')
else:
if (key == '\x03'):
break
controller.car_pub.publish(udp)
# draw plot
try:
if controller.show_animation: # pragma: no cover
plt.cla()
plt.plot(cx, cy, ".r", label="course")
plt.plot(states.x, states.y, "-b", label="trajectory")
plt.legend()
plt.xlabel("x[m]")
plt.ylabel("y[m]")
plt.axis("equal")
plt.grid(True)
plt.subplots(1)
plt.plot(states.t, [iv for iv in states.v], "-r")
plt.xlabel("Time[s]")
plt.ylabel("Speed[km/h]")
plt.grid(True)
plt.show()
except AttributeError:
print("*-*-*-n-o-d-a-t-a-*-*-*")
def main():
global settings
settings = termios.tcgetattr(sys.stdin)
rospy.init_node('controller')
controller = Controller()
PID_controller = Pid_Controller()
cfg = Config()
udp = sub_udp()
states = States()
pose_dict = mt.get_csv()
cx = pose_dict['x']
cy = pose_dict['y']
target_course = TargetCourse(cx, cy)
udp.GearNo = -9
udp.VC_SwitchOn = 1
controller.loop = 0
v_dt = 0
distance_old = 0
count = 0
y_line_state = 9
tar_vel_ = 0
Lf_k = 1.6
listener = tf.TransformListener()
# start -----------------------------------------------
while 1:
# cal dt
if controller.car_lasted_time == 0:
controller.car_lasted_time = controller.car_now_time
controller.car_dt_time = controller.car_now_time - controller.car_lasted_time
controller.car_lasted_time = controller.car_now_time
key = getKey()
try:
(trans, rot) = listener.lookupTransform('localization',
'Wheel_Rear',
rospy.Time(0))
print(trans)
print(rot)
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
continue
try:
# use localization
state = State(x=controller.car_local_point_x,
y=controller.car_local_point_y,
yaw=controller.car_local_point_theta,
v=controller.cur_vel)
last_index = len(cx) - 1
target_ind, Ld, now_ind, test_mode, test_ind_1, test_ind_2 = target_course.search_target_index(
state, Lf_k)
cur_vel_ = controller.cur_vel
cur_dt = controller.car_dt_time
except (AttributeError, IndexError) as set_err:
print(set_err)
print("*-*-*-L-O-D-I-N-G-*-*-*")
continue
if last_index > target_ind:
Ks = 0.08
mov_vel = PID_controller.accel_control(tar_vel_, cur_vel_, cur_dt,
1, 0, 1)
udp.Ax = mov_vel
nx = target_course.cx[now_ind]
zx = target_course.cx[0]
ny = target_course.cy[now_ind]
zy = target_course.cy[0]
# pure mode
# delta_gain, tx, ty = pure_pursuit_steer_control(state, target_course, target_ind, Ks)
delta_gain, tx, ty, alpha, delta_rad, delta, alpha_m, delta_m_rad, delta_m, Ld_m, car_alpha_m, car_pr, car_fr, pu_ind, pu_pind = pure_pursuit_steer_control(
state, target_course, target_ind, Ks)
udp.SteeringWheel = delta_gain
print(
"------------------------------------------------------------------------------"
)
print("yaw : ", (controller.cur_yaw) % (np.pi * 2))
print("car v : ", state.v)
print("delta_gain : ", delta_gain)
print("")
print("tx - : ", tx, "ty - : ", ty)
print("Ld _ origin : ", Ld, "Ld _ cal : ", Ld_m)
print("alpha _ origin : ", alpha, "alpha _ cal : ", alpha_m)
print("delta rad _ origin : ", delta_rad, "delta rad _ cal : ",
delta_m_rad)
print("delta _ origin : ", delta, "delta _ cal : ", delta_m)
print("yaw _ origin : ", controller.cur_yaw)
print("yaw _ 360 ", (controller.cur_yaw) % (np.pi * 2))
print("yaw _ - tar : ", car_pr, "yaw _ - cal : ", car_fr,
"yaw - ", car_pr - car_fr)
print("yaw _ - cal : ", car_alpha_m)
tx_tar = target_course.cx[target_ind]
tx_now = target_course.cx[now_ind]
ty_tar = target_course.cy[target_ind]
ty_now = target_course.cy[now_ind]
tx_tar_1 = target_course.cx[now_ind + 1]
ty_tar_1 = target_course.cy[now_ind + 1]
tx_tar_2 = target_course.cx[now_ind + 2]
ty_tar_2 = target_course.cy[now_ind + 2]
tx_tar_3 = target_course.cx[now_ind + 3]
ty_tar_3 = target_course.cy[now_ind + 3]
print("test mode", test_mode, "\n test ind 1", test_ind_1,
"test in 2", test_ind_2)
print(" tx_tar : ", tx_tar, " ty_tar : ", ty_tar)
print(" tx_tar_1 : ", tx_tar_1, " ty_tar_1 : ", ty_tar_1)
print(" tx_tar_2 : ", tx_tar_2, " ty_tar_2 : ", ty_tar_2)
print(" tx_now : ", tx_now, " ty_now : ", ty_now)
print(target_ind - now_ind, "", last_index)
print("")
print("localization1 x : ", controller.car_local_point_x,
"localization1 y : ", controller.car_local_point_y)
print("localization2 x : ", controller.car_local_2_point_x,
"localization2 y : ", controller.car_local_2_point_y)
print("rear x : ", state.rear_x, "rear y : ", state.rear_y)
print("front x : ", state.front_x, "front y : ", state.front_y)
print("lo_1 dx : ", controller.car_local_point_x - tx,
"lo_1 dy : ", controller.car_local_point_y - ty)
print("lo_2 dx : ", controller.car_local_2_point_x - tx,
"lo_2 dy : ", controller.car_local_2_point_y - ty)
dlo_1 = np.hypot(tx - controller.car_local_point_x,
ty - controller.car_local_point_y)
dlo_1_0 = np.hypot(tx_tar - controller.car_local_point_x,
ty_tar - controller.car_local_point_y)
dlo_1_1 = np.hypot(tx_tar_1 - controller.car_local_point_x,
ty_tar_1 - controller.car_local_point_y)
dlo_1_2 = np.hypot(tx_tar_2 - controller.car_local_point_x,
ty_tar_2 - controller.car_local_point_y)
dlo_1_3 = np.hypot(tx_tar_3 - controller.car_local_point_x,
ty_tar_3 - controller.car_local_point_y)
dlo_1_now = np.hypot(tx_now - controller.car_local_point_x,
ty_now - controller.car_local_point_y)
dlo_2 = np.hypot(tx - controller.car_local_2_point_x,
ty - controller.car_local_2_point_y)
dlo_2_0 = np.hypot(tx_tar - controller.car_local_2_point_x,
ty_tar - controller.car_local_2_point_y)
dlo_2_1 = np.hypot(tx_tar_1 - controller.car_local_2_point_x,
ty_tar_1 - controller.car_local_2_point_y)
dlo_2_2 = np.hypot(tx_tar_2 - controller.car_local_2_point_x,
ty_tar_2 - controller.car_local_2_point_y)
dlo_2_3 = np.hypot(tx_tar_3 - controller.car_local_2_point_x,
ty_tar_3 - controller.car_local_2_point_y)
dlo_2_now = np.hypot(tx_now - controller.car_local_2_point_x,
ty_now - controller.car_local_2_point_y)
print("lo_1 d : ", dlo_1_now, dlo_1_0, dlo_1_1, dlo_1_2, dlo_1_3)
print("lo_2 d : ", dlo_2_now, dlo_2_0, dlo_2_1, dlo_2_2, dlo_2_3)
print(" target ind : ", target_ind, " | ", last_index)
print(" now ind : ", now_ind, " | ", last_index)
print(" pure_ cal ind : ", pu_ind, "pu ind : ", pu_pind)
print("")
print("tx : ", tx, "ty : ", ty)
print("nx : ", nx, "ny : ", ny)
print("state x : ", state.x, "state y : ", state.y)
print("distance target x : ", tx - state.x, "distance target y : ",
ty - state.y)
dtar = np.hypot(state.x - tx, state.y - ty)
dnow = np.hypot(state.x - nx, state.y - ny)
print("")
print("distance target : ", dtar, "distance now : ", dnow)
print("Ld _ origin : ", Ld, "Ld _ cal : ", Ld_m)
# ----------------------------------------------------------------------------------
controller.loop += 1
# draw plot
v_dt += cur_dt
states.append(v_dt, state)
# -------------------------------------------------------------------------------------
# drawing plot
if cfg.show_animation: # pragma: no cover
plt.cla()
# for stopping simulation with the esc key.
plt.gcf().canvas.mpl_connect(
'key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
plot_arrow(state.x, state.y, state.yaw)
plt.plot(cx, cy, "-r", label="course")
plt.plot(states.x, states.y, "-b", label="trajectory")
plt.plot(cx[target_ind], cy[target_ind], "xg", label="target")
plt.axis("equal")
plt.grid(True)
plt.title("Speed[km/h]:" + str(state.v)[:4])
plt.pause(0.001)
if cfg.show_animation_gain:
plt.cla()
plt.gcf().canvas.mpl_connect(
'key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
plt.grid(True)
plt.plot(states.t, [iv for iv in states.v], "-r")
plt.legend()
plt.xlabel("Time[s]")
plt.ylabel("Speed[km/h]")
plt.axis("equal")
plt.title("Speed[km/h]:" + str(state.v)[:4])
plt.grid(True)
plt.pause(0.00001)
#
else:
print("lastIndex < target_ind")
print("*-*-*-*-*-*-END-*-*-*-*-*-*")
# ----------------------------------------------------
# set target V
if key == '0':
udp.VC_SwitchOn = 0
print('maneuver')
if key == '1':
udp.VC_SwitchOn = 1
udp.GearNo = 1
tar_vel_ = 10
if key == '2':
udp.VC_SwitchOn = 1
udp.GearNo = 1
tar_vel_ = 20
if key == '3':
udp.VC_SwitchOn = 1
udp.GearNo = 1
tar_vel_ = 30
if key == '4':
udp.VC_SwitchOn = 1
udp.GearNo = 1
tar_vel_ = 40
if key == '5':
udp.VC_SwitchOn = 1
udp.GearNo = 1
tar_vel_ = 50
if key == '9':
udp.VC_SwitchOn = 1
udp.GearNo = 1
tar_vel_ = 1
if key == '8':
udp.VC_SwitchOn = 1
udp.GearNo = 1
tar_vel_ = 3
if key == '7':
udp.VC_SwitchOn = 1
udp.GearNo = 1
tar_vel_ = 5
if key == 'w':
udp.VC_SwitchOn = 1
if udp.Ax >= 0:
udp.GearNo = 1
else:
udp.GearNo = -1
udp.Ax += 1
if key == 'a':
udp.VC_SwitchOn = 1
udp.SteeringWheel += 0.1
# mode_pub.publish(udp)
print('SteeringWheel +')
if key == 'd':
udp.VC_SwitchOn = 1
udp.SteeringWheel -= 0.1
# mode_pub.publish(udp)
print('SteeringWheel -')
if key == 'e':
udp.VC_SwitchOn = 1
udp.SteeringWheel = 0.0
# mode_pub.publish(udp)
print('SteeringWheel 0')
if key == 's':
udp.VC_SwitchOn = 1
if udp.Ax >= 0:
udp.GearNo = 1
else:
udp.GearNo = -1
udp.Ax -= 1
# mode_pub.publish(udp)
print('Ax -')
# Brake
if key == 'c':
udp.VC_SwitchOn = 1
udp.GearNo = 0
udp.Ax = 0
udp.SteeringWheel = 0
print('stop - 0.5')
if key == 'x':
udp.VC_SwitchOn = 1
udp.GearNo = -9
udp.Ax = 0
udp.SteeringWheel = 0
print('stop - 1')
else:
if (key == '\x03'):
break
controller.car_pub.publish(udp)
# draw plot
try:
if controller.show_animation: # pragma: no cover
plt.cla()
plt.plot(cx, cy, ".r", label="course")
plt.plot(states.x, states.y, "-b", label="trajectory")
plt.legend()
plt.xlabel("x[m]")
plt.ylabel("y[m]")
plt.axis("equal")
plt.grid(True)
plt.subplots(1)
plt.plot(states.t, [iv for iv in states.v], "-r")
plt.xlabel("Time[s]")
plt.ylabel("Speed[km/h]")
plt.grid(True)
plt.show()
except AttributeError:
print("*-*-*-n-o-d-a-t-a-*-*-*")