ROS 2 robot_state_publisher improvements

Next Generation ROS
,
1

I’m currently near the end of a rewrite of robot_state_publisher. There are a few goals to this rewrite:

  1. Simplify the code, as it was too complicated before.
  2. Bring the code up to modern ROS 2 standards.
  3. Reinstate some of the features that were missed during the original port.
  4. Allow the robot_description to be changed during runtime.

I want to discuss the last point a bit further, and get some thoughts from the community. To start with, I want to make it clear that I am not trying to solve the entire problem of “make a URDF changeable across a robot”. That involves a lot more pieces than just robot_state_publisher. However, adding some level of dynamicity to robot_state_publisher at least solves part of the problem.

With ROS 2, we have a bunch of tools that make solving the problem of a dynamic robot_description possible. We have the ability to get and set parameters (and get notified when they change), we have the ability to publish transient_local (latched) topics, etc. Here are a few different ways I’ve though about to make the robot_description changeable:

  1. Make the robot_description a required parameter during node startup. To access the robot_description, consumers have to do the equivalent of ros2 param get /robot_state_publisher robot_description, and to change it, the equivalent of ros2 param set /robot_state_publisher robot_description <new_description>.

    • Pros: Most like ROS 1 in concept. Makes robot_state_publisher the “owner” of the robot_description. Uses existing ROS 2 parameter infrastructure to the fullest.
    • Cons: Most unlike existing ROS 2 (meaning we have to make changes to things like rviz2). Makes robot_state_publisher the “owner” of the robot_description. The robot_description is no longer anonymous; consumers have to know the name of the robot_state_publisher node.

  2. Make the robot_description a required parameter during node startup. Publish the current robot_description on a latched topic. To change robot_description, the equivalent of ros2 param set /robot_state_publisher robot_description <new_description> has to be run.

    • Pros: Uses the parameter infrastructure for initial configuration and setting. Keeps the topic “anonymous” for getting the robot_description. Doesn’t require changes to RViz2.
    • Cons: Setting the robot_description isn’t anonymous; the name of the node is necessary. Is a bit inconsistent, in that getting and setting the robot_description is done via different mechanisms.

  3. Make the robot_description a required parameter during node startup. Publish the current robot_description on a latched topic. Provide a service to allow changes to the robot_description, which would then propagate to the topic.

    • Pros: Makes getting the robot_description anonymous, so robot_state_publisher no longer “owns” the robot_description. Same for the service. RViz2 doesn’t have to change.
    • Cons: Only uses parameters as an “initialization” mechanism; doesn’t take full advantage of the parameters infrastructure.

  4. Make the robot_description not provided during node startup. robot_state_publisher would do nothing until something called a service to set the robot_description, at which point it would start publishing transforms. Changes to the robot_description would be done via the same service call. There would be a latched topic that publishes the current state of robot_description for downstream consumers.

    • Pros: Ensures that robot_state_publisher isn’t the “owner” of the robot_description. Doesn’t require changes to RViz2.
    • Cons: robot_state_publisher is no longer standalone; it needs an external “something” to tell it the initial robot_description.

These are the options I’ve thought of; there are some others in this same vein. The ros2-refactor currently implements solution 2, but I’m looking for opinions from the community on any of these options, or an option I haven’t thought of. Thoughts?

Ping @jacob, @sloretz, @IanTheEngineer, @peci1, @karsten as a (non-exhaustive) list of people who have expressed interest in this topic.

2 Likes
2

I think option #3 is the best.

Option 4 I don’t like having to have something external on startup give it the current information. Option 1 I think is also a reasonable choice but I prefer 3 because if its a topic, then as it updates, it will automatically update the state of all the subscriber nodes that depend on it. Also having a node need to talk to another node’s parametesr seems to break (?) the intent of separation of parameters across nodes.

Option 2 is similar enough to 3, but I like having a service call so that if someone makes my_awesome_new_robot_state_publisher it can be a drop in replacement if it advertises the same service.

3

A 5th alternative: using a topic on which nodes publish urdf (sdf?) fragments and consumers of robot_description (not the parameter any more here) would have to compute a consistent view of the current robot_description based on the fragments they’ve received so far.

This would make the robot description similar to how TF works, where consumers listen in on updates and a utility library does the heavy lifting of piecing together a consistent view of the current state.

Adding a temporal dimension to the fragment stream could then allow “time travel” similar to how TF has it, and it would automagically support dynamically updating robots.

robot_description could still be a private parameter of nodes to provide them their fragment(s), if those are static for instance, but it would no longer need to be.

This could of course make use of parameter update events to keep track of changes, of latched topics to reduce traffic and computational overhead and to quickly seed the local view for late-joiners, and if needed could be based on techniques for achieving coherent shared state from the distributed algorithms domain (distributed file systems, distributed shared state, lock-stepping, transactions, atomic updates, quora, etc).

Hm, perhaps this falls in the “try[…] to solve the entire problem of ‘make a URDF changeable across a robot’” category.

1 Like
4

Does it though? Without support in other parts of the system I don’t think it does that much…

You could dynamically change the robot description and perhaps use it in part of your own code which uses the robot description, but then rviz probably wouldn’t work right (unless we’ve verified it can handle this). I’d personally prefer to see a proposal for how to do dynamic robot descriptions on both sides before assuming that one of the options you proposed solves part of it. As @gavanderhoorn said, perhaps you need to have some “distributed consensus” or “eventually consistent” pattern in place and that might require more information or a different pattern of communication. It’s not obvious to me that any of these options will be part of the solution without exploring it at least a bit.

The reason for using a topic in ROS 2 was primarily so that it could be recorded easily with rosbag2, at least until recording parameters is a thing.

W.r.t. accessing the robot description anonymously, I actually don’t think that’s the case now, as you need to know the topic name it’s on. I don’t actually think having to know the node name to access the parameter is any different. You just have to make sure that the node name is “X” where “X” is the agreed convention, and any tools that “read” it need to be configurable on which one they access. The only slight advantage of the topic (that I see) is that you can use remapping on both sides, where as you would need a parameter or something to configure the “robot description parameter address” for nodes that read it.

However, for writing the robot description (changing it) I again don’t view the service name (let’s say rosservice:///change_robot_description) to be any different really from a parameter address (let’s say something like rosparam://robot_state_publisher/robot_description).

For @gavanderhoorn’s idea about having multiple contributors to the robot description, then the topic becomes much more viable when reading it, but I still think for setting the value, using the parameter infrastructure is ok.

So if you really want to do one of these, I’d vote for option 2, since it avoids extra services, which themselves have overhead. The parameter ones are already there anyways.

Following my own logic, having the topic is redundant and not any more anonymous than a parameter (imo), but I think it’s fine to keep since that keeps compatability with what is in ROS 2 now, and it allows it to be easily recorded with rosbag2.

5

This is the case that I’m trying to address; at least give people the option to handle it in their own code. As I said, on its own this definitely won’t completely solve the problem, but I feel like it is incremental progress towards some use cases. Basically, having this ability (even in a limited form) seems better than people forking robot_state_publisher to do their own dynamic thing. And we can always improve/change it later, including updating rviz2 more dynamicallly.

Make sense.

But I feel like that is anonymous, or at least, anonymous in the ROS sense. Any node can publish to the /robot_description topic, so downstream consumers don’t have to care which node is publishing. Accessing parameters on a node, on the other hand, ties you to that node. I guess you could argue that you could rename my_custom_state_publisher_node to robot_state_publisher, and thus parameters would work, but that seems pretty icky to me. In any case, you aren’t actually arguing against the robot_description topic as far as I can tell, so no reason to discuss further here :).

That’s sort of what I thought when I initially implemented what I have now. After I wrote up the alternatives, 2 seemed the best to me as well, but 3 isn’t far behind.

6

This is an interesting proposal, and would probably solve more of the problems, but it is honestly outside of the scope of what I have the time to do here. So I’d be all for someone proposing the pieces to do this, but it likely won’t be me (and may not be robot_state_publisher at all, for that matter).

7

Hi, just a quick thought now. I think there’s long been the misconception that robot_state_publisher is a somewhat superior “consumer” of robot_description. It isn’t.

I think the problem isn’t whether robot_state_publisher should become an “owner” of the robot model. If you stick to the Linux paradigm of simple tools doing one thing precisely, then robot_state_publisher is a thing that takes a robot description and joint states, and publishes some TFs generated from these.

You see, from this point of view, it isn’t different from e.g. RViz or robot_body_filter, which are also tools that need to somehow receive the current model of the robot and do some nice stuff with it.

So I suggest this rewrite should go in this direction - i.e. do not care about how the model is generated. And in no way take ownership of the model. The only thing that needs to be agreed upon is how to transmit the model to the “clients” (topic/parameter). You don’t even have to take care about the “publishers” of the model during the rewrite.

What follows from this idea is that ROS could be “robot-model-producer” independent. There can be several ways of generating the robot model, ranging from simple parsing of a URDF file as ROS 1 does, to some segment-based construction as others suggest. I would leave this logic to the community. ROS2 could for example implement just some simple robot_model_publisher, which could take the robot model from some (private) parameter/URDF file/whatever, and publish it either as a topic or make it available as its public parameter. People who need more flexibility could then write more complicated generators, segment assemblers and so on. The only requirement for “clients” would then be to be able to accept the whole generated URDF model.

1 Like
8

I’m concerned that gives someone the impression that dynamically changing the robot description should “just work”, which isn’t true. I’d prefer someone forking it until it supposed to actually work with at least the standard tools like rviz and rqt.

That’s exactly what I’m saying. Why is it “icky”?

At the end of the day a topic and a parameter (and a node name too) are just a resource name. The only benefit for the topic is that there can be multiple publishers and they replaying from a bag is easier.

To be clear, I think we should keep the topic for reading the value. It’s setting where I find having an additional service to be questionable.

9

Note related to the robot_description though, but can we make sure that at any point in time, there is a full state of the robot? The issue I frequently had in ROS1 was the use of latched topics for static transforms (https://github.com/ros/robot_state_publisher/issues/105) where static TFs were only published once at the beginning, leading to a loss of those messages when a bagfile is not replayed from t=0.
As far as I know, there are no latched topics in ROS2. I just want to make sure that we do not run into the same issue again with ROS2. To make it clear: Randomly accessing any point in time in a logfile that logged the robot_state_publisher should produce a full and valid robot state of all joints that have been published up until then.

10

I tend to agree with this. A topic for readers seems like something everyone is on board with, though it does dodge the question of what to do when it changes during runtime (republished). The main reason it’s loaded into robot_state_publisher is convenience. And the main reason I’m somewhat hesitant to endorse having a way for robot_state_publisher (or the proposed robot_model_publisher) to change it is that it’s setting up for an expectation that changed robot models are supported OOB by at least the standard tools.

11

There’s the “durability” QoS:

https://index.ros.org/doc/ros2/Concepts/About-Quality-of-Service-Settings/#qos-policies

I don’t think there’s a solution for that in rosbag2, I think that this is just a corner case that needs to be addressed by the playback tool.

w.r.t. the robot_description, I think it’s a step sideways since in ROS 1 you had to manually load the parameter value into the parameter server (and also be careful to record it) as the bag could not capture it at all.

12

Well, I think every tool has to decide whether it can react to robot model changes or not. If it can’t, it should e.g. write an error to rosout, restart itself, or do whatever seems to be correct if the model changed and the tool can’t process it.

What are actually the tools using the robot model? In ROS 1, I can remember just robot_state_publisher, rviz, the few attempts at robot_*_filter, and that’s probably it. Or am I missing something? You can of course have more tools involved, like moveit (but there the robot model itself isn’t sufficient as you need the whole SRDF).

13

I suppose that’s true, but it would be nice to make sure at least the standard tools do something reasonable, which I’m not convinced about. Currently the only tool that publishes to the topic doesn’t allow you to change it without restarting, but that’s essentially the same thing as republishing it.

I don’t know of any others, but that’s part of my hesitation because I know that this has been discussed in the past around end-effectors and planning/self-filtering, and in those cases I don’t know if you need something more or not. I just don’t know what the use case for dynamically updating the robot model is right now, and so I cannot say whether or not this will be sufficient or not.

14

I find @gavanderhoorn’s idea intriguing.

For robot_state_publisher's port in general I agree with @peci1. We should have a separate tool for providing the robot description from the tool that turns joint states into TF information.

Do we want to allow multiple robot descriptions? (Within a single DDS domain?) If so, then I think the robot description must be provided via a name-spaced topic. So far the discussion seems to be implicitly assuming a single robot.

15

For what it’s worth, all of my considerations above were thinking about allowing more than one robot description per domain. All of topics, services, and parameter’s (including node names) are namespace-able, and so allow having more than one at a time.

1 Like
16

I’m not sure I understand what the role of robot_model_publisher is here. It’s just a node that takes a file from disk and publishes it? I’m not sure that has too much value; you can do the same thing by running a global parameter_blackboard today, no special node needed. On the other hand, people definitely do depend on robot_state_publisher publishing the TF transforms for their robot, so it is unclear to me where that would come from in this model.

I guess it is a matter of perspective. We allow changing the node name for some good reasons, but I still think of it more like an “identifier”, rather than a totally fungible resource. So renaming yourself to “become” another node seems like a lie to me. I view topics, on the other hand, as fungible. But you are right, your method would work.

17

In the simplest case, it would be as you describe. Read a file from disk and expose its contents as a param. But, as many people here mention, there is need for more complicated robot model “generators” (e.g. assembly of URDF fragments coming on a topic). There, the model publisher would be a more complicated thing.

18

@clalancette I’m really happy to see some attention being paid to robot_state_publisher. I was going to look into some dynamic behaviors, so I’m glad you’ve started us down that path!

One thing I’d really like to see is an ability to call a service to change one of the fields that would have been parsed out of the xml.

For example, it might be desirable to call a service to change the color of a link’s mesh. This would be extremely useful.

That said, I share @wjwwood’s concern with respect to RViz and its ability to refresh this in a performant fashion. While I have seen RViz change to a different robot model, it takes several seconds to actually occur. I would want these changes to happen much faster, and without reloading the entire model. Maybe there could be a way to forward the delta on the URDF to RViz in a way that would make it so that RViz doesn’t have to reload the entire model?

19

I agree. I don’t like the idea of remapping the node name of my awesome_new_robot_state_publisher to robot_state_publisher just to make sure the params map up correctly. If they both just expose global topic /robot_description and service /change_robot_description it entirely goes around the issue of making it dependent on an implementation.

I view the robot state publisher as something that reads a URDF and gives me some static transforms. If some of these transforms are changing, they’re probably not being given out by the URDF but by TF updates. I could see for MoveIt if you want to add tools or if you have some extremely funky robot and want to have some dynamically-changing-static transformations that would be nice, but I don’t see what any of that has to do with the initial discussion. I agree with William and Hunter on this front for what its worth.

20

The only case where the node name would be used (in my proposal) would be setting the description remotely, which I think will be uncommon, and in that case you don’t need to change the node name of the awesome_new_robot_state_publisher, but instead you just make the “caller” that’s setting the description change the parameter on the awesome_new_robot_state_publisher rather than the robot_state_publisher node.

The only reason I think a parameter could be a good fit here is that this service, e.g. /change_robot_description, only has one side effect which is to change the robot description which was likely initialized with a parameter and is likely stored in one too, just for convenience, and therefore it seems like we could just request to set the parameter instead, saving the need for an additional service server. But in the end, I don’t actually mind if there one or the other or both.

I think everyone agrees that using a topic for reading the description is more convenient and future proof.