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..b555daa6 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,31 @@ 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 - # ----- HANDLE IMU HERE + def manual_value_cb(self, msg): + self.manual_value = msg.data - # ----- HANDLE CALIBRATION HERE (maybe) + def update(self): - bearing = self.bearing( - self.base_fix.latitude, - self.base_fix.longitude, - self.rover_fix.latitude, - self.rover_fix.longitude, - ) + # ----- HANDLE CALIBRATION HERE (maybe) - 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 + self.publish_bearing(self.manual_value) def publish_bearing(self, bearing): msg = Float32()