From 8fc53dbb9d695b89601a2f4330d76306854198aa Mon Sep 17 00:00:00 2001 From: Niklas Date: Mon, 11 May 2026 21:24:00 +0200 Subject: [PATCH 1/3] Added geopoint POI to action server. not tested yet --- z1_pro_driver/scripts/gimbal_action.py | 114 +++++++++++++++++++++++-- 1 file changed, 108 insertions(+), 6 deletions(-) diff --git a/z1_pro_driver/scripts/gimbal_action.py b/z1_pro_driver/scripts/gimbal_action.py index 2d957c4..029c5bd 100755 --- a/z1_pro_driver/scripts/gimbal_action.py +++ b/z1_pro_driver/scripts/gimbal_action.py @@ -7,11 +7,18 @@ from rclpy.executors import Future, MultiThreadedExecutor from geographic_msgs.msg import GeoPoint -from geometry_msgs.msg import Vector3 +from geometry_msgs.msg import Vector3, PointStamped +from nav_msgs.msg import Odometry +from tf2_ros import Buffer, TransformListener +import math from smarc_action_base.gentler_action_server import GentlerActionServer from z1_pro_msgs.msg import Topics as Z1Topics from z1_pro_msgs.msg import GimbalFeedback +from tf2_geometry_msgs.tf2_geometry_msgs import do_transform_point +from tf_transformations import euler_from_quaternion + +from smarc_utilities.georef_utils import convert_latlon_to_utm @@ -24,6 +31,16 @@ def __init__(self, node: Node): node.declare_parameter("rpy_pub_hz", 10.0) self._rpy_pub_hz : float = node.get_parameter("rpy_pub_hz").get_parameter_value().double_value + node.declare_parameter("odom_topic", "/evolo/smarc/odom") + odom_topic = node.get_parameter("odom_topic").get_parameter_value().string_value + + self._tf_buffer = Buffer() + self._tf_listener = TransformListener(self._tf_buffer, node) + + self.carrier_odom : Odometry = Odometry() #Odom of carrier + self._odom_subscriber = node.create_subscription(Odometry, odom_topic, self.odom_cb, 10) + + self.desired_geopoint_local_coordinates : PointStamped = PointStamped() self.desired_rpy : Vector3 = Vector3() self._rpy_publisher = node.create_publisher(Vector3, Z1Topics.GIMBAL_CMD_TOPIC, 10) @@ -48,7 +65,7 @@ def __init__(self, node: Node): self._geopoint_as = GentlerActionServer( self._node, "gimbal_set_geopoint", - lambda goal_request: self._node.get_logger().warn("Geopoint action not implemented yet") and False, + self._on_goal_received_geopoint, lambda: True, lambda: None, lambda: True, @@ -93,7 +110,8 @@ def __init__(self, node: Node): self.log(f"GimbalActionServer initialized.") - + def odom_cb(self, msg:Odometry): + self.carrier_odom = msg def publish_rpy_and_fb(self): self.feedback.gimbal_mode = self.tracking_mode @@ -102,9 +120,50 @@ def publish_rpy_and_fb(self): if self.tracking_mode == GimbalFeedback.GIMBAL_MODE_OFF: self.log("Gimbal is off, not publishing RPY commands.") return - - self._rpy_publisher.publish(self.desired_rpy) - self.log(f"Published desired RPY: {self.desired_rpy}") + elif self.tracking_mode == GimbalFeedback.GIMBAL_MODE_GEOPOINT: + + # (1) bearing between evolo and POI + dx = self.desired_geopoint_local_coordinates.point.x - self.carrier_odom.pose.pose.position.x + dy = self.desired_geopoint_local_coordinates.point.y - self.carrier_odom.pose.pose.position.y + poi_bearing = math.atan2(dy,dx) + + # (2) carrier heading + # get pitch roll yaw from quaternion + orientation_q = self.carrier_odom.pose.pose.orientation + orientation_list = [orientation_q.x, orientation_q.y, orientation_q.z, orientation_q.w] + (roll, pitch, carrier_yaw) = euler_from_quaternion(orientation_list) + + # (3) calcualte pitch + carrier_altitude = self.carrier_odom.pose.pose.position.z + poi_altitude = self.desired_geopoint_local_coordinates.point.z + distance = math.sqrt(dx*dx + dy*dy) + target_pitch_deg = math.degrees(math.atan2(poi_altitude - carrier_altitude, distance)) + + # (4) Calculate camera angle needed for looking at the POI + target_yaw = carrier_yaw - poi_bearing + target_yaw_deg = math.degrees(target_yaw) + #Unwrap + if(target_yaw_deg < -180): + target_yaw_deg += 360 + if(target_yaw_deg > 180): + target_yaw_deg -=360 + + desired_rpy : Vector3 = Vector3() + desired_rpy.x = 0.0 #roll + desired_rpy.y = target_pitch_deg + desired_rpy.z = target_yaw_deg + + elif self.tracking_mode == GimbalFeedback.GIMBAL_MODE_ODOM_POI: + #TODO calculate rpy for POI + return + elif self.tracking_mode == GimbalFeedback.GIMBAL_MODE_IMG_POI: + #TODO calculate rpy for POI + return + else: # RPY + desired_rpy = self.desired_rpy + + self._rpy_publisher.publish(desired_rpy) + self.log(f"Published RPY: {desired_rpy}") def log(self, msg:str): @@ -137,6 +196,49 @@ def _on_goal_received_rpy(self, goal_request: dict) -> bool: self.log("Invalid value in goal request") return False + def _on_goal_received_geopoint(self, goal_request: dict) -> bool: + """ + float64 latitude # [deg] + float64 longitude # [deg] + float64 altitude # [m] + """ + self.log(f"Received RPY goal: {goal_request}") + try: + geopoint = GeoPoint() + geopoint.latitude = float(goal_request["latitude"]) + geopoint.longitude = float(goal_request["longitude"]) + geopoint.altitude = float(goal_request["altitude"]) + + #Get point in UTM + geopoint_local_coordinates :PointStamped = convert_latlon_to_utm(geopoint) #pointStamped + + #transform to odom frame + transform = self._tf_buffer.lookup_transform( + self.carrier_odom.header.frame_id, + geopoint_local_coordinates.header.frame_id, + rclpy.time.Time() + ) + + self.desired_geopoint_local_coordinates = do_transform_point( + geopoint_local_coordinates, + transform + ) + + self._node.get_logger().info(f"Converted POI to local coordinates: {self.desired_geopoint_local_coordinates.point.x} ,\ + {self.desired_geopoint_local_coordinates.point.y}, \ + {self.desired_geopoint_local_coordinates.point.z}, \ + {self.desired_geopoint_local_coordinates.header.frame_id}") + + + self.tracking_mode = GimbalFeedback.GIMBAL_MODE_GEOPOINT + return True + except KeyError as e: + self.log("Missing key in goal request") + return False + except ValueError as e: + self.log("Invalid value in goal request") + return False + def main(): rclpy.init() From bc8928c260f9ed799adc6d880ffe815f46416661 Mon Sep 17 00:00:00 2001 From: Niklas Date: Tue, 12 May 2026 21:04:00 +0200 Subject: [PATCH 2/3] Added poi tracking from quaternion message --- z1_pro_driver/scripts/gimbal_action.py | 80 ++++++++++++++++++++++++-- 1 file changed, 74 insertions(+), 6 deletions(-) diff --git a/z1_pro_driver/scripts/gimbal_action.py b/z1_pro_driver/scripts/gimbal_action.py index 029c5bd..b9138e4 100755 --- a/z1_pro_driver/scripts/gimbal_action.py +++ b/z1_pro_driver/scripts/gimbal_action.py @@ -7,14 +7,14 @@ from rclpy.executors import Future, MultiThreadedExecutor from geographic_msgs.msg import GeoPoint -from geometry_msgs.msg import Vector3, PointStamped +from geometry_msgs.msg import Vector3, PointStamped, QuaternionStamped from nav_msgs.msg import Odometry from tf2_ros import Buffer, TransformListener import math from smarc_action_base.gentler_action_server import GentlerActionServer from z1_pro_msgs.msg import Topics as Z1Topics -from z1_pro_msgs.msg import GimbalFeedback +from z1_pro_msgs.msg import GimbalFeedback, Gcudata from tf2_geometry_msgs.tf2_geometry_msgs import do_transform_point from tf_transformations import euler_from_quaternion @@ -34,6 +34,9 @@ def __init__(self, node: Node): node.declare_parameter("odom_topic", "/evolo/smarc/odom") odom_topic = node.get_parameter("odom_topic").get_parameter_value().string_value + node.declare_parameter("img_poi_topic", "/yolo/tracked_poi") + img_poi_topic = node.get_parameter("img_poi_topic").get_parameter_value().string_value + self._tf_buffer = Buffer() self._tf_listener = TransformListener(self._tf_buffer, node) @@ -44,6 +47,12 @@ def __init__(self, node: Node): self.desired_rpy : Vector3 = Vector3() self._rpy_publisher = node.create_publisher(Vector3, Z1Topics.GIMBAL_CMD_TOPIC, 10) + self.gcu_feedback = Gcudata() + self.gcu_feedback_subscriber = node.create_subscription(Gcudata, Z1Topics.GIMBAL_GCU_FB_TOPIC, self.gcu_cb, 10) + + self.image_poi = None + self.image_poi_subscriber = node.create_subscription(QuaternionStamped, img_poi_topic, self.image_poi_cb, 10) + self.feedback : GimbalFeedback = GimbalFeedback() self._feedback_publisher = node.create_publisher(GimbalFeedback, Z1Topics.GIMBAL_FB_TOPIC, 10) self.tracking_mode : str = GimbalFeedback.GIMBAL_MODE_OFF @@ -76,7 +85,7 @@ def __init__(self, node: Node): self._track_img_poi_as = GentlerActionServer( self._node, "gimbal_track_img_poi", - lambda goal_request: self._node.get_logger().warn("Image POI tracking action not implemented yet") and False, + self._on_goal_received_img_poi, lambda: True, lambda: None, lambda: True, @@ -110,9 +119,15 @@ def __init__(self, node: Node): self.log(f"GimbalActionServer initialized.") - def odom_cb(self, msg:Odometry): + def odom_cb(self, msg: Odometry): self.carrier_odom = msg + def image_poi_cb(self, msg: QuaternionStamped): + self.image_poi = msg + + def gcu_cb(self, msg: Gcudata): + self.gcu_feedback = msg + def publish_rpy_and_fb(self): self.feedback.gimbal_mode = self.tracking_mode self._feedback_publisher.publish(self.feedback) @@ -153,12 +168,51 @@ def publish_rpy_and_fb(self): desired_rpy.y = target_pitch_deg desired_rpy.z = target_yaw_deg + return + elif self.tracking_mode == GimbalFeedback.GIMBAL_MODE_ODOM_POI: #TODO calculate rpy for POI return + elif self.tracking_mode == GimbalFeedback.GIMBAL_MODE_IMG_POI: - #TODO calculate rpy for POI - return + if(self.image_poi != None): #we have received a new tracking location + #(1) Calculate angle we need to turn the camera to face the POI) + + orientation_q = self.image_poi.quaternion + orientation_list = [orientation_q.x, orientation_q.y, orientation_q.z, orientation_q.w] + (roll, pitch, yaw) = euler_from_quaternion(orientation_list) + + pitch = math.degrees(pitch) + yaw = math.degrees(yaw) + + #Slow down the movements of the camera + pitch /= 4.0 + yaw /= 4.0 + + new_pitch = self.gcu_feedback.relative_pitch - pitch + new_yaw = self.gcu_feedback.relative_yaw + yaw + + # (2) Limit angles to be nice to the camera + IMG_POI_MAX_YAW = 120.0 + IMG_POI_MAX_PITCH = 30.0 + new_yaw = max(-IMG_POI_MAX_YAW, min(IMG_POI_MAX_YAW, new_yaw)) + new_pitch = max(-IMG_POI_MAX_PITCH, min(IMG_POI_MAX_PITCH, new_pitch)) + + self.log(f"POI pitch: {pitch}, POI_YAW: {yaw}") + + + desired_rpy : Vector3 = Vector3() + desired_rpy.x = 0.0 #roll + desired_rpy.y = new_pitch + desired_rpy.z = new_yaw + + #Clear image POI so we don't use it again + self.image_poi = None + else: + #No new tracking information. Don't move camera + return + + else: # RPY desired_rpy = self.desired_rpy @@ -238,6 +292,20 @@ def _on_goal_received_geopoint(self, goal_request: dict) -> bool: except ValueError as e: self.log("Invalid value in goal request") return False + + def _on_goal_received_img_poi(self, goal_request: dict) -> bool: + self.log(f"Received Image track goal: {goal_request}") + try: + #No parameters + self.tracking_mode = GimbalFeedback.GIMBAL_MODE_IMG_POI + return True + except KeyError as e: + self.log("Missing key in goal request") + return False + except ValueError as e: + self.log("Invalid value in goal request") + return False + def main(): rclpy.init() From f713f429118fe02476c8f0b5c818f2899f18f1ce Mon Sep 17 00:00:00 2001 From: Niklas Date: Wed, 13 May 2026 12:52:24 +0200 Subject: [PATCH 3/3] fixed pitch direction and changed how fast the camera moves --- z1_pro_driver/scripts/gimbal_action.py | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/z1_pro_driver/scripts/gimbal_action.py b/z1_pro_driver/scripts/gimbal_action.py index b9138e4..6390023 100755 --- a/z1_pro_driver/scripts/gimbal_action.py +++ b/z1_pro_driver/scripts/gimbal_action.py @@ -182,12 +182,12 @@ def publish_rpy_and_fb(self): orientation_list = [orientation_q.x, orientation_q.y, orientation_q.z, orientation_q.w] (roll, pitch, yaw) = euler_from_quaternion(orientation_list) - pitch = math.degrees(pitch) + pitch = math.degrees(-pitch) yaw = math.degrees(yaw) - #Slow down the movements of the camera - pitch /= 4.0 - yaw /= 4.0 + #Slow down the movements of the camera to make it work even with the delays in the detecion pipeline + pitch *= 0.4 + yaw *= 0.4 new_pitch = self.gcu_feedback.relative_pitch - pitch new_yaw = self.gcu_feedback.relative_yaw + yaw @@ -199,7 +199,6 @@ def publish_rpy_and_fb(self): new_pitch = max(-IMG_POI_MAX_PITCH, min(IMG_POI_MAX_PITCH, new_pitch)) self.log(f"POI pitch: {pitch}, POI_YAW: {yaw}") - desired_rpy : Vector3 = Vector3() desired_rpy.x = 0.0 #roll