How can I integrate a value that I receive from a node?
Hi, I'm new to ROS and python so I do not know many things and recently I have a problem that I can not solve.
At one of my nodes, I receive a velocity vector [V, W], where W is the angular velocity and V the linear velocity of a unicycle robot. To get the model of this robot I need to integrate W and get Theta.
The question is how can I integrate the W value and get theta? I have searched several integration topics but I can not do it. One method that I think can work is to use 'odeint' from the scipy library, however, I do not know how to use it, since the function receives (f, X0, t) a function, an initial condition, and a time vector. and since I do not have a function, only a value that reaches me every 0.01 seconds, I do not know how to use it.
I enclose my code in case someone can give me some guidance on this method or any other
import rospy
import numpy as np
from math import cos, sin
from prueba1.msg import TrayMensaje , VelMensaje #Mensajes propios
from geometry_msgs.msg import Twist
from scipy.integrate import odeint
class Modelo():
def __init__(self):
rospy.init_node('Modelo', anonymous=True)
rospy.Subscriber('velLider', VelMensaje, self.callback)
self.pub_modelo=rospy.Publisher('/cmd_vel', Twist, queue_size=10)
self.vel_msg = VelMensaje()
self.Mod_msg = Twist()
self.rate = rospy.Rate(10)
def callback(self,datos):
self.vel_msg=datos
def f(y,t):
return y
def robot(self):
t0=-0.1
t1=0
theta0=0
while not rospy.is_shutdown():
V=self.vel_msg.V
W=self.vel_msg.W
t0+=0.1
t1+=0.1
t=[t0 , t1]
#Integration of W = theta
#[th , error]=odeint(self.f,theta0,t)
#print th
theta=0
XLp=V*cos(theta)
YLp=V*sin(theta)
Thetap=W
self.Mod_msg.linear.x=XLp
self.Mod_msg.linear.y=YLp
self.Mod_msg.linear.z=0
self.Mod_msg.angular.x=0
self.Mod_msg.angular.y=0
self.Mod_msg.angular.z=Thetap
self.pub_modelo.publish(self.Mod_msg)
self.rate.sleep()
rospy.spin()
if __name__ == '__main__':
try:
x= Modelo()
x.robot()
except rospy.ROSInterruptException: pass