diff --git a/scripts/landmark_localization.py b/scripts/landmark_localization.py new file mode 100644 index 0000000..d52f44f --- /dev/null +++ b/scripts/landmark_localization.py @@ -0,0 +1,115 @@ +import argparse +import os +import sys +import time +from pathlib import Path + +import bosdyn.client +import bosdyn.client.estop +import bosdyn.client.lease +import bosdyn.client.util +import cv2 +import numpy as np +from bosdyn.api import geometry_pb2, image_pb2, manipulation_api_pb2 +from bosdyn.client.image import ImageClient +from bosdyn.client.manipulation_api_client import ManipulationApiClient +from bosdyn.client.robot_command import (RobotCommandBuilder, + RobotCommandClient, + block_until_arm_arrives, + blocking_stand) +from bosdyn.client.robot_state import RobotStateClient +from google.protobuf import wrappers_pb2 +from PIL import Image + +from conq.cameras_utils import DEPTH_SOURCES, RGB_SOURCES, get_color_img +from conq.clients import Clients +from conq.hand_motion import hand_pose_cmd +from conq.manipulation import blocking_arm_command + + +def stand_at_place(config): + bosdyn.client.util.setup_logging(config.verbose) + + sdk = bosdyn.client.create_standard_sdk('StandAtPlace') + robot = sdk.create_robot(config.hostname) + bosdyn.client.util.authenticate(robot) + robot.time_sync.wait_for_sync() + + assert robot.has_arm(), 'Robot requires an arm to run this example.' + assert not robot.is_estopped(), 'Robot is estopped. Please use an external E-Stop client, ' \ + 'such as the estop SDK example, to configure E-Stop.' + + lease_client = robot.ensure_client(bosdyn.client.lease.LeaseClient.default_service_name) + image_client = robot.ensure_client(ImageClient.default_service_name) + manipulation_api_client = robot.ensure_client(ManipulationApiClient.default_service_name) + + with bosdyn.client.lease.LeaseKeepAlive(lease_client, must_acquire=True, return_at_exit=True): + robot.logger.info('Powering on robot... This may take a several seconds.') + robot.power_on(timeout_sec=20) + assert robot.is_powered_on(), 'Robot power on failed.' + + robot.logger.info('Robot powered on.') + robot.logger.info('Commanding robot to stand...') + + command_client = robot.ensure_client(RobotCommandClient.default_service_name) + blocking_stand(command_client, timeout_sec=10) + robot.logger.info('Robot standing.') + + camera_sources = [ + 'back_fisheye_image', + 'frontleft_fisheye_image', + 'frontright_fisheye_image', + 'hand_color_image', + 'left_fisheye_image', + 'right_fisheye_image' + ] + + output_folder = "/Users/saketpradhan/Desktop/ARMLAB/conq_python/scripts/" + + if not os.path.exists(output_folder): + print('Error: output folder does not exist: ' + output_folder) + return + + for camera_source in camera_sources: + counter = 0 + image_responses = image_client.get_image_from_sources([camera_source]) + rgb_np, _ = get_color_img(image_client, camera_source) + + while True: + image_saved_path = os.path.join(output_folder, image_responses[0].source.name + '_{:0>4d}'.format(counter) + '.jpg') + counter += 1 + + if not os.path.exists(image_saved_path): break + + cv2.imwrite(image_saved_path, rgb_np) + print('Wrote: ' + image_saved_path) + time.sleep(0.7) + + robot.logger.info('Finished.') + robot.logger.info('Sitting down and turning off.') + + +def main(argv): + parser = argparse.ArgumentParser() + bosdyn.client.util.add_base_arguments(parser) + parser.add_argument('--folder', help='Path to write images to', default='') + options = parser.parse_args(argv) + + sdk = bosdyn.client.create_standard_sdk('image_capture') + robot = sdk.create_robot(options.hostname) + bosdyn.client.util.authenticate(robot) + robot.sync_with_directory() + robot.time_sync.wait_for_sync() + + try: + stand_at_place(options) + return True + except Exception as exc: # pylint: disable=broad-except + logger = bosdyn.client.util.get_logger() + logger.exception('Threw an exception') + return False + + +if __name__ == "__main__": + if not main(sys.argv[1:]): + sys.exit(1) \ No newline at end of file