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-*-*-*")