Add Imaging ROS Node that returns a sample response (#48)

CMakeLists.txt

@@ -53,6 +53,7 @@ install(PROGRAMS
   scripts/demo_commander_node.py
   scripts/trajectory_planner_node.py
   scripts/waypoint_tracker_node.py
+  scripts/imaging_node.py
   DESTINATION lib/${PROJECT_NAME}
 )
 install(DIRECTORY include/${PROJECT_NAME}/

README.md

@@ -48,3 +48,10 @@ I copied a lot of the config from this tutorial: https://docs.ros.org/en/foxy/Ho
 ### Manual
 
 (WIP). It's recommended to use the Dockerfile for development.
+
+### Running Imaging ROS Node
+1. go into `/home/ws`
+2. `source install/setup.sh`
+3. `colcon build --merge-install`
+4. `ros2 run uavf_2024 imaging_node.py` (this starts the service)
+5. from another terminal, run `ros2 service call /imaging_service uavf_2024/srv/TakePicture`

msg/TargetDetection.msg

@@ -1,13 +1,25 @@
 uint64 timestamp
 
-float64 lat
-float64 lon
+float64 x
+float64 y
+float64 z
 
 float64[8] shape_conf 
 # should sum to 1
 # indices in order:
 # circle, semicircle, quarter circle, triangle, rectangle, pentagon, star, cross
-float64[8] color_conf
+
+float64[36] letter_conf
+# should sum to 1
+# indices in order:
+# ABCDEFGHIJKLMNOPQRSTUVWXYZ123456789
+
+float64[8] shape_color_conf
+# should sum to 1
+# indices in order:
+# white, black, red, blue, green, purple, brown, orange
+
+float64[8] letter_color_conf
 # should sum to 1
 # indices in order:
 # white, black, red, blue, green, purple, brown, orange

scripts/imaging_node.py

@@ -0,0 +1,42 @@
+#!/usr/bin/env python3
+
+import rclpy
+from rclpy.node import Node
+from uavf_2024.msg import TargetDetection
+from uavf_2024.srv import TakePicture
+import numpy as np
+
+class ImagingNode(Node):
+    def __init__(self) -> None:
+        super().__init__('imaging_node')
+        self.imaging_service = self.create_service(TakePicture, 'imaging_service', self.imaging_callback)
+
+    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(),
+            )
+        ]
+        return response
+
+def main(args=None) -> None:
+    print('Starting imaging node...')
+    rclpy.init(args=args)
+    node = ImagingNode()
+    rclpy.spin(node)
+    node.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    try:
+        main()
+    except Exception as e:
+        print(e)