Generic Drake/ROS systems + RS Flip Flop example

[sloretz]

This adds generic ROS publisher/subscriber systems as described in RobotLocomotion/drake#9500. It's similar to https://github.com/gizatt/drake_ros_systems , but this uses ROS 2 and matches the LCM publisher/subscriber system's design more closely.

RS Flip Flop example

There's an example of an RS flip flop system that listens to topics /S, and /R and publishes to topics /Q and /Q_not.

To try this example out:

1. Install ROS Rolling - it might work with other ROS 2 versions, but I haven't tried it
2. Clone this repo into a colcon workspace
3. colcon build --packages-select drake_ros_systems
4. Run one of the rs_flip_flop examples
   • C++ from the build folder: ./build/drake_ros_systems/example/rs_flip_flop
   • Python from the example folder: python3 rs_flip_flop.py
5. Play with it with some of these commands:

Echo output topics:

$ ros2 topic echo /Q

ros2 topic echo /Q_not

Send values to the input topics

$ ros2 topic pub /S std_msgs/msg/Bool "data: false"
$ ros2 topic pub /S std_msgs/msg/Bool "data: true"

$ ros2 topic pub /R std_msgs/msg/Bool "data: false"
$ ros2 topic pub /R std_msgs/msg/Bool "data: true"

Future enhancements

C++

• Allow picking the executor and executor options in DrakeRos
• Add return value to spin functions in rclcpp that indicate f if spin did something, and if so pause Drake's concept of time to handle multiple messages just like LcmInterfaceSystem does.
• Replace Publisher and Subscriber classes with Generic Publisher and Subscriber being worked on in Add rclcpp_generic, a publisher and subscription for serialized messages ros2/rclcpp#1452

Python

• Use pybind11::capsule::get_pointer() when available

Code copying

The generic publisher and subscriber using raw messages was significantly copied from rosbag2_transport: https://github.com/ros2/rosbag2/tree/master/rosbag2_transport but it should probably be change to use these classes (not yet merged ros2/rclcpp#1452)

The Drake boilerplate in RosInterfaceSystem, RosSubscriberSystem, and RosPublisherSystem was significantly copied from the equivalent LCM systems in drake.

---

This change is 

[sloretz]

@IanTheEngineer @EricCousineau-TRI @azeey FYI

[azeey]

LGTM! However, I don't understand much of the code in py_serializer.hpp, so another pair of eyes would be welcome.

I have built and tested the example with both the colcon and cmake workflows, and they both work!

[EricCousineau-TRI]

LGTM! However, I don't understand much of the code in py_serializer.hpp, so another pair of eyes would be welcome.

Placing one pair of 👀 here

[EricCousineau-TRI]

(Going to use Reviewable tho)

[EricCousineau-TRI]

Reviewable status: 0 of 25 files reviewed, 3 unresolved discussions (waiting on @sloretz)

---

drake_ros_systems/src/python_bindings/py_serializer.hpp, line 30 at r4 (raw file):

{
// Serialize/Deserialize Python ROS types to rclcpp::SerializedMessage
class PySerializer : public SerializerInterface

It's a bit challenging to analyze and review Python code flow in C++.
(Also, pumping stuff back and forth btw the two may be less efficient?)

As my own rule of thumb, anything that calls pybind11s Python C++ API more than 3 times in succession should generally be done in a Python function.

Is it possible for you to relegate this code to Python, and use something like what Drake to does to invoke Python code?

Example:
https://github.com/RobotLocomotion/drake/blob/2baad6f34b12bfea1c583dfd63b0970b7eeb479a/bindings/pydrake/pydrake_pybind.h#L127

https://github.com/RobotLocomotion/drake/blob/2baad6f34b12bfea1c583dfd63b0970b7eeb479a/bindings/pydrake/__init__.py#L77-L80

[sloretz]

Reviewable status: 0 of 27 files reviewed, 3 unresolved discussions (waiting on @EricCousineau-TRI and @sloretz)

---

drake_ros_systems/src/python_bindings/py_serializer.hpp, line 30 at r4 (raw file):

Previously, EricCousineau-TRI (Eric Cousineau) wrote…

It's a bit challenging to analyze and review Python code flow in C++.
(Also, pumping stuff back and forth btw the two may be less efficient?)

As my own rule of thumb, anything that calls pybind11s Python C++ API more than 3 times in succession should generally be done in a Python function.

Is it possible for you to relegate this code to Python, and use something like what Drake to does to invoke Python code?

Example:
https://github.com/RobotLocomotion/drake/blob/2baad6f34b12bfea1c583dfd63b0970b7eeb479a/bindings/pydrake/pydrake_pybind.h#L127

https://github.com/RobotLocomotion/drake/blob/2baad6f34b12bfea1c583dfd63b0970b7eeb479a/bindings/pydrake/__init__.py#L77-L80

Relegated a bit more to Python in 22880a8

[sloretz]

I think this PR is just waiting on review. There's a little bit of test coverage now in the form of linters, a couple integration tests (one for C++, one for Python) and a small unit test for the DrakeRos class.

[sloretz]

Most of this is going into https://github.com/RobotLocomotion/drake-ros

I'll merge this and archive this repo.