def click_callback(event, x, y, flags, param):
global ix, iy
if event == cv2.EVENT_LBUTTONDOWN:
ix, iy = x, y
print("point1:=", x, y)
print(cameraInfo)
print(mat)
x_w, y_w, z_w = preparation.PointOnTable(ix, iy, cameraInfo, mat, 0.75)
print(x_w, y_w)
if not execution:
clopen(0)
moveThread = threading.Thread(target=move_to, args=(x_w, y_w, 1.1))
moveThread.start()
else:
print("Cannot plan while moving")
def execution():
global action, result, executing
while True:
if action != None:
executing = True
for i in range(10):
print("start executing")
ix, iy, dir = action
objHeight = 0.78
for i in range(-10, 10):
for j in range(-10, 10):
if iy + i >= 0 and ix + j >= 0 and iy + i < 480 and ix + j < 640:
print(iy + i, ix + j)
objHeight = max(objHeight, height_map[iy + i, ix + j])
x_w, y_w, z_w = preparation.PointOnTable(ix, iy, cameraInfo, mat,
0.73)
flag = 0
for model in models:
pose = geometry_msgs.msg.PoseStamped()
pose.header.frame_id = "world"
pose.pose = states_backup[model].pose
fname = filenames[model]
print(fname)
scene.add_mesh(model, pose, fname)
print("scene add mesh")
# if not move_to(x_w, y_w, 1.2, dir):
# reset()
# result = -1
# for i in range(100): print(result)
# action = None
# executing = False
# for model in models:
# scene.remove_world_object(model)
# continue
height_plus = 0.05
while not move_to(x_w, y_w, objHeight + height_plus, dir, 0.1):
height_plus += 0.03
if objHeight + height_plus > 1.2:
result = flag
for i in range(1):
print(result)
action = None
executing = False
for model in models:
scene.remove_world_object(model)
break
if objHeight + height_plus > 1.2:
continue
for model in models:
scene.remove_world_object(model)
time.sleep(1)
clopen(1)
for i in range(1):
print("closed")
# if not move_to(x_w, y_w, 1.2, dir):
# reset()
# result = flag
# for i in range(100): print(result)
# action = None
# executing = False
# continue
x_w, y_w, z_w = preparation.PointOnTable(600, 230, cameraInfo, mat,
0.73)
move_to(x_w, y_w, 1.3, 0, 1)
for model in models:
state = getModelState(model, "world")
if state.pose.position.z > 0.8 and state.pose.position.x > 0.4:
flag = 1
reset()
result = flag
for i in range(1):
print(result)
action = None
executing = False
def execution():
global action, result, executing
while True:
if action != None:
executing = True
for i in range(10):
print("start executing")
ix, iy, dir = action
objHeight = 0.78
for i in range(-5, 6):
for j in range(-5, 6):
try:
objHeight = max(objHeight, height_map[iy + i, ix + j])
except:
pass
x_w, y_w, z_w = preparation.PointOnTable(ix, iy, cameraInfo, mat,
0.73)
flag = 0
if not move_to(x_w, y_w, 1.2, dir):
reset()
result = -1
for i in range(100):
print(result)
action = None
executing = False
continue
if not move_to(x_w, y_w, objHeight + 0.15, dir):
reset()
result = flag
for i in range(100):
print(result)
action = None
executing = False
continue
clopen(1)
for i in range(10):
print("closed")
if not move_to(x_w, y_w, 1.2, dir):
reset()
result = flag
for i in range(100):
print(result)
action = None
executing = False
continue
x_w, y_w, z_w = preparation.PointOnTable(600, 230, cameraInfo, mat,
0.73)
move_to(x_w, y_w, 1.3, 0)
for model in models:
state = getModelState(model, "world")
if state.pose.position.z > 0.8 and state.pose.position.x > 0.4:
flag = 1
reset()
result = flag
for i in range(100):
print(result)
action = None
executing = False
cv2.setMouseCallback('x', click_callback)
while True:
if not image is None:
cv2.imshow("x", image)
# print("..")
key = cv2.waitKey(10)
if key == 27: # wait for ESC
subprocess.call("rosnode kill -a", shell=True)
preparation.clear()
exit()
elif key == ord('a'):
subprocess.call("python3 addObj.py", shell=True)
elif key == ord('s'):
currentHeight -= 0.02
x_w, y_w, z_w = preparation.PointOnTable(ix, iy, cameraInfo, mat, 0.75)
print(x_w, y_w)
if not execution:
clopen(0)
moveThread = threading.Thread(target=move_to,
args=(x_w, y_w, currentHeight))
moveThread.start()
else:
print("Cannot plan while moving")
elif key == ord('w'):
currentHeight += 0.02
x_w, y_w, z_w = preparation.PointOnTable(ix, iy, cameraInfo, mat, 0.75)
print(x_w, y_w)
if not execution:
clopen(0)
moveThread = threading.Thread(target=move_to,