#!/usr/bin/python3

# import time, tracback
import math
import rospy
from geometry_msgs.msg import Twist, PoseStamped


class MultiPanFix(object):
    """ emit /lg_navlib/twist messages with 'pan x' where 'zoom' detected """
    def __init__(self):
        self.node = rospy.init_node('zoom_panner')
        self.puber = rospy.Publisher('/navtransform/twist', Twist, queue_size=10)
        self.sub_twist = rospy.Subscriber("/lg_navlib/twist", Twist, self.update_twist)
        self.sub_pose = rospy.Subscriber("/earth/pose", PoseStamped, self.update_pose)
        self.tilt = 0

    def update_pose(self, pose):
        """ update tilt, any other values from pose """
        # if pose.pose.orientation.x != 0.0:
        self.tilt = pose.pose.orientation.x

    def update_twist(self, twist):
        """ fix twist, publish """

        down = min(max(twist.linear.z, -1), 1) * -1
        tiltness = min(1.0, max(0.0, self.tilt / 90))
        print(f"tilt: {self.tilt} tiltness: {tiltness}")

        if twist.linear.z != 0:
            twist.linear.x = twist.linear.x * (1 - tiltness) + down * tiltness
            twist.linear.z *= 1.0 - tiltness
        #twist.linear.x = tiltness * down if twist.linear.z != 0 else twist.linear.x * (1 + tiltness)
        #twist.linear.x *= 1 + tiltness
        #twist.linear.y *= 1 + tiltness
        #twist.linear.x = twist.linear.x * (1.0 if twist.linear.z == 0 else 1 + tiltness)
        self.puber.publish(twist)
        """
        # pan for zoom (won't be used if panning before zoom)
        if abs(twist.linear.z) > self.min_zoom and self.altitude < self.globe_altitude and self.tilt >= 1:
            # no tilting/rotating while zooming.
            twist.angular.z = 0.0
            twist.angular.x = 0.0
            twist.angular.y = 0.0

            # mathit
            #twist.linear.x = (twist.linear.z * self.altitude) / (1080 / (91 - self.tilt))

            # print(f"tilt: {self.tilt}, altitude: {self.altitude}")
            tilt_varying_zoom = (twist.linear.z / 90) * max(((self.tilt + (2 * (self.tilt - 45))) + 15), self.tilt * 0.0001)
            # factor in tilt on altitude multiplier
            tilt_altitude = self.altitude * self.tilt / 90
            twist.linear.x = -tilt_varying_zoom * tilt_altitude
            # print(f'tvz: {tilt_varying_zoom}, ta: {tilt_altitude}, out_pan_x: {twist.linear.x}')
            # test
            #twist.linear.x = -(twist.linear.z / 90) * (self.altitude * self.tilt / 90) * (self.tilt + (self.tilt ** 2 / 90))
            print(f"in_zoom: {twist.linear.z}, out pan x: {twist.linear.x}")

            # zoom less at higher tilts when panning
            if self.tilt < 90 and self.altitude < self.globe_altitude:
                twist.linear.z = 90 * twist.linear.z / (90 - self.tilt)
                print(f"zoom corrected to {twist.linear.z}")

        # correct tilt towards 0 when zooming out past an altitude so earth will be centered on screen
        if twist.linear.z > self.min_zoom and self.altitude > self.globe_altitude and self.tilt > 1:
            twist.angular.y = 0.1   #self.altitude / self.globe_altitude
            print(f"tilt: {self.tilt}, tilt correct: {twist.angular.y}")

        # no zoom over 90 tilt
        if self.tilt >= 90:
            twist.linear.z = -0.00000000001
            print(f'tilt: {self.tilt} so zoom: {twist.linear.z}')

        # publish all changed and unchanged
        print(f"twist out: {twist}")
        self.puber.publish(twist)
        """


if __name__ == '__main__':
    MultiPanFix()
    rospy.spin()

@@ -1155,2348 +1155,33 @@
     
-#twist.linear.x = tiltness * down if twist.linear.z != 0 else twist.linear.x * (1 + tiltness)%0A        #twist.linear.x *= 1 + tiltness%0A        #twist.linear.y *= 1 + tiltness%0A        #twist.linear.x = twist.linear.x * (1.0 if twist.linear.z == 0 else 1 + tiltness)%0A        self.puber.publish(twist)%0A        %22%22%22%0A        # pan for zoom (won't be used if panning before zoom)%0A        if abs(twist.linear.z) %3E self.min_zoom and self.altitude %3C self.globe_altitude and self.tilt %3E= 1:%0A            # no tilting/rotating while zooming.%0A            twist.angular.z = 0.0%0A            twist.angular.x = 0.0%0A            twist.angular.y = 0.0%0A%0A            # mathit%0A            #twist.linear.x = (twist.linear.z * self.altitude) / (1080 / (91 - self.tilt))%0A%0A            # print(f%22tilt: %7Bself.tilt%7D, altitude: %7Bself.altitude%7D%22)%0A            tilt_varying_zoom = (twist.linear.z / 90) * max(((self.tilt + (2 * (self.tilt - 45))) + 15), self.tilt * 0.0001)%0A            # factor in tilt on altitude multiplier%0A            tilt_altitude = self.altitude * self.tilt / 90%0A            twist.linear.x = -tilt_varying_zoom * tilt_altitude%0A            # print(f'tvz: %7Btilt_varying_zoom%7D, ta: %7Btilt_altitude%7D, out_pan_x: %7Btwist.linear.x%7D')%0A            # test%0A            #twist.linear.x = -(twist.linear.z / 90) * (self.altitude * self.tilt / 90) * (self.tilt + (self.tilt ** 2 / 90))%0A            print(f%22in_zoom: %7Btwist.linear.z%7D, out pan x: %7Btwist.linear.x%7D%22)%0A%0A            # zoom less at higher tilts when panning%0A            if self.tilt %3C 90 and self.altitude %3C self.globe_altitude:%0A                twist.linear.z = 90 * twist.linear.z / (90 - self.tilt)%0A                print(f%22zoom corrected to %7Btwist.linear.z%7D%22)%0A%0A        # correct tilt towards 0 when zooming out past an altitude so earth will be centered on screen%0A        if twist.linear.z %3E self.min_zoom and self.altitude %3E self.globe_altitude and self.tilt %3E 1:%0A            twist.angular.y = 0.1   #self.altitude / self.globe_altitude%0A            print(f%22tilt: %7Bself.tilt%7D, tilt correct: %7Btwist.angular.y%7D%22)%0A%0A        # no zoom over 90 tilt%0A        if self.tilt %3E= 90:%0A            twist.linear.z = -0.00000000001%0A            print(f'tilt: %7Bself.tilt%7D so zoom: %7Btwist.linear.z%7D')%0A%0A        # publish all changed and unchanged%0A        print(f%22twist out: %7Btwist%7D%22)%0A        self.puber.publish(twist)%0A        %22%22%22
+self.puber.publish(twist)
 %0A%0A%0Ai

remove comments