From efc7f7c8ba097728bfdc3d4c37a260c4c1b09e13 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Tue, 9 Nov 2021 15:04:47 -0300 Subject: [PATCH 01/15] Revamp drake_ros_core package - Sort out public/private API boundaries - Use raw pointers instead of std::shared_ptr - Improve compliance with Drake's C++ style - Extend Doxygen docstrings Signed-off-by: Michel Hidalgo --- drake_ros_core/CMakeLists.txt | 16 ++-- drake_ros_core/CPPLINT.cfg | 6 ++ .../include/drake_ros_core/drake_ros.h | 59 ++++++++++++ .../include/drake_ros_core/drake_ros.hpp | 60 ------------ .../drake_ros_core/drake_ros_interface.hpp | 47 --------- ...face_system.hpp => ros_interface_system.h} | 26 +++-- .../drake_ros_core/ros_publisher_system.h | 91 ++++++++++++++++++ .../drake_ros_core/ros_publisher_system.hpp | 89 ----------------- .../drake_ros_core/ros_subscriber_system.h | 71 ++++++++++++++ .../drake_ros_core/ros_subscriber_system.hpp | 62 ------------ .../{serializer.hpp => serializer.h} | 43 ++++----- .../drake_ros_core/serializer_interface.h | 58 +++++++++++ .../drake_ros_core/serializer_interface.hpp | 38 -------- drake_ros_core/src/drake_ros.cc | 78 +++++++++++++++ drake_ros_core/src/drake_ros.cpp | 91 ------------------ .../src/{publisher.cpp => publisher.cc} | 6 +- .../src/{publisher.hpp => publisher.h} | 8 +- ...ace_system.cpp => ros_interface_system.cc} | 29 +++--- ...her_system.cpp => ros_publisher_system.cc} | 46 ++++----- ...er_system.cpp => ros_subscriber_system.cc} | 96 ++++++++++--------- .../src/{subscription.cpp => subscription.cc} | 7 +- .../src/{subscription.hpp => subscription.h} | 18 ++-- .../{test_drake_ros.cpp => test_drake_ros.cc} | 2 +- .../{test_pub_sub.cpp => test_pub_sub.cc} | 21 ++-- 24 files changed, 529 insertions(+), 539 deletions(-) create mode 100644 drake_ros_core/include/drake_ros_core/drake_ros.h delete mode 100644 drake_ros_core/include/drake_ros_core/drake_ros.hpp delete mode 100644 drake_ros_core/include/drake_ros_core/drake_ros_interface.hpp rename drake_ros_core/include/drake_ros_core/{ros_interface_system.hpp => ros_interface_system.h} (61%) create mode 100644 drake_ros_core/include/drake_ros_core/ros_publisher_system.h delete mode 100644 drake_ros_core/include/drake_ros_core/ros_publisher_system.hpp create mode 100644 drake_ros_core/include/drake_ros_core/ros_subscriber_system.h delete mode 100644 drake_ros_core/include/drake_ros_core/ros_subscriber_system.hpp rename drake_ros_core/include/drake_ros_core/{serializer.hpp => serializer.h} (51%) create mode 100644 drake_ros_core/include/drake_ros_core/serializer_interface.h delete mode 100644 drake_ros_core/include/drake_ros_core/serializer_interface.hpp create mode 100644 drake_ros_core/src/drake_ros.cc delete mode 100644 drake_ros_core/src/drake_ros.cpp rename drake_ros_core/src/{publisher.cpp => publisher.cc} (93%) rename drake_ros_core/src/{publisher.hpp => publisher.h} (87%) rename drake_ros_core/src/{ros_interface_system.cpp => ros_interface_system.cc} (62%) rename drake_ros_core/src/{ros_publisher_system.cpp => ros_publisher_system.cc} (70%) rename drake_ros_core/src/{ros_subscriber_system.cpp => ros_subscriber_system.cc} (54%) rename drake_ros_core/src/{subscription.cpp => subscription.cc} (95%) rename drake_ros_core/src/{subscription.hpp => subscription.h} (85%) rename drake_ros_core/test/{test_drake_ros.cpp => test_drake_ros.cc} (96%) rename drake_ros_core/test/{test_pub_sub.cpp => test_pub_sub.cc} (84%) diff --git a/drake_ros_core/CMakeLists.txt b/drake_ros_core/CMakeLists.txt index 074908bb..9139c8c0 100644 --- a/drake_ros_core/CMakeLists.txt +++ b/drake_ros_core/CMakeLists.txt @@ -17,12 +17,12 @@ 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 @@ -69,7 +69,7 @@ if(BUILD_TESTING) ament_clang_format(CONFIG_FILE .clang-format) ament_cpplint() - ament_add_gtest(test_pub_sub test/test_pub_sub.cpp) + ament_add_gtest(test_pub_sub test/test_pub_sub.cc) target_link_libraries(test_pub_sub drake::drake @@ -77,7 +77,7 @@ if(BUILD_TESTING) ${test_msgs_TARGETS} ) - ament_add_gtest(test_drake_ros test/test_drake_ros.cpp) + ament_add_gtest(test_drake_ros test/test_drake_ros.cc) target_link_libraries(test_drake_ros drake_ros_core ) diff --git a/drake_ros_core/CPPLINT.cfg b/drake_ros_core/CPPLINT.cfg index 98c9cdd4..476dca32 100644 --- a/drake_ros_core/CPPLINT.cfg +++ b/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 diff --git a/drake_ros_core/include/drake_ros_core/drake_ros.h b/drake_ros_core/include/drake_ros_core/drake_ros.h new file mode 100644 index 00000000..94315406 --- /dev/null +++ b/drake_ros_core/include/drake_ros_core/drake_ros.h @@ -0,0 +1,59 @@ +// 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 +#include + +#include +#include + +namespace drake_ros_core { + +/** A Drake ROS interface that wraps a live ROS2 node. + This interface manages both ROS2 node construction and scheduling. + */ +class DrakeRos final { + public: + /** A constructor that wraps a "drake_ros" ROS2 node with default options. */ + DrakeRos() : DrakeRos("drake_ros", rclcpp::NodeOptions{}) {} + + /** A constructor that wraps a `node_name` ROS2 node with `node_options`. + See `rclcpp::Node` documentation for further reference on arguments. + */ + DrakeRos(const std::string& node_name, rclcpp::NodeOptions node_options); + + ~DrakeRos(); + + /** Returns a constant reference to the underlying ROS2 node. */ + const rclcpp::Node& get_node() const; + + /** Returns a mutable reference to the underlying ROS2 node. */ + rclcpp::Node* get_mutable_node() const; + + /** Spins the underlying ROS2 node, dispatching all available work. + @param[in] timeout_millis Timeout, in milliseconds. + If timeout is less than 0, the call will block indefinitely + until some work has been dispatched. If timeout is 0, the call + will dispatch available work without blocking. If timeout is + larger than 0, the call will wait up to the given timeout for + work to dispatch. + */ + void Spin(int timeout_millis = 0); + + private: + struct Impl; + std::unique_ptr impl_; +}; +} // namespace drake_ros_core diff --git a/drake_ros_core/include/drake_ros_core/drake_ros.hpp b/drake_ros_core/include/drake_ros_core/drake_ros.hpp deleted file mode 100644 index 58c7b64e..00000000 --- a/drake_ros_core/include/drake_ros_core/drake_ros.hpp +++ /dev/null @@ -1,60 +0,0 @@ -// 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. -#ifndef DRAKE_ROS_CORE__DRAKE_ROS_HPP_ -#define DRAKE_ROS_CORE__DRAKE_ROS_HPP_ - -#include -#include -#include - -#include -#include -#include -#include -#include - -#include "drake_ros_core/drake_ros_interface.hpp" - -namespace drake_ros_core { -/// 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_core -#endif // DRAKE_ROS_CORE__DRAKE_ROS_HPP_ diff --git a/drake_ros_core/include/drake_ros_core/drake_ros_interface.hpp b/drake_ros_core/include/drake_ros_core/drake_ros_interface.hpp deleted file mode 100644 index a1f201db..00000000 --- a/drake_ros_core/include/drake_ros_core/drake_ros_interface.hpp +++ /dev/null @@ -1,47 +0,0 @@ -// 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. -#ifndef DRAKE_ROS_CORE__DRAKE_ROS_INTERFACE_HPP_ -#define DRAKE_ROS_CORE__DRAKE_ROS_INTERFACE_HPP_ - -#include -#include -#include - -#include -#include -#include -#include - -namespace drake_ros_core { -// 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_core -#endif // DRAKE_ROS_CORE__DRAKE_ROS_INTERFACE_HPP_ diff --git a/drake_ros_core/include/drake_ros_core/ros_interface_system.hpp b/drake_ros_core/include/drake_ros_core/ros_interface_system.h similarity index 61% rename from drake_ros_core/include/drake_ros_core/ros_interface_system.hpp rename to drake_ros_core/include/drake_ros_core/ros_interface_system.h index 46c6c78b..55806364 100644 --- a/drake_ros_core/include/drake_ros_core/ros_interface_system.hpp +++ b/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 #include -#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 { public: - explicit RosInterfaceSystem(std::unique_ptr ros); - virtual ~RosInterfaceSystem(); + /** A constructor that takes ownership of the `ros` interface. */ + explicit RosInterfaceSystem(std::unique_ptr ros); + + ~RosInterfaceSystem() override; - /// Return a handle for interacting with ROS - std::shared_ptr 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&, drake::systems::CompositeEventCollection*, double*) const override; - std::unique_ptr impl_; + private: + struct Impl; + std::unique_ptr impl_; }; } // namespace drake_ros_core -#endif // DRAKE_ROS_CORE__ROS_INTERFACE_SYSTEM_HPP_ diff --git a/drake_ros_core/include/drake_ros_core/ros_publisher_system.h b/drake_ros_core/include/drake_ros_core/ros_publisher_system.h new file mode 100644 index 00000000..9b1bcdfc --- /dev/null +++ b/drake_ros_core/include/drake_ros_core/ros_publisher_system.h @@ -0,0 +1,91 @@ +// 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 +#include +#include +#include + +#include +#include +#include + +#include "drake_ros_core/drake_ros.h" +#include "drake_ros_core/serializer.h" +#include "drake_ros_core/serializer_interface.h" + +namespace drake_ros_core { +/** A system that can publish ROS messages. + It accepts ROS messages on its sole input port and publishes them + to a ROS topic. + */ +class RosPublisherSystem : public drake::systems::LeafSystem { + public: + /** Instantiates a publisher system for a given ROS message type. + See `RosPublisherSystem::RosPublisherSystem` documentation for + further reference on function arguments. + + @tparam MessageT C++ ROS message type. + */ + template + static std::unique_ptr Make(ArgsT&&... args) { + // Assume C++ typesupport since this is a C++ template function + return std::make_unique( + std::make_unique>(), std::forward(args)...); + } + + /** A constructor for the ROS publisher system. + It takes a `serializer` to deal with outgoing messages. + + @param[in] serializer a (de)serialization interface for the + expected ROS message type. + @param[in] topic_name Name of the ROS topic to publish to. + @param[in] qos QoS profile for the underlying ROS pubslisher. + @param[in] ros interface to a live ROS node to publish from. + @param[in] publish_triggers optional set of triggers that determine + when messages will be published. By default it will publish on + every step and when forced (via Publish). + @param[in] publish_period optional publishing period, in seconds. + Only applicable when periodic publishing is enabled. + */ + RosPublisherSystem( + std::unique_ptr serializer, + const std::string& topic_name, const rclcpp::QoS& qos, DrakeRos* ros, + const std::unordered_set& publish_triggers = + {drake::systems::TriggerType::kPerStep, + drake::systems::TriggerType::kForced}, + double publish_period = 0.0); + + ~RosPublisherSystem() override; + + /** Publishes a C++ ROS `message`. */ + template + void Publish(const MessageT& message) { + static const Serializer serializer; + publish(serializer.serialize(message)); + } + + /** Publishes a serialized ROS message. */ + void Publish(const rclcpp::SerializedMessage& serialized_message); + + protected: + drake::systems::EventStatus PublishInput( + const drake::systems::Context& context) const; + + private: + struct Impl; + std::unique_ptr impl_; +}; +} // namespace drake_ros_core diff --git a/drake_ros_core/include/drake_ros_core/ros_publisher_system.hpp b/drake_ros_core/include/drake_ros_core/ros_publisher_system.hpp deleted file mode 100644 index ea9bbc52..00000000 --- a/drake_ros_core/include/drake_ros_core/ros_publisher_system.hpp +++ /dev/null @@ -1,89 +0,0 @@ -// 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. -#ifndef DRAKE_ROS_CORE__ROS_PUBLISHER_SYSTEM_HPP_ -#define DRAKE_ROS_CORE__ROS_PUBLISHER_SYSTEM_HPP_ - -#include -#include -#include - -#include -#include -#include - -#include "drake_ros_core/drake_ros_interface.hpp" -#include "drake_ros_core/serializer.hpp" -#include "drake_ros_core/serializer_interface.hpp" - -namespace drake_ros_core { -/// 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_core -#endif // DRAKE_ROS_CORE__ROS_PUBLISHER_SYSTEM_HPP_ diff --git a/drake_ros_core/include/drake_ros_core/ros_subscriber_system.h b/drake_ros_core/include/drake_ros_core/ros_subscriber_system.h new file mode 100644 index 00000000..31c06dcc --- /dev/null +++ b/drake_ros_core/include/drake_ros_core/ros_subscriber_system.h @@ -0,0 +1,71 @@ +// 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 +#include +#include + +#include +#include + +#include "drake_ros_core/drake_ros.h" +#include "drake_ros_core/serializer.h" +#include "drake_ros_core/serializer_interface.h" + +namespace drake_ros_core { +/** A system that can subscribe to ROS messages. + It subscribes to a ROS topic and makes ROS messages available on + its sole output port. + */ +class RosSubscriberSystem : public drake::systems::LeafSystem { + public: + /** Instantiates a ROS subscriber system for a given ROS message type. + See `RosSubscriberSystem::RosSubscriberSystem` documentation for + further reference on function arguments. + + @tparam MessageT C++ ROS message type. + */ + template + static std::unique_ptr Make(ArgsT&&... args) { + // Assume C++ typesupport since this is a C++ template function + return std::make_unique( + std::make_unique>(), std::forward(args)...); + } + + /** A constructor for the ROS subscriber system. + It takes a `serializer` to deal with incoming messages. + + @param[in] serializer a (de)serialization interface for the + expected ROS message type. + @param[in] topic_name Name of the ROS topic to subscribe to. + @param[in] qos QoS profile for the underlying ROS subscription. + @param[in] ros interface to a live ROS node to publish from. + */ + RosSubscriberSystem(std::unique_ptr serializer, + const std::string& topic_name, const rclcpp::QoS& qos, + DrakeRos* ros); + + ~RosSubscriberSystem() override; + + protected: + void DoCalcNextUpdateTime(const drake::systems::Context&, + drake::systems::CompositeEventCollection*, + double*) const override; + + private: + struct Impl; + std::unique_ptr impl_; +}; +} // namespace drake_ros_core diff --git a/drake_ros_core/include/drake_ros_core/ros_subscriber_system.hpp b/drake_ros_core/include/drake_ros_core/ros_subscriber_system.hpp deleted file mode 100644 index 8e55f13a..00000000 --- a/drake_ros_core/include/drake_ros_core/ros_subscriber_system.hpp +++ /dev/null @@ -1,62 +0,0 @@ -// 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. -#ifndef DRAKE_ROS_CORE__ROS_SUBSCRIBER_SYSTEM_HPP_ -#define DRAKE_ROS_CORE__ROS_SUBSCRIBER_SYSTEM_HPP_ - -#include -#include - -#include -#include - -#include "drake_ros_core/drake_ros_interface.hpp" -#include "drake_ros_core/serializer.hpp" -#include "drake_ros_core/serializer_interface.hpp" - -namespace drake_ros_core { -// 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_core -#endif // DRAKE_ROS_CORE__ROS_SUBSCRIBER_SYSTEM_HPP_ diff --git a/drake_ros_core/include/drake_ros_core/serializer.hpp b/drake_ros_core/include/drake_ros_core/serializer.h similarity index 51% rename from drake_ros_core/include/drake_ros_core/serializer.hpp rename to drake_ros_core/include/drake_ros_core/serializer.h index 693f44ab..a23d2224 100644 --- a/drake_ros_core/include/drake_ros_core/serializer.hpp +++ b/drake_ros_core/include/drake_ros_core/serializer.h @@ -11,51 +11,46 @@ // 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__SERIALIZER_HPP_ -#define DRAKE_ROS_CORE__SERIALIZER_HPP_ +#pragma once #include #include +#include #include -#include #include -#include "drake_ros_core/serializer_interface.hpp" +#include "drake_ros_core/serializer_interface.h" namespace drake_ros_core { +/** A (de)serialization interface implementation that is + bound to C++ ROS messages of `MessageT` type. */ template class Serializer : public SerializerInterface { public: - rclcpp::SerializedMessage serialize( + 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; + rclcpp::SerializedMessage serialized_message; + protocol_.serialize_message(&abstract_value.get_value(), + &serialized_message); + return serialized_message; } - 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; + void Deserialize(const rclcpp::SerializedMessage& serialized_message, + drake::AbstractValue* abstract_value) const override { + protocol_.deserialize_message( + &serialized_message, &abstract_value->get_mutable_value()); } - std::unique_ptr create_default_value() const override { + std::unique_ptr CreateDefaultValue() const override { return std::make_unique>(MessageT()); } - const rosidl_message_type_support_t* get_type_support() const override { + const rosidl_message_type_support_t* GetTypeSupport() const override { return rosidl_typesupport_cpp::get_message_type_support_handle(); } + + private: + rclcpp::Serialization protocol_; }; } // namespace drake_ros_core -#endif // DRAKE_ROS_CORE__SERIALIZER_HPP_ diff --git a/drake_ros_core/include/drake_ros_core/serializer_interface.h b/drake_ros_core/include/drake_ros_core/serializer_interface.h new file mode 100644 index 00000000..419c04f9 --- /dev/null +++ b/drake_ros_core/include/drake_ros_core/serializer_interface.h @@ -0,0 +1,58 @@ +// 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 + +#include +#include +#include + +namespace drake_ros_core { +/** An interface for (de)serialization of a ROS message. + + This interface deals with ROS messages of a fixed message + type, which cannot be inferred from its API. Therefore, + caller code must either know what the type is or otherwise + rely on type-erased ROS APIs that can take ROS message + typesupport. + */ +class SerializerInterface { + public: + virtual ~SerializerInterface() = default; + + /** Serializes a ROS message of a given type. + @param[in] abstract_value type-erased value + wrapping the ROS message to be serialized. + @return the serialized ROS message. + */ + virtual rclcpp::SerializedMessage Serialize( + const drake::AbstractValue& abstract_value) const = 0; + + /** Deserializes a ROS message of a given type. + @param[in] message the serialized ROS message. + @param[inout] abstract_value type-erased value wrapping + the ROS message to be populated upon deserialization. + */ + virtual void Deserialize(const rclcpp::SerializedMessage& message, + drake::AbstractValue* abstract_value) const = 0; + + /** Creates a default ROS message wrapped in an abstract, type-erased + value. */ + virtual std::unique_ptr CreateDefaultValue() const = 0; + + /** Returns a reference to the ROS message typesupport. */ + virtual const rosidl_message_type_support_t* GetTypeSupport() const = 0; +}; +} // namespace drake_ros_core diff --git a/drake_ros_core/include/drake_ros_core/serializer_interface.hpp b/drake_ros_core/include/drake_ros_core/serializer_interface.hpp deleted file mode 100644 index 817ced08..00000000 --- a/drake_ros_core/include/drake_ros_core/serializer_interface.hpp +++ /dev/null @@ -1,38 +0,0 @@ -// 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. -#ifndef DRAKE_ROS_CORE__SERIALIZER_INTERFACE_HPP_ -#define DRAKE_ROS_CORE__SERIALIZER_INTERFACE_HPP_ - -#include - -#include -#include -#include - -namespace drake_ros_core { -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_core -#endif // DRAKE_ROS_CORE__SERIALIZER_INTERFACE_HPP_ diff --git a/drake_ros_core/src/drake_ros.cc b/drake_ros_core/src/drake_ros.cc new file mode 100644 index 00000000..5b26b82e --- /dev/null +++ b/drake_ros_core/src/drake_ros.cc @@ -0,0 +1,78 @@ +// 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. + +#include "drake_ros_core/drake_ros.h" + +#include +#include +#include + +#include +#include + +namespace drake_ros_core { +struct DrakeRos::Impl { + 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 Impl()) { + 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()"); + } +} + +const rclcpp::Node& DrakeRos::get_node() const { return *impl_->node; } + +rclcpp::Node* DrakeRos::get_mutable_node() const { return impl_->node.get(); } + +void DrakeRos::Spin(int timeout_millis) { + // To match `DrakeLcm::HandleSubscriptions()`'s behavior, in the following + // we wait up to the given timeout to process one work item and then process + // all pending work items, if any, without waiting. + impl_->executor->spin_once(std::chrono::milliseconds(timeout_millis)); + impl_->executor->spin_some(std::chrono::milliseconds(0)); +} +} // namespace drake_ros_core diff --git a/drake_ros_core/src/drake_ros.cpp b/drake_ros_core/src/drake_ros.cpp deleted file mode 100644 index 19277dbc..00000000 --- a/drake_ros_core/src/drake_ros.cpp +++ /dev/null @@ -1,91 +0,0 @@ -// 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. - -#include "drake_ros_core/drake_ros.hpp" - -#include -#include - -#include "publisher.hpp" -#include "subscription.hpp" -#include -#include - -namespace drake_ros_core { -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_core diff --git a/drake_ros_core/src/publisher.cpp b/drake_ros_core/src/publisher.cc similarity index 93% rename from drake_ros_core/src/publisher.cpp rename to drake_ros_core/src/publisher.cc index f719953c..417df9e8 100644 --- a/drake_ros_core/src/publisher.cpp +++ b/drake_ros_core/src/publisher.cc @@ -12,17 +12,20 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "publisher.hpp" +#include "publisher.h" // NOLINT(build/include) #include namespace drake_ros_core { +namespace internal { +namespace { // 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; } +} // namespace Publisher::Publisher(rclcpp::node_interfaces::NodeBaseInterface* node_base, const rosidl_message_type_support_t& type_support, @@ -44,4 +47,5 @@ void Publisher::publish(const rclcpp::SerializedMessage& serialized_msg) { return_code, "failed to publish serialized message"); } } +} // namespace internal } // namespace drake_ros_core diff --git a/drake_ros_core/src/publisher.hpp b/drake_ros_core/src/publisher.h similarity index 87% rename from drake_ros_core/src/publisher.hpp rename to drake_ros_core/src/publisher.h index 9df3b381..dc212104 100644 --- a/drake_ros_core/src/publisher.hpp +++ b/drake_ros_core/src/publisher.h @@ -11,8 +11,7 @@ // 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_ +#pragma once #include #include @@ -23,6 +22,9 @@ #include namespace drake_ros_core { +namespace internal { +// A type-erased version of rclcpp:::Publisher. +// This class conforms to the ROS 2 C++ style for consistency. class Publisher final : public rclcpp::PublisherBase { public: Publisher(rclcpp::node_interfaces::NodeBaseInterface* node_base, @@ -33,5 +35,5 @@ class Publisher final : public rclcpp::PublisherBase { void publish(const rclcpp::SerializedMessage& serialized_msg); }; +} // namespace internal } // namespace drake_ros_core -#endif // PUBLISHER_HPP_ diff --git a/drake_ros_core/src/ros_interface_system.cpp b/drake_ros_core/src/ros_interface_system.cc similarity index 62% rename from drake_ros_core/src/ros_interface_system.cpp rename to drake_ros_core/src/ros_interface_system.cc index 542ed845..5966a565 100644 --- a/drake_ros_core/src/ros_interface_system.cpp +++ b/drake_ros_core/src/ros_interface_system.cc @@ -12,41 +12,38 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "drake_ros_core/ros_interface_system.hpp" +#include "drake_ros_core/ros_interface_system.h" #include #include #include namespace drake_ros_core { -class RosInterfaceSystemPrivate { - public: - std::shared_ptr ros_; +struct RosInterfaceSystem::Impl { + // Interface to ROS (through a node). + std::unique_ptr ros; }; -RosInterfaceSystem::RosInterfaceSystem(std::unique_ptr ros) - : impl_(new RosInterfaceSystemPrivate()) { - impl_->ros_ = std::move(ros); +RosInterfaceSystem::RosInterfaceSystem(std::unique_ptr ros) + : impl_(new Impl()) { + 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_; +DrakeRos* RosInterfaceSystem::get_ros_interface() const { + return impl_->ros.get(); } -/// 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); + constexpr int kMaxWorkMillis = 0; // Do not block. + impl_->ros->Spin(kMaxWorkMillis); // 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? + // TODO(hidmic): test for subscription latency in context time, how does the + // order of node spinning and message taking affects it? *time = std::numeric_limits::infinity(); } } // namespace drake_ros_core diff --git a/drake_ros_core/src/ros_publisher_system.cpp b/drake_ros_core/src/ros_publisher_system.cc similarity index 70% rename from drake_ros_core/src/ros_publisher_system.cpp rename to drake_ros_core/src/ros_publisher_system.cc index d2fe8464..ad60b83b 100644 --- a/drake_ros_core/src/ros_publisher_system.cpp +++ b/drake_ros_core/src/ros_publisher_system.cc @@ -12,37 +12,39 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "drake_ros_core/ros_publisher_system.hpp" +#include "drake_ros_core/ros_publisher_system.h" #include #include #include #include -#include "publisher.hpp" +#include "publisher.h" // NOLINT(build/include) -#include "drake_ros_core/serializer_interface.hpp" +#include "drake_ros_core/serializer_interface.h" namespace drake_ros_core { -class RosPublisherSystemPrivate { - public: - std::unique_ptr serializer_; - std::unique_ptr pub_; +struct RosPublisherSystem::Impl { + // Interface for message (de)serialization. + std::unique_ptr serializer; + // Publisher for serialized messages. + std::unique_ptr pub; }; RosPublisherSystem::RosPublisherSystem( - std::unique_ptr& serializer, - const std::string& topic_name, const rclcpp::QoS& qos, - std::shared_ptr ros, + std::unique_ptr serializer, + const std::string& topic_name, const rclcpp::QoS& qos, DrakeRos* 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); + : impl_(new Impl()) { + impl_->serializer = std::move(serializer); + + impl_->pub = std::make_unique( + ros->get_mutable_node()->get_node_base_interface().get(), + *impl_->serializer->GetTypeSupport(), topic_name, qos); DeclareAbstractInputPort("message", - *(impl_->serializer_->create_default_value())); + *(impl_->serializer->CreateDefaultValue())); // vvv Mostly copied from LcmPublisherSystem vvv // Check that publish_triggers does not contain an unsupported trigger. @@ -59,7 +61,7 @@ RosPublisherSystem::RosPublisherSystem( // 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); + this->DeclareForcedPublishEvent(&RosPublisherSystem::PublishInput); } if (publish_triggers.find(drake::systems::TriggerType::kPeriodic) != @@ -69,7 +71,7 @@ RosPublisherSystem::RosPublisherSystem( } const double offset = 0.0; this->DeclarePeriodicPublishEvent(publish_period, offset, - &RosPublisherSystem::publish_input); + &RosPublisherSystem::PublishInput); } else if (publish_period > 0) { // publish_period > 0 without drake::systems::TriggerType::kPeriodic has no // meaning and is likely a mistake. @@ -81,7 +83,7 @@ RosPublisherSystem::RosPublisherSystem( DeclarePerStepEvent(drake::systems::PublishEvent( [this](const drake::systems::Context& context, const drake::systems::PublishEvent&) { - publish_input(context); + PublishInput(context); })); } // ^^^ Mostly copied from LcmPublisherSystem ^^^ @@ -89,16 +91,16 @@ RosPublisherSystem::RosPublisherSystem( RosPublisherSystem::~RosPublisherSystem() {} -void RosPublisherSystem::publish( +void RosPublisherSystem::Publish( const rclcpp::SerializedMessage& serialized_msg) { - impl_->pub_->publish(serialized_msg); + impl_->pub->publish(serialized_msg); } -drake::systems::EventStatus RosPublisherSystem::publish_input( +drake::systems::EventStatus RosPublisherSystem::PublishInput( const drake::systems::Context& context) const { const drake::AbstractValue& input = get_input_port().Eval(context); - impl_->pub_->publish(impl_->serializer_->serialize(input)); + impl_->pub->publish(impl_->serializer->Serialize(input)); return drake::systems::EventStatus::Succeeded(); } } // namespace drake_ros_core diff --git a/drake_ros_core/src/ros_subscriber_system.cpp b/drake_ros_core/src/ros_subscriber_system.cc similarity index 54% rename from drake_ros_core/src/ros_subscriber_system.cpp rename to drake_ros_core/src/ros_subscriber_system.cc index b06f1587..bf610403 100644 --- a/drake_ros_core/src/ros_subscriber_system.cpp +++ b/drake_ros_core/src/ros_subscriber_system.cc @@ -11,59 +11,71 @@ // 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 "drake_ros_core/ros_subscriber_system.hpp" + +#include "drake_ros_core/ros_subscriber_system.h" #include #include #include #include -#include "subscription.hpp" +#include "subscription.h" // NOLINT(build/include) #include namespace drake_ros_core { -// Index in AbstractState that deserialized message is stored -const int kStateIndexMessage = 0; - -class RosSubscriberSystemPrivate { +namespace { +// A synchronized queue of `MessageT` messages. +template +class MessageQueue { 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; + void PutMessage(std::shared_ptr message) { + std::lock_guard lock(mutex_); + message_ = message; } - std::shared_ptr take_message() { - std::lock_guard message_lock(mutex_); - return std::move(msg_); + std::shared_ptr TakeMessage() { + std::lock_guard lock(mutex_); + return std::move(message_); } - std::unique_ptr serializer_; - // A handle to a subscription - std::shared_ptr sub_; - // Mutex to prevent multiple threads from modifying this class + private: + // Mutex to synchronize access to the queue. std::mutex mutex_; - // The last received message that has not yet been put into a context. - std::shared_ptr msg_; + // Last received message (i.e. queue of size 1). + std::shared_ptr message_; +}; +} // namespace + +struct RosSubscriberSystem::Impl { + // Interface for message (de)serialization. + std::unique_ptr serializer; + // Subscription to serialized messages. + std::shared_ptr sub; + // Queue of serialized messages. + MessageQueue queue; + // AbstractState index where the message is stored. + drake::systems::AbstractStateIndex message_state_index; }; 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)); - - static_assert(kStateIndexMessage == 0, ""); - auto message_state_index = - DeclareAbstractState(*(impl_->serializer_->create_default_value())); - - DeclareStateOutputPort(drake::systems::kUseDefaultName, message_state_index); + std::unique_ptr serializer, + const std::string& topic_name, const rclcpp::QoS& qos, DrakeRos* ros) + : impl_(new Impl()) { + impl_->serializer = std::move(serializer); + + rclcpp::Node* node = ros->get_mutable_node(); + impl_->sub = std::make_shared( + node->get_node_base_interface().get(), + *impl_->serializer->GetTypeSupport(), topic_name, qos, + std::bind(&MessageQueue::PutMessage, + &impl_->queue, std::placeholders::_1)); + node->get_node_topics_interface()->add_subscription(impl_->sub, nullptr); + + impl_->message_state_index = + DeclareAbstractState(*(impl_->serializer->CreateDefaultValue())); + + DeclareStateOutputPort(drake::systems::kUseDefaultName, + impl_->message_state_index); } RosSubscriberSystem::~RosSubscriberSystem() {} @@ -79,10 +91,11 @@ void RosSubscriberSystem::DoCalcNextUpdateTime( DRAKE_THROW_UNLESS(events->HasEvents() == false); DRAKE_THROW_UNLESS(std::isinf(*time)); - std::shared_ptr message = impl_->take_message(); + std::shared_ptr message = + impl_->queue.TakeMessage(); // Do nothing unless we have a new message. - if (nullptr == message.get()) { + if (!message) { return; } @@ -98,13 +111,8 @@ void RosSubscriberSystem::DoCalcNextUpdateTime( 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"); - } + abstract_state.get_mutable_value(impl_->message_state_index); + impl_->serializer->Deserialize(*serialized_message, &abstract_value); return drake::systems::EventStatus::Succeeded(); }; diff --git a/drake_ros_core/src/subscription.cpp b/drake_ros_core/src/subscription.cc similarity index 95% rename from drake_ros_core/src/subscription.cpp rename to drake_ros_core/src/subscription.cc index 36211b02..46aa176e 100644 --- a/drake_ros_core/src/subscription.cpp +++ b/drake_ros_core/src/subscription.cc @@ -11,18 +11,22 @@ // 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 "subscription.h" // NOLINT(build/include) #include #include namespace drake_ros_core { +namespace internal { +namespace { // 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; } +} // namespace Subscription::Subscription( rclcpp::node_interfaces::NodeBaseInterface* node_base, @@ -77,4 +81,5 @@ void Subscription::return_serialized_message( std::shared_ptr& message) { message.reset(); } +} // namespace internal } // namespace drake_ros_core diff --git a/drake_ros_core/src/subscription.hpp b/drake_ros_core/src/subscription.h similarity index 85% rename from drake_ros_core/src/subscription.hpp rename to drake_ros_core/src/subscription.h index 036b89c3..93783942 100644 --- a/drake_ros_core/src/subscription.hpp +++ b/drake_ros_core/src/subscription.h @@ -11,8 +11,7 @@ // 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_ +#pragma once #include #include @@ -23,6 +22,9 @@ #include namespace drake_ros_core { +namespace internal { +// A type-erased version of rclcpp:::Subscription. +// This class conforms to the ROS 2 C++ style for consistency. class Subscription final : public rclcpp::SubscriptionBase { public: Subscription( @@ -34,16 +36,16 @@ class Subscription final : public rclcpp::SubscriptionBase { ~Subscription(); protected: - /// Borrow a new message. + // Borrow a new message. /** \return Shared pointer to the fresh message. */ std::shared_ptr create_message() override; - /// Borrow a new serialized message + // 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. + // 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. @@ -59,11 +61,11 @@ class Subscription final : public rclcpp::SubscriptionBase { const std::shared_ptr& message, const rclcpp::MessageInfo& message_info); - /// Return the message borrowed in create_message. + // 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. + // 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; @@ -71,5 +73,5 @@ class Subscription final : public rclcpp::SubscriptionBase { private: std::function)> callback_; }; +} // namespace internal } // namespace drake_ros_core -#endif // SUBSCRIPTION_HPP_ diff --git a/drake_ros_core/test/test_drake_ros.cpp b/drake_ros_core/test/test_drake_ros.cc similarity index 96% rename from drake_ros_core/test/test_drake_ros.cpp rename to drake_ros_core/test/test_drake_ros.cc index f5aa94ec..6cf82494 100644 --- a/drake_ros_core/test/test_drake_ros.cpp +++ b/drake_ros_core/test/test_drake_ros.cc @@ -19,7 +19,7 @@ #include #include -#include "drake_ros_core/drake_ros.hpp" +#include "drake_ros_core/drake_ros.h" using drake_ros_core::DrakeRos; diff --git a/drake_ros_core/test/test_pub_sub.cpp b/drake_ros_core/test/test_pub_sub.cc similarity index 84% rename from drake_ros_core/test/test_pub_sub.cpp rename to drake_ros_core/test/test_pub_sub.cc index 0e849868..bd3d7634 100644 --- a/drake_ros_core/test/test_pub_sub.cpp +++ b/drake_ros_core/test/test_pub_sub.cc @@ -22,10 +22,10 @@ #include #include -#include "drake_ros_core/drake_ros.hpp" -#include "drake_ros_core/ros_interface_system.hpp" -#include "drake_ros_core/ros_publisher_system.hpp" -#include "drake_ros_core/ros_subscriber_system.hpp" +#include "drake_ros_core/drake_ros.h" +#include "drake_ros_core/ros_interface_system.h" +#include "drake_ros_core/ros_publisher_system.h" +#include "drake_ros_core/ros_subscriber_system.h" using drake_ros_core::DrakeRos; using drake_ros_core::RosInterfaceSystem; @@ -40,16 +40,17 @@ TEST(Integration, sub_to_pub) { rclcpp::QoS qos{rclcpp::KeepLast(num_msgs)}; qos.transient_local().reliable(); - auto sys_ros_interface = + auto ros_interface_system = builder.AddSystem(std::make_unique()); - auto sys_sub = + auto ros_subscriber_system = builder.AddSystem(RosSubscriberSystem::Make( - "in", qos, sys_ros_interface->get_ros_interface())); - auto sys_pub = + "in", qos, ros_interface_system->get_ros_interface())); + auto ros_publisher_system = builder.AddSystem(RosPublisherSystem::Make( - "out", qos, sys_ros_interface->get_ros_interface())); + "out", qos, ros_interface_system->get_ros_interface())); - builder.Connect(sys_sub->get_output_port(0), sys_pub->get_input_port(0)); + builder.Connect(ros_subscriber_system->get_output_port(0), + ros_publisher_system->get_input_port(0)); auto diagram = builder.Build(); auto context = diagram->CreateDefaultContext(); From 4a9404f292f7bfd56607426297efe828086009b5 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Tue, 9 Nov 2021 15:08:42 -0300 Subject: [PATCH 02/15] Add Python bindings to drake_ros_core package. Signed-off-by: Michel Hidalgo --- drake_ros_core/CMakeLists.txt | 31 +++++ drake_ros_core/package.xml | 3 + drake_ros_core/pycodestyle.ini | 4 + drake_ros_core/src/drake_ros_core/__init__.py | 76 ++++++++++++ .../src/python_bindings/drake_ros_core.cc | 112 ++++++++++++++++++ .../py_serializer_interface_pybind.h | 75 ++++++++++++ .../python_bindings/qos_type_caster_pybind.h | 98 +++++++++++++++ drake_ros_core/test/test_pub_sub.py | 82 +++++++++++++ 8 files changed, 481 insertions(+) create mode 100644 drake_ros_core/pycodestyle.ini create mode 100644 drake_ros_core/src/drake_ros_core/__init__.py create mode 100644 drake_ros_core/src/python_bindings/drake_ros_core.cc create mode 100644 drake_ros_core/src/python_bindings/py_serializer_interface_pybind.h create mode 100644 drake_ros_core/src/python_bindings/qos_type_caster_pybind.h create mode 100644 drake_ros_core/test/test_pub_sub.py diff --git a/drake_ros_core/CMakeLists.txt b/drake_ros_core/CMakeLists.txt index 9139c8c0..c01a0796 100644 --- a/drake_ros_core/CMakeLists.txt +++ b/drake_ros_core/CMakeLists.txt @@ -12,6 +12,8 @@ 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) @@ -59,16 +61,43 @@ 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 + "$" +) + +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_pycodestyle(--config pycodestyle.ini) + ament_add_gtest(test_pub_sub test/test_pub_sub.cc) target_link_libraries(test_pub_sub @@ -77,6 +106,8 @@ if(BUILD_TESTING) ${test_msgs_TARGETS} ) + 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 diff --git a/drake_ros_core/package.xml b/drake_ros_core/package.xml index 6861e634..8217a63f 100644 --- a/drake_ros_core/package.xml +++ b/drake_ros_core/package.xml @@ -14,9 +14,12 @@ rosidl_runtime_c rosidl_typesupport_cpp + rclpy + ament_cmake_clang_format ament_cmake_cpplint ament_cmake_gtest + ament_cmake_pycodestyle ament_cmake_pytest test_msgs diff --git a/drake_ros_core/pycodestyle.ini b/drake_ros_core/pycodestyle.ini new file mode 100644 index 00000000..4520e3f6 --- /dev/null +++ b/drake_ros_core/pycodestyle.ini @@ -0,0 +1,4 @@ +[pycodestyle] +# This is close to the ignore's that Drake uses, as of +# https://github.com/RobotLocomotion/drake/blob/v0.35.0/tools/lint/python_lint.bzl#L7 +ignore = 'E121,E123,E126,E226,E24,E704,W503' diff --git a/drake_ros_core/src/drake_ros_core/__init__.py b/drake_ros_core/src/drake_ros_core/__init__.py new file mode 100644 index 00000000..d13a9bef --- /dev/null +++ b/drake_ros_core/src/drake_ros_core/__init__.py @@ -0,0 +1,76 @@ +"""Python wrapper for drake_ros_core.""" + +from _drake_ros_core import RosInterfaceSystem +from _drake_ros_core import RosPublisherSystem +from _drake_ros_core import RosSubscriberSystem +from _drake_ros_core import SerializerInterface + +from pydrake.common.value import AbstractValue +from pydrake.systems.framework import TriggerType + +from rclpy.serialization import serialize_message +from rclpy.serialization import deserialize_message +from rclpy.type_support import check_for_type_support + + +class PySerializer(SerializerInterface): + """ + A (de)serialization interface for Python ROS messages. + """ + + def __init__(self, message_type): + SerializerInterface.__init__(self) + check_for_type_support(message_type) + self._message_type = message_type + + def GetTypeSupport(self): + return self._message_type._TYPE_SUPPORT + + def CreateDefaultValue(self): + return AbstractValue.Make(self._message_type()) + + def Serialize(self, abstract_value): + return serialize_message(abstract_value.get_value()) + + def Deserialize(self, serialized_message, abstract_value): + abstract_value.set_value(deserialize_message( + serialized_message, self._message_type)) + + +@staticmethod +def _make_ros_publisher_system( + message_type, topic_name, qos, ros_interface, + publish_triggers={ + TriggerType.kPerStep, + TriggerType.kForced}, + publish_period=0.0 +): + return RosPublisherSystem( + PySerializer(message_type), + topic_name, qos, ros_interface, + publish_triggers, publish_period) + + +RosPublisherSystem.Make = _make_ros_publisher_system + + +@staticmethod +def _make_ros_subscriber_system( + message_type, topic_name, qos, ros_interface +): + return RosSubscriberSystem( + PySerializer(message_type), + topic_name, qos, ros_interface) + + +RosSubscriberSystem.Make = _make_ros_subscriber_system + + +__all__ = [ + 'DrakeRosInterface', + 'PySerializer', + 'RosInterfaceSystem', + 'RosPublisherSystem', + 'RosSubscriberSystem', + 'SerializerInterface' +] diff --git a/drake_ros_core/src/python_bindings/drake_ros_core.cc b/drake_ros_core/src/python_bindings/drake_ros_core.cc new file mode 100644 index 00000000..d9f5d701 --- /dev/null +++ b/drake_ros_core/src/python_bindings/drake_ros_core.cc @@ -0,0 +1,112 @@ +// 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. +#include +#include + +#include "py_serializer_interface_pybind.h" // NOLINT(build/include) +#include "qos_type_caster_pybind.h" // NOLINT(build/include) +#include +#include +#include + +#include "drake_ros_core/drake_ros.h" +#include "drake_ros_core/ros_interface_system.h" +#include "drake_ros_core/ros_publisher_system.h" +#include "drake_ros_core/ros_subscriber_system.h" + +namespace py = pybind11; + +using drake::systems::LeafSystem; +using drake::systems::TriggerType; + +using drake_ros_core::DrakeRos; +using drake_ros_core::PySerializerInterface; +using drake_ros_core::RosInterfaceSystem; +using drake_ros_core::RosPublisherSystem; +using drake_ros_core::RosSubscriberSystem; +using drake_ros_core::SerializerInterface; + +PYBIND11_MODULE(_drake_ros_core, m) { + m.doc() = "Python bindings for drake_ros_core"; + // Force module name in docstrings to match + // that of the outer module. + m.attr("__name__") = "drake_ros_core"; + + py::module::import("pydrake.systems.framework"); + py::module::import("pydrake.multibody.plant"); + + // TODD(hidmic): populate Python docstrings with + // C++ docstrings. Consider using mkdoc to keep + // them in sync, like pydrake does. + py::class_(m, "DrakeRos"); + + py::class_>(m, "RosInterfaceSystem") + .def(py::init([]() { + return std::make_unique( + std::make_unique()); + })) + .def("get_ros_interface", &RosInterfaceSystem::get_ros_interface, + py::return_value_policy::reference_internal); + + py::class_(m, + "SerializerInterface") + .def(py::init([]() { return std::make_unique(); })) + .def("CreateDefaultValue", &SerializerInterface::CreateDefaultValue) + .def("GetTypeSupport", + [](const SerializerInterface& self) { + return py::capsule(self.GetTypeSupport()); + }) + .def("Serialize", + [](const SerializerInterface& self, + const drake::AbstractValue& abstract_value) { + rclcpp::SerializedMessage serialized_message = + self.Serialize(abstract_value); + const rcl_serialized_message_t& rcl_serialized_message = + serialized_message.get_rcl_serialized_message(); + return py::bytes( + reinterpret_cast(rcl_serialized_message.buffer), + rcl_serialized_message.buffer_length); + }) + .def("Deserialize", + [](const SerializerInterface& self, py::bytes raw_bytes, + drake::AbstractValue* abstract_value) { + std::string bytes = raw_bytes; + rclcpp::SerializedMessage serialized_message(bytes.size()); + rcl_serialized_message_t& rcl_serialized_message = + serialized_message.get_rcl_serialized_message(); + std::copy(bytes.data(), bytes.data() + bytes.size(), + rcl_serialized_message.buffer); + rcl_serialized_message.buffer_length = bytes.size(); + self.Deserialize(serialized_message, abstract_value); + }); + + py::class_>(m, "RosPublisherSystem") + .def(py::init([](std::unique_ptr serializer, + const char* topic_name, drake_ros_core::QoS qos, + DrakeRos* ros_interface, + std::unordered_set publish_triggers, + double publish_period) { + return std::make_unique( + std::move(serializer), topic_name, qos, ros_interface, + publish_triggers, publish_period); + })); + + py::class_>(m, "RosSubscriberSystem") + .def(py::init([](std::unique_ptr serializer, + const char* topic_name, drake_ros_core::QoS qos, + DrakeRos* ros_interface) { + return std::make_unique( + std::move(serializer), topic_name, qos, ros_interface); + })); +} diff --git a/drake_ros_core/src/python_bindings/py_serializer_interface_pybind.h b/drake_ros_core/src/python_bindings/py_serializer_interface_pybind.h new file mode 100644 index 00000000..5691c9cf --- /dev/null +++ b/drake_ros_core/src/python_bindings/py_serializer_interface_pybind.h @@ -0,0 +1,75 @@ +// 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 +#include +#include + +#include +#include + +#include "drake_ros_core/serializer_interface.h" + +namespace py = pybind11; + +namespace drake_ros_core { +// A (de)serialization interface implementation for Python ROS messages +// that can be overriden from Python itself. +class PySerializerInterface : public py::wrapper { + public: + using Base = py::wrapper; + + PySerializerInterface() : Base() {} + + const rosidl_message_type_support_t* GetTypeSupport() const override { + auto overload = [&]() -> py::capsule { + PYBIND11_OVERLOAD_PURE(py::capsule, SerializerInterface, GetTypeSupport); + }; + return static_cast(overload()); + } + + std::unique_ptr CreateDefaultValue() const override { + PYBIND11_OVERLOAD_PURE(std::unique_ptr, + SerializerInterface, CreateDefaultValue); + } + + rclcpp::SerializedMessage Serialize( + const drake::AbstractValue& abstract_value) const override { + auto overload = [&]() -> py::bytes { + PYBIND11_OVERLOAD_PURE(py::bytes, SerializerInterface, Serialize, + &abstract_value); + }; + std::string bytes = overload(); + rclcpp::SerializedMessage serialized_message(bytes.size()); + rcl_serialized_message_t& rcl_serialized_message = + serialized_message.get_rcl_serialized_message(); + std::copy(bytes.data(), bytes.data() + bytes.size(), + rcl_serialized_message.buffer); + rcl_serialized_message.buffer_length = bytes.size(); + return serialized_message; + } + + void Deserialize(const rclcpp::SerializedMessage& serialized_message, + drake::AbstractValue* abstract_value) const override { + const rcl_serialized_message_t& rcl_serialized_message = + serialized_message.get_rcl_serialized_message(); + py::bytes serialized_message_bytes( + reinterpret_cast(rcl_serialized_message.buffer), + rcl_serialized_message.buffer_length); + PYBIND11_OVERLOAD_PURE(void, SerializerInterface, Deserialize, + serialized_message_bytes, abstract_value); + } +}; +} // namespace drake_ros_core diff --git a/drake_ros_core/src/python_bindings/qos_type_caster_pybind.h b/drake_ros_core/src/python_bindings/qos_type_caster_pybind.h new file mode 100644 index 00000000..948a9e4a --- /dev/null +++ b/drake_ros_core/src/python_bindings/qos_type_caster_pybind.h @@ -0,0 +1,98 @@ +// 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 + +#include +#include + +namespace drake_ros_core { +// Thin wrapper that adds a default constructor, since rclcpp::QoS deletes +// its own and PYBIND11_TYPE_CASTER requires one. +class QoS : public rclcpp::QoS { + public: + QoS() : rclcpp::QoS(1) {} +}; +} // namespace drake_ros_core + +namespace pybind11 { +namespace detail { +// TODO(hidmic): rely on rclpy.qos.QoSProfile when and if a pybind11 binding +// is introduced upstream. See https://github.com/ros2/rclpy/issues/843. +template <> +struct type_caster { + public: + // declares local 'value' of type QoS + PYBIND11_TYPE_CASTER(drake_ros_core::QoS, _("rclpy.qos.QoSProfile")); + + // Convert from Python rclpy.qos.QoSProfile to QoS + bool load(handle src, bool) { + handle cls = module::import("rclpy.qos").attr("QoSProfile"); + if (!isinstance(src, cls)) { + return false; + } + + object source = reinterpret_borrow(src); + // 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 + + value.history(static_cast( + source.attr("history").cast())); + + if (value.history() == rclcpp::HistoryPolicy::KeepLast) { + value.keep_last(source.attr("depth").cast()); + } + + value.reliability(static_cast( + source.attr("reliability").cast())); + + value.durability(static_cast( + source.attr("durability").cast())); + + value.lifespan(rclcpp::Duration::from_nanoseconds( + source.attr("lifespan").attr("nanoseconds").cast())); + + value.deadline(rclcpp::Duration::from_nanoseconds( + source.attr("deadline").attr("nanoseconds").cast())); + + value.liveliness_lease_duration(rclcpp::Duration::from_nanoseconds( + source.attr("liveliness_lease_duration") + .attr("nanoseconds") + .cast())); + + value.avoid_ros_namespace_conventions( + source.attr("avoid_ros_namespace_conventions").cast()); + + return true; + } + + // Convert from Python QoS to rclpy.qos.QoSProfile + static handle cast(drake_ros_core::QoS src, return_value_policy policy, + handle parent) { + (void)src; + (void)policy; + (void)parent; + Py_RETURN_NOTIMPLEMENTED; + } +}; +} // namespace detail +} // namespace pybind11 diff --git a/drake_ros_core/test/test_pub_sub.py b/drake_ros_core/test/test_pub_sub.py new file mode 100644 index 00000000..0f0cca33 --- /dev/null +++ b/drake_ros_core/test/test_pub_sub.py @@ -0,0 +1,82 @@ +# 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. + +from pydrake.systems.analysis import Simulator +from pydrake.systems.framework import DiagramBuilder + +import rclpy +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 + +from drake_ros_core import RosInterfaceSystem +from drake_ros_core import RosPublisherSystem +from drake_ros_core import RosSubscriberSystem + + +def test_nominal_case(): + builder = DiagramBuilder() + + ros_interface_system = builder.AddSystem(RosInterfaceSystem()) + + qos = QoSProfile( + depth=10, + history=HistoryPolicy.KEEP_LAST, + reliability=ReliabilityPolicy.RELIABLE, + durability=DurabilityPolicy.TRANSIENT_LOCAL) + + ros_publisher_system = builder.AddSystem(RosPublisherSystem.Make( + BasicTypes, 'out_py', qos, ros_interface_system.get_ros_interface())) + + ros_subscriber_system = builder.AddSystem(RosSubscriberSystem.Make( + BasicTypes, 'in_py', qos, ros_interface_system.get_ros_interface())) + + builder.Connect( + ros_subscriber_system.get_output_port(0), + ros_publisher_system.get_input_port(0)) + diagram = builder.Build() + simulator = Simulator(diagram) + simulator_context = simulator.get_mutable_context() + simulator.set_target_realtime_rate(1.0) + + rclpy.init() + node = rclpy.create_node('sub_to_pub_py') + + # Create publisher talking to subscriber system. + publisher = node.create_publisher(BasicTypes, 'in_py', qos) + + # Create subscription listening to publisher system + rx_msgs = [] + rx_callback = (lambda msg: rx_msgs.append(msg)) + node.create_subscription(BasicTypes, 'out_py', rx_callback, qos) + + # Send messages into the drake system + num_msgs = 5 + for _ in range(num_msgs): + publisher.publish(BasicTypes()) + + timeout_sec = 5 + spins_per_sec = 10 + time_delta = 1.0 / spins_per_sec + for _ in range(timeout_sec * spins_per_sec): + if len(rx_msgs) >= num_msgs: + break + rclpy.spin_once(node, timeout_sec=time_delta) + simulator.AdvanceTo(simulator_context.get_time() + time_delta) + + # Make sure same number of messages got out + assert num_msgs == len(rx_msgs) From e8a78598481fb211bd2bb6cbaa4ef883ec12295c Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Tue, 9 Nov 2021 15:10:31 -0300 Subject: [PATCH 03/15] Cope with Python 2 pollution in CI. Signed-off-by: Michel Hidalgo --- .github/workflows/main.yml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index d4163002..c4901ae5 100644 --- a/.github/workflows/main.yml +++ b/.github/workflows/main.yml @@ -22,6 +22,8 @@ jobs: - uses: ros-tooling/setup-ros@v0.2 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/action-ros-ci@v0.2 with: From 4842d7604a8403f66fe85d6f57ea0f69e62bccf6 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Mon, 8 Nov 2021 13:05:57 -0300 Subject: [PATCH 04/15] Fix drake_ros_core C++ and Python pub/sub tests. Signed-off-by: Michel Hidalgo --- drake_ros_core/test/test_pub_sub.cc | 30 +++++++++++++++++++++-------- drake_ros_core/test/test_pub_sub.py | 25 ++++++++++++++++++------ 2 files changed, 41 insertions(+), 14 deletions(-) diff --git a/drake_ros_core/test/test_pub_sub.cc b/drake_ros_core/test/test_pub_sub.cc index bd3d7634..aab19212 100644 --- a/drake_ros_core/test/test_pub_sub.cc +++ b/drake_ros_core/test/test_pub_sub.cc @@ -73,25 +73,39 @@ TEST(Integration, sub_to_pub) { // Create subscription listening to publisher system std::vector> rx_msgs; auto rx_callback = [&](std::unique_ptr msg) { - rx_msgs.push_back(std::move(msg)); + // Cope with lack of synchronization between subscriber + // and publisher systems by ignoring duplicate messages. + if (rx_msgs.empty() || rx_msgs.back()->uint64_value != msg->uint64_value) { + 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()); - } - + size_t num_msgs_sent = 0; 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) { + for (int i = 0; i < timeout_sec * spins_per_sec; ++i) { + if (rx_msgs.size() >= num_msgs) { + break; + } + // Cope with lack of synchronization between subscriber + // and publisher systems by sending one message at a time. + if (rx_msgs.size() == num_msgs_sent) { + // Send message into the drake system + auto message = std::make_unique(); + message->uint64_value = num_msgs_sent++; + publisher->publish(std::move(message)); + } 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()); + // Make sure all messages got out and in the right order + for (size_t p = 0; p < num_msgs; ++p) { + EXPECT_EQ(rx_msgs[p]->uint64_value, p); + } } diff --git a/drake_ros_core/test/test_pub_sub.py b/drake_ros_core/test/test_pub_sub.py index 0f0cca33..55097fec 100644 --- a/drake_ros_core/test/test_pub_sub.py +++ b/drake_ros_core/test/test_pub_sub.py @@ -60,23 +60,36 @@ def test_nominal_case(): publisher = node.create_publisher(BasicTypes, 'in_py', qos) # Create subscription listening to publisher system + num_msgs = 5 rx_msgs = [] - rx_callback = (lambda msg: rx_msgs.append(msg)) - node.create_subscription(BasicTypes, 'out_py', rx_callback, qos) - # Send messages into the drake system - num_msgs = 5 - for _ in range(num_msgs): - publisher.publish(BasicTypes()) + def rx_callback(msg): + # Cope with lack of synchronization between subscriber + # and publisher systems by ignoring duplicate messages. + if not rx_msgs or rx_msgs[-1].uint64_value != msg.uint64_value: + rx_msgs.append(msg) + node.create_subscription(BasicTypes, 'out_py', rx_callback, qos) + num_msgs_sent = 0 timeout_sec = 5 spins_per_sec = 10 time_delta = 1.0 / spins_per_sec for _ in range(timeout_sec * spins_per_sec): if len(rx_msgs) >= num_msgs: break + # Cope with lack of synchronization between subscriber + # and publisher systems by sending one message at a time. + if len(rx_msgs) == num_msgs_sent: + # Send messages into the drake system + message = BasicTypes() + message.uint64_value = num_msgs_sent + publisher.publish(message) + num_msgs_sent = num_msgs_sent + 1 rclpy.spin_once(node, timeout_sec=time_delta) simulator.AdvanceTo(simulator_context.get_time() + time_delta) # Make sure same number of messages got out assert num_msgs == len(rx_msgs) + # Make sure all messages got out and in the right order + for i in range(num_msgs): + assert rx_msgs[i].uint64_value == i From 81859093cbe2a16529b2690f19c122c3b3fb7fb4 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Wed, 10 Nov 2021 18:16:09 -0300 Subject: [PATCH 05/15] Improve drake_ros_core::DrakeRos API Signed-off-by: Michel Hidalgo --- .../include/drake_ros_core/drake_ros.h | 42 ++++++++++++------- drake_ros_core/src/drake_ros.cc | 5 +++ 2 files changed, 33 insertions(+), 14 deletions(-) diff --git a/drake_ros_core/include/drake_ros_core/drake_ros.h b/drake_ros_core/include/drake_ros_core/drake_ros.h index 94315406..4526d692 100644 --- a/drake_ros_core/include/drake_ros_core/drake_ros.h +++ b/drake_ros_core/include/drake_ros_core/drake_ros.h @@ -21,34 +21,48 @@ namespace drake_ros_core { -/** A Drake ROS interface that wraps a live ROS2 node. - This interface manages both ROS2 node construction and scheduling. +/** 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 "drake_ros" ROS2 node with default options. */ + /** A constructor that wraps a "drake_ros" ROS node with default options. */ DrakeRos() : DrakeRos("drake_ros", rclcpp::NodeOptions{}) {} - /** A constructor that wraps a `node_name` ROS2 node with `node_options`. - See `rclcpp::Node` documentation for further reference on arguments. + /** 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); ~DrakeRos(); - /** Returns a constant reference to the underlying ROS2 node. */ + /** Returns a constant reference to the underlying ROS node. */ const rclcpp::Node& get_node() const; - /** Returns a mutable reference to the underlying ROS2 node. */ + /** Returns a mutable reference to the underlying ROS node. */ rclcpp::Node* get_mutable_node() const; - /** Spins the underlying ROS2 node, dispatching all available work. - @param[in] timeout_millis Timeout, in milliseconds. - If timeout is less than 0, the call will block indefinitely - until some work has been dispatched. If timeout is 0, the call - will dispatch available work without blocking. If timeout is - larger than 0, the call will wait up to the given timeout for - work to dispatch. + /** 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. + + @param[in] timeout_millis Time, in milliseconds, to wait for work to + be made available before executing it. It has not impact whatsoever + when work is available already at the time of call. 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 wait for work + up the given timeout. + @throws std::runtime_error if timeout is negative. */ void Spin(int timeout_millis = 0); diff --git a/drake_ros_core/src/drake_ros.cc b/drake_ros_core/src/drake_ros.cc index 5b26b82e..be1f18e2 100644 --- a/drake_ros_core/src/drake_ros.cc +++ b/drake_ros_core/src/drake_ros.cc @@ -69,6 +69,11 @@ const rclcpp::Node& DrakeRos::get_node() const { return *impl_->node; } rclcpp::Node* DrakeRos::get_mutable_node() const { return impl_->node.get(); } void DrakeRos::Spin(int timeout_millis) { + if (timeout_millis < 0) { + // To match `DrakeLcm::HandleSubscriptions()`'s behavior, + // throw if timeout is negative. + throw std::runtime_error("timeout cannot be negative"); + } // To match `DrakeLcm::HandleSubscriptions()`'s behavior, in the following // we wait up to the given timeout to process one work item and then process // all pending work items, if any, without waiting. From 92c921dcc6aa0540f4b57dc85c3820d8e554134e Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Wed, 10 Nov 2021 21:19:09 -0300 Subject: [PATCH 06/15] Allow drake_ros_core::RosSubscriberSystem::Make argument type inference Signed-off-by: Michel Hidalgo --- .../drake_ros_core/ros_publisher_system.h | 28 ++++++++++++------- 1 file changed, 18 insertions(+), 10 deletions(-) diff --git a/drake_ros_core/include/drake_ros_core/ros_publisher_system.h b/drake_ros_core/include/drake_ros_core/ros_publisher_system.h index 9b1bcdfc..d355b197 100644 --- a/drake_ros_core/include/drake_ros_core/ros_publisher_system.h +++ b/drake_ros_core/include/drake_ros_core/ros_publisher_system.h @@ -33,17 +33,26 @@ namespace drake_ros_core { */ class RosPublisherSystem : public drake::systems::LeafSystem { public: + static constexpr std::initializer_list + kDefaultTriggerTypes{drake::systems::TriggerType::kPerStep, + drake::systems::TriggerType::kForced}; + /** Instantiates a publisher system for a given ROS message type. See `RosPublisherSystem::RosPublisherSystem` documentation for further reference on function arguments. @tparam MessageT C++ ROS message type. */ - template - static std::unique_ptr Make(ArgsT&&... args) { + template + static std::unique_ptr Make( + const std::string& topic_name, const rclcpp::QoS& qos, DrakeRos* ros, + const std::unordered_set& publish_triggers = + kDefaultTriggerTypes, + double publish_period = 0.0) { // Assume C++ typesupport since this is a C++ template function return std::make_unique( - std::make_unique>(), std::forward(args)...); + std::make_unique>(), topic_name, qos, ros, + publish_triggers, publish_period); } /** A constructor for the ROS publisher system. @@ -60,13 +69,12 @@ class RosPublisherSystem : public drake::systems::LeafSystem { @param[in] publish_period optional publishing period, in seconds. Only applicable when periodic publishing is enabled. */ - RosPublisherSystem( - std::unique_ptr serializer, - const std::string& topic_name, const rclcpp::QoS& qos, DrakeRos* ros, - const std::unordered_set& publish_triggers = - {drake::systems::TriggerType::kPerStep, - drake::systems::TriggerType::kForced}, - double publish_period = 0.0); + RosPublisherSystem(std::unique_ptr serializer, + const std::string& topic_name, const rclcpp::QoS& qos, + DrakeRos* ros, + const std::unordered_set& + publish_triggers = kDefaultTriggerTypes, + double publish_period = 0.0); ~RosPublisherSystem() override; From b56f0736c8ae36c2e969e20eaf670ddcf99de86d Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Wed, 10 Nov 2021 21:19:35 -0300 Subject: [PATCH 07/15] Simplify drake_ros_core C++ and Python pub/sub tests. Signed-off-by: Michel Hidalgo --- drake_ros_core/test/test_pub_sub.cc | 86 +++++++++++++--------------- drake_ros_core/test/test_pub_sub.py | 88 ++++++++++++++--------------- 2 files changed, 83 insertions(+), 91 deletions(-) diff --git a/drake_ros_core/test/test_pub_sub.cc b/drake_ros_core/test/test_pub_sub.cc index aab19212..8c7c47ae 100644 --- a/drake_ros_core/test/test_pub_sub.cc +++ b/drake_ros_core/test/test_pub_sub.cc @@ -35,22 +35,21 @@ using drake_ros_core::RosSubscriberSystem; TEST(Integration, sub_to_pub) { drake::systems::DiagramBuilder builder; - const size_t num_msgs = 5; + constexpr double kPublishPeriod = 1.0; + const auto qos = rclcpp::QoS{rclcpp::KeepLast(10)}.reliable(); - rclcpp::QoS qos{rclcpp::KeepLast(num_msgs)}; - qos.transient_local().reliable(); - - auto ros_interface_system = + auto system_ros = builder.AddSystem(std::make_unique()); - auto ros_subscriber_system = + auto system_sub_in = builder.AddSystem(RosSubscriberSystem::Make( - "in", qos, ros_interface_system->get_ros_interface())); - auto ros_publisher_system = + "in", qos, system_ros->get_ros_interface())); + auto system_pub_out = builder.AddSystem(RosPublisherSystem::Make( - "out", qos, ros_interface_system->get_ros_interface())); + "out", qos, system_ros->get_ros_interface(), + {drake::systems::TriggerType::kPeriodic}, kPublishPeriod)); - builder.Connect(ros_subscriber_system->get_output_port(0), - ros_publisher_system->get_input_port(0)); + builder.Connect(system_sub_in->get_output_port(0), + system_pub_out->get_input_port(0)); auto diagram = builder.Build(); auto context = diagram->CreateDefaultContext(); @@ -67,45 +66,38 @@ TEST(Integration, sub_to_pub) { auto node = rclcpp::Node::make_shared("sub_to_pub"); // Create publisher talking to subscriber system. - auto publisher = + auto direct_pub_in = node->create_publisher("in", qos); // Create subscription listening to publisher system - std::vector> rx_msgs; - auto rx_callback = [&](std::unique_ptr msg) { - // Cope with lack of synchronization between subscriber - // and publisher systems by ignoring duplicate messages. - if (rx_msgs.empty() || rx_msgs.back()->uint64_value != msg->uint64_value) { - rx_msgs.push_back(std::move(msg)); - } - }; - auto subscription = node->create_subscription( - "out", qos, rx_callback); - - size_t num_msgs_sent = 0; - 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; ++i) { - if (rx_msgs.size() >= num_msgs) { - break; - } - // Cope with lack of synchronization between subscriber - // and publisher systems by sending one message at a time. - if (rx_msgs.size() == num_msgs_sent) { - // Send message into the drake system - auto message = std::make_unique(); - message->uint64_value = num_msgs_sent++; - publisher->publish(std::move(message)); - } + std::vector> + rx_msgs_direct_sub_out; + auto rx_callback_direct_sub_out = + [&](std::unique_ptr message) { + rx_msgs_direct_sub_out.push_back(std::move(message)); + }; + auto direct_sub_out = node->create_subscription( + "out", qos, rx_callback_direct_sub_out); + + constexpr size_t kPubSubRounds = 5; + for (size_t i = 1; i <= kPubSubRounds; ++i) { + const size_t rx_msgs_count_before_pubsub = rx_msgs_direct_sub_out.size(); + // Publish a message to the drake ros subscriber system. + auto message = std::make_unique(); + message->uint64_value = i; + direct_pub_in->publish(std::move(message)); + // Step forward to allow the message to be dispatched to the drake ros + // subscriber system. The drake ros publisher system should not publish + // just yet. 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()); - // Make sure all messages got out and in the right order - for (size_t p = 0; p < num_msgs; ++p) { - EXPECT_EQ(rx_msgs[p]->uint64_value, p); + simulator->AdvanceTo(simulator_context.get_time() + kPublishPeriod / 2.); + ASSERT_EQ(rx_msgs_direct_sub_out.size(), rx_msgs_count_before_pubsub); + // Step forward until it is about time the drake ros publisher publishes. + // Allow the message to be dispatched to the direct subscription. + simulator->AdvanceTo(simulator_context.get_time() + kPublishPeriod / 2.); + rclcpp::spin_some(node); + const size_t rx_msgs_count_after_pubsub = rx_msgs_count_before_pubsub + 1; + ASSERT_EQ(rx_msgs_direct_sub_out.size(), rx_msgs_count_after_pubsub); + EXPECT_EQ(rx_msgs_direct_sub_out.back()->uint64_value, i); } } diff --git a/drake_ros_core/test/test_pub_sub.py b/drake_ros_core/test/test_pub_sub.py index 55097fec..1da17726 100644 --- a/drake_ros_core/test/test_pub_sub.py +++ b/drake_ros_core/test/test_pub_sub.py @@ -14,6 +14,7 @@ from pydrake.systems.analysis import Simulator from pydrake.systems.framework import DiagramBuilder +from pydrake.systems.framework import TriggerType import rclpy from rclpy.qos import DurabilityPolicy @@ -31,65 +32,64 @@ def test_nominal_case(): builder = DiagramBuilder() - ros_interface_system = builder.AddSystem(RosInterfaceSystem()) + system_ros = builder.AddSystem(RosInterfaceSystem()) + + publish_period = 1.0 qos = QoSProfile( depth=10, history=HistoryPolicy.KEEP_LAST, - reliability=ReliabilityPolicy.RELIABLE, - durability=DurabilityPolicy.TRANSIENT_LOCAL) + reliability=ReliabilityPolicy.RELIABLE) - ros_publisher_system = builder.AddSystem(RosPublisherSystem.Make( - BasicTypes, 'out_py', qos, ros_interface_system.get_ros_interface())) + system_pub_out = builder.AddSystem(RosPublisherSystem.Make( + BasicTypes, 'out_py', qos, system_ros.get_ros_interface(), + {TriggerType.kPeriodic}, publish_period)) - ros_subscriber_system = builder.AddSystem(RosSubscriberSystem.Make( - BasicTypes, 'in_py', qos, ros_interface_system.get_ros_interface())) + system_sub_in = builder.AddSystem(RosSubscriberSystem.Make( + BasicTypes, 'in_py', qos, system_ros.get_ros_interface())) builder.Connect( - ros_subscriber_system.get_output_port(0), - ros_publisher_system.get_input_port(0)) + system_sub_in.get_output_port(0), + system_pub_out.get_input_port(0)) diagram = builder.Build() simulator = Simulator(diagram) - simulator_context = simulator.get_mutable_context() simulator.set_target_realtime_rate(1.0) + simulator.Initialize() + + simulator_context = simulator.get_mutable_context() rclpy.init() node = rclpy.create_node('sub_to_pub_py') # Create publisher talking to subscriber system. - publisher = node.create_publisher(BasicTypes, 'in_py', qos) + direct_pub_in = node.create_publisher(BasicTypes, 'in_py', qos) # Create subscription listening to publisher system - num_msgs = 5 - rx_msgs = [] - - def rx_callback(msg): - # Cope with lack of synchronization between subscriber - # and publisher systems by ignoring duplicate messages. - if not rx_msgs or rx_msgs[-1].uint64_value != msg.uint64_value: - rx_msgs.append(msg) - node.create_subscription(BasicTypes, 'out_py', rx_callback, qos) - - num_msgs_sent = 0 - timeout_sec = 5 - spins_per_sec = 10 - time_delta = 1.0 / spins_per_sec - for _ in range(timeout_sec * spins_per_sec): - if len(rx_msgs) >= num_msgs: - break - # Cope with lack of synchronization between subscriber - # and publisher systems by sending one message at a time. - if len(rx_msgs) == num_msgs_sent: - # Send messages into the drake system - message = BasicTypes() - message.uint64_value = num_msgs_sent - publisher.publish(message) - num_msgs_sent = num_msgs_sent + 1 - rclpy.spin_once(node, timeout_sec=time_delta) - simulator.AdvanceTo(simulator_context.get_time() + time_delta) - - # Make sure same number of messages got out - assert num_msgs == len(rx_msgs) - # Make sure all messages got out and in the right order - for i in range(num_msgs): - assert rx_msgs[i].uint64_value == i + rx_msgs_direct_sub_out = [] + + def rx_callback_direct_sub_out(msg): + rx_msgs_direct_sub_out.append(msg) + direct_sub_out = node.create_subscription( + BasicTypes, 'out_py', rx_callback_direct_sub_out, qos) + + pub_sub_rounds = 5 + for i in range(1, pub_sub_rounds + 1): + rx_msgs_count_before_pubsub = len(rx_msgs_direct_sub_out) + # Publish a message to the drake ros subscriber system. + message = BasicTypes() + message.uint64_value = i + direct_pub_in.publish(message) + # Step forward to allow the message to be dispatched to the drake ros + # subscriber system. The drake ros publisher system should not publish + # just yet. + rclpy.spin_once(node, timeout_sec=0.5) + simulator.AdvanceTo(simulator_context.get_time() + publish_period / 2.) + assert len(rx_msgs_direct_sub_out) == rx_msgs_count_before_pubsub + # Step forward until it is about time the drake ros publisher + # publishes. Allow the message to be dispatched to the direct + # subscription. + simulator.AdvanceTo(simulator_context.get_time() + publish_period / 2.) + rclpy.spin_once(node, timeout_sec=0.) + rx_msgs_count_after_pubsub = rx_msgs_count_before_pubsub + 1 + assert len(rx_msgs_direct_sub_out) == rx_msgs_count_after_pubsub + assert rx_msgs_direct_sub_out[-1].uint64_value == i From 8e23c115cf635f2c42f319b0c5f45bf6dba335a9 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Thu, 11 Nov 2021 13:36:59 -0300 Subject: [PATCH 08/15] Remove large timeout from Python pub/sub test. Signed-off-by: Michel Hidalgo --- drake_ros_core/test/test_pub_sub.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/drake_ros_core/test/test_pub_sub.py b/drake_ros_core/test/test_pub_sub.py index 1da17726..2f8e46aa 100644 --- a/drake_ros_core/test/test_pub_sub.py +++ b/drake_ros_core/test/test_pub_sub.py @@ -82,7 +82,7 @@ def rx_callback_direct_sub_out(msg): # Step forward to allow the message to be dispatched to the drake ros # subscriber system. The drake ros publisher system should not publish # just yet. - rclpy.spin_once(node, timeout_sec=0.5) + rclpy.spin_once(node, timeout_sec=0.) simulator.AdvanceTo(simulator_context.get_time() + publish_period / 2.) assert len(rx_msgs_direct_sub_out) == rx_msgs_count_before_pubsub # Step forward until it is about time the drake ros publisher From 8836bc42a60a586e9e9c319cad56fc55121de1fa Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Thu, 11 Nov 2021 13:37:46 -0300 Subject: [PATCH 09/15] Use spin_some() in DrakeRos::Spin() implementation. Signed-off-by: Michel Hidalgo --- drake_ros_core/include/drake_ros_core/drake_ros.h | 13 ++++++------- drake_ros_core/src/drake_ros.cc | 6 +----- 2 files changed, 7 insertions(+), 12 deletions(-) diff --git a/drake_ros_core/include/drake_ros_core/drake_ros.h b/drake_ros_core/include/drake_ros_core/drake_ros.h index 4526d692..8b47e2a6 100644 --- a/drake_ros_core/include/drake_ros_core/drake_ros.h +++ b/drake_ros_core/include/drake_ros_core/drake_ros.h @@ -54,14 +54,13 @@ class DrakeRos final { 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. + `drake::lcm::DrakeLcm::HandleSubscriptions()` method (to a partial extent). - @param[in] timeout_millis Time, in milliseconds, to wait for work to - be made available before executing it. It has not impact whatsoever - when work is available already at the time of call. 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 wait for work - up the given timeout. + @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); diff --git a/drake_ros_core/src/drake_ros.cc b/drake_ros_core/src/drake_ros.cc index be1f18e2..1ec1f5d9 100644 --- a/drake_ros_core/src/drake_ros.cc +++ b/drake_ros_core/src/drake_ros.cc @@ -74,10 +74,6 @@ void DrakeRos::Spin(int timeout_millis) { // throw if timeout is negative. throw std::runtime_error("timeout cannot be negative"); } - // To match `DrakeLcm::HandleSubscriptions()`'s behavior, in the following - // we wait up to the given timeout to process one work item and then process - // all pending work items, if any, without waiting. - impl_->executor->spin_once(std::chrono::milliseconds(timeout_millis)); - impl_->executor->spin_some(std::chrono::milliseconds(0)); + impl_->executor->spin_some(std::chrono::milliseconds(timeout_millis)); } } // namespace drake_ros_core From 7db7f5808daaae8dba9e97cff4fc70587a19b8d7 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Thu, 11 Nov 2021 13:51:03 -0300 Subject: [PATCH 10/15] Pass explicit node name to DrakeRos Signed-off-by: Michel Hidalgo --- drake_ros_core/include/drake_ros_core/drake_ros.h | 7 +++---- drake_ros_core/src/python_bindings/drake_ros_core.cc | 4 ++-- drake_ros_core/test/test_drake_ros.cc | 2 +- drake_ros_core/test/test_pub_sub.cc | 3 ++- drake_ros_core/test/test_pub_sub.py | 3 ++- 5 files changed, 10 insertions(+), 9 deletions(-) diff --git a/drake_ros_core/include/drake_ros_core/drake_ros.h b/drake_ros_core/include/drake_ros_core/drake_ros.h index 8b47e2a6..6d25ff20 100644 --- a/drake_ros_core/include/drake_ros_core/drake_ros.h +++ b/drake_ros_core/include/drake_ros_core/drake_ros.h @@ -30,13 +30,12 @@ namespace drake_ros_core { */ class DrakeRos final { public: - /** A constructor that wraps a "drake_ros" ROS node with default options. */ - DrakeRos() : DrakeRos("drake_ros", rclcpp::NodeOptions{}) {} - /** 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); + DrakeRos( + const std::string& node_name, + rclcpp::NodeOptions node_options = rclcpp::NodeOptions{}); ~DrakeRos(); diff --git a/drake_ros_core/src/python_bindings/drake_ros_core.cc b/drake_ros_core/src/python_bindings/drake_ros_core.cc index d9f5d701..f4c68237 100644 --- a/drake_ros_core/src/python_bindings/drake_ros_core.cc +++ b/drake_ros_core/src/python_bindings/drake_ros_core.cc @@ -52,9 +52,9 @@ PYBIND11_MODULE(_drake_ros_core, m) { py::class_(m, "DrakeRos"); py::class_>(m, "RosInterfaceSystem") - .def(py::init([]() { + .def(py::init([](const std::string& node_name) { return std::make_unique( - std::make_unique()); + std::make_unique(node_name)); })) .def("get_ros_interface", &RosInterfaceSystem::get_ros_interface, py::return_value_policy::reference_internal); diff --git a/drake_ros_core/test/test_drake_ros.cc b/drake_ros_core/test/test_drake_ros.cc index 6cf82494..afbe9952 100644 --- a/drake_ros_core/test/test_drake_ros.cc +++ b/drake_ros_core/test/test_drake_ros.cc @@ -24,7 +24,7 @@ using drake_ros_core::DrakeRos; TEST(DrakeRos, default_construct) { - EXPECT_NO_THROW(std::make_unique()); + EXPECT_NO_THROW(std::make_unique("default_node")); } TEST(DrakeRos, local_context) { diff --git a/drake_ros_core/test/test_pub_sub.cc b/drake_ros_core/test/test_pub_sub.cc index 8c7c47ae..a2baa6f8 100644 --- a/drake_ros_core/test/test_pub_sub.cc +++ b/drake_ros_core/test/test_pub_sub.cc @@ -39,7 +39,8 @@ TEST(Integration, sub_to_pub) { const auto qos = rclcpp::QoS{rclcpp::KeepLast(10)}.reliable(); auto system_ros = - builder.AddSystem(std::make_unique()); + builder.AddSystem( + std::make_unique("pub_to_sub")); auto system_sub_in = builder.AddSystem(RosSubscriberSystem::Make( "in", qos, system_ros->get_ros_interface())); diff --git a/drake_ros_core/test/test_pub_sub.py b/drake_ros_core/test/test_pub_sub.py index 2f8e46aa..63e774e4 100644 --- a/drake_ros_core/test/test_pub_sub.py +++ b/drake_ros_core/test/test_pub_sub.py @@ -32,7 +32,8 @@ def test_nominal_case(): builder = DiagramBuilder() - system_ros = builder.AddSystem(RosInterfaceSystem()) + system_ros = builder.AddSystem( + RosInterfaceSystem('pub_to_sub_py')) publish_period = 1.0 From a395cdb3a3af27bae9c554034e00283a537b2090 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Thu, 11 Nov 2021 13:56:46 -0300 Subject: [PATCH 11/15] Update variable names in tests. Signed-off-by: Michel Hidalgo --- drake_ros_core/test/test_pub_sub.cc | 13 +++++++------ drake_ros_core/test/test_pub_sub.py | 10 +++++----- 2 files changed, 12 insertions(+), 11 deletions(-) diff --git a/drake_ros_core/test/test_pub_sub.cc b/drake_ros_core/test/test_pub_sub.cc index a2baa6f8..f8ed794a 100644 --- a/drake_ros_core/test/test_pub_sub.cc +++ b/drake_ros_core/test/test_pub_sub.cc @@ -64,11 +64,11 @@ TEST(Integration, sub_to_pub) { // Don't need to rclcpp::init because DrakeRos uses global rclcpp::Context by // default - auto node = rclcpp::Node::make_shared("sub_to_pub"); + auto direct_ros_node = rclcpp::Node::make_shared("sub_to_pub"); // Create publisher talking to subscriber system. auto direct_pub_in = - node->create_publisher("in", qos); + direct_ros_node->create_publisher("in", qos); // Create subscription listening to publisher system std::vector> @@ -77,8 +77,9 @@ TEST(Integration, sub_to_pub) { [&](std::unique_ptr message) { rx_msgs_direct_sub_out.push_back(std::move(message)); }; - auto direct_sub_out = node->create_subscription( - "out", qos, rx_callback_direct_sub_out); + auto direct_sub_out = + direct_ros_node->create_subscription( + "out", qos, rx_callback_direct_sub_out); constexpr size_t kPubSubRounds = 5; for (size_t i = 1; i <= kPubSubRounds; ++i) { @@ -90,13 +91,13 @@ TEST(Integration, sub_to_pub) { // Step forward to allow the message to be dispatched to the drake ros // subscriber system. The drake ros publisher system should not publish // just yet. - rclcpp::spin_some(node); + rclcpp::spin_some(direct_ros_node); simulator->AdvanceTo(simulator_context.get_time() + kPublishPeriod / 2.); ASSERT_EQ(rx_msgs_direct_sub_out.size(), rx_msgs_count_before_pubsub); // Step forward until it is about time the drake ros publisher publishes. // Allow the message to be dispatched to the direct subscription. simulator->AdvanceTo(simulator_context.get_time() + kPublishPeriod / 2.); - rclcpp::spin_some(node); + rclcpp::spin_some(direct_ros_node); const size_t rx_msgs_count_after_pubsub = rx_msgs_count_before_pubsub + 1; ASSERT_EQ(rx_msgs_direct_sub_out.size(), rx_msgs_count_after_pubsub); EXPECT_EQ(rx_msgs_direct_sub_out.back()->uint64_value, i); diff --git a/drake_ros_core/test/test_pub_sub.py b/drake_ros_core/test/test_pub_sub.py index 63e774e4..db22cf4f 100644 --- a/drake_ros_core/test/test_pub_sub.py +++ b/drake_ros_core/test/test_pub_sub.py @@ -60,17 +60,17 @@ def test_nominal_case(): simulator_context = simulator.get_mutable_context() rclpy.init() - node = rclpy.create_node('sub_to_pub_py') + direct_ros_node = rclpy.create_node('sub_to_pub_py') # Create publisher talking to subscriber system. - direct_pub_in = node.create_publisher(BasicTypes, 'in_py', qos) + direct_pub_in = direct_ros_node.create_publisher(BasicTypes, 'in_py', qos) # Create subscription listening to publisher system rx_msgs_direct_sub_out = [] def rx_callback_direct_sub_out(msg): rx_msgs_direct_sub_out.append(msg) - direct_sub_out = node.create_subscription( + direct_sub_out = direct_ros_node.create_subscription( BasicTypes, 'out_py', rx_callback_direct_sub_out, qos) pub_sub_rounds = 5 @@ -83,14 +83,14 @@ def rx_callback_direct_sub_out(msg): # Step forward to allow the message to be dispatched to the drake ros # subscriber system. The drake ros publisher system should not publish # just yet. - rclpy.spin_once(node, timeout_sec=0.) + rclpy.spin_once(direct_ros_node, timeout_sec=0.) simulator.AdvanceTo(simulator_context.get_time() + publish_period / 2.) assert len(rx_msgs_direct_sub_out) == rx_msgs_count_before_pubsub # Step forward until it is about time the drake ros publisher # publishes. Allow the message to be dispatched to the direct # subscription. simulator.AdvanceTo(simulator_context.get_time() + publish_period / 2.) - rclpy.spin_once(node, timeout_sec=0.) + rclpy.spin_once(direct_ros_node, timeout_sec=0.) rx_msgs_count_after_pubsub = rx_msgs_count_before_pubsub + 1 assert len(rx_msgs_direct_sub_out) == rx_msgs_count_after_pubsub assert rx_msgs_direct_sub_out[-1].uint64_value == i From da5dcfbeb894d746e7beaa398d76808c14e5fb2e Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Thu, 11 Nov 2021 13:57:26 -0300 Subject: [PATCH 12/15] Correct DrakeRos implementation comments Signed-off-by: Michel Hidalgo --- drake_ros_core/src/drake_ros.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/drake_ros_core/src/drake_ros.cc b/drake_ros_core/src/drake_ros.cc index 1ec1f5d9..5c95fd0f 100644 --- a/drake_ros_core/src/drake_ros.cc +++ b/drake_ros_core/src/drake_ros.cc @@ -39,10 +39,10 @@ DrakeRos::DrakeRos(const std::string& node_name, } impl_->context = node_options.context(); if (impl_->context->is_valid()) { - // Context is being init/shutdown outside of this system + // Context has been init'd and will be shutdown outside of this system impl_->externally_init = true; } else { - // This system will init/shutdown the context + // This system will initialize and shutdown the context impl_->externally_init = false; impl_->context->init(0, nullptr); } From a423aa3363324a7696d83a038eb4d1a743dd8808 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Thu, 11 Nov 2021 14:01:14 -0300 Subject: [PATCH 13/15] Please clang-format Signed-off-by: Michel Hidalgo --- drake_ros_core/include/drake_ros_core/drake_ros.h | 5 ++--- drake_ros_core/test/test_pub_sub.cc | 5 ++--- 2 files changed, 4 insertions(+), 6 deletions(-) diff --git a/drake_ros_core/include/drake_ros_core/drake_ros.h b/drake_ros_core/include/drake_ros_core/drake_ros.h index 6d25ff20..6e9a09a6 100644 --- a/drake_ros_core/include/drake_ros_core/drake_ros.h +++ b/drake_ros_core/include/drake_ros_core/drake_ros.h @@ -33,9 +33,8 @@ class DrakeRos final { /** 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(const std::string& node_name, + rclcpp::NodeOptions node_options = rclcpp::NodeOptions{}); ~DrakeRos(); diff --git a/drake_ros_core/test/test_pub_sub.cc b/drake_ros_core/test/test_pub_sub.cc index f8ed794a..ce872726 100644 --- a/drake_ros_core/test/test_pub_sub.cc +++ b/drake_ros_core/test/test_pub_sub.cc @@ -38,9 +38,8 @@ TEST(Integration, sub_to_pub) { constexpr double kPublishPeriod = 1.0; const auto qos = rclcpp::QoS{rclcpp::KeepLast(10)}.reliable(); - auto system_ros = - builder.AddSystem( - std::make_unique("pub_to_sub")); + auto system_ros = builder.AddSystem( + std::make_unique("pub_to_sub")); auto system_sub_in = builder.AddSystem(RosSubscriberSystem::Make( "in", qos, system_ros->get_ros_interface())); From 74f4a80b05085531a0adfa975b434a8679c5c48a Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Thu, 11 Nov 2021 15:16:58 -0300 Subject: [PATCH 14/15] Add TODO to DrakeRos::Spin implementation. Signed-off-by: Michel Hidalgo --- drake_ros_core/src/drake_ros.cc | 2 ++ 1 file changed, 2 insertions(+) diff --git a/drake_ros_core/src/drake_ros.cc b/drake_ros_core/src/drake_ros.cc index 5c95fd0f..1ee89004 100644 --- a/drake_ros_core/src/drake_ros.cc +++ b/drake_ros_core/src/drake_ros.cc @@ -74,6 +74,8 @@ void DrakeRos::Spin(int timeout_millis) { // throw if timeout is negative. throw std::runtime_error("timeout cannot be negative"); } + // TODO(hidmic): switch to rclcpp::Executor::spin_all() when and if a zero + // timeout is supported. See https://github.com/ros2/rclcpp/issues/1825. impl_->executor->spin_some(std::chrono::milliseconds(timeout_millis)); } } // namespace drake_ros_core From 471c8c2cd9dbff8c0f7aee31a503ca6418b1b151 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Thu, 11 Nov 2021 17:41:47 -0300 Subject: [PATCH 15/15] Add explicit init/shutdown API to drake_ros_core Signed-off-by: Michel Hidalgo --- .../include/drake_ros_core/drake_ros.h | 11 ++++++++ drake_ros_core/src/drake_ros.cc | 27 ++++++++----------- drake_ros_core/src/drake_ros_core/__init__.py | 6 ++++- .../src/python_bindings/drake_ros_core.cc | 13 +++++++++ drake_ros_core/test/test_drake_ros.cc | 7 ++--- drake_ros_core/test/test_pub_sub.cc | 4 +++ drake_ros_core/test/test_pub_sub.py | 5 ++++ 7 files changed, 53 insertions(+), 20 deletions(-) diff --git a/drake_ros_core/include/drake_ros_core/drake_ros.h b/drake_ros_core/include/drake_ros_core/drake_ros.h index 6e9a09a6..11031e31 100644 --- a/drake_ros_core/include/drake_ros_core/drake_ros.h +++ b/drake_ros_core/include/drake_ros_core/drake_ros.h @@ -67,4 +67,15 @@ class DrakeRos final { struct Impl; std::unique_ptr 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 diff --git a/drake_ros_core/src/drake_ros.cc b/drake_ros_core/src/drake_ros.cc index 1ee89004..d54f8d2a 100644 --- a/drake_ros_core/src/drake_ros.cc +++ b/drake_ros_core/src/drake_ros.cc @@ -23,7 +23,6 @@ namespace drake_ros_core { struct DrakeRos::Impl { - bool externally_init; rclcpp::Context::SharedPtr context; rclcpp::Node::UniquePtr node; rclcpp::executors::SingleThreadedExecutor::UniquePtr executor; @@ -38,18 +37,10 @@ DrakeRos::DrakeRos(const std::string& node_name, throw std::invalid_argument("NodeOptions must contain a non-null context"); } impl_->context = node_options.context(); - if (impl_->context->is_valid()) { - // Context has been init'd and will be shutdown outside of this system - impl_->externally_init = true; - } else { - // This system will initialize and 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 + // TODO(hidmic): optionally take a user-provided Executor instance rclcpp::ExecutorOptions eo; eo.context = impl_->context; impl_->executor.reset(new rclcpp::executors::SingleThreadedExecutor(eo)); @@ -57,12 +48,7 @@ DrakeRos::DrakeRos(const std::string& node_name, 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()"); - } -} +DrakeRos::~DrakeRos() {} const rclcpp::Node& DrakeRos::get_node() const { return *impl_->node; } @@ -78,4 +64,13 @@ void DrakeRos::Spin(int timeout_millis) { // timeout is supported. See https://github.com/ros2/rclcpp/issues/1825. impl_->executor->spin_some(std::chrono::milliseconds(timeout_millis)); } + +void init(int argc, const char** argv) { + if (!rclcpp::ok()) { + rclcpp::init(argc, argv); + } +} + +bool shutdown() { return rclcpp::shutdown(); } + } // namespace drake_ros_core diff --git a/drake_ros_core/src/drake_ros_core/__init__.py b/drake_ros_core/src/drake_ros_core/__init__.py index d13a9bef..c0837687 100644 --- a/drake_ros_core/src/drake_ros_core/__init__.py +++ b/drake_ros_core/src/drake_ros_core/__init__.py @@ -1,9 +1,11 @@ """Python wrapper for drake_ros_core.""" +from _drake_ros_core import init from _drake_ros_core import RosInterfaceSystem from _drake_ros_core import RosPublisherSystem from _drake_ros_core import RosSubscriberSystem from _drake_ros_core import SerializerInterface +from _drake_ros_core import shutdown from pydrake.common.value import AbstractValue from pydrake.systems.framework import TriggerType @@ -68,9 +70,11 @@ def _make_ros_subscriber_system( __all__ = [ 'DrakeRosInterface', + 'init', 'PySerializer', 'RosInterfaceSystem', 'RosPublisherSystem', 'RosSubscriberSystem', - 'SerializerInterface' + 'SerializerInterface', + 'shutdown', ] diff --git a/drake_ros_core/src/python_bindings/drake_ros_core.cc b/drake_ros_core/src/python_bindings/drake_ros_core.cc index f4c68237..a81ecd1d 100644 --- a/drake_ros_core/src/python_bindings/drake_ros_core.cc +++ b/drake_ros_core/src/python_bindings/drake_ros_core.cc @@ -51,6 +51,19 @@ PYBIND11_MODULE(_drake_ros_core, m) { // them in sync, like pydrake does. py::class_(m, "DrakeRos"); + m.def( + "init", + [](std::vector args) { + std::vector raw_args; + raw_args.reserve(args.size()); + for (auto& arg : args) { + raw_args.push_back(arg.c_str()); + } + drake_ros_core::init(raw_args.size(), raw_args.data()); + }, + py::arg("args") = std::vector{}); + m.def("shutdown", &drake_ros_core::shutdown); + py::class_>(m, "RosInterfaceSystem") .def(py::init([](const std::string& node_name) { return std::make_unique( diff --git a/drake_ros_core/test/test_drake_ros.cc b/drake_ros_core/test/test_drake_ros.cc index afbe9952..8b1ea387 100644 --- a/drake_ros_core/test/test_drake_ros.cc +++ b/drake_ros_core/test/test_drake_ros.cc @@ -24,18 +24,19 @@ using drake_ros_core::DrakeRos; TEST(DrakeRos, default_construct) { + drake_ros_core::init(); EXPECT_NO_THROW(std::make_unique("default_node")); + EXPECT_TRUE(drake_ros_core::shutdown()); } TEST(DrakeRos, local_context) { auto context = std::make_shared(); rclcpp::NodeOptions node_options; node_options.context(context); - + context->init(0, nullptr); 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()); + context->shutdown("done"); } diff --git a/drake_ros_core/test/test_pub_sub.cc b/drake_ros_core/test/test_pub_sub.cc index ce872726..45156a1e 100644 --- a/drake_ros_core/test/test_pub_sub.cc +++ b/drake_ros_core/test/test_pub_sub.cc @@ -33,6 +33,8 @@ using drake_ros_core::RosPublisherSystem; using drake_ros_core::RosSubscriberSystem; TEST(Integration, sub_to_pub) { + drake_ros_core::init(0, nullptr); + drake::systems::DiagramBuilder builder; constexpr double kPublishPeriod = 1.0; @@ -101,4 +103,6 @@ TEST(Integration, sub_to_pub) { ASSERT_EQ(rx_msgs_direct_sub_out.size(), rx_msgs_count_after_pubsub); EXPECT_EQ(rx_msgs_direct_sub_out.back()->uint64_value, i); } + + drake_ros_core::shutdown(); } diff --git a/drake_ros_core/test/test_pub_sub.py b/drake_ros_core/test/test_pub_sub.py index db22cf4f..a9ab9bde 100644 --- a/drake_ros_core/test/test_pub_sub.py +++ b/drake_ros_core/test/test_pub_sub.py @@ -24,12 +24,15 @@ from test_msgs.msg import BasicTypes +import drake_ros_core from drake_ros_core import RosInterfaceSystem from drake_ros_core import RosPublisherSystem from drake_ros_core import RosSubscriberSystem def test_nominal_case(): + drake_ros_core.init() + builder = DiagramBuilder() system_ros = builder.AddSystem( @@ -94,3 +97,5 @@ def rx_callback_direct_sub_out(msg): rx_msgs_count_after_pubsub = rx_msgs_count_before_pubsub + 1 assert len(rx_msgs_direct_sub_out) == rx_msgs_count_after_pubsub assert rx_msgs_direct_sub_out[-1].uint64_value == i + + drake_ros_core.shutdown()