0
$\begingroup$

Rosanswers logo

I am trying to run a system where my ROS code subscribes to 'n' different topics, each representing one agent. For each agent I need to run a series of calculations (based on the callback function) which I then publish later for a different node to handle. The problem is that the calculations I need to do take some time, and I need the system to run as quickly as possible. What I would like to have happen is for my script to subscribe to each agent and run this calculation parallel (or pretty close to parallel). Here is my pseudo-code:

#!/usr/bin/env python
import numpy as np
from numpy import linalg as LA
from multi_agent_simulation.msg import IndividualPositions, Contains, EntOcc
import rospy
from occupancy_entropy_plot import Aggent_Occupancy_Entropy
import sys
import time
from matplotlib import path
import matplotlib.pyplot as plt

class contains:
    def __init__(self):
        
        '''
        Initalize Data
        '''

        self.AOE = Aggent_Occupancy_Entropy()
        self.agents = self.AOE.agents
        self.x_min_dist = self.AOE.x_min_dist #distribtuion limits
        self.x_max_dist = self.AOE.x_max_dist
        self.y_min_dist = self.AOE.y_min_dist
        self.y_max_dist = self.AOE.y_max_dist
        self.delta = self.AOE.delta
        self.all_positions_tuple = self.AOE.all_positions_tuple
        self.center_dict = {}
        self.dim_x = 0
        self.dim_y = 0
        self.ent_list = [0]*self.agents
        for i in range(0, self.agents):
            name = 'agent%s' % (i + 1)
            self.center_dict[name] = 0.0
        '''
        Subscribers and publisher
        '''
        self.agent_dictionary = {}
        self.contains_dict = {}
        for i in range(0, self.agents):
            name = 'agent%s' % (i + 1)
            self.agent_dictionary[name] = [0, 0]
            rospy.Subscriber('/' + name, IndividualPositions, self.individual_callback, queue_size=1, buff_size=2**24)             

    def individual_callback(self, msg):
        '''
        DO CALCULATION
        '''




def main():
    while not rospy.is_shutdown():
         '''
         DO STUFF/PUBLISH DATA
         '''
                         

if __name__ == '__main__':
    rospy.init_node('contains')
    C = contains()
    C.main()

So the idea is that the each agent's information would pass through the individual_callback function where I do my calculations, and then the function main() actually publishes it. If I use this code for 1 agent, I can run my calculation at ~20Hz which is perfectly acceptable. But with more agents, say around 20, the speed drops to ~1Hz which is too slow. How can I reorganize my code so that the processing is more parallel? Is that even possible?

Note: I have looked at Python multiprocessing, that was my initial idea. But there seems to be issues using multiprocessing and ROS to calculate and publish data. That was the case for me, anyway.

EDIT: Based on maxsvetlik's suggestion I rewrote the node so that each topic was passed through it's own callback function. Unfortantely, this did not work. Updated code is:

#!/usr/bin/env python

import numpy as np
from numpy import linalg as LA
from multi_agent_simulation.msg import IndividualPositions, Contains, EntOcc
import rospy
from occupancy_entropy_plot import Aggent_Occupancy_Entropy
import sys
import time
from matplotlib import path
import matplotlib.pyplot as plt

class contains:
    def __init__(self):
        
        '''
        Initalize Data
        '''
        #self.fig1 = plt.figure(1)
        #self.fig1.show()
        self.ent_time = 0.0
        self.dim_x = 0
        self.dim_y = 0
        self.dim_x = 0
        self.dim_y = 0
        for i in range(0, self.agents):
            name = 'agent%s' % (i + 1)
            self.center_dict[name] = 0.0
       
        
            
        '''
        Generate functions
        '''  
        self.function_dictionary = {}          
        for i in range(0, self.agents):
            def individual_callback(msg):
                """
                DO CALCULATION
                """
            name_dict = 'agent%s' % (i +1)

            self.function_dictionary[name_dict] = individual_callback
            
        '''
        Subscribers and publisher
        '''
        self.agent_dictionary = {}
        self.contains_dict = {}
        for i in range(0, self.agents):
            name = 'agent%s' % (i + 1)
            self.agent_dictionary[name] = [0, 0]
            self.contains_dict[name] = [[0],[0]]
            rospy.Subscriber('/' + name, IndividualPositions, self.function_dictionary[name], queue_size = 1)      


        
     
    
    def main(self):
         '''
         DO STUFF/PUBLISH DATA
         '''


if __name__ == '__main__':
    rospy.init_node('contains')
    C = contains()
    C.main()

So the goal was to generate as many functions as needed, store them in a dictionary, and then call the unique function later for callback purposes. Like I mentioned, this did not work... is it possible the way I'm defining my functions is still wrong? Thanks in advance.

EDIT 2: So I tried implementing the code from NEngelhard and that still did not increase speed. This is my code now (Edited slightly, I'm using string instead of empty so I can pass actual information):

#! /usr/bin/python

import rospy
from std_msgs.msg import UInt16, String
import Queue
from occupancy_entropy_plot import Aggent_Occupancy_Entropy
from thread import start_new_thread
from threading import Lock
from matplotlib import path
import time

class ThreadClient:
    def __init__(self):
        self.q = Queue.Queue() # input queue for jobs

        self.new_job_id = 0
        self.job_id_lock = Lock()
        self.pub = rospy.Publisher("/processed", UInt16, queue_size=100)
        self.AOE = Aggent_Occupancy_Entropy()
        self.all_positions_tuple = self.AOE.all_positions_tuple
        self.dim_x = 0
        self.dim_y = 0
        # collect several input topics in single callback
        self.agent_dictionary = {}
        for i in range(3):
            self.sub = rospy.Subscriber("/data"+str(i), String, self.collect_cb)

        # create some worker thread that process in incoming data in parallel
        for thread_name in range(10):
            start_new_thread(self.worker, (thread_name, ))


    def worker(self, thread_id):
        rospy.loginfo("Starting thread %i", thread_id)
        while not rospy.is_shutdown():
            try:
                task = self.q.get(block=False)  # Queue is already thread safe
                #rospy.loginfo("Thread %i is Processing Task %i", thread_id, task)
                rospy.sleep(1) # do some fancy computation...
                self.pub.publish(UInt16(data=task))  # uses internal queue of publisher
            except Queue.Empty:
                rospy.sleep(0.1)

    def collect_cb(self, msg):  # create a new ID for this task
        assert isinstance(msg, String)
        with self.job_id_lock:
            current_id = self.new_job_id
            vert_tup = [(142.6929649672458, -7.574622614639708), (1239.063531790756, -495.39793099903306), (1162.7761297559962, 1468.7628175046418), (320.3391304347826, 614.1869565217391), (142.6929649672458, -7.574622614639708)]
            p = path.Path(vert_tup)
            contains = p.contains_points(self.all_positions_tuple)            
            self.new_job_id += 1
            zip_list = zip(contains, self.all_positions_tuple)
            true_list = [j[1] for j in zip_list if j[0] == True] 
            x_list = [i[0] for i in true_list]
            y_list = [i[1] for i in true_list]
        self.q.put(current_id)
        #rospy.loginfo("Added task %i to the queue", current_id)


if __name__ == "__main__":
    rospy.init_node("subs")
    tc = ThreadClient()
    rospy.spin()

The rate of the callback function collect_cb decreases linearly with the number of strings that I subscribe to. What really needs to happen for my code to work is that the x_list and y_list values need to be added to a dictionary which will then be published on worker. With the above code, the x_list and y_list values update at ~12 Hz for 1 subscriber, ~6 hz for 2, ~4 Hz for 3, ect (eventually that value vert_tup is going to be a variable depending on the message I'm subscribed to, and the value self.all_positions_tuple is a very large constant-valued list defined previously. The 'contains = p.contains_points(self.all_positions_tuple)' line is the line that takes a long time to compute, it involves finding points within an 'n' dimensional polygon.).


Originally posted by Mitchell on ROS Answers with karma: 1 on 2017-05-29

Post score: 0

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

When you subscribe to a topic with rospy the callback is given its own thread automatically to process the data from the topic. What seems to be happening here is that all threads are given a single method to do their computation (individual_callback()), forcing each thread to compete for time. This is why you see the roughly linear relationship between calculation speed and number of subscribed agents.

So the parallelism is already happening, but its being counter acted by having only a single processing callback. The solution is to use a unique callback for each subscriber. E.g.

class contains:
def __init__(self):

    self.agent_msgs = [ ] #for speed, this should by a numpy array
    for i in range(0, self.agents):
        name = 'agent%s' % (i + 1)
        self.center_dict[name] = 0.0

def agent_cb0(msg):
      self.agent_msgs[0] = msg

def agent_cb1(msg):
      self.agent_msgs[1] = msg
    
 ....

 def setup():
    '''
    Subscribers and publisher
    '''
    self.agent_dictionary = {}
    self.contains_dict = {}
    rospy.Subscriber('/agent0', IndividualPositions, self.agent_cb0, queue_size=1)
    rospy.Subscriber('/agent1', IndividualPositions, self.agent_cb1, queue_size=1)
    ...

Originally posted by maxsvetlik with karma: 161 on 2017-05-30

This answer was ACCEPTED on the original site

Post score: 0


Original comments

Comment by Mitchell on 2017-05-30:
Thanks for responding. Unfortunately, that did not work for me. I edited the question above with how I re-wrote the node.

Comment by maxsvetlik on 2017-05-31:
I'm unfamiliar with how you're generating functions here, and it seems possible to me that the function is just being overwritten, mapping every agent to the same callback (as before). I've updated my answer with code using standalone callbacks. Please try this out and see if it scales

Comment by Mitchell on 2017-06-01:
Oh yeah I've tried it this way before, the performance is identical.

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.