Ask Your Question
0

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?

------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 ...
(more)
edit retag flag offensive close merge delete

2 Answers

Sort by » oldest newest most voted
1

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:

   con_rate=rospy.Rate(rate)
   while pub.get_num_connections() == 0:
   con_rate.sleep()
edit flag offensive delete link more
0

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

Ed Venator gravatar image

The problem is in Simulator.py. 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

Comments

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 imagethejesseslayer ( 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 imageEd 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 imageEd 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 Simulator.py. Do you have any tip for that?

thejesseslayer gravatar imagethejesseslayer ( 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 imageEd 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 imagethejesseslayer ( 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 imagethejesseslayer ( 2017-08-14 03:12:07 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

Stats

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

Seen: 154 times

Last updated: Aug 21 '17