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