Zadanie 2 - warsztaty ros

#!/usr/bin/env python3
from __future__ import annotations
from tf.transformations import euler_from_quaternion
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Twist

import rospy
import numpy as np

from kalman_workshops_utils.srv import SimpleRotation, SimpleRotationResponse

def yaw_from_odom(odom: Odometry) -> float:
    '''
    Wyciaga obrót wokół osi Z z Odometry msg 
    '''
    quaternion = (
        odom.pose.pose.orientation.x,
        odom.pose.pose.orientation.y,
        odom.pose.pose.orientation.z,
        odom.pose.pose.orientation.w)

    euler = euler_from_quaternion(quaternion)
    yaw = euler[2]

    return np.rad2deg(yaw)

def pick_shorter_distance(angle1_deg: float, angle2_deg: float) -> float:
    '''
    Wybiera krótszy dystans kątowy między dwoma orientacjami w płaszczyźnie XY
    '''
    dist = angle2_deg - angle1_deg
    if np.abs(dist) > 180:
        dist = dist - np.sign(dist) * 360
    
    return dist


odometry_msg = Odometry()


def update_odom(msg):
    global odometry_msg
    odometry_msg = msg

def rotate(req):
    global odometry_msg
    # kąt o jaki chcemy przejechać - pochodzi z requesta
    angle = req.degrees

    previous_yaw = yaw_from_odom(odometry_msg)

    # sumaryczny przebyty kąt
    travelled_angle = 0

    pub = rospy.Publisher("/kalman/cmd_vel", Twist, queue_size=1)

    # kręcimy się dopóki sumaryczny kąt jest mniejszy niż
    # kaß docelowy
    while (abs(travelled_angle) < angle):
        # publikowanie komendy preðkości
        t = Twist()
        t.angular.z = 1.0 * np.sign(angle)
        pub.publish(t)

        # inkrementacja przejechanego kąta
        current_yaw = yaw_from_odom(odometry_msg)

        # wybranie krótszego kąta, żeby było okej w 
        # przypadku przeskoku pomiędzy 180 i -180
        travelled_angle += pick_shorter_distance(previous_yaw, current_yaw)
        previous_yaw = current_yaw
        
        print(travelled_angle)

        rospy.sleep(0.1)
    return SimpleRotationResponse(success=True)


if __name__ == "__main__":
    rospy.init_node('rotate')
    # deklaracja serwera 
    s = rospy.Service("/rotate", SimpleRotation, rotate)
    # deklaracja subskrybenta pobierajacego dane o położeniu łazika
    odom_sub = rospy.Subscriber("/odometry/filtered", Odometry, update_odom)
    rospy.spin()