Extend drake_ros_core package.

.github/workflows/main.yml

@@ -22,6 +22,8 @@ jobs:
       - uses: ros-tooling/[email protected]
         with:
           required-ros-distributions: ${{ matrix.ros_distribution }}
+      - name: Cope with Python 2 pollution
+        run: apt-get update && apt-get install -y python-is-python3
       - name: Build and test all packages
         uses: ros-tooling/[email protected]
         with:

drake_ros_core/CMakeLists.txt

@@ -12,17 +12,19 @@ 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_core
-  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
+  src/drake_ros.cc
+  src/publisher.cc
+  src/ros_interface_system.cc
+  src/ros_publisher_system.cc
+  src/ros_subscriber_system.cc
+  src/subscription.cc
 )
 
 target_link_libraries(drake_ros_core PUBLIC
@@ -59,25 +61,54 @@ install(
   DESTINATION include
 )
 
+ament_python_install_package(
+  drake_ros_core PACKAGE_DIR src/drake_ros_core)
+
+###
+# Python bindings
+###
+pybind11_add_module(drake_ros_core_py SHARED
+  src/python_bindings/drake_ros_core.cc
+)
+target_link_libraries(drake_ros_core_py PRIVATE drake_ros_core)
+set_target_properties(drake_ros_core_py PROPERTIES OUTPUT_NAME "_drake_ros_core")
+target_include_directories(drake_ros_core_py
+  PRIVATE
+    "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/python_bindings>"
+)
+
+ament_get_python_install_dir(python_install_dir)
+
+install(
+  TARGETS drake_ros_core_py
+  DESTINATION "${python_install_dir}"
+)
+### End Python bindings
+
 if(BUILD_TESTING)
   find_package(ament_cmake_clang_format REQUIRED)
   find_package(ament_cmake_cpplint REQUIRED)
   find_package(ament_cmake_gtest REQUIRED)
   find_package(ament_cmake_pytest REQUIRED)
+  find_package(ament_cmake_pycodestyle REQUIRED)
   find_package(test_msgs REQUIRED)
 
   ament_clang_format(CONFIG_FILE .clang-format)
   ament_cpplint()
 
-  ament_add_gtest(test_pub_sub test/test_pub_sub.cpp)
+  ament_pycodestyle(--config pycodestyle.ini)
+
+  ament_add_gtest(test_pub_sub test/test_pub_sub.cc)
 
   target_link_libraries(test_pub_sub
     drake::drake
     drake_ros_core
     ${test_msgs_TARGETS}
   )
 
-  ament_add_gtest(test_drake_ros test/test_drake_ros.cpp)
+  ament_add_pytest_test(test_pub_sub_py test/test_pub_sub.py)
+
+  ament_add_gtest(test_drake_ros test/test_drake_ros.cc)
   target_link_libraries(test_drake_ros
     drake_ros_core
   )

drake_ros_core/CPPLINT.cfg

@@ -41,6 +41,12 @@ filter=+build/pragma_once
 # Disable cpplint's include order.  We have our own via //tools:drakelint.
 filter=-build/include_order
 
+# TODO(hidmic): uncomment when ament_cpplint updates its vendored cpplint
+# version to support the following filters. Also remove corresponding NOLINT
+# codetags in sources.
+## Allow private, sibling include files.
+#filter=-build/include_subdir
+
 # We do not care about the whitespace details of a TODO comment.  It is not
 # relevant for easy grepping, and the GSG does not specify any particular
 # whitespace style.  (We *do* care what the "TODO(username)" itself looks like

drake_ros_core/include/drake_ros_core/drake_ros.h

@@ -0,0 +1,81 @@
+// Copyright 2021 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+#pragma once
+
+#include <memory>
+#include <string>
+
+#include <rclcpp/node.hpp>
+#include <rclcpp/node_options.hpp>
+
+namespace drake_ros_core {
+
+/** A Drake ROS interface that wraps a live ROS node.
+
+ This interface manages both ROS node construction and scheduling
+ (using a `rclcpp::executors::SingleThreadedExecutor` instance).
+ See `rclcpp::Node` and `rclcpp::Executor` documentation for further
+ reference on expected behavior.
+ */
+class DrakeRos final {
+ public:
+  /** A constructor that wraps a `node_name` ROS node with `node_options`.
+   See `rclcpp::Node::Node` documentation for further reference on arguments.
+   */
+  DrakeRos(const std::string& node_name,
+           rclcpp::NodeOptions node_options = rclcpp::NodeOptions{});
+
+  ~DrakeRos();
+
+  /** Returns a constant reference to the underlying ROS node. */
+  const rclcpp::Node& get_node() const;
+
+  /** Returns a mutable reference to the underlying ROS node. */
+  rclcpp::Node* get_mutable_node() const;
+
+  /** Spins the underlying ROS node, dispatching all available work if any.
+
+   In this context, work refers to subscription callbacks, timer callbacks,
+   service request and reply callbacks, etc., that are registered with the
+   underlying `rclcpp::Node` instance. Availability implies these are ready
+   to be serviced by the underlying `rclcpp::Executor` instance.
+
+   This method's behavior has been modeled after that of the
+   `drake::lcm::DrakeLcm::HandleSubscriptions()` method (to a partial extent).
+
+   @param[in] timeout_millis Timeout, in milliseconds, when fetching work.
+     Negative timeout values are not allowed. If timeout is 0, the call will
+     not wait for any new work. If timeout is larger than 0, the call will
+     continue fetching work up to the given timeout or until no work is
+     available.
+   @throws std::runtime_error if timeout is negative.
+   */
+  void Spin(int timeout_millis = 0);
+
+ private:
+  struct Impl;
+  std::unique_ptr<Impl> impl_;
+};
+
+/**Initialize Drake ROS's global context.
+ This function decorates a `rclcpp::init` invocation.
+*/
+void init(int argc = 0, const char** argv = nullptr);
+
+/**Shutdown Drake ROS's global context.
+ This function decorates a `rclcpp::shutdown` invocation.
+*/
+bool shutdown();
+
+}  // namespace drake_ros_core

drake_ros_core/include/drake_ros_core/ros_interface_system.hpp → drake_ros_core/include/drake_ros_core/ros_interface_system.h

@@ -11,35 +11,33 @@
 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 // See the License for the specific language governing permissions and
 // limitations under the License.
-#ifndef DRAKE_ROS_CORE__ROS_INTERFACE_SYSTEM_HPP_
-#define DRAKE_ROS_CORE__ROS_INTERFACE_SYSTEM_HPP_
+#pragma once
 
 #include <memory>
 
 #include <drake/systems/framework/leaf_system.h>
 
-#include "drake_ros_core/drake_ros_interface.hpp"
+#include "drake_ros_core/drake_ros.h"
 
 namespace drake_ros_core {
-// PIMPL forward declaration
-class RosInterfaceSystemPrivate;
-
-/// System that takes care of calling spin() in Drake's systems framework
+/** A system that manages a Drake ROS interface. */
 class RosInterfaceSystem : public drake::systems::LeafSystem<double> {
  public:
-  explicit RosInterfaceSystem(std::unique_ptr<DrakeRosInterface> ros);
-  virtual ~RosInterfaceSystem();
+  /** A constructor that takes ownership of the `ros` interface. */
+  explicit RosInterfaceSystem(std::unique_ptr<DrakeRos> ros);
+
+  ~RosInterfaceSystem() override;
 
-  /// Return a handle for interacting with ROS
-  std::shared_ptr<DrakeRosInterface> get_ros_interface() const;
+  /** Returns a mutable reference to the underlying ROS interface. */
+  DrakeRos* get_ros_interface() const;
 
  protected:
-  /// Override as a place to call rclcpp::spin()
   void DoCalcNextUpdateTime(const drake::systems::Context<double>&,
                             drake::systems::CompositeEventCollection<double>*,
                             double*) const override;
 
-  std::unique_ptr<RosInterfaceSystemPrivate> impl_;
+ private:
+  struct Impl;
+  std::unique_ptr<Impl> impl_;
 };
 }  // namespace drake_ros_core
-#endif  // DRAKE_ROS_CORE__ROS_INTERFACE_SYSTEM_HPP_