process same callback from topics under different namespaces
I'm not sure wether the title is correct for my issue, any edits are welcome.
I am running a multirobot simulation. The two main scripts I have are:
navigator.py
: runs on many nodes, each under an appropriate namespace.observer.py
: runs on a single node, dispatches goals to each navigator.py node
In observer.py
I need to subscribe to the same topic (in this case /foo_topic
) but under all the different robot namespaces. The topic in my case has a custom message: for simplicity, assume that is called MyPose
and it has the following definition:
geometry_msgs/Pose pose
string robot_ns
string status
My algorithm is as follows:
for n in namespaces:
rospy.Subscriber(n+'/foo_topic', MyPose, on_pose)
def on_pose(msg):
if msg.status == 'ready':
# based on msg.pose, if the robot is 'ready'
# sends a list of goals to a topic under
# msg.robot_ns namespace
send_goal(msg.robot_ns, msg.pose)
On navigator.py
, the algorithm for the (many, different) publishers runs on a loop on a separate thread: it takes in the class-variables
pose
: periodically updated by another threadns
: remains staticstatus
: initialized as 'ready', can be 'ready' or 'busy'
This is the code:
def publish_state(self): # runs on a thread
pub = rospy.Publisher(self.ns + '/foo_topic', MyPose, queue_size = 10)
rate = rospy.Rate(10)
while not rospy.is_shutdown():
msg = MyPose()
msg.status = self.status
msg.pose = self.pose
msg.ns = self.ns
pub.publish(msg)
rate.sleep()
What should happen is that the observer listens to the pose of each robot and sends it a list of goals if the robot's status is 'ready'. When the robot receives it, changes its status to 'busy' and starts navigating towards the goals. Afterwards, it should publish its status as 'busy' and not receive any goals untils it has arrived and changed the status back to 'ready'
Now, my issue is: since observer.py
has a single callback that handles all the messages from all the different topics, I'm worried that it suffers of some sort of delay. By running even a single robot, I can log two different type of messages:
- in observer's
on_pose
function, logging msg.status says 'ready' even after the list of goals has been sent - running
rostopic echo /robot_1/foo_topic
, it logs 'busy' correctely just after the first list of goals is received
Since the callback msg is "delayed", msg.status will stay 'ready' for a while and the observer will continue to treat the respective robot as 'ready', thus sending him new goals.
I don't know if my assumption is correct, and wether this answer could help me.
EDIT: yes, I can confirm that upon inspection, messages get sent at a much higher rate than they are received. Even without timestamps, approx. 1/5 of the messages sent gets received. How can I get around this problem?