def run(self):
self.init_handeye()
self.handeye_cali()
simple_campose = self.camera_pose_simple()
simple_campose = self.select_pose_by_view(simple_campose)
robot_pose_list = []
while (len(self.image) < self.picture_number):
cam_list = self.score_main(simple_campose)
for pose in cam_list:
robot_pose = self.get_Expect_robot_pose(pose)
if not self.robot.moveable(robot_pose):
continue
flag = self.robot.move(robot_pose)
if not flag:
continue
rgb_image = self.camera.get_rgb_image()
flag, objpoint, imgpoint = self.board.getObjImgPointList(
rgb_image)
if not flag:
continue
robot_pose_list.append(robot_pose)
camerapose = self.board.extrinsic(imgpoint, objpoint,
self.camera.intrinsic,
self.camera.dist)
self.objpoint_list.append(objpoint)
self.imgpoint_list.append(imgpoint)
self.Hend2base.append(robot_pose)
self.Hobj2camera.append(camerapose)
self.image.append(rgb_image)
break
self.handeye_cali()
utils.json_save(robot_pose_list, "../config/init_robot_pose.json")
time.sleep(2)
# print("lock")
self.lock=1
flag = self.robot.move(pose)
if not flag:
self.lock=0
return False,None
pic = camera.get_rgb_image()
self.lock = 0
return True,pic
if __name__ == '__main__':
board = apriltagboard.AprilTagBoard("../config/apriltag.yml", "../config/tagId.csv")
cliend = vrep_connect.getVrep_connect(19999)
camera = Kinect_test.Camera(cliend,"../config/intrinsic_gt.yml")
robot = LBR4p.Robot(cliend)
robot.set_simu(0)
move_lock = lock(robot, camera)
auto_handeye = auto_handeye_calibration_thread_numba.auto_handeye_calibration(board, robot, camera,
"../config/auto_set_to.yml", move_lock)
timestamp = time.time()
timestruct = time.localtime(timestamp)
time_str = time.strftime('%m_%d_%H_%M', timestruct)
auto_handeye.set_select_method(5)
auto_handeye.init_handeye()
auto_handeye.handeye_cali()
auto_handeye.run()
utils.json_save(random.sample(auto_handeye.Hend2base,10), "../config/init_robot_pose_handtoeye.json")
#auto_handeye.save_result("../result/no_local_{}.json".format(time_str))
def save_result(self,file):
from auto import utils
utils.json_save(self.result,file)