After migration from Kinetic to Melodic my script doesn't work
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 ...
"doesn't work" is simply not enough, consider providing more (relevant) details to get an answer!
Please edit your question and post your script (
101010
button for formatting). It would be a big help in understanding and troubleshooting your issue.Yes i'm sorry. I just edited the post
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?
"unicicle doesn't follow the line and goes round" -- when you
rostopic echo /cmd_vel
is anything out of ordinary?Yes, the launch file and urdf model are in this book https://github.com/PacktPublishing/Ma... . In particular:
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]