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)