Raspberrypi

workspace/.gitignore

@@ -1,5 +1,4 @@
 build/**
 install/**
 log/**
-.cache/
 **/__pycache__/**

workspace/src/build/.built_by

@@ -0,0 +1 @@
+colcon

workspace/src/build/swarm/build/lib/swarm/dist.py

@@ -0,0 +1,94 @@
+#!/usr/bin/env python
+
+import rclpy
+from rclpy.node import Node
+from geometry_msgs.msg import PointStamped
+from sensor_msgs.msg import NavSatFix
+from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
+import math
+from pyproj import Geod
+
+class DronePointCalculator(Node):
+    def __init__(self):
+        super().__init__('drone_point_calculator')
+        self.g = Geod(ellps='clrk66')
+        qos_profile = QoSProfile(
+            reliability=ReliabilityPolicy.BEST_EFFORT,
+            history=HistoryPolicy.KEEP_LAST,
+            depth=10
+        )
+
+        self.drone1_topic = "/uav1/global_position/global"
+        self.drone2_topic = "/uav2/global_position/global"
+        self.output_topic = "/offset"
+
+        self.drone1_pose = None
+        self.drone2_pose = None
+
+        self.drone1_sub = self.create_subscription(
+            NavSatFix,
+            self.drone1_topic,
+            self.drone1_callback,
+            qos_profile
+        )
+
+        self.drone2_sub = self.create_subscription(
+            NavSatFix,
+            self.drone2_topic,
+            self.drone2_callback,
+            qos_profile
+        )
+        self.result_publisher = self.create_publisher(
+            PointStamped,
+            self.output_topic,
+            10
+        )
+
+        self.time_period = 0.1
+        self.timer = self.create_timer(self.time_period, self.timer_callback)
+
+        self.get_logger().info(f"Subscribing to drone 1 pose on: {self.drone1_topic}")
+        self.get_logger().info(f"Subscribing to drone 2 pose on: {self.drone2_topic}")
+        self.get_logger().info(f"Publishing calculated point to: {self.output_topic}")
+
+    def drone1_callback(self, msg):
+        self.drone1_pose = msg
+
+    def drone2_callback(self, msg):
+        self.drone2_pose = msg
+
+    def timer_callback(self):
+        if self.drone1_pose and self.drone2_pose:
+
+            pos1 = self.drone1_pose
+            pos2 = self.drone2_pose
+
+            result_point_msg = PointStamped()
+            a1,a2,d = self.g.inv(pos1.longitude,pos1.latitude,pos2.longitude,pos2.latitude)
+
+            result_point_msg.point.x = d*math.cos(a1)
+            result_point_msg.point.y = d*math.sin(a1)
+
+            if result_point_msg:
+                self.result_publisher.publish(result_point_msg)
+
+        elif not self.drone1_pose:
+            self.get_logger().warn("Waiting for pose from drone 1...")
+        elif not self.drone2_pose:
+            self.get_logger().warn("Waiting for pose from drone 2...")
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    calculator_node = DronePointCalculator()
+
+    try:
+        rclpy.spin(calculator_node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        calculator_node.destroy_node()
+        rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()

workspace/src/build/swarm/build/lib/swarm/drone_sync.py

@@ -0,0 +1,52 @@
+#!/usr/bin/env python3
+import rclpy
+from rclpy.node import Node
+from mavros_msgs.msg import HomePosition
+from mavros_msgs.srv import CommandHome
+from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
+
+class HomeSync(Node):
+    def __init__(self):
+        super().__init__('home_sync')
+        qos_profile = QoSProfile(
+            reliability=ReliabilityPolicy.BEST_EFFORT,
+            history=HistoryPolicy.KEEP_LAST,
+            depth=10
+        )
+        self.client = self.create_client(CommandHome, '/uav2/cmd/set_home')
+        while not self.client.wait_for_service(timeout_sec=1.0):
+            self.get_logger().info('Waiting for /uav2/cmd/set_home...')
+        self.sub = self.create_subscription(
+            HomePosition,
+            '/uav1/home_position/home',
+            self.callback,
+            qos_profile
+        )
+        self.get_logger().info('Listening for UAV1 home position...')
+
+    def callback(self, msg):
+        req = CommandHome.Request()
+        req.current_gps = False
+        req.latitude = msg.geo.latitude
+        req.longitude = msg.geo.longitude
+        req.altitude = msg.geo.altitude
+        self.get_logger().info(
+            f'Setting UAV2 home to: ({req.latitude:.6f}, {req.longitude:.6f}, {req.altitude:.2f})'
+        )
+        future = self.client.call_async(req)
+        rclpy.spin_until_future_complete(self, future)
+        if future.result() and future.result().success:
+            self.get_logger().info('UAV2 home position successfully updated.')
+        else:
+            self.get_logger().error('Failed to set UAV2 home position.')
+
+def main():
+    rclpy.init()
+    node = HomeSync()
+    rclpy.spin(node)
+    node.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()
+

workspace/src/object_tracker/object_tracker/tracker.py → workspace/src/build/swarm/build/lib/swarm/follow.py

@@ -1,14 +1,11 @@
 import rclpy
+import os
 from rclpy.node import Node
-from sensor_msgs.msg import Image
-from std_msgs.msg import Int32MultiArray
-from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
-from geometry_msgs.msg import TwistStamped,PoseStamped
+from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy,DurabilityPolicy
+from geometry_msgs.msg import PoseStamped,PointStamped
 from mavros_msgs.msg import State
 from mavros_msgs.srv import CommandBool, SetMode, CommandTOL
 import numpy as np
-import cv2
-from object_tracker.hand_detection import get_state
 from scipy.spatial.transform import Rotation as R
 
 def quaternion_to_euler(x, y, z, w):
@@ -24,52 +21,75 @@ def rotate_vector(vec,theta):
     return rot_matrix @ vec
 
 
-class ImageViewer(Node):
+class Follower(Node):
     def __init__(self):
-        super().__init__('hand_tracker')
+        super().__init__('follower')
 
         qos = QoSProfile(
             reliability=ReliabilityPolicy.BEST_EFFORT,
             history=HistoryPolicy.KEEP_LAST,
             depth=10
         )
-        self.namespace="uav1"
+        gps_qos_profile = QoSProfile(
+            reliability=ReliabilityPolicy.RELIABLE,
+            durability=DurabilityPolicy.TRANSIENT_LOCAL,  # Critical!
+            history=HistoryPolicy.KEEP_LAST,
+            depth=10
+        )
+
+
+        self.namespace=f"uav{os.environ.get('MAV_ID')}"
+        self.get_logger().info(f"namespace {self.namespace}")
+        self.z_pos=0.0
         self.altitude = 0.0
         self.stage = 0
         self.roll = 0
         self.pitch = 0
         self.yaw = 0
+        self.offset =None
 
         self.state = State()
-        self.cam_topic = "/rpi_cam/image_raw"
-        self.cmd = "HOLD"
-
-        self.img_sub = self.create_subscription(
-            Image,
-            self.cam_topic,
-            self.listener_callback,
-            10
-        )
-        self.vel_pub = self.create_publisher(
-            TwistStamped,
-            f'/{self.namespace}/setpoint_velocity/cmd_vel',
-            qos
-        )
         self.pos_pub = self.create_publisher(
             PoseStamped,
             f'/{self.namespace}/setpoint_position/local',
             qos
         )
+        self.pos_sub = self.create_subscription(
+            PointStamped,
+            f'/{self.namespace}/desired_pos',
+            self.pos_callback,
+            qos
+        )
         self.create_subscription(State, f'/{self.namespace}/state', self.state_cb, 10)
         self.create_subscription(PoseStamped, f'/{self.namespace}/local_position/pose', self.pos_cb, qos)
 
+        self.offset_topic = f"/{self.namespace}/offset"
+        self.create_subscription(PointStamped, self.offset_topic, self.offset_callback, gps_qos_profile)
 
         self.arming_client = self.create_client(CommandBool, f'/{self.namespace}/cmd/arming')
         self.mode_client = self.create_client(SetMode, f'/{self.namespace}/set_mode')
         self.takeoff_client = self.create_client(CommandTOL, f'/{self.namespace}/cmd/takeoff')
 
         self.timer = self.create_timer(0.1, self.control_loop)
 
+    def offset_callback(self,msg):
+        self.offset = msg
+
+    def pos_callback(self,msg):
+        if self.stage==5:
+            self.get_logger().info(f"Going to {msg.point}")
+            pos_msg = PoseStamped()
+            pos_msg.pose.position.x = msg.point.x- self.offset.point.x
+            pos_msg.pose.position.y = msg.point.y- self.offset.point.y
+            pos_msg.pose.position.z = 3.0
+            self.pos_pub.publish(pos_msg)
+        elif self.offset is None:
+            self.get_logger().warn(f"{self.namespace} offset not found")
+        else:
+            pass
+            # self.get_logger().info(f"{self.namespace} going {msg} ")
+
+
     def control_loop(self):
 
         # ---------- WAIT FOR FCU ----------
@@ -151,7 +171,7 @@ def control_loop(self):
             elapsed = (self.get_clock().now() - self.stage_start_time).nanoseconds / 1e9
             self.get_logger().info(f'Hovering: {elapsed:.1f} s')
             if elapsed >=10.0:
-                self.get_logger().info('Starting velocity control...')
+                self.get_logger().info('Started following')
                 self.stage_start_time = self.get_clock().now()
                 self.stage = 5
 
@@ -172,32 +192,15 @@ def control_loop(self):
 
                 self.pos_pub.publish(pose)
 
-        # ---------- STAGE 4: ----------
         elif self.stage == 5:
-            msg = TwistStamped()
-            msg.twist.linear.x = 0.0
-            msg.twist.linear.y = 0.0
-            msg.twist.linear.z = 0.0
-
-            msg.twist.angular.x = 0.0
-            msg.twist.angular.y = 0.0
-            msg.twist.angular.z = 0.0
-
-            if self.cmd == "HOLD":
+            elapsed = (self.get_clock().now() - self.stage_start_time).nanoseconds / 1e9
+            self.get_logger().info(f'Following: {elapsed:.1f} s')
+            if elapsed >=60.0:
+                self.get_logger().info('Following End')
                 pass
-            if self.cmd == "LEFT":
-                dir = np.array([[0],[1]])
-                vec = rotate_vector(dir,self.yaw) * 0.1
-                msg.twist.linear.x= float(vec[0])
-                msg.twist.linear.y= float(vec[1])
-            if self.cmd == "RIGHT":
-                dir = np.array([[0],[-1]])
-                vec = rotate_vector(dir,self.yaw) * 0.1
-                msg.twist.linear.x= float(vec[0])
-                msg.twist.linear.y= float(vec[1])
-
-
-            self.vel_pub.publish(msg)
+        elif self.stage == 6:
+            pass
+
     def state_cb(self, msg):
         self.state = msg
 
@@ -228,27 +231,9 @@ def vel_cb(self,msg):
         self.yaw_rate   = msg.twist.angular.z
 
 
-
-    def listener_callback(self,msg):
-        raw_data = np.frombuffer(msg.data, dtype=np.uint8)
-        # 2. Reshape the data
-        # For NV21/YUV420, the buffer size is (height * 1.5) * width
-        # We need to reshape it to this specific height to allow cvtColor to work
-        height = msg.height
-        width = msg.width
-        yuv_frame = raw_data.reshape((height + height // 2, width))
-
-        # 3. Convert from YUV (NV21) to BGR for OpenCV/MediaPipe
-        frame = cv2.cvtColor(yuv_frame, cv2.COLOR_YUV2RGB_NV21)
-        frame = cv2.flip(frame,-1)
-        # frame = cv2.rotate(frame, cv2.ROTATE_90_COUNTERCLOCKWISE)
-        self.cmd = get_state(frame)
-        print(self.cmd)
-
-
 def main(args=None):
     rclpy.init(args=args)
-    node = ImageViewer()
+    node = Follower()
     try:
         rclpy.spin(node)
     except KeyboardInterrupt: