Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Update examples for v0.0.4 #66

Merged
merged 26 commits into from
Jan 18, 2023
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
26 commits
Select commit Hold shift + click to select a range
9ccffcb
simplify services api
edgarriba Jan 11, 2023
6f2cb02
fix test for farm-ng-core 0.1.4
edgarriba Jan 11, 2023
31e675c
Merge branch 'main' of github.com:farm-ng/farm-ng-amiga into main
edgarriba Jan 11, 2023
fd8d953
Merge branch 'main' into simplify-services
edgarriba Jan 11, 2023
b21582a
Add stream() wrapper on streamCanbusMessages
Hackerman342 Jan 12, 2023
fb7dab3
Common ServiceState api
Hackerman342 Jan 14, 2023
83b7c3c
Remove STOPPED service state
Hackerman342 Jan 16, 2023
27a1aec
Add repeated Timestamp to ServiceStateReply
Hackerman342 Jan 16, 2023
b1fed5a
getServiceState as service.proto defined rpc
Hackerman342 Jan 16, 2023
9563790
Replace ReplyStatus with bool in proto def
Hackerman342 Jan 16, 2023
d979349
Generic ServiceClient
Hackerman342 Jan 16, 2023
e7d368b
whoops logger spam
Hackerman342 Jan 16, 2023
246571c
get_calibration stub wrapper
Hackerman342 Jan 17, 2023
3d4c20b
Minimal send utility
Hackerman342 Jan 17, 2023
d9d5011
Remove send_can_messages util
Hackerman342 Jan 17, 2023
3d73c19
Update tests to use generic Service state class / protos
Hackerman342 Jan 17, 2023
ef7b25b
Missed one
Hackerman342 Jan 17, 2023
62033ac
Update camera_client example
Hackerman342 Jan 17, 2023
f414e0b
Update camera_client_gui example
Hackerman342 Jan 17, 2023
55f0fa8
Update virtual_joystick example
Hackerman342 Jan 17, 2023
f244dc1
try/except for image display
Hackerman342 Jan 17, 2023
2f78671
bi-directional stream for sending CAN msgs used in virtual joystick
Hackerman342 Jan 17, 2023
c7ad3cf
Try/except in camera_client for empty buffer
Hackerman342 Jan 17, 2023
399d1bd
Define sendCanbusMessage as a bi-directional RPC
Hackerman342 Jan 17, 2023
09d4c51
Use TurboJPEG for image decoding (faster than kivy)
Hackerman342 Jan 18, 2023
28dca0d
update version req.
jerrycowell96 Jan 18, 2023
File filter

Filter by extension

Filter by extension


Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
44 changes: 21 additions & 23 deletions py/examples/camera_client/main.py
Original file line number Diff line number Diff line change
Expand Up @@ -20,56 +20,54 @@
import numpy as np
from farm_ng.oak import oak_pb2
from farm_ng.oak.camera_client import OakCameraClient
from farm_ng.oak.camera_client import OakCameraClientConfig
from farm_ng.oak.camera_client import OakCameraServiceState
from farm_ng.service import service_pb2
from farm_ng.service.service_client import ClientConfig


async def main(address: str, port: int, stream_every_n: int) -> None:
# configure the camera client
config = OakCameraClientConfig(address=address, port=port)
config = ClientConfig(address=address, port=port)
client = OakCameraClient(config)

# get the streaming object
# Get the streaming object from the service
response_stream = client.stream_frames(every_n=stream_every_n)

# start the streaming service
await client.connect_to_service()

while True:
# query the service state
# NOTE: This could be done asynchronously with client.poll_service_state()
# as in other examples, such as camera_client_gui
state: OakCameraServiceState = await client.get_state()

if state.value != oak_pb2.OakServiceState.RUNNING:
# check the service state
state = await client.get_state()
if state.value != service_pb2.ServiceState.RUNNING:
print("Camera is not streaming!")
continue

response: oak_pb2.StreamFramesReply = await response_stream.read()
if response and response.status == oak_pb2.ReplyStatus.OK:

if response:
# get the sync frame
frame: oak_pb2.OakSyncFrame = response.frame
print(f"Got frame: {frame.sequence_num}")
print(f"Device info: {frame.device_info}")
print(f"Timestamp: {frame.rgb.meta.timestamp}")
print("#################################\n")

# cast image data bytes to numpy and decode
# NOTE: explore frame.[rgb, disparity, left, right]
image = np.frombuffer(frame.rgb.image_data, dtype="uint8")
image = cv2.imdecode(image, cv2.IMREAD_UNCHANGED)
try:
# cast image data bytes to numpy and decode
# NOTE: explore frame.[rgb, disparity, left, right]
image = np.frombuffer(frame.rgb.image_data, dtype="uint8")
image = cv2.imdecode(image, cv2.IMREAD_UNCHANGED)

# visualize the image
cv2.namedWindow("image", cv2.WINDOW_NORMAL)
cv2.imshow("image", image)
cv2.waitKey(1)
# visualize the image
cv2.namedWindow("image", cv2.WINDOW_NORMAL)
cv2.imshow("image", image)
cv2.waitKey(1)
except Exception as e:
print(e)


if __name__ == "__main__":
parser = argparse.ArgumentParser(prog="amiga-camera-app")
parser.add_argument("--port", type=int, required=True, help="The camera port.")
parser.add_argument("--address", type=str, default="localhost", help="The camera address")
parser.add_argument("--stream-every-n", type=int, default=4, help="Streaming frequency")
parser.add_argument("--stream-every-n", type=int, default=1, help="Streaming frequency")
args = parser.parse_args()

asyncio.run(main(args.address, args.port, args.stream_every_n))
79 changes: 50 additions & 29 deletions py/examples/camera_client_gui/main.py
Original file line number Diff line number Diff line change
Expand Up @@ -13,13 +13,15 @@
# limitations under the License.
import argparse
import asyncio
import io
import os
from typing import List

import grpc
from farm_ng.oak import oak_pb2
from farm_ng.oak.camera_client import OakCameraClient
from farm_ng.oak.camera_client import OakCameraClientConfig
from farm_ng.service import service_pb2
from farm_ng.service.service_client import ClientConfig
from turbojpeg import TurboJPEG

os.environ["KIVY_NO_ARGS"] = "1"

Expand All @@ -30,7 +32,7 @@

from kivy.app import App # noqa: E402
from kivy.lang.builder import Builder # noqa: E402
from kivy.core.image import Image as CoreImage # noqa: E402
from kivy.graphics.texture import Texture # noqa: E402

kv = """
TabbedPanel:
Expand Down Expand Up @@ -61,11 +63,9 @@ def __init__(self, address: str, port: int, stream_every_n: int) -> None:
self.port = port
self.stream_every_n = stream_every_n

self.image_decoder = TurboJPEG()
self.tasks: List[asyncio.Task] = []

self.client: OakCameraClient
self.config: OakCameraClientConfig

def build(self):
return Builder.load_string(kv)

Expand All @@ -78,44 +78,65 @@ async def run_wrapper():
task.cancel()

# configure the camera client
self.config = OakCameraClientConfig(address=self.address, port=self.port)
self.client = OakCameraClient(self.config)
config = ClientConfig(address=self.address, port=self.port)
client = OakCameraClient(config)

# Stream camera frames
self.tasks.append(asyncio.ensure_future(self.stream_camera(self.client)))
# Continuously monitor camera service state
self.tasks.append(asyncio.ensure_future(self.client.poll_service_state()))
self.tasks.append(asyncio.ensure_future(self.stream_camera(client)))

return await asyncio.gather(run_wrapper(), *self.tasks)

async def stream_camera(self, client: OakCameraClient) -> None:
"""This task listens to the camera client's stream and populates the tabbed panel with all 4 image streams
from the oak camera."""
while self.root is None:
await asyncio.sleep(0.01)

response_stream = None

while True:
if client.state.value != oak_pb2.OakServiceState.RUNNING:
# start the streaming service
await client.connect_to_service()
await asyncio.sleep(0.01)
continue
elif response_stream is None:
# get the streaming object
response_stream = client.stream_frames(every_n=self.stream_every_n)
await asyncio.sleep(0.01)
# check the state of the service
state = await client.get_state()

if state.value not in [service_pb2.ServiceState.IDLE, service_pb2.ServiceState.RUNNING]:
# Cancel existing stream, if it exists
if response_stream is not None:
response_stream.cancel()
response_stream = None
print("Camera service is not streaming or ready to stream")
await asyncio.sleep(0.1)
continue

response: oak_pb2.StreamFramesReply = await response_stream.read()
if response and response.status == oak_pb2.ReplyStatus.OK:
# get the sync frame
frame: oak_pb2.OakSyncFrame = response.frame
# Create the stream
if response_stream is None:
response_stream = client.stream_frames(every_n=1)

try:
# try/except so app doesn't crash on killed service
response: oak_pb2.StreamFramesReply = await response_stream.read()
assert response and response != grpc.aio.EOF, "End of stream"
except Exception as e:
print(e)
response_stream.cancel()
response_stream = None
continue

# get image and show
for view_name in ["rgb", "disparity", "left", "right"]:
self.root.ids[view_name].texture = CoreImage(
io.BytesIO(getattr(frame, view_name).image_data), ext="jpg"
).texture
# get the sync frame
frame: oak_pb2.OakSyncFrame = response.frame

# get image and show
for view_name in ["rgb", "disparity", "left", "right"]:
# Skip if view_name was not included in frame
try:
# Decode the image and render it in the correct kivy texture
img = self.image_decoder.decode(getattr(frame, view_name).image_data)
texture = Texture.create(size=(img.shape[1], img.shape[0]), icolorfmt="bgr")
texture.flip_vertical()
texture.blit_buffer(img.tobytes(), colorfmt="bgr", bufferfmt="ubyte", mipmap_generation=False)
self.root.ids[view_name].texture = texture

except Exception as e:
print(e)


if __name__ == "__main__":
Expand Down
1 change: 1 addition & 0 deletions py/examples/camera_client_gui/requirements.txt
Original file line number Diff line number Diff line change
@@ -1,2 +1,3 @@
farm-ng-amiga
kivy>=2.1.0
PyTurboJPEG
Loading