[DO NOT MERGE] Add generic systems for Drake ROS integration

[hidmic]

Opening PR for awareness and discussion only. This patch builds on top of sloretz/drake_ros2_demos#generic_systems, and adds more systems and examples. We should probably split this further for proper review once we're happy with the result.

---

This change is 

[jwnimmer-tri]

Skimming the discussions again, I see I was mostly mistaken about the level of nitty-gritty discussion. There were a few "nits" at the start that threw me off -- later threads are more architecturally oriented.

I don't know that trying to rework this code back into a design document adds much value. The code speaks for itself, and is generally clearer than an abstract document would be.

I'm also not worried about API stability; of course things will be quite unstable as we bootstrap and figure out what to do here.

The point I'd mainly advocate for is good hygiene, because that is appropriate for any phase of the development lifecycle. Something like the type_conversion functions are small and standalone, and could be a first bit of code to reach the main branch. You could use that small PR to establish the review process (including deciding which coding conventions to adopt), and land the code without CI -- it's only a few hundred lines. Then you have a working codebase that you can build & test locally, and next you can turn on basic CI to cover it. Then you can add linters and any automated checks on top of that, and start working on the CI pipeline that publishes your API reference documentation. Somewhere along that line, the tooling will be mature enough to start adding in real code, like some of the message pub/sub system classes, one at a time, with unit tests.

My discomfort most revolves around not being privvy to any discussions about getting software development basics in place, as if were being ignored. Maybe the plans are so obvious that it just hasn't made it into GitHub discussions, and you've talked about it in meetings. That's fine too, but it might help to place a few breadcrumbs into GitHub here to plot a course.

[calderpg-tri]

What does this produce for frame names of identically-named frames in multiple model instances?

[hidmic]

Good question. I did not test that particular case. Having said that, AFAIK the SceneGraph does not seem to have a concept of model. Source systems like the MultibodyPlant use prefixes for geometry and frame names e.g. robot_0::leg_link.

Thus,

identically-named frames in multiple model instances?

shouldn't be a problem.

[hidmic]

On a related note, I considered substituting :: with / to better align with ROS conventions for frame names, but I figured I'd be putting unnecessary cognitive load on a dev trying to correlate frames in the SceneGraph and /tf.

[calderpg-tri]

Multiple model instances with identically-named frames is a very common case in anzu, so we need to make sure this works reasonably. This is part of the reason the anzu MbP+SG->Rviz process uses the MbP names, where model instances are known. (Also note that only some of these items (MbP bodies, MbP frames, SG frames) are guaranteed to have unique names, let alone names at all)

considered substituting :: with / to better align with ROS conventions for frame names

We've used something like this in anzu

std::string GetTfFrameName(const std::string& model_instance_name, const std::string& element_name) {
  const std::regex mbp_separator_regex("::");
  std::string raw_name = model_instance_name + "/" + element_name;
  return std::regex_replace(raw_name, mbp_separator_regex, "/");
}

to do the conversion, and I think having ROS-like frame names where :: always becomes / is well worth it to be clearer to users from the ROS side of things.

[hidmic]

This is part of the reason the anzu MbP+SG->Rviz process uses the MbP names, where model instances are known.

Also note that only some of these items (MbP bodies, MbP frames, SG frames) are guaranteed to have unique names, let alone names at all

I see. Indeed there's no guarantee SceneGraph frame names are unique... Only frame IDs are unique.

@IanTheEngineer this is a bit of a problem. We cannot

convert SceneGraph QueryObjects to [...] TF messages

if frame names are not unique. Well, we could generate frame names like "frame_ID" but it is not very practical. We need additional information e.g. by querying the MultibodyPlant as @calderpg-tri suggests.

I'm curious though, how is the scene described by a SceneGraph supposed to be visualized and introspected if the only non-ambiguous identification metadata is a number? I understand frame names become less useful as you scale into the hundredths or thousands of frames, but there must be some semantic metadata somewhere to make sense out of the scene, no?

I think having ROS-like frame names where :: always becomes / is well worth it to be clearer to users from the ROS side of things.

That's fair. We can do the conversion.

[calderpg-tri]

See planning/rviz_geometry_visualizer.{cc, h} and planning/shape_description.{cc, h} in anzu if you haven't already, to see how we've done this. We fall back to something like <model_instance>/unnamed_frame_id_<frame_id> in the case of a frame that doesn't have a name, but that's usually only a few frames in a complex system.

[IanTheEngineer]

In my mind, this is as much a ROS/RViz issue as it is for the drake-ros interface. Even in RViz, without special namespace handling, you cannot have multiple instances of robots (having the same names in their frames) robot publishing to /tf, and therefore visualized in RViz. It's not immediately obvious to me how we should handle this to make both Drake & tf happy.

I think querying MBP for more information could work for now. We should make note if there is a particular MBP API that would make this process easier, we can propose it to the Drake Developers. From the tf/RViz side, we could bring back to life something akin to tf_prefix, which was apparently only deprecated due to a lack of development cycles.

[hidmic]

Alright, b365190 and 1f69048 now allows MultibodyPlant instances in the scene to be registered with the RvizVisualizer to help refine frame IDs and marker namespaces.

[calderpg-tri]

Isn't this going to generate a separate namespace for each marker? That's not very usable if people are trying to visualize a larger system and want to select/deselect chunks of it.

[hidmic]

It will, yes. I considered using source names as marker namespaces and geometry IDs as marker IDs, but:

a. Geometry IDs are 64 bit integers whereas marker IDs are 32 bit integers. This is relatively minor, as I'd expect things to fall apart way before one can attempt to visualize 2^31 + 1 markers.
b. To render a single, capsule-shaped geometry, three (3) markers are necessary. And with that, the correlation between marker ID and geometry ID is lost.

I guess we could allocate 29 bits for the ID, 2 bits to distinguish capsule geometries' constitutive markers, and throw if bit truncation of the geometry ID would lose information. Then we could afford putting all markers for all geometries from a given source in its namespace. WDYT?

[calderpg-tri]

We've made the choice that marker namespaces are useful, but ids within that namespace are entirely opaque and don't have meaning - basically the approach of

marker.id = static_cast<int>(marker_array.markers.size() + 1);
marker_array.markers.push_back(marker);

In the general "visualize a whole SceneGraph" case, we use namespace = source name, and in other cases we break it down by body/frame names to allow markers corresponding to specific bodies to be selected. See SceneGraphCollisionChecker::MakeContextRvizRepresentationOfConfigurationCollisions for the most extreme case of this.

[hidmic]

That's a reasonable approach, but it won't work as-is if the geometries are being added and removed dynamically from the SceneGraph. Neither will this patch, though it's easy to adapt it (i.e. to track which geometry IDs have been published already).

[calderpg-tri]

It's not the only reason IDs may need to be tracked, but knowing what to delete later is the biggest reason we've needed to keep track of IDs published, especially when geometries may be added or removed. One solution to the issue of needing the track which IDs have been published (and also more useful generally as well) is to add a namespace-scoped DELETE_ALL action, so an entire namespace can be cleared at a time. IIRC from the last time I added it, the code change in Rviz is very simple, something like two lines - DELETE_ALL was already handled roughly like

if (marker.action == DELETE_ALL) {
  for (auto& ns : namespaces_) {
    ClearMarkersInNamespace(ns);
  }
}

and so the entire change was just

if (marker.action == DELETE_ALL) {
  for (auto& ns : namespaces_) {
    if (marker.ns == "" || marker.ns = ns) {
      ClearMarkersInNamespace(ns);
    }
  }
}

[calderpg-tri]

Looking at the code now, it's a little different than it was then/what I remembered, but the key method MarkerDisplay::deleteMarkersInNamespace is already there, so it's basically a question of doing

if (marker.action == DELETE_ALL) {
  if (marker.ns.empty()) {
    deleteAllMarkers();
  } else {
    deleteMarkersInNamespace(marker.ns);
  }
} 

[IanTheEngineer]

This seems like a reasonable upstream contribution to me. @hidmic do you think RViz maintainers would accept selective marker namespace clearing?

[hidmic]

I believe so, yeah. It seems practical to me. I can submit a patch.

[hidmic]

ros2/rviz#685 is up.

[hidmic]

ros2/rviz#685 was merged and released into Rolling. Feature's there.

[gbalke]

When working on some work for hydroelastic collisions I found out that there are some dimension translation discontinuities between ros markers and drake objects. I'm guessing there's an issue with the translations defined here: https://github.com/RobotLocomotion/drake-ros/blob/hidmic/generic-systems/drake_ros_systems/src/scene_markers_system.cpp

discontinuity.mp4

My example is available here: https://github.com/gbalke/drake-ros/tree/generic-systems-extended

[hidmic]

I found out that there are some dimension translation discontinuities between ros markers and drake objects

Wrong marker scales! See f01c6c3. Thanks @gbalke.

[gbalke]

Wrong marker scales! See f01c6c3. Thanks @gbalke.

Looking great, thanks @hidmic!

[sloretz]

All content from this PR has been merged as of #39. Closing 🎉