Generic Drake/ROS systems + RS Flip Flop example

drake_ros_systems/CMakeLists.txt

@@ -0,0 +1,81 @@
+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)
+# find_package(pybind11_vendor)
+# find_package(pybind11 REQUIRED)
+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
+)
+
+# pybind11_add_module(rccl SHARED
+#   use_ros.cpp
+# )
+# target_link_libraries(rccl PRIVATE
+#   rclcpp::rclcpp
+#   ${std_msgs_TARGETS}
+# )
+# 
+# # Sets PYTHON_INSTALL_DIR
+# _ament_cmake_python_get_python_install_dir()
+# 
+# install(
+#   TARGETS rccl
+#   DESTINATION "${PYTHON_INSTALL_DIR}"
+# )
+
+ament_package()
+

drake_ros_systems/README.md

@@ -0,0 +1,62 @@
+# 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`.
+
+# Code examples
+
+Create a system that publishes `std_msgs::msg::String`.
+
+```C++
+#include <drake_ros_systems/ros_interface_system.h>
+#include <drake_ros_systems/ros_publisher_system.h>
+
+using drake_ros_systems::RosInterfaceSystem;
+using drake_ros_systems::RosPublisherSystem;
+
+// This system initializes ROS and calls spin()
+// It creates a single ROS node called "drake_ros".
+auto spin_system = RosInterfaceSystem::Make(/* optionally rclcpp init options? */);
+
+const std::string topic{"chatter"};
+const double period_sec{0.1};  // 10Hz
+
+auto pub_system = RosPublisherSystem::Make<std_msgs::msg::String>(
+    spin_system.get_ros_interface(), topic, period_sec);
+
+auto pub_context = pub_system->CreateDefaultContext();
+std_msgs::msg::String pub_msg;
+pub_msg.data = "Hello from Drake";
+pub_context->FixInputPort(0,  AbstractValue::Make(pub_msg));
+pub_system->Publish(*pub_context);
+```
+
+Create a system that subscribes to `std_msgs::msg::String`.
+
+```C++
+#include <drake_ros_systems/ros_interface_system.h>
+#include <drake_ros_systems/ros_subsciber_system.h>
+
+using drake_ros_systems::RosInterfaceSystem;
+using drake_ros_systems::RosSubscriberSystem;
+
+// This system initializes ROS and calls spin()
+// It creates a single ROS node called "drake_ros".
+auto spin_system = RosInterfaceSystem(/* optionally rclcpp init options? */);
+
+const std::string topic{"chatter"};
+
+auto pub_system = RosSubscriberSystem::Make<std_msgs::msg::String>(
+    spin_system.get_ros_interface(), topic, period_sec);
+
+auto sub_context = sub_system->CreateDefaultContext();
+// somehow this sub context is added to a diagram builder with the system
+// so the subscriber can update that message
+
+// huh...?
+std_msgs::msg::String sub_msg = sub_context->get_output_stuff(...);
+```
+
+Could use an example of drake systems built with a diagram builder and connected to input/output ports of other systems.
+

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;
+}

drake_ros_systems/include/drake_ros_systems/drake_ros.hpp

@@ -0,0 +1,52 @@
+#ifndef DRAKE_ROS_SYSTEMS__DRAKE_ROS_HPP_
+#define DRAKE_ROS_SYSTEMS__DRAKE_ROS_HPP_
+
+#include <rosidl_runtime_c/message_type_support_struct.h>
+
+#include <drake_ros_systems/drake_ros_interface.hpp>
+
+#include <functional>
+#include <memory>
+#include <string>
+
+#include <rclcpp/qos.hpp>
+#include <rclcpp/serialized_message.hpp>
+
+namespace drake_ros_systems
+{
+/// PIMPL forward declarations
+class DrakeRosPrivate;
+class Publisher;
+class Subscription;
+
+/// System that abstracts working with ROS
+class DrakeRos final: public DrakeRosInterface
+{
+public:
+  DrakeRos();
+
+  virtual ~DrakeRos();
+
+  std::unique_ptr<Publisher>
+  create_publisher(
+    const rosidl_message_type_support_t & ts,
+    const std::string & topic_name,
+    const rclcpp::QoS & qos) final;
+
+  virtual
+  std::shared_ptr<Subscription>
+  create_subscription(
+    const rosidl_message_type_support_t & ts,
+    const std::string & topic_name,
+    const rclcpp::QoS & qos,
+    std::function<void(std::shared_ptr<rclcpp::SerializedMessage>)> callback) final;
+
+  void
+  spin(
+    int timeout_millis) final;
+
+private:
+  std::unique_ptr<DrakeRosPrivate> impl_;
+};
+}  // namespace drake_ros_systems
+#endif  // DRAKE_ROS_SYSTEMS__DRAKE_ROS_HPP_

drake_ros_systems/include/drake_ros_systems/drake_ros_interface.hpp

@@ -0,0 +1,44 @@
+#ifndef DRAKE_ROS_SYSTEMS__DRAKE_ROS_INTERFACE_HPP_
+#define DRAKE_ROS_SYSTEMS__DRAKE_ROS_INTERFACE_HPP_
+
+#include <rosidl_runtime_c/message_type_support_struct.h>
+
+#include <functional>
+#include <memory>
+#include <string>
+
+#include <rclcpp/qos.hpp>
+#include <rclcpp/serialized_message.hpp>
+
+namespace drake_ros_systems
+{
+// Forward declarations for non-public-API classes
+class Publisher;
+class Subscription;
+
+/// System that abstracts working with ROS
+class DrakeRosInterface
+{
+public:
+  virtual
+  std::unique_ptr<Publisher>
+  create_publisher(
+    const rosidl_message_type_support_t & ts,
+    const std::string & topic_name,
+    const rclcpp::QoS & qos) = 0;
+
+  virtual
+  std::shared_ptr<Subscription>
+  create_subscription(
+    const rosidl_message_type_support_t & ts,
+    const std::string & topic_name,
+    const rclcpp::QoS & qos,
+    std::function<void(std::shared_ptr<rclcpp::SerializedMessage>)> callback) = 0;
+
+  virtual
+  void
+  spin(
+    int timeout_millis) = 0;
+};
+}  // namespace drake_ros_systems
+#endif  // DRAKE_ROS_SYSTEMS__DRAKE_ROS_INTERFACE_HPP_