Implement a priority mux

[mllofriu]

Description

Implement a priority multiplexer like cmd_vel_mux (https://github.com/kobuki-base/cmd_vel_mux/blob/devel/src/cmd_vel_mux.cpp) for generic topic types. The multiplexer would give preference to the highest priority topic that published within a given time window, suppressing all other topics.

Note that this tool would replace cmd_vel_mux, but allow for uses with other topic types.

We are currently pursuing the implementation of input/output suppressors/inhibitors, as described in the subsumtion architecture, as described in here:

https://people.csail.mit.edu/brooks/papers/AIM-864.pdf

This other paper claims to have implemented it in ROS (1) but no source is available:

file:///home/mllofriu/Downloads/Subsumption%20Model%20Implemented%20on%20ROS%20for%20Mobile%20Robots%20_Li16.pdf

Completion Criteria

A new tool, say priority_mux, is implemented.

It allows to specify a list of topics of a generic, but equal, type. The order in which topics are published determines their relative priority, the first one being the highest priority one.

It contains a parameter that specifies the length of the time window, T, in seconds. Once an input topic publishes a message which is forwarded by the multiplexer, all lower priority topics will be suppressed for T seconds. Any topics with a higher priority topics, however, can be forwarded right away.

Implementation Notes / Suggestions

The feature could be implemented by combining features from topic_tools/mux and cmd_vel_mux. The services and selection logic can be stripped from mux, and the time window logic can be migrated from cmd_vel_mux.

We can certainly help implement or implement it. We just wanted to check if it is something this team would be interested in incorporating to topic_tools. As well as to check if you maybe know any other existing tool that can be used for this purpose.

[Cakem1x]

I think this would be a very nice tool to have. I also had exactly the same name like you in mind for it (priority mux), when searching for it.
Have you made any (public) progress on this somewhere?

My use-case would be similar to what the cmd_vel_mux does, but I need to be able to "priority mux" both TwistStamped msgs and FourWheelSteeringMsgs. I use a controller that supports both Twist msgs (widespread) and FourWheelSteeringMsgs (supports more fine-graned control of wheels, e.g. change wheel orientation without moving the base).
However, I think supporting two (or, generally, more than one) input types is probably a bit cumbersome to implement. And I am not sure if that use-case is common enough to warrant a generic implementation.

[mllofriu]

Nice to hear.

I made the unit tests to prepare for a nice refactor. Those are merged.

I have a ~70% progress on the refactor on a branch in the project. I'm happy to share the progress, as that project is somewhat stalled at the moment.

[josephduchesne]

I found this thread looking for such a tool. My use case is putting it in front of a ros2 control controller (e.g. effort_controllers/JointGroupEffortController) which takes std_msgs/msg/Float64MultiArray.

In particular it would be nice to have a timeout parameter that either stops publishing or publishes a default value when the source has stopped. As it stands all of the ForwardCommandController subclasses in ros2 control lack any form of timeout, and I think a generic topic tool might be a better solution than implementing the same behavior over and over.

[Cakem1x]

I implemented something to fit my use-case in May 2024. It is currently closed-source, because I was lazy.
But I can publish it (BSD Licsense) in a separate repository next week if that would help you. I am not sure about integrating it into this repository.

My implementation does not support a default "fallback" value, but the timeout feature like mllofriu described exists. I think you could kind of "hack" the default fallback feature by adding a latched/transient_local publisher (e.g. python node) that outputs the default value and use it as lowest priority input.