diff --git a/z1_pro_driver/scripts/gimbal_action.py b/z1_pro_driver/scripts/gimbal_action.py index 2d957c4..6390023 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, 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 + +from smarc_utilities.georef_utils import convert_latlon_to_utm @@ -24,9 +31,28 @@ 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 + + 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) + + 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) + 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 @@ -48,7 +74,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, @@ -59,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, @@ -93,7 +119,14 @@ def __init__(self, node: Node): self.log(f"GimbalActionServer initialized.") + 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 @@ -102,9 +135,88 @@ 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 + + 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: + 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 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 + + # (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 + + self._rpy_publisher.publish(desired_rpy) + self.log(f"Published RPY: {desired_rpy}") def log(self, msg:str): @@ -137,6 +249,63 @@ 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 _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()