def process_lidar(lidar_data): """ process the lidar image """ if not SCHEDULER.is_lidar_enable(): return if SCHEDULER.is_lidar_occupied() or TURTLE.is_occupied(): return state = SCHEDULER.get_state() if SCHEDULER.debug_option["show_timer"] and (state != "construction"): SCHEDULER.check_time("lidar", min=0.4, stop_when_delay=False) set_lidar_values(lidar_data) state = SCHEDULER.get_state() front = get_object_distance("front") if (front < 0.15) and (front > 0): TURTLE.stop() return if state is "default": leftside = get_object_distance("leftside") print(leftside) if (leftside < 0.35) and (leftside > 0): rospy.Timer(rospy.Duration(5), SCHEDULER.release_lidar_occupied, oneshot=True) SCHEDULER.set_state("to_construction") return elif state is "to_construction": leftside = get_object_distance("leftside") print("to_construction: " + str(leftside)) if (leftside < 0.35) and (leftside > 0): SCHEDULER.set_state("construction") return # rospy.Timer( # rospy.Duration(0.15), SCHEDULER.release_lidar_occupied, oneshot=True # ) # process_construction() elif state is "construction": process_construction() elif state is "parking": process_parking()
def process_lidar(lidar_data): """ process the lidar image """ if not SCHEDULER.is_lidar_enable(): return if SCHEDULER.is_lidar_occupied() or TURTLE.is_occupied(): return state = SCHEDULER.get_state() if SCHEDULER.debug_option["show_timer"] and (state != "construction"): SCHEDULER.check_time("lidar", min=0.4, stop_when_delay=False) set_lidar_values(lidar_data) state = SCHEDULER.get_state() front = get_object_distance("front") if (front < 0.15) and (front > 0): TURTLE.stop() return # if STEP >= 10: # process_parking() # else: # process_construction() # return if state is "intersection_left": left = get_object_distance("left") if (left > 0) and (left < 0.50): TURTLE.set_speed("slow") SCHEDULER.set_state("to_construction") elif state is "to_construction": rospy.Timer(rospy.Duration(0.15), SCHEDULER.release_lidar_occupied, oneshot=True) process_construction_new() elif state is "construction": process_construction_new() elif state is "parking": process_parking() elif state is "tunnel": process_tunnel()
def process_frontcam(image): """ process the frontcam image """ if not SCHEDULER.is_frontcam_enable(): return STATE = SCHEDULER.get_state() info = EYE.see_front(image) if SCHEDULER.debug_option["show_front_info"]: rospy.logdebug(info) if STATE == "default": # if EYE.is_boostable(image): process_acceleration(info) # signal = is_construction(image) # rospy.logdebug(signal) if STATE == "traffic_light": if is_light_green(image): TURTLE.enable() SCHEDULER.set_state("to_intersection") return if STATE == "to_intersection": signal = check_left_right_sign(image) if signal == "right": SCHEDULER.set_state("intersection_right") elif signal == "left": SCHEDULER.set_state("intersection_left") return if STATE == "intersection_right": # TODO: make algorithms for right if EYE.is_boostable(image): TURTLE.boost() SCHEDULER.set_state("to_construction") return if STATE == "intersection_left": if EYE.is_boostable(image): TURTLE.boost() SCHEDULER.set_state("to_construction") return if STATE == "to_construction": if EYE.is_boostable(image): TURTLE.boost() if is_construction(image): SCHEDULER.set_state("construction_searching") if STATE == "construction_searching": pass
def process_lidar(lidar_data): """ process the lidar image """ if not SCHEDULER.is_lidar_enable(): return set_lidar_values(lidar_data) state = SCHEDULER.get_state() if state is "construction": process_construction() elif state is "parking": process_parking() elif state is "tunnel": process_tunnel()
def test_frontcam(image): """ process the frontcam image """ # print("image is subscribed") if not SCHEDULER.is_frontcam_enable(): return if not EYE.is_front_occupied(): STATE = SCHEDULER.get_state() if STATE == "traffic_light": signal = is_light_green(image) rospy.logdebug("[PROC] is_light_green: {}" .format(signal)) if signal: SCHEDULER.set_state("to_intersection") TURTLE.enable() TURTLE.set_speed("fast") TURTLE.set_speed_smooth("normal") elif STATE == "to_intersection": signal = check_left_right_sign(image) rospy.logdebug("[PROC] left or right: {}" .format(signal)) if signal == "right": SCHEDULER.set_state("intersection_right") elif signal == "left": SCHEDULER.set_state("intersection_left") if SCHEDULER.debug_option["show_center_slope"]: SCHEDULER.check_time("frontcam", min=0.5) info = EYE.see_front(image) if info is None: return # rospy.logdebug("[PROC] test_frontcam: state {} {}".format( # info["state"], info["horizon_position"])) # if (info["horizon_position"] < 150): # TURTLE.set_speed("fast") # TURTLE.set_speed_smooth("normal") if (info["state"] == "turning") and (info["turning_to"] is not "None"): rospy.logdebug( "[PROC] turn off for 1.5s") rospy.Timer(rospy.Duration(1.5), EYE.release_front_occupied, oneshot=True) else: rospy.Timer(rospy.Duration(0.1), EYE.release_front_occupied, oneshot=True)
def process_frontcam(image): """ process the frontcam image """ if not SCHEDULER.is_frontcam_enable(): return state = SCHEDULER.get_state() info = EYE.see_front(image) if SCHEDULER.debug_option["show_front_info"]: rospy.logdebug(info) if state == "default": # if EYE.is_boostable(image): process_acceleration(info) # SCHEDULER.set_state("to_intersection") # signal = is_construction(image) # rospy.logdebug(signal) # if is_construction(image): # TURTLE.boost() # SCHEDULER.set_state("construction") if state == "traffic_light": if is_light_green(image): TURTLE.enable() SCHEDULER.set_state("to_intersection") return # NOTE: temporary settings: if state == "to_intersection": # rospy.Timer(rospy.Duration(35), SCHEDULER.enable_lidar, oneshot=True) SCHEDULER.set_state("intersection_left") # if state == "to_intersection": # signal = check_left_right_sign(image) # if signal == "right": # SCHEDULER.set_state("intersection_right") # elif signal == "left": # SCHEDULER.set_state("intersection_left") # return if state == "intersection_right": # TODO: make algorithms for right if EYE.is_boostable(image): TURTLE.boost() SCHEDULER.set_state("to_construction") return # NOTE: temporary settings: if state == "intersection_left": # if EYE.is_boostable(image): # TURTLE.boost() # SCHEDULER.set_state("to_construction") return if state == "to_construction": # if EYE.is_boostable(image): # TURTLE.boost() EYE.check_yellow = True # if is_construction(image): # SCHEDULER.set_state("construction_searching") if state == "construction_searching": pass
def process_subcam(image): """ process the subcam image """ if not SCHEDULER.is_subcam_enable(): return if SCHEDULER.is_subcam_occupied() or TURTLE.is_occupied(): return info = EYE.see_sub(image) if SCHEDULER.debug_option["show_timer"]: # Check delay only if has line # SCHEDULER.check_time("subcam", min=0.28, stop_when_delay=info["has_line"]) SCHEDULER.check_time("subcam", min=0.28, stop_when_delay=False) if info is None: rospy.logwarn("[PROC] No Information!") return center = info["center"] slope = info["slope"] if slope < -0.5: limit = 1.6 amplitude = 1.0 else: limit = 1.2 amplitude = 0.8 limit /= 1.9 # amplitude /= 2 state = SCHEDULER.get_state() if (EYE.get_front_state() == "straight") and (state is not "zigzag"): if (abs(center) < 30) and slope < -0.4: degree = pow(abs(slope) / 1.8, 1.1) * amplitude elif center < 0: degree = pow(abs(center) / 100, 2.0) * amplitude / 2 elif center > 0: degree = -pow(abs(center) / 100, 2.0) * amplitude else: degree = 0 else: if slope < 0: degree = -pow(abs(slope) / 1.8, 2.9) * amplitude * 28.0 else: degree = pow(abs(slope) / 1.0, 1.2) * amplitude * 4.0 # degree = pow(abs(slope) / 1.8, 0.9) * amplitude # elif center < 0: # degree = pow(abs(center) / 100, 1.9) * amplitude # elif center > 0: # degree = - pow(abs(center) / 100, 1.9) * amplitude # else: # degree = 0 buf_sum = sum(BUF_ANGULAR) if EYE.get_front_state() == "straight": adjust_angular = BUF_ANGULAR.pop(0) * 0.9 BUF_ANGULAR.append(degree) degree -= adjust_angular # if abs(buf_sum) > 1: else: reset_buffer() adjust_angular = 0 degree = max(min(degree, limit), -limit) if not info["has_line"]: if center > 200: # degree = -1.2 degree = -1.1 # For enhancing frequency # elif EYE.get_front_state() == "straight": # degree = 0.6 else: # degree = 1.4 degree = 1.3 # For enhancing frequency # if not info["has_line"]: # if center < -55: # # degree = 1.6 # degree = 1.4 # For slow speed # elif center > 50: # degree = -1.2 # # degree = -1.2 # For slow speed # # elif center > 19: # # degree = -1.2 # else: # degree = 1.4 if SCHEDULER.debug_option["show_center_slope"]: rospy.logdebug( "[PROC] center: {:.2f} slope: {:.2f} degree: {:.2f} adj: {:.2f} buf_sum: {:.2f} {} {}" .format(center, slope, degree, adjust_angular, buf_sum, EYE.get_front_state(), info["has_line"])) rospy.Timer(rospy.Duration(0.15), SCHEDULER.release_subcam_occupied, oneshot=True) TURTLE.turn(0.13, degree)