def test_images_respond_in_order(image_client, image_sources, verbose): """Check that each image source returns multiple images with in-order acquisition timestamps. Args: image_client (ImageClient): The client for the image service being tested. image_sources (List[string]): The image source names from the image service. verbose (boolean): Print additional logging information on failure. Returns: Boolean indicating if every image source responds to multiple image requests with images that have in-order acquisition timestamps. """ # Request an image source 3 times and ensure each acquisition timestamp is later. for image in image_sources: for formatt in ALL_FORMATS: responses = [] for i in range(0, 3): try: img_resp = image_client.get_image( [build_image_request(image, image_format=formatt)]) assert len(img_resp) == 1 responses.append(img_resp[0]) except Exception as err: break # Check that the timestamps are increasing. for i in range(len(responses) - 1): curr_img_time = responses[ i].shot.acquisition_time.seconds + responses[ i].shot.acquisition_time.nanos * 1e-9 next_img_time = responses[ i + 1].shot.acquisition_time.seconds + responses[ i + 1].shot.acquisition_time.nanos * 1e-9 if curr_img_time > next_img_time: _LOGGER.error( "Image source %s (requested as format %s) has out-of-order timestamps. Ensure only one image " "service is running and the timestamps are being set properly.", image, image_pb2.Image.Format.Name(formatt)) if verbose: _LOGGER.info( "First image responds with time: %s (%d seconds). The next image responds " "with time: %s (%s seconds).", responses[i].shot.acquisition_time, curr_img_time, responses[i + 1].shot.acquisition_time, next_img_time) return False # All images respond with consecutive (potentially new) images. return True
def image_to_bounding_box(self): """Determine which camera source has a fiducial. Return the bounding box of the first detected fiducial.""" #Iterate through all five camera sources to check for a fiducial for i in range(len(self._source_names) + 1): # Get the image from the source camera. if i == 0: if self._previous_source != None: # Prioritize the camera the fiducial was last detected in. source_name = self._previous_source else: continue elif self._source_names[i - 1] == self._previous_source: continue else: source_name = self._source_names[i - 1] img_req = build_image_request( source_name, quality_percent=100, image_format=image_pb2.Image.FORMAT_RAW) image_response = self._image_client.get_image([img_req]) self._camera_tform_body = get_a_tform_b( image_response[0].shot.transforms_snapshot, image_response[0].shot.frame_name_image_sensor, BODY_FRAME_NAME) self._body_tform_world = get_a_tform_b( image_response[0].shot.transforms_snapshot, BODY_FRAME_NAME, VISION_FRAME_NAME) # Camera intrinsics for the given source camera. self._intrinsics = image_response[0].source.pinhole.intrinsics width = image_response[0].shot.image.cols height = image_response[0].shot.image.rows # detect given fiducial in image and return the bounding box of it bboxes = self.detect_fiducial_in_image( image_response[0].shot.image, (width, height), source_name) if bboxes: print("Found bounding box for " + str(source_name)) return bboxes, source_name else: self._tag_not_located = True print("Failed to find bounding box for " + str(source_name)) return [], None
def image_to_bounding_box(self): """Determine which camera source has a fiducial. Return the bounding box of the first detected fiducial.""" #Iterate through all five camera sources to check for a fiducial for i in range(len(self._source_names) + 1): #Get the image from the source camera if i == 0: if self._previous_source != None: source_name = self._previous_source else: continue elif self._source_names[i - 1] == self._previous_source: continue else: source_name = self._source_names[i - 1] img_req = build_image_request(source_name, quality_percent=100, image_format=bosdyn_image.FORMAT_RAW) image_response = self._image_client.get_image([img_req]) #transformation between camera frame to body frame self._camera_T_body = image_response[ 0].shot.sample.frame.base_tform_offset #transformation between body to world self._body_T_world = image_response[0].ko_tform_body #Camera intrinsics for the given source camera self._intrinsics = image_response[0].source.pinhole.intrinsics width = image_response[0].shot.image.cols height = image_response[0].shot.image.rows # detect given fiducial in image and return the bounding box of it bboxes = self.find_image_tag_lib(image_response[0].shot.image, (width, height), source_name) if bboxes: return bboxes, source_name else: self._tag_not_located = True print("Failed to find bounding box for " + str(source_name)) return [], None
def main(argv): # Parse args parser = argparse.ArgumentParser() bosdyn.client.util.add_common_arguments(parser) parser.add_argument('--image-sources', help='Get image from source(s)', action='append') parser.add_argument('--image-service', help='Name of the image service to query.', default=ImageClient.default_service_name) parser.add_argument('-j', '--jpeg-quality-percent', help="JPEG quality percentage (0-100)", type=int, default=50) parser.add_argument('-c', '--capture-delay', help="Time [ms] to wait before the next capture", type=int, default=100) parser.add_argument('--disable-full-screen', help="A single image source gets displayed full screen by default. This flag disables that.", action='store_true') parser.add_argument('--auto-rotate', help='rotate right and front images to be upright', action='store_true') 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) robot.authenticate(options.username, options.password) robot.sync_with_directory() robot.time_sync.wait_for_sync() image_client = robot.ensure_client(options.image_service) requests = [ build_image_request(source, quality_percent=options.jpeg_quality_percent) for source in options.image_sources ] for image_source in options.image_sources: cv2.namedWindow(image_source, cv2.WINDOW_NORMAL) if len(options.image_sources) > 1 or options.disable_full_screen: cv2.setWindowProperty(image_source, cv2.WND_PROP_AUTOSIZE, cv2.WINDOW_AUTOSIZE) else: cv2.setWindowProperty(image_source, cv2.WND_PROP_FULLSCREEN, cv2.WINDOW_FULLSCREEN) keystroke = None timeout_count_before_reset = 0 while keystroke != VALUE_FOR_Q_KEYSTROKE and keystroke != VALUE_FOR_ESC_KEYSTROKE: try: images_future = image_client.get_image_async(requests, timeout=0.5) while not images_future.done(): keystroke = cv2.waitKey(25) print(keystroke) if keystroke == VALUE_FOR_ESC_KEYSTROKE or keystroke == VALUE_FOR_Q_KEYSTROKE: sys.exit(1) images = images_future.result() except TimedOutError as time_err: if timeout_count_before_reset == 5: # To attempt to handle bad comms and continue the live image stream, try recreating the # image client after having an RPC timeout 5 times. _LOGGER.info("Resetting image client after 5+ timeout errors.") image_client = reset_image_client(robot) timeout_count_before_reset = 0 else: timeout_count_before_reset += 1 except Exception as err: _LOGGER.warning(err) continue for i in range(len(images)): image, _ = image_to_opencv(images[i], options.auto_rotate) cv2.imshow(images[i].source.name, image) keystroke = cv2.waitKey(options.capture_delay)
def __init__(self, username, password, hostname, logger, rates={}, callbacks={}): self._username = username self._password = password self._hostname = hostname self._logger = logger self._rates = rates self._callbacks = callbacks self._keep_alive = True self._valid = True self._mobility_params = RobotCommandBuilder.mobility_params() self._is_standing = False self._is_sitting = True self._is_moving = False self._last_stand_command = None self._last_sit_command = None self._last_motion_command = None self._last_motion_command_time = None self._front_image_requests = [] for source in front_image_sources: self._front_image_requests.append( build_image_request( source, image_format=image_pb2.Image.Format.FORMAT_RAW)) self._side_image_requests = [] for source in side_image_sources: self._side_image_requests.append( build_image_request( source, image_format=image_pb2.Image.Format.FORMAT_RAW)) self._rear_image_requests = [] for source in rear_image_sources: self._rear_image_requests.append( build_image_request( source, image_format=image_pb2.Image.Format.FORMAT_RAW)) try: self._sdk = create_standard_sdk('ros_spot') except Exception as e: self._logger.error("Error creating SDK object: %s", e) self._valid = False return self._robot = self._sdk.create_robot(self._hostname) try: self._robot.authenticate(self._username, self._password) self._robot.start_time_sync() except RpcError as err: self._logger.error("Failed to communicate with robot: %s", err) self._valid = False return if self._robot: # Clients try: self._robot_state_client = self._robot.ensure_client( RobotStateClient.default_service_name) self._robot_command_client = self._robot.ensure_client( RobotCommandClient.default_service_name) self._graph_nav_client = self._robot.ensure_client( GraphNavClient.default_service_name) self._power_client = self._robot.ensure_client( PowerClient.default_service_name) self._lease_client = self._robot.ensure_client( LeaseClient.default_service_name) self._lease_wallet = self._lease_client.lease_wallet self._image_client = self._robot.ensure_client( ImageClient.default_service_name) self._estop_client = self._robot.ensure_client( EstopClient.default_service_name) except Exception as e: self._logger.error("Unable to create client service: %s", e) self._valid = False return # Store the most recent knowledge of the state of the robot based on rpc calls. self._current_graph = None self._current_edges = dict( ) #maps to_waypoint to list(from_waypoint) self._current_waypoint_snapshots = dict( ) # maps id to waypoint snapshot self._current_edge_snapshots = dict() # maps id to edge snapshot self._current_annotation_name_to_wp_id = dict() # Async Tasks self._async_task_list = [] self._robot_state_task = AsyncRobotState( self._robot_state_client, self._logger, max(0.0, self._rates.get("robot_state", 0.0)), self._callbacks.get("robot_state", lambda: None)) self._robot_metrics_task = AsyncMetrics( self._robot_state_client, self._logger, max(0.0, self._rates.get("metrics", 0.0)), self._callbacks.get("metrics", lambda: None)) self._lease_task = AsyncLease( self._lease_client, self._logger, max(0.0, self._rates.get("lease", 0.0)), self._callbacks.get("lease", lambda: None)) self._front_image_task = AsyncImageService( self._image_client, self._logger, max(0.0, self._rates.get("front_image", 0.0)), self._callbacks.get("front_image", lambda: None), self._front_image_requests) self._side_image_task = AsyncImageService( self._image_client, self._logger, max(0.0, self._rates.get("side_image", 0.0)), self._callbacks.get("side_image", lambda: None), self._side_image_requests) self._rear_image_task = AsyncImageService( self._image_client, self._logger, max(0.0, self._rates.get("rear_image", 0.0)), self._callbacks.get("rear_image", lambda: None), self._rear_image_requests) self._idle_task = AsyncIdle(self._robot_command_client, self._logger, 10.0, self) self._estop_endpoint = None self._async_tasks = AsyncTasks([ self._robot_state_task, self._robot_metrics_task, self._lease_task, self._front_image_task, self._side_image_task, self._rear_image_task, self._idle_task ]) self._robot_id = None self._lease = None
def request_image_and_save(image_sources, image_client, filepath, verbose=False): """Request the image sources in all possible formats and save the data. This makes GetImage RPCs for the image sources for each type of image format. The requests which respond without errors are then checked for completeness in the proto response, and saved. A warning message is printed if an image source cannot respond in a format that the tablet will be able to understand. Args: image_sources (List[string]): The image source names from the image service. image_client (ImageClient): The client for the image service being tested. filepath (string): A destination folder to save the images at. verbose (boolean): Print additional logging information on failure. Returns: Boolean indicating if the image sources could be successfully requested in one of the possible image formats and the image response is validated. """ successful_request_found = False successful_tablet_request_found = False # Check that one of the formats requested by the tablet will work. for img_format in ALL_FORMATS: img_req = [ build_image_request(source_name, image_format=img_format) for source_name in image_sources ] img_resps = None try: img_resps = image_client.get_image(img_req) except UnsupportedImageFormatRequestedError as err: _LOGGER.error("The image format %s is unsupported for image sources %s.", image_pb2.Image.Format.Name(img_format), image_sources) if verbose: log_debug_information(err, img_req, strip_response=True) continue except ImageDataError as err: _LOGGER.error( "The image sources (%s) were unable to be captured and decoded in format %s.", image_sources, image_pb2.Image.Format.Name(img_format)) if verbose: log_debug_information(err, img_req, strip_response=True) continue except UnknownImageSourceError as err: unknown_sources = [] for img_resp in err.response.image_responses: if img_resp.status == image_pb2.ImageResponse.STATUS_UNKNOWN_CAMERA: unknown_sources.append(img_resp.source.name) _LOGGER.error("The image sources %s are unknown by the image service.", unknown_sources) if verbose: log_debug_information(err, img_req, strip_response=True) continue except SourceDataError as err: _LOGGER.error("The image sources (%s) do not have image source information.", image_sources) if verbose: log_debug_information(err, img_req, strip_response=True) continue except ResponseTooLargeError as err: _LOGGER.warning( "Note: the response for requesting image sources %s in format %s is too large and they cannot " "all be requested at once unless the ImageClient's grpc message limit is increased.", image_sources, image_pb2.Image.Format.Name(img_format)) if verbose: log_debug_information(err, img_req, strip_response=True) # Exit out when the request is too large. return True # Check that the bare minimum required fields of the image response are populated. if len(img_resps) != len(img_req): # Found too many or too few image responses in a request for only one image. _LOGGER.warning( "The GetImageResponse RPC contains %d image responses, when %d images were requested.", len(img_resps), len(img_req)) if verbose: _LOGGER.info("GetImage requests: %s", img_req) _LOGGER.info("GetImage response: %s", [copy_image_response_and_strip_bytes(img) for img in img_resp]) continue _LOGGER.info("Successfully saved image sources %s in format %s", image_sources, image_pb2.Image.Format.Name(img_format)) for img_data in img_resps: if not validate_image_response(img_data, img_req, img_format, verbose): # The image response did not succeed in the validation checks, therefore the format # requested does not completely work. Continue to the next potential image format # and attempt to request it. continue # All checks for the image response have succeeded for this image format! successful_request_found = True if img_format in TABLET_REQUIRED_IMAGE_FORMATS: successful_tablet_request_found = True # Save all the collect images. save_images_as_files(img_resps, filepath=filepath) if not successful_tablet_request_found: _LOGGER.warning( "The image sources %s did not respond successfully to a GetImage RPC with one of the " "known image formats (%s) used by the tablet. This means the images will NOT appear successfully " "on the tablet.", image_sources, [image_pb2.Image.Format.Name(f) for f in TABLET_REQUIRED_IMAGE_FORMATS]) return successful_request_found
def __init__(self, username, password, token, hostname, logger, rates={}, callbacks={}): self._username = username self._password = password self._token = token self._hostname = hostname self._logger = logger self._rates = rates self._callbacks = callbacks self._keep_alive = True self._valid = True self._mobility_params = RobotCommandBuilder.mobility_params() self._is_standing = False self._is_sitting = True self._is_moving = False self._last_stand_command = None self._last_sit_command = None self._last_motion_command = None self._last_motion_command_time = None self._front_image_requests = [] for source in front_image_sources: self._front_image_requests.append( build_image_request( source, image_format=image_pb2.Image.Format.FORMAT_RAW)) self._side_image_requests = [] for source in side_image_sources: self._side_image_requests.append( build_image_request( source, image_format=image_pb2.Image.Format.FORMAT_RAW)) self._rear_image_requests = [] for source in rear_image_sources: self._rear_image_requests.append( build_image_request( source, image_format=image_pb2.Image.Format.FORMAT_RAW)) try: self._sdk = create_standard_sdk('ros_spot') except Exception as e: self._logger.error("Error creating SDK object: %s", e) self._valid = False return try: self._sdk.load_app_token(self._token) except Exception as e: self._logger.error("Error loading developer token: %s", e) self._valid = False return self._robot = self._sdk.create_robot(self._hostname) try: self._robot.authenticate(self._username, self._password) self._robot.start_time_sync() except RpcError as err: self._logger.error("Failed to communicate with robot: %s", err) self._valid = False return if self._robot: # Clients try: self._robot_state_client = self._robot.ensure_client( RobotStateClient.default_service_name) self._robot_command_client = self._robot.ensure_client( RobotCommandClient.default_service_name) self._power_client = self._robot.ensure_client( PowerClient.default_service_name) self._lease_client = self._robot.ensure_client( LeaseClient.default_service_name) self._image_client = self._robot.ensure_client( ImageClient.default_service_name) self._estop_client = self._robot.ensure_client( EstopClient.default_service_name) except Exception as e: self._logger.error("Unable to create client service: %s", e) self._valid = False return # Async Tasks self._async_task_list = [] self._robot_state_task = AsyncRobotState( self._robot_state_client, self._logger, max(0.0, self._rates.get("robot_state", 0.0)), self._callbacks.get("robot_state", lambda: None)) self._robot_metrics_task = AsyncMetrics( self._robot_state_client, self._logger, max(0.0, self._rates.get("metrics", 0.0)), self._callbacks.get("metrics", lambda: None)) self._lease_task = AsyncLease( self._lease_client, self._logger, max(0.0, self._rates.get("lease", 0.0)), self._callbacks.get("lease", lambda: None)) self._front_image_task = AsyncImageService( self._image_client, self._logger, max(0.0, self._rates.get("front_image", 0.0)), self._callbacks.get("front_image", lambda: None), self._front_image_requests) self._side_image_task = AsyncImageService( self._image_client, self._logger, max(0.0, self._rates.get("side_image", 0.0)), self._callbacks.get("side_image", lambda: None), self._side_image_requests) self._rear_image_task = AsyncImageService( self._image_client, self._logger, max(0.0, self._rates.get("rear_image", 0.0)), self._callbacks.get("rear_image", lambda: None), self._rear_image_requests) self._idle_task = AsyncIdle(self._robot_command_client, self._logger, 10.0, self) self._estop_endpoint = EstopEndpoint(self._estop_client, 'ros', 9.0) self._async_tasks = AsyncTasks([ self._robot_state_task, self._robot_metrics_task, self._lease_task, self._front_image_task, self._side_image_task, self._rear_image_task, self._idle_task ]) self._robot_id = None self._lease = None