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

asked 2021-04-06 09:47:54 -0500

Elektron97 gravatar image

updated 2021-04-06 19:55:16 -0500

tryan gravatar image

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 ...
(more)
edit retag flag offensive close merge delete

Comments

2

"doesn't work" is simply not enough, consider providing more (relevant) details to get an answer!

abhishek47kashyap gravatar image abhishek47kashyap  ( 2021-04-06 11:39:16 -0500 )edit

Please edit your question and post your script (101010 button for formatting). It would be a big help in understanding and troubleshooting your issue.

tryan gravatar image tryan  ( 2021-04-06 12:15:47 -0500 )edit
1

Yes i'm sorry. I just edited the post

Elektron97 gravatar image Elektron97  ( 2021-04-06 15:51:04 -0500 )edit
1

I don't notice anything that should be affected by migrating to Melodic, but maybe someone else will catch something. In the mean time, can you provide your launch files and URDF?

tryan gravatar image tryan  ( 2021-04-06 20:06:42 -0500 )edit

"unicicle doesn't follow the line and goes round" -- when you rostopic echo /cmd_vel is anything out of ordinary?

abhishek47kashyap gravatar image abhishek47kashyap  ( 2021-04-07 00:38:40 -0500 )edit

Yes, the launch file and urdf model are in this book https://github.com/PacktPublishing/Ma... . In particular:

Launch file: /chapter4/diff_wheeled_robot_gazebo/launch/diff_wheeled_gazebo_full.launch URDF file: /chapter3/urdf/diff_wheeled_robot_with_laser.xacro

Elektron97 gravatar image Elektron97  ( 2021-04-07 02:33:31 -0500 )edit

For rostopic echo: Yes, nothing. However, i tried to plot /cmd_vel and yaw angle with a second topic (Float64) and the resulting plot is very interesting. When quaternion.z is -1, yaw (and then angular rate) seems like a sawtooth wave. [https://imgur.com/a/5yOmL3C] Is math.atan2()? In Kinetic we have this plot: [https://imgur.com/a/s3GqC0p]

Elektron97 gravatar image Elektron97  ( 2021-04-07 02:40:26 -0500 )edit