Fix Imaging ROS Node (#106)

Co-authored-by: Thomas Neill <[email protected]>

.devcontainer/devcontainer.json

@@ -6,8 +6,8 @@
         "context":".."
 
     },
-    "workspaceFolder": "/home/ws/uavf_2024",
-    "workspaceMount": "source=${localWorkspaceFolder},target=/home/ws/uavf_2024,type=bind",
+    "workspaceFolder": "/home/ws/libuavf_2024",
+    "workspaceMount": "source=${localWorkspaceFolder},target=/home/ws/libuavf_2024,type=bind",
     "containerEnv": {
         // For x86_64
         "DISPLAY": "unix:0",

CMakeLists.txt

@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.5)
-project(uavf_2024)
+project(libuavf_2024)
 
 
 # Default to C99
@@ -43,7 +43,7 @@ set(srv_files
   "srv/TakePicture.srv"
 )
 
-rosidl_generate_interfaces(uavf_2024
+rosidl_generate_interfaces(libuavf_2024
   ${msg_files}
   ${srv_files}
 )
@@ -55,15 +55,11 @@ install(PROGRAMS
   scripts/demo_dropzone_planner.py
   scripts/trajectory_planner_node.py
   scripts/waypoint_tracker_node.py
+  scripts/demo_imaging_node.py
   scripts/imaging_node.py
   scripts/mock_imaging_node.py
   DESTINATION lib/${PROJECT_NAME}
 )
-install(DIRECTORY
-  uavf_2024/gnc
-  uavf_2024/imaging
-  DESTINATION lib/uavf_2024/libuavf_2024
-)
 install(DIRECTORY include/${PROJECT_NAME}/
   DESTINATION include/${PROJECT_NAME}
 )

Dockerfile

@@ -159,6 +159,8 @@ RUN usermod -a -G dialout qgc
 # Sourcing script at runtime
 COPY .devcontainer/bashrc_setup.sh /usr/local/bin/bashrc_setup.sh
 RUN chmod 777 /usr/local/bin/bashrc_setup.sh
-RUN /usr/local/bin/bashrc_setup.sh
+#CMD ["/usr/local/bin/bashrc_setup.sh"]
+# TOTAL HACK, just doing this to avoid an entire Docker rebuild
+RUN mv /home/ws/uavf_2024 /home/ws/libuavf_2024 
 
 CMD ["/bin/bash"]

README.md

@@ -51,7 +51,7 @@ Do this AFTER doing `pip install -e .` If you do that after, it'll overwrite the
 sudo apt-get install libjpeg-dev zlib1g-dev libpython3-dev libopenblas-dev libavcodec-dev libavformat-dev libswscale-dev
 git clone --branch v0.16.1 https://github.com/pytorch/vision torchvision
 cd torchvision
-export BUILD_VERSION = 0.16.1
+export BUILD_VERSION=0.16.1
 python3 setup.py install --user
 ```
 

launches/imaging_demo.launch

@@ -0,0 +1,4 @@
+<launch>
+    <node pkg="libuavf_2024" exec="imaging_node.py" name="imaging_server" />
+    <node pkg="libuavf_2024" exec="demo_imaging_node.py" name="imaging_client" />
+</launch>

msg/TargetDetection.msg

@@ -4,7 +4,7 @@ float64 x
 float64 y
 float64 z
 
-float64[8] shape_conf 
+float64[13] shape_conf 
 # should sum to 1
 # indices in order:
 # circle, semicircle, quarter circle, triangle, rectangle, pentagon, star, cross

package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
-  <name>uavf_2024</name>
+  <name>libuavf_2024</name>
   <version>0.0.0</version>
   <description>TODO: Package description</description>
   <maintainer email="[email protected]">Herpderk</maintainer>

scripts/camera_test_node.py

@@ -0,0 +1,38 @@
+#!/usr/bin/env python3
+
+import rclpy
+from rclpy.node import Node
+from uavf_2024.imaging import Camera
+from time import sleep
+
+class CameraTestNode(Node):
+    def __init__(self) -> None:
+        super().__init__('imaging_node')
+        self.camera = Camera()
+        self.camera.setAbsoluteZoom(1)
+        self.camera.cam.requestAbsolutePosition(0, 0)
+        sleep(2)
+
+    def loop(self):
+        while True:
+            print(self.camera.cam.getAttitude())
+            sleep(1/10)
+            # if self.camera.cam.requestGimbalAttitude():
+            #     attitude = self.camera.cam.getAttitude()
+            #     self.get_logger().info(str(attitude))
+            # else:
+            #     self.get_logger().info(":(")
+            # sleep(1)
+
+
+def main(args=None) -> None:
+    print('Starting imaging node...')
+    rclpy.init(args=args)
+    node = CameraTestNode()
+    node.loop()
+
+if __name__ == '__main__':
+    try:
+        main()
+    except Exception as e:
+        print(e)

scripts/demo_commander_node.py

@@ -1,6 +1,6 @@
 #!/usr/bin/env python3
 
-from libuavf_2024.gnc.commander_node import CommanderNode
+from uavf_2024.gnc.commander_node import CommanderNode
 import mavros_msgs.msg
 import mavros_msgs.srv
 import rclpy
@@ -9,7 +9,7 @@
 from threading import Thread
 import sys
 
-# Command to run: ros2 run uavf_2024 demo_commander_node.py /home/ws/uavf_2024/uavf_2024/gnc/data/TEST_MISSION /home/ws/uavf_2024/uavf_2024/gnc/data/AIRDROP_BOUNDARY /home/ws/uavf_2024/uavf_2024/gnc/data/PAYLOAD_LIST 12 9
+# Command to run: ros2 run libuavf_2024 demo_commander_node.py /home/ws/uavf_2024/uavf_2024/gnc/data/TEST_MISSION /home/ws/uavf_2024/uavf_2024/gnc/data/AIRDROP_BOUNDARY /home/ws/uavf_2024/uavf_2024/gnc/data/PAYLOAD_LIST 12 9
 
 if __name__ == '__main__':
     rclpy.init()

scripts/demo_dropzone_planner.py

@@ -1,14 +1,14 @@
 #!/usr/bin/env python3
 
-from libuavf_2024.gnc.util import read_gps
-from libuavf_2024.gnc.dropzone_planner import DropzonePlanner
-from libuavf_2024.gnc.commander_node import CommanderNode
+from uavf_2024.gnc.util import read_gps
+from uavf_2024.gnc.dropzone_planner import DropzonePlanner
+from uavf_2024.gnc.commander_node import CommanderNode
 from threading import Thread
 import rclpy
 import argparse
 
 
-# example: ros2 run uavf_2024 demo_dropzone_planner.py /home/ws/uavf_2024/uavf_2024/gnc/data/TEST_MISSION /home/ws/uavf_2024/uavf_2024/gnc/data/AIRDROP_BOUNDARY 0 0 0 0 12 9
+# example: ros2 run libuavf_2024 demo_dropzone_planner.py /home/ws/uavf_2024/uavf_2024/gnc/data/TEST_MISSION /home/ws/uavf_2024/uavf_2024/gnc/data/AIRDROP_BOUNDARY 0 0 0 0 12 9
 
 if __name__ == '__main__':
     rclpy.init()

scripts/demo_imaging_node.py

@@ -0,0 +1,36 @@
+#!/usr/bin/env python3
+from libuavf_2024.srv import TakePicture
+import rclpy
+from rclpy.node import Node
+from time import sleep
+
+class DemoImagingClient(Node):
+
+    def __init__(self):
+        super().__init__('demo_imaging_client')
+        self.get_logger().info("Initializing Client")
+        self.cli = self.create_client(TakePicture, 'imaging_service')
+        while not self.cli.wait_for_service(timeout_sec=1.0):
+            self.get_logger().info('service not available, waiting again...')
+        self.get_logger().info("Finished intializing client")
+        self.req = TakePicture.Request()
+        sleep(5)
+        res = self.send_request()
+        self.get_logger().info(str(res.detections))
+
+    def send_request(self):
+        self.get_logger().info("Sending request")
+        self.future = self.cli.call_async(self.req)
+        rclpy.spin_until_future_complete(self, self.future)
+        res = self.future.result()
+        return res
+
+# Command to run: ros2 run uavf_2024 demo_imaging_node.py
+
+if __name__ == '__main__':
+    print('Starting client node...')
+    rclpy.init()
+    node = DemoImagingClient()
+    rclpy.spin(node)
+    node.destroy_node()
+    rclpy.shutdown()

scripts/imaging_node.py

@@ -2,28 +2,57 @@
 
 import rclpy
 from rclpy.node import Node
-from uavf_2024.msg import TargetDetection
-from uavf_2024.srv import TakePicture
+from libuavf_2024.msg import TargetDetection
+from libuavf_2024.srv import TakePicture
+from uavf_2024.imaging import Camera, ImageProcessor, Localizer
 import numpy as np
+from time import strftime, time
 
 class ImagingNode(Node):
     def __init__(self) -> None:
         super().__init__('imaging_node')
         self.imaging_service = self.create_service(TakePicture, 'imaging_service', self.imaging_callback)
-
+        self.camera = Camera()
+        self.camera.setAbsoluteZoom(1)
+        self.image_processor = ImageProcessor(f'logs/{strftime("%m-%d %H:%M")}')
+        self.localizer = Localizer(30, (1920, 1080))
+        self.get_logger().info("Finished initializing imaging node")
+
     def imaging_callback(self, request, response: list[TargetDetection]):
-        response.detections = [
-            TargetDetection(
-                timestamp = 69420,
-                x = 1.0,
-                y = 2.0,
-                z = 3.0,
-                shape_conf = np.zeros(8).tolist(),
-                letter_conf = np.zeros(36).tolist(),
-                shape_color_conf = np.zeros(8).tolist(),
-                letter_color_conf = np.zeros(8).tolist(),
+        self.get_logger().info("Received Request")
+        self.camera.request_center()
+        self.camera.request_autofocus()
+        img = self.camera.take_picture()
+        timestamp = time()
+
+        self.get_logger().info("Picture taken")
+
+        detections = self.image_processor.process_image(img)
+
+        self.get_logger().info("Images processed")
+
+        cam_pose = np.array([0,0,0,0,0,0])
+        preds_3d = [self.localizer.prediction_to_coords(d, cam_pose) for d in detections]
+
+        self.get_logger().info("Localization finished")
+
+        response.detections = []
+
+        for i, p in enumerate(preds_3d):
+            t = TargetDetection(
+                timestamp = int(timestamp*1000),
+                x = p.position[0],
+                y = p.position[1],
+                z = p.position[2],
+                shape_conf = p.description.shape_probs.tolist(),
+                letter_conf = p.description.letter_probs.tolist(),
+                shape_color_conf = p.description.shape_col_probs.tolist(),
+                letter_color_conf = p.description.letter_col_probs.tolist()
             )
-        ]
+
+            response.detections.append(t)
+
+        self.get_logger().info("Returning Response")
         return response
 
 def main(args=None) -> None:

scripts/mock_imaging_node.py

@@ -1,6 +1,6 @@
 #!/usr/bin/env python3
 
-# example usage: ros2 run uavf_2024 mock_imaging_node.py /home/ws/uavf_2024/uavf_2024/gnc/data/AIRDROP_BOUNDARY 12 9
+# example usage: ros2 run libuavf_2024 mock_imaging_node.py /home/ws/libuavf_2024/uavf_2024/gnc/data/AIRDROP_BOUNDARY 12 9
 
 # generates and mocks 5 unique targets
 
@@ -11,11 +11,11 @@
 
 import rclpy
 from rclpy.node import Node
-from uavf_2024.msg import TargetDetection
-from uavf_2024.srv import TakePicture
+from libuavf_2024.msg import TargetDetection
+from libuavf_2024.srv import TakePicture
 from geometry_msgs.msg import PoseStamped
 from rclpy.qos import QoSProfile, ReliabilityPolicy, DurabilityPolicy, HistoryPolicy
-from libuavf_2024.gnc.util import read_gps, convert_delta_gps_to_local_m
+from uavf_2024.gnc.util import read_gps, convert_delta_gps_to_local_m
 from scipy.spatial.transform import Rotation as R
 import numpy as np
 import argparse

sim_instructions.md

@@ -39,12 +39,12 @@ cd /home/ws && colcon build --merge-install && source install/setup.bash
 
 Launch the mock imaging node:
 ```
-ros2 run uavf_2024 mock_imaging_node.py /home/ws/uavf_2024/uavf_2024/gnc/data/AIRDROP_BOUNDARY 12 9
+ros2 run libuavf_2024 mock_imaging_node.py /home/ws/libuavf_2024/uavf_2024/gnc/data/AIRDROP_BOUNDARY 12 9
 ```
 
 Launch the demo commander node:
 ```
-ros2 run uavf_2024 demo_commander_node.py /home/ws/uavf_2024/uavf_2024/gnc/data/TEST_MISSION /home/ws/uavf_2024/uavf_2024/gnc/data/AIRDROP_BOUNDARY /home/ws/uavf_2024/uavf_2024/gnc/data/PAYLOAD_LIST 12 9
+ros2 run libuavf_2024 demo_commander_node.py /home/ws/libuavf_2024/uavf_2024/gnc/data/TEST_MISSION /home/ws/uavf_2024/libuavf_2024/gnc/data/AIRDROP_BOUNDARY /home/ws/libuavf_2024/uavf_2024/gnc/data/PAYLOAD_LIST 12 9
 ```
 
 This will execute one lap of the mission in SITL.

srv/TakePicture.srv

@@ -1,2 +1,2 @@
 ---
-uavf_2024/TargetDetection[] detections
+libuavf_2024/TargetDetection[] detections

tests/imaging/image_processor_tests.py

@@ -132,8 +132,11 @@ def test_runs_without_crashing(self):
 
     @profiler
     def test_benchmark_fullsize_images(self):
-        image_processor = ImageProcessor()
-        sample_input = Image.from_file(f"{CURRENT_FILE_PATH}/imaging_data/fullsize_dataset/images/image0.png")
+        image_processor = ImageProcessor(
+            shape_batch_size=20,
+            letter_batch_size=30
+        )
+        sample_input = Image.from_file(f"{CURRENT_FILE_PATH}/imaging_data/fullsize_dataset/images/5k.png")
         times = []
         N_runs = 10
         for i in tqdm(range(N_runs)):
@@ -143,8 +146,8 @@ def test_benchmark_fullsize_images(self):
             times.append(elapsed)
         print(f"Fullsize image benchmarks (average of {N_runs} runs):")
         print(f"Avg: {np.mean(times)}, StdDev: {np.std(times)}")
-        lstats = profiler.get_stats()
-        line_profiler.show_text(lstats.timings, lstats.unit)
+        # lstats = profiler.get_stats()
+        # line_profiler.show_text(lstats.timings, lstats.unit)
 
     def test_no_duplicates(self):
         # Given 5 identified bounding boxes, removes duplicate bounding box using nms such that there are 4 bounding boxes left

uavf_2024/gnc/commander_node.py

@@ -6,9 +6,9 @@
 from rclpy.qos import QoSProfile, ReliabilityPolicy, DurabilityPolicy, HistoryPolicy
 import sensor_msgs.msg
 import geometry_msgs.msg 
-import uavf_2024.srv
-from libuavf_2024.gnc.util import read_gps, convert_delta_gps_to_local_m, convert_local_m_to_delta_gps, calculate_turn_angles_deg, read_payload_list
-from libuavf_2024.gnc.dropzone_planner import DropzonePlanner
+import libuavf_2024.srv
+from uavf_2024.gnc.util import read_gps, convert_delta_gps_to_local_m, convert_local_m_to_delta_gps, calculate_turn_angles_deg, read_payload_list
+from uavf_2024.gnc.dropzone_planner import DropzonePlanner
 from scipy.spatial.transform import Rotation as R
 import time
 

uavf_2024/gnc/dropzone_planner.py

@@ -1,5 +1,5 @@
 from typing import List, Tuple
-from libuavf_2024.gnc.util import convert_local_m_to_delta_gps
+from uavf_2024.gnc.util import convert_local_m_to_delta_gps
 import numpy as np
 import math
 

uavf_2024/gnc/util.py

@@ -1,4 +1,4 @@
-from libuavf_2024.gnc.payload import Payload
+from uavf_2024.gnc.payload import Payload
 from geographiclib.geodesic import Geodesic
 import numpy as np
 

uavf_2024/imaging/__init__.py

@@ -1,2 +1,6 @@
 import line_profiler
-profiler = line_profiler.LineProfiler()
+profiler = line_profiler.LineProfiler()
+
+from .image_processor import ImageProcessor
+from .localizer import Localizer
+from .camera import Camera