rospy callback subscribe to topics in parallel
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 ...