From fcccefc875a3af44fc49f11de82eec5ed4a19871 Mon Sep 17 00:00:00 2001 From: Manglemix Date: Wed, 20 Sep 2023 14:49:27 -0600 Subject: [PATCH] pozyx test and camera streaming progress --- lunadev/luna.dockerfile | 6 ++ src/camera/camera/__init__.py | 0 src/camera/camera/camera_compress.py | 99 +++++++++++++++++++ src/camera/package.xml | 21 ++++ src/camera/resource/camera | 0 src/camera/setup.cfg | 4 + src/camera/setup.py | 26 +++++ src/camera/test/test_copyright.py | 25 +++++ src/camera/test/test_flake8.py | 25 +++++ src/camera/test/test_pep257.py | 23 +++++ src/global_msgs/CMakeLists.txt | 1 + src/global_msgs/msg/CompressedImagePacket.msg | 1 + .../positioning/pozyx_localizer.py | 14 ++- src/telemetry/telemetry/telemetry.py | 16 +++ 14 files changed, 253 insertions(+), 8 deletions(-) create mode 100644 src/camera/camera/__init__.py create mode 100644 src/camera/camera/camera_compress.py create mode 100644 src/camera/package.xml create mode 100644 src/camera/resource/camera create mode 100644 src/camera/setup.cfg create mode 100644 src/camera/setup.py create mode 100644 src/camera/test/test_copyright.py create mode 100644 src/camera/test/test_flake8.py create mode 100644 src/camera/test/test_pep257.py create mode 100644 src/global_msgs/msg/CompressedImagePacket.msg diff --git a/lunadev/luna.dockerfile b/lunadev/luna.dockerfile index 181035ae..b51fdeb8 100644 --- a/lunadev/luna.dockerfile +++ b/lunadev/luna.dockerfile @@ -61,5 +61,11 @@ RUN apt-get install -y --no-install-recommends \ # Install VNC and socat RUN apt-get install -y x11vnc socat xvfb +# Configure git +RUN git config pull.rebase false + +# Install ffmpeg +RUN apt install ffmpeg + COPY base_bashrc_append.sh /bashrc_append RUN ["/bin/bash", "-c", "cat /bashrc_append >> /root/.bashrc && rm /bashrc_append"] diff --git a/src/camera/camera/__init__.py b/src/camera/camera/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/src/camera/camera/camera_compress.py b/src/camera/camera/camera_compress.py new file mode 100644 index 00000000..0c77ef75 --- /dev/null +++ b/src/camera/camera/camera_compress.py @@ -0,0 +1,99 @@ +import rclpy +from rclpy.node import Node +import subprocess +from sensor_msgs.msg import Image +from global_msgs.msg import CompressedImagePacket +from threading import Thread +from time import time + + +class CameraCompress(Node): + def __init__(self): + super().__init__("camera_compress") + + self.target_fps = 10 + self.ffmpeg = subprocess.Popen( + [ + "ffmpeg", + "-f", "rawvideo", + "-pix_fmt", "rgb24", + "-s", "1280x720", + "-r", "10", + "-i", "-", + "-c:v", "libvpx-vp9", + "-b:v", "1500k", + # "-g", "72", + "-f", "matroska", "-" + ], + stdin=subprocess.PIPE, + stdout=subprocess.PIPE, + stderr=subprocess.PIPE, + ) + + self.camera_listener = self.create_subscription( + Image, + '/camera/color/image_raw', + self.camera_callback_callback, + 10 + ) + self.compressed_pub = self.create_publisher(CompressedImagePacket, 'compressed_image', 10) + + self.count = 0 + self.probe_complete = False + self.last_processed_frame_time = 0 + + self.thr = Thread(target=self.read_stdout) + self.thr.start() + + def read_stdout(self): + try: + while True: + data = self.ffmpeg.stdout.read(1024 * 8) + + if len(data) == 0: + err = b"" + while True: + packet = self.ffmpeg.stderr.read(1024) + if len(packet) == 0: + break + err += packet + raise Exception(err.decode()) + + self.compressed_pub.publish(CompressedImagePacket(data=data)) + self.probe_complete = True + + except BrokenPipeError: + # Error will be caught in camera callback + return + + def camera_callback_callback(self, img: Image): + if self.probe_complete and time() - self.last_processed_frame_time < 1.0 / self.target_fps: + return + try: + i = 0 + while i < len(img.data): + i += self.ffmpeg.stdin.write(bytes(img.data[i::])) + + except BrokenPipeError: + err = b"" + while True: + packet = self.ffmpeg.stderr.read(1024) + if len(packet) == 0: + break + err += packet + raise Exception(err.decode()) + + +def main(args=None): + rclpy.init(args=args) + + node = CameraCompress() + + rclpy.spin(node) + + node.destroy_node() + # rclpy.shutdown() + + +if __name__ == "__main__": + raise Exception("Please run from ROS 2") diff --git a/src/camera/package.xml b/src/camera/package.xml new file mode 100644 index 00000000..3c8b1b5c --- /dev/null +++ b/src/camera/package.xml @@ -0,0 +1,21 @@ + + + + camera + 0.0.0 + TODO: Package description + root + TODO: License declaration + + sensor_msgs + global_msgs + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/src/camera/resource/camera b/src/camera/resource/camera new file mode 100644 index 00000000..e69de29b diff --git a/src/camera/setup.cfg b/src/camera/setup.cfg new file mode 100644 index 00000000..cbcf7c15 --- /dev/null +++ b/src/camera/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/camera +[install] +install_scripts=$base/lib/camera diff --git a/src/camera/setup.py b/src/camera/setup.py new file mode 100644 index 00000000..abb0585e --- /dev/null +++ b/src/camera/setup.py @@ -0,0 +1,26 @@ +from setuptools import find_packages, setup + +package_name = 'camera' + +setup( + name=package_name, + version='0.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='root', + maintainer_email='shabouza030@gmail.com', + description='TODO: Package description', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'compress = camera.camera_compress:main' + ], + }, +) diff --git a/src/camera/test/test_copyright.py b/src/camera/test/test_copyright.py new file mode 100644 index 00000000..97a39196 --- /dev/null +++ b/src/camera/test/test_copyright.py @@ -0,0 +1,25 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_copyright.main import main +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/src/camera/test/test_flake8.py b/src/camera/test/test_flake8.py new file mode 100644 index 00000000..27ee1078 --- /dev/null +++ b/src/camera/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/src/camera/test/test_pep257.py b/src/camera/test/test_pep257.py new file mode 100644 index 00000000..b234a384 --- /dev/null +++ b/src/camera/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' diff --git a/src/global_msgs/CMakeLists.txt b/src/global_msgs/CMakeLists.txt index ae9ec32e..14e2ab34 100644 --- a/src/global_msgs/CMakeLists.txt +++ b/src/global_msgs/CMakeLists.txt @@ -26,6 +26,7 @@ find_package(rosidl_default_generators REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} "msg/Steering.msg" "action/SetArmAngle.action" + "msg/CompressedImagePacket.msg" ) if(BUILD_TESTING) diff --git a/src/global_msgs/msg/CompressedImagePacket.msg b/src/global_msgs/msg/CompressedImagePacket.msg new file mode 100644 index 00000000..17350525 --- /dev/null +++ b/src/global_msgs/msg/CompressedImagePacket.msg @@ -0,0 +1 @@ +uint8[] data \ No newline at end of file diff --git a/src/positioning/positioning/pozyx_localizer.py b/src/positioning/positioning/pozyx_localizer.py index eb6ed03a..71ad6a95 100644 --- a/src/positioning/positioning/pozyx_localizer.py +++ b/src/positioning/positioning/pozyx_localizer.py @@ -177,17 +177,15 @@ def loop(self): orientation=MsgQuaternion(x=orientation.x, y=orientation.y, z=orientation.z, w=orientation.w) ) - if status == POZYX_SUCCESS: - print(self.posAndOrientatonToString()) - else: + if status != POZYX_SUCCESS: statusString = "failure" if status == POZYX_FAILURE else "timeout" print('Error: Do positioning failed due to ' + statusString) - def posAndOrientatonToString(self): - """ - Returning a string with the x,y,z coordinates of the position and the orientation. - """ - return 'Current position:\n ' + str(self.position) + '\nCurrent orientation\n ' + str(self.orientation) + '\n' + # def posAndOrientatonToString(self): + # """ + # Returning a string with the x,y,z coordinates of the position and the orientation. + # """ + # return 'Current position:\n ' + str(self.position) + '\nCurrent orientation\n ' + str(self.orientation) + '\n' if __name__ == '__main__': diff --git a/src/telemetry/telemetry/telemetry.py b/src/telemetry/telemetry/telemetry.py index 8615a7e2..d851e046 100644 --- a/src/telemetry/telemetry/telemetry.py +++ b/src/telemetry/telemetry/telemetry.py @@ -5,6 +5,7 @@ from rclpy.node import Node from struct import Struct from global_msgs.msg import Steering +from global_msgs.msg import CompressedImagePacket from std_msgs.msg import Float32 @@ -35,6 +36,13 @@ def __init__(self, scheme: int): self.arm_vel_pub = self.create_publisher( Float32, 'target_arm_velocity', 10 ) + self.camera_image_buffer = bytearray() + self.camera_listener = self.create_subscription( + CompressedImagePacket, + 'compressed_image', + self.receive_image, + 10 + ) self.scheme = scheme if scheme == ControlScheme.ARCHIMEDES: @@ -48,6 +56,9 @@ def __init__(self, scheme: int): self.thr = Thread(target=self.run) self.thr.start() + def receive_image(self, img: CompressedImagePacket): + self.camera_image_buffer += img.data + def run(self): logger = self.get_logger() host = enet.Host(None, 1, 0, 0, 0) @@ -108,6 +119,11 @@ def run(self): logger.error("Host has returned an error! Restarting...") break + peer.send( + Channels.CAMERA, + enet.Packet(self.camera_image_buffer, enet.PACKET_FLAG_UNSEQUENCED) + ) + def on_receive(self, channel: int, data: bytes, peer) -> None: logger = self.get_logger()