def record_to_msg(self, record):
level = self.record_level_to_proto_level(record.levelno)
msg = log_annotation_protos.LogAnnotationTextMessage(service=self.service, level=level)
msg.message = self.format(record)
if self.time_sync_endpoint is not None:
try:
msg.timestamp.CopyFrom(
self.time_sync_endpoint.robot_timestamp_from_local_secs(time.time()))
except time_sync.NotEstablishedError:
msg.message = '(No time sync!): ' + msg.message
msg.timestamp.CopyFrom(core_util.now_timestamp())
else:
msg.timestamp.CopyFrom(core_util.now_timestamp())
return msg
def acquire_data_async(self,
acquisition_requests,
action_name,
group_name,
data_timestamp=None,
metadata=None,
**kwargs):
"""Async version of the acquire_data() RPC."""
if data_timestamp is None:
if not self._timesync_endpoint:
data_timestamp = now_timestamp()
else:
data_timestamp = self._timesync_endpoint.robot_timestamp_from_local_secs(
time.time())
action_id = data_acquisition.CaptureActionId(action_name=action_name,
group_name=group_name,
timestamp=data_timestamp)
metadata_proto = metadata_to_proto(metadata)
request = data_acquisition.AcquireDataRequest(
metadata=metadata_proto,
acquisition_requests=acquisition_requests,
action_id=action_id)
return self.call_async(self._stub.AcquireData,
request,
value_from_response=get_request_id,
error_from_response=acquire_data_error,
**kwargs)
def create_drawable_sphere_object():
"""Create a drawable sphere world object that can be added through a mutation request."""
# Add an edge where the transform vision_tform_object is only a position transform with no rotation.
vision_tform_drawable = geom.SE3Pose(position=geom.Vec3(x=2, y=3, z=2),
rotation=geom.Quaternion(x=0, y=0, z=0, w=1))
# Create a map between the child frame name and the parent frame name/SE3Pose parent_tform_child
edges = {}
# Create an edge in the frame tree snapshot that includes vision_tform_drawable
drawable_frame_name = "drawing_sphere"
edges = add_edge_to_tree(edges, vision_tform_drawable, VISION_FRAME_NAME, drawable_frame_name)
snapshot = geom.FrameTreeSnapshot(child_to_parent_edge_map=edges)
# Set the acquisition time for the sphere using a function to get google.protobuf.Timestamp of the current system time.
time_now = now_timestamp()
# Create the sphere drawable object
sphere = world_object_pb2.DrawableSphere(radius=1)
red_color = world_object_pb2.DrawableProperties.Color(r=255, b=0, g=0, a=1)
sphere_drawable_prop = world_object_pb2.DrawableProperties(
color=red_color, label="red sphere", wireframe=False, sphere=sphere,
frame_name_drawable=drawable_frame_name)
# Create the complete world object with transform information, a unique name, and the drawable sphere properties.
world_object_sphere = world_object_pb2.WorldObject(id=16, name="red_sphere_ball",
transforms_snapshot=snapshot,
acquisition_time=time_now,
drawable_properties=[sphere_drawable_prop])
return world_object_sphere
def test_grpc_read_write():
"""Test writing GRPC data."""
file_annotations = {'robot': 'spot', 'individual': 'spot-BD-99990001'}
service_name = 'robot-id'
filename = os.path.join(tempfile.gettempdir(), service_name + '.bddf')
filename = service_name + '.bddf'
# pylint: disable=no-member
request = robot_id.RobotIdRequest()
request.header.request_timestamp.CopyFrom(now_timestamp())
request.header.client_name = 'test_bddf'
response = robot_id.RobotIdResponse()
response.header.response_timestamp.CopyFrom(now_timestamp())
response.header.request.Pack(request)
# Test writing the file.
with open(filename, 'wb') as outfile, \
DataWriter(outfile, annotations=file_annotations) as data_writer:
grpc_log = GrpcServiceWriter(data_writer, service_name)
grpc_log.log_request(request)
grpc_log.log_response(response)
# Test reading the file.
with open(filename, 'rb') as infile:
data_reader = DataReader(infile)
grpc_reader = GrpcReader(
data_reader, [robot_id.RobotIdRequest, robot_id.RobotIdResponse])
proto_reader = grpc_reader.get_proto_reader(
robot_id.RobotIdRequest.DESCRIPTOR.full_name)
assert proto_reader.num_messages == 1
nsec, msg = proto_reader.get_message(0)
assert msg == request
assert nsec_to_timestamp(nsec) == msg.header.request_timestamp
proto_reader = grpc_reader.get_proto_reader(
robot_id.RobotIdResponse.DESCRIPTOR.full_name)
nsec, msg = proto_reader.get_message(0)
assert msg == response
assert nsec_to_timestamp(nsec) == msg.header.response_timestamp
def make_acquire_data_request(self, acquisition_requests, action_name, group_name, data_timestamp=None, metadata=None):
"""Helper utility to generate an AcquireDataRequest."""
if data_timestamp is None:
if not self._timesync_endpoint:
data_timestamp = now_timestamp()
else:
data_timestamp = self._timesync_endpoint.robot_timestamp_from_local_secs(
time.time())
action_id = data_acquisition.CaptureActionId(action_name=action_name, group_name=group_name,
timestamp=data_timestamp)
return data_acquisition.AcquireDataRequest(acquisition_requests=acquisition_requests,
action_id=action_id,
metadata=metadata_to_proto(metadata))
def create_apriltag_object():
"""Create an apriltag world object that can be added through a mutation request."""
# Set the acquisition time for the additional april tag in robot time using a function to
# get google.protobuf.Timestamp of the current system time.
time_now = now_timestamp()
# The apriltag id for the object we want to add.
tag_id = 308
# Set the frame names used for the two variants of the apriltag (filtered, raw)
frame_name_fiducial = "fiducial_" + str(tag_id)
frame_name_fiducial_filtered = "filtered_fiducial_" + str(tag_id)
# Make the april tag (slightly offset from the first tag detection) as a world object. Create the
# different edges necessary to create an expressive tree. The root node will be the world frame.
default_a_tform_b = geom.SE3Pose(position=geom.Vec3(x=.2, y=.2, z=.2),
rotation=geom.Quaternion(x=.1, y=.1, z=.1, w=.1))
# Create a map between the child frame name and the parent frame name/SE3Pose parent_tform_child
edges = {}
# Create an edge for the raw fiducial detection in the world.
vision_tform_fiducial = update_frame(tf=default_a_tform_b, position_change=(0, 0, -.2),
rotation_change=(0, 0, 0, 0))
edges = add_edge_to_tree(edges, vision_tform_fiducial, VISION_FRAME_NAME, frame_name_fiducial)
# Create a edge for the filtered version of the fiducial in the world.
vision_tform_filtered_fiducial = update_frame(tf=default_a_tform_b, position_change=(0, 0, -.2),
rotation_change=(0, 0, 0, 0))
edges = add_edge_to_tree(edges, vision_tform_filtered_fiducial, VISION_FRAME_NAME,
frame_name_fiducial_filtered)
# Create the transform to express vision_tform_odom
vision_tform_odom = update_frame(tf=default_a_tform_b, position_change=(0, 0, -.2),
rotation_change=(0, 0, 0, 0))
edges = add_edge_to_tree(edges, vision_tform_odom, VISION_FRAME_NAME, ODOM_FRAME_NAME)
# Can also add custom frames into the frame tree snapshot as long as they keep the tree structure,
# so the parent_frame must also be in the tree.
vision_tform_special_frame = update_frame(tf=default_a_tform_b, position_change=(0, 0, -.2),
rotation_change=(0, 0, 0, 0))
edges = add_edge_to_tree(edges, vision_tform_special_frame, VISION_FRAME_NAME,
"my_special_frame")
snapshot = geom.FrameTreeSnapshot(child_to_parent_edge_map=edges)
# Create the specific properties for the apriltag including the frame names for the transforms
# describing the apriltag's position.
tag_prop = world_object_pb2.AprilTagProperties(
tag_id=tag_id, dimensions=geom.Vec2(x=.2, y=.2), frame_name_fiducial=frame_name_fiducial,
frame_name_fiducial_filtered=frame_name_fiducial_filtered)
#Create the complete world object with transform information and the apriltag properties.
wo_obj_to_add = world_object_pb2.WorldObject(id=21, transforms_snapshot=snapshot,
acquisition_time=time_now,
apriltag_properties=tag_prop)
return wo_obj_to_add
def main(argv):
"""An example using the API demonstrating adding image coordinates to the world object service."""
parser = argparse.ArgumentParser()
bosdyn.client.util.add_common_arguments(parser)
options = parser.parse_args(argv)
# Create robot object with a world object client.
sdk = bosdyn.client.create_standard_sdk('WorldObjectClient')
sdk.load_app_token(options.app_token)
robot = sdk.create_robot(options.hostname)
robot.authenticate(options.username, options.password)
# Time sync is necessary so that time-based filter requests can be converted.
robot.time_sync.wait_for_sync()
# Create the world object client.
world_object_client = robot.ensure_client(
WorldObjectClient.default_service_name)
# List all world objects in the scene before any mutation.
world_objects = world_object_client.list_world_objects().world_objects
print("Current World objects before mutations: " +
str([obj for obj in world_objects]))
# Set the detection time for the additional april tag. The client library will convert the time into robot time.
# Uses a function to get google.protobuf.Timestamp of the current system time.
timestamp = now_timestamp()
# Create the image coordinate object. This type of object does not require a base frame for the world object.
# Since we are not providing a transform to the object expressed by the image coordinates, than it is not necessary
# to set the frame_name_image_properties, as this describes the frame used in a transform (such as world_tform_image_coords)
img_coord = wo.ImageProperties(
camera_source="back",
coordinates=geom.Polygon(vertexes=[geom.Vec2(x=100, y=100)]))
wo_obj = wo.WorldObject(id=2,
name="img_coord_tester",
acquisition_time=timestamp,
image_properties=img_coord)
# Request to add the image coordinates detection to the world object service.
add_coords = make_add_world_object_req(wo_obj)
resp = world_object_client.mutate_world_objects(mutation_req=add_coords)
# List all world objects in the scene after the mutation was applied.
world_objects = world_object_client.list_world_objects().world_objects
print("Current World objects after adding coordinates: " +
str([obj for obj in world_objects]))
return True
def acquire_data(self,
acquisition_requests,
action_name,
group_name,
data_timestamp=None,
metadata=None,
**kwargs):
"""Trigger a data acquisition to save data and metadata to the data buffer.
Args:
acquisition_requests (bosdyn.api.AcquisitionRequestList): The different image sources and
data sources to capture from and save to the data buffer with the same timestamp.
action_name(string): The unique action name that all data will be saved with.
group_name(string): The unique group name that all data will be saved with.
data_timestamp (google.protobuf.Timestamp): The unique timestamp that all data will be
saved with.
metadata (bosdyn.api.Metadata | dict): The JSON structured metadata to be associated with
the data returned by the DataAcquisitionService when logged in the data buffer
service.
Raises:
RpcError: Problem communicating with the robot.
Returns:
If the RPC is successful, then it will return the acquire data request id, which can be
used to check the status of the acquisition and get feedback.
"""
if data_timestamp is None:
if not self._timesync_endpoint:
data_timestamp = now_timestamp()
else:
data_timestamp = self._timesync_endpoint.robot_timestamp_from_local_secs(
time.time())
action_id = data_acquisition.CaptureActionId(action_name=action_name,
group_name=group_name,
timestamp=data_timestamp)
metadata_proto = metadata_to_proto(metadata)
request = data_acquisition.AcquireDataRequest(
metadata=metadata_proto,
acquisition_requests=acquisition_requests,
action_id=action_id)
return self.call(self._stub.AcquireData,
request,
value_from_response=get_request_id,
error_from_response=acquire_data_error,
**kwargs)
def main(argv):
"""An example using the API to apply mutations to world objects."""
parser = argparse.ArgumentParser()
bosdyn.client.util.add_common_arguments(parser)
options = parser.parse_args(argv)
# Create robot object with a world object client.
sdk = bosdyn.client.create_standard_sdk('WorldObjectClient')
robot = sdk.create_robot(options.hostname)
robot.authenticate(options.username, options.password)
# Time sync is necessary so that time-based filter requests can be converted.
robot.time_sync.wait_for_sync()
# Create the world object client.
world_object_client = robot.ensure_client(
WorldObjectClient.default_service_name)
# List all world objects in the scene.
world_objects = world_object_client.list_world_objects().world_objects
print("Current World objects' ids " +
str([obj.id for obj in world_objects]))
# If there are any world objects in Spot's perception scene, then attempt to mutate one.
# This should fail and return a STATUS_NO_PERMISSION since a client cannot mutate
# objects that they did not add into the scene.
if len(world_objects) > 0:
obj_to_mutate = world_objects[0]
# Attempt to delete the object.
delete_req = make_delete_world_object_req(obj_to_mutate)
status = world_object_client.mutate_world_objects(delete_req).status
assert (status == world_object_pb2.MutateWorldObjectResponse.
STATUS_NO_PERMISSION)
# Attempt to change the object.
for edge in obj_to_mutate.transforms_snapshot.child_to_parent_edge_map:
obj_to_mutate.transforms_snapshot.child_to_parent_edge_map[
edge].parent_tform_child.position.x = 1.0
change_req = make_change_world_object_req(obj_to_mutate)
status = world_object_client.mutate_world_objects(change_req).status
assert (status == world_object_pb2.MutateWorldObjectResponse.
STATUS_NO_PERMISSION)
# Request to add the new april tag detection to the world object service.
wo_obj_to_add = create_apriltag_object()
add_apriltag = make_add_world_object_req(wo_obj_to_add)
resp = world_object_client.mutate_world_objects(mutation_req=add_apriltag)
# Get the world object ID set by the service, so that we can make additional changes to this object.
added_apriltag_world_obj_id = resp.mutated_object_id
# List all world objects in the scene after the mutation was applied.
world_objects = world_object_client.list_world_objects().world_objects
print("World object IDs after object addition: " +
str([obj.apriltag_properties.tag_id for obj in world_objects]))
for world_obj in world_objects:
if world_obj.id == added_apriltag_world_obj_id:
# Look for the custom frame that was included in the add-request, where the child frame name was "my_special_frame"
full_snapshot = world_obj.transforms_snapshot
for edge in full_snapshot.child_to_parent_edge_map:
if edge == "my_special_frame":
print(
"The world object includes the custom frame vision_tform_my_special_frame!"
)
# Request to change an existing apriltag's dimensions. This will succeed because it is changing
# an object that was added by a client program. We are using the ID returned by the service to
# change the correct apriltag.
time_now = now_timestamp()
tag_prop_modified = world_object_pb2.AprilTagProperties(
tag_id=308, dimensions=geom.Vec2(x=.35, y=.35))
wo_obj_to_change = world_object_pb2.WorldObject(
id=added_apriltag_world_obj_id,
name="world_obj_apriltag",
transforms_snapshot=wo_obj_to_add.transforms_snapshot,
acquisition_time=time_now,
apriltag_properties=tag_prop_modified)
print("World object X dimension of apriltag size before change: " +
str([obj.apriltag_properties.dimensions.x for obj in world_objects]))
change_apriltag = make_change_world_object_req(wo_obj_to_change)
resp = world_object_client.mutate_world_objects(
mutation_req=change_apriltag)
assert (
resp.status == world_object_pb2.MutateWorldObjectResponse.STATUS_OK)
# List all world objects in the scene after the mutation was applied.
world_objects = world_object_client.list_world_objects().world_objects
print("World object X dimension of apriltag size after change: " +
str([obj.apriltag_properties.dimensions.x for obj in world_objects]))
# Add a apriltag and then delete it. This will succeed because it is deleting an object added by
# a client program and not specific to Spot's perception
add_apriltag = make_add_world_object_req(wo_obj_to_add)
resp = world_object_client.mutate_world_objects(mutation_req=add_apriltag)
assert (
resp.status == world_object_pb2.MutateWorldObjectResponse.STATUS_OK)
apriltag_to_delete_id = resp.mutated_object_id
# Update the list of world object's after adding a apriltag
world_objects = world_object_client.list_world_objects().world_objects
# Delete the april tag that was just added. This will succeed because it is changing an object that was
# just added by a client program (and not an object Spot's perception system detected). The world object
# can be identified by the ID returned from the service after the mutation request succeeded.
wo_obj_to_delete = world_object_pb2.WorldObject(id=apriltag_to_delete_id)
delete_apriltag = make_delete_world_object_req(wo_obj_to_delete)
resp = world_object_client.mutate_world_objects(
mutation_req=delete_apriltag)
assert (
resp.status == world_object_pb2.MutateWorldObjectResponse.STATUS_OK)
# List all world objects in the scene after the deletion was applied.
world_objects = world_object_client.list_world_objects().world_objects
print("World object IDs after object deletion: " +
str([obj.apriltag_properties.tag_id for obj in world_objects]))
# Add a drawable object into the perception scene with a custom frame and unique name.
sphere_to_add = create_drawable_sphere_object()
add_sphere = make_add_world_object_req(sphere_to_add)
resp = world_object_client.mutate_world_objects(mutation_req=add_sphere)
# Get the world object ID set by the service.
sphere_id = resp.mutated_object_id
# List all world objects in the scene after the mutation was applied. Find the sphere in the list
# and see the transforms added into the frame tree snapshot by Spot in addition to the custom frame.
world_objects = world_object_client.list_world_objects().world_objects
for world_obj in world_objects:
if world_obj.id == sphere_id:
print("Found sphere named " + world_obj.name)
full_snapshot = world_obj.transforms_snapshot
for edge in full_snapshot.child_to_parent_edge_map:
print("Child frame name: " + edge + ". Parent frame name: " +
full_snapshot.child_to_parent_edge_map[edge].
parent_frame_name)
return True
def test_write_read():
"""Test writing a data to a file, and reading it back."""
file_annotations = {'robot': 'spot', 'individual': 'spot-BD-99990001'}
channel_annotations = {'ccc': '3', 'd': '4444'}
filename = os.path.join(tempfile.gettempdir(), 'test1.bdf')
series1_type = 'bosdyn/test/1'
series1_spec = {'channel': 'channel_a'}
series1_content_type = 'text/plain'
series1_additional_indexes = ['idxa', 'idxb']
timestamp_nsec = now_nsec()
msg_data = b'This is some data'
operator_message = LogAnnotationOperatorMessage(message="End of test",
timestamp=now_timestamp())
pod_series_type = 'bosdyn/test/pod'
pod_spec = {'varname': 'test_var'}
# Test writing the file.
with open(filename, 'wb') as outfile, \
DataWriter(outfile, annotations=file_annotations) as data_writer:
# Write generic message data to the file.
series1_index = data_writer.add_message_series(
series1_type,
series1_spec,
series1_content_type,
'test_type',
annotations=channel_annotations,
additional_index_names=series1_additional_indexes)
data_writer.write_data(series1_index, timestamp_nsec, msg_data, [1, 2])
# Write a protobuf to the file.
proto_writer = ProtoSeriesWriter(data_writer,
LogAnnotationOperatorMessage)
proto_writer.write(timestamp_to_nsec(operator_message.timestamp),
operator_message)
# Write POD data (floats) to the file.
pod_writer = PodSeriesWriter(data_writer,
pod_series_type,
pod_spec,
bddf.TYPE_FLOAT32,
annotations={'units': 'm/s^2'})
for val in range(10, 20):
pod_writer.write(timestamp_nsec, val)
# Test reading the file.
with open(filename, 'rb') as infile, DataReader(infile) as data_reader:
# Check the file version number.
assert data_reader.version.major_version == 1
assert data_reader.version.minor_version == 0
assert data_reader.version.patch_level == 0
assert data_reader.annotations == file_annotations
expected_timestamp = Timestamp()
expected_timestamp.FromNanoseconds(timestamp_nsec)
assert data_reader.series_block_index(
0).block_entries[0].timestamp == expected_timestamp
assert data_reader.series_block_index(
0).block_entries[0].additional_indexes[0] == 1
assert data_reader.series_block_index(
0).block_entries[0].additional_indexes[1] == 2
# Check that there are 3 series in the file.
assert len(data_reader.file_index.series_identifiers) == 3
# Read generic message data from the file.
series_a_index = data_reader.series_spec_to_index(series1_spec)
assert data_reader.num_data_blocks(series_a_index) == 1
assert data_reader.total_bytes(series_a_index) == len(msg_data)
_desc, timestamp_, data_ = data_reader.read(series_a_index, 0)
assert timestamp_ == timestamp_nsec
assert data_ == msg_data
assert _desc.timestamp == expected_timestamp
assert _desc.additional_indexes[0] == 1
assert _desc.additional_indexes[1] == 2
# Read a protobuf from the file.
proto_reader = ProtobufReader(data_reader)
operator_message_reader = ProtobufChannelReader(
proto_reader, LogAnnotationOperatorMessage)
assert operator_message_reader.num_messages == 1
timestamp_, protobuf = operator_message_reader.get_message(0)
assert protobuf == operator_message
assert timestamp_ == timestamp_to_nsec(operator_message.timestamp)
# Read POD (float) data from the file.
with pytest.raises(ValueError):
pod_reader = PodSeriesReader(data_reader, {'spec': 'bogus'})
pod_reader = PodSeriesReader(data_reader, pod_spec)
assert pod_reader.pod_type.pod_type == bddf.TYPE_FLOAT32
assert pod_reader.series_descriptor.annotations['units'] == 'm/s^2'
assert pod_reader.num_data_blocks == 1
timestamp_, samples = pod_reader.read_samples(0)
assert timestamp_ == timestamp_nsec
assert samples == [float(val) for val in range(10, 20)]
with open(filename,
'rb') as infile, StreamDataReader(infile) as data_reader:
# Check the file version number.
assert data_reader.version.major_version == 1
assert data_reader.version.minor_version == 0
assert data_reader.version.patch_level == 0
assert data_reader.annotations == file_annotations
desc_, sdesc_, data_ = data_reader.read_data_block()
assert desc_.timestamp == expected_timestamp
assert desc_.additional_indexes[0] == 1
assert desc_.additional_indexes[1] == 2
assert sdesc_.message_type.content_type == series1_content_type
assert sdesc_.message_type.type_name == 'test_type'
assert data_ == msg_data
desc_, sdesc_, data_ = data_reader.read_data_block()
assert desc_.timestamp == operator_message.timestamp
assert sdesc_.message_type.content_type == 'application/protobuf'
assert sdesc_.message_type.type_name == LogAnnotationOperatorMessage.DESCRIPTOR.full_name
dec_msg = LogAnnotationOperatorMessage()
dec_msg.ParseFromString(data_)
assert dec_msg == operator_message
desc_, sdesc_, data_ = data_reader.read_data_block()
assert desc_.timestamp == expected_timestamp
assert sdesc_.pod_type.pod_type == bddf.TYPE_FLOAT32
assert not data_reader.eof
with pytest.raises(EOFError):
data_reader.read_data_block()
assert data_reader.eof
# Check that there are 3 series in the file.
assert len(data_reader.file_index.series_identifiers) == 3
assert data_reader.series_block_indexes[0].block_entries[
0].timestamp == expected_timestamp
assert data_reader.series_block_index(
0).block_entries[0].additional_indexes[0] == 1
assert data_reader.series_block_index(
0).block_entries[0].additional_indexes[1] == 2
assert (data_reader.file_index.series_identifiers ==
data_reader.stream_file_index.series_identifiers)
os.unlink(filename)
