From bbd03ca197db7a7bebf82e4562c06d99a34ca928 Mon Sep 17 00:00:00 2001 From: Saket Pradhan Date: Thu, 6 Jun 2024 20:12:58 -0400 Subject: [PATCH 1/3] landmark localization --- scripts/landmark_localization.py | 78 ++++++++++++++++++++++++++++++++ 1 file changed, 78 insertions(+) create mode 100644 scripts/landmark_localization.py diff --git a/scripts/landmark_localization.py b/scripts/landmark_localization.py new file mode 100644 index 0000000..41ba8c0 --- /dev/null +++ b/scripts/landmark_localization.py @@ -0,0 +1,78 @@ +# Copyright (c) 2023 Boston Dynamics, Inc. All rights reserved. +# +# Downloading, reproducing, distributing or otherwise using the SDK Software +# is subject to the terms and conditions of the Boston Dynamics Software +# Development Kit License (20191101-BDSDK-SL). + +import argparse +import sys +import os +import bosdyn.client +import bosdyn.client.util +from bosdyn.client.image import ImageClient +import cv2 +import numpy as np +import time + +def main(argv): + parser = argparse.ArgumentParser() + bosdyn.client.util.add_base_arguments(parser) + parser.add_argument('--image-source', help='Get image from source(s)', default='frontleft_fisheye_image') + parser.add_argument('--folder', help='Path to write images to', default='') + options = parser.parse_args(argv) + + # Create robot object with an image client. + 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() + + image_client = robot.ensure_client(ImageClient.default_service_name) + + # Make sure the folder exists. + if not os.path.exists(options.folder): + print('Error: output folder does not exist: ' + options.folder) + return + + counter = 0 + + while True: + # We want to capture from one camera at a time. + + # Capture and save images to disk + image_responses = image_client.get_image_from_sources([options.image_source]) + + dtype = np.uint8 + + img = np.frombuffer(image_responses[0].shot.image.data, dtype=dtype) + img = cv2.imdecode(img, -1) + + # Approximately rotate the image to level. + if image_responses[0].source.name[0:5] == "front": + img = cv2.rotate(img, cv2.ROTATE_90_CLOCKWISE) + + elif image_responses[0].source.name[0:5] == "right": + img = cv2.rotate(img, cv2.ROTATE_180) + + # Don't overwrite an existing image + while True: + image_saved_path = os.path.join(options.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, img) + + print('Wrote: ' + image_saved_path) + + # Wait for some time so we can drive the robot to a new position. + time.sleep(0.7) + + + return True + +if __name__ == "__main__": + if not main(sys.argv[1:]): + sys.exit(1) \ No newline at end of file From f8d075e8a116d17deb819699eb183f77ceed35bf Mon Sep 17 00:00:00 2001 From: Saket Pradhan Date: Thu, 6 Jun 2024 23:15:01 -0400 Subject: [PATCH 2/3] adds content to landmark-localization --- scripts/landmark_localization.py | 78 -------------------------------- 1 file changed, 78 deletions(-) delete mode 100644 scripts/landmark_localization.py diff --git a/scripts/landmark_localization.py b/scripts/landmark_localization.py deleted file mode 100644 index 41ba8c0..0000000 --- a/scripts/landmark_localization.py +++ /dev/null @@ -1,78 +0,0 @@ -# Copyright (c) 2023 Boston Dynamics, Inc. All rights reserved. -# -# Downloading, reproducing, distributing or otherwise using the SDK Software -# is subject to the terms and conditions of the Boston Dynamics Software -# Development Kit License (20191101-BDSDK-SL). - -import argparse -import sys -import os -import bosdyn.client -import bosdyn.client.util -from bosdyn.client.image import ImageClient -import cv2 -import numpy as np -import time - -def main(argv): - parser = argparse.ArgumentParser() - bosdyn.client.util.add_base_arguments(parser) - parser.add_argument('--image-source', help='Get image from source(s)', default='frontleft_fisheye_image') - parser.add_argument('--folder', help='Path to write images to', default='') - options = parser.parse_args(argv) - - # Create robot object with an image client. - 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() - - image_client = robot.ensure_client(ImageClient.default_service_name) - - # Make sure the folder exists. - if not os.path.exists(options.folder): - print('Error: output folder does not exist: ' + options.folder) - return - - counter = 0 - - while True: - # We want to capture from one camera at a time. - - # Capture and save images to disk - image_responses = image_client.get_image_from_sources([options.image_source]) - - dtype = np.uint8 - - img = np.frombuffer(image_responses[0].shot.image.data, dtype=dtype) - img = cv2.imdecode(img, -1) - - # Approximately rotate the image to level. - if image_responses[0].source.name[0:5] == "front": - img = cv2.rotate(img, cv2.ROTATE_90_CLOCKWISE) - - elif image_responses[0].source.name[0:5] == "right": - img = cv2.rotate(img, cv2.ROTATE_180) - - # Don't overwrite an existing image - while True: - image_saved_path = os.path.join(options.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, img) - - print('Wrote: ' + image_saved_path) - - # Wait for some time so we can drive the robot to a new position. - time.sleep(0.7) - - - return True - -if __name__ == "__main__": - if not main(sys.argv[1:]): - sys.exit(1) \ No newline at end of file From daa7b4bd0a606a458f28658bd0485ddd636e15a6 Mon Sep 17 00:00:00 2001 From: Saket Pradhan Date: Thu, 6 Jun 2024 23:24:34 -0400 Subject: [PATCH 3/3] adds content to landmark-localization --- scripts/landmark_localization.py | 115 +++++++++++++++++++++++++++++++ 1 file changed, 115 insertions(+) create mode 100644 scripts/landmark_localization.py 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