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