diff --git a/LICENSE b/LICENSE new file mode 100644 index 0000000..d645695 --- /dev/null +++ b/LICENSE @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + 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. diff --git a/drake_ros_systems/CMakeLists.txt b/drake_ros_systems/CMakeLists.txt new file mode 100644 index 0000000..ef3bbcf --- /dev/null +++ b/drake_ros_systems/CMakeLists.txt @@ -0,0 +1,113 @@ +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 + "$" + "$" + "$" +) + +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 + "$" +) + +# Sets PYTHON_INSTALL_DIR +_ament_cmake_python_get_python_install_dir() + +install( + TARGETS py_drake_ros_systems + DESTINATION "${PYTHON_INSTALL_DIR}" +) +### End Python bindings + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + find_package(test_msgs REQUIRED) + ament_lint_auto_find_test_dependencies() + + ament_add_gtest(test_integration test/integration.cpp) + + # TODO(sloretz) Why isn't pybind11::embed including python libs? + find_package(PythonLibs REQUIRED) + + target_link_libraries(test_integration + drake::drake + drake_ros_systems + ${test_msgs_TARGETS} + pybind11::embed + # TODO(sloretz) Remove when this is included in pybind11::embed + ${PYTHON_LIBRARIES} + ) + + ament_add_gtest(test_drake_ros test/drake_ros.cpp) + target_link_libraries(test_drake_ros + drake_ros_systems + ) +endif() + +ament_package() diff --git a/drake_ros_systems/README.md b/drake_ros_systems/README.md new file mode 100644 index 0000000..231f837 --- /dev/null +++ b/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 +``` diff --git a/drake_ros_systems/example/CMakeLists.txt b/drake_ros_systems/example/CMakeLists.txt new file mode 100644 index 0000000..284db04 --- /dev/null +++ b/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} +) diff --git a/drake_ros_systems/example/rs_flip_flop.cpp b/drake_ros_systems/example/rs_flip_flop.cpp new file mode 100644 index 0000000..16c38da --- /dev/null +++ b/drake_ros_systems/example/rs_flip_flop.cpp @@ -0,0 +1,154 @@ +// Copyright 2020 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. +#include +#include +#include + +#include +#include +#include +#include + +#include + +#include +#include + +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 +{ +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 & context, + std_msgs::msg::Bool * output) const + { + const bool a = GetInputPort("A").Eval(context).data; + const bool b = GetInputPort("B").Eval(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 +class Memory : public drake::systems::LeafSystem +{ +public: + explicit 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( + [this]( + const drake::systems::Context & context, + const drake::systems::UnrestrictedUpdateEvent &, + drake::systems::State * 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(context)); + })); + } + + virtual ~Memory() = default; + +private: + void + calc_output_value(const drake::systems::Context & context, T * output) const + { + *output = context.get_abstract_state().get_value(0).get_value(); + } +}; + +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 builder; + + rclcpp::QoS qos{10}; + + auto sys_ros_interface = builder.AddSystem(std::make_unique()); + auto sys_pub_Q = builder.AddSystem( + RosPublisherSystem::Make( + "Q", qos, sys_ros_interface->get_ros_interface())); + auto sys_pub_Q_not = builder.AddSystem( + RosPublisherSystem::Make( + "Q_not", qos, sys_ros_interface->get_ros_interface())); + auto sys_sub_S = builder.AddSystem( + RosSubscriberSystem::Make( + "S", qos, sys_ros_interface->get_ros_interface())); + auto sys_sub_R = builder.AddSystem( + RosSubscriberSystem::Make( + "R", qos, sys_ros_interface->get_ros_interface())); + auto sys_nor_gate_1 = builder.AddSystem(); + auto sys_nor_gate_2 = builder.AddSystem(); + auto sys_memory = builder.AddSystem>(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>(*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; +} diff --git a/drake_ros_systems/example/rs_flip_flop.py b/drake_ros_systems/example/rs_flip_flop.py new file mode 100755 index 0000000..708aee2 --- /dev/null +++ b/drake_ros_systems/example/rs_flip_flop.py @@ -0,0 +1,148 @@ +#!/usr/bin/env python3 +# Copyright 2020 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. + +from drake_ros_systems import RosInterfaceSystem +from drake_ros_systems import RosPublisherSystem +from drake_ros_systems import RosSubscriberSystem + +from pydrake.systems.analysis import Simulator +from pydrake.systems.framework import DiagramBuilder +from pydrake.systems.framework import UnrestrictedUpdateEvent +from pydrake.common.value import AbstractValue +from pydrake.systems.framework import LeafSystem + +from std_msgs.msg import Bool + +from rclpy.qos import QoSProfile + + +class NorGate(LeafSystem): + + def __init__(self): + super().__init__() + self._a = self.DeclareAbstractInputPort("A", AbstractValue.Make(Bool())) + self._b = self.DeclareAbstractInputPort("B", AbstractValue.Make(Bool())) + + self.DeclareAbstractOutputPort( + 'Q', + lambda: AbstractValue.Make(Bool()), + self._calc_output_value) + + def _calc_output_value(self, context, data): + a = self._a.Eval(context) + b = self._b.Eval(context) + data.get_mutable_value().data = not (a.data or b.data) + + +class Memory(LeafSystem): + """Delay input port by one time step to avoid algebraic loop error.""" + + def __init__(self, initial_value): + super().__init__() + + self._input = self.DeclareAbstractInputPort("A", AbstractValue.Make(initial_value)) + + self.DeclareAbstractState(AbstractValue.Make(initial_value)) + + self.DeclareAbstractOutputPort( + 'Q', + lambda: AbstractValue.Make(initial_value), + self._calc_output_value, + {self.all_state_ticket()}) + + self.DeclarePerStepEvent(UnrestrictedUpdateEvent(self._move_input_to_state)) + + def _move_input_to_state(self, context, event, state): + state.get_mutable_abstract_state().get_mutable_value(0).SetFrom( + AbstractValue.Make(self._input.Eval(context))) + + def _calc_output_value(self, context, output): + output.SetFrom(context.get_abstract_state().get_value(0)) + + +def 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 + builder = DiagramBuilder() + + sys_ros_interface = builder.AddSystem(RosInterfaceSystem()) + + qos = QoSProfile(depth=10) + + sys_pub_Q = builder.AddSystem( + RosPublisherSystem(Bool, "Q", qos, sys_ros_interface.get_ros_interface())) + sys_pub_Q_not = builder.AddSystem( + RosPublisherSystem(Bool, "Q_not", qos, sys_ros_interface.get_ros_interface())) + + sys_sub_S = builder.AddSystem( + RosSubscriberSystem(Bool, "S", qos, sys_ros_interface.get_ros_interface())) + sys_sub_R = builder.AddSystem( + RosSubscriberSystem(Bool, "R", qos, sys_ros_interface.get_ros_interface())) + + sys_nor_gate_1 = builder.AddSystem(NorGate()) + sys_nor_gate_2 = builder.AddSystem(NorGate()) + + sys_memory = builder.AddSystem(Memory(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) + ) + + diagram = builder.Build() + simulator = Simulator(diagram) + simulator_context = simulator.get_mutable_context() + simulator.set_target_realtime_rate(1.0) + + while True: + simulator.AdvanceTo(simulator_context.get_time() + 0.1) + + +if __name__ == '__main__': + main() diff --git a/drake_ros_systems/include/drake_ros_systems/drake_ros.hpp b/drake_ros_systems/include/drake_ros_systems/drake_ros.hpp new file mode 100644 index 0000000..b0d07a4 --- /dev/null +++ b/drake_ros_systems/include/drake_ros_systems/drake_ros.hpp @@ -0,0 +1,69 @@ +// Copyright 2020 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. +#ifndef DRAKE_ROS_SYSTEMS__DRAKE_ROS_HPP_ +#define DRAKE_ROS_SYSTEMS__DRAKE_ROS_HPP_ + +#include +#include +#include +#include + +#include +#include +#include + +#include "drake_ros_systems/drake_ros_interface.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() + : DrakeRos("DrakeRosSystems", rclcpp::NodeOptions{}) {} + + DrakeRos( + const std::string & node_name, + rclcpp::NodeOptions node_options); + + virtual ~DrakeRos(); + + std::unique_ptr + create_publisher( + const rosidl_message_type_support_t & ts, + const std::string & topic_name, + const rclcpp::QoS & qos) final; + + std::shared_ptr + create_subscription( + const rosidl_message_type_support_t & ts, + const std::string & topic_name, + const rclcpp::QoS & qos, + std::function)> callback) final; + + void + spin( + int timeout_millis) final; + +private: + std::unique_ptr impl_; +}; +} // namespace drake_ros_systems +#endif // DRAKE_ROS_SYSTEMS__DRAKE_ROS_HPP_ diff --git a/drake_ros_systems/include/drake_ros_systems/drake_ros_interface.hpp b/drake_ros_systems/include/drake_ros_systems/drake_ros_interface.hpp new file mode 100644 index 0000000..775477d --- /dev/null +++ b/drake_ros_systems/include/drake_ros_systems/drake_ros_interface.hpp @@ -0,0 +1,56 @@ +// Copyright 2020 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. +#ifndef DRAKE_ROS_SYSTEMS__DRAKE_ROS_INTERFACE_HPP_ +#define DRAKE_ROS_SYSTEMS__DRAKE_ROS_INTERFACE_HPP_ + +#include +#include +#include + +#include +#include +#include + +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 + create_publisher( + const rosidl_message_type_support_t & ts, + const std::string & topic_name, + const rclcpp::QoS & qos) = 0; + + virtual + std::shared_ptr + create_subscription( + const rosidl_message_type_support_t & ts, + const std::string & topic_name, + const rclcpp::QoS & qos, + std::function)> callback) = 0; + + virtual + void + spin( + int timeout_millis) = 0; +}; +} // namespace drake_ros_systems +#endif // DRAKE_ROS_SYSTEMS__DRAKE_ROS_INTERFACE_HPP_ diff --git a/drake_ros_systems/include/drake_ros_systems/ros_interface_system.hpp b/drake_ros_systems/include/drake_ros_systems/ros_interface_system.hpp new file mode 100644 index 0000000..2d9d3f5 --- /dev/null +++ b/drake_ros_systems/include/drake_ros_systems/ros_interface_system.hpp @@ -0,0 +1,49 @@ +// Copyright 2020 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. +#ifndef DRAKE_ROS_SYSTEMS__ROS_INTERFACE_SYSTEM_HPP_ +#define DRAKE_ROS_SYSTEMS__ROS_INTERFACE_SYSTEM_HPP_ + +#include + +#include + +#include "drake_ros_systems/drake_ros_interface.hpp" + +namespace drake_ros_systems +{ +// PIMPL forward declaration +class RosInterfaceSystemPrivate; + +/// System that takes care of calling spin() in Drake's systems framework +class RosInterfaceSystem : public drake::systems::LeafSystem +{ +public: + explicit RosInterfaceSystem(std::unique_ptr ros); + virtual ~RosInterfaceSystem(); + + /// Return a handle for interacting with ROS + std::shared_ptr + get_ros_interface() const; + +protected: + /// Override as a place to call rclcpp::spin() + void DoCalcNextUpdateTime( + const drake::systems::Context &, + drake::systems::CompositeEventCollection *, + double *) const override; + + std::unique_ptr impl_; +}; +} // namespace drake_ros_systems +#endif // DRAKE_ROS_SYSTEMS__ROS_INTERFACE_SYSTEM_HPP_ diff --git a/drake_ros_systems/include/drake_ros_systems/ros_publisher_system.hpp b/drake_ros_systems/include/drake_ros_systems/ros_publisher_system.hpp new file mode 100644 index 0000000..051b8f8 --- /dev/null +++ b/drake_ros_systems/include/drake_ros_systems/ros_publisher_system.hpp @@ -0,0 +1,101 @@ +// Copyright 2020 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. +#ifndef DRAKE_ROS_SYSTEMS__ROS_PUBLISHER_SYSTEM_HPP_ +#define DRAKE_ROS_SYSTEMS__ROS_PUBLISHER_SYSTEM_HPP_ + +#include +#include + +#include + +#include +#include +#include + +#include "drake_ros_systems/drake_ros_interface.hpp" +#include "drake_ros_systems/serializer.hpp" +#include "drake_ros_systems/serializer_interface.hpp" + + +namespace drake_ros_systems +{ +/// PIMPL forward declaration +class RosPublisherSystemPrivate; + +/// Accepts ROS messages on an input port and publishes them to a ROS topic +class RosPublisherSystem : public drake::systems::LeafSystem +{ +public: + /// Convenience method to make a publisher system given a ROS message type + template + static + std::unique_ptr + Make( + const std::string & topic_name, + const rclcpp::QoS & qos, + std::shared_ptr ros_interface) + { + return Make( + topic_name, qos, ros_interface, + {drake::systems::TriggerType::kPerStep, drake::systems::TriggerType::kForced}, 0.0); + } + + /// Convenience method to make a publisher system given a ROS message type and publish triggers + template + static + std::unique_ptr + Make( + const std::string & topic_name, + const rclcpp::QoS & qos, + std::shared_ptr ros_interface, + const std::unordered_set & publish_triggers, + double publish_period = 0.0) + { + // Assume C++ typesupport since this is a C++ template function + std::unique_ptr serializer = std::make_unique>(); + return std::make_unique( + serializer, topic_name, qos, ros_interface, publish_triggers, publish_period); + } + + RosPublisherSystem( + std::unique_ptr & serializer, + const std::string & topic_name, + const rclcpp::QoS & qos, + std::shared_ptr ros_interface, + const std::unordered_set & publish_triggers, + double publish_period = 0.0); + + virtual ~RosPublisherSystem(); + + /// Convenience method to publish a C++ ROS message + template + void + publish(const MessageT & message) + { + static const Serializer serializer; + publish(serializer->serialize(message)); + } + + /// Publish a serialized ROS message + void + publish(const rclcpp::SerializedMessage & serialized_msg); + +protected: + drake::systems::EventStatus + publish_input(const drake::systems::Context & context) const; + + std::unique_ptr impl_; +}; +} // namespace drake_ros_systems +#endif // DRAKE_ROS_SYSTEMS__ROS_PUBLISHER_SYSTEM_HPP_ diff --git a/drake_ros_systems/include/drake_ros_systems/ros_subscriber_system.hpp b/drake_ros_systems/include/drake_ros_systems/ros_subscriber_system.hpp new file mode 100644 index 0000000..e29eb28 --- /dev/null +++ b/drake_ros_systems/include/drake_ros_systems/ros_subscriber_system.hpp @@ -0,0 +1,68 @@ +// Copyright 2020 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. +#ifndef DRAKE_ROS_SYSTEMS__ROS_SUBSCRIBER_SYSTEM_HPP_ +#define DRAKE_ROS_SYSTEMS__ROS_SUBSCRIBER_SYSTEM_HPP_ + +#include +#include + +#include +#include + +#include "drake_ros_systems/drake_ros_interface.hpp" +#include "drake_ros_systems/serializer.hpp" +#include "drake_ros_systems/serializer_interface.hpp" + +namespace drake_ros_systems +{ +// PIMPL forward declaration +class RosSubscriberSystemPrivate; + +/// System that subscribes to a ROS topic and makes it available on an output port +class RosSubscriberSystem : public drake::systems::LeafSystem +{ +public: + /// Convenience method to make a subscriber system given a ROS message type + template + static + std::unique_ptr + Make( + const std::string & topic_name, + const rclcpp::QoS & qos, + std::shared_ptr ros_interface) + { + // Assume C++ typesupport since this is a C++ template function + std::unique_ptr serializer = std::make_unique>(); + return std::make_unique(serializer, topic_name, qos, ros_interface); + } + + RosSubscriberSystem( + std::unique_ptr & serializer, + const std::string & topic_name, + const rclcpp::QoS & qos, + std::shared_ptr ros_interface); + + virtual ~RosSubscriberSystem(); + +protected: + /// Override as a place to schedule event to move ROS message into a context + void DoCalcNextUpdateTime( + const drake::systems::Context &, + drake::systems::CompositeEventCollection *, + double *) const override; + + std::unique_ptr impl_; +}; +} // namespace drake_ros_systems +#endif // DRAKE_ROS_SYSTEMS__ROS_SUBSCRIBER_SYSTEM_HPP_ diff --git a/drake_ros_systems/include/drake_ros_systems/serializer.hpp b/drake_ros_systems/include/drake_ros_systems/serializer.hpp new file mode 100644 index 0000000..60301e9 --- /dev/null +++ b/drake_ros_systems/include/drake_ros_systems/serializer.hpp @@ -0,0 +1,74 @@ +// Copyright 2020 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. +#ifndef DRAKE_ROS_SYSTEMS__SERIALIZER_HPP_ +#define DRAKE_ROS_SYSTEMS__SERIALIZER_HPP_ + +#include +#include + +#include +#include + +#include + +#include "drake_ros_systems/serializer_interface.hpp" + +namespace drake_ros_systems +{ +template +class Serializer : public SerializerInterface +{ +public: + rclcpp::SerializedMessage + serialize(const drake::AbstractValue & abstract_value) const override + { + rclcpp::SerializedMessage serialized_msg; + const MessageT & message = abstract_value.get_value(); + const auto ret = rmw_serialize( + &message, + get_type_support(), + &serialized_msg.get_rcl_serialized_message()); + if (ret != RMW_RET_OK) { + // TODO(sloretz) do something if serialization fails + (void)ret; + } + return serialized_msg; + } + + bool + deserialize( + const rclcpp::SerializedMessage & serialized_message, + drake::AbstractValue & abstract_value) const override + { + const auto ret = rmw_deserialize( + &serialized_message.get_rcl_serialized_message(), + get_type_support(), + &abstract_value.get_mutable_value()); + return ret == RMW_RET_OK; + } + + std::unique_ptr + create_default_value() const override + { + return std::make_unique>(MessageT()); + } + + const rosidl_message_type_support_t * + get_type_support() const override + { + return rosidl_typesupport_cpp::get_message_type_support_handle(); + } +}; +} // namespace drake_ros_systems +#endif // DRAKE_ROS_SYSTEMS__SERIALIZER_HPP_ diff --git a/drake_ros_systems/include/drake_ros_systems/serializer_interface.hpp b/drake_ros_systems/include/drake_ros_systems/serializer_interface.hpp new file mode 100644 index 0000000..7f3354e --- /dev/null +++ b/drake_ros_systems/include/drake_ros_systems/serializer_interface.hpp @@ -0,0 +1,47 @@ +// Copyright 2020 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. +#ifndef DRAKE_ROS_SYSTEMS__SERIALIZER_INTERFACE_HPP_ +#define DRAKE_ROS_SYSTEMS__SERIALIZER_INTERFACE_HPP_ + +#include +#include +#include + +#include + +namespace drake_ros_systems +{ +class SerializerInterface +{ +public: + virtual + rclcpp::SerializedMessage + serialize(const drake::AbstractValue & abstract_value) const = 0; + + virtual + bool + deserialize( + const rclcpp::SerializedMessage & message, + drake::AbstractValue & abstract_value) const = 0; + + virtual + std::unique_ptr + create_default_value() const = 0; + + virtual + const rosidl_message_type_support_t * + get_type_support() const = 0; +}; +} // namespace drake_ros_systems +#endif // DRAKE_ROS_SYSTEMS__SERIALIZER_INTERFACE_HPP_ diff --git a/drake_ros_systems/package.xml b/drake_ros_systems/package.xml new file mode 100644 index 0000000..0f5d0d2 --- /dev/null +++ b/drake_ros_systems/package.xml @@ -0,0 +1,22 @@ + + + drake_ros_systems + 0.1.0 + Drake systems for interacting with ROS 2. + Shane Loretz + Apache License 2.0 + + ament_cmake_ros + + rclcpp + rosidl_runtime_c + rosidl_typesupport_cpp + + ament_cmake_gtest + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/drake_ros_systems/src/drake_ros.cpp b/drake_ros_systems/src/drake_ros.cpp new file mode 100644 index 0000000..ca0b374 --- /dev/null +++ b/drake_ros_systems/src/drake_ros.cpp @@ -0,0 +1,103 @@ +// Copyright 2020 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. + +#include +#include + +#include +#include + +#include "drake_ros_systems/drake_ros.hpp" +#include "publisher.hpp" +#include "subscription.hpp" + +namespace drake_ros_systems +{ +class DrakeRosPrivate +{ +public: + bool externally_init_; + rclcpp::Context::SharedPtr context_; + rclcpp::Node::UniquePtr node_; + rclcpp::executors::SingleThreadedExecutor::UniquePtr executor_; +}; + +DrakeRos::DrakeRos( + const std::string & node_name, + rclcpp::NodeOptions node_options) +: impl_(new DrakeRosPrivate()) +{ + if (!node_options.context()) { + // Require context is constructed (NodeOptions uses Global Context by default) + throw std::invalid_argument("NodeOptions must contain a non-null context"); + } + impl_->context_ = node_options.context(); + if (impl_->context_->is_valid()) { + // Context is being init/shutdown outside of this system + impl_->externally_init_ = true; + } else { + // This system will init/shutdown the context + impl_->externally_init_ = false; + impl_->context_->init(0, nullptr); + } + + impl_->node_.reset(new rclcpp::Node(node_name, node_options)); + + // TODO(sloretz) allow passing in executor options + rclcpp::ExecutorOptions eo; + eo.context = impl_->context_; + impl_->executor_.reset(new rclcpp::executors::SingleThreadedExecutor(eo)); + + impl_->executor_->add_node(impl_->node_->get_node_base_interface()); +} + +DrakeRos::~DrakeRos() +{ + if (!impl_->externally_init_) { + // This system init'd the context, so this system will shut it down too. + impl_->context_->shutdown("~DrakeRos()"); + } +} + +std::unique_ptr +DrakeRos::create_publisher( + const rosidl_message_type_support_t & ts, + const std::string & topic_name, + const rclcpp::QoS & qos) +{ + return std::make_unique( + impl_->node_->get_node_base_interface().get(), ts, topic_name, qos); +} + +std::shared_ptr +DrakeRos::create_subscription( + const rosidl_message_type_support_t & ts, + const std::string & topic_name, + const rclcpp::QoS & qos, + std::function)> callback) +{ + auto sub = std::make_shared( + impl_->node_->get_node_base_interface().get(), ts, topic_name, qos, callback); + impl_->node_->get_node_topics_interface()->add_subscription(sub, nullptr); + + return sub; +} + +void +DrakeRos::spin( + int timeout_millis) +{ + impl_->executor_->spin_some(std::chrono::milliseconds(timeout_millis)); +} +} // namespace drake_ros_systems diff --git a/drake_ros_systems/src/publisher.cpp b/drake_ros_systems/src/publisher.cpp new file mode 100644 index 0000000..f726e00 --- /dev/null +++ b/drake_ros_systems/src/publisher.cpp @@ -0,0 +1,53 @@ +// Copyright 2020 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. + +#include + +#include "publisher.hpp" + +namespace drake_ros_systems +{ +// Copied from rosbag2_transport rosbag2_get_publisher_options +rcl_publisher_options_t publisher_options(const rclcpp::QoS & qos) +{ + auto options = rcl_publisher_get_default_options(); + options.qos = qos.get_rmw_qos_profile(); + return options; +} + +Publisher::Publisher( + rclcpp::node_interfaces::NodeBaseInterface * node_base, + const rosidl_message_type_support_t & type_support, + const std::string & topic_name, + const rclcpp::QoS & qos) +: rclcpp::PublisherBase(node_base, topic_name, type_support, publisher_options(qos)) +{ +} + +Publisher::~Publisher() +{ +} + +void +Publisher::publish(const rclcpp::SerializedMessage & serialized_msg) +{ + // TODO(sloretz) Copied from rosbag2_transport GenericPublisher, can it be upstreamed to rclcpp? + auto return_code = rcl_publish_serialized_message( + get_publisher_handle().get(), &serialized_msg.get_rcl_serialized_message(), NULL); + + if (return_code != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error(return_code, "failed to publish serialized message"); + } +} +} // namespace drake_ros_systems diff --git a/drake_ros_systems/src/publisher.hpp b/drake_ros_systems/src/publisher.hpp new file mode 100644 index 0000000..623a357 --- /dev/null +++ b/drake_ros_systems/src/publisher.hpp @@ -0,0 +1,44 @@ +// Copyright 2020 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. +#ifndef PUBLISHER_HPP_ +#define PUBLISHER_HPP_ + +#include + +#include +#include +#include + +#include +#include + + +namespace drake_ros_systems +{ +class Publisher final : public rclcpp::PublisherBase +{ +public: + Publisher( + rclcpp::node_interfaces::NodeBaseInterface * node_base, + const rosidl_message_type_support_t & ts, + const std::string & topic_name, + const rclcpp::QoS & qos); + + ~Publisher(); + + void + publish(const rclcpp::SerializedMessage & serialized_msg); +}; +} // namespace drake_ros_systems +#endif // PUBLISHER_HPP_ diff --git a/drake_ros_systems/src/python_bindings/module_drake_ros_systems.cpp b/drake_ros_systems/src/python_bindings/module_drake_ros_systems.cpp new file mode 100644 index 0000000..977395e --- /dev/null +++ b/drake_ros_systems/src/python_bindings/module_drake_ros_systems.cpp @@ -0,0 +1,112 @@ +// Copyright 2020 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. +#include + +#include + +#include +#include + +#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 "py_serializer.hpp" +#include "qos_type_caster.hpp" + +namespace py = pybind11; + +using drake::systems::LeafSystem; +using drake::systems::TriggerType; + +using drake_ros_systems::DrakeRos; +using drake_ros_systems::DrakeRosInterface; +using drake_ros_systems::PySerializer; +using drake_ros_systems::RosInterfaceSystem; +using drake_ros_systems::RosPublisherSystem; +using drake_ros_systems::RosSubscriberSystem; +using drake_ros_systems::SerializerInterface; + + +PYBIND11_MODULE(drake_ros_systems, m) { + m.doc() = "Python wrapper for drake_ros_systems"; + + py::module::import("pydrake.systems.framework"); + + // Use std::shared_ptr holder so pybind11 doesn't try to delete interfaces returned from + // get_ros_interface + py::class_>(m, "DrakeRosInterface"); + + py::class_>(m, "RosInterfaceSystem") + .def( + py::init( + []() {return std::make_unique(std::make_unique());})) + .def("get_ros_interface", &RosInterfaceSystem::get_ros_interface); + + py::class_>(m, "RosPublisherSystem") + .def( + py::init( + []( + pybind11::object type, + const char * topic_name, + drake_ros_systems::QoS qos, + std::shared_ptr ros_interface) + { + std::unique_ptr serializer = std::make_unique(type); + return std::make_unique( + serializer, + topic_name, + qos, + ros_interface, + std::unordered_set{TriggerType::kPerStep, TriggerType::kForced}, + 0.0); + })) + .def( + py::init( + []( + pybind11::object type, + const char * topic_name, + drake_ros_systems::QoS qos, + std::shared_ptr ros_interface, + std::unordered_set publish_triggers, + double publish_period) + { + std::unique_ptr serializer = std::make_unique(type); + return std::make_unique( + serializer, + topic_name, + qos, + ros_interface, + publish_triggers, + publish_period); + })); + + py::class_>(m, "RosSubscriberSystem") + .def( + py::init( + []( + pybind11::object type, + const char * topic_name, + drake_ros_systems::QoS qos, + std::shared_ptr ros_interface) + { + std::unique_ptr serializer = std::make_unique(type); + return std::make_unique( + serializer, + topic_name, + qos, + ros_interface); + })); +} diff --git a/drake_ros_systems/src/python_bindings/py_serializer.hpp b/drake_ros_systems/src/python_bindings/py_serializer.hpp new file mode 100644 index 0000000..f5fd6d3 --- /dev/null +++ b/drake_ros_systems/src/python_bindings/py_serializer.hpp @@ -0,0 +1,181 @@ +// Copyright 2020 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. +#ifndef PYTHON_BINDINGS__PY_SERIALIZER_HPP_ +#define PYTHON_BINDINGS__PY_SERIALIZER_HPP_ + +#include +#include +#include + +#include + +#include "drake_ros_systems/serializer_interface.hpp" + +namespace py = pybind11; + +namespace drake_ros_systems +{ +// Serialize/Deserialize Python ROS types to rclcpp::SerializedMessage +class PySerializer : public SerializerInterface +{ +public: + explicit PySerializer(py::object message_type) + : message_type_(message_type) + { + py::dict scope; + py::exec( + R"delim( +def get_typesupport(msg_type, attribute_name): + metaclass = msg_type.__class__ + if metaclass._TYPE_SUPPORT is None: + # Import typesupport if not done already + metaclass.__import_type_support__() + return getattr(metaclass, attribute_name) + + +def make_abstract_value(some_type): + from pydrake.common.value import AbstractValue + return AbstractValue.Make(some_type()) + )delim", + py::globals(), scope); + + py_make_abstract_value_ = scope["make_abstract_value"]; + py::object py_get_typesupport = scope["get_typesupport"]; + + // Get type support capsule and pointer + auto typesupport = py::cast(py_get_typesupport(message_type, "_TYPE_SUPPORT")); + // TODO(sloretz) use get_pointer() in py 2.6+ + type_support_ = static_cast(typesupport); + + auto convert_from_py = + py::cast(py_get_typesupport(message_type, "_CONVERT_FROM_PY")); + // TODO(sloretz) use get_pointer() in py 2.6+ + convert_from_py_ = reinterpret_cast( + static_cast(convert_from_py)); + + auto convert_to_py = + py::cast(py_get_typesupport(message_type, "_CONVERT_TO_PY")); + // TODO(sloretz) use get_pointer() in py 2.6+ + convert_to_py_ = reinterpret_cast( + static_cast(convert_to_py)); + + auto create_ros_message = + py::cast(py_get_typesupport(message_type, "_CREATE_ROS_MESSAGE")); + // TODO(sloretz) use get_pointer() in py 2.6+ + create_ros_message_ = reinterpret_cast( + static_cast(create_ros_message)); + + auto destroy_ros_message = + py::cast(py_get_typesupport(message_type, "_DESTROY_ROS_MESSAGE")); + // TODO(sloretz) use get_pointer() in py 2.6+ + destroy_ros_message_ = reinterpret_cast( + static_cast(destroy_ros_message)); + } + + rclcpp::SerializedMessage + serialize(const drake::AbstractValue & abstract_value) const override + { + // convert from inaccessible drake::pydrake::Object type + py::dict scope; + scope["av"] = &abstract_value; + py::object message = py::eval("av.Clone().get_mutable_value()", scope); + + // Create C ROS message + auto c_ros_message = std::unique_ptr( + create_ros_message_(), destroy_ros_message_); + + // Convert the Python message to a C ROS message + if (!convert_from_py_(message.ptr(), c_ros_message.get())) { + throw std::runtime_error("Failed to convert Python message to C ROS message"); + } + + // Serialize the C message + rclcpp::SerializedMessage serialized_msg; + const auto ret = rmw_serialize( + c_ros_message.get(), + type_support_, + &serialized_msg.get_rcl_serialized_message()); + if (ret != RMW_RET_OK) { + throw std::runtime_error("Failed to serialize C ROS message"); + } + return serialized_msg; + } + + bool + deserialize( + const rclcpp::SerializedMessage & serialized_message, + drake::AbstractValue & abstract_value) const override + { + // TODO(sloretz) it would be so much more convenient if I didn't have to + // care that the Python typesupport used the C type support internally. + // Why isn't this encapsulated in the python type support itself? + + // Create a C type support version of the message + auto c_ros_message = std::unique_ptr( + create_ros_message_(), destroy_ros_message_); + if (nullptr == c_ros_message.get()) { + return false; + } + + // Deserialize to C message type + const auto ret = rmw_deserialize( + &serialized_message.get_rcl_serialized_message(), + type_support_, + c_ros_message.get()); + + if (RMW_RET_OK != ret) { + return false; + } + + // Convert C type to Python type + PyObject * pymessage = convert_to_py_(c_ros_message.get()); + if (!pymessage) { + return false; + } + + // Store the Python message in the AbstractValue + // convert to inaccessible drake::pydrake::Object type + py::dict scope; + scope["av"] = &abstract_value; + scope["message"] = pymessage; + py::exec("av.set_value(message)", scope); + + return true; + } + + std::unique_ptr + create_default_value() const override + { + return py::cast>(py_make_abstract_value_(message_type_)); + } + + const rosidl_message_type_support_t * + get_type_support() const override + { + return type_support_; + } + +private: + py::object message_type_; + rosidl_message_type_support_t * type_support_; + + py::object py_make_abstract_value_; + + bool (* convert_from_py_)(PyObject *, void *); + PyObject * (* convert_to_py_)(void *); + void * (* create_ros_message_)(void); + void (* destroy_ros_message_)(void *); +}; +} // namespace drake_ros_systems +#endif // PYTHON_BINDINGS__PY_SERIALIZER_HPP_ diff --git a/drake_ros_systems/src/python_bindings/qos_type_caster.hpp b/drake_ros_systems/src/python_bindings/qos_type_caster.hpp new file mode 100644 index 0000000..217c226 --- /dev/null +++ b/drake_ros_systems/src/python_bindings/qos_type_caster.hpp @@ -0,0 +1,269 @@ +// Copyright 2020 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. +#ifndef PYTHON_BINDINGS__QOS_TYPE_CASTER_HPP_ +#define PYTHON_BINDINGS__QOS_TYPE_CASTER_HPP_ + +#include + +#include + +#include + +namespace drake_ros_systems +{ +// Thin wrapper that adds default constructor +class QoS : public rclcpp::QoS +{ +public: + QoS() + : rclcpp::QoS(1) {} +}; +} // namespace drake_ros_systems + +namespace pybind11 +{ +namespace detail +{ +template<> +struct type_caster +{ +public: + // declares local 'value' of type rclcpp::QoS + PYBIND11_TYPE_CASTER(drake_ros_systems::QoS, _("rclpy.qos.QoSProfile")); + + // Convert from Python rclpy.qos.QoSProfile to rclcpp::QoS + bool load(handle src, bool) + { + /* Extract PyObject from handle */ + PyObject * source = src.ptr(); + + // history : enum int + // depth : int + // reliability : enum int + // durability : enum int + // lifespan : rclpy.Duration + // deadline : rclpy.Duration + // liveliness : enum int + // liveliness_lease_duration : rclpy.Duration + // avoid_ros_namespace_conventions : bool + + PyObject * py_history = nullptr; + PyObject * py_depth = nullptr; + PyObject * py_reliability = nullptr; + PyObject * py_durability = nullptr; + PyObject * py_lifespan = nullptr; + PyObject * py_lifespan_ns = nullptr; + PyObject * py_deadline = nullptr; + PyObject * py_deadline_ns = nullptr; + PyObject * py_liveliness = nullptr; + PyObject * py_liveliness_lease_duration = nullptr; + PyObject * py_liveliness_lease_duration_ns = nullptr; + PyObject * py_avoid_ros_namespace_conventions = nullptr; + + // Clean up references when function exits. + std::unique_ptr> scope_exit( + nullptr, [&](void *) { + Py_XDECREF(py_avoid_ros_namespace_conventions); + Py_XDECREF(py_liveliness_lease_duration_ns); + Py_XDECREF(py_liveliness_lease_duration); + Py_XDECREF(py_liveliness); + Py_XDECREF(py_deadline_ns); + Py_XDECREF(py_deadline); + Py_XDECREF(py_lifespan_ns); + Py_XDECREF(py_lifespan); + Py_XDECREF(py_durability); + Py_XDECREF(py_reliability); + Py_XDECREF(py_depth); + Py_XDECREF(py_history); + }); + + size_t history; + size_t depth; + size_t reliability; + size_t durability; + size_t lifespan_ns; + size_t deadline_ns; + size_t liveliness; + size_t liveliness_lease_duration_ns; + int avoid_ros_namespace_conventions; + + // Get all the QoS Attributes + py_history = PyObject_GetAttrString(source, "history"); + if (!py_history) { + return false; + } + py_depth = PyObject_GetAttrString(source, "depth"); + if (!py_depth) { + return false; + } + py_reliability = PyObject_GetAttrString(source, "reliability"); + if (!py_reliability) { + return false; + } + py_durability = PyObject_GetAttrString(source, "durability"); + if (!py_durability) { + return false; + } + py_lifespan = PyObject_GetAttrString(source, "lifespan"); + if (!py_lifespan) { + return false; + } + py_deadline = PyObject_GetAttrString(source, "deadline"); + if (!py_deadline) { + return false; + } + py_liveliness = PyObject_GetAttrString(source, "liveliness"); + if (!py_liveliness) { + return false; + } + py_liveliness_lease_duration = + PyObject_GetAttrString(source, "liveliness_lease_duration"); + if (!py_liveliness_lease_duration) { + return false; + } + py_avoid_ros_namespace_conventions = + PyObject_GetAttrString(source, "avoid_ros_namespace_conventions"); + if (!py_avoid_ros_namespace_conventions) { + return false; + } + + // Do Type Conversions + // History and depth if history is keep_last + history = PyNumber_AsSsize_t(py_history, NULL); + switch (history) { + case RMW_QOS_POLICY_HISTORY_KEEP_LAST: + depth = PyNumber_AsSsize_t(py_depth, PyExc_OverflowError); + if (PyErr_Occurred()) { + return false; + } + value.keep_last(depth); + break; + case RMW_QOS_POLICY_HISTORY_KEEP_ALL: + case RMW_QOS_POLICY_HISTORY_SYSTEM_DEFAULT: + case RMW_QOS_POLICY_HISTORY_UNKNOWN: + value.history(static_cast(history)); + break; + default: + if (!PyErr_Occurred()) { + PyErr_Format(PyExc_ValueError, "Unsupported history policy %zu", history); + } + return false; + } + + // Reliability + reliability = PyNumber_AsSsize_t(py_reliability, NULL); + switch (reliability) { + case RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT: + case RMW_QOS_POLICY_RELIABILITY_RELIABLE: + case RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT: + case RMW_QOS_POLICY_RELIABILITY_UNKNOWN: + value.reliability(static_cast(reliability)); + break; + default: + if (!PyErr_Occurred()) { + PyErr_Format(PyExc_ValueError, "Unsupported reliability policy %zu", reliability); + } + return false; + } + + // Durability + durability = PyNumber_AsSsize_t(py_durability, NULL); + switch (durability) { + case RMW_QOS_POLICY_DURABILITY_SYSTEM_DEFAULT: + case RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL: + case RMW_QOS_POLICY_DURABILITY_VOLATILE: + case RMW_QOS_POLICY_DURABILITY_UNKNOWN: + value.durability(static_cast(durability)); + break; + default: + if (!PyErr_Occurred()) { + PyErr_Format(PyExc_ValueError, "Unsupported durability policy %zu", durability); + } + return false; + } + + // lifespan + py_lifespan_ns = PyObject_GetAttrString(py_lifespan, "nanoseconds"); + if (!py_lifespan_ns) { + return false; + } + lifespan_ns = PyNumber_AsSsize_t(py_lifespan_ns, PyExc_OverflowError); + if (PyErr_Occurred()) { + return false; + } + value.lifespan(rclcpp::Duration::from_nanoseconds(lifespan_ns)); + + // deadline + py_deadline_ns = PyObject_GetAttrString(py_deadline, "nanoseconds"); + if (!py_deadline_ns) { + return false; + } + deadline_ns = PyNumber_AsSsize_t(py_deadline_ns, PyExc_OverflowError); + if (PyErr_Occurred()) { + return false; + } + value.deadline(rclcpp::Duration::from_nanoseconds(deadline_ns)); + + // liveliness + liveliness = PyNumber_AsSsize_t(py_liveliness, NULL); + switch (liveliness) { + case RMW_QOS_POLICY_LIVELINESS_AUTOMATIC: + case RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC: + case RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT: + case RMW_QOS_POLICY_LIVELINESS_UNKNOWN: + value.liveliness(static_cast(liveliness)); + break; + default: + if (!PyErr_Occurred()) { + PyErr_Format(PyExc_ValueError, "Unsupported liveliness policy %zu", liveliness); + } + return false; + } + + // liveliness_lease_duration + py_liveliness_lease_duration_ns = PyObject_GetAttrString( + py_liveliness_lease_duration, "nanoseconds"); + if (!py_liveliness_lease_duration_ns) { + return false; + } + liveliness_lease_duration_ns = PyNumber_AsSsize_t( + py_liveliness_lease_duration_ns, PyExc_OverflowError); + if (PyErr_Occurred()) { + return false; + } + value.liveliness_lease_duration( + rclcpp::Duration::from_nanoseconds(liveliness_lease_duration_ns)); + + // avoid_ros_namespace_conventions + avoid_ros_namespace_conventions = PyObject_IsTrue(py_avoid_ros_namespace_conventions); + if (-1 == avoid_ros_namespace_conventions) { + return false; + } + value.avoid_ros_namespace_conventions(avoid_ros_namespace_conventions); + + return true; + } + + // Convert from Python rclcpp::QoS to rclpy.qos.QoSProfile + static handle cast(drake_ros_systems::QoS src, return_value_policy policy, handle parent) + { + (void) src; + (void) policy; + (void) parent; + Py_RETURN_NOTIMPLEMENTED; + } +}; +} // namespace detail +} // namespace pybind11 +#endif // PYTHON_BINDINGS__QOS_TYPE_CASTER_HPP_ diff --git a/drake_ros_systems/src/ros_interface_system.cpp b/drake_ros_systems/src/ros_interface_system.cpp new file mode 100644 index 0000000..28dc97d --- /dev/null +++ b/drake_ros_systems/src/ros_interface_system.cpp @@ -0,0 +1,61 @@ +// Copyright 2020 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. + +#include +#include +#include + +#include "drake_ros_systems/ros_interface_system.hpp" + +namespace drake_ros_systems +{ +class RosInterfaceSystemPrivate +{ +public: + std::shared_ptr ros_; +}; + + +RosInterfaceSystem::RosInterfaceSystem(std::unique_ptr ros) +: impl_(new RosInterfaceSystemPrivate()) +{ + impl_->ros_ = std::move(ros); +} + +RosInterfaceSystem::~RosInterfaceSystem() +{ +} + +/// Return a handle for interacting with ROS +std::shared_ptr +RosInterfaceSystem::get_ros_interface() const +{ + return impl_->ros_; +} + +/// Override as a place to call rclcpp::spin() +void +RosInterfaceSystem::DoCalcNextUpdateTime( + const drake::systems::Context &, + drake::systems::CompositeEventCollection *, + double * time) const +{ + // Do work for at most 1ms so system doesn't get blocked if there's more work than it can handle + const int max_work_time_millis = 1; + impl_->ros_->spin(max_work_time_millis); + // TODO(sloretz) Lcm system pauses time if some work was done, but ROS 2 API doesn't say if + // any work was done. How to reconcile that? + *time = std::numeric_limits::infinity(); +} +} // namespace drake_ros_systems diff --git a/drake_ros_systems/src/ros_publisher_system.cpp b/drake_ros_systems/src/ros_publisher_system.cpp new file mode 100644 index 0000000..a69f978 --- /dev/null +++ b/drake_ros_systems/src/ros_publisher_system.cpp @@ -0,0 +1,111 @@ +// Copyright 2020 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. + +#include +#include +#include +#include + +#include "drake_ros_systems/ros_publisher_system.hpp" +#include "drake_ros_systems/serializer_interface.hpp" + +#include "publisher.hpp" + +namespace drake_ros_systems +{ +class RosPublisherSystemPrivate +{ +public: + std::unique_ptr serializer_; + std::unique_ptr pub_; +}; + +RosPublisherSystem::RosPublisherSystem( + std::unique_ptr & serializer, + const std::string & topic_name, + const rclcpp::QoS & qos, + std::shared_ptr ros, + const std::unordered_set & publish_triggers, + double publish_period) +: impl_(new RosPublisherSystemPrivate()) +{ + impl_->serializer_ = std::move(serializer); + impl_->pub_ = ros->create_publisher( + *impl_->serializer_->get_type_support(), topic_name, qos); + + DeclareAbstractInputPort("message", *(impl_->serializer_->create_default_value())); + + // vvv Mostly copied from LcmPublisherSystem vvv + // Check that publish_triggers does not contain an unsupported trigger. + for (const auto & trigger : publish_triggers) { + if ( + (trigger != drake::systems::TriggerType::kForced) && + (trigger != drake::systems::TriggerType::kPeriodic) && + (trigger != drake::systems::TriggerType::kPerStep)) + { + throw std::invalid_argument("Only kForced, kPeriodic, or kPerStep are supported"); + } + } + + // Declare a forced publish so that any time Publish(.) is called on this + // system (or a Diagram containing it), a message is emitted. + if (publish_triggers.find(drake::systems::TriggerType::kForced) != publish_triggers.end()) { + this->DeclareForcedPublishEvent( + &RosPublisherSystem::publish_input); + } + + if (publish_triggers.find(drake::systems::TriggerType::kPeriodic) != publish_triggers.end()) { + if (publish_period <= 0.0) { + throw std::invalid_argument("kPeriodic requires publish_period > 0"); + } + const double offset = 0.0; + this->DeclarePeriodicPublishEvent( + publish_period, offset, + &RosPublisherSystem::publish_input); + } else if (publish_period > 0) { + // publish_period > 0 without drake::systems::TriggerType::kPeriodic has no meaning and is + // likely a mistake. + throw std::invalid_argument("publish_period > 0 requires kPeriodic"); + } + + if (publish_triggers.find(drake::systems::TriggerType::kPerStep) != publish_triggers.end()) { + DeclarePerStepEvent( + drake::systems::PublishEvent( + [this]( + const drake::systems::Context & context, + const drake::systems::PublishEvent &) { + publish_input(context); + })); + } + // ^^^ Mostly copied from LcmPublisherSystem ^^^ +} + +RosPublisherSystem::~RosPublisherSystem() +{ +} + +void +RosPublisherSystem::publish(const rclcpp::SerializedMessage & serialized_msg) +{ + impl_->pub_->publish(serialized_msg); +} + +drake::systems::EventStatus +RosPublisherSystem::publish_input(const drake::systems::Context & context) const +{ + const drake::AbstractValue & input = get_input_port().Eval(context); + impl_->pub_->publish(impl_->serializer_->serialize(input)); + return drake::systems::EventStatus::Succeeded(); +} +} // namespace drake_ros_systems diff --git a/drake_ros_systems/src/ros_subscriber_system.cpp b/drake_ros_systems/src/ros_subscriber_system.cpp new file mode 100644 index 0000000..4ead3cd --- /dev/null +++ b/drake_ros_systems/src/ros_subscriber_system.cpp @@ -0,0 +1,130 @@ +// Copyright 2020 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. +#include + +#include +#include +#include +#include + +#include "drake_ros_systems/ros_subscriber_system.hpp" +#include "subscription.hpp" + +namespace drake_ros_systems +{ +// Index in AbstractState that deserialized message is stored +const int kStateIndexMessage = 0; + +class RosSubscriberSystemPrivate +{ +public: + void + handle_message(std::shared_ptr callback) + { + std::lock_guard message_lock(mutex_); + // TODO(sloretz) Queue messages here? Lcm subscriber doesn't, so maybe lost messages are ok + // Overwrite last message + msg_ = callback; + } + + std::shared_ptr + take_message() + { + std::lock_guard message_lock(mutex_); + return std::move(msg_); + } + + std::unique_ptr serializer_; + // A handle to a subscription + std::shared_ptr sub_; + // Mutex to prevent multiple threads from modifying this class + std::mutex mutex_; + // The last received message that has not yet been put into a context. + std::shared_ptr msg_; +}; + +RosSubscriberSystem::RosSubscriberSystem( + std::unique_ptr & serializer, + const std::string & topic_name, + const rclcpp::QoS & qos, + std::shared_ptr ros) +: impl_(new RosSubscriberSystemPrivate()) +{ + impl_->serializer_ = std::move(serializer); + impl_->sub_ = ros->create_subscription( + *impl_->serializer_->get_type_support(), topic_name, qos, + std::bind(&RosSubscriberSystemPrivate::handle_message, impl_.get(), std::placeholders::_1)); + + DeclareAbstractOutputPort( + [serializer{impl_->serializer_.get()}]() {return serializer->create_default_value();}, + [](const drake::systems::Context & context, drake::AbstractValue * output_value) { + // Transfer message from state to output port + output_value->SetFrom(context.get_abstract_state().get_value(kStateIndexMessage)); + }); + + static_assert(kStateIndexMessage == 0, ""); + DeclareAbstractState(impl_->serializer_->create_default_value()); +} + +RosSubscriberSystem::~RosSubscriberSystem() +{ +} + +void +RosSubscriberSystem::DoCalcNextUpdateTime( + const drake::systems::Context & context, + drake::systems::CompositeEventCollection * events, + double * time) const +{ + // Vvv Copied from LcmSubscriberSystem vvv + + // We do not support events other than our own message timing events. + LeafSystem::DoCalcNextUpdateTime(context, events, time); + DRAKE_THROW_UNLESS(events->HasEvents() == false); + DRAKE_THROW_UNLESS(std::isinf(*time)); + + std::shared_ptr message = impl_->take_message(); + + // Do nothing unless we have a new message. + if (nullptr == message.get()) { + return; + } + + // Create a unrestricted event and tie the handler to the corresponding + // function. + drake::systems::UnrestrictedUpdateEvent::UnrestrictedUpdateCallback + callback = [this, serialized_message{std::move(message)}]( + const drake::systems::Context &, + const drake::systems::UnrestrictedUpdateEvent &, + drake::systems::State * state) + { + // Deserialize the message and store it in the abstract state on the context + drake::systems::AbstractValues & abstract_state = state->get_mutable_abstract_state(); + auto & abstract_value = abstract_state.get_mutable_value(kStateIndexMessage); + const bool ret = impl_->serializer_->deserialize(*serialized_message, abstract_value); + if (ret != RMW_RET_OK) { + return drake::systems::EventStatus::Failed(this, "Failed to deserialize ROS message"); + } + return drake::systems::EventStatus::Succeeded(); + }; + + // Schedule an update event at the current time. + *time = context.get_time(); + drake::systems::EventCollection> & uu_events = + events->get_mutable_unrestricted_update_events(); + uu_events.add_event( + std::make_unique>( + drake::systems::TriggerType::kTimed, callback)); +} +} // namespace drake_ros_systems diff --git a/drake_ros_systems/src/subscription.cpp b/drake_ros_systems/src/subscription.cpp new file mode 100644 index 0000000..585ae43 --- /dev/null +++ b/drake_ros_systems/src/subscription.cpp @@ -0,0 +1,87 @@ +// Copyright 2020 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. +#include "subscription.hpp" + +#include +#include + +namespace drake_ros_systems +{ +// Copied from rosbag2_transport rosbag2_get_subscription_options +rcl_subscription_options_t subscription_options(const rclcpp::QoS & qos) +{ + auto options = rcl_subscription_get_default_options(); + options.qos = qos.get_rmw_qos_profile(); + return options; +} + +Subscription::Subscription( + rclcpp::node_interfaces::NodeBaseInterface * node_base, + const rosidl_message_type_support_t & ts, + const std::string & topic_name, + const rclcpp::QoS & qos, + std::function)> callback) +: rclcpp::SubscriptionBase(node_base, ts, topic_name, subscription_options(qos), true), + callback_(callback) +{ +} + +Subscription::~Subscription() +{ +} + +std::shared_ptr +Subscription::create_message() +{ + // Subscriber only does serialized messages + return create_serialized_message(); +} + +std::shared_ptr +Subscription::create_serialized_message() +{ + return std::make_shared(); +} + +void +Subscription::handle_message( + std::shared_ptr & message, + const rclcpp::MessageInfo & message_info) +{ + (void) message_info; + callback_(std::static_pointer_cast(message)); +} + +void +Subscription::handle_loaned_message( + void * loaned_message, const rclcpp::MessageInfo & message_info) +{ + (void)loaned_message; + (void)message_info; + throw std::runtime_error("handle_loaned_message() not supported by drake_ros_systems"); +} + +void +Subscription::return_message(std::shared_ptr & message) +{ + auto serialized_msg_ptr = std::static_pointer_cast(message); + return_serialized_message(serialized_msg_ptr); +} + +void +Subscription::return_serialized_message(std::shared_ptr & message) +{ + message.reset(); +} +} // namespace drake_ros_systems diff --git a/drake_ros_systems/src/subscription.hpp b/drake_ros_systems/src/subscription.hpp new file mode 100644 index 0000000..199834f --- /dev/null +++ b/drake_ros_systems/src/subscription.hpp @@ -0,0 +1,79 @@ +// Copyright 2020 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. +#ifndef SUBSCRIPTION_HPP_ +#define SUBSCRIPTION_HPP_ + +#include +#include + +#include +#include + +#include "rclcpp/qos.hpp" +#include "rclcpp/subscription_base.hpp" + +namespace drake_ros_systems +{ +class Subscription final : public rclcpp::SubscriptionBase +{ +public: + Subscription( + rclcpp::node_interfaces::NodeBaseInterface * node_base, + const rosidl_message_type_support_t & ts, + const std::string & topic_name, + const rclcpp::QoS & qos, + std::function)> callback); + + ~Subscription(); + +protected: + /// Borrow a new message. + /** \return Shared pointer to the fresh message. */ + std::shared_ptr + create_message() override; + + /// Borrow a new serialized message + /** \return Shared pointer to a rcl_message_serialized_t. */ + std::shared_ptr + create_serialized_message() override; + + /// Check if we need to handle the message, and execute the callback if we do. + /** + * \param[in] message Shared pointer to the message to handle. + * \param[in] message_info Metadata associated with this message. + */ + void + handle_message( + std::shared_ptr & message, + const rclcpp::MessageInfo & message_info) override; + + void + handle_loaned_message( + void * loaned_message, const rclcpp::MessageInfo & message_info) override; + + /// Return the message borrowed in create_message. + /** \param[in] message Shared pointer to the returned message. */ + void + return_message(std::shared_ptr & message) override; + + /// Return the message borrowed in create_serialized_message. + /** \param[in] message Shared pointer to the returned message. */ + void + return_serialized_message(std::shared_ptr & message) override; + +private: + std::function)> callback_; +}; +} // namespace drake_ros_systems +#endif // SUBSCRIPTION_HPP_ diff --git a/drake_ros_systems/test/drake_ros.cpp b/drake_ros_systems/test/drake_ros.cpp new file mode 100644 index 0000000..3b974f1 --- /dev/null +++ b/drake_ros_systems/test/drake_ros.cpp @@ -0,0 +1,44 @@ +// Copyright 2020 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. + +#include + +#include + +#include +#include +#include + +#include "drake_ros_systems/drake_ros.hpp" + +using drake_ros_systems::DrakeRos; + +TEST(DrakeRos, default_construct) +{ + EXPECT_NO_THROW(std::make_unique()); +} + +TEST(DrakeRos, local_context) +{ + auto context = std::make_shared(); + rclcpp::NodeOptions node_options; + node_options.context(context); + + auto drake_ros = std::make_unique("local_ctx_node", node_options); + (void) drake_ros; + + // Should not have initialized global context + EXPECT_FALSE(rclcpp::contexts::get_global_default_context()->is_valid()); + EXPECT_TRUE(context->is_valid()); +} diff --git a/drake_ros_systems/test/integration.cpp b/drake_ros_systems/test/integration.cpp new file mode 100644 index 0000000..63aab3d --- /dev/null +++ b/drake_ros_systems/test/integration.cpp @@ -0,0 +1,179 @@ +// Copyright 2020 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. + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include + +#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" + +using drake_ros_systems::DrakeRos; +using drake_ros_systems::RosInterfaceSystem; +using drake_ros_systems::RosPublisherSystem; +using drake_ros_systems::RosSubscriberSystem; + +namespace py = pybind11; + +TEST(Integration, sub_to_pub) { + drake::systems::DiagramBuilder builder; + + const size_t num_msgs = 5; + + rclcpp::QoS qos{rclcpp::KeepLast(num_msgs)}; + qos.transient_local().reliable(); + + auto sys_ros_interface = builder.AddSystem(std::make_unique()); + auto sys_sub = builder.AddSystem( + RosSubscriberSystem::Make( + "in", qos, sys_ros_interface->get_ros_interface())); + auto sys_pub = builder.AddSystem( + RosPublisherSystem::Make( + "out", qos, sys_ros_interface->get_ros_interface())); + + builder.Connect(sys_sub->get_output_port(0), sys_pub->get_input_port(0)); + + auto diagram = builder.Build(); + auto context = diagram->CreateDefaultContext(); + + auto simulator = + std::make_unique>(*diagram, std::move(context)); + simulator->set_target_realtime_rate(1.0); + simulator->Initialize(); + + auto & simulator_context = simulator->get_mutable_context(); + + // Don't need to rclcpp::init because DrakeRos uses global rclcpp::Context by default + auto node = rclcpp::Node::make_shared("sub_to_pub"); + + // Create publisher talking to subscriber system. + auto publisher = node->create_publisher("in", qos); + + // Create subscription listening to publisher system + std::vector> rx_msgs; + auto rx_callback = [&](std::unique_ptr msg) + { + rx_msgs.push_back(std::move(msg)); + }; + auto subscription = + node->create_subscription("out", qos, rx_callback); + + // Send messages into the drake system + for (size_t p = 0; p < num_msgs; ++p) { + publisher->publish(std::make_unique()); + } + + const int timeout_sec = 5; + const int spins_per_sec = 10; + const float time_delta = 1.0f / spins_per_sec; + for (int i = 0; i < timeout_sec * spins_per_sec && rx_msgs.size() < num_msgs; ++i) { + rclcpp::spin_some(node); + simulator->AdvanceTo(simulator_context.get_time() + time_delta); + } + + // Make sure same number of messages got out + ASSERT_EQ(num_msgs, rx_msgs.size()); +} + +TEST(Integration, sub_to_pub_py) { + py::scoped_interpreter guard{}; + + py::dict scope; + py::exec( + R"delim( +from drake_ros_systems import RosInterfaceSystem +from drake_ros_systems import RosPublisherSystem +from drake_ros_systems import RosSubscriberSystem + +from pydrake.systems.analysis import Simulator +from pydrake.systems.framework import DiagramBuilder + +from rclpy.qos import DurabilityPolicy +from rclpy.qos import HistoryPolicy +from rclpy.qos import QoSProfile +from rclpy.qos import ReliabilityPolicy + +from test_msgs.msg import BasicTypes + +builder = DiagramBuilder() + +sys_ros_interface = builder.AddSystem(RosInterfaceSystem()) + +qos = QoSProfile( + depth=10, + history=HistoryPolicy.KEEP_LAST, + reliability=ReliabilityPolicy.RELIABLE, + durability=DurabilityPolicy.TRANSIENT_LOCAL) + +sys_pub = builder.AddSystem( + RosPublisherSystem(BasicTypes, 'out_py', qos, sys_ros_interface.get_ros_interface())) + +sys_sub = builder.AddSystem( + RosSubscriberSystem(BasicTypes, 'in_py', qos, sys_ros_interface.get_ros_interface())) + +builder.Connect(sys_sub.get_output_port(0), sys_pub.get_input_port(0)) +diagram = builder.Build() +simulator = Simulator(diagram) +simulator_context = simulator.get_mutable_context() +simulator.set_target_realtime_rate(1.0) +)delim", + py::globals(), scope); + + const size_t num_msgs = 5; + rclcpp::QoS qos{rclcpp::KeepLast(num_msgs)}; + qos.transient_local().reliable(); + + // Don't need to rclcpp::init because DrakeRos uses global rclcpp::Context by default + auto node = rclcpp::Node::make_shared("sub_to_pub_py"); + + // Create publisher talking to subscriber system. + auto publisher = node->create_publisher("in_py", qos); + + // Create subscription listening to publisher system + std::vector> rx_msgs; + auto rx_callback = [&](std::unique_ptr msg) + { + rx_msgs.push_back(std::move(msg)); + }; + auto subscription = + node->create_subscription("out_py", qos, rx_callback); + + // Send messages into the drake system + for (size_t p = 0; p < num_msgs; ++p) { + publisher->publish(std::make_unique()); + } + + const int timeout_sec = 5; + const int spins_per_sec = 10; + const float time_delta = 1.0f / spins_per_sec; + scope["time_delta"] = time_delta; + for (int i = 0; i < timeout_sec * spins_per_sec && rx_msgs.size() < num_msgs; ++i) { + rclcpp::spin_some(node); + py::exec( + "simulator.AdvanceTo(simulator_context.get_time() + time_delta)", py::globals(), scope); + } + + // Make sure same number of messages got out + ASSERT_EQ(num_msgs, rx_msgs.size()); +}