Ask Your Question
0

process same callback from topics under different namespaces

asked 2019-12-26 12:21:48 -0600

davidem gravatar image

updated 2019-12-26 12:45:06 -0600

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 thread
  • ns: remains static
  • status: 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?

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2019-12-27 08:20:16 -0600

grzegorz.f-16 gravatar image

Robot knows when it finish the path, so can save internally it's current status and ignore all new nav goals until it reach the goal. You can also consider using Service-Client communication or actionlib.

edit flag offensive delete link more

Your Answer

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

Add Answer

Question Tools

2 followers

Stats

Asked: 2019-12-26 12:21:48 -0600

Seen: 35 times

Last updated: Dec 27 '19