ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

How can I integrate a value that I receive from a node?

asked 2018-01-16 13:40:00 -0500

carlosal24 gravatar image

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
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
1

answered 2018-01-16 18:10:28 -0500

This is actually not as hard as you're making it, but you probably want to take a step back and look at this from first principals.

Firstly the ODE integrator is intended to be used on a function so that the ODE function can choose to sample the function at different points in order to estimate the integral. This is not the case in your system because samples are only available when they're measured.

A more appropriate integration method would be the Trapezoidal Rule which can work with an arbitrary set of samples. This is fairly simple to code from first principals because you're basically calculating the area of a rectangle and a right angle triangle each time step.

edit flag offensive delete link more

Comments

Thanks, it seems that with the trapezoid rule it works perfectly.

carlosal24 gravatar image carlosal24  ( 2018-01-17 20:41:40 -0500 )edit

Question Tools

2 followers

Stats

Asked: 2018-01-16 13:40:00 -0500

Seen: 642 times

Last updated: Jan 16 '18