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_lidar(lidar_data):
global CURRENT_STATE
if CURRENT_STATE is not "construction":
return
""" process the lidar data """
# global IS_IN_TUNNEL
# global DISTANCE_FRONT
# global HAS_OBJECT_IN_20
global IS_TURNING
# global SEEN_PARKING_SIGN
# global SEEN_LEFT_SIGN
global STATE_CONSTRUCTION
global DIRECTION
global TURN_COUNT
if TURN_COUNT == 3:
CURRENT_STATE = "parking"
rospy.signal_shutdown("\n[PROC] Shutting down...")
set_lidar_values(lidar_data)
if not TURTLE.is_settable():
return
rospy.loginfo(
str(STATE_CONSTRUCTION) + " " + str(get_object_distance('left')) +
" " + str(get_object_distance('rightside')))
if STATE_CONSTRUCTION == "searching":
distance = get_object_distance("left")
if distance == 0:
return
elif distance < 0.35:
STATE_CONSTRUCTION = "ready"
elif distance > 0.5 and distance < 0.9:
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 < 1:
STATE_CONSTRUCTION = "fitting"
DIRECTION = "left"
TURTLE.enable_fish = False
# TURTLE.set_angular(0)
TURTLE.turn("right", 0.5, True)
return
elif STATE_CONSTRUCTION == "fitting":
TURTLE.turn("right", 0.5, True)
STATE_CONSTRUCTION = "start"
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
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()
TURTLE.turn(DIRECTION, 1.2)
TURN_COUNT = TURN_COUNT + 1
IS_TURNING = False