def process_tunnel2(lidar_data):
global LIDAR_FLAG
if LIDAR_FLAG is False:
return
global DIRECTION
global IS_TURNING
TURTLE.enable_fish = False
set_lidar_values(lidar_data)
TURTLE.set_speed_smooth("fast")
directions = ["front", "leftside", "rightside"]
distances = {}
for direction in directions:
distances[direction] = get_object_distance(direction)
if not IS_TURNING:
print(distances["leftside"], distances["rightside"])
if distances["leftside"] < 0.45 and distances["leftside"] > 0:
TURTLE.turn("right", 1.5)
DIRECTION = "left"
IS_TURNING = True
elif distances["rightside"] < 0.45 and distances["rightside"] > 0:
TURTLE.turn("left", 1.5)
DIRECTION = "right"
IS_TURNING = True
else:
print(get_object_distance("front"))
if get_object_distance("front") == 0:
rospy.signal_shutdown("[TURTLE] shutting down...")
else:
TURTLE.turn(DIRECTION, 1.5)
# TURTLE.turn(DIRECTION, 1.5)
IS_TURNING = False
def process_tunnel2():
global DIRECTION
global IS_TURNING
global IS_INSIDE_TUNNEL
TURTLE.set_speed_smooth("fast")
directions = ["front", "leftside", "rightside"]
distances = {}
for direction in directions:
distances[direction] = get_object_distance(direction)
if not IS_INSIDE_TUNNEL:
distance = get_object_distance("right")
if (distance < 0.3 and distance < 0):
IS_INSIDE_TUNNEL = True
TURTLE.enable_fish = False
elif not IS_TURNING:
rospy.loginfo("\n[PROC] tunnel: " + distances["leftside"] +
str(" ") + distances["rightside"])
if distances["leftside"] < 0.45 and distances["leftside"] > 0:
TURTLE.turn("right", 1.5)
DIRECTION = "left"
IS_TURNING = True
elif distances["rightside"] < 0.45 and distances["rightside"] > 0:
TURTLE.turn("left", 1.5)
DIRECTION = "right"
IS_TURNING = True
else:
print(get_object_distance("front"))
if get_object_distance("front") == 0:
rospy.signal_shutdown("[TURTLE] shutting down...")
else:
TURTLE.turn(DIRECTION, 1.5)
# TURTLE.turn(DIRECTION, 1.5)
IS_TURNING = False
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_tunnel():
global DIRECTION
TURTLE.set_speed_smooth("fast")
directions = ["front", "leftside", "rightside"]
min_distance = 1000
for direction in directions:
distance = get_object_distance(direction)
if distance > 0 and distance < min_distance:
min_distance = distance
# distance = min(get_object_distance("front"), get_object_distance("leftside"), get_object_distance("rightside"))
# distance = get_object_distance("front")
rospy.loginfo(str(min_distance) + str(distance))
if min_distance < 0.50 and min_distance > 0:
TURTLE.turn(DIRECTION, 2.03, True, True)
if DIRECTION == "left":
DIRECTION = "right"
elif DIRECTION == "right":
DIRECTION = "left"
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_blocking():
directions = ["front", "leftside", "rightside"]
distances = {}
rospy.loginfo("-------------")
for direction in directions:
distances[direction] = get_object_distance(direction)
rospy.loginfo("\n[PROC] " + str(direction) + ": " + distances[direction])
rospy.loginfo("-------------")
if distances["rightside"] < 0.3 and distances["rightside"] > 0:
TURTLE.enable_fish = False
TURTLE.go_forward(2)
SEEN_BLOCKING_BAR = True
elif SEEN_BLOCKING_BAR:
if distances["front"] > 0 or distances["leftside"] > 0 or distances["rightside"] > 0:
TURTLE.enable_fish = True
CURRENT_STATE = "tunnel"
print_state()
else:
rospy.Timer(rospy.Duration(5.0), go_anyway, oneshot=True, reset=True)
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 initialize():
""" initialize processing """
EYE.calibrate()
TURTLE.set_speed('normal')
if CURRENT_STATE is "traffic_light":
TURTLE.disable()
else:
TURTLE.enable()
def process_tunnel(lidar_data):
global LIDAR_FLAG
if LIDAR_FLAG is False:
return
global DIRECTION
TURTLE.enable_fish = False
set_lidar_values(lidar_data)
TURTLE.set_speed_smooth("fast")
directions = ["front", "leftside", "rightside"]
min_distance = 1000
for direction in directions:
distance = get_object_distance(direction)
if distance > 0 and distance < min_distance:
min_distance = distance
# distance = min(get_object_distance("front"), get_object_distance("leftside"), get_object_distance("rightside"))
# distance = get_object_distance("front")
rospy.loginfo(str(min_distance) + str(distance))
if min_distance < 0.50 and min_distance > 0:
TURTLE.turn(DIRECTION, 2.03, True, True)
if DIRECTION == "left":
DIRECTION = "right"
elif DIRECTION == "right":
DIRECTION = "left"
def process_acceleration(info):
if not info:
return
horizon = info['horizon_position']
center = abs(info['center'])
if EYE.get_front_state() == "straight":
if horizon < 200 and center < 50:
TURTLE.boost()
if SCHEDULER.debug_option["show_front_info"]:
rospy.logwarn("[PROC] Boosting")
elif horizon < 260 and center < 50:
TURTLE.set_speed_smooth('little')
if horizon > 320 or info['state'] == 'turning':
TURTLE.set_speed('normal')
elif horizon > 260:
TURTLE.set_speed_smooth('normal')
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_construction_new():
""" process construction state """
global STEP
global NUM_OBSTRUCTION
global LANE_TO
global BUF_ANGULAR
global BUF_SIZE
if TURTLE.is_occupied():
return
if STEP == 0:
# TURTLE.set_speed("normal")
leftside = get_object_distance("leftside")
left = get_object_distance("left")
if leftside > 0:
rospy.logdebug("[PROC] LIDAR LEFTSIDE: {}".format(leftside))
if (leftside > 0) and (leftside < 0.50) and (left > 1.00):
EYE.check_yellow = False
SCHEDULER.set_state("construction")
rospy.loginfo("[PROC] construction state started.")
STEP = 1
rospy.logdebug("[PROC] STEP changed to {}".format(STEP))
TURTLE.go_forward(3.5)
return
else:
return
elif STEP == 1:
TURTLE.set_speed("normal")
TURTLE.set_speed_smooth("slow")
TURTLE.turn(0.13, 0)
left = get_object_distance("left")
leftback = get_object_distance("leftback")
rospy.logdebug("[PROC] LIDAR LEFT: {:.2f} LEFTBACK: {:.2f}}".format(
left, leftback))
if (left > 0) and (left < 0.50):
return
else:
TURTLE.set_speed("slow")
if (leftback > 0.5):
# TURTLE.go_forward(2.5)
STEP = 3
rospy.logdebug("[PROC] STEP changed to {}".format(STEP))
return
elif STEP == 2:
# TODO: write code for first left lane
pass
elif STEP == 3:
TURTLE.set_speed("normal")
TURTLE.set_speed_smooth("stop")
front = get_object_distance("front")
right_biased = get_object_distance("right_biased")
if (front > 1.0) and (right_biased < 1.0) and (right_biased > 0.0):
print("passed", front, right_biased)
print("BUF_SIZE: ", BUF_SIZE)
BUF_SIZE = 15
reset_buffer()
TURTLE.set_speed_smooth("slow")
pass
elif front * right_biased == 0:
return
else:
TURTLE.set_speed("stop")
print("turning...", front, right_biased)
TURTLE.turn(0.13, 1.0)
# rospy.sleep(rospy.Duration(5.0))
return
# TURTLE.go_turn("left")
elif STEP == 4:
right_biased = get_object_distance("right_biased")
left_biased = get_object_distance("left_biased")
front = get_object_distance("front")
if right_biased == 0.0:
right_biased = 3.0
if left_biased == 0.0:
left_biased = 3.0
if front == 0.0:
front = 3.0
elif front < 0.2:
TURTLE.set_speed_smooth("stop")
else:
TURTLE.set_speed_smooth("slow")
min_distance = min(right_biased, left_biased)
degree = 0
if (front < 1.0):
degree += max(pow(1.0 - front, 2), 0)
else:
degree += max(0.5 - min_distance, 0) * 3
# if min_distance < 0.5:
# degree += max((0.5 - min_distance), 0) * 1.5
# elif (min_distance > 1.0) and (min_distance < 3.0):
# degree = 0.2
if (left_biased == min_distance) and (min_distance < 0.5):
degree *= -1
# max_distance = max(right_biased, left_biased)
# if (left_biased == max_distance):
# degree *= -1
# degree = 0
# if min_distance > 0 and min_distance < 0.5:
# if right_biased > left_biased:
# degree = (0.5 - min_distance) * (-7)
# LANE_TO = "right"
# elif right_biased < left_biased:
# degree = (0.5 - min_distance) * (7)
# LANE_TO = "left"
# if is_left_crashable():
# degree = -1.7
# elif is_right_crashable():
# degree = 1.7
degree *= 3
degree = max(min(degree, 2.0), -2.0)
BUF_ANGULAR.append(degree)
degree -= BUF_ANGULAR.pop(0)
print("BUF_ANGULAR:", BUF_ANGULAR)
# if degree != 0:
# BUF_ANGULAR.append(degree)
# elif len(BUF_ANGULAR) > 9:
# STEP = 5
if SCHEDULER.debug_option["show_construction_lidar"]:
rospy.logdebug(
"[PROC] r_based: {:.2f} l_based: {:.2f} min: {:.2f} front: {:.2f} deg: {:.2f}"
.format(right_biased, left_biased, min_distance, front,
degree))
TURTLE.turn(0.13, degree)
return
elif STEP == 5:
print("[StEP 5]")
if len(BUF_ANGULAR) > 0:
TURTLE.turn(0.13, -BUF_ANGULAR.pop(0))
return
else:
front = get_object_distance("front")
print(front)
# if (front > 0) and (front < 1.0):
# if LANE_TO == "right":
# TURTLE.turn(0.13, 0.8)
# else:
# TURTLE.turn(0.13, -0.8)
# return
# else:
if NUM_OBSTRUCTION < 1:
NUM_OBSTRUCTION += 1
STEP = 4
return
elif STEP == 6:
TURTLE.go_forward(1)
TURTLE.set_speed("normal")
TURTLE.go_turn("left")
TURTLE.set_speed("normal")
TURTLE.set_speed("fast")
TURTLE.set_speed_smooth("normal")
TURTLE.go_forward(5)
STEP = 10
SCHEDULER.set_state("parking")
BUF_SIZE = 3
reset_buffer()
return
STEP += 1
rospy.logdebug("[PROC] STEP changed to {}".format(STEP))
#!/usr/bin/env python3
import rospy
# from std_msgs.msg import String
from sensor_msgs.msg import CompressedImage, LaserScan
import processor
from constants import PATH_RASPICAM, PATH_USBCAM, PATH_LIDAR, PATH_GALAPAGOS_STATE
from turtlebot import TURTLE
from scheduler import SCHEDULER # ! not used in contest
TURTLE.set_speed("normal")
# TURTLE.disable()
# TURTLE.set_speed("fast")
# TURTLE.set_speed_smooth("normal")
# SCHEDULER.set_state("traffic_light")
SCHEDULER.set_state("default")
# SCHEDULER.set_state("to_intersection")
# SCHEDULER.set_state("construction")
# SCHEDULER.set_state("parking")
rospy.Subscriber(PATH_USBCAM,
CompressedImage,
processor.process_subcam,
queue_size=1)
# rospy.Subscriber(PATH_LIDAR, LaserScan,
# processor.test_block_escaping, queue_size=1)
# rospy.Subscriber(PATH_LIDAR, LaserScan,
# processor.process_lidar, queue_size=1)
# rospy.Subscriber(PATH_RASPICAM, CompressedImage,
# processor.process_frontcam, queue_size=1)
def process_lidar(lidar_data):
""" process the lidar data """
global CURRENT_STATE
global IS_TURNING
global STATE_CONSTRUCTION
global DIRECTION
global TURN_COUNT
global ENABLE_LIDAR
global ENABLE_FRONT
if not ENABLE_LIDAR:
return
set_lidar_values(lidar_data)
if CURRENT_STATE is "construction":
if not TURTLE.is_settable():
return
rospy.loginfo(
str(STATE_CONSTRUCTION) + " " +
str(get_object_distance('left')) + " " +
str(get_object_distance('front')))
if STATE_CONSTRUCTION == "searching":
distance = get_object_distance("left")
if distance == 0:
return
elif distance < 0.35:
STATE_CONSTRUCTION = "ready"
elif distance > 0.35 and distance < 1:
STATE_CONSTRUCTION = "start"
DIRECTION = "right"
TURTLE.enable_fish = False
TURTLE.turn("left", 2.03, True)
return
elif STATE_CONSTRUCTION == "ready":
distance = get_object_distance("front")
if distance == 0:
return
elif distance < 0.5:
# TURTLE.change_line("left", 0.5)
DIRECTION = "left"
TURTLE.enable_fish = False
TURTLE.change_line("left", 1)
# TODO: FIX change_line()
STATE_CONSTRUCTION = "start"
return
elif STATE_CONSTRUCTION == "start":
if not IS_TURNING:
if DIRECTION == 'right':
distance = get_object_distance('leftside')
else:
distance = get_object_distance('rightside')
rospy.loginfo('\n[PROC] distance on ' + DIRECTION + ' : ' +
str(distance))
if distance == 0:
return
elif distance < 0.35:
TURTLE.turn(DIRECTION, 1.2)
IS_TURNING = True
# TURTLE.change_line(DIRECTION, 1)
reverse_direction()
TURN_COUNT = TURN_COUNT + 1
else:
TURTLE.set_speed('normal')
else:
if DIRECTION == 'right':
distance = get_object_distance('frontleft')
else:
distance = get_object_distance('frontright')
# rospy.loginfo('\n[PROC] turning distance on ' +
# DIRECTION + ' : ' + str(distance))
if distance > 0.34 or distance == 0:
reverse_direction()
TURN_COUNT = TURN_COUNT + 1
TURTLE.turn(DIRECTION, 1.2)
IS_TURNING = False
if TURN_COUNT == 3:
TURTLE.turn("left", 1.5, True)
STATE_CONSTRUCTION = "ending"
elif STATE_CONSTRUCTION == "ending":
TURTLE.go_forward(1.0)
CURRENT_STATE = "before_parking"
print_state()
# rospy.signal_shutdown("\n[PROC] Shutting down...")
if CURRENT_STATE is "blocking":
ENABLE_FRONT = False
process_blocking()
if CURRENT_STATE is "tunnel":
process_tunnel2()
def process_tunnel():
""" process tunnel state """
if TURTLE.is_occupied():
return
if STEP == 30:
# TODO: turn to 45 degrees
pass
elif STEP == 31:
TURTLE.set_speed("normal")
# TURTLE.go_turn("right", 1, angular=2, speed=0.15)
front = get_object_distance("front")
left = get_object_distance("left")
right = get_object_distance("right")
rospy.logdebug(
"front: {} left: {} right: {}".format(front, left, right))
if left < 0.20:
# TODO: turn to variable degrees
TURTLE.turn(0.15, -6)
STEP = 34
return
elif right < 0.20:
# TODO: turn to variable degrees
TURTLE.turn(-0.15, -6)
STEP = 35
return
elif front == 0 or front > 0.40:
return
else:
TURTLE.turn(0.15, -6)
TURTLE.turn(0.15, -6)
elif STEP == 32:
TURTLE.set_speed("normal")
frontleft = get_object_distance("frontleft")
rospy.logdebug("frontleft: {}".format(frontleft))
if frontleft == 0 or frontleft < 0.40:
return
else:
rospy.sleep(rospy.Duration(1.8))
TURTLE.turn(0.13, 6)
elif STEP == 33:
TURTLE.set_speed("normal")
frontleft = get_object_distance("frontleft")
rospy.logdebug("frontleft: {}".format(frontleft))
if frontleft == 0 or frontleft < 0.40:
return
else:
STEP = 31
return
elif STEP == 34:
front = get_object_distance("front")
if front == 0 or front > 0.20:
return
# TODO : turn to left
TURTLE.turn(-0.3, -6)
elif STEP == 35:
# NOTE: ending step
TURTLE.set_speed("normal")
else:
return
STEP += 1
#!/usr/bin/env python3
import rospy
from sensor_msgs.msg import CompressedImage, LaserScan
import processor
from constants import PATH_RASPICAM, PATH_USBCAM, PATH_LIDAR
from turtlebot import TURTLE
from scheduler import SCHEDULER
TURTLE.set_speed("fast")
TURTLE.set_speed_smooth("normal")
SCHEDULER.set_state("default")
# SCHEDULER.set_state("parking")
# SCHEDULER.set_state("construction")
rospy.Subscriber(PATH_USBCAM,
CompressedImage,
processor.process_subcam,
queue_size=1)
rospy.Subscriber(PATH_LIDAR, LaserScan, processor.process_lidar, queue_size=1)
rospy.Subscriber(PATH_RASPICAM,
CompressedImage,
processor.process_frontcam,
queue_size=1)
def process_fish_image(image):
""" process the fisheye lens image """
trace_line(image)
if CURRENT_STATE == 'intersection':
if sign_intersection == 'left':
TURTLE.set_speed_by_percentage(0.5)
TURTLE.set_angular(TURTLE._angular + 0.2)
elif sign_intersection == 'right':
TURTLE.set_speed_by_percentage(0.5)
TURTLE.set_angular(TURTLE._angular - 0.2)
elif CURRENT_STATE == 'stop_sign':
if left_detected > right_detected :
TURTLE.set_speed_by_percentage(0.5)
TURTLE.set_angular(TURTLE._angular + 0.2)
elif right_detected < left_detected :
TURTLE.set_speed_by_percentage(0.5)
TURTLE.set_angular(TURTLE._angular - 0.2)
TURTLE.move()
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)
def test_block_escaping(lidar_data):
global STEP
set_lidar_values(lidar_data)
if not TURTLE.is_occupied():
if STEP == 1:
TURTLE.set_speed("normal")
# TURTLE.go_turn("right", 1, angular=2, speed=0.15)
front = get_object_distance("front")
rospy.logdebug("front: {}".format(front))
if front == 0 or front > 0.40:
return
else:
TURTLE.turn(0.15, -6)
elif STEP == 2:
TURTLE.set_speed("normal")
frontleft = get_object_distance("frontleft")
rospy.logdebug("frontleft: {}".format(frontleft))
if frontleft == 0 or frontleft < 0.40:
return
else:
rospy.sleep(rospy.Duration(1.8))
TURTLE.turn(0.13, 6)
elif STEP == 3:
TURTLE.set_speed("slow")
else:
return
STEP += 1
else:
print("IS OCCUPIED!")
def test_parking(image):
global STEP
if not TURTLE.is_occupied():
if STEP == 1:
TURTLE.go_forward(0.5)
elif STEP == 2:
TURTLE.set_speed("normal")
TURTLE.go_turn("right", 2)
elif STEP == 3:
TURTLE.go_forward(0.7)
# for stopping for a while
rospy.sleep(rospy.Duration(0.5))
elif STEP == 4:
TURTLE.go_backward(1)
elif STEP == 5:
TURTLE.go_turn_backward(1.2)
elif STEP == 6:
TURTLE.go_forward(2)
elif STEP == 7:
TURTLE.set_speed("fast")
TURTLE.set_speed_smooth("normal")
# TURTLE.set_speed("normal")
else:
return
STEP += 1
def process_front_image(image):
""" process the image of raspicam """
global CURRENT_STATE
global MOVING_POSITION
global SEEN_PARKING_SIGN
global SEEN_TUNNEL_SIGN
raw_data = np.fromstring(image.data, np.uint8)
cv_img = cv2.imdecode(raw_data, cv2.IMREAD_COLOR)
#### ROI SETTING ######
blob_ROI = cv_img[100:, :]
#######################
if CURRENT_STATE == 'traffic_light':
if is_light_green(cv_img):
TURTLE.enable()
#TURTLE.set_speed('fast')
CURRENT_STATE = 'intersection'
return
else:
return
if CURRENT_STATE == 'intersection':
if is_intersection(cv_img):
TURTLE.set_weight(0.7)
CURRENT_STATE = 'left_or_right'
return
else:
return
if CURRENT_STATE == 'left_or_right':
tmp_state = check_left_right_sign(blob_ROI)
if tmp_state == 'right':
#print("11tmp state: ",tmp_state, ", right cnt: ",inter_right_cnt)
TURTLE.LINE_BASE = 2
#print("11tmp state: ",tmp_state, ", right cnt: ",inter_right_cnt)
CURRENT_STATE = 'inter_curving'
TURTLE.set_weight(1.0)
elif tmp_state == 'left':
#print("11tmp state: ",tmp_state, ", left cnt: ",inter_left_cnt)
TURTLE.LINE_BASE = 1
#print("22tmp state: ",tmp_state, ", left cnt: ",inter_left_cnt)
CURRENT_STATE = 'inter_curving'
TURTLE.set_weight(1.0)
elif tmp_state == 'none':
return
if CURRENT_STATE == 'inter_curving':
global straight_cnt
if abs(TURTLE.weight*TURTLE._angular) < 0.1:
straight_cnt += 1
if straight_cnt > 3 :
straight_cnt = 0
TURTLE.LINE_BASE = 3
CURRENT_STATE = 'construct_recog'
return
else:
return
else:
return
if CURRENT_STATE == 'construct_recog':
tmp_state = is_construction(blob_ROI)
if tmp_state is True:
TURTLE.LINE_BASE = 2
CURRENT_STATE = 'construction_ready'
else:
return
if CURRENT_STATE == 'construction_ready':
global curving_cnt
if abs(TURTLE.weight*TURTLE._angular) > 0.25 :
curving_cnt += 1
if curving_cnt > 5 :
curving_cnt = 0
CURRENT_STATE = 'construction'
TURTLE.enable_fish = False
else:
return
else:
return
if CURRENT_STATE == 'construction':
'''
task for Ji-hyung
'''
'''
def process_construction():
""" process construction state """
global STEP
global NUM_OBSTRUCTION
global LANE_TO
if TURTLE.is_occupied():
return
if STEP == 0:
leftside = get_object_distance("leftside")
left = get_object_distance("left")
if leftside > 0:
rospy.logdebug("[PROC] LIDAR LEFTSIDE: {}".format(
leftside))
if (leftside > 0) and (leftside < 0.40):
STEP = 1
rospy.logdebug("[PROC] STEP changed to {}".format(STEP))
SCHEDULER.disable_cams()
rospy.loginfo("[PROC] construction state started.")
TURTLE.go_forward(3.5)
rospy.sleep(rospy.Duration(0.5))
return
else:
return
# elif (left > 0) and (left < 1.5):
# STEP = 2
# rospy.logdebug("[PROC] STEP changed to {}".format(STEP))
# return
# rospy.sleep(rospy.Duration(2))
elif STEP == 1:
TURTLE.set_speed("normal")
TURTLE.set_speed_smooth("slow")
left = get_object_distance("left")
if left > 0:
rospy.logdebug("[PROC] LIDAR LEFT: {}".format(
left))
if (left < 0.50) or (left > 1.5):
return
else:
STEP = 3
rospy.logdebug("[PROC] STEP changed to {}".format(STEP))
return
elif STEP == 2:
# TODO: write code for first left lane
pass
elif STEP == 3:
TURTLE.go_turn("left", speed=0.11)
LANE_TO = "left"
elif STEP == 4:
TURTLE.set_speed("normal")
reverse_lane()
biased = get_object_distance(LANE_TO + "_biased")
if biased > 0:
rospy.logdebug("[PROC] LIDAR {:s}_BIASED: {}"
.format(LANE_TO, biased))
reverse_lane()
if (biased == 0) or (biased > 0.30):
return
TURTLE.go_turn(LANE_TO, duration=0.5, angular=4.2)
TURTLE.set_speed("normal")
# TURTLE.set_speed("fast")
# if LANE_TO is "left":
# TURTLE.turn(0.13, 1.3, consuming_time=1.5)
# else:
# TURTLE.turn(0.13, -1.3, consuming_time=1.5)
rospy.sleep(rospy.Duration(2.2))
reverse_lane()
elif STEP == 5:
TURTLE.go_turn(LANE_TO, duration=0.7, angular=4.2)
TURTLE.set_speed("normal")
# TURTLE.set_speed("fast")
# if LANE_TO is "left":
# TURTLE.turn(0.13, 1.3, consuming_time=1.5)
# else:
# TURTLE.turn(0.13, -1.3, consuming_time=1.5)
NUM_OBSTRUCTION += 1
if NUM_OBSTRUCTION < 2:
STEP = 4
return
elif STEP == 6:
TURTLE.go_forward(1)
rospy.sleep(rospy.Duration(0.6))
elif STEP == 7:
TURTLE.go_turn("left", duration=0.8, angular=3)
elif STEP == 8:
TURTLE.set_speed("fast")
TURTLE.set_speed_smooth("normal")
TURTLE.go_forward(5)
rospy.sleep(rospy.Duration(0.5))
elif STEP == 9:
# NOTE: turn to parking step
STEP = 10
SCHEDULER.set_state("parking")
else:
return
STEP += 1
rospy.logdebug("[PROC] STEP changed to {}".format(STEP))
def process_parking():
""" process parking state """
global STEP
# if TURTLE.is_occupied():
# return
if STEP == 10:
frontleft = get_object_distance("frontleft")
frontright = get_object_distance("frontright")
if SCHEDULER.debug_option["show_parking_lidar"]:
rospy.logdebug("front: {:.2f} frontleft: {:.2f}".format(
get_object_distance("front"), get_object_distance("frontleft")
))
if (frontleft > 0) and (frontleft < 0.5):
STEP = 11
rospy.logdebug("[PROC] STEP changed to {}".format(STEP))
elif (frontright > 0) and (frontright < 0.5):
STEP = 12
rospy.logdebug("[PROC] STEP changed to {}".format(STEP))
# NOTE: return is needed to prevent executing STEP += 1
return
elif STEP == 11:
SCHEDULER.disable_cams()
TURTLE.set_speed("normal")
TURTLE.go_turn("right", 2)
STEP = 13
rospy.logdebug("[PROC] STEP changed to {}".format(STEP))
return
elif STEP == 12:
SCHEDULER.disable_cams()
TURTLE.set_speed("normal")
TURTLE.go_turn("left", 2)
STEP = 13
rospy.logdebug("[PROC] STEP changed to {}".format(STEP))
return
elif STEP == 13:
TURTLE.go_forward(0.7)
# for stopping for a while
rospy.sleep(rospy.Duration(0.5))
elif STEP == 14:
TURTLE.go_backward(1.0)
elif STEP == 15:
TURTLE.go_turn_backward(1.1)
elif STEP == 16:
TURTLE.set_speed("normal")
TURTLE.go_forward(1)
elif STEP == 17:
# TURTLE.set_speed("fast")
# TURTLE.set_speed_smooth("normal")
TURTLE.set_speed("normal")
SCHEDULER.set_state("zigzag")
else:
return
STEP += 1
def process_subcam(image):
""" process the subcam image """
start = timeit.default_timer()
global LINE_BASE
global TRACKING
global TURNING_TO
global TEST_ANGULAR
if TRACKING is not "fish":
return
if not SCHEDULER.is_subcam_occupied():
# if True:
rospy.Timer(rospy.Duration(0.04),
SCHEDULER.release_subcam_occupied,
oneshot=True)
info = EYE.see_sub(image)
# rospy.Timer(rospy.Duration(0.04), SCHEDULER.release_subcam_occupied, oneshot=True, reset=True)
if info is None:
print("NO INFO!")
# TURTLE.set_angular_smooth(0.12)
# pass
else:
# TURTLE.set_speed('slow')
# if info["slope"]:
# TURTLE.set_speed_by_percentage(-abs(info["slope"] / 6))
# else:
TURTLE.set_speed('normal')
center_x, center_y, point_x, point_y = (
value for value in info["line_center"])
gap_x = abs(point_x - center_x)
rospy.loginfo("\n[PROC] info: \n" + str(info))
rospy.loginfo("\nGAP: " + str(gap_x))
if info["slope"] > 0:
slope = math.sqrt(0.5 * info["slope"])
else:
slope = -math.sqrt(-0.5 * info["slope"])
if not info["has_line"]:
if gap_x > 100:
if TEST_ONCE:
TURTLE.set_angular(-1.75)
TURTLE.set_angular_smooth(-0.1)
else:
TURTLE.set_angular_smooth(-0.1)
elif gap_x < 0:
if TEST_ONCE:
TURTLE.set_angular(1.75)
TURTLE.set_angular_smooth(0.2)
else:
TURTLE.set_angular_smooth(0.2)
TEST_ONCE = False
# elif gap_x > 100:
# TURTLE.set_angular(-0.15 * info["slope"] * 4)
# TURTLE.set_angular_smooth(-0.1)
else:
TEST_ONCE = True
if gap_x > 100:
TURTLE.set_angular(-0.15 + slope)
TURTLE.set_angular_smooth(-0.1)
elif gap_x > 30:
TURTLE.set_angular(0)
else:
# if slope < 0:
# TURTLE.set_angular(-0.25 + slope * 1.5)
# TURTLE.set_angular_smooth(-0.1)
# else:
rospy.loginfo("LOST")
TURTLE.set_angular(0.45 + slope)
TURTLE.set_angular_smooth(0.1)
# SCHEDULER.release_subcam_occupied()
end = timeit.default_timer()
# print("sub l: {:d}".format(info["left"]) + " s: {:.01f}".format(slope)
# + " time: {:.02f}".format(end - start))
print("\nTIME: {:.02f}".format(end - start))
#!/usr/bin/env python3
import rospy
# from std_msgs.msg import String
from sensor_msgs.msg import CompressedImage, LaserScan
from geometry_msgs.msg import Twist
import viewer
import processor
from turtlebot import TURTLE
from scheduler import SCHEDULER # ! not used in contest
from constants import PATH_RASPICAM, PATH_USBCAM, PATH_LIDAR, PATH_GALAPAGOS_STATE
TURTLE.disable()
SCHEDULER.enable_lidar()
SCHEDULER.set_state("default")
# SCHEDULER.set_state("to_intersection")
# SCHEDULER.set_state("traffic_light")
rospy.Subscriber(PATH_USBCAM,
CompressedImage,
processor.process_subcam,
queue_size=1)
rospy.Subscriber(PATH_RASPICAM,
CompressedImage,
processor.process_frontcam,
queue_size=1)
# rospy.Subscriber(PATH_LIDAR, LaserScan,
# processor.process_lidar, queue_size=1)
# rospy.Subscriber('/cmd_vel', Twist, viewer.view_speed, queue_size=1)
def process_eye(image):
""" process the Eye System """
global TRACKING
global TURNING_TO
# if TRACKING is not "front":
# return
# rospy.loginfo('\n[PROC] frontcam image received')
# rospy.loginfo(EYE.is_occupied())
if not SCHEDULER.is_frontcam_occupied():
info = EYE.see(image)
# rospy.loginfo("\n[PROC] info: " + str(info))
rospy.Timer(rospy.Duration(0.1), EYE.release_occupied, oneshot=True)
if TRACKING is not "front":
return
if info is None:
pass
else:
center = info["center"]
TURTLE.set_speed('normal')
if info["state"] is "straight":
# TURTLE.set_speed('normal')
pass
elif info["state"] is "lost_track":
rospy.loginfo("\n[PROC] state: lost_track")
TURNING_TO = info["turning_to"]
TURTLE.go_forward(1)
rospy.Timer(rospy.Duration(1),
track_fish,
oneshot=True,
reset=True)
# elif info["state"] is "turning":
# TURNING_TO = info["turning_to"]
# track_fish()
# rospy.Timer(rospy.Duration(5.5), track_fish, oneshot=True, reset=True)
if center > 500:
TURTLE.set_angular(0.27)
rospy.loginfo("\n[PROC] [!] turning to right at " +
str(center))
return
if center < -500:
TURTLE.set_angular(-0.27)
rospy.loginfo("\n[PROC] [!] turning to left at " + str(center))
return
if center > 60:
TURTLE.set_angular(0.12)
# TURTLE.set_angular(0.08)
rospy.loginfo("\n[PROC] turning to right at " + str(center))
return
elif center < -60:
TURTLE.set_angular(-0.12)
# TURTLE.set_angular(-0.08)
rospy.loginfo("\n[PROC] turning to left at " + str(center))
return
TURTLE.set_angular_smooth(0)
return
if __name__ == '__main__':
try:
RUN_TYPE = 'run_' + sys.argv[1]
# NOTE: publisher to stop viewers
PUBLISHER_PACKAGE = rospy.Publisher(PATH_GALAPAGOS_STATE, String, queue_size=5)
MSG = String()
MSG.data = RUN_TYPE
PUBLISHER_PACKAGE.publish(MSG)
rospy.loginfo("executing: %s" % RUN_TYPE)
__import__('%s' % (RUN_TYPE), fromlist=[RUN_TYPE])
rospy.loginfo("Galapagos package started.")
while not rospy.is_shutdown():
TURTLE.move()
rospy.Rate(5).sleep()
#rospy.spin() # ! Deprecated
# NOTE: The codes below will never be worked
# except rospy.ROSInterruptException:
# rospy.loginfo('exception occurred')
# rospy.signal_shutdown("bye")
# pass
except KeyboardInterrupt:
print("\r", end='')
TURTLE.disable()
MSG.data = "view"
print(str(PUBLISHER_PACKAGE.publish(MSG)))
def process_frontcam(image):
global ENABLE_FRONT
if ENABLE_FRONT is False:
return
""" process the image of raspicam """
global CURRENT_STATE
raw_data = np.fromstring(image.data, np.uint8)
cv_img = cv2.imdecode(raw_data, cv2.IMREAD_COLOR)
# NOTE: ROI Setting
blob_ROI = cv_img[100:, :]
if CURRENT_STATE == 'traffic_light':
if is_light_green(cv_img):
TURTLE.enable()
#TURTLE.set_speed('fast')
print("detected green")
CURRENT_STATE = 'left_or_right'
print_state()
#TURTLE.set_weight(0.8)
return
else:
print("no green")
""""
TURTLE.enable()
#TURTLE.set_speed('fast')
print("detected green")
CURRENT_STATE = 'left_or_right'
print_state()
#TURTLE.set_weight(0.8)
"""
return
'''
if CURRENT_STATE == 'intersection':
cv2.imshow("blob_ROI",blob_ROI)
# cv2.waitKey(1)
print("intersection state")
if is_intersection(cv_img):
TURTLE.set_weight(0.8)
CURRENT_STATE = 'left_or_right'
print_state()
return
else:
return
'''
if CURRENT_STATE == 'left_or_right':
print("left or right state")
cv2.imshow("blob_ROI", blob_ROI)
# cv2.waitKey(1)
tmp_state = check_left_right_sign(blob_ROI)
print("tmp state: ", tmp_state)
if tmp_state == 'right':
#print("11tmp state: ",tmp_state, ", ri ght cnt: ",inter_right_cnt)
TURTLE.LINE_BASE = 2
#print("11tmp state: ",tmp_state, ", right cnt: ",inter_right_cnt)
CURRENT_STATE = 'inter_curving'
print_state()
TURTLE.set_weight(1.0)
elif tmp_state == 'left':
#print("11tmp state: ",tmp_state, ", left cnt: ",inter_left_cnt)
TURTLE.LINE_BASE = 1
#print("22tmp state: ",tmp_state, ", left cnt: ",inter_left_cnt)
CURRENT_STATE = 'inter_curving'
print_state()
TURTLE.set_weight(1.0)
elif tmp_state == 'none':
return
if CURRENT_STATE == 'inter_curving':
global COUNT_STRAIGHT
global ENABLE_LIDAR
if abs(TURTLE.weight * TURTLE._angular) < 0.1:
COUNT_STRAIGHT += 1
print("straight counting : ", COUNT_STRAIGHT, " is counted")
if COUNT_STRAIGHT > 25:
COUNT_STRAIGHT = 0
TURTLE.LINE_BASE = 2
ENABLE_LIDAR = True
CURRENT_STATE = "construction"
print_state()
return
else:
return
else:
COUNT_STRAIGHT = 0
return
if CURRENT_STATE is "before_parking":
TURTLE.LINE_BASE = 1
rospy.Timer(rospy.Duration(10), set_state_to_parking, oneshot=True)
return
def process_subcam(image):
""" process the subcam image """
if not SCHEDULER.is_subcam_enable():
return
if EYE.is_sub_occupied() or TURTLE.is_occupied():
return
if SCHEDULER.debug_option["show_timer"]:
SCHEDULER.check_time("subcam", min=0.25)
info = EYE.see_sub(image)
if info is None:
# rospy.logwarn("[PROC] No Information!")
return
center = info["center"]
slope = info["slope"]
if slope < -0.5:
# limit = 1.6
limit = 1.5 # For slow speed
amplitude = 1.6
# elif slope > 0.5:
# limit = 1.6
# amplitude = 1.2
else:
# limit = 1.2
# amplitude = 1.2
limit = 1.2 # For slow speed
amplitude = 1.2 # For slow speed
if EYE.get_front_state() == "straight":
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
elif center > 0:
degree = - pow(abs(center) / 100, 2.0) * amplitude
else:
degree = 0
else:
if (abs(center) < 30) and slope < -0.4:
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 < -55:
# degree = 1.6
degree = 1.4 # For slow speed
elif center > 50:
degree = -1.2
# degree = -1.2 # For slow speed
elif center > 10: # editing...
degree = -0.9 # editing...
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), EYE.release_sub_occupied, oneshot=True
)
TURTLE.turn(0.13, degree)
