def main():
turtle = Turtlebot(pc=True, depth=True)
K = turtle.get_depth_K()
#Init map
init_map()
while not turtle.is_shutting_down():
# Process depth image to pointcloud
depth = turtle.get_depth_image()
if depth is None:
print("Depth image is None")
continue
# Pointcloud
pcl = image2cloud(depth, K, True)
fit_pointcloud(pcl)
move = True
turtle.reset_odometry()
move.rotate(turtle, 3.14, verbose=True)
move.translate(turtle, 2, verbose=True)
print("Back in main")
break
def orientation(bearing):
curr_b = location.bearing()
diff = angle_diff(bearing, curr_b)
while not rotate_arrived(bearing):
move.rotate(rotate_speed(bearing))
move.drive(0)
ang += 90
halfway = 320 # Basically the width by half
error = int(x_min - halfway)
ang = int(ang)
box = cv2.boxPoints(red_box)
box = np.int0(box)
cv2.drawContours(crop_img, [box], 0, (0,255,0), 3)
cv2.putText(crop_img,str(ang), (10,40), cv2.FONT_HERSHEY_SIMPLEX,1, (0,0,255), 2)
cv2.putText(crop_img,str(error), (10,320), cv2.FONT_HERSHEY_SIMPLEX,1, (255,0,0), 2)
cv2.line(crop_img, (int(x_min), int(y_min)), (halfway, int(y_min)), (255,0,0), 3)
Rad_Angle = math.radians(ang)
<<<<<<< HEAD
error_angle = math.radians(error/4)
move.rotate(dxl_io,1000, dt*Rad_Angle)
Correction(dxl_io, dt*error_angle)
time.sleep(dt)
=======
err = math.radians(error/4)
rotate(dxl_io, 300,dt*err + dt*Rad_Angle)
time.sleep(dt)
>>>>>>> ce5c36cad41c5be65dfc3544a01fe047bc8741a7
print(error, ang)
else:
print("I don't see a line")
Look_for_line(dxl_io)
time.sleep(dt)
#Display the resulting frame
#cv2.imshow('frame',crop_img)
if cv2.waitKey(20) & 0xFF == ord('k'):
def carre(motors, t):
mod = np.fmod(t, 2)
if mod < 1:
return move.rotate(motors, 0.1, 0)
else:
return move.rotate(motors, 0, np.pi / 8)
if sum(means) > 5:
if K_index == 2:
sign = 1 if max_index == 0 else -1
linear_speed = speed[K_index]
angular_speed = sign * K[K_index]
print("We're in borders\t", linear_speed, angular_speed)
elif K_index == 1:
err = (2 - max_index) * (means[max_index] - means[2])
linear_speed = speed[K_index]
angular_speed = K[K_index] * err
print("We're in bands\t", linear_speed, angular_speed)
else:
err = means[1] - means[3]
linear_speed = speed[K_index]
angular_speed = K[K_index] * err
print("We're in centre\t", linear_speed, angular_speed)
move.rotate(dxl_io, linear_speed, angular_speed)
if mean_green > 70 and t > dt * 10:
color += 1
move.rotate(dxl_io, 0.2, 0)
t += dt
# Coupe le moteur
move.rotate(dxl_io, 0, 0)
#time.sleep(dt)
#t += dt
def move_asynch(chosen_path, state):
global DUMPED, NEXT_NODE
instruction = None
# Calling stop on this thread throws ThreadKiller at the current point of
# execution, wrapping all of the logic in a try catch allows for cleanup and
# prevents the stack trace.
try:
while True:
# Get the next instruction
instruction = chosen_path.pop(0)
success = True
# Dispatch to the relavent functions depending on the command
if isinstance(instruction, Move):
#print("moving")
success = forward(instruction.dist, tolerance = instruction.tolerance)
elif isinstance(instruction, Dump):
# Communication between the two bricks was turned off during
# profiling
if not PROFILING:
# Instruct the second brick to dump
CLIENT.publish("dump", json.dumps(instruction.slots))
# Spin until it replies
while True:
with dumped_lock:
if DUMPED:
DUMPED = False
break
elif isinstance(instruction, Rotate):
#print("rotating")
# The route planner always generates instructions telling the
# robot to turn right, this adapts to turn left when that is
# more efficient
if instruction.angle <= 180:
direction = Directions.ROT_RIGHT
angle = instruction.angle
else:
direction = Directions.ROT_LEFT
angle = instruction.angle - 180
success = rotate(angle, tolerance = instruction.tolerance, direction = direction)
elif isinstance(instruction, ToDesk):
#print("approaching desk")
angle = instruction.angle
if instruction.is_left:
direction = Directions.ROT_LEFT
else:
direction = Directions.ROT_RIGHT
# This function unconditionally turns 90 degrees and so can't
# fail
approach(angle=angle, direction=direction)
elif isinstance(instruction, FromDesk):
#print("leaving desk")
angle = instruction.angle
if instruction.is_left:
direction = Directions.ROT_LEFT
else:
direction = Directions.ROT_RIGHT
# On the way back however it searches for a line so can
success = approach(angle=angle, tolerance=instruction.tolerance, direction=direction, reverse=True)
elif isinstance(instruction, Report):
#print("reporting")
CLIENT.publish("location_info", payload=instruction.where)
# If an instruction failed panic
if not success:
#print("panicking")
STATE_QUEUE.put(T_PANICKING)
break
# If we ran out of instructions without panicking make a transition
# depending on what state we are currently in
if len(chosen_path) == 0:
if state == State.DELIVERING:
#print("Returning")
STATE_QUEUE.put(T_RETURNING)
#print(STATE_QUEUE)
break
elif state == State.RETURNING:
#print("Loading")
STATE_QUEUE.put(T_LOADING)
break
# Last reported location for return
with next_node_lock:
if isinstance(instruction, Report):
NEXT_NODE = instruction.where
#print(NEXT_NODE)
# TODO right now the code spins here forever after executing the movement
# commands - does not need to
while True:
pass
except ThreadKiller as e:
# There is a certain amount of unreliability in the method used to kill
# threads (See thread_decorator.py) so the code keeps sending exceptions
# until the thread dies or calls acknowledge. After a call to
# acklowledge the thread is allowed as much time as it needs to cleanup.
acknowledge(e)
stop_motors()
final = []
# Resolve the various instructions where we could have been interupted
if isinstance(instruction, Move):
# Inside move figure out how far we still need to go and store that
# for next time. (If it is too small search for the junction
# anywhere between 0 and 20)
dist = instruction.dist - get_odometry()
if dist <= 20:
final = [Move(20, 100)]
else:
final = [Move(dist, 50)]
# If there is a rotate next also take that
if chosen_path and isinstance(chosen_path[0], Rotate):
final.append(chosen_path.pop(0))
elif isinstance(instruction, Dump):
# If it's a dump assume we've managed to send the message (It's
# highly unlikley we will be interupted exactly between poping a
# Dump and sending the message). Wait for the confirmation before
# finishing
while True:
with dumped_lock:
if DUMPED:
DUMPED = False
break
elif isinstance(instruction, Rotate):
# Rotate follows the same theory as move while also dealing with
# whether we were turning left or right
if instruction.angle <= 180:
angle = instruction.angle - get_odometry(rotating=True)
if angle <= 20:
final = [Rotate(20, 100)]
else:
final = [Rotate(angle, 50)]
else:
angle = instruction.angle + get_odometry(rotating=True)
if angle >= 340:
final = [Rotate(340, 100)]
else:
final = [Rotate(angle, 50)]
elif isinstance(instruction, FromDesk):
# Same as move and rotate
final = [FromDesk(instruction.is_left, instruction.angle - get_odometry(rotating=True), 50)]
elif isinstance(instruction, ToDesk):
# Here also dispense the letter given we are at a desk
final = [ToDesk(instruction.is_left, instruction.angle - get_odometry(rotating=True)),
chosen_path.pop(0), chosen_path.pop(0)] # atm it dispenses the letter even after recall
# Save the generated command
with final_cmd_lock:
global FINAL_CMD
FINAL_CMD = final
# Save the current path (Will be used for resume)
with chosen_path_lock:
global CHOSEN_PATH
CHOSEN_PATH = chosen_path
# Store the node we are going to reach next (Will be used for callback)
with next_node_lock:
for instructione in chosen_path:
if isinstance(instructione, Report):
NEXT_NODE = instructione.where
break
sys.exit()
means = []
max = 0
max_index = 0
for i in range(len(lines)-1):
bands.append(red_line[0:180, lines[i]:lines[i+1]])
means.append(np.mean(bands[i]))
if max < means[i]:
max = means[i]
max_index = i
K = [0.001, 0.01, 0.001]
speed = [0.2, 0.15, 0.05]
K_index = np.abs(max_index-2)
if sum(means) < 5:
move.rotate(dxl_io,-0.05,0)
elif K_index == 2:
move.rotate(dxl_io,speed[K_index],K[K_index]*means[max_index])
print("we're in borders")
else:
err = means[max_index-1] - means[max_index+1]
move.rotate(dxl_io,speed[K_index],K[K_index]*err)
print("Land ahoy!")
#print(err)
#elif left_mean > 5*center_mean:
# move.rotate(dxl_io, 0.1, err)
# print("left")
# elif right_mean > 5*center_mean:
# move.rotate(dxl_io, 0.1, err)
# print("right")