After migration from Kinetic to Melodic my script doesn't work

edit

Hi everyone. I'm new in ROS, sorry for my ignorance. I wrote a script in Kinetic that implements a trajectory tracking (track: y=0) for my unicicle in Gazebo. My script subscribe /odom and convert in rpy angles. Then, publishes /cmd_vel with a specific angular rate. In Kinetic, my robot follows the line correctly. After migration in Melodic, unicicle doesn't follow the line and goes round.

#!/usr/bin/env python

import rospy
import math
import numpy
from geometry_msgs.msg import Twist
from nav_msgs.msg import Odometry
import sys

class Vector:

    def __init__(self, x, y, z, name):
        self.x=x
        self.y=y
        self.z=z
        self.name=name

    #print vector
    def print_v(self):
        rospy.loginfo(self.name + ": [%f, %f, %f]\n", self.x, self.y, self.z)

    #update vector
    def update_v(self, x_new, y_new, z_new):
        self.x=x_new
        self.y=y_new
        self.z=z_new


class Quaternion:

    def __init__(self, q0, q1, q2, q3):
        self.q0=q0
        self.q1=q1
        self.q2=q2
        self.q3=q3

    #print quaternion
    def print_q(self):
        rospy.loginfo("Quaternion: %f + %f i + %f j + %f k\n", self.q0, self.q1, self.q2, self.q3) 

    #convert in RPY
    def quat2eul(self):
        roll=math.atan2(2*(self.q0*self.q1+self.q2*self.q3), 1-2*(self.q1*self.q1+self.q2*self.q2))
        pitch=math.asin(2*(self.q0*self.q2-self.q3*self.q1))
        yaw=math.atan2(2*(self.q0*self.q3+self.q1*self.q2), 1-2*(self.q2*self.q2+self.q3*self.q3))

        rpy=Vector(roll, pitch, yaw, 'RPY_converted')
        return rpy

    #update vector
    def update_q(self, q0_new, q1_new, q2_new, q3_new):
        self.q0=q0_new
        self.q1=q1_new
        self.q2=q2_new
        self.q3=q3_new

p=Vector(0, 0, 0, 'Position')
rpy=Vector(0, 0, 0, 'RPY')
quat=Quaternion(0, 0, 0, 0)
v=Vector(0, 0, 0, 'Linear Vel')
w=Vector(0, 0, 0, 'Angolar Vel')

def pose_callback(msg):

    #update position
    p.update_v(msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z)

    #update orientation
    quat.update_q(msg.pose.pose.orientation.w, msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z)
    #convert in rpy and update rpy vector
    rpy_conv=quat.quat2eul()
    rpy.update_v(rpy_conv.x, rpy_conv.y, rpy_conv.z)

    #update linear and angular velocity
    v.update_v(msg.twist.twist.linear.x, msg.twist.twist.linear.y, msg.twist.twist.linear.z)
    w.update_v(msg.twist.twist.angular.x, msg.twist.twist.angular.y, msg.twist.twist.angular.z)

    rospy.logwarn("Sensor Data:\n")
    p.print_v()
    rpy.print_v()
    quat.print_q()
    v.print_v()
    w.print_v()


def main():
    ##Init node##
    rospy.init_node('feedback', anonymous=False)

    ##Pub
    pub=rospy.Publisher('/cmd_vel', Twist, queue_size=10)

    ##Sub
    rospy.Subscriber('/odom', Odometry, pose_callback)
    rate=rospy.Rate(100)

    vel=Twist() 

    #simple control law: straight line y=0
    K=5
    lin_vel=2

    while not rospy.is_shutdown():
        #Read Data
        rate.sleep()        

        #Compute control law
        vel.linear.x=lin_vel
        vel.linear.y=0
        vel.linear.z=0

        vel.angular.x=0
        vel.angular.y ...