From f20c624e586f1186fa1a782ffdef4bf684a51ef1 Mon Sep 17 00:00:00 2001 From: MaybeWilli <51888361+MaybeWilli@users.noreply.github.com> Date: Sun, 10 May 2026 19:51:00 -0400 Subject: [PATCH 1/8] Manual antenna --- .../aruco_board_detector_node.py | 2 +- .../aruco_localizer/camera_publisher_node.py | 2 +- src/Nav/gps/gps/antenna_pointing_node.py | 42 ++++++++++++++----- 3 files changed, 33 insertions(+), 13 deletions(-) diff --git a/src/Nav/aruco_localizer/aruco_localizer/aruco_board_detector_node.py b/src/Nav/aruco_localizer/aruco_localizer/aruco_board_detector_node.py index 684d3441..c9fc91ed 100644 --- a/src/Nav/aruco_localizer/aruco_localizer/aruco_board_detector_node.py +++ b/src/Nav/aruco_localizer/aruco_localizer/aruco_board_detector_node.py @@ -13,7 +13,7 @@ Published Topics: /aruco_boards (interfaces.msg.ArucoBoard) Pose of detected ArUco boards - + /aruco_debug (sensor_msgs.msg.Image) Debug image with detected boards and axes drawn diff --git a/src/Nav/aruco_localizer/aruco_localizer/camera_publisher_node.py b/src/Nav/aruco_localizer/aruco_localizer/camera_publisher_node.py index 9e9e26e7..d20886b9 100644 --- a/src/Nav/aruco_localizer/aruco_localizer/camera_publisher_node.py +++ b/src/Nav/aruco_localizer/aruco_localizer/camera_publisher_node.py @@ -8,7 +8,7 @@ Published Topics: /camera/image_raw (sensor_msgs.msg.Image) Raw camera images - + /camera/camera_info (sensor_msgs.msg.CameraInfo) Camera calibration information diff --git a/src/Nav/gps/gps/antenna_pointing_node.py b/src/Nav/gps/gps/antenna_pointing_node.py index 4de55d23..e65eaa47 100644 --- a/src/Nav/gps/gps/antenna_pointing_node.py +++ b/src/Nav/gps/gps/antenna_pointing_node.py @@ -21,6 +21,14 @@ def __init__(self): self.zero_offset = 0 self.calibrated = False + # manual mode + self.manual_value = 0 + self.manual = False + self.create_subscription(Bool, "/antenna/manual", self.manual_cb, 10) + self.create_subscription( + Float32, "/antenna/manual_value", self.manual_value_cb, 10 + ) + # ROS things self.create_subscription(NavSatFix, "/base_station/fix", self.base_cb, 10) self.create_subscription(NavSatFix, "/gps/fix", self.rover_cb, 10) @@ -38,22 +46,34 @@ def base_cb(self, msg): def rover_cb(self, msg): self.rover_fix = msg - def update(self): - if self.svin_valid == -2 or not self.base_fix or not self.rover_fix: - return + def manual_cb(self, msg): + self.manual = msg.data + + def manual_value_cb(self, msg): + self.manual_value = msg.data + def update(self): # ----- HANDLE IMU HERE # ----- HANDLE CALIBRATION HERE (maybe) - bearing = self.bearing( - self.base_fix.latitude, - self.base_fix.longitude, - self.rover_fix.latitude, - self.rover_fix.longitude, - ) - - self.publish_bearing(bearing) + # if not manual, proceed as usual + if not self.manual: + if self.svin_valid == -2 or not self.base_fix or not self.rover_fix: + return + + bearing = self.bearing( + self.base_fix.latitude, + self.base_fix.longitude, + self.rover_fix.latitude, + self.rover_fix.longitude, + ) + + self.publish_bearing(bearing) + else: # is manual mode + msg = Float32() + msg.data = float(self.manual_value) + self.bearing_pub.publish(msg) def publish_bearing(self, bearing): msg = Float32() From 98b4871a22ade82fd849204db3753178e04e7498 Mon Sep 17 00:00:00 2001 From: MaybeWilli <51888361+MaybeWilli@users.noreply.github.com> Date: Sun, 10 May 2026 21:12:45 -0400 Subject: [PATCH 2/8] Publish --- src/Nav/gps/gps/antenna_pointing_node.py | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/Nav/gps/gps/antenna_pointing_node.py b/src/Nav/gps/gps/antenna_pointing_node.py index e65eaa47..d9b9a552 100644 --- a/src/Nav/gps/gps/antenna_pointing_node.py +++ b/src/Nav/gps/gps/antenna_pointing_node.py @@ -71,9 +71,7 @@ def update(self): self.publish_bearing(bearing) else: # is manual mode - msg = Float32() - msg.data = float(self.manual_value) - self.bearing_pub.publish(msg) + self.publish_bearing(self.manual_value) def publish_bearing(self, bearing): msg = Float32() From 60273daf87563e63f41902686d9007772cb5daa3 Mon Sep 17 00:00:00 2001 From: landwhich Date: Mon, 11 May 2026 20:50:35 -0400 Subject: [PATCH 3/8] just a backup for the automatic control so we don't blast --- src/Nav/gps/gps/antenna_pointing_node.py | 20 ++++++++++++++++++-- 1 file changed, 18 insertions(+), 2 deletions(-) diff --git a/src/Nav/gps/gps/antenna_pointing_node.py b/src/Nav/gps/gps/antenna_pointing_node.py index d9b9a552..328cd072 100644 --- a/src/Nav/gps/gps/antenna_pointing_node.py +++ b/src/Nav/gps/gps/antenna_pointing_node.py @@ -53,7 +53,6 @@ def manual_value_cb(self, msg): self.manual_value = msg.data def update(self): - # ----- HANDLE IMU HERE # ----- HANDLE CALIBRATION HERE (maybe) @@ -69,6 +68,16 @@ def update(self): self.rover_fix.longitude, ) + distance = self.haversine( + self.base_fix.latitude, + self.base_fix.longitude, + self.rover_fix.latitude, + self.rover_fix.longitude, + ) + + if distance < 5.0: + bearing = (bearing + math.pi / 2) % (math.pi * 2) + self.publish_bearing(bearing) else: # is manual mode self.publish_bearing(self.manual_value) @@ -86,7 +95,14 @@ def bearing(self, lat1, lon1, lat2, lon2): x = math.sin(dl) * math.cos(p2) y = math.cos(p1) * math.sin(p2) - math.sin(p1) * math.cos(p2) * math.cos(dl) return (math.atan2(x, y) + math.pi * 2) % (math.pi * 2) - + + def haversine(self, lat1, lon1, lat2, lon2): + R = 6_371_000 # Earth radius in metres, haversine method returns dist in meters + p1, p2 = math.radians(lat1), math.radians(lat2) + dp = math.radians(lat2 - lat1) + dl = math.radians(lon2 - lon1) + a = math.sin(dp / 2)**2 + math.cos(p1) * math.cos(p2) * math.sin(dl / 2)**2 + return 2 * R * math.asin(math.sqrt(a)) def main(args=None): rclpy.init(args=args) From a0107b19f63972546d01b72f846cf9110b29226a Mon Sep 17 00:00:00 2001 From: landwhich Date: Wed, 13 May 2026 22:15:00 -0400 Subject: [PATCH 4/8] Revert "Publish" This reverts commit 98b4871a22ade82fd849204db3753178e04e7498. --- src/Nav/gps/gps/antenna_pointing_node.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/Nav/gps/gps/antenna_pointing_node.py b/src/Nav/gps/gps/antenna_pointing_node.py index 328cd072..7e542fbf 100644 --- a/src/Nav/gps/gps/antenna_pointing_node.py +++ b/src/Nav/gps/gps/antenna_pointing_node.py @@ -80,7 +80,9 @@ def update(self): self.publish_bearing(bearing) else: # is manual mode - self.publish_bearing(self.manual_value) + msg = Float32() + msg.data = float(self.manual_value) + self.bearing_pub.publish(msg) def publish_bearing(self, bearing): msg = Float32() From df09f24db8e47fbfa3c65463457e3b5efb25247e Mon Sep 17 00:00:00 2001 From: landwhich Date: Wed, 13 May 2026 22:17:53 -0400 Subject: [PATCH 5/8] removed the threshold check --- src/Nav/gps/gps/antenna_pointing_node.py | 21 +++------------------ 1 file changed, 3 insertions(+), 18 deletions(-) diff --git a/src/Nav/gps/gps/antenna_pointing_node.py b/src/Nav/gps/gps/antenna_pointing_node.py index 7e542fbf..e9e93ad6 100644 --- a/src/Nav/gps/gps/antenna_pointing_node.py +++ b/src/Nav/gps/gps/antenna_pointing_node.py @@ -13,6 +13,9 @@ def __init__(self): self.declare_parameter("Freq", 10) self.freq = self.get_parameter("Freq").value + self.declare_parameter("AutoThreshold", 10) + self.freq = self.get_parameter("AutoThreshold").value + # State and fix values self.base_fix = None self.rover_fix = None @@ -68,16 +71,6 @@ def update(self): self.rover_fix.longitude, ) - distance = self.haversine( - self.base_fix.latitude, - self.base_fix.longitude, - self.rover_fix.latitude, - self.rover_fix.longitude, - ) - - if distance < 5.0: - bearing = (bearing + math.pi / 2) % (math.pi * 2) - self.publish_bearing(bearing) else: # is manual mode msg = Float32() @@ -97,14 +90,6 @@ def bearing(self, lat1, lon1, lat2, lon2): x = math.sin(dl) * math.cos(p2) y = math.cos(p1) * math.sin(p2) - math.sin(p1) * math.cos(p2) * math.cos(dl) return (math.atan2(x, y) + math.pi * 2) % (math.pi * 2) - - def haversine(self, lat1, lon1, lat2, lon2): - R = 6_371_000 # Earth radius in metres, haversine method returns dist in meters - p1, p2 = math.radians(lat1), math.radians(lat2) - dp = math.radians(lat2 - lat1) - dl = math.radians(lon2 - lon1) - a = math.sin(dp / 2)**2 + math.cos(p1) * math.cos(p2) * math.sin(dl / 2)**2 - return 2 * R * math.asin(math.sqrt(a)) def main(args=None): rclpy.init(args=args) From 4cdf0566d7fdfd4cb5f6ba840dcdfa3ffaa61b04 Mon Sep 17 00:00:00 2001 From: landwhich Date: Wed, 13 May 2026 22:20:19 -0400 Subject: [PATCH 6/8] didn't mean to commit the revert --- src/Nav/gps/gps/antenna_pointing_node.py | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/Nav/gps/gps/antenna_pointing_node.py b/src/Nav/gps/gps/antenna_pointing_node.py index e9e93ad6..6b7cd4c3 100644 --- a/src/Nav/gps/gps/antenna_pointing_node.py +++ b/src/Nav/gps/gps/antenna_pointing_node.py @@ -73,9 +73,7 @@ def update(self): self.publish_bearing(bearing) else: # is manual mode - msg = Float32() - msg.data = float(self.manual_value) - self.bearing_pub.publish(msg) + self.publish_bearing(self.manual_value) def publish_bearing(self, bearing): msg = Float32() From bd0253279a67ef16b107104ed39bad263e65a478 Mon Sep 17 00:00:00 2001 From: Landwhich Date: Thu, 14 May 2026 03:05:13 +0000 Subject: [PATCH 7/8] formatter loves me --- src/Nav/gps/gps/antenna_pointing_node.py | 1 + 1 file changed, 1 insertion(+) diff --git a/src/Nav/gps/gps/antenna_pointing_node.py b/src/Nav/gps/gps/antenna_pointing_node.py index 6b7cd4c3..88f50604 100644 --- a/src/Nav/gps/gps/antenna_pointing_node.py +++ b/src/Nav/gps/gps/antenna_pointing_node.py @@ -89,6 +89,7 @@ def bearing(self, lat1, lon1, lat2, lon2): y = math.cos(p1) * math.sin(p2) - math.sin(p1) * math.cos(p2) * math.cos(dl) return (math.atan2(x, y) + math.pi * 2) % (math.pi * 2) + def main(args=None): rclpy.init(args=args) node = AntennaPointingNode() From 9665113dfb8f5231951edba2a6f3281617c8a47a Mon Sep 17 00:00:00 2001 From: Will Richards <156349813+Landwhich@users.noreply.github.com> Date: Thu, 14 May 2026 16:58:42 -0400 Subject: [PATCH 8/8] rm useless ros param Removed AutoThreshold parameter declaration and assignment. --- src/Nav/gps/gps/antenna_pointing_node.py | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/Nav/gps/gps/antenna_pointing_node.py b/src/Nav/gps/gps/antenna_pointing_node.py index 88f50604..b555daa6 100644 --- a/src/Nav/gps/gps/antenna_pointing_node.py +++ b/src/Nav/gps/gps/antenna_pointing_node.py @@ -13,9 +13,6 @@ def __init__(self): self.declare_parameter("Freq", 10) self.freq = self.get_parameter("Freq").value - self.declare_parameter("AutoThreshold", 10) - self.freq = self.get_parameter("AutoThreshold").value - # State and fix values self.base_fix = None self.rover_fix = None