Generic Drake/ROS systems + RS Flip Flop example

drake_ros_systems/CMakeLists.txt

@@ -0,0 +1,89 @@
+cmake_minimum_required(VERSION 3.10)
+project(drake_ros_systems)
+
+# Default to C99
+if(NOT CMAKE_C_STANDARD)
+  set(CMAKE_C_STANDARD 99)
+endif()
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic -Werror)
+endif()
+
+find_package(ament_cmake_ros REQUIRED)
+find_package(drake REQUIRED)
+# Must use Drake's fork of Pybind11
+find_package(pybind11 REQUIRED HINTS "${drake_DIR}/../pybind11" NO_DEFAULT_PATH)
+find_package(rclcpp REQUIRED)
+find_package(rosidl_runtime_c REQUIRED)
+find_package(rosidl_typesupport_cpp REQUIRED)
+
+add_library(drake_ros_systems
+  src/drake_ros.cpp
+  src/publisher.cpp
+  src/ros_interface_system.cpp
+  src/ros_publisher_system.cpp
+  src/ros_subscriber_system.cpp
+  src/subscription.cpp
+)
+target_link_libraries(drake_ros_systems PUBLIC
+  drake::drake
+  rclcpp::rclcpp
+  rosidl_runtime_c::rosidl_runtime_c
+  rosidl_typesupport_cpp::rosidl_typesupport_cpp
+)
+target_include_directories(drake_ros_systems
+  PUBLIC
+    "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+    "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src>"
+    "$<INSTALL_INTERFACE:include>"
+)
+
+add_subdirectory(example)
+
+ament_export_dependencies(drake)
+ament_export_dependencies(rclcpp)
+ament_export_dependencies(rosidl_generator_c)
+
+install(TARGETS ${PROJECT_NAME} EXPORT ${PROJECT_NAME}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+install(
+  DIRECTORY include/
+  DESTINATION include
+)
+
+###
+# Python bindings
+###
+pybind11_add_module(py_drake_ros_systems SHARED
+  src/python_bindings/module_drake_ros_systems.cpp
+)
+set_target_properties(py_drake_ros_systems PROPERTIES OUTPUT_NAME "drake_ros_systems")
+target_link_libraries(py_drake_ros_systems PRIVATE
+  drake_ros_systems
+)
+target_include_directories(drake_ros_systems
+  PRIVATE
+    "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/python_bindings>"
+)
+
+# Sets PYTHON_INSTALL_DIR
+_ament_cmake_python_get_python_install_dir()
+
+install(
+  TARGETS py_drake_ros_systems
+  DESTINATION "${PYTHON_INSTALL_DIR}"
+)
+### End Python bindings
+
+ament_package()
+

drake_ros_systems/README.md

@@ -0,0 +1,110 @@
+# Drake ROS Systems
+
+This is a ROS 2 prototype of a solution to [robotlocomotion/Drake#9500](https://github.com/RobotLocomotion/drake/issues/9500).
+It is similar to this ROS 1 prototype [`gizatt/drake_ros_systems`](https://github.com/gizatt/drake_ros_systems).
+It explores ROS 2 equivalents of `LcmInterfaceSystem`, `LcmPublisherSystem`, and `LcmSubscriberSystem`.
+
+# Building
+
+This package has been built and tested on Ubuntu Focal with ROS Rolling, using a Drake nightly from November 2020.
+It may work on other versions of ROS and Drake, but it hasn't been tested.
+
+To build it:
+
+1. [Install ROS Rolling](https://index.ros.org/doc/ros2/Installation/Rolling/)
+1. Source your ROS installation `. /opt/ros/rolling/setup.bash`
+1. [Install drake from November-ish 2020](https://drake.mit.edu/from_binary.html)
+1. Extract the Drake binary installation, install it's prerequisites, and [use this Python virutalenv trick](https://drake.mit.edu/python_bindings.html#inside-virtualenv).
+1. Activate the drake virtual environment
+1. Build it using Colcon, or using CMake directly
+
+    **Colcon**
+    1. Make a workspace `mkdir -p ./ws/src`
+    1. `cd ./ws/src`
+    1. Get this code `git clone https://github.com/sloretz/drake_ros2_demos.git`
+    1. `cd ..`
+    1. Build this package `colcon build --packages-select drake_ros_systems`
+
+    **CMake**
+    1. Manually set `CMAKE_PREFIX_PATH`: `export CMAKE_PREFIX_PATH=$CMAKE_PREFIX_PATH:$AMENT_PREFIX_PATH`
+    1. Get this code `git clone https://github.com/sloretz/drake_ros2_demos.git`
+    1. `cd drake_ros2_demos/drake_ros_systems`
+    1. Make a build and install folder to avoid installing to the whole system `mkdir build install`
+    1. `cd build`
+    1. Configure the project `cmake -DCMAKE_INSTALL_PREFIX=$(pwd)/../install ..`
+    1. Build the project `make && make install`
+
+# Example
+
+An example of using these systems is given in the [`example`](./example) folder in two languages: Python and C++.
+Both examples implement an RS flip flop using NOR gates.
+They subscribe to the following topics:
+
+* `/R`
+* `/S`
+
+And publish to the following topics
+
+* `/Q`
+* `/Q_not`
+
+Run these commands in different terminals with your ROS installation sourced to echo the output topics:
+
+```
+ros2 topic echo /Q
+```
+
+```
+ros2 topic echo /Q_not
+```
+
+Run these commands in different terminals with your ROS installation sourced to play with 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"
+```
+
+## Running the Example
+
+If you built with `colcon`, then source your workspace.
+
+```
+. ./ws/install/setup.bash
+# Also make sure to activate drake virtual environment
+```
+
+If you built with plain CMake, then source the ROS workspace and set these variables.
+
+```
+. /opt/ros/rolling/setup.bash
+# Also make sure to activate drake virtual environment
+# CD to folder containing `build` and `install` created earlier, commands below use PWD to find correct path
+export LD_LIBRARY_PATH=$(pwd)/install/lib:$LD_LIBRARY_PATH
+export PYTHONPATH=$(pwd)/install/lib/$(python -c 'import sys; print(f"python{sys.version_info[0]}.{sys.version_info[1]}")')/site-packages:$PYTHONPATH
+```
+
+Now you can run the C++ example from the build folder
+
+If built with **colcon** using an isolated build (default)
+
+```
+./build/drake_ros_systems/example/rs_flip_flop
+```
+
+If built with **cmake** or a non-isolated build of **colcon**
+
+```
+./build/example/rs_flip_flop
+```
+
+The Python example can be run from the source folder in either case.
+
+```
+python3 ./example/rs_flip_flop.py
+```

drake_ros_systems/example/CMakeLists.txt

@@ -0,0 +1,8 @@
+find_package(std_msgs REQUIRED)
+
+add_executable(rs_flip_flop rs_flip_flop.cpp)
+target_link_libraries(rs_flip_flop
+  drake::drake
+  drake_ros_systems
+  ${std_msgs_TARGETS}
+)

drake_ros_systems/example/rs_flip_flop.cpp

@@ -0,0 +1,134 @@
+#include <drake/systems/analysis/simulator.h>
+#include <drake/systems/framework/diagram_builder.h>
+#include <drake/systems/framework/leaf_system.h>
+
+#include <drake_ros_systems/drake_ros.hpp>
+#include <drake_ros_systems/ros_interface_system.hpp>
+#include <drake_ros_systems/ros_publisher_system.hpp>
+#include <drake_ros_systems/ros_subscriber_system.hpp>
+
+#include <std_msgs/msg/bool.hpp>
+
+using drake_ros_systems::DrakeRos;
+using drake_ros_systems::RosInterfaceSystem;
+using drake_ros_systems::RosPublisherSystem;
+using drake_ros_systems::RosSubscriberSystem;
+
+class NorGate : public drake::systems::LeafSystem<double>
+{
+public:
+  NorGate()
+  {
+    DeclareAbstractInputPort("A", *drake::AbstractValue::Make(std_msgs::msg::Bool()));
+    DeclareAbstractInputPort("B", *drake::AbstractValue::Make(std_msgs::msg::Bool()));
+
+    DeclareAbstractOutputPort("Q", &NorGate::calc_output_value);
+  }
+
+  virtual ~NorGate() = default;
+
+private:
+  void
+  calc_output_value(const drake::systems::Context<double> & context, std_msgs::msg::Bool * output) const
+  {
+    const bool a = GetInputPort("A").Eval<std_msgs::msg::Bool>(context).data;
+    const bool b = GetInputPort("B").Eval<std_msgs::msg::Bool>(context).data;
+    output->data = !(a || b);
+  }
+};
+
+// Delay's input port by one timestep to avoid algebraic loop error
+// Inspired by Simulink's Memory block
+template <typename T>
+class Memory : public drake::systems::LeafSystem<double>
+{
+public:
+  Memory(const T & initial_value)
+  {
+    DeclareAbstractInputPort("value", *drake::AbstractValue::Make(T()));
+
+    // State for value
+    DeclareAbstractState(drake::AbstractValue::Make(initial_value));
+
+    // Output depends only on the previous state
+    DeclareAbstractOutputPort("value", &Memory::calc_output_value, {all_state_ticket()});
+
+    DeclarePerStepEvent(
+      drake::systems::UnrestrictedUpdateEvent<double>([this](
+          const drake::systems::Context<double>& context,
+          const drake::systems::UnrestrictedUpdateEvent<double>&,
+          drake::systems::State<double> * state) {
+        // Copy input value to state
+        drake::systems::AbstractValues & abstract_state = state->get_mutable_abstract_state();
+        abstract_state.get_mutable_value(0).SetFrom(
+          get_input_port().Eval<drake::AbstractValue>(context));
+      }));
+  }
+
+  virtual ~Memory() = default;
+
+private:
+  void
+  calc_output_value(const drake::systems::Context<double> & context, T * output) const
+  {
+    *output = context.get_abstract_state().get_value(0).get_value<T>();
+  }
+};
+
+int main()
+{
+  // NOR gate RS flip flop example
+  // Input topics /S and /R are active high (true is logic 1 and false is logic 0)
+  // Output topics  /Q and /Q_not are active low (true is logic 0 and false is logic 1)
+
+  // Input/Output table
+  // S: false R: false | Q: no change  Q_not: no change
+  // S: true  R: false | Q: false      Q_not: true
+  // S: false R: true  | Q: true       Q_not: false
+  // S: true  R: true  | Q: invalid    Q_not: invalid
+  drake::systems::DiagramBuilder<double> builder;
+
+  rclcpp::QoS qos{10};
+
+  auto sys_ros_interface = builder.AddSystem<RosInterfaceSystem>(std::make_unique<DrakeRos>());
+  auto sys_pub_Q = builder.AddSystem(
+    RosPublisherSystem::Make<std_msgs::msg::Bool>(
+      "Q", qos, sys_ros_interface->get_ros_interface()));
+  auto sys_pub_Q_not = builder.AddSystem(
+    RosPublisherSystem::Make<std_msgs::msg::Bool>(
+      "Q_not", qos, sys_ros_interface->get_ros_interface()));
+  auto sys_sub_S = builder.AddSystem(
+    RosSubscriberSystem::Make<std_msgs::msg::Bool>(
+      "S", qos, sys_ros_interface->get_ros_interface()));
+  auto sys_sub_R = builder.AddSystem(
+    RosSubscriberSystem::Make<std_msgs::msg::Bool>(
+      "R", qos, sys_ros_interface->get_ros_interface()));
+  auto sys_nor_gate_1 = builder.AddSystem<NorGate>();
+  auto sys_nor_gate_2 = builder.AddSystem<NorGate>();
+  auto sys_memory = builder.AddSystem<Memory<std_msgs::msg::Bool>>(std_msgs::msg::Bool());
+
+  builder.Connect(sys_nor_gate_1->GetOutputPort("Q"), sys_memory->get_input_port(0));
+
+  builder.Connect(sys_sub_S->get_output_port(0), sys_nor_gate_1->GetInputPort("A"));
+  builder.Connect(sys_nor_gate_2->GetOutputPort("Q"), sys_nor_gate_1->GetInputPort("B"));
+
+  builder.Connect(sys_memory->get_output_port(0), sys_nor_gate_2->GetInputPort("A"));
+  builder.Connect(sys_sub_R->get_output_port(0), sys_nor_gate_2->GetInputPort("B"));
+
+  builder.Connect(sys_nor_gate_1->GetOutputPort("Q"), sys_pub_Q->get_input_port(0));
+  builder.Connect(sys_nor_gate_2->GetOutputPort("Q"), sys_pub_Q_not->get_input_port(0));
+
+  auto diagram = builder.Build();
+  auto context = diagram->CreateDefaultContext();
+
+  auto simulator = std::make_unique<drake::systems::Simulator<double>>(*diagram, std::move(context));
+  simulator->set_target_realtime_rate(1.0);
+  simulator->Initialize();
+
+  auto & simulator_context = simulator->get_mutable_context();
+
+  while (true) {
+    simulator->AdvanceTo(simulator_context.get_time() + 0.1);
+  }
+  return 0;
+}