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

Problem at timing in communicating nodes?

asked 2017-08-08 10:09:11 -0500

thejesseslayer gravatar image

updated 2017-08-14 03:35:43 -0500

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?

#!/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
   def get_msg(self, timeout=None):
       """Blocks until the data is rx'd with optional timeout
       Returns the received message
       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])
     #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__':
      #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)
   except rospy.ROSInterruptException:

#!/usr ...
edit retag flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted

answered 2017-08-21 04:27:09 -0500

thejesseslayer gravatar image

I fixed it. The problem was that controller takes some time to subscribe to simulator topic and viceversa. So I put this in my code:

   while pub.get_num_connections() == 0:
edit flag offensive delete link more

answered 2017-08-10 17:53:52 -0500

Ed Venator gravatar image

The problem is in You have a rospy.spin() before your main loop. rospy.spin() contains an infinite loop, so your program never gets past that line. You can remove the rospy.spin(). Unlike roscpp, which uses a single event queue to process incoming messages and dispatch callbacks, rospy creates a new thread for every Subscriber. All rospy.spin() does is loop until the node is shutdown. All of the message processing is done in the Subscriber's thread.

As in all multithreaded applications, you should be careful to avoid race conditions and use locks or thread-safe queues.

edit flag offensive delete link more


Thanks for the answer, probably one of the problem got solved doing like you suggested, but still doesn't work. I'm rather sure that the problem is in ig.get_msg() because it blocks until message is received. Can you suggest how can I get a message type from callback in a more efficient way?Thanks

thejesseslayer gravatar image thejesseslayer  ( 2017-08-11 04:39:11 -0500 )edit

I found another problem, which exists in both of your nodes. When you create a rospy.Subscriber, you must assign it to a variable, or else it is immediately destructed.

Ed Venator gravatar image Ed Venator  ( 2017-08-11 20:26:11 -0500 )edit

Also, creating a publisher over and over again as you do in publishParams() is wasteful and likely unreliable. Instead, create the publisher as a global variable or class member and use it over and over.

Ed Venator gravatar image Ed Venator  ( 2017-08-11 20:26:44 -0500 )edit

Thank you, I fixed it. Still doesn't work properly the return of data of callback in Do you have any tip for that?

thejesseslayer gravatar image thejesseslayer  ( 2017-08-13 09:41:25 -0500 )edit

What does your code look like now? What does it do now? What doesn't it do?

Ed Venator gravatar image Ed Venator  ( 2017-08-13 18:15:34 -0500 )edit

I edited the comment with the update code. What I want is to do a ping-pong communication between the nodes. What it does now is: simulator publish the first message in /stateTopic but controller doesn't catch the message therefore it doesn't answer and stay still, so the communication doesn't work.

thejesseslayer gravatar image thejesseslayer  ( 2017-08-14 03:11:53 -0500 )edit

But, if I comment params = ig.get_msg(), the communication works properly but obviously I can't get the message from /paramsTopic by controller. The problem I'm rather sure is the class InfoGetter

thejesseslayer gravatar image thejesseslayer  ( 2017-08-14 03:12:07 -0500 )edit

Question Tools


Asked: 2017-08-08 10:09:11 -0500

Seen: 306 times

Last updated: Aug 21 '17