Problem at timing in communicating nodes?
Hello ROS community, I'm really newbie to ROS and what I am trying to do is to make an architecture in which there is a Simulator that compute his trajectory in every moment and a Controller that get the Simulator's position (x,y,theta) at every t and pass the params (v,w) in order to compute the next position in which Simulator has to go. To do this, Simulator publish his position in /stateTopic, Controller is subscribed to /stateTopic so it gets the values, does the maths, and publish new params v,w in /paramsTopic. What I want, and what I'm failing to do, is that Simulator gets the new params, compute the next position and publish it to /stateTopic. When I try to run this, it just doesn't work, nothing get publish on screen. Is a timing trouble?
------Simulator.py:
#!/usr/bin/env python
import rospy
import numpy as np
import scipy as sp
from std_msgs.msg import String
from comunicating_robots.msg import Position, Params
from scipy.integrate import odeint
import threading
#uso la classe InfoGetter per ottenere come risposta il messaggio da paramsTopic
class InfoGetter(object):
def __init__(self):
#event that will block until the info is received
self._event = threading.Event()
#attribute for storing the rx'd message
self._msg = None
def __call__(self, msg):
#Uses __call__ so the object itself acts as the callback
#save the data, trigger the event
self._msg = msg
self._event.set()
def get_msg(self, timeout=None):
"""Blocks until the data is rx'd with optional timeout
Returns the received message
"""
self._event.wait(timeout)
return self._msg
#funzione per calcolare nuova posizione
def newPosition(state, t, v, w):
# unpack the state vector
x = state[0]
y = state[1]
theta = state[2]
#compute state derivatives
xd = v*np.cos(theta)
yd = v*np.sin(theta)
thetad = w;
#return the state derivatives
return [xd, yd, thetad]
def publishState():
global pub
#definisco stato iniziale e divido il tempo in passi da 0.01secondi
state0 = [1.0, 1.0, 4.0]
t = np.arange(0.0, 20.0, 0.01)
#risolvo il sistema di equazioni usando la funzione odeint di scipy.integrate v=1 w=0 condizioni iniziali
state = odeint(newPosition, state0, t, args=(1,0))
rate = rospy.Rate(100) # 100hz
#Get the info
ig = InfoGetter()
sub = rospy.Subscriber('paramsTopic', Params, ig)
while not rospy.is_shutdown():
#creo e publico il messaggio
msg = Position()
msg.x = float(state[0][0])
msg.y = float(state[0][1])
msg.theta = float(state[0][2])
rospy.loginfo(msg)
pub.publish(msg)
#prendo i nuovi parametri e risolvo l'equazione differenziale
#ig.get_msg() Blocks until message is received
params = ig.get_msg()
state = odeint(newPosition, state0, t, args=(floatparams.v),float(params.w)))
if __name__ == '__main__':
try:
#dichiaro il nodo al master e definisco il topic a cui mando messaggi di tipo Position (x,y,theta)
rospy.init_node('simulator', anonymous=True)
pub = rospy.Publisher('stateTopic', Position, queue_size=10)
publishState()
except rospy.ROSInterruptException:
pass
------Controller.py:
#!/usr ...
