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: diff --git a/drake_ros_core/CMakeLists.txt b/drake_ros_core/CMakeLists.txt index 074908bb..c01a0796 100644 --- a/drake_ros_core/CMakeLists.txt +++ b/drake_ros_core/CMakeLists.txt @@ -12,17 +12,19 @@ endif() find_package(ament_cmake_ros REQUIRED) find_package(drake REQUIRED) +# Must use Drake's fork of Pybind11 +find_package(pybind11 REQUIRED HINTS "${drake_DIR}/../pybind11" NO_DEFAULT_PATH) find_package(rclcpp REQUIRED) find_package(rosidl_runtime_c REQUIRED) find_package(rosidl_typesupport_cpp REQUIRED) add_library(drake_ros_core - src/drake_ros.cpp - src/publisher.cpp - src/ros_interface_system.cpp - src/ros_publisher_system.cpp - src/ros_subscriber_system.cpp - src/subscription.cpp + src/drake_ros.cc + src/publisher.cc + src/ros_interface_system.cc + src/ros_publisher_system.cc + src/ros_subscriber_system.cc + src/subscription.cc ) target_link_libraries(drake_ros_core PUBLIC @@ -59,17 +61,44 @@ 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_add_gtest(test_pub_sub test/test_pub_sub.cpp) + ament_pycodestyle(--config pycodestyle.ini) + + ament_add_gtest(test_pub_sub test/test_pub_sub.cc) target_link_libraries(test_pub_sub drake::drake @@ -77,7 +106,9 @@ if(BUILD_TESTING) ${test_msgs_TARGETS} ) - ament_add_gtest(test_drake_ros test/test_drake_ros.cpp) + ament_add_pytest_test(test_pub_sub_py test/test_pub_sub.py) + + ament_add_gtest(test_drake_ros test/test_drake_ros.cc) target_link_libraries(test_drake_ros drake_ros_core ) 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..11031e31 --- /dev/null +++ b/drake_ros_core/include/drake_ros_core/drake_ros.h @@ -0,0 +1,81 @@ +// Copyright 2021 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#pragma once + +#include +#include + +#include +#include + +namespace drake_ros_core { + +/** A Drake ROS interface that wraps a live ROS node. + + This interface manages both ROS node construction and scheduling + (using a `rclcpp::executors::SingleThreadedExecutor` instance). + See `rclcpp::Node` and `rclcpp::Executor` documentation for further + reference on expected behavior. + */ +class DrakeRos final { + public: + /** A constructor that wraps a `node_name` ROS node with `node_options`. + See `rclcpp::Node::Node` documentation for further reference on arguments. + */ + DrakeRos(const std::string& node_name, + rclcpp::NodeOptions node_options = rclcpp::NodeOptions{}); + + ~DrakeRos(); + + /** Returns a constant reference to the underlying ROS node. */ + const rclcpp::Node& get_node() const; + + /** Returns a mutable reference to the underlying ROS node. */ + rclcpp::Node* get_mutable_node() const; + + /** Spins the underlying ROS node, dispatching all available work if any. + + In this context, work refers to subscription callbacks, timer callbacks, + service request and reply callbacks, etc., that are registered with the + underlying `rclcpp::Node` instance. Availability implies these are ready + to be serviced by the underlying `rclcpp::Executor` instance. + + This method's behavior has been modeled after that of the + `drake::lcm::DrakeLcm::HandleSubscriptions()` method (to a partial extent). + + @param[in] timeout_millis Timeout, in milliseconds, when fetching work. + Negative timeout values are not allowed. If timeout is 0, the call will + not wait for any new work. If timeout is larger than 0, the call will + continue fetching work up to the given timeout or until no work is + available. + @throws std::runtime_error if timeout is negative. + */ + void Spin(int timeout_millis = 0); + + private: + struct Impl; + std::unique_ptr impl_; +}; + +/**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/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..d355b197 --- /dev/null +++ b/drake_ros_core/include/drake_ros_core/ros_publisher_system.h @@ -0,0 +1,99 @@ +// 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: + 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( + 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>(), topic_name, qos, ros, + publish_triggers, publish_period); + } + + /** 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 = kDefaultTriggerTypes, + 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/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.cc b/drake_ros_core/src/drake_ros.cc new file mode 100644 index 00000000..d54f8d2a --- /dev/null +++ b/drake_ros_core/src/drake_ros.cc @@ -0,0 +1,76 @@ +// 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 { + 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(); + + impl_->node.reset(new rclcpp::Node(node_name, node_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)); + + impl_->executor->add_node(impl_->node->get_node_base_interface()); +} + +DrakeRos::~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) { + if (timeout_millis < 0) { + // To match `DrakeLcm::HandleSubscriptions()`'s behavior, + // 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)); +} + +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.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/drake_ros_core/__init__.py b/drake_ros_core/src/drake_ros_core/__init__.py new file mode 100644 index 00000000..c0837687 --- /dev/null +++ b/drake_ros_core/src/drake_ros_core/__init__.py @@ -0,0 +1,80 @@ +"""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 + +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', + 'init', + 'PySerializer', + 'RosInterfaceSystem', + 'RosPublisherSystem', + 'RosSubscriberSystem', + 'SerializerInterface', + 'shutdown', +] 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/python_bindings/drake_ros_core.cc b/drake_ros_core/src/python_bindings/drake_ros_core.cc new file mode 100644 index 00000000..a81ecd1d --- /dev/null +++ b/drake_ros_core/src/python_bindings/drake_ros_core.cc @@ -0,0 +1,125 @@ +// 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"); + + 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( + std::make_unique(node_name)); + })) + .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/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 83% rename from drake_ros_core/test/test_drake_ros.cpp rename to drake_ros_core/test/test_drake_ros.cc index f5aa94ec..8b1ea387 100644 --- a/drake_ros_core/test/test_drake_ros.cpp +++ b/drake_ros_core/test/test_drake_ros.cc @@ -19,23 +19,24 @@ #include #include -#include "drake_ros_core/drake_ros.hpp" +#include "drake_ros_core/drake_ros.h" using drake_ros_core::DrakeRos; TEST(DrakeRos, default_construct) { - EXPECT_NO_THROW(std::make_unique()); + 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 new file mode 100644 index 00000000..45156a1e --- /dev/null +++ b/drake_ros_core/test/test_pub_sub.cc @@ -0,0 +1,108 @@ +// 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 + +#include +#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" + +using drake_ros_core::DrakeRos; +using drake_ros_core::RosInterfaceSystem; +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; + const auto qos = rclcpp::QoS{rclcpp::KeepLast(10)}.reliable(); + + 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())); + auto system_pub_out = + builder.AddSystem(RosPublisherSystem::Make( + "out", qos, system_ros->get_ros_interface(), + {drake::systems::TriggerType::kPeriodic}, kPublishPeriod)); + + builder.Connect(system_sub_in->get_output_port(0), + system_pub_out->get_input_port(0)); + + auto diagram = builder.Build(); + auto context = diagram->CreateDefaultContext(); + + auto simulator = std::make_unique>( + *diagram, std::move(context)); + simulator->set_target_realtime_rate(1.0); + simulator->Initialize(); + + auto& simulator_context = simulator->get_mutable_context(); + + // Don't need to rclcpp::init because DrakeRos uses global rclcpp::Context by + // default + auto direct_ros_node = rclcpp::Node::make_shared("sub_to_pub"); + + // Create publisher talking to subscriber system. + auto direct_pub_in = + direct_ros_node->create_publisher("in", qos); + + // Create subscription listening to publisher system + 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 = + 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) { + 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(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(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); + } + + drake_ros_core::shutdown(); +} diff --git a/drake_ros_core/test/test_pub_sub.cpp b/drake_ros_core/test/test_pub_sub.cpp deleted file mode 100644 index 0e849868..00000000 --- a/drake_ros_core/test/test_pub_sub.cpp +++ /dev/null @@ -1,96 +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 -#include -#include - -#include -#include -#include -#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" - -using drake_ros_core::DrakeRos; -using drake_ros_core::RosInterfaceSystem; -using drake_ros_core::RosPublisherSystem; -using drake_ros_core::RosSubscriberSystem; - -TEST(Integration, sub_to_pub) { - drake::systems::DiagramBuilder builder; - - const size_t num_msgs = 5; - - rclcpp::QoS qos{rclcpp::KeepLast(num_msgs)}; - qos.transient_local().reliable(); - - auto sys_ros_interface = - builder.AddSystem(std::make_unique()); - auto sys_sub = - builder.AddSystem(RosSubscriberSystem::Make( - "in", qos, sys_ros_interface->get_ros_interface())); - auto sys_pub = - builder.AddSystem(RosPublisherSystem::Make( - "out", qos, sys_ros_interface->get_ros_interface())); - - builder.Connect(sys_sub->get_output_port(0), sys_pub->get_input_port(0)); - - auto diagram = builder.Build(); - auto context = diagram->CreateDefaultContext(); - - auto simulator = std::make_unique>( - *diagram, std::move(context)); - simulator->set_target_realtime_rate(1.0); - simulator->Initialize(); - - auto& simulator_context = simulator->get_mutable_context(); - - // Don't need to rclcpp::init because DrakeRos uses global rclcpp::Context by - // default - auto node = rclcpp::Node::make_shared("sub_to_pub"); - - // Create publisher talking to subscriber system. - auto publisher = - node->create_publisher("in", qos); - - // Create subscription listening to publisher system - std::vector> rx_msgs; - auto rx_callback = [&](std::unique_ptr msg) { - rx_msgs.push_back(std::move(msg)); - }; - auto subscription = node->create_subscription( - "out", qos, rx_callback); - - // Send messages into the drake system - for (size_t p = 0; p < num_msgs; ++p) { - publisher->publish(std::make_unique()); - } - - const int timeout_sec = 5; - const int spins_per_sec = 10; - const float time_delta = 1.0f / spins_per_sec; - for (int i = 0; i < timeout_sec * spins_per_sec && rx_msgs.size() < num_msgs; - ++i) { - rclcpp::spin_some(node); - simulator->AdvanceTo(simulator_context.get_time() + time_delta); - } - - // Make sure same number of messages got out - ASSERT_EQ(num_msgs, rx_msgs.size()); -} 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..a9ab9bde --- /dev/null +++ b/drake_ros_core/test/test_pub_sub.py @@ -0,0 +1,101 @@ +# 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 +from pydrake.systems.framework import TriggerType + +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 + +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( + RosInterfaceSystem('pub_to_sub_py')) + + publish_period = 1.0 + + qos = QoSProfile( + depth=10, + history=HistoryPolicy.KEEP_LAST, + reliability=ReliabilityPolicy.RELIABLE) + + system_pub_out = builder.AddSystem(RosPublisherSystem.Make( + BasicTypes, 'out_py', qos, system_ros.get_ros_interface(), + {TriggerType.kPeriodic}, publish_period)) + + system_sub_in = builder.AddSystem(RosSubscriberSystem.Make( + BasicTypes, 'in_py', qos, system_ros.get_ros_interface())) + + builder.Connect( + system_sub_in.get_output_port(0), + system_pub_out.get_input_port(0)) + diagram = builder.Build() + simulator = Simulator(diagram) + simulator.set_target_realtime_rate(1.0) + simulator.Initialize() + + simulator_context = simulator.get_mutable_context() + + rclpy.init() + direct_ros_node = rclpy.create_node('sub_to_pub_py') + + # Create publisher talking to subscriber system. + 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 = direct_ros_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(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(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 + + drake_ros_core.shutdown()