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)