From 25b6b2976ebe83ffb63ce95967793cbe3e8d2003 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Mon, 9 Nov 2020 17:03:21 -0800 Subject: [PATCH 01/39] Stub code builds Signed-off-by: Shane Loretz --- drake_ros_systems/CMakeLists.txt | 79 +++++++++++++++++++ drake_ros_systems/README.md | 62 +++++++++++++++ .../include/drake_ros_systems/drake_ros.hpp | 48 +++++++++++ .../drake_ros_systems/drake_ros_interface.hpp | 43 ++++++++++ .../ros_interface_system.hpp | 36 +++++++++ .../ros_publisher_system.hpp | 69 ++++++++++++++++ .../ros_subscriber_system.hpp | 49 ++++++++++++ drake_ros_systems/package.xml | 16 ++++ drake_ros_systems/src/drake_ros.cpp | 0 drake_ros_systems/src/publisher.cpp | 37 +++++++++ drake_ros_systems/src/publisher.hpp | 30 +++++++ .../src/ros_interface_system.cpp | 0 .../src/ros_publisher_system.cpp | 0 .../src/ros_subscriber_system.cpp | 0 drake_ros_systems/src/subscription.cpp | 28 +++++++ drake_ros_systems/src/subscription.hpp | 33 ++++++++ 16 files changed, 530 insertions(+) create mode 100644 drake_ros_systems/CMakeLists.txt create mode 100644 drake_ros_systems/README.md create mode 100644 drake_ros_systems/include/drake_ros_systems/drake_ros.hpp create mode 100644 drake_ros_systems/include/drake_ros_systems/drake_ros_interface.hpp create mode 100644 drake_ros_systems/include/drake_ros_systems/ros_interface_system.hpp create mode 100644 drake_ros_systems/include/drake_ros_systems/ros_publisher_system.hpp create mode 100644 drake_ros_systems/include/drake_ros_systems/ros_subscriber_system.hpp create mode 100644 drake_ros_systems/package.xml create mode 100644 drake_ros_systems/src/drake_ros.cpp create mode 100644 drake_ros_systems/src/publisher.cpp create mode 100644 drake_ros_systems/src/publisher.hpp create mode 100644 drake_ros_systems/src/ros_interface_system.cpp create mode 100644 drake_ros_systems/src/ros_publisher_system.cpp create mode 100644 drake_ros_systems/src/ros_subscriber_system.cpp create mode 100644 drake_ros_systems/src/subscription.cpp create mode 100644 drake_ros_systems/src/subscription.hpp diff --git a/drake_ros_systems/CMakeLists.txt b/drake_ros_systems/CMakeLists.txt new file mode 100644 index 0000000..681dcbb --- /dev/null +++ b/drake_ros_systems/CMakeLists.txt @@ -0,0 +1,79 @@ +cmake_minimum_required(VERSION 3.10) +project(drake_ros_systems) + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic -Werror) +endif() + +find_package(ament_cmake_ros REQUIRED) +find_package(drake REQUIRED) +# find_package(pybind11_vendor) +# find_package(pybind11 REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rosidl_runtime_c REQUIRED) +find_package(rosidl_typesupport_cpp REQUIRED) + +add_library(drake_ros_systems + src/drake_ros.cpp + src/publisher.cpp + src/ros_interface_system.cpp + src/ros_publisher_system.cpp + src/ros_subscriber_system.cpp + src/subscription.cpp +) +target_link_libraries(drake_ros_systems PUBLIC + drake::drake + rclcpp::rclcpp + rosidl_runtime_c::rosidl_runtime_c + rosidl_typesupport_cpp::rosidl_typesupport_cpp +) +target_include_directories(drake_ros_systems + PUBLIC + "$" + "$" + "$" +) + +ament_export_dependencies(drake) +ament_export_dependencies(rclcpp) +ament_export_dependencies(rosidl_generator_c) + +install(TARGETS ${PROJECT_NAME} EXPORT ${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install( + DIRECTORY include/ + DESTINATION include +) + +# pybind11_add_module(rccl SHARED +# use_ros.cpp +# ) +# target_link_libraries(rccl PRIVATE +# rclcpp::rclcpp +# ${std_msgs_TARGETS} +# ) +# +# # Sets PYTHON_INSTALL_DIR +# _ament_cmake_python_get_python_install_dir() +# +# install( +# TARGETS rccl +# DESTINATION "${PYTHON_INSTALL_DIR}" +# ) + +ament_package() + diff --git a/drake_ros_systems/README.md b/drake_ros_systems/README.md new file mode 100644 index 0000000..3637476 --- /dev/null +++ b/drake_ros_systems/README.md @@ -0,0 +1,62 @@ +# Drake ROS Systems + +This is a ROS 2 prototype of a solution to [robotlocomotion/Drake#9500](https://github.com/RobotLocomotion/drake/issues/9500). +It is similar to this ROS 1 prototype [`gizatt/drake_ros_systems`](https://github.com/gizatt/drake_ros_systems). +It explores ROS 2 equivalents of `LcmInterfaceSystem`, `LcmPublisherSystem`, and `LcmSubscriberSystem`. + +# Code examples + +Create a system that publishes `std_msgs::msg::String`. + +```C++ +#include +#include + +using drake_ros_systems::RosInterfaceSystem; +using drake_ros_systems::RosPublisherSystem; + +// This system initializes ROS and calls spin() +// It creates a single ROS node called "drake_ros". +auto spin_system = RosInterfaceSystem::Make(/* optionally rclcpp init options? */); + +const std::string topic{"chatter"}; +const double period_sec{0.1}; // 10Hz + +auto pub_system = RosPublisherSystem::Make( + spin_system.get_ros_interface(), topic, period_sec); + +auto pub_context = pub_system->CreateDefaultContext(); +std_msgs::msg::String pub_msg; +pub_msg.data = "Hello from Drake"; +pub_context->FixInputPort(0, AbstractValue::Make(pub_msg)); +pub_system->Publish(*pub_context); +``` + +Create a system that subscribes to `std_msgs::msg::String`. + +```C++ +#include +#include + +using drake_ros_systems::RosInterfaceSystem; +using drake_ros_systems::RosSubscriberSystem; + +// This system initializes ROS and calls spin() +// It creates a single ROS node called "drake_ros". +auto spin_system = RosInterfaceSystem(/* optionally rclcpp init options? */); + +const std::string topic{"chatter"}; + +auto pub_system = RosSubscriberSystem::Make( + spin_system.get_ros_interface(), topic, period_sec); + +auto sub_context = sub_system->CreateDefaultContext(); +// somehow this sub context is added to a diagram builder with the system +// so the subscriber can update that message + +// huh...? +std_msgs::msg::String sub_msg = sub_context->get_output_stuff(...); +``` + +Could use an example of drake systems built with a diagram builder and connected to input/output ports of other systems. + diff --git a/drake_ros_systems/include/drake_ros_systems/drake_ros.hpp b/drake_ros_systems/include/drake_ros_systems/drake_ros.hpp new file mode 100644 index 0000000..9ef8b4a --- /dev/null +++ b/drake_ros_systems/include/drake_ros_systems/drake_ros.hpp @@ -0,0 +1,48 @@ +#ifndef DRAKE_ROS_SYSTEMS__DRAKE_ROS_HPP_ +#define DRAKE_ROS_SYSTEMS__DRAKE_ROS_HPP_ + +#include +#include + +#include +#include + +#include + +namespace drake_ros_systems +{ +/// PIMPL forward declaration +class DrakeRosPrivate; + +/// System that abstracts working with ROS +class DrakeRos final: public DrakeRosInterface +{ +public: + DrakeRos(); + + virtual ~DrakeRos(); + + std::unique_ptr + create_publisher( + const rosidl_message_type_support_t & ts, + const std::string & topic_name, + const rclcpp::QoS & qos) final; + + virtual + std::unique_ptr + create_subscription( + const rosidl_message_type_support_t & ts, + const std::string & topic_name, + const rclcpp::QoS & qos + std::function)> callback) final; + + void + spin( + int timeout_millis) final; + +private: + std::unique_ptr impl_; +}; +} // namespace drake_ros_systems +#endif // DRAKE_ROS_SYSTEMS__DRAKE_ROS_HPP_ + diff --git a/drake_ros_systems/include/drake_ros_systems/drake_ros_interface.hpp b/drake_ros_systems/include/drake_ros_systems/drake_ros_interface.hpp new file mode 100644 index 0000000..88bb0d5 --- /dev/null +++ b/drake_ros_systems/include/drake_ros_systems/drake_ros_interface.hpp @@ -0,0 +1,43 @@ +#ifndef DRAKE_ROS_SYSTEMS__DRAKE_ROS_INTERFACE_HPP_ +#define DRAKE_ROS_SYSTEMS__DRAKE_ROS_INTERFACE_HPP_ + +#include +#include + +#include +#include + +#include + +namespace drake_ros_systems +{ +// Forward declarations for non-public-API classes +class Publisher; +class Subscription; + +/// System that abstracts working with ROS +class DrakeRosInterface +{ +public: + virtual + std::unique_ptr + create_publisher( + const rosidl_message_type_support_t & ts, + const std::string & topic_name, + const rclcpp::QoS & qos) = 0; + + virtual + std::unique_ptr + create_subscription( + const rosidl_message_type_support_t & ts, + const std::string & topic_name, + const rclcpp::QoS & qos + std::function)> callback) = 0; + + virtual + void + spin( + int timeout_millis) = 0; +}; +} // namespace drake_ros_systems +#endif // DRAKE_ROS_SYSTEMS__DRAKE_ROS_INTERFACE_HPP_ diff --git a/drake_ros_systems/include/drake_ros_systems/ros_interface_system.hpp b/drake_ros_systems/include/drake_ros_systems/ros_interface_system.hpp new file mode 100644 index 0000000..b0d0fd7 --- /dev/null +++ b/drake_ros_systems/include/drake_ros_systems/ros_interface_system.hpp @@ -0,0 +1,36 @@ +#ifndef DRAKE_ROS_SYSTEMS__ROS_INTERFACE_SYSTEM_HPP_ +#define DRAKE_ROS_SYSTEMS__ROS_INTERFACE_SYSTEM_HPP_ + +#include + +#include + +#include + +namespace drake_ros_systems +{ +// PIMPL forward declaration +class RosInterfaceSystemPrivate; + +/// System that takes care of calling spin() in Drake's systems framework +class RosInterfaceSystem: public LeafSystem +{ +public: + RosInterfaceSystem(); + virtual ~RosInterfaceSystem(); + + /// Return a handle for interacting with ROS + std::shared_ptr + get_ros_interface() const; + +protected: + /// Override as a place to call rclcpp::spin() + void DoCalcNextUpdateTime( + const Context&, + systems::CompositeEventCollection*, + double*) const override; + + std::unique_ptr impl_; +}; +} // namespace drake_ros_systems +#endif // DRAKE_ROS_SYSTEMS__ROS_INTERFACE_SYSTEM_HPP_ diff --git a/drake_ros_systems/include/drake_ros_systems/ros_publisher_system.hpp b/drake_ros_systems/include/drake_ros_systems/ros_publisher_system.hpp new file mode 100644 index 0000000..0a5d045 --- /dev/null +++ b/drake_ros_systems/include/drake_ros_systems/ros_publisher_system.hpp @@ -0,0 +1,69 @@ +#ifndef DRAKE_ROS_SYSTEMS__ROS_PUBLISHER_SYSTEM_HPP_ +#define DRAKE_ROS_SYSTEMS__ROS_PUBLISHER_SYSTEM_HPP_ + +#include + +#include +#include + +#include + +#include +#include + +namespace drake_ros_systems +{ +/// PIMPL forward declaration +class RosPublisherSystemPrivate; + +/// System that subscribes to a ROS topic and makes it available on an output port +class RosPublisherSystem: public LeafSystem +{ +public: + + /// Convenience method to make a subscriber system given a ROS message type + template + std::unique_ptr + Make(std::shared_ptr ros_interface) + { + // Assume C++ typesupport since this is a C++ template function + return std::make_shared( + rosidl_typesupport_cpp::get_message_type_support_handle(), + ros_interface); + } + + RosPublisherSystem( + const rosidl_message_type_support_t & ts, + std::shared_ptr ros_interface); + + virtual ~RosPublisherSystem(); + + /// Publish a ROS message + template + void + publish(const MessageT & message) + { + rclcpp::SerializedMessage serialized_msg; + const auto ret = rmw_serialize( + &message, + rosidl_typesupport_cpp::get_message_type_support_handle(), + &serialized_msg->get_rcl_serialized_message()); + // TODO(sloretz) throw if failed to serialize + publish(serialized_msg); + } + + /// Publish a serialized ROS message + void + publish(const rclcpp::SerializedMessage & serialized_msg); + +protected: + /// Override as a place to schedule event to move ROS message into a context + void DoCalcNextUpdateTime( + const Context&, + systems::CompositeEventCollection*, + double*) const override; + + std::unique_ptr impl_; +}; +} // namespace drake_ros_systems +#endif // DRAKE_ROS_SYSTEMS__ROS_INTERFACE_SYSTEM_HPP_ diff --git a/drake_ros_systems/include/drake_ros_systems/ros_subscriber_system.hpp b/drake_ros_systems/include/drake_ros_systems/ros_subscriber_system.hpp new file mode 100644 index 0000000..242e8ce --- /dev/null +++ b/drake_ros_systems/include/drake_ros_systems/ros_subscriber_system.hpp @@ -0,0 +1,49 @@ +#ifndef DRAKE_ROS_SYSTEMS__ROS_SUBSCRIBER_SYSTEM_HPP_ +#define DRAKE_ROS_SYSTEMS__ROS_SUBSCRIBER_SYSTEM_HPP_ + +#include + +#include + +#include + +#include + +namespace drake_ros_systems +{ +// PIMPL forward declaration +class RosSubscriberSystemPrivate; + +/// System that subscribes to a ROS topic and makes it available on an output port +class RosSubscriberSystem: public LeafSystem +{ +public: + + /// Convenience method to make a subscriber system given a ROS message type + template + std::unique_ptr + Make(std::shared_ptr ros_interface) + { + // Assume C++ typesupport since this is a C++ template function + return std::make_shared( + rosidl_typesupport_cpp::get_message_type_support_handle(), + ros_interface); + } + + RosSubscriberSystem( + const rosidl_message_type_support_t & ts, + 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 Context&, + systems::CompositeEventCollection*, + double*) const override; + + std::unique_ptr impl_; +}; +} // namespace drake_ros_systems +#endif // DRAKE_ROS_SYSTEMS__ROS_INTERFACE_SYSTEM_HPP_ diff --git a/drake_ros_systems/package.xml b/drake_ros_systems/package.xml new file mode 100644 index 0000000..a7d9593 --- /dev/null +++ b/drake_ros_systems/package.xml @@ -0,0 +1,16 @@ + + + drake_ros_systems + 0.1.0 + Drake systems for interacting with ROS 2. + Shane Loretz + Apache License 2.0 + + rclcpp + rosidl_runtime_c + rosidl_typesupport_cpp + + + cmake + + diff --git a/drake_ros_systems/src/drake_ros.cpp b/drake_ros_systems/src/drake_ros.cpp new file mode 100644 index 0000000..e69de29 diff --git a/drake_ros_systems/src/publisher.cpp b/drake_ros_systems/src/publisher.cpp new file mode 100644 index 0000000..1a67a6e --- /dev/null +++ b/drake_ros_systems/src/publisher.cpp @@ -0,0 +1,37 @@ +#include "publisher.hpp" + +namespace drake_ros_systems +{ +// Copied from rosbag2_transport rosbag2_get_publisher_options +rcl_publisher_options_t publisher_options(const rclcpp::QoS & qos) +{ + auto options = rcl_publisher_get_default_options(); + options.qos = qos.get_rmw_qos_profile(); + return options; +} + +Publisher::Publisher( + rclcpp::node_interfaces::NodeBaseInterface * node_base, + const rosidl_message_type_support_t & type_support, + const std::string & topic_name, + const rclcpp::QoS & qos) +: rclcpp::PublisherBase(node_base, topic_name, type_support, publisher_options(qos)) +{ +} + +Publisher::~Publisher() +{ +} + +void +Publisher::publish(const rclcpp::SerializedMessage & serialized_msg) +{ + // TODO(sloretz) Copied from rosbag2_transport GenericPublisher, can it be upstreamed to rclcpp? + auto return_code = rcl_publish_serialized_message( + get_publisher_handle().get(), &serialized_msg.get_rcl_serialized_message(), NULL); + + if (return_code != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error(return_code, "failed to publish serialized message"); + } +} +} // namespace drake_ros_systems diff --git a/drake_ros_systems/src/publisher.hpp b/drake_ros_systems/src/publisher.hpp new file mode 100644 index 0000000..366d47f --- /dev/null +++ b/drake_ros_systems/src/publisher.hpp @@ -0,0 +1,30 @@ +#ifndef DRAKE_ROS_SYSTEMS__PUBLISHER_HPP_ +#define DRAKE_ROS_SYSTEMS__PUBLISHER_HPP_ + +#include + +#include +#include + +#include +#include +#include + +namespace drake_ros_systems +{ +class Publisher final : public rclcpp::PublisherBase +{ +public: + Publisher( + rclcpp::node_interfaces::NodeBaseInterface * node_base, + const rosidl_message_type_support_t & ts, + const std::string & topic_name, + const rclcpp::QoS & qos); + + ~Publisher(); + + void + publish(const rclcpp::SerializedMessage & serialized_msg); +}; +} // namespace drake_ros_systems +#endif // DRAKE_ROS_SYSTEMS__PUBLISHER_HPP_ diff --git a/drake_ros_systems/src/ros_interface_system.cpp b/drake_ros_systems/src/ros_interface_system.cpp new file mode 100644 index 0000000..e69de29 diff --git a/drake_ros_systems/src/ros_publisher_system.cpp b/drake_ros_systems/src/ros_publisher_system.cpp new file mode 100644 index 0000000..e69de29 diff --git a/drake_ros_systems/src/ros_subscriber_system.cpp b/drake_ros_systems/src/ros_subscriber_system.cpp new file mode 100644 index 0000000..e69de29 diff --git a/drake_ros_systems/src/subscription.cpp b/drake_ros_systems/src/subscription.cpp new file mode 100644 index 0000000..17078f4 --- /dev/null +++ b/drake_ros_systems/src/subscription.cpp @@ -0,0 +1,28 @@ +#include "subscription.hpp" + +namespace drake_ros_systems +{ +// Copied from rosbag2_transport rosbag2_get_subscription_options +rcl_subscription_options_t subscription_options(const rclcpp::QoS & qos) +{ + auto options = rcl_subscription_get_default_options(); + options.qos = qos.get_rmw_qos_profile(); + return options; +} + +Subscription::Subscription( + rclcpp::node_interfaces::NodeBaseInterface * node_base, + const rosidl_message_type_support_t & ts, + const std::string & topic_name, + const rclcpp::QoS & qos, + std::function)> callback) +: rclcpp::SubscriptionBase(node_base, ts, topic_name, subscription_options(qos), true), + callback_(callback) +{ +} + +Subscription::~Subscription() +{ +} +} // namespace drake_ros_systems + diff --git a/drake_ros_systems/src/subscription.hpp b/drake_ros_systems/src/subscription.hpp new file mode 100644 index 0000000..6fd0ce5 --- /dev/null +++ b/drake_ros_systems/src/subscription.hpp @@ -0,0 +1,33 @@ +#ifndef DRAKE_ROS_SYSTEMS__SUBSCRIPTION_HPP_ +#define DRAKE_ROS_SYSTEMS__SUBSCRIPTION_HPP_ + +#include +#include + +#include +#include + +#include +#include + +namespace drake_ros_systems +{ +class Subscription final : public rclcpp::SubscriptionBase +{ +public: + Subscription( + rclcpp::node_interfaces::NodeBaseInterface * node_base, + const rosidl_message_type_support_t & ts, + const std::string & topic_name, + const rclcpp::QoS & qos, + std::function)> callback); + + ~Subscription(); + +private: + std::function)> callback_; +}; +} // namespace drake_ros_systems +#endif // DRAKE_ROS_SYSTEMS__SUBSCRIPTION_HPP_ + + From beb8c7dcba0eeb8dfc9274e45dd2c15ed749b79a Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Tue, 10 Nov 2020 13:49:59 -0800 Subject: [PATCH 02/39] Misc fixes to get it to build (but not link) with stub example Signed-off-by: Shane Loretz --- .../include/drake_ros_systems/drake_ros.hpp | 5 ++++- .../drake_ros_systems/drake_ros_interface.hpp | 3 ++- .../drake_ros_systems/ros_interface_system.hpp | 6 +++--- .../drake_ros_systems/ros_publisher_system.hpp | 15 ++++++++------- .../drake_ros_systems/ros_subscriber_system.hpp | 11 ++++++----- 5 files changed, 23 insertions(+), 17 deletions(-) diff --git a/drake_ros_systems/include/drake_ros_systems/drake_ros.hpp b/drake_ros_systems/include/drake_ros_systems/drake_ros.hpp index 9ef8b4a..39926c7 100644 --- a/drake_ros_systems/include/drake_ros_systems/drake_ros.hpp +++ b/drake_ros_systems/include/drake_ros_systems/drake_ros.hpp @@ -4,6 +4,9 @@ #include #include +#include + +#include #include #include @@ -33,7 +36,7 @@ class DrakeRos final: public DrakeRosInterface create_subscription( const rosidl_message_type_support_t & ts, const std::string & topic_name, - const rclcpp::QoS & qos + const rclcpp::QoS & qos, std::function)> callback) final; void diff --git a/drake_ros_systems/include/drake_ros_systems/drake_ros_interface.hpp b/drake_ros_systems/include/drake_ros_systems/drake_ros_interface.hpp index 88bb0d5..9daccd5 100644 --- a/drake_ros_systems/include/drake_ros_systems/drake_ros_interface.hpp +++ b/drake_ros_systems/include/drake_ros_systems/drake_ros_interface.hpp @@ -4,6 +4,7 @@ #include #include +#include #include #include @@ -31,7 +32,7 @@ class DrakeRosInterface create_subscription( const rosidl_message_type_support_t & ts, const std::string & topic_name, - const rclcpp::QoS & qos + const rclcpp::QoS & qos, std::function)> callback) = 0; virtual diff --git a/drake_ros_systems/include/drake_ros_systems/ros_interface_system.hpp b/drake_ros_systems/include/drake_ros_systems/ros_interface_system.hpp index b0d0fd7..0d5c558 100644 --- a/drake_ros_systems/include/drake_ros_systems/ros_interface_system.hpp +++ b/drake_ros_systems/include/drake_ros_systems/ros_interface_system.hpp @@ -13,7 +13,7 @@ namespace drake_ros_systems class RosInterfaceSystemPrivate; /// System that takes care of calling spin() in Drake's systems framework -class RosInterfaceSystem: public LeafSystem +class RosInterfaceSystem : public drake::systems::LeafSystem { public: RosInterfaceSystem(); @@ -26,8 +26,8 @@ class RosInterfaceSystem: public LeafSystem protected: /// Override as a place to call rclcpp::spin() void DoCalcNextUpdateTime( - const Context&, - systems::CompositeEventCollection*, + const drake::systems::Context&, + drake::systems::CompositeEventCollection*, double*) const override; std::unique_ptr impl_; diff --git a/drake_ros_systems/include/drake_ros_systems/ros_publisher_system.hpp b/drake_ros_systems/include/drake_ros_systems/ros_publisher_system.hpp index 0a5d045..6cbeb35 100644 --- a/drake_ros_systems/include/drake_ros_systems/ros_publisher_system.hpp +++ b/drake_ros_systems/include/drake_ros_systems/ros_publisher_system.hpp @@ -17,17 +17,18 @@ namespace drake_ros_systems class RosPublisherSystemPrivate; /// System that subscribes to a ROS topic and makes it available on an output port -class RosPublisherSystem: public LeafSystem +class RosPublisherSystem: public drake::systems::LeafSystem { public: /// Convenience method to make a subscriber system given a ROS message type template + static std::unique_ptr Make(std::shared_ptr ros_interface) { // Assume C++ typesupport since this is a C++ template function - return std::make_shared( + return std::make_unique( rosidl_typesupport_cpp::get_message_type_support_handle(), ros_interface); } @@ -47,7 +48,7 @@ class RosPublisherSystem: public LeafSystem const auto ret = rmw_serialize( &message, rosidl_typesupport_cpp::get_message_type_support_handle(), - &serialized_msg->get_rcl_serialized_message()); + &serialized_msg.get_rcl_serialized_message()); // TODO(sloretz) throw if failed to serialize publish(serialized_msg); } @@ -58,10 +59,10 @@ class RosPublisherSystem: public LeafSystem protected: /// Override as a place to schedule event to move ROS message into a context - void DoCalcNextUpdateTime( - const Context&, - systems::CompositeEventCollection*, - double*) const override; + // void DoCalcNextUpdateTime( + // const Context&, + // systems::CompositeEventCollection*, + // double*) const override; std::unique_ptr impl_; }; diff --git a/drake_ros_systems/include/drake_ros_systems/ros_subscriber_system.hpp b/drake_ros_systems/include/drake_ros_systems/ros_subscriber_system.hpp index 242e8ce..df7720f 100644 --- a/drake_ros_systems/include/drake_ros_systems/ros_subscriber_system.hpp +++ b/drake_ros_systems/include/drake_ros_systems/ros_subscriber_system.hpp @@ -15,23 +15,24 @@ namespace drake_ros_systems class RosSubscriberSystemPrivate; /// System that subscribes to a ROS topic and makes it available on an output port -class RosSubscriberSystem: public LeafSystem +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(std::shared_ptr ros_interface) { // Assume C++ typesupport since this is a C++ template function - return std::make_shared( + return std::make_unique( rosidl_typesupport_cpp::get_message_type_support_handle(), ros_interface); } RosSubscriberSystem( - const rosidl_message_type_support_t & ts, + const rosidl_message_type_support_t * ts, std::shared_ptr ros_interface); virtual ~RosSubscriberSystem(); @@ -39,8 +40,8 @@ class RosSubscriberSystem: public LeafSystem protected: /// Override as a place to schedule event to move ROS message into a context void DoCalcNextUpdateTime( - const Context&, - systems::CompositeEventCollection*, + const drake::systems::Context&, + drake::systems::CompositeEventCollection*, double*) const override; std::unique_ptr impl_; From 3c7dd28b0085e6bfacb00c92f738614cd6aad48a Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Tue, 10 Nov 2020 13:53:02 -0800 Subject: [PATCH 03/39] Fix comments on publisher system Signed-off-by: Shane Loretz --- .../drake_ros_systems/ros_publisher_system.hpp | 11 ++--------- 1 file changed, 2 insertions(+), 9 deletions(-) diff --git a/drake_ros_systems/include/drake_ros_systems/ros_publisher_system.hpp b/drake_ros_systems/include/drake_ros_systems/ros_publisher_system.hpp index 6cbeb35..393eb9b 100644 --- a/drake_ros_systems/include/drake_ros_systems/ros_publisher_system.hpp +++ b/drake_ros_systems/include/drake_ros_systems/ros_publisher_system.hpp @@ -16,12 +16,11 @@ namespace drake_ros_systems /// PIMPL forward declaration class RosPublisherSystemPrivate; -/// System that subscribes to a ROS topic and makes it available on an output port +/// 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 subscriber system given a ROS message type + /// Convenience method to make a publisher system given a ROS message type template static std::unique_ptr @@ -58,12 +57,6 @@ class RosPublisherSystem: public drake::systems::LeafSystem publish(const rclcpp::SerializedMessage & serialized_msg); protected: - /// Override as a place to schedule event to move ROS message into a context - // void DoCalcNextUpdateTime( - // const Context&, - // systems::CompositeEventCollection*, - // double*) const override; - std::unique_ptr impl_; }; } // namespace drake_ros_systems From 84b91a36921d8ae069abdc81a5e6488b56652c20 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Tue, 10 Nov 2020 15:38:29 -0800 Subject: [PATCH 04/39] ROS interface implementation fleshed out Signed-off-by: Shane Loretz --- .../include/drake_ros_systems/drake_ros.hpp | 9 +-- .../ros_publisher_system.hpp | 2 +- .../ros_subscriber_system.hpp | 4 +- drake_ros_systems/src/drake_ros.cpp | 71 +++++++++++++++++++ drake_ros_systems/src/subscription.cpp | 46 +++++++++++- drake_ros_systems/src/subscription.hpp | 39 +++++++++- 6 files changed, 161 insertions(+), 10 deletions(-) diff --git a/drake_ros_systems/include/drake_ros_systems/drake_ros.hpp b/drake_ros_systems/include/drake_ros_systems/drake_ros.hpp index 39926c7..f3b3b61 100644 --- a/drake_ros_systems/include/drake_ros_systems/drake_ros.hpp +++ b/drake_ros_systems/include/drake_ros_systems/drake_ros.hpp @@ -1,7 +1,6 @@ #ifndef DRAKE_ROS_SYSTEMS__DRAKE_ROS_HPP_ #define DRAKE_ROS_SYSTEMS__DRAKE_ROS_HPP_ -#include #include #include @@ -11,11 +10,14 @@ #include #include +#include namespace drake_ros_systems { -/// PIMPL forward declaration +/// PIMPL forward declarations class DrakeRosPrivate; +class Publisher; +class Subscription; /// System that abstracts working with ROS class DrakeRos final: public DrakeRosInterface @@ -37,7 +39,7 @@ class DrakeRos final: public DrakeRosInterface const rosidl_message_type_support_t & ts, const std::string & topic_name, const rclcpp::QoS & qos, - std::function)> callback) final; + std::function)> callback) final; void spin( @@ -48,4 +50,3 @@ class DrakeRos final: public DrakeRosInterface }; } // namespace drake_ros_systems #endif // DRAKE_ROS_SYSTEMS__DRAKE_ROS_HPP_ - diff --git a/drake_ros_systems/include/drake_ros_systems/ros_publisher_system.hpp b/drake_ros_systems/include/drake_ros_systems/ros_publisher_system.hpp index 393eb9b..da39611 100644 --- a/drake_ros_systems/include/drake_ros_systems/ros_publisher_system.hpp +++ b/drake_ros_systems/include/drake_ros_systems/ros_publisher_system.hpp @@ -28,7 +28,7 @@ class RosPublisherSystem: public drake::systems::LeafSystem { // Assume C++ typesupport since this is a C++ template function return std::make_unique( - rosidl_typesupport_cpp::get_message_type_support_handle(), + *rosidl_typesupport_cpp::get_message_type_support_handle(), ros_interface); } diff --git a/drake_ros_systems/include/drake_ros_systems/ros_subscriber_system.hpp b/drake_ros_systems/include/drake_ros_systems/ros_subscriber_system.hpp index df7720f..416b19f 100644 --- a/drake_ros_systems/include/drake_ros_systems/ros_subscriber_system.hpp +++ b/drake_ros_systems/include/drake_ros_systems/ros_subscriber_system.hpp @@ -27,12 +27,12 @@ class RosSubscriberSystem : public drake::systems::LeafSystem { // Assume C++ typesupport since this is a C++ template function return std::make_unique( - rosidl_typesupport_cpp::get_message_type_support_handle(), + *rosidl_typesupport_cpp::get_message_type_support_handle(), ros_interface); } RosSubscriberSystem( - const rosidl_message_type_support_t * ts, + const rosidl_message_type_support_t & ts, std::shared_ptr ros_interface); virtual ~RosSubscriberSystem(); diff --git a/drake_ros_systems/src/drake_ros.cpp b/drake_ros_systems/src/drake_ros.cpp index e69de29..6486258 100644 --- a/drake_ros_systems/src/drake_ros.cpp +++ b/drake_ros_systems/src/drake_ros.cpp @@ -0,0 +1,71 @@ +#include +#include "publisher.hpp" +#include "subscription.hpp" + +#include +#include + +namespace drake_ros_systems +{ +class DrakeRosPrivate +{ +public: + rclcpp::Context::SharedPtr context_; + rclcpp::Node::UniquePtr node_; + rclcpp::executors::SingleThreadedExecutor::UniquePtr executor_; +}; + +DrakeRos::DrakeRos() + : impl_(new DrakeRosPrivate()) +{ + impl_->context_ = std::make_shared(); + + // TODO(sloretz) allow passing args and init options in constructor + impl_->context_->init(0, nullptr); + + // TODO(sloretz) allow passing in node name and node options + rclcpp::NodeOptions no; + no.context(impl_->context_); + impl_->node_.reset(new rclcpp::Node("DrakeRosSystems", no)); + + // 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() +{ + 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::unique_ptr( + new Publisher(impl_->node_->get_node_base_interface().get(), ts, topic_name, qos)); +} + +std::unique_ptr +DrakeRos::create_subscription( + const rosidl_message_type_support_t & ts, + const std::string & topic_name, + const rclcpp::QoS & qos, + std::function)> callback) +{ + return std::unique_ptr( + new Subscription(impl_->node_->get_node_base_interface().get(), ts, topic_name, qos, callback)); +} + +void +DrakeRos::spin( + int timeout_millis) +{ + impl_->executor_->spin_some(std::chrono::milliseconds(timeout_millis)); +} +} // namespace drake_ros_systems diff --git a/drake_ros_systems/src/subscription.cpp b/drake_ros_systems/src/subscription.cpp index 17078f4..e70d778 100644 --- a/drake_ros_systems/src/subscription.cpp +++ b/drake_ros_systems/src/subscription.cpp @@ -15,7 +15,7 @@ Subscription::Subscription( const rosidl_message_type_support_t & ts, const std::string & topic_name, const rclcpp::QoS & qos, - std::function)> callback) + std::function)> callback) : rclcpp::SubscriptionBase(node_base, ts, topic_name, subscription_options(qos), true), callback_(callback) { @@ -24,5 +24,49 @@ Subscription::Subscription( Subscription::~Subscription() { } + +std::shared_ptr +Subscription::create_message() +{ + // Subscriber only does serialized messages + return create_serialized_message(); +} + +std::shared_ptr +Subscription::create_serialized_message() +{ + return std::make_shared(); +} + +void +Subscription::handle_message( + std::shared_ptr & message, + const rclcpp::MessageInfo & message_info) +{ + (void) message_info; + callback_(std::static_pointer_cast(message)); +} + +void +Subscription::handle_loaned_message( + void * loaned_message, const rclcpp::MessageInfo & message_info) +{ + (void)loaned_message; + (void)message_info; + throw std::runtime_error("handle_loaned_message() not supported by drake_ros_systems"); +} + +void +Subscription::return_message(std::shared_ptr & message) +{ + auto serialized_msg_ptr = std::static_pointer_cast(message); + return_serialized_message(serialized_msg_ptr); +} + +void +Subscription::return_serialized_message(std::shared_ptr & message) +{ + message.reset(); +} } // namespace drake_ros_systems diff --git a/drake_ros_systems/src/subscription.hpp b/drake_ros_systems/src/subscription.hpp index 6fd0ce5..289fbc6 100644 --- a/drake_ros_systems/src/subscription.hpp +++ b/drake_ros_systems/src/subscription.hpp @@ -20,12 +20,47 @@ class Subscription final : public rclcpp::SubscriptionBase const rosidl_message_type_support_t & ts, const std::string & topic_name, const rclcpp::QoS & qos, - std::function)> callback); + std::function)> callback); ~Subscription(); +protected: + /// Borrow a new message. + /** \return Shared pointer to the fresh message. */ + std::shared_ptr + create_message() override; + + /// Borrow a new serialized message + /** \return Shared pointer to a rcl_message_serialized_t. */ + std::shared_ptr + create_serialized_message() override; + + /// Check if we need to handle the message, and execute the callback if we do. + /** + * \param[in] message Shared pointer to the message to handle. + * \param[in] message_info Metadata associated with this message. + */ + void + handle_message( + std::shared_ptr & message, + const rclcpp::MessageInfo & message_info) override; + + void + handle_loaned_message( + void * loaned_message, const rclcpp::MessageInfo & message_info) override; + + /// Return the message borrowed in create_message. + /** \param[in] message Shared pointer to the returned message. */ + void + return_message(std::shared_ptr & message) override; + + /// Return the message borrowed in create_serialized_message. + /** \param[in] message Shared pointer to the returned message. */ + void + return_serialized_message(std::shared_ptr & message) override; + private: - std::function)> callback_; + std::function)> callback_; }; } // namespace drake_ros_systems #endif // DRAKE_ROS_SYSTEMS__SUBSCRIPTION_HPP_ From 7cebeeca5e2ddb47deca0cee62c38bb38ce73a32 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Tue, 10 Nov 2020 16:39:13 -0800 Subject: [PATCH 05/39] Possible RosInterfaceSystem impl Signed-off-by: Shane Loretz --- .../drake_ros_systems/drake_ros_interface.hpp | 4 +- .../ros_interface_system.hpp | 2 +- .../src/ros_interface_system.cpp | 43 +++++++++++++++++++ 3 files changed, 46 insertions(+), 3 deletions(-) diff --git a/drake_ros_systems/include/drake_ros_systems/drake_ros_interface.hpp b/drake_ros_systems/include/drake_ros_systems/drake_ros_interface.hpp index 9daccd5..7600def 100644 --- a/drake_ros_systems/include/drake_ros_systems/drake_ros_interface.hpp +++ b/drake_ros_systems/include/drake_ros_systems/drake_ros_interface.hpp @@ -1,7 +1,6 @@ #ifndef DRAKE_ROS_SYSTEMS__DRAKE_ROS_INTERFACE_HPP_ #define DRAKE_ROS_SYSTEMS__DRAKE_ROS_INTERFACE_HPP_ -#include #include #include @@ -9,6 +8,7 @@ #include #include +#include namespace drake_ros_systems { @@ -33,7 +33,7 @@ class DrakeRosInterface const rosidl_message_type_support_t & ts, const std::string & topic_name, const rclcpp::QoS & qos, - std::function)> callback) = 0; + std::function)> callback) = 0; virtual void diff --git a/drake_ros_systems/include/drake_ros_systems/ros_interface_system.hpp b/drake_ros_systems/include/drake_ros_systems/ros_interface_system.hpp index 0d5c558..1eef43e 100644 --- a/drake_ros_systems/include/drake_ros_systems/ros_interface_system.hpp +++ b/drake_ros_systems/include/drake_ros_systems/ros_interface_system.hpp @@ -16,7 +16,7 @@ class RosInterfaceSystemPrivate; class RosInterfaceSystem : public drake::systems::LeafSystem { public: - RosInterfaceSystem(); + RosInterfaceSystem(std::unique_ptr ros); virtual ~RosInterfaceSystem(); /// Return a handle for interacting with ROS diff --git a/drake_ros_systems/src/ros_interface_system.cpp b/drake_ros_systems/src/ros_interface_system.cpp index e69de29..a9a27db 100644 --- a/drake_ros_systems/src/ros_interface_system.cpp +++ b/drake_ros_systems/src/ros_interface_system.cpp @@ -0,0 +1,43 @@ +#include + +namespace drake_ros_systems +{ +class RosInterfaceSystemPrivate +{ +public: + std::shared_ptr ros_; +}; + + +RosInterfaceSystem::RosInterfaceSystem(std::unique_ptr ros) +: impl_(new RosInterfaceSystemPrivate()) +{ + impl_->ros_ = std::move(ros); +} + +RosInterfaceSystem::~RosInterfaceSystem() +{ +} + +/// Return a handle for interacting with ROS +std::shared_ptr +RosInterfaceSystem::get_ros_interface() const +{ + return impl_->ros_; +} + +/// Override as a place to call rclcpp::spin() +void +RosInterfaceSystem::DoCalcNextUpdateTime( + const drake::systems::Context &, + drake::systems::CompositeEventCollection *, + double * time) const +{ + // Do work for at most 1ms so system doesn't get blocked if there's more work than it can handle + const int max_work_time_millis = 1; + impl_->ros_->spin(max_work_time_millis); + // TODO(sloretz) Lcm system pauses time if some work was done, but ROS 2 API doesn't say if + // any work was done. How to reconcile that? + *time = std::numeric_limits::infinity(); +} +} // namespace drake_ros_systems From 7b3a07cd37eabedccca10d90ad5ee47a9689f807 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 11 Nov 2020 10:33:06 -0800 Subject: [PATCH 06/39] Possible RosSubscriberSystem impl Signed-off-by: Shane Loretz --- .../ros_subscriber_system.hpp | 11 +- .../src/ros_subscriber_system.cpp | 116 ++++++++++++++++++ 2 files changed, 125 insertions(+), 2 deletions(-) diff --git a/drake_ros_systems/include/drake_ros_systems/ros_subscriber_system.hpp b/drake_ros_systems/include/drake_ros_systems/ros_subscriber_system.hpp index 416b19f..1039f56 100644 --- a/drake_ros_systems/include/drake_ros_systems/ros_subscriber_system.hpp +++ b/drake_ros_systems/include/drake_ros_systems/ros_subscriber_system.hpp @@ -23,16 +23,23 @@ class RosSubscriberSystem : public drake::systems::LeafSystem template static std::unique_ptr - Make(std::shared_ptr ros_interface) + 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 return std::make_unique( *rosidl_typesupport_cpp::get_message_type_support_handle(), - ros_interface); + [](){return std::make_unique>(MessageT());}, + topic_name, qos, ros_interface); } RosSubscriberSystem( const rosidl_message_type_support_t & ts, + std::function(void)> create_default_value, + const std::string & topic_name, + const rclcpp::QoS & qos, std::shared_ptr ros_interface); virtual ~RosSubscriberSystem(); diff --git a/drake_ros_systems/src/ros_subscriber_system.cpp b/drake_ros_systems/src/ros_subscriber_system.cpp index e69de29..6f8abe7 100644 --- a/drake_ros_systems/src/ros_subscriber_system.cpp +++ b/drake_ros_systems/src/ros_subscriber_system.cpp @@ -0,0 +1,116 @@ +#include + +#include + +#include + +#include "subscription.hpp" + +namespace drake_ros_systems +{ +// Index in AbstractState that deserialized message is stored +const int kStateIndexMessage = 0; + +class RosSubscriberSystemPrivate +{ +public: + void + handle_message(std::shared_ptr callback) + { + std::lock_guard message_lock(mutex_); + // TODO(sloretz) Queue messages here? Lcm subscriber doesn't, so maybe lost messages are ok + // Overwrite last message + msg_ = callback; + } + + std::shared_ptr + take_message() + { + std::lock_guard message_lock(mutex_); + return std::move(msg_); + } + + const rosidl_message_type_support_t * type_support_; + // A handle to a subscription - we're subscribed as long as this is alive + std::unique_ptr sub_; + // Mutex to prevent multiple threads from modifying this class + std::mutex mutex_; + // The last received message that has not yet been put into a context. + std::shared_ptr msg_; +}; + +RosSubscriberSystem::RosSubscriberSystem( + const rosidl_message_type_support_t & ts, + std::function(void)> create_default_value, + const std::string & topic_name, + const rclcpp::QoS & qos, + std::shared_ptr ros) +: impl_(new RosSubscriberSystemPrivate()) +{ + impl_->type_support_ = &ts; + impl_->sub_ = ros->create_subscription(ts, topic_name, qos, + std::bind(&RosSubscriberSystemPrivate::handle_message, impl_.get(), std::placeholders::_1)); + + DeclareAbstractOutputPort( + create_default_value, + [](const drake::systems::Context & context, drake::AbstractValue * output_value) { + // Transfer message from state to output port + output_value->SetFrom(context.get_abstract_state().get_value(kStateIndexMessage)); + }); + + static_assert(kStateIndexMessage == 0, ""); + DeclareAbstractState(create_default_value()); +} + +RosSubscriberSystem::~RosSubscriberSystem() +{ +} + +void +RosSubscriberSystem::DoCalcNextUpdateTime( + const drake::systems::Context & context, + drake::systems::CompositeEventCollection * events, + double * time) const +{ + // Vvv Copied from LcmSubscriberSystem vvv + + // We do not support events other than our own message timing events. + LeafSystem::DoCalcNextUpdateTime(context, events, time); + DRAKE_THROW_UNLESS(events->HasEvents() == false); + DRAKE_THROW_UNLESS(std::isinf(*time)); + + std::shared_ptr message = impl_->take_message(); + + // Do nothing unless we have a new message. + if (nullptr == message.get()) { + return; + } + + // Create a unrestricted event and tie the handler to the corresponding + // function. + drake::systems::UnrestrictedUpdateEvent::UnrestrictedUpdateCallback + callback = [this, serialized_message{std::move(message)}, ts{impl_->type_support_}]( + const drake::systems::Context&, + const drake::systems::UnrestrictedUpdateEvent&, + drake::systems::State* state) + { + // Deserialize the message and store it in the abstract state on the context + drake::systems::AbstractValues & abstract_state = state->get_mutable_abstract_state(); + const auto ret = rmw_deserialize( + &serialized_message->get_rcl_serialized_message(), ts, + &abstract_state.get_mutable_value(kStateIndexMessage)); + if (ret != RMW_RET_OK) { + return drake::systems::EventStatus::Failed(this, "Failed to deserialize ROS message"); + } + return drake::systems::EventStatus::Succeeded(); + }; + + // Schedule an update event at the current time. + *time = context.get_time(); + drake::systems::EventCollection> & uu_events = + events->get_mutable_unrestricted_update_events(); + uu_events.add_event( + std::make_unique>( + drake::systems::TriggerType::kTimed, callback)); +} +} // namespace drake_ros_systems From e3bc6439d3aed38e910d128929df577c2f3c0bd4 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 11 Nov 2020 13:01:32 -0800 Subject: [PATCH 07/39] Possible RosPublisherSystem implementation Signed-off-by: Shane Loretz --- .../ros_publisher_system.hpp | 25 +++++++- .../src/ros_publisher_system.cpp | 59 +++++++++++++++++++ 2 files changed, 82 insertions(+), 2 deletions(-) diff --git a/drake_ros_systems/include/drake_ros_systems/ros_publisher_system.hpp b/drake_ros_systems/include/drake_ros_systems/ros_publisher_system.hpp index da39611..2a6aa7d 100644 --- a/drake_ros_systems/include/drake_ros_systems/ros_publisher_system.hpp +++ b/drake_ros_systems/include/drake_ros_systems/ros_publisher_system.hpp @@ -24,16 +24,33 @@ class RosPublisherSystem: public drake::systems::LeafSystem template static std::unique_ptr - Make(std::shared_ptr ros_interface) + 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 return std::make_unique( *rosidl_typesupport_cpp::get_message_type_support_handle(), - ros_interface); + [](){return std::make_unique>(MessageT());}, + [](const drake::AbstractValue & av, rclcpp::SerializedMessage & serialized_msg){ + const MessageT & message = av.get_value(); + const auto ret = rmw_serialize( + &message, + rosidl_typesupport_cpp::get_message_type_support_handle(), + &serialized_msg.get_rcl_serialized_message()); + // TODO(sloretz) throw if failed to serialize + (void)ret; + }, + topic_name, qos, ros_interface); } RosPublisherSystem( const rosidl_message_type_support_t & ts, + std::function(void)> create_default_value, + std::function serialize_abstract_value, + const std::string & topic_name, + const rclcpp::QoS & qos, std::shared_ptr ros_interface); virtual ~RosPublisherSystem(); @@ -57,6 +74,10 @@ class RosPublisherSystem: public drake::systems::LeafSystem publish(const rclcpp::SerializedMessage & serialized_msg); protected: + + void + publish_input(const drake::systems::Context & context); + std::unique_ptr impl_; }; } // namespace drake_ros_systems diff --git a/drake_ros_systems/src/ros_publisher_system.cpp b/drake_ros_systems/src/ros_publisher_system.cpp index e69de29..0627e96 100644 --- a/drake_ros_systems/src/ros_publisher_system.cpp +++ b/drake_ros_systems/src/ros_publisher_system.cpp @@ -0,0 +1,59 @@ +#include + +#include "publisher.hpp" + +namespace drake_ros_systems +{ +class RosPublisherSystemPrivate +{ +public: + const rosidl_message_type_support_t * type_support_; + std::function serialize_abstract_value_; + std::unique_ptr pub_; +}; + +RosPublisherSystem::RosPublisherSystem( + const rosidl_message_type_support_t & ts, + std::function(void)> create_default_value, + std::function serialize_abstract_value, + const std::string & topic_name, + const rclcpp::QoS & qos, + std::shared_ptr ros) +: impl_(new RosPublisherSystemPrivate()) +{ + impl_->type_support_ = &ts; + impl_->serialize_abstract_value_ = serialize_abstract_value; + impl_->pub_ = ros->create_publisher(ts, topic_name, qos); + + DeclareAbstractInputPort("message", *create_default_value()); + + // TODO(sloretz) customizable triggers like lcm system + DeclarePerStepEvent( + drake::systems::PublishEvent([this]( + const drake::systems::Context& context, + const drake::systems::PublishEvent&) { + publish_input(context); + })); +} + +RosPublisherSystem::~RosPublisherSystem() +{ +} + +void +RosPublisherSystem::publish(const rclcpp::SerializedMessage & serialized_msg) +{ + impl_->pub_->publish(serialized_msg); +} + +void +RosPublisherSystem::publish_input(const drake::systems::Context & context) +{ + // Converts the input into LCM message bytes. + const drake::AbstractValue & input = get_input_port().Eval(context); + rclcpp::SerializedMessage serialized_msg; + impl_->serialize_abstract_value_(input, serialized_msg); + + impl_->pub_->publish(serialized_msg); +} +} // namespace drake_ros_systems From f84754c3ebe1a771226644cad89dc3de6b6fedee Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Fri, 13 Nov 2020 15:08:08 -0800 Subject: [PATCH 08/39] RosPublisherSystem uses separate Serializer interface Signed-off-by: Shane Loretz --- .../ros_publisher_system.hpp | 29 +++-------- .../include/drake_ros_systems/serializer.hpp | 51 +++++++++++++++++++ .../serializer_interface.hpp | 24 +++++++++ .../src/ros_publisher_system.cpp | 16 +++--- 4 files changed, 89 insertions(+), 31 deletions(-) create mode 100644 drake_ros_systems/include/drake_ros_systems/serializer.hpp create mode 100644 drake_ros_systems/include/drake_ros_systems/serializer_interface.hpp diff --git a/drake_ros_systems/include/drake_ros_systems/ros_publisher_system.hpp b/drake_ros_systems/include/drake_ros_systems/ros_publisher_system.hpp index 2a6aa7d..936e60b 100644 --- a/drake_ros_systems/include/drake_ros_systems/ros_publisher_system.hpp +++ b/drake_ros_systems/include/drake_ros_systems/ros_publisher_system.hpp @@ -7,6 +7,8 @@ #include #include +#include +#include #include #include @@ -30,43 +32,28 @@ class RosPublisherSystem: public drake::systems::LeafSystem 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( *rosidl_typesupport_cpp::get_message_type_support_handle(), - [](){return std::make_unique>(MessageT());}, - [](const drake::AbstractValue & av, rclcpp::SerializedMessage & serialized_msg){ - const MessageT & message = av.get_value(); - const auto ret = rmw_serialize( - &message, - rosidl_typesupport_cpp::get_message_type_support_handle(), - &serialized_msg.get_rcl_serialized_message()); - // TODO(sloretz) throw if failed to serialize - (void)ret; - }, - topic_name, qos, ros_interface); + serializer, topic_name, qos, ros_interface); } RosPublisherSystem( const rosidl_message_type_support_t & ts, - std::function(void)> create_default_value, - std::function serialize_abstract_value, + std::unique_ptr & serializer, const std::string & topic_name, const rclcpp::QoS & qos, std::shared_ptr ros_interface); virtual ~RosPublisherSystem(); - /// Publish a ROS message + /// Convenience method to publish a C++ ROS message template void publish(const MessageT & message) { - rclcpp::SerializedMessage serialized_msg; - const auto ret = rmw_serialize( - &message, - rosidl_typesupport_cpp::get_message_type_support_handle(), - &serialized_msg.get_rcl_serialized_message()); - // TODO(sloretz) throw if failed to serialize - publish(serialized_msg); + static const Serializer serializer; + publish(serializer->serialize(message)); } /// Publish a serialized ROS message diff --git a/drake_ros_systems/include/drake_ros_systems/serializer.hpp b/drake_ros_systems/include/drake_ros_systems/serializer.hpp new file mode 100644 index 0000000..fea7832 --- /dev/null +++ b/drake_ros_systems/include/drake_ros_systems/serializer.hpp @@ -0,0 +1,51 @@ +#ifndef DRAKE_ROS_SYSTEMS__SERIALIZER_HPP_ +#define DRAKE_ROS_SYSTEMS__SERIALIZER_HPP_ + +#include +#include + +#include + +#include +#include + +namespace drake_ros_systems +{ +template +class Serializer : public SerializerInterface +{ +public: + rclcpp::SerializedMessage + serialize(const drake::AbstractValue & abstract_value) const override + { + rclcpp::SerializedMessage serialized_msg; + const MessageT & message = abstract_value.get_value(); + const auto ret = rmw_serialize( + &message, + rosidl_typesupport_cpp::get_message_type_support_handle(), + &serialized_msg.get_rcl_serialized_message()); + if (ret != RMW_RET_OK) { + // TODO(sloretz) do something if serialization fails + (void)ret; + } + return serialized_msg; + } + + void + deserialize( + const rclcpp::SerializedMessage & message, + drake::AbstractValue & abstract_value) const override + { + // TODO + (void) message; + (void) abstract_value; + } + + std::unique_ptr + create_default_value() const override + { + return std::make_unique>(MessageT()); + } +}; +} // namespace drake_ros_systems +#endif // DRAKE_ROS_SYSTEMS__SERIALIZER_HPP_ diff --git a/drake_ros_systems/include/drake_ros_systems/serializer_interface.hpp b/drake_ros_systems/include/drake_ros_systems/serializer_interface.hpp new file mode 100644 index 0000000..f0669af --- /dev/null +++ b/drake_ros_systems/include/drake_ros_systems/serializer_interface.hpp @@ -0,0 +1,24 @@ +#ifndef DRAKE_ROS_SYSTEMS__SERIALIZER_INTERFACE_HPP_ +#define DRAKE_ROS_SYSTEMS__SERIALIZER_INTERFACE_HPP_ + +namespace drake_ros_systems +{ +class SerializerInterface +{ +public: + virtual + rclcpp::SerializedMessage + serialize(const drake::AbstractValue & abstract_value) const = 0; + + virtual + void + deserialize( + const rclcpp::SerializedMessage & message, + drake::AbstractValue & abstract_value) const = 0; + + virtual + std::unique_ptr + create_default_value() const = 0; +}; +} // namespace drake_ros_systems +#endif // DRAKE_ROS_SYSTEMS__SERIALIZER_INTERFACE_HPP_ diff --git a/drake_ros_systems/src/ros_publisher_system.cpp b/drake_ros_systems/src/ros_publisher_system.cpp index 0627e96..a5aa52a 100644 --- a/drake_ros_systems/src/ros_publisher_system.cpp +++ b/drake_ros_systems/src/ros_publisher_system.cpp @@ -1,4 +1,5 @@ #include +#include #include "publisher.hpp" @@ -8,24 +9,23 @@ class RosPublisherSystemPrivate { public: const rosidl_message_type_support_t * type_support_; - std::function serialize_abstract_value_; + std::unique_ptr serializer_; std::unique_ptr pub_; }; RosPublisherSystem::RosPublisherSystem( const rosidl_message_type_support_t & ts, - std::function(void)> create_default_value, - std::function serialize_abstract_value, + std::unique_ptr & serializer, const std::string & topic_name, const rclcpp::QoS & qos, std::shared_ptr ros) : impl_(new RosPublisherSystemPrivate()) { impl_->type_support_ = &ts; - impl_->serialize_abstract_value_ = serialize_abstract_value; + impl_->serializer_ = std::move(serializer); impl_->pub_ = ros->create_publisher(ts, topic_name, qos); - DeclareAbstractInputPort("message", *create_default_value()); + DeclareAbstractInputPort("message", *(impl_->serializer_->create_default_value())); // TODO(sloretz) customizable triggers like lcm system DeclarePerStepEvent( @@ -49,11 +49,7 @@ RosPublisherSystem::publish(const rclcpp::SerializedMessage & serialized_msg) void RosPublisherSystem::publish_input(const drake::systems::Context & context) { - // Converts the input into LCM message bytes. const drake::AbstractValue & input = get_input_port().Eval(context); - rclcpp::SerializedMessage serialized_msg; - impl_->serialize_abstract_value_(input, serialized_msg); - - impl_->pub_->publish(serialized_msg); + impl_->pub_->publish(impl_->serializer_->serialize(input)); } } // namespace drake_ros_systems From cd82726c7e22392a7d61c7e55be151e95e035eb0 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Fri, 13 Nov 2020 16:34:16 -0800 Subject: [PATCH 09/39] RosSubscriberSystem uses separate Serializer interface Signed-off-by: Shane Loretz --- .../include/drake_ros_systems/drake_ros.hpp | 2 +- .../drake_ros_systems/drake_ros_interface.hpp | 2 +- .../ros_subscriber_system.hpp | 8 +++++--- .../include/drake_ros_systems/serializer.hpp | 12 ++++++----- .../serializer_interface.hpp | 2 +- drake_ros_systems/src/drake_ros.cpp | 16 ++++++++++----- .../src/ros_subscriber_system.cpp | 20 ++++++++++--------- drake_ros_systems/src/subscription.hpp | 2 -- 8 files changed, 37 insertions(+), 27 deletions(-) diff --git a/drake_ros_systems/include/drake_ros_systems/drake_ros.hpp b/drake_ros_systems/include/drake_ros_systems/drake_ros.hpp index f3b3b61..02ea012 100644 --- a/drake_ros_systems/include/drake_ros_systems/drake_ros.hpp +++ b/drake_ros_systems/include/drake_ros_systems/drake_ros.hpp @@ -34,7 +34,7 @@ class DrakeRos final: public DrakeRosInterface const rclcpp::QoS & qos) final; virtual - std::unique_ptr + std::shared_ptr create_subscription( const rosidl_message_type_support_t & ts, const std::string & topic_name, diff --git a/drake_ros_systems/include/drake_ros_systems/drake_ros_interface.hpp b/drake_ros_systems/include/drake_ros_systems/drake_ros_interface.hpp index 7600def..c86ef60 100644 --- a/drake_ros_systems/include/drake_ros_systems/drake_ros_interface.hpp +++ b/drake_ros_systems/include/drake_ros_systems/drake_ros_interface.hpp @@ -28,7 +28,7 @@ class DrakeRosInterface const rclcpp::QoS & qos) = 0; virtual - std::unique_ptr + std::shared_ptr create_subscription( const rosidl_message_type_support_t & ts, const std::string & topic_name, diff --git a/drake_ros_systems/include/drake_ros_systems/ros_subscriber_system.hpp b/drake_ros_systems/include/drake_ros_systems/ros_subscriber_system.hpp index 1039f56..469ff87 100644 --- a/drake_ros_systems/include/drake_ros_systems/ros_subscriber_system.hpp +++ b/drake_ros_systems/include/drake_ros_systems/ros_subscriber_system.hpp @@ -6,6 +6,8 @@ #include #include +#include +#include #include @@ -29,15 +31,15 @@ class RosSubscriberSystem : public drake::systems::LeafSystem 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( *rosidl_typesupport_cpp::get_message_type_support_handle(), - [](){return std::make_unique>(MessageT());}, - topic_name, qos, ros_interface); + serializer, topic_name, qos, ros_interface); } RosSubscriberSystem( const rosidl_message_type_support_t & ts, - std::function(void)> create_default_value, + std::unique_ptr & serializer, const std::string & topic_name, const rclcpp::QoS & qos, std::shared_ptr ros_interface); diff --git a/drake_ros_systems/include/drake_ros_systems/serializer.hpp b/drake_ros_systems/include/drake_ros_systems/serializer.hpp index fea7832..9e0dd12 100644 --- a/drake_ros_systems/include/drake_ros_systems/serializer.hpp +++ b/drake_ros_systems/include/drake_ros_systems/serializer.hpp @@ -31,14 +31,16 @@ class Serializer : public SerializerInterface return serialized_msg; } - void + bool deserialize( - const rclcpp::SerializedMessage & message, + const rclcpp::SerializedMessage & serialized_message, drake::AbstractValue & abstract_value) const override { - // TODO - (void) message; - (void) abstract_value; + const auto ret = rmw_deserialize( + &serialized_message.get_rcl_serialized_message(), + rosidl_typesupport_cpp::get_message_type_support_handle(), + &abstract_value.get_mutable_value()); + return ret == RMW_RET_OK; } std::unique_ptr diff --git a/drake_ros_systems/include/drake_ros_systems/serializer_interface.hpp b/drake_ros_systems/include/drake_ros_systems/serializer_interface.hpp index f0669af..66b11e0 100644 --- a/drake_ros_systems/include/drake_ros_systems/serializer_interface.hpp +++ b/drake_ros_systems/include/drake_ros_systems/serializer_interface.hpp @@ -11,7 +11,7 @@ class SerializerInterface serialize(const drake::AbstractValue & abstract_value) const = 0; virtual - void + bool deserialize( const rclcpp::SerializedMessage & message, drake::AbstractValue & abstract_value) const = 0; diff --git a/drake_ros_systems/src/drake_ros.cpp b/drake_ros_systems/src/drake_ros.cpp index 6486258..2cbf31f 100644 --- a/drake_ros_systems/src/drake_ros.cpp +++ b/drake_ros_systems/src/drake_ros.cpp @@ -47,19 +47,25 @@ DrakeRos::create_publisher( const std::string & topic_name, const rclcpp::QoS & qos) { - return std::unique_ptr( - new Publisher(impl_->node_->get_node_base_interface().get(), ts, topic_name, qos)); + return std::make_unique( + impl_->node_->get_node_base_interface().get(), ts, topic_name, qos); } -std::unique_ptr +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) { - return std::unique_ptr( - new Subscription(impl_->node_->get_node_base_interface().get(), ts, topic_name, qos, 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); + + // TODO(sloretz) return unique pointer to subscription and make subscription + // automatically unsubscribe when it's deleted + + return sub; } void diff --git a/drake_ros_systems/src/ros_subscriber_system.cpp b/drake_ros_systems/src/ros_subscriber_system.cpp index 6f8abe7..7ccf8e9 100644 --- a/drake_ros_systems/src/ros_subscriber_system.cpp +++ b/drake_ros_systems/src/ros_subscriber_system.cpp @@ -31,8 +31,10 @@ class RosSubscriberSystemPrivate } const rosidl_message_type_support_t * type_support_; - // A handle to a subscription - we're subscribed as long as this is alive - std::unique_ptr sub_; + std::unique_ptr serializer_; + // A handle to a subscription + // TODO(sloretz) unique_ptr that unsubscribes in destructor + std::shared_ptr sub_; // Mutex to prevent multiple threads from modifying this class std::mutex mutex_; // The last received message that has not yet been put into a context. @@ -41,25 +43,26 @@ class RosSubscriberSystemPrivate RosSubscriberSystem::RosSubscriberSystem( const rosidl_message_type_support_t & ts, - std::function(void)> create_default_value, + std::unique_ptr & serializer, const std::string & topic_name, const rclcpp::QoS & qos, std::shared_ptr ros) : impl_(new RosSubscriberSystemPrivate()) { impl_->type_support_ = &ts; + impl_->serializer_ = std::move(serializer); impl_->sub_ = ros->create_subscription(ts, topic_name, qos, std::bind(&RosSubscriberSystemPrivate::handle_message, impl_.get(), std::placeholders::_1)); DeclareAbstractOutputPort( - create_default_value, + [serializer{impl_->serializer_.get()}](){return serializer->create_default_value();}, [](const drake::systems::Context & context, drake::AbstractValue * output_value) { // Transfer message from state to output port output_value->SetFrom(context.get_abstract_state().get_value(kStateIndexMessage)); }); static_assert(kStateIndexMessage == 0, ""); - DeclareAbstractState(create_default_value()); + DeclareAbstractState(impl_->serializer_->create_default_value()); } RosSubscriberSystem::~RosSubscriberSystem() @@ -89,16 +92,15 @@ RosSubscriberSystem::DoCalcNextUpdateTime( // Create a unrestricted event and tie the handler to the corresponding // function. drake::systems::UnrestrictedUpdateEvent::UnrestrictedUpdateCallback - callback = [this, serialized_message{std::move(message)}, ts{impl_->type_support_}]( + callback = [this, serialized_message{std::move(message)}]( const drake::systems::Context&, const drake::systems::UnrestrictedUpdateEvent&, drake::systems::State* state) { // Deserialize the message and store it in the abstract state on the context drake::systems::AbstractValues & abstract_state = state->get_mutable_abstract_state(); - const auto ret = rmw_deserialize( - &serialized_message->get_rcl_serialized_message(), ts, - &abstract_state.get_mutable_value(kStateIndexMessage)); + 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"); } diff --git a/drake_ros_systems/src/subscription.hpp b/drake_ros_systems/src/subscription.hpp index 289fbc6..03d74af 100644 --- a/drake_ros_systems/src/subscription.hpp +++ b/drake_ros_systems/src/subscription.hpp @@ -64,5 +64,3 @@ class Subscription final : public rclcpp::SubscriptionBase }; } // namespace drake_ros_systems #endif // DRAKE_ROS_SYSTEMS__SUBSCRIPTION_HPP_ - - From e85f60d329b2c0e93feec890c0ef26fc45fc2c18 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Fri, 13 Nov 2020 16:35:23 -0800 Subject: [PATCH 10/39] Add RS-flip-flop example Signed-off-by: Shane Loretz --- drake_ros_systems/CMakeLists.txt | 2 + drake_ros_systems/example/CMakeLists.txt | 8 ++ drake_ros_systems/example/rs_flip_flop.cpp | 134 +++++++++++++++++++++ 3 files changed, 144 insertions(+) create mode 100644 drake_ros_systems/example/CMakeLists.txt create mode 100644 drake_ros_systems/example/rs_flip_flop.cpp diff --git a/drake_ros_systems/CMakeLists.txt b/drake_ros_systems/CMakeLists.txt index 681dcbb..c873370 100644 --- a/drake_ros_systems/CMakeLists.txt +++ b/drake_ros_systems/CMakeLists.txt @@ -44,6 +44,8 @@ target_include_directories(drake_ros_systems "$" ) +add_subdirectory(example) + ament_export_dependencies(drake) ament_export_dependencies(rclcpp) ament_export_dependencies(rosidl_generator_c) diff --git a/drake_ros_systems/example/CMakeLists.txt b/drake_ros_systems/example/CMakeLists.txt new file mode 100644 index 0000000..284db04 --- /dev/null +++ b/drake_ros_systems/example/CMakeLists.txt @@ -0,0 +1,8 @@ +find_package(std_msgs REQUIRED) + +add_executable(rs_flip_flop rs_flip_flop.cpp) +target_link_libraries(rs_flip_flop + drake::drake + drake_ros_systems + ${std_msgs_TARGETS} +) diff --git a/drake_ros_systems/example/rs_flip_flop.cpp b/drake_ros_systems/example/rs_flip_flop.cpp new file mode 100644 index 0000000..7c33002 --- /dev/null +++ b/drake_ros_systems/example/rs_flip_flop.cpp @@ -0,0 +1,134 @@ +#include +#include +#include + +#include +#include +#include +#include + +#include + +using drake_ros_systems::DrakeRos; +using drake_ros_systems::RosInterfaceSystem; +using drake_ros_systems::RosPublisherSystem; +using drake_ros_systems::RosSubscriberSystem; + +class NorGate : public drake::systems::LeafSystem +{ +public: + NorGate() + { + DeclareAbstractInputPort("A", *drake::AbstractValue::Make(std_msgs::msg::Bool())); + DeclareAbstractInputPort("B", *drake::AbstractValue::Make(std_msgs::msg::Bool())); + + DeclareAbstractOutputPort("Q", &NorGate::calc_output_value); + } + + virtual ~NorGate() = default; + +private: + void + calc_output_value(const drake::systems::Context & context, std_msgs::msg::Bool * output) const + { + const bool a = GetInputPort("A").Eval(context).data; + const bool b = GetInputPort("B").Eval(context).data; + output->data = !(a || b); + } +}; + +// Delay's input port by one timestep to avoid algebraic loop error +// Inspired by Simulink's Memory block +template +class Memory : public drake::systems::LeafSystem +{ +public: + Memory(const T & initial_value) + { + DeclareAbstractInputPort("value", *drake::AbstractValue::Make(T())); + + // State for value + DeclareAbstractState(drake::AbstractValue::Make(initial_value)); + + // Output depends only on the previous state + DeclareAbstractOutputPort("value", &Memory::calc_output_value, {all_state_ticket()}); + + DeclarePerStepEvent( + drake::systems::UnrestrictedUpdateEvent([this]( + const drake::systems::Context& context, + const drake::systems::UnrestrictedUpdateEvent&, + drake::systems::State * state) { + // Copy input value to state + drake::systems::AbstractValues & abstract_state = state->get_mutable_abstract_state(); + abstract_state.get_mutable_value(0).SetFrom( + get_input_port().Eval(context)); + })); + } + + virtual ~Memory() = default; + +private: + void + calc_output_value(const drake::systems::Context & context, T * output) const + { + *output = context.get_abstract_state().get_value(0).get_value(); + } +}; + +int main() +{ + // NOR gate RS flip flop example + // Input topics /S and /R are active high (true is logic 1 and false is logic 0) + // Output topics /Q and /Q_not are active low (true is logic 0 and false is logic 1) + + // Input/Output table + // S: false R: false | Q: no change Q_not: no change + // S: true R: false | Q: false Q_not: true + // S: false R: true | Q: true Q_not: false + // S: true R: true | Q: invalid Q_not: invalid + drake::systems::DiagramBuilder builder; + + rclcpp::QoS qos{10}; + + auto sys_ros_interface = builder.AddSystem(std::make_unique()); + auto sys_pub_Q = builder.AddSystem( + RosPublisherSystem::Make( + "Q", qos, sys_ros_interface->get_ros_interface())); + auto sys_pub_Q_not = builder.AddSystem( + RosPublisherSystem::Make( + "Q_not", qos, sys_ros_interface->get_ros_interface())); + auto sys_sub_S = builder.AddSystem( + RosSubscriberSystem::Make( + "S", qos, sys_ros_interface->get_ros_interface())); + auto sys_sub_R = builder.AddSystem( + RosSubscriberSystem::Make( + "R", qos, sys_ros_interface->get_ros_interface())); + auto sys_nor_gate_1 = builder.AddSystem(); + auto sys_nor_gate_2 = builder.AddSystem(); + auto sys_memory = builder.AddSystem>(std_msgs::msg::Bool()); + + builder.Connect(sys_nor_gate_1->GetOutputPort("Q"), sys_memory->get_input_port(0)); + + builder.Connect(sys_sub_S->get_output_port(0), sys_nor_gate_1->GetInputPort("A")); + builder.Connect(sys_nor_gate_2->GetOutputPort("Q"), sys_nor_gate_1->GetInputPort("B")); + + builder.Connect(sys_memory->get_output_port(0), sys_nor_gate_2->GetInputPort("A")); + builder.Connect(sys_sub_R->get_output_port(0), sys_nor_gate_2->GetInputPort("B")); + + builder.Connect(sys_nor_gate_1->GetOutputPort("Q"), sys_pub_Q->get_input_port(0)); + builder.Connect(sys_nor_gate_2->GetOutputPort("Q"), sys_pub_Q_not->get_input_port(0)); + + auto diagram = builder.Build(); + auto context = diagram->CreateDefaultContext(); + + auto simulator = std::make_unique>(*diagram, std::move(context)); + simulator->set_target_realtime_rate(1.0); + simulator->Initialize(); + + auto & simulator_context = simulator->get_mutable_context(); + + while (true) { + simulator->AdvanceTo(simulator_context.get_time() + 0.1); + } + return 0; +} From 586100b3bb6b9623943031c9fa3424b7a8a74f7c Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Thu, 19 Nov 2020 09:35:03 -0800 Subject: [PATCH 11/39] Add get_type_support() to SerializerInterface Signed-off-by: Shane Loretz --- .../drake_ros_systems/ros_publisher_system.hpp | 6 +----- .../drake_ros_systems/ros_subscriber_system.hpp | 5 +---- .../include/drake_ros_systems/serializer.hpp | 10 ++++++++-- .../drake_ros_systems/serializer_interface.hpp | 11 +++++++++++ drake_ros_systems/src/ros_publisher_system.cpp | 6 ++---- drake_ros_systems/src/ros_subscriber_system.cpp | 5 +---- 6 files changed, 24 insertions(+), 19 deletions(-) diff --git a/drake_ros_systems/include/drake_ros_systems/ros_publisher_system.hpp b/drake_ros_systems/include/drake_ros_systems/ros_publisher_system.hpp index 936e60b..03f19b1 100644 --- a/drake_ros_systems/include/drake_ros_systems/ros_publisher_system.hpp +++ b/drake_ros_systems/include/drake_ros_systems/ros_publisher_system.hpp @@ -11,7 +11,6 @@ #include #include -#include namespace drake_ros_systems { @@ -33,13 +32,10 @@ class RosPublisherSystem: public drake::systems::LeafSystem { // Assume C++ typesupport since this is a C++ template function std::unique_ptr serializer = std::make_unique>(); - return std::make_unique( - *rosidl_typesupport_cpp::get_message_type_support_handle(), - serializer, topic_name, qos, ros_interface); + return std::make_unique(serializer, topic_name, qos, ros_interface); } RosPublisherSystem( - const rosidl_message_type_support_t & ts, std::unique_ptr & serializer, const std::string & topic_name, const rclcpp::QoS & qos, diff --git a/drake_ros_systems/include/drake_ros_systems/ros_subscriber_system.hpp b/drake_ros_systems/include/drake_ros_systems/ros_subscriber_system.hpp index 469ff87..2e3c008 100644 --- a/drake_ros_systems/include/drake_ros_systems/ros_subscriber_system.hpp +++ b/drake_ros_systems/include/drake_ros_systems/ros_subscriber_system.hpp @@ -32,13 +32,10 @@ class RosSubscriberSystem : public drake::systems::LeafSystem { // Assume C++ typesupport since this is a C++ template function std::unique_ptr serializer = std::make_unique>(); - return std::make_unique( - *rosidl_typesupport_cpp::get_message_type_support_handle(), - serializer, topic_name, qos, ros_interface); + return std::make_unique(serializer, topic_name, qos, ros_interface); } RosSubscriberSystem( - const rosidl_message_type_support_t & ts, std::unique_ptr & serializer, const std::string & topic_name, const rclcpp::QoS & qos, diff --git a/drake_ros_systems/include/drake_ros_systems/serializer.hpp b/drake_ros_systems/include/drake_ros_systems/serializer.hpp index 9e0dd12..a370e98 100644 --- a/drake_ros_systems/include/drake_ros_systems/serializer.hpp +++ b/drake_ros_systems/include/drake_ros_systems/serializer.hpp @@ -22,7 +22,7 @@ class Serializer : public SerializerInterface const MessageT & message = abstract_value.get_value(); const auto ret = rmw_serialize( &message, - rosidl_typesupport_cpp::get_message_type_support_handle(), + get_type_support(), &serialized_msg.get_rcl_serialized_message()); if (ret != RMW_RET_OK) { // TODO(sloretz) do something if serialization fails @@ -38,7 +38,7 @@ class Serializer : public SerializerInterface { const auto ret = rmw_deserialize( &serialized_message.get_rcl_serialized_message(), - rosidl_typesupport_cpp::get_message_type_support_handle(), + get_type_support(), &abstract_value.get_mutable_value()); return ret == RMW_RET_OK; } @@ -48,6 +48,12 @@ class Serializer : public SerializerInterface { return std::make_unique>(MessageT()); } + + const rosidl_message_type_support_t * + get_type_support() const override + { + return rosidl_typesupport_cpp::get_message_type_support_handle(); + } }; } // namespace drake_ros_systems #endif // DRAKE_ROS_SYSTEMS__SERIALIZER_HPP_ diff --git a/drake_ros_systems/include/drake_ros_systems/serializer_interface.hpp b/drake_ros_systems/include/drake_ros_systems/serializer_interface.hpp index 66b11e0..a9df7e5 100644 --- a/drake_ros_systems/include/drake_ros_systems/serializer_interface.hpp +++ b/drake_ros_systems/include/drake_ros_systems/serializer_interface.hpp @@ -1,6 +1,13 @@ #ifndef DRAKE_ROS_SYSTEMS__SERIALIZER_INTERFACE_HPP_ #define DRAKE_ROS_SYSTEMS__SERIALIZER_INTERFACE_HPP_ +#include + +#include + +#include +#include + namespace drake_ros_systems { class SerializerInterface @@ -19,6 +26,10 @@ class SerializerInterface virtual std::unique_ptr create_default_value() const = 0; + + virtual + const rosidl_message_type_support_t * + get_type_support() const = 0; }; } // namespace drake_ros_systems #endif // DRAKE_ROS_SYSTEMS__SERIALIZER_INTERFACE_HPP_ diff --git a/drake_ros_systems/src/ros_publisher_system.cpp b/drake_ros_systems/src/ros_publisher_system.cpp index a5aa52a..176fdce 100644 --- a/drake_ros_systems/src/ros_publisher_system.cpp +++ b/drake_ros_systems/src/ros_publisher_system.cpp @@ -8,22 +8,20 @@ namespace drake_ros_systems class RosPublisherSystemPrivate { public: - const rosidl_message_type_support_t * type_support_; std::unique_ptr serializer_; std::unique_ptr pub_; }; RosPublisherSystem::RosPublisherSystem( - const rosidl_message_type_support_t & ts, std::unique_ptr & serializer, const std::string & topic_name, const rclcpp::QoS & qos, std::shared_ptr ros) : impl_(new RosPublisherSystemPrivate()) { - impl_->type_support_ = &ts; impl_->serializer_ = std::move(serializer); - impl_->pub_ = ros->create_publisher(ts, topic_name, qos); + impl_->pub_ = ros->create_publisher( + *impl_->serializer_->get_type_support(), topic_name, qos); DeclareAbstractInputPort("message", *(impl_->serializer_->create_default_value())); diff --git a/drake_ros_systems/src/ros_subscriber_system.cpp b/drake_ros_systems/src/ros_subscriber_system.cpp index 7ccf8e9..02b43cc 100644 --- a/drake_ros_systems/src/ros_subscriber_system.cpp +++ b/drake_ros_systems/src/ros_subscriber_system.cpp @@ -30,7 +30,6 @@ class RosSubscriberSystemPrivate return std::move(msg_); } - const rosidl_message_type_support_t * type_support_; std::unique_ptr serializer_; // A handle to a subscription // TODO(sloretz) unique_ptr that unsubscribes in destructor @@ -42,16 +41,14 @@ class RosSubscriberSystemPrivate }; RosSubscriberSystem::RosSubscriberSystem( - const rosidl_message_type_support_t & ts, std::unique_ptr & serializer, const std::string & topic_name, const rclcpp::QoS & qos, std::shared_ptr ros) : impl_(new RosSubscriberSystemPrivate()) { - impl_->type_support_ = &ts; impl_->serializer_ = std::move(serializer); - impl_->sub_ = ros->create_subscription(ts, topic_name, qos, + impl_->sub_ = ros->create_subscription(*impl_->serializer_->get_type_support(), topic_name, qos, std::bind(&RosSubscriberSystemPrivate::handle_message, impl_.get(), std::placeholders::_1)); DeclareAbstractOutputPort( From 1b012f2910666fdf5e2551efa5943b1e1ab55123 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 2 Dec 2020 11:10:35 -0800 Subject: [PATCH 12/39] Hello world pub example Publisher and ROS interface working, subscriber not yet working. Signed-off-by: Shane Loretz Signed-off-by: Shane Loretz --- drake_ros_systems/CMakeLists.txt | 42 +++-- drake_ros_systems/example/rs_flip_flop.py | 81 +++++++++ .../module_drake_ros_systems.cpp | 61 +++++++ .../src/python_bindings/py_serializer.hpp | 157 ++++++++++++++++++ 4 files changed, 324 insertions(+), 17 deletions(-) create mode 100755 drake_ros_systems/example/rs_flip_flop.py create mode 100644 drake_ros_systems/src/python_bindings/module_drake_ros_systems.cpp create mode 100644 drake_ros_systems/src/python_bindings/py_serializer.hpp diff --git a/drake_ros_systems/CMakeLists.txt b/drake_ros_systems/CMakeLists.txt index c873370..e3caf42 100644 --- a/drake_ros_systems/CMakeLists.txt +++ b/drake_ros_systems/CMakeLists.txt @@ -17,8 +17,8 @@ endif() find_package(ament_cmake_ros REQUIRED) find_package(drake REQUIRED) -# find_package(pybind11_vendor) -# find_package(pybind11 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) @@ -61,21 +61,29 @@ install( DESTINATION include ) -# pybind11_add_module(rccl SHARED -# use_ros.cpp -# ) -# target_link_libraries(rccl PRIVATE -# rclcpp::rclcpp -# ${std_msgs_TARGETS} -# ) -# -# # Sets PYTHON_INSTALL_DIR -# _ament_cmake_python_get_python_install_dir() -# -# install( -# TARGETS rccl -# DESTINATION "${PYTHON_INSTALL_DIR}" -# ) +### +# Python bindings +### +pybind11_add_module(py_drake_ros_systems SHARED + src/python_bindings/module_drake_ros_systems.cpp +) +set_target_properties(py_drake_ros_systems PROPERTIES OUTPUT_NAME "drake_ros_systems") +target_link_libraries(py_drake_ros_systems PRIVATE + drake_ros_systems +) +target_include_directories(drake_ros_systems + PRIVATE + "$" +) + +# Sets PYTHON_INSTALL_DIR +_ament_cmake_python_get_python_install_dir() + +install( + TARGETS py_drake_ros_systems + DESTINATION "${PYTHON_INSTALL_DIR}" +) +### End Python bindings ament_package() diff --git a/drake_ros_systems/example/rs_flip_flop.py b/drake_ros_systems/example/rs_flip_flop.py new file mode 100755 index 0000000..acf763a --- /dev/null +++ b/drake_ros_systems/example/rs_flip_flop.py @@ -0,0 +1,81 @@ +#!/usr/bin/env python3 + +from drake_ros_systems import RosInterfaceSystem +from drake_ros_systems import RosPublisherSystem + +from pydrake.systems.analysis import Simulator +from pydrake.systems.framework import DiagramBuilder +# from pydrake.systems.lcm import LcmInterfaceSystem +# from pydrake.lcm import DrakeLcm +from pydrake.common.value import AbstractValue +from pydrake.systems.framework import LeafSystem + +from std_msgs.msg import String + + +def traced(func, ignoredirs=None): + """Decorates func such that its execution is traced, but filters out any + Python code outside of the system prefix.""" + import functools + import sys + import trace + if ignoredirs is None: + ignoredirs = ["/usr", sys.prefix] + tracer = trace.Trace(trace=1, count=0, ignoredirs=ignoredirs) + + @functools.wraps(func) + def wrapped(*args, **kwargs): + return tracer.runfunc(func, *args, **kwargs) + + return wrapped + + +class HelloWorld(LeafSystem): + """Outputs Hello World bool message.""" + + def __init__(self): + super().__init__() + + self.DeclareAbstractOutputPort( + 'text', + lambda: AbstractValue.Make(String()), + self._do_output_text) + + def _do_output_text(self, context, data): + data.get_mutable_value().data = "hello world" + + +# @traced +def main(): + builder = DiagramBuilder() + + sys_ros_interface = builder.AddSystem(RosInterfaceSystem()) + + # can add publisher systems + # Need bindings for subscriber system + # Need python version of memory class + # Need python version of NAND gate class + # Need boilerplate to create system + + # rps = RosPublisherSystem(String, "asdf", ris.get_ros_interface()) + + sys_hello_world = builder.AddSystem(HelloWorld()) + sys_ros_pub = builder.AddSystem( + RosPublisherSystem(String, "asdf", sys_ros_interface.get_ros_interface())) + + builder.Connect( + sys_hello_world.get_output_port(0), + sys_ros_pub.get_input_port(0) + ) + + diagram = builder.Build() + simulator = Simulator(diagram) + simulator_context = simulator.get_mutable_context() + simulator.set_target_realtime_rate(1.0) + + while True: + simulator.AdvanceTo(simulator_context.get_time() + 0.1) + + +if __name__ == '__main__': + main() diff --git a/drake_ros_systems/src/python_bindings/module_drake_ros_systems.cpp b/drake_ros_systems/src/python_bindings/module_drake_ros_systems.cpp new file mode 100644 index 0000000..3fd8cc5 --- /dev/null +++ b/drake_ros_systems/src/python_bindings/module_drake_ros_systems.cpp @@ -0,0 +1,61 @@ +#include + +#include + +#include + +#include "py_ros_interface_system.hpp" +#include "py_serializer.hpp" + +namespace py = pybind11; + +using drake::systems::LeafSystem; + +using drake_ros_systems::DrakeRos; +using drake_ros_systems::DrakeRosInterface; +using drake_ros_systems::PySerializer; +using drake_ros_systems::RosInterfaceSystem; +using drake_ros_systems::RosPublisherSystem; +using drake_ros_systems::SerializerInterface; + + +PYBIND11_MODULE(drake_ros_systems, m) { + m.doc() = "Python wrapper for drake_ros_systems"; + + py::module::import("pydrake.systems.framework"); + + // Use std::shared_ptr holder so pybind11 doesn't try to delete interfaces returned from + // get_ros_interface + py::class_>(m, "DrakeRosInterface"); + + py::class_>(m, "RosInterfaceSystem") + .def(py::init([](){ + return std::make_unique(std::make_unique());})) + .def("get_ros_interface", &RosInterfaceSystem::get_ros_interface); + + py::class_>(m, "RosPublisherSystem") + .def(py::init([]( + pybind11::object type, + const char * topic_name, + std::shared_ptr ros_interface) + { + std::unique_ptr serializer = std::make_unique(type); + return std::make_unique( + serializer, + topic_name, + rclcpp::QoS(10), // TODO(sloretz) Custom cast for rclpy.QoSProfile <--> rclcpp::Qos + ros_interface); + })); + + + /* + py::class_(m, "ROS") + .def(py::init()) + .def("spin", &rccl::ROS::spin); + + py::class_(m, "PingPong") + .def(py::init()) + .def("ping", &rccl::PingPong::ping, + py::arg("msg") = "C++ ping"); + */ +} diff --git a/drake_ros_systems/src/python_bindings/py_serializer.hpp b/drake_ros_systems/src/python_bindings/py_serializer.hpp new file mode 100644 index 0000000..6c9026d --- /dev/null +++ b/drake_ros_systems/src/python_bindings/py_serializer.hpp @@ -0,0 +1,157 @@ +#ifndef PY_SERIALIZER_HPP_ +#define PY_SERIALIZER_HPP_ + +#include +#include +#include + +#include + +#include + +namespace py = pybind11; + +namespace drake_ros_systems +{ +// Serialize/Deserialize Python ROS types to rclcpp::SerializedMessage +class PySerializer : public SerializerInterface +{ +public: + PySerializer(py::object message_type) + : message_type_(message_type) + { + // Check if message_type.__class__._TYPE_SUPPORT is None, + py::object metaclass = message_type.attr("__class__"); + if (metaclass.attr("_TYPE_SUPPORT").is_none()) { + // call message_type.__class__.__import_type_support__() + metaclass.attr("__import_type_support__")(); + } + // Get type support capsule and pointer + auto typesupport = py::cast(metaclass.attr("_TYPE_SUPPORT")); + // TODO(sloretz) use get_pointer() in py 2.6+ + type_support_ = static_cast(typesupport); + + auto convert_from_py = + py::cast(metaclass.attr("_CONVERT_FROM_PY")); + // TODO(sloretz) use get_pointer() in py 2.6+ + convert_from_py_ = reinterpret_cast( + static_cast(convert_from_py)); + + auto convert_to_py = + py::cast(metaclass.attr("_CONVERT_TO_PY")); + // TODO(sloretz) use get_pointer() in py 2.6+ + convert_to_py_ = reinterpret_cast( + static_cast(convert_to_py)); + + auto create_ros_message = + py::cast(metaclass.attr("_CREATE_ROS_MESSAGE")); + // TODO(sloretz) use get_pointer() in py 2.6+ + create_ros_message_ = reinterpret_cast( + static_cast(create_ros_message)); + + auto destroy_ros_message = + py::cast(metaclass.attr("_DESTROY_ROS_MESSAGE")); + // TODO(sloretz) use get_pointer() in py 2.6+ + destroy_ros_message_ = reinterpret_cast( + static_cast(destroy_ros_message)); + } + + rclcpp::SerializedMessage + serialize(const drake::AbstractValue & abstract_value) const override + { + // convert from inaccessible drake::pydrake::Object type + py::dict scope; + scope["av"] = &abstract_value; + py::object message = py::eval("av.Clone().get_mutable_value()", scope); + + // Create C ROS message + auto c_ros_message = std::unique_ptr( + create_ros_message_(), destroy_ros_message_); + + // Convert the Python message to a C ROS message + if (!convert_from_py_(message.ptr(), c_ros_message.get())) { + // TODO(sloretz) Error handling? Throw? + return rclcpp::SerializedMessage(); + } + + // Serialize the C message + rclcpp::SerializedMessage serialized_msg; + const auto ret = rmw_serialize( + c_ros_message.get(), + type_support_, + &serialized_msg.get_rcl_serialized_message()); + if (ret != RMW_RET_OK) { + // TODO(sloretz) do something if serialization fails + return rclcpp::SerializedMessage(); + } + return serialized_msg; + } + + bool + deserialize( + const rclcpp::SerializedMessage & serialized_message, + drake::AbstractValue & abstract_value) const override + { + // TODO(sloretz) it would be so much more convenient if I didn't have to + // care that the Python typesupport used the C type support internally. + // Why isn't this encapsulated in the python type support itself? + + // Create a C type support version of the message + auto c_ros_message = std::unique_ptr( + create_ros_message_(), destroy_ros_message_); + if (nullptr == c_ros_message.get()) { + return false; + } + + // Deserialize to C message type + const auto ret = rmw_deserialize( + &serialized_message.get_rcl_serialized_message(), + type_support_, + c_ros_message.get()); + + if (RMW_RET_OK != ret) { + return false; + } + + // Convert C type to Python type + PyObject * pymessage = convert_to_py_(c_ros_message.get()); + if (!pymessage) { + return false; + } + + // Store the Python message in the AbstractValue + // TODO use python code to get access to drake::pydrake::Object type + abstract_value.get_mutable_value() = + py::cast(pymessage); + + return true; + } + + std::unique_ptr + create_default_value() const override + { + // convert to inaccessible drake::pydrake::Object type + py::object scope = py::module::import("pydrake.common.value").attr("__dict__"); + py::object lambda = py::eval("lambda msg: AbstractValue.Make(msg)", scope); + py::object result = lambda(message_type_()); + + return py::cast>(result); + } + + const rosidl_message_type_support_t * + get_type_support() const override + { + return type_support_; + } + +private: + py::object message_type_; + rosidl_message_type_support_t * type_support_; + + bool (*convert_from_py_)(PyObject *, void *); + PyObject * (*convert_to_py_)(void *); + void * (*create_ros_message_)(void); + void (*destroy_ros_message_)(void *); +}; +} // namespace drake_ros_systems +#endif // PY_SERIALIZER_HPP_ From 26661f0e63825638c3568595b4918b1545917a80 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 2 Dec 2020 11:23:26 -0800 Subject: [PATCH 13/39] Fix includes for DrakeRosInterface and RosInterfaceSystem Signed-off-by: Shane Loretz --- .../src/python_bindings/module_drake_ros_systems.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/drake_ros_systems/src/python_bindings/module_drake_ros_systems.cpp b/drake_ros_systems/src/python_bindings/module_drake_ros_systems.cpp index 3fd8cc5..e8190b7 100644 --- a/drake_ros_systems/src/python_bindings/module_drake_ros_systems.cpp +++ b/drake_ros_systems/src/python_bindings/module_drake_ros_systems.cpp @@ -2,9 +2,10 @@ #include +#include +#include #include -#include "py_ros_interface_system.hpp" #include "py_serializer.hpp" namespace py = pybind11; From 944b2e6e5424f8c0937dca2828ddfc41735ee4e9 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 2 Dec 2020 14:41:32 -0800 Subject: [PATCH 14/39] ROS Subscriber works in Python Signed-off-by: Shane Loretz --- drake_ros_systems/example/rs_flip_flop.py | 8 ++++++++ .../python_bindings/module_drake_ros_systems.cpp | 15 +++++++++++++++ .../src/python_bindings/py_serializer.hpp | 8 +++++--- 3 files changed, 28 insertions(+), 3 deletions(-) diff --git a/drake_ros_systems/example/rs_flip_flop.py b/drake_ros_systems/example/rs_flip_flop.py index acf763a..7f8cd63 100755 --- a/drake_ros_systems/example/rs_flip_flop.py +++ b/drake_ros_systems/example/rs_flip_flop.py @@ -2,6 +2,7 @@ from drake_ros_systems import RosInterfaceSystem from drake_ros_systems import RosPublisherSystem +from drake_ros_systems import RosSubscriberSystem from pydrake.systems.analysis import Simulator from pydrake.systems.framework import DiagramBuilder @@ -68,6 +69,13 @@ def main(): sys_ros_pub.get_input_port(0) ) + sys_ros_sub_pt = builder.AddSystem(RosSubscriberSystem(String, "input", sys_ros_interface.get_ros_interface())); + sys_ros_pub_pt = builder.AddSystem(RosPublisherSystem(String, "output", sys_ros_interface.get_ros_interface())); + builder.Connect( + sys_ros_sub_pt.get_output_port(0), + sys_ros_pub_pt.get_input_port(0) + ) + diagram = builder.Build() simulator = Simulator(diagram) simulator_context = simulator.get_mutable_context() diff --git a/drake_ros_systems/src/python_bindings/module_drake_ros_systems.cpp b/drake_ros_systems/src/python_bindings/module_drake_ros_systems.cpp index e8190b7..8bd4b17 100644 --- a/drake_ros_systems/src/python_bindings/module_drake_ros_systems.cpp +++ b/drake_ros_systems/src/python_bindings/module_drake_ros_systems.cpp @@ -5,6 +5,7 @@ #include #include #include +#include #include "py_serializer.hpp" @@ -17,6 +18,7 @@ using drake_ros_systems::DrakeRosInterface; using drake_ros_systems::PySerializer; using drake_ros_systems::RosInterfaceSystem; using drake_ros_systems::RosPublisherSystem; +using drake_ros_systems::RosSubscriberSystem; using drake_ros_systems::SerializerInterface; @@ -48,6 +50,19 @@ PYBIND11_MODULE(drake_ros_systems, m) { ros_interface); })); + py::class_>(m, "RosSubscriberSystem") + .def(py::init([]( + pybind11::object type, + const char * topic_name, + std::shared_ptr ros_interface) + { + std::unique_ptr serializer = std::make_unique(type); + return std::make_unique( + serializer, + topic_name, + rclcpp::QoS(10), // TODO(sloretz) Custom cast for rclpy.QoSProfile <--> rclcpp::Qos + ros_interface); + })); /* py::class_(m, "ROS") diff --git a/drake_ros_systems/src/python_bindings/py_serializer.hpp b/drake_ros_systems/src/python_bindings/py_serializer.hpp index 6c9026d..7b4a96c 100644 --- a/drake_ros_systems/src/python_bindings/py_serializer.hpp +++ b/drake_ros_systems/src/python_bindings/py_serializer.hpp @@ -120,9 +120,11 @@ class PySerializer : public SerializerInterface } // Store the Python message in the AbstractValue - // TODO use python code to get access to drake::pydrake::Object type - abstract_value.get_mutable_value() = - py::cast(pymessage); + // convert to inaccessible drake::pydrake::Object type + py::dict scope; + scope["av"] = &abstract_value; + scope["message"] = pymessage; + py::exec("av.set_value(message)", scope); return true; } From df079cf74e57788b68268d422490844e2f7a0d66 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 2 Dec 2020 16:19:27 -0800 Subject: [PATCH 15/39] Implement flip-flop example in Python Signed-off-by: Shane Loretz --- drake_ros_systems/example/rs_flip_flop.py | 121 +++++++++++++++------- 1 file changed, 82 insertions(+), 39 deletions(-) diff --git a/drake_ros_systems/example/rs_flip_flop.py b/drake_ros_systems/example/rs_flip_flop.py index 7f8cd63..6dfbfe0 100755 --- a/drake_ros_systems/example/rs_flip_flop.py +++ b/drake_ros_systems/example/rs_flip_flop.py @@ -6,74 +6,117 @@ from pydrake.systems.analysis import Simulator from pydrake.systems.framework import DiagramBuilder -# from pydrake.systems.lcm import LcmInterfaceSystem -# from pydrake.lcm import DrakeLcm +from pydrake.systems.framework import UnrestrictedUpdateEvent from pydrake.common.value import AbstractValue from pydrake.systems.framework import LeafSystem from std_msgs.msg import String +from std_msgs.msg import Bool -def traced(func, ignoredirs=None): - """Decorates func such that its execution is traced, but filters out any - Python code outside of the system prefix.""" - import functools - import sys - import trace - if ignoredirs is None: - ignoredirs = ["/usr", sys.prefix] - tracer = trace.Trace(trace=1, count=0, ignoredirs=ignoredirs) +class NorGate(LeafSystem): - @functools.wraps(func) - def wrapped(*args, **kwargs): - return tracer.runfunc(func, *args, **kwargs) + def __init__(self): + super().__init__() + self._a = self.DeclareAbstractInputPort("A", AbstractValue.Make(Bool())) + self._b = self.DeclareAbstractInputPort("B", AbstractValue.Make(Bool())) + + self.DeclareAbstractOutputPort( + 'Q', + lambda: AbstractValue.Make(Bool()), + self._calc_output_value) - return wrapped + def _calc_output_value(self, context, data): + a = self._a.Eval(context) + b = self._b.Eval(context) + data.get_mutable_value().data = not (a.data or b.data) -class HelloWorld(LeafSystem): - """Outputs Hello World bool message.""" +class Memory(LeafSystem): + """Delay input port by one time step to avoid algebraic loop error.""" - def __init__(self): + def __init__(self, initial_value): super().__init__() + self._input = self.DeclareAbstractInputPort("A", AbstractValue.Make(initial_value)) + + self.DeclareAbstractState(AbstractValue.Make(initial_value)) + self.DeclareAbstractOutputPort( - 'text', - lambda: AbstractValue.Make(String()), - self._do_output_text) + 'Q', + lambda: AbstractValue.Make(initial_value), + self._calc_output_value, + {self.all_state_ticket()}) + + self.DeclarePerStepEvent(UnrestrictedUpdateEvent(self._move_input_to_state)) + + def _move_input_to_state(self, context, event, state): + state.get_mutable_abstract_state().get_mutable_value(0).SetFrom( + AbstractValue.Make(self._input.Eval(context))) - def _do_output_text(self, context, data): - data.get_mutable_value().data = "hello world" + def _calc_output_value(self, context, output): + output.SetFrom(context.get_abstract_state().get_value(0)) -# @traced def main(): + # NOR gate RS flip flop example + # Input topics /S and /R are active high (true is logic 1 and false is logic 0) + # Output topics /Q and /Q_not are active low (true is logic 0 and false is logic 1) + + # Input/Output table + # S: false R: false | Q: no change Q_not: no change + # S: true R: false | Q: false Q_not: true + # S: false R: true | Q: true Q_not: false + # S: true R: true | Q: invalid Q_not: invalid builder = DiagramBuilder() sys_ros_interface = builder.AddSystem(RosInterfaceSystem()) - # can add publisher systems - # Need bindings for subscriber system - # Need python version of memory class - # Need python version of NAND gate class - # Need boilerplate to create system + sys_pub_Q = builder.AddSystem( + RosPublisherSystem(Bool, "Q", sys_ros_interface.get_ros_interface())) + sys_pub_Q_not = builder.AddSystem( + RosPublisherSystem(Bool, "Q_not", sys_ros_interface.get_ros_interface())) + + sys_sub_S = builder.AddSystem( + RosSubscriberSystem(Bool, "S", sys_ros_interface.get_ros_interface())) + sys_sub_R = builder.AddSystem( + RosSubscriberSystem(Bool, "R", sys_ros_interface.get_ros_interface())) + + sys_nor_gate_1 = builder.AddSystem(NorGate()) + sys_nor_gate_2 = builder.AddSystem(NorGate()) - # rps = RosPublisherSystem(String, "asdf", ris.get_ros_interface()) + sys_memory = builder.AddSystem(Memory(Bool())) - sys_hello_world = builder.AddSystem(HelloWorld()) - sys_ros_pub = builder.AddSystem( - RosPublisherSystem(String, "asdf", sys_ros_interface.get_ros_interface())) + builder.Connect( + sys_nor_gate_1.GetOutputPort('Q'), + sys_memory.get_input_port(0) + ) builder.Connect( - sys_hello_world.get_output_port(0), - sys_ros_pub.get_input_port(0) + sys_sub_S.get_output_port(0), + sys_nor_gate_1.GetInputPort('A'), + ) + builder.Connect( + sys_nor_gate_2.GetOutputPort('Q'), + sys_nor_gate_1.GetInputPort('B'), ) - sys_ros_sub_pt = builder.AddSystem(RosSubscriberSystem(String, "input", sys_ros_interface.get_ros_interface())); - sys_ros_pub_pt = builder.AddSystem(RosPublisherSystem(String, "output", sys_ros_interface.get_ros_interface())); builder.Connect( - sys_ros_sub_pt.get_output_port(0), - sys_ros_pub_pt.get_input_port(0) + sys_memory.get_output_port(0), + sys_nor_gate_2.GetInputPort('A'), + ) + builder.Connect( + sys_sub_R.get_output_port(0), + sys_nor_gate_2.GetInputPort('B'), + ) + + builder.Connect( + sys_nor_gate_1.GetOutputPort('Q'), + sys_pub_Q.get_input_port(0) + ) + builder.Connect( + sys_nor_gate_2.GetOutputPort('Q'), + sys_pub_Q_not.get_input_port(0) ) diagram = builder.Build() From 9283a5fb1b13d773604d2bf96809717c76e38753 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 2 Dec 2020 18:47:20 -0800 Subject: [PATCH 16/39] Update README for C++ or Python example Signed-off-by: Shane Loretz --- drake_ros_systems/README.md | 100 ++++++++++++++++++++---------------- 1 file changed, 56 insertions(+), 44 deletions(-) diff --git a/drake_ros_systems/README.md b/drake_ros_systems/README.md index 3637476..5125081 100644 --- a/drake_ros_systems/README.md +++ b/drake_ros_systems/README.md @@ -4,59 +4,71 @@ This is a ROS 2 prototype of a solution to [robotlocomotion/Drake#9500](https:// It is similar to this ROS 1 prototype [`gizatt/drake_ros_systems`](https://github.com/gizatt/drake_ros_systems). It explores ROS 2 equivalents of `LcmInterfaceSystem`, `LcmPublisherSystem`, and `LcmSubscriberSystem`. -# Code examples +# Building + +This package has been built and tested on Ubuntu Focal with ROS Rolling, using a Drake nightly from November 2020. +It may work on other versions of ROS and Drake, but it hasn't been tested. + +To build it: + +1. [Install ROS Rolling](https://index.ros.org/doc/ros2/Installation/Rolling/) +1. Source your ROS installation `. /opt/ros/rolling/setup.bash` +* [Install drake from November-ish 2020](https://drake.mit.edu/from_binary.html) +1. Extract the Drake binary installation, install it's prerequisites, and [use this Python virutalenv trick](https://drake.mit.edu/python_bindings.html#inside-virtualenv). +1. Activate the drake virtual environment +1. Build it using Colcon, or using CMake directly + **Colcon** + 1. Make a workspace `mkdir -p ./ws/src` + 1. `cd ./ws/src` + 1. Get this code `git clone https://github.com/sloretz/drake_ros2_demos.git` + 1. `cd ..` + 1. Build this package `colcon build --packages-select drake_ros_systems` + **CMake** + 1. Manually set `CMAKE_PREFIX_PATH`: `export CMAKE_PREFIX_PATH=$CMAKE_PREFIX_PATH:$AMENT_PREFIX_PATH` + 1. Get this code `git clone https://github.com/sloretz/drake_ros2_demos.git` + 1. `cd drake_ros2_demos/drake_ros_systems` + 1. Make a build and install folder to avoid installing to the whole system `mkdir build install` + 1. `cd build` + 1. Configure the project `cmake -DCMAKE_INSTALL_PREFIX=$(pwd)/../install ..` + 1. Build the project `make && make install` + +# Running the Example + +An example of using these systems is given in the [`example`](./example) folder in two languages: Python and C++. + +If you built with `colcon`, then source your workspace. -Create a system that publishes `std_msgs::msg::String`. - -```C++ -#include -#include - -using drake_ros_systems::RosInterfaceSystem; -using drake_ros_systems::RosPublisherSystem; - -// This system initializes ROS and calls spin() -// It creates a single ROS node called "drake_ros". -auto spin_system = RosInterfaceSystem::Make(/* optionally rclcpp init options? */); - -const std::string topic{"chatter"}; -const double period_sec{0.1}; // 10Hz - -auto pub_system = RosPublisherSystem::Make( - spin_system.get_ros_interface(), topic, period_sec); - -auto pub_context = pub_system->CreateDefaultContext(); -std_msgs::msg::String pub_msg; -pub_msg.data = "Hello from Drake"; -pub_context->FixInputPort(0, AbstractValue::Make(pub_msg)); -pub_system->Publish(*pub_context); +``` +. ./ws/install/setup.bash +# Also make sure to activate drake virtual environment ``` -Create a system that subscribes to `std_msgs::msg::String`. - -```C++ -#include -#include +If you built with plain CMake, then source the ROS workspace and set these variables. -using drake_ros_systems::RosInterfaceSystem; -using drake_ros_systems::RosSubscriberSystem; +``` +. /opt/ros/rolling/setup.bash +# Also make sure to activate drake virtual environment +# CD to folder containing `build` and `install` created earlier, commands below use PWD to find correct path +export LD_LIBRARY_PATH=$(pwd)/install/lib:$LD_LIBRARY_PATH +export PYTHONPATH=$(pwd)/install/lib/$(python -c 'import sys; print(f"python{sys.version_info[0]}.{sys.version_info[1]}")')/site-packages:$PYTHONPATH +``` -// This system initializes ROS and calls spin() -// It creates a single ROS node called "drake_ros". -auto spin_system = RosInterfaceSystem(/* optionally rclcpp init options? */); +Now you can run the C++ example from the build folder -const std::string topic{"chatter"}; +If built with **colcon** using an isolated build (default) -auto pub_system = RosSubscriberSystem::Make( - spin_system.get_ros_interface(), topic, period_sec); +``` +./build/drake_ros_systems/example/rs_flip_flop +``` -auto sub_context = sub_system->CreateDefaultContext(); -// somehow this sub context is added to a diagram builder with the system -// so the subscriber can update that message +If built with **cmake** or a non-isolated build of **colcon** -// huh...? -std_msgs::msg::String sub_msg = sub_context->get_output_stuff(...); +``` +./build/example/rs_flip_flop ``` -Could use an example of drake systems built with a diagram builder and connected to input/output ports of other systems. +The Python example can be run from the source folder. +``` +python3 ./example/rs_flip_flop.py +``` From 3460dd23106c7ab3e658e8ad48a5a5a234acaaa9 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 2 Dec 2020 18:48:41 -0800 Subject: [PATCH 17/39] Try more whitespace Signed-off-by: Shane Loretz --- drake_ros_systems/README.md | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/drake_ros_systems/README.md b/drake_ros_systems/README.md index 5125081..cea77c2 100644 --- a/drake_ros_systems/README.md +++ b/drake_ros_systems/README.md @@ -17,12 +17,14 @@ To build it: 1. Extract the Drake binary installation, install it's prerequisites, and [use this Python virutalenv trick](https://drake.mit.edu/python_bindings.html#inside-virtualenv). 1. Activate the drake virtual environment 1. Build it using Colcon, or using CMake directly + **Colcon** 1. Make a workspace `mkdir -p ./ws/src` 1. `cd ./ws/src` 1. Get this code `git clone https://github.com/sloretz/drake_ros2_demos.git` 1. `cd ..` 1. Build this package `colcon build --packages-select drake_ros_systems` + **CMake** 1. Manually set `CMAKE_PREFIX_PATH`: `export CMAKE_PREFIX_PATH=$CMAKE_PREFIX_PATH:$AMENT_PREFIX_PATH` 1. Get this code `git clone https://github.com/sloretz/drake_ros2_demos.git` @@ -67,7 +69,7 @@ If built with **cmake** or a non-isolated build of **colcon** ./build/example/rs_flip_flop ``` -The Python example can be run from the source folder. +The Python example can be run from the source folder in either case. ``` python3 ./example/rs_flip_flop.py From 4ce1db903428b2cff3a38b6845fb3a5e0baae2be Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 2 Dec 2020 18:49:24 -0800 Subject: [PATCH 18/39] Bullet to numbered list Signed-off-by: Shane Loretz --- drake_ros_systems/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/drake_ros_systems/README.md b/drake_ros_systems/README.md index cea77c2..e78f13d 100644 --- a/drake_ros_systems/README.md +++ b/drake_ros_systems/README.md @@ -13,7 +13,7 @@ To build it: 1. [Install ROS Rolling](https://index.ros.org/doc/ros2/Installation/Rolling/) 1. Source your ROS installation `. /opt/ros/rolling/setup.bash` -* [Install drake from November-ish 2020](https://drake.mit.edu/from_binary.html) +1. [Install drake from November-ish 2020](https://drake.mit.edu/from_binary.html) 1. Extract the Drake binary installation, install it's prerequisites, and [use this Python virutalenv trick](https://drake.mit.edu/python_bindings.html#inside-virtualenv). 1. Activate the drake virtual environment 1. Build it using Colcon, or using CMake directly From 8449db305aea5fb183034e743350bc1d96566e26 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 2 Dec 2020 18:53:31 -0800 Subject: [PATCH 19/39] Commands to play with the example Signed-off-by: Shane Loretz --- drake_ros_systems/README.md | 36 +++++++++++++++++++++++++++++++++++- 1 file changed, 35 insertions(+), 1 deletion(-) diff --git a/drake_ros_systems/README.md b/drake_ros_systems/README.md index e78f13d..231f837 100644 --- a/drake_ros_systems/README.md +++ b/drake_ros_systems/README.md @@ -34,9 +34,43 @@ To build it: 1. Configure the project `cmake -DCMAKE_INSTALL_PREFIX=$(pwd)/../install ..` 1. Build the project `make && make install` -# Running the Example +# Example An example of using these systems is given in the [`example`](./example) folder in two languages: Python and C++. +Both examples implement an RS flip flop using NOR gates. +They subscribe to the following topics: + +* `/R` +* `/S` + +And publish to the following topics + +* `/Q` +* `/Q_not` + +Run these commands in different terminals with your ROS installation sourced to echo the output topics: + +``` +ros2 topic echo /Q +``` + +``` +ros2 topic echo /Q_not +``` + +Run these commands in different terminals with your ROS installation sourced to play with the input topics. + +``` +ros2 topic pub /S std_msgs/msg/Bool "data: false" +ros2 topic pub /S std_msgs/msg/Bool "data: true" +``` + +``` +ros2 topic pub /R std_msgs/msg/Bool "data: false" +ros2 topic pub /R std_msgs/msg/Bool "data: true" +``` + +## Running the Example If you built with `colcon`, then source your workspace. From ec6e3b951e50c299c6be901f162469cc4f69fd66 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Mon, 14 Dec 2020 15:01:55 -0800 Subject: [PATCH 20/39] Blank line at EOF Signed-off-by: Shane Loretz --- drake_ros_systems/CMakeLists.txt | 1 - 1 file changed, 1 deletion(-) diff --git a/drake_ros_systems/CMakeLists.txt b/drake_ros_systems/CMakeLists.txt index e3caf42..1cc7b0a 100644 --- a/drake_ros_systems/CMakeLists.txt +++ b/drake_ros_systems/CMakeLists.txt @@ -86,4 +86,3 @@ install( ### End Python bindings ament_package() - From ae20bb10f83d74613f5ffb98f0585750b0e45b36 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Mon, 14 Dec 2020 15:24:38 -0800 Subject: [PATCH 21/39] Add linter tests Signed-off-by: Shane Loretz --- drake_ros_systems/CMakeLists.txt | 5 +++++ drake_ros_systems/package.xml | 3 +++ 2 files changed, 8 insertions(+) diff --git a/drake_ros_systems/CMakeLists.txt b/drake_ros_systems/CMakeLists.txt index 1cc7b0a..c6a1c45 100644 --- a/drake_ros_systems/CMakeLists.txt +++ b/drake_ros_systems/CMakeLists.txt @@ -85,4 +85,9 @@ install( ) ### End Python bindings +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + ament_package() diff --git a/drake_ros_systems/package.xml b/drake_ros_systems/package.xml index a7d9593..180c83b 100644 --- a/drake_ros_systems/package.xml +++ b/drake_ros_systems/package.xml @@ -10,6 +10,9 @@ rosidl_runtime_c rosidl_typesupport_cpp + ament_lint_auto + ament_lint_common + cmake From 00da7c7dbeac3db32d2698299b488283c2407abf Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Mon, 14 Dec 2020 15:30:47 -0800 Subject: [PATCH 22/39] Add LICENSE file and header comments Signed-off-by: Shane Loretz --- LICENSE | 202 ++++++++++++++++++ drake_ros_systems/example/rs_flip_flop.cpp | 13 ++ drake_ros_systems/example/rs_flip_flop.py | 13 ++ .../include/drake_ros_systems/drake_ros.hpp | 13 ++ .../drake_ros_systems/drake_ros_interface.hpp | 13 ++ .../ros_interface_system.hpp | 13 ++ .../ros_publisher_system.hpp | 13 ++ .../ros_subscriber_system.hpp | 13 ++ .../include/drake_ros_systems/serializer.hpp | 13 ++ .../serializer_interface.hpp | 13 ++ drake_ros_systems/src/drake_ros.cpp | 13 ++ drake_ros_systems/src/publisher.cpp | 13 ++ drake_ros_systems/src/publisher.hpp | 13 ++ .../module_drake_ros_systems.cpp | 13 ++ .../src/python_bindings/py_serializer.hpp | 13 ++ .../src/ros_interface_system.cpp | 13 ++ .../src/ros_publisher_system.cpp | 13 ++ .../src/ros_subscriber_system.cpp | 13 ++ drake_ros_systems/src/subscription.cpp | 13 ++ drake_ros_systems/src/subscription.hpp | 13 ++ 20 files changed, 449 insertions(+) create mode 100644 LICENSE diff --git a/LICENSE b/LICENSE new file mode 100644 index 0000000..d645695 --- /dev/null +++ b/LICENSE @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/drake_ros_systems/example/rs_flip_flop.cpp b/drake_ros_systems/example/rs_flip_flop.cpp index 7c33002..a8c3b34 100644 --- a/drake_ros_systems/example/rs_flip_flop.cpp +++ b/drake_ros_systems/example/rs_flip_flop.cpp @@ -1,3 +1,16 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #include #include #include diff --git a/drake_ros_systems/example/rs_flip_flop.py b/drake_ros_systems/example/rs_flip_flop.py index 6dfbfe0..5e2b3fd 100755 --- a/drake_ros_systems/example/rs_flip_flop.py +++ b/drake_ros_systems/example/rs_flip_flop.py @@ -1,4 +1,17 @@ #!/usr/bin/env python3 +# Copyright 2020 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. from drake_ros_systems import RosInterfaceSystem from drake_ros_systems import RosPublisherSystem diff --git a/drake_ros_systems/include/drake_ros_systems/drake_ros.hpp b/drake_ros_systems/include/drake_ros_systems/drake_ros.hpp index 02ea012..503245b 100644 --- a/drake_ros_systems/include/drake_ros_systems/drake_ros.hpp +++ b/drake_ros_systems/include/drake_ros_systems/drake_ros.hpp @@ -1,3 +1,16 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #ifndef DRAKE_ROS_SYSTEMS__DRAKE_ROS_HPP_ #define DRAKE_ROS_SYSTEMS__DRAKE_ROS_HPP_ diff --git a/drake_ros_systems/include/drake_ros_systems/drake_ros_interface.hpp b/drake_ros_systems/include/drake_ros_systems/drake_ros_interface.hpp index c86ef60..7504724 100644 --- a/drake_ros_systems/include/drake_ros_systems/drake_ros_interface.hpp +++ b/drake_ros_systems/include/drake_ros_systems/drake_ros_interface.hpp @@ -1,3 +1,16 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #ifndef DRAKE_ROS_SYSTEMS__DRAKE_ROS_INTERFACE_HPP_ #define DRAKE_ROS_SYSTEMS__DRAKE_ROS_INTERFACE_HPP_ diff --git a/drake_ros_systems/include/drake_ros_systems/ros_interface_system.hpp b/drake_ros_systems/include/drake_ros_systems/ros_interface_system.hpp index 1eef43e..b4a35c7 100644 --- a/drake_ros_systems/include/drake_ros_systems/ros_interface_system.hpp +++ b/drake_ros_systems/include/drake_ros_systems/ros_interface_system.hpp @@ -1,3 +1,16 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #ifndef DRAKE_ROS_SYSTEMS__ROS_INTERFACE_SYSTEM_HPP_ #define DRAKE_ROS_SYSTEMS__ROS_INTERFACE_SYSTEM_HPP_ diff --git a/drake_ros_systems/include/drake_ros_systems/ros_publisher_system.hpp b/drake_ros_systems/include/drake_ros_systems/ros_publisher_system.hpp index 03f19b1..fc5ca11 100644 --- a/drake_ros_systems/include/drake_ros_systems/ros_publisher_system.hpp +++ b/drake_ros_systems/include/drake_ros_systems/ros_publisher_system.hpp @@ -1,3 +1,16 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #ifndef DRAKE_ROS_SYSTEMS__ROS_PUBLISHER_SYSTEM_HPP_ #define DRAKE_ROS_SYSTEMS__ROS_PUBLISHER_SYSTEM_HPP_ diff --git a/drake_ros_systems/include/drake_ros_systems/ros_subscriber_system.hpp b/drake_ros_systems/include/drake_ros_systems/ros_subscriber_system.hpp index 2e3c008..5aac2be 100644 --- a/drake_ros_systems/include/drake_ros_systems/ros_subscriber_system.hpp +++ b/drake_ros_systems/include/drake_ros_systems/ros_subscriber_system.hpp @@ -1,3 +1,16 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #ifndef DRAKE_ROS_SYSTEMS__ROS_SUBSCRIBER_SYSTEM_HPP_ #define DRAKE_ROS_SYSTEMS__ROS_SUBSCRIBER_SYSTEM_HPP_ diff --git a/drake_ros_systems/include/drake_ros_systems/serializer.hpp b/drake_ros_systems/include/drake_ros_systems/serializer.hpp index a370e98..8745891 100644 --- a/drake_ros_systems/include/drake_ros_systems/serializer.hpp +++ b/drake_ros_systems/include/drake_ros_systems/serializer.hpp @@ -1,3 +1,16 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #ifndef DRAKE_ROS_SYSTEMS__SERIALIZER_HPP_ #define DRAKE_ROS_SYSTEMS__SERIALIZER_HPP_ diff --git a/drake_ros_systems/include/drake_ros_systems/serializer_interface.hpp b/drake_ros_systems/include/drake_ros_systems/serializer_interface.hpp index a9df7e5..67102a9 100644 --- a/drake_ros_systems/include/drake_ros_systems/serializer_interface.hpp +++ b/drake_ros_systems/include/drake_ros_systems/serializer_interface.hpp @@ -1,3 +1,16 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #ifndef DRAKE_ROS_SYSTEMS__SERIALIZER_INTERFACE_HPP_ #define DRAKE_ROS_SYSTEMS__SERIALIZER_INTERFACE_HPP_ diff --git a/drake_ros_systems/src/drake_ros.cpp b/drake_ros_systems/src/drake_ros.cpp index 2cbf31f..0db52af 100644 --- a/drake_ros_systems/src/drake_ros.cpp +++ b/drake_ros_systems/src/drake_ros.cpp @@ -1,3 +1,16 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #include #include "publisher.hpp" #include "subscription.hpp" diff --git a/drake_ros_systems/src/publisher.cpp b/drake_ros_systems/src/publisher.cpp index 1a67a6e..8a7c9af 100644 --- a/drake_ros_systems/src/publisher.cpp +++ b/drake_ros_systems/src/publisher.cpp @@ -1,3 +1,16 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #include "publisher.hpp" namespace drake_ros_systems diff --git a/drake_ros_systems/src/publisher.hpp b/drake_ros_systems/src/publisher.hpp index 366d47f..0009f45 100644 --- a/drake_ros_systems/src/publisher.hpp +++ b/drake_ros_systems/src/publisher.hpp @@ -1,3 +1,16 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #ifndef DRAKE_ROS_SYSTEMS__PUBLISHER_HPP_ #define DRAKE_ROS_SYSTEMS__PUBLISHER_HPP_ diff --git a/drake_ros_systems/src/python_bindings/module_drake_ros_systems.cpp b/drake_ros_systems/src/python_bindings/module_drake_ros_systems.cpp index 8bd4b17..ad22d87 100644 --- a/drake_ros_systems/src/python_bindings/module_drake_ros_systems.cpp +++ b/drake_ros_systems/src/python_bindings/module_drake_ros_systems.cpp @@ -1,3 +1,16 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #include #include diff --git a/drake_ros_systems/src/python_bindings/py_serializer.hpp b/drake_ros_systems/src/python_bindings/py_serializer.hpp index 7b4a96c..4097ec8 100644 --- a/drake_ros_systems/src/python_bindings/py_serializer.hpp +++ b/drake_ros_systems/src/python_bindings/py_serializer.hpp @@ -1,3 +1,16 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #ifndef PY_SERIALIZER_HPP_ #define PY_SERIALIZER_HPP_ diff --git a/drake_ros_systems/src/ros_interface_system.cpp b/drake_ros_systems/src/ros_interface_system.cpp index a9a27db..00e0dff 100644 --- a/drake_ros_systems/src/ros_interface_system.cpp +++ b/drake_ros_systems/src/ros_interface_system.cpp @@ -1,3 +1,16 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #include namespace drake_ros_systems diff --git a/drake_ros_systems/src/ros_publisher_system.cpp b/drake_ros_systems/src/ros_publisher_system.cpp index 176fdce..5895e55 100644 --- a/drake_ros_systems/src/ros_publisher_system.cpp +++ b/drake_ros_systems/src/ros_publisher_system.cpp @@ -1,3 +1,16 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #include #include diff --git a/drake_ros_systems/src/ros_subscriber_system.cpp b/drake_ros_systems/src/ros_subscriber_system.cpp index 02b43cc..cecaf0f 100644 --- a/drake_ros_systems/src/ros_subscriber_system.cpp +++ b/drake_ros_systems/src/ros_subscriber_system.cpp @@ -1,3 +1,16 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #include #include diff --git a/drake_ros_systems/src/subscription.cpp b/drake_ros_systems/src/subscription.cpp index e70d778..d0ed88d 100644 --- a/drake_ros_systems/src/subscription.cpp +++ b/drake_ros_systems/src/subscription.cpp @@ -1,3 +1,16 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #include "subscription.hpp" namespace drake_ros_systems diff --git a/drake_ros_systems/src/subscription.hpp b/drake_ros_systems/src/subscription.hpp index 03d74af..b9e4173 100644 --- a/drake_ros_systems/src/subscription.hpp +++ b/drake_ros_systems/src/subscription.hpp @@ -1,3 +1,16 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #ifndef DRAKE_ROS_SYSTEMS__SUBSCRIPTION_HPP_ #define DRAKE_ROS_SYSTEMS__SUBSCRIPTION_HPP_ From fcd7fdd8245c74f77b5d724c1a5023cd15d957b7 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Mon, 14 Dec 2020 15:31:43 -0800 Subject: [PATCH 23/39] Remove unused String import Signed-off-by: Shane Loretz --- drake_ros_systems/example/rs_flip_flop.py | 1 - 1 file changed, 1 deletion(-) diff --git a/drake_ros_systems/example/rs_flip_flop.py b/drake_ros_systems/example/rs_flip_flop.py index 5e2b3fd..344cfa9 100755 --- a/drake_ros_systems/example/rs_flip_flop.py +++ b/drake_ros_systems/example/rs_flip_flop.py @@ -23,7 +23,6 @@ from pydrake.common.value import AbstractValue from pydrake.systems.framework import LeafSystem -from std_msgs.msg import String from std_msgs.msg import Bool From 8746fdfc3031e8be5dcc8c9a5f0e0f07a706824a Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Mon, 14 Dec 2020 15:56:09 -0800 Subject: [PATCH 24/39] Satisfy uncrustify Signed-off-by: Shane Loretz --- drake_ros_systems/example/rs_flip_flop.cpp | 26 ++++--- .../include/drake_ros_systems/drake_ros.hpp | 2 +- .../ros_interface_system.hpp | 6 +- .../ros_publisher_system.hpp | 7 +- .../ros_subscriber_system.hpp | 9 ++- .../include/drake_ros_systems/serializer.hpp | 2 +- drake_ros_systems/src/drake_ros.cpp | 2 +- .../module_drake_ros_systems.cpp | 70 +++++++++---------- .../src/python_bindings/py_serializer.hpp | 10 +-- .../src/ros_publisher_system.cpp | 11 +-- .../src/ros_subscriber_system.cpp | 47 +++++++------ drake_ros_systems/src/subscription.cpp | 1 - 12 files changed, 95 insertions(+), 98 deletions(-) diff --git a/drake_ros_systems/example/rs_flip_flop.cpp b/drake_ros_systems/example/rs_flip_flop.cpp index a8c3b34..e80615e 100644 --- a/drake_ros_systems/example/rs_flip_flop.cpp +++ b/drake_ros_systems/example/rs_flip_flop.cpp @@ -42,7 +42,9 @@ class NorGate : public drake::systems::LeafSystem private: void - calc_output_value(const drake::systems::Context & context, std_msgs::msg::Bool * output) const + calc_output_value( + const drake::systems::Context & context, + std_msgs::msg::Bool * output) const { const bool a = GetInputPort("A").Eval(context).data; const bool b = GetInputPort("B").Eval(context).data; @@ -52,7 +54,7 @@ class NorGate : public drake::systems::LeafSystem // Delay's input port by one timestep to avoid algebraic loop error // Inspired by Simulink's Memory block -template +template class Memory : public drake::systems::LeafSystem { public: @@ -67,15 +69,16 @@ class Memory : public drake::systems::LeafSystem DeclareAbstractOutputPort("value", &Memory::calc_output_value, {all_state_ticket()}); DeclarePerStepEvent( - drake::systems::UnrestrictedUpdateEvent([this]( - const drake::systems::Context& context, - const drake::systems::UnrestrictedUpdateEvent&, + drake::systems::UnrestrictedUpdateEvent( + [this]( + const drake::systems::Context & context, + const drake::systems::UnrestrictedUpdateEvent &, drake::systems::State * state) { - // Copy input value to state - drake::systems::AbstractValues & abstract_state = state->get_mutable_abstract_state(); - abstract_state.get_mutable_value(0).SetFrom( - get_input_port().Eval(context)); - })); + // Copy input value to state + drake::systems::AbstractValues & abstract_state = state->get_mutable_abstract_state(); + abstract_state.get_mutable_value(0).SetFrom( + get_input_port().Eval(context)); + })); } virtual ~Memory() = default; @@ -134,7 +137,8 @@ int main() auto diagram = builder.Build(); auto context = diagram->CreateDefaultContext(); - auto simulator = std::make_unique>(*diagram, std::move(context)); + auto simulator = + std::make_unique>(*diagram, std::move(context)); simulator->set_target_realtime_rate(1.0); simulator->Initialize(); diff --git a/drake_ros_systems/include/drake_ros_systems/drake_ros.hpp b/drake_ros_systems/include/drake_ros_systems/drake_ros.hpp index 503245b..a59003b 100644 --- a/drake_ros_systems/include/drake_ros_systems/drake_ros.hpp +++ b/drake_ros_systems/include/drake_ros_systems/drake_ros.hpp @@ -33,7 +33,7 @@ class Publisher; class Subscription; /// System that abstracts working with ROS -class DrakeRos final: public DrakeRosInterface +class DrakeRos final : public DrakeRosInterface { public: DrakeRos(); diff --git a/drake_ros_systems/include/drake_ros_systems/ros_interface_system.hpp b/drake_ros_systems/include/drake_ros_systems/ros_interface_system.hpp index b4a35c7..6638c44 100644 --- a/drake_ros_systems/include/drake_ros_systems/ros_interface_system.hpp +++ b/drake_ros_systems/include/drake_ros_systems/ros_interface_system.hpp @@ -39,9 +39,9 @@ class RosInterfaceSystem : public drake::systems::LeafSystem protected: /// Override as a place to call rclcpp::spin() void DoCalcNextUpdateTime( - const drake::systems::Context&, - drake::systems::CompositeEventCollection*, - double*) const override; + const drake::systems::Context &, + drake::systems::CompositeEventCollection *, + double *) const override; std::unique_ptr impl_; }; diff --git a/drake_ros_systems/include/drake_ros_systems/ros_publisher_system.hpp b/drake_ros_systems/include/drake_ros_systems/ros_publisher_system.hpp index fc5ca11..683606c 100644 --- a/drake_ros_systems/include/drake_ros_systems/ros_publisher_system.hpp +++ b/drake_ros_systems/include/drake_ros_systems/ros_publisher_system.hpp @@ -31,11 +31,11 @@ namespace drake_ros_systems class RosPublisherSystemPrivate; /// Accepts ROS messages on an input port and publishes them to a ROS topic -class RosPublisherSystem: public drake::systems::LeafSystem +class RosPublisherSystem : public drake::systems::LeafSystem { public: /// Convenience method to make a publisher system given a ROS message type - template + template static std::unique_ptr Make( @@ -57,7 +57,7 @@ class RosPublisherSystem: public drake::systems::LeafSystem virtual ~RosPublisherSystem(); /// Convenience method to publish a C++ ROS message - template + template void publish(const MessageT & message) { @@ -70,7 +70,6 @@ class RosPublisherSystem: public drake::systems::LeafSystem publish(const rclcpp::SerializedMessage & serialized_msg); protected: - void publish_input(const drake::systems::Context & context); diff --git a/drake_ros_systems/include/drake_ros_systems/ros_subscriber_system.hpp b/drake_ros_systems/include/drake_ros_systems/ros_subscriber_system.hpp index 5aac2be..8daeb0f 100644 --- a/drake_ros_systems/include/drake_ros_systems/ros_subscriber_system.hpp +++ b/drake_ros_systems/include/drake_ros_systems/ros_subscriber_system.hpp @@ -33,9 +33,8 @@ class RosSubscriberSystemPrivate; class RosSubscriberSystem : public drake::systems::LeafSystem { public: - /// Convenience method to make a subscriber system given a ROS message type - template + template static std::unique_ptr Make( @@ -59,9 +58,9 @@ class RosSubscriberSystem : public drake::systems::LeafSystem 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; + const drake::systems::Context &, + drake::systems::CompositeEventCollection *, + double *) const override; std::unique_ptr impl_; }; diff --git a/drake_ros_systems/include/drake_ros_systems/serializer.hpp b/drake_ros_systems/include/drake_ros_systems/serializer.hpp index 8745891..add15b0 100644 --- a/drake_ros_systems/include/drake_ros_systems/serializer.hpp +++ b/drake_ros_systems/include/drake_ros_systems/serializer.hpp @@ -24,7 +24,7 @@ namespace drake_ros_systems { -template +template class Serializer : public SerializerInterface { public: diff --git a/drake_ros_systems/src/drake_ros.cpp b/drake_ros_systems/src/drake_ros.cpp index 0db52af..e02c7f9 100644 --- a/drake_ros_systems/src/drake_ros.cpp +++ b/drake_ros_systems/src/drake_ros.cpp @@ -29,7 +29,7 @@ class DrakeRosPrivate }; DrakeRos::DrakeRos() - : impl_(new DrakeRosPrivate()) +: impl_(new DrakeRosPrivate()) { impl_->context_ = std::make_shared(); diff --git a/drake_ros_systems/src/python_bindings/module_drake_ros_systems.cpp b/drake_ros_systems/src/python_bindings/module_drake_ros_systems.cpp index ad22d87..fcb2cc8 100644 --- a/drake_ros_systems/src/python_bindings/module_drake_ros_systems.cpp +++ b/drake_ros_systems/src/python_bindings/module_drake_ros_systems.cpp @@ -45,46 +45,40 @@ PYBIND11_MODULE(drake_ros_systems, m) { py::class_>(m, "DrakeRosInterface"); py::class_>(m, "RosInterfaceSystem") - .def(py::init([](){ - return std::make_unique(std::make_unique());})) - .def("get_ros_interface", &RosInterfaceSystem::get_ros_interface); + .def( + py::init( + []() {return std::make_unique(std::make_unique());})) + .def("get_ros_interface", &RosInterfaceSystem::get_ros_interface); py::class_>(m, "RosPublisherSystem") - .def(py::init([]( - pybind11::object type, - const char * topic_name, - std::shared_ptr ros_interface) - { - std::unique_ptr serializer = std::make_unique(type); - return std::make_unique( - serializer, - topic_name, - rclcpp::QoS(10), // TODO(sloretz) Custom cast for rclpy.QoSProfile <--> rclcpp::Qos - ros_interface); - })); + .def( + py::init( + []( + pybind11::object type, + const char * topic_name, + std::shared_ptr ros_interface) + { + std::unique_ptr serializer = std::make_unique(type); + return std::make_unique( + serializer, + topic_name, + rclcpp::QoS(10), // TODO(sloretz) Custom cast for rclpy.QoSProfile <--> rclcpp::Qos + ros_interface); + })); py::class_>(m, "RosSubscriberSystem") - .def(py::init([]( - pybind11::object type, - const char * topic_name, - std::shared_ptr ros_interface) - { - std::unique_ptr serializer = std::make_unique(type); - return std::make_unique( - serializer, - topic_name, - rclcpp::QoS(10), // TODO(sloretz) Custom cast for rclpy.QoSProfile <--> rclcpp::Qos - ros_interface); - })); - - /* - py::class_(m, "ROS") - .def(py::init()) - .def("spin", &rccl::ROS::spin); - - py::class_(m, "PingPong") - .def(py::init()) - .def("ping", &rccl::PingPong::ping, - py::arg("msg") = "C++ ping"); - */ + .def( + py::init( + []( + pybind11::object type, + const char * topic_name, + std::shared_ptr ros_interface) + { + std::unique_ptr serializer = std::make_unique(type); + return std::make_unique( + serializer, + topic_name, + rclcpp::QoS(10), // TODO(sloretz) Custom cast for rclpy.QoSProfile <--> rclcpp::Qos + ros_interface); + })); } diff --git a/drake_ros_systems/src/python_bindings/py_serializer.hpp b/drake_ros_systems/src/python_bindings/py_serializer.hpp index 4097ec8..c7b5905 100644 --- a/drake_ros_systems/src/python_bindings/py_serializer.hpp +++ b/drake_ros_systems/src/python_bindings/py_serializer.hpp @@ -31,7 +31,7 @@ class PySerializer : public SerializerInterface { public: PySerializer(py::object message_type) - : message_type_(message_type) + : message_type_(message_type) { // Check if message_type.__class__._TYPE_SUPPORT is None, py::object metaclass = message_type.attr("__class__"); @@ -163,10 +163,10 @@ class PySerializer : public SerializerInterface py::object message_type_; rosidl_message_type_support_t * type_support_; - bool (*convert_from_py_)(PyObject *, void *); - PyObject * (*convert_to_py_)(void *); - void * (*create_ros_message_)(void); - void (*destroy_ros_message_)(void *); + bool (* convert_from_py_)(PyObject *, void *); + PyObject * (* convert_to_py_)(void *); + void * (* create_ros_message_)(void); + void (* destroy_ros_message_)(void *); }; } // namespace drake_ros_systems #endif // PY_SERIALIZER_HPP_ diff --git a/drake_ros_systems/src/ros_publisher_system.cpp b/drake_ros_systems/src/ros_publisher_system.cpp index 5895e55..0a3ab95 100644 --- a/drake_ros_systems/src/ros_publisher_system.cpp +++ b/drake_ros_systems/src/ros_publisher_system.cpp @@ -40,11 +40,12 @@ RosPublisherSystem::RosPublisherSystem( // TODO(sloretz) customizable triggers like lcm system DeclarePerStepEvent( - drake::systems::PublishEvent([this]( - const drake::systems::Context& context, - const drake::systems::PublishEvent&) { - publish_input(context); - })); + drake::systems::PublishEvent( + [this]( + const drake::systems::Context & context, + const drake::systems::PublishEvent &) { + publish_input(context); + })); } RosPublisherSystem::~RosPublisherSystem() diff --git a/drake_ros_systems/src/ros_subscriber_system.cpp b/drake_ros_systems/src/ros_subscriber_system.cpp index cecaf0f..0cd49f0 100644 --- a/drake_ros_systems/src/ros_subscriber_system.cpp +++ b/drake_ros_systems/src/ros_subscriber_system.cpp @@ -61,15 +61,16 @@ RosSubscriberSystem::RosSubscriberSystem( : impl_(new RosSubscriberSystemPrivate()) { impl_->serializer_ = std::move(serializer); - impl_->sub_ = ros->create_subscription(*impl_->serializer_->get_type_support(), topic_name, qos, + impl_->sub_ = ros->create_subscription( + *impl_->serializer_->get_type_support(), topic_name, qos, std::bind(&RosSubscriberSystemPrivate::handle_message, impl_.get(), std::placeholders::_1)); DeclareAbstractOutputPort( - [serializer{impl_->serializer_.get()}](){return serializer->create_default_value();}, - [](const drake::systems::Context & context, drake::AbstractValue * output_value) { - // Transfer message from state to output port - output_value->SetFrom(context.get_abstract_state().get_value(kStateIndexMessage)); - }); + [serializer{impl_->serializer_.get()}]() {return serializer->create_default_value();}, + [](const drake::systems::Context & context, drake::AbstractValue * output_value) { + // Transfer message from state to output port + output_value->SetFrom(context.get_abstract_state().get_value(kStateIndexMessage)); + }); static_assert(kStateIndexMessage == 0, ""); DeclareAbstractState(impl_->serializer_->create_default_value()); @@ -102,27 +103,27 @@ RosSubscriberSystem::DoCalcNextUpdateTime( // Create a unrestricted event and tie the handler to the corresponding // function. drake::systems::UnrestrictedUpdateEvent::UnrestrictedUpdateCallback - callback = [this, serialized_message{std::move(message)}]( - const drake::systems::Context&, - const drake::systems::UnrestrictedUpdateEvent&, - drake::systems::State* state) - { - // Deserialize the message and store it in the abstract state on the context - drake::systems::AbstractValues & abstract_state = state->get_mutable_abstract_state(); - auto & abstract_value = abstract_state.get_mutable_value(kStateIndexMessage); - const bool ret = impl_->serializer_->deserialize(*serialized_message, abstract_value); - if (ret != RMW_RET_OK) { - return drake::systems::EventStatus::Failed(this, "Failed to deserialize ROS message"); - } - return drake::systems::EventStatus::Succeeded(); - }; + callback = [this, serialized_message{std::move(message)}]( + const drake::systems::Context &, + const drake::systems::UnrestrictedUpdateEvent &, + drake::systems::State * state) + { + // Deserialize the message and store it in the abstract state on the context + drake::systems::AbstractValues & abstract_state = state->get_mutable_abstract_state(); + auto & abstract_value = abstract_state.get_mutable_value(kStateIndexMessage); + const bool ret = impl_->serializer_->deserialize(*serialized_message, abstract_value); + if (ret != RMW_RET_OK) { + return drake::systems::EventStatus::Failed(this, "Failed to deserialize ROS message"); + } + return drake::systems::EventStatus::Succeeded(); + }; // Schedule an update event at the current time. *time = context.get_time(); drake::systems::EventCollection> & uu_events = - events->get_mutable_unrestricted_update_events(); + events->get_mutable_unrestricted_update_events(); uu_events.add_event( - std::make_unique>( - drake::systems::TriggerType::kTimed, callback)); + std::make_unique>( + drake::systems::TriggerType::kTimed, callback)); } } // namespace drake_ros_systems diff --git a/drake_ros_systems/src/subscription.cpp b/drake_ros_systems/src/subscription.cpp index d0ed88d..1d719cd 100644 --- a/drake_ros_systems/src/subscription.cpp +++ b/drake_ros_systems/src/subscription.cpp @@ -82,4 +82,3 @@ Subscription::return_serialized_message(std::shared_ptr Date: Mon, 14 Dec 2020 16:32:09 -0800 Subject: [PATCH 25/39] Satisfy cpplint Signed-off-by: Shane Loretz --- drake_ros_systems/example/rs_flip_flop.cpp | 5 ++++- .../include/drake_ros_systems/drake_ros.hpp | 8 +++----- .../drake_ros_systems/drake_ros_interface.hpp | 5 ++--- .../drake_ros_systems/ros_interface_system.hpp | 8 ++++---- .../drake_ros_systems/ros_publisher_system.hpp | 16 +++++++++------- .../drake_ros_systems/ros_subscriber_system.hpp | 14 +++++++------- .../include/drake_ros_systems/serializer.hpp | 6 ++++-- .../drake_ros_systems/serializer_interface.hpp | 5 ++--- drake_ros_systems/src/drake_ros.cpp | 10 +++++++--- drake_ros_systems/src/publisher.cpp | 3 +++ drake_ros_systems/src/publisher.hpp | 13 +++++++------ .../python_bindings/module_drake_ros_systems.cpp | 14 ++++++++------ .../src/python_bindings/py_serializer.hpp | 10 +++++----- drake_ros_systems/src/ros_interface_system.cpp | 7 ++++++- drake_ros_systems/src/ros_publisher_system.cpp | 9 +++++++-- drake_ros_systems/src/ros_subscriber_system.cpp | 8 +++++--- drake_ros_systems/src/subscription.cpp | 3 +++ drake_ros_systems/src/subscription.hpp | 10 +++++----- 18 files changed, 91 insertions(+), 63 deletions(-) diff --git a/drake_ros_systems/example/rs_flip_flop.cpp b/drake_ros_systems/example/rs_flip_flop.cpp index e80615e..16c38da 100644 --- a/drake_ros_systems/example/rs_flip_flop.cpp +++ b/drake_ros_systems/example/rs_flip_flop.cpp @@ -22,6 +22,9 @@ #include +#include +#include + using drake_ros_systems::DrakeRos; using drake_ros_systems::RosInterfaceSystem; using drake_ros_systems::RosPublisherSystem; @@ -58,7 +61,7 @@ template class Memory : public drake::systems::LeafSystem { public: - Memory(const T & initial_value) + explicit Memory(const T & initial_value) { DeclareAbstractInputPort("value", *drake::AbstractValue::Make(T())); diff --git a/drake_ros_systems/include/drake_ros_systems/drake_ros.hpp b/drake_ros_systems/include/drake_ros_systems/drake_ros.hpp index a59003b..c1799b2 100644 --- a/drake_ros_systems/include/drake_ros_systems/drake_ros.hpp +++ b/drake_ros_systems/include/drake_ros_systems/drake_ros.hpp @@ -15,15 +15,14 @@ #define DRAKE_ROS_SYSTEMS__DRAKE_ROS_HPP_ #include - -#include +#include +#include #include #include #include -#include -#include +#include "drake_ros_systems/drake_ros_interface.hpp" namespace drake_ros_systems { @@ -46,7 +45,6 @@ class DrakeRos final : public DrakeRosInterface const std::string & topic_name, const rclcpp::QoS & qos) final; - virtual std::shared_ptr create_subscription( const rosidl_message_type_support_t & ts, diff --git a/drake_ros_systems/include/drake_ros_systems/drake_ros_interface.hpp b/drake_ros_systems/include/drake_ros_systems/drake_ros_interface.hpp index 7504724..775477d 100644 --- a/drake_ros_systems/include/drake_ros_systems/drake_ros_interface.hpp +++ b/drake_ros_systems/include/drake_ros_systems/drake_ros_interface.hpp @@ -15,14 +15,13 @@ #define DRAKE_ROS_SYSTEMS__DRAKE_ROS_INTERFACE_HPP_ #include +#include +#include #include #include #include -#include -#include - namespace drake_ros_systems { // Forward declarations for non-public-API classes diff --git a/drake_ros_systems/include/drake_ros_systems/ros_interface_system.hpp b/drake_ros_systems/include/drake_ros_systems/ros_interface_system.hpp index 6638c44..2d9d3f5 100644 --- a/drake_ros_systems/include/drake_ros_systems/ros_interface_system.hpp +++ b/drake_ros_systems/include/drake_ros_systems/ros_interface_system.hpp @@ -14,11 +14,11 @@ #ifndef DRAKE_ROS_SYSTEMS__ROS_INTERFACE_SYSTEM_HPP_ #define DRAKE_ROS_SYSTEMS__ROS_INTERFACE_SYSTEM_HPP_ -#include - #include -#include +#include + +#include "drake_ros_systems/drake_ros_interface.hpp" namespace drake_ros_systems { @@ -29,7 +29,7 @@ class RosInterfaceSystemPrivate; class RosInterfaceSystem : public drake::systems::LeafSystem { public: - RosInterfaceSystem(std::unique_ptr ros); + explicit RosInterfaceSystem(std::unique_ptr ros); virtual ~RosInterfaceSystem(); /// Return a handle for interacting with ROS diff --git a/drake_ros_systems/include/drake_ros_systems/ros_publisher_system.hpp b/drake_ros_systems/include/drake_ros_systems/ros_publisher_system.hpp index 683606c..86b83e4 100644 --- a/drake_ros_systems/include/drake_ros_systems/ros_publisher_system.hpp +++ b/drake_ros_systems/include/drake_ros_systems/ros_publisher_system.hpp @@ -14,17 +14,19 @@ #ifndef DRAKE_ROS_SYSTEMS__ROS_PUBLISHER_SYSTEM_HPP_ #define DRAKE_ROS_SYSTEMS__ROS_PUBLISHER_SYSTEM_HPP_ -#include - #include #include -#include -#include -#include - #include +#include +#include + +#include "drake_ros_systems/drake_ros_interface.hpp" +#include "drake_ros_systems/serializer.hpp" +#include "drake_ros_systems/serializer_interface.hpp" + + namespace drake_ros_systems { /// PIMPL forward declaration @@ -76,4 +78,4 @@ class RosPublisherSystem : public drake::systems::LeafSystem std::unique_ptr impl_; }; } // namespace drake_ros_systems -#endif // DRAKE_ROS_SYSTEMS__ROS_INTERFACE_SYSTEM_HPP_ +#endif // DRAKE_ROS_SYSTEMS__ROS_PUBLISHER_SYSTEM_HPP_ diff --git a/drake_ros_systems/include/drake_ros_systems/ros_subscriber_system.hpp b/drake_ros_systems/include/drake_ros_systems/ros_subscriber_system.hpp index 8daeb0f..e29eb28 100644 --- a/drake_ros_systems/include/drake_ros_systems/ros_subscriber_system.hpp +++ b/drake_ros_systems/include/drake_ros_systems/ros_subscriber_system.hpp @@ -14,15 +14,15 @@ #ifndef DRAKE_ROS_SYSTEMS__ROS_SUBSCRIBER_SYSTEM_HPP_ #define DRAKE_ROS_SYSTEMS__ROS_SUBSCRIBER_SYSTEM_HPP_ -#include - #include +#include -#include -#include -#include +#include +#include -#include +#include "drake_ros_systems/drake_ros_interface.hpp" +#include "drake_ros_systems/serializer.hpp" +#include "drake_ros_systems/serializer_interface.hpp" namespace drake_ros_systems { @@ -65,4 +65,4 @@ class RosSubscriberSystem : public drake::systems::LeafSystem std::unique_ptr impl_; }; } // namespace drake_ros_systems -#endif // DRAKE_ROS_SYSTEMS__ROS_INTERFACE_SYSTEM_HPP_ +#endif // DRAKE_ROS_SYSTEMS__ROS_SUBSCRIBER_SYSTEM_HPP_ diff --git a/drake_ros_systems/include/drake_ros_systems/serializer.hpp b/drake_ros_systems/include/drake_ros_systems/serializer.hpp index add15b0..60301e9 100644 --- a/drake_ros_systems/include/drake_ros_systems/serializer.hpp +++ b/drake_ros_systems/include/drake_ros_systems/serializer.hpp @@ -17,11 +17,13 @@ #include #include -#include - #include #include +#include + +#include "drake_ros_systems/serializer_interface.hpp" + namespace drake_ros_systems { template diff --git a/drake_ros_systems/include/drake_ros_systems/serializer_interface.hpp b/drake_ros_systems/include/drake_ros_systems/serializer_interface.hpp index 67102a9..7f3354e 100644 --- a/drake_ros_systems/include/drake_ros_systems/serializer_interface.hpp +++ b/drake_ros_systems/include/drake_ros_systems/serializer_interface.hpp @@ -15,12 +15,11 @@ #define DRAKE_ROS_SYSTEMS__SERIALIZER_INTERFACE_HPP_ #include - -#include - #include #include +#include + namespace drake_ros_systems { class SerializerInterface diff --git a/drake_ros_systems/src/drake_ros.cpp b/drake_ros_systems/src/drake_ros.cpp index e02c7f9..90ac30d 100644 --- a/drake_ros_systems/src/drake_ros.cpp +++ b/drake_ros_systems/src/drake_ros.cpp @@ -11,13 +11,17 @@ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. -#include -#include "publisher.hpp" -#include "subscription.hpp" #include #include +#include +#include + +#include "drake_ros_systems/drake_ros.hpp" +#include "publisher.hpp" +#include "subscription.hpp" + namespace drake_ros_systems { class DrakeRosPrivate diff --git a/drake_ros_systems/src/publisher.cpp b/drake_ros_systems/src/publisher.cpp index 8a7c9af..f726e00 100644 --- a/drake_ros_systems/src/publisher.cpp +++ b/drake_ros_systems/src/publisher.cpp @@ -11,6 +11,9 @@ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. + +#include + #include "publisher.hpp" namespace drake_ros_systems diff --git a/drake_ros_systems/src/publisher.hpp b/drake_ros_systems/src/publisher.hpp index 0009f45..623a357 100644 --- a/drake_ros_systems/src/publisher.hpp +++ b/drake_ros_systems/src/publisher.hpp @@ -11,18 +11,19 @@ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. -#ifndef DRAKE_ROS_SYSTEMS__PUBLISHER_HPP_ -#define DRAKE_ROS_SYSTEMS__PUBLISHER_HPP_ +#ifndef PUBLISHER_HPP_ +#define PUBLISHER_HPP_ #include -#include -#include - #include #include #include +#include +#include + + namespace drake_ros_systems { class Publisher final : public rclcpp::PublisherBase @@ -40,4 +41,4 @@ class Publisher final : public rclcpp::PublisherBase publish(const rclcpp::SerializedMessage & serialized_msg); }; } // namespace drake_ros_systems -#endif // DRAKE_ROS_SYSTEMS__PUBLISHER_HPP_ +#endif // PUBLISHER_HPP_ diff --git a/drake_ros_systems/src/python_bindings/module_drake_ros_systems.cpp b/drake_ros_systems/src/python_bindings/module_drake_ros_systems.cpp index fcb2cc8..1701258 100644 --- a/drake_ros_systems/src/python_bindings/module_drake_ros_systems.cpp +++ b/drake_ros_systems/src/python_bindings/module_drake_ros_systems.cpp @@ -15,10 +15,12 @@ #include -#include -#include -#include -#include +#include + +#include "drake_ros_systems/drake_ros.hpp" +#include "drake_ros_systems/ros_interface_system.hpp" +#include "drake_ros_systems/ros_publisher_system.hpp" +#include "drake_ros_systems/ros_subscriber_system.hpp" #include "py_serializer.hpp" @@ -62,7 +64,7 @@ PYBIND11_MODULE(drake_ros_systems, m) { return std::make_unique( serializer, topic_name, - rclcpp::QoS(10), // TODO(sloretz) Custom cast for rclpy.QoSProfile <--> rclcpp::Qos + rclcpp::QoS(10), // TODO(sloretz) Custom cast for rclpy.QoSProfile <--> rclcpp::Qos ros_interface); })); @@ -78,7 +80,7 @@ PYBIND11_MODULE(drake_ros_systems, m) { return std::make_unique( serializer, topic_name, - rclcpp::QoS(10), // TODO(sloretz) Custom cast for rclpy.QoSProfile <--> rclcpp::Qos + rclcpp::QoS(10), // TODO(sloretz) Custom cast for rclpy.QoSProfile <--> rclcpp::Qos ros_interface); })); } diff --git a/drake_ros_systems/src/python_bindings/py_serializer.hpp b/drake_ros_systems/src/python_bindings/py_serializer.hpp index c7b5905..859f89f 100644 --- a/drake_ros_systems/src/python_bindings/py_serializer.hpp +++ b/drake_ros_systems/src/python_bindings/py_serializer.hpp @@ -11,8 +11,8 @@ // 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 PY_SERIALIZER_HPP_ -#define PY_SERIALIZER_HPP_ +#ifndef PYTHON_BINDINGS__PY_SERIALIZER_HPP_ +#define PYTHON_BINDINGS__PY_SERIALIZER_HPP_ #include #include @@ -20,7 +20,7 @@ #include -#include +#include "drake_ros_systems/serializer_interface.hpp" namespace py = pybind11; @@ -30,7 +30,7 @@ namespace drake_ros_systems class PySerializer : public SerializerInterface { public: - PySerializer(py::object message_type) + explicit PySerializer(py::object message_type) : message_type_(message_type) { // Check if message_type.__class__._TYPE_SUPPORT is None, @@ -169,4 +169,4 @@ class PySerializer : public SerializerInterface void (* destroy_ros_message_)(void *); }; } // namespace drake_ros_systems -#endif // PY_SERIALIZER_HPP_ +#endif // PYTHON_BINDINGS__PY_SERIALIZER_HPP_ diff --git a/drake_ros_systems/src/ros_interface_system.cpp b/drake_ros_systems/src/ros_interface_system.cpp index 00e0dff..28dc97d 100644 --- a/drake_ros_systems/src/ros_interface_system.cpp +++ b/drake_ros_systems/src/ros_interface_system.cpp @@ -11,7 +11,12 @@ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. -#include + +#include +#include +#include + +#include "drake_ros_systems/ros_interface_system.hpp" namespace drake_ros_systems { diff --git a/drake_ros_systems/src/ros_publisher_system.cpp b/drake_ros_systems/src/ros_publisher_system.cpp index 0a3ab95..c4ac11b 100644 --- a/drake_ros_systems/src/ros_publisher_system.cpp +++ b/drake_ros_systems/src/ros_publisher_system.cpp @@ -11,8 +11,13 @@ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. -#include -#include + +#include +#include +#include + +#include "drake_ros_systems/ros_publisher_system.hpp" +#include "drake_ros_systems/serializer_interface.hpp" #include "publisher.hpp" diff --git a/drake_ros_systems/src/ros_subscriber_system.cpp b/drake_ros_systems/src/ros_subscriber_system.cpp index 0cd49f0..e5aba99 100644 --- a/drake_ros_systems/src/ros_subscriber_system.cpp +++ b/drake_ros_systems/src/ros_subscriber_system.cpp @@ -11,12 +11,14 @@ // 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 "drake_ros_systems/ros_subscriber_system.hpp" #include "subscription.hpp" namespace drake_ros_systems diff --git a/drake_ros_systems/src/subscription.cpp b/drake_ros_systems/src/subscription.cpp index 1d719cd..585ae43 100644 --- a/drake_ros_systems/src/subscription.cpp +++ b/drake_ros_systems/src/subscription.cpp @@ -13,6 +13,9 @@ // limitations under the License. #include "subscription.hpp" +#include +#include + namespace drake_ros_systems { // Copied from rosbag2_transport rosbag2_get_subscription_options diff --git a/drake_ros_systems/src/subscription.hpp b/drake_ros_systems/src/subscription.hpp index b9e4173..199834f 100644 --- a/drake_ros_systems/src/subscription.hpp +++ b/drake_ros_systems/src/subscription.hpp @@ -11,8 +11,8 @@ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. -#ifndef DRAKE_ROS_SYSTEMS__SUBSCRIPTION_HPP_ -#define DRAKE_ROS_SYSTEMS__SUBSCRIPTION_HPP_ +#ifndef SUBSCRIPTION_HPP_ +#define SUBSCRIPTION_HPP_ #include #include @@ -20,8 +20,8 @@ #include #include -#include -#include +#include "rclcpp/qos.hpp" +#include "rclcpp/subscription_base.hpp" namespace drake_ros_systems { @@ -76,4 +76,4 @@ class Subscription final : public rclcpp::SubscriptionBase std::function)> callback_; }; } // namespace drake_ros_systems -#endif // DRAKE_ROS_SYSTEMS__SUBSCRIPTION_HPP_ +#endif // SUBSCRIPTION_HPP_ From b24f18d1fc720e0764292e3872aa91939d8e699d Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Mon, 14 Dec 2020 17:14:48 -0800 Subject: [PATCH 26/39] Custom node name, node opts, and init opts This adds a way to provide the node name and node options, as well as a way to externally manage init/shutdown on a context. If the given node options have a valid context, then DrakeRos won't call init or shutdown on it. In this way external code can pass init options. Signed-off-by: Shane Loretz Signed-off-by: Shane Loretz --- .../include/drake_ros_systems/drake_ros.hpp | 8 ++++- drake_ros_systems/src/drake_ros.cpp | 34 ++++++++++++++----- 2 files changed, 32 insertions(+), 10 deletions(-) diff --git a/drake_ros_systems/include/drake_ros_systems/drake_ros.hpp b/drake_ros_systems/include/drake_ros_systems/drake_ros.hpp index c1799b2..b0d07a4 100644 --- a/drake_ros_systems/include/drake_ros_systems/drake_ros.hpp +++ b/drake_ros_systems/include/drake_ros_systems/drake_ros.hpp @@ -15,6 +15,7 @@ #define DRAKE_ROS_SYSTEMS__DRAKE_ROS_HPP_ #include +#include #include #include @@ -35,7 +36,12 @@ class Subscription; class DrakeRos final : public DrakeRosInterface { public: - DrakeRos(); + DrakeRos() + : DrakeRos("DrakeRosSystems", rclcpp::NodeOptions{}) {} + + DrakeRos( + const std::string & node_name, + rclcpp::NodeOptions node_options); virtual ~DrakeRos(); diff --git a/drake_ros_systems/src/drake_ros.cpp b/drake_ros_systems/src/drake_ros.cpp index 90ac30d..d607bb4 100644 --- a/drake_ros_systems/src/drake_ros.cpp +++ b/drake_ros_systems/src/drake_ros.cpp @@ -27,23 +27,36 @@ namespace drake_ros_systems class DrakeRosPrivate { public: + bool externally_init_; rclcpp::Context::SharedPtr context_; rclcpp::Node::UniquePtr node_; rclcpp::executors::SingleThreadedExecutor::UniquePtr executor_; }; -DrakeRos::DrakeRos() +DrakeRos::DrakeRos( + const std::string & node_name, + rclcpp::NodeOptions node_options) : impl_(new DrakeRosPrivate()) { - impl_->context_ = std::make_shared(); + if (node_options.context()) { + // Already given a context - don't create a new one + impl_->context_ = node_options.context(); + } else { + // Need a context - create one + impl_->context_ = std::make_shared(); + node_options.context(impl_->context_); + } - // TODO(sloretz) allow passing args and init options in constructor - impl_->context_->init(0, nullptr); + 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); + } - // TODO(sloretz) allow passing in node name and node options - rclcpp::NodeOptions no; - no.context(impl_->context_); - impl_->node_.reset(new rclcpp::Node("DrakeRosSystems", no)); + impl_->node_.reset(new rclcpp::Node(node_name, node_options)); // TODO(sloretz) allow passing in executor options rclcpp::ExecutorOptions eo; @@ -55,7 +68,10 @@ DrakeRos::DrakeRos() DrakeRos::~DrakeRos() { - impl_->context_->shutdown("~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 From 30186b793f83324c62df82576458af43b7e41a29 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Mon, 14 Dec 2020 18:31:59 -0800 Subject: [PATCH 27/39] Add additional publish triggers Signed-off-by: Shane Loretz --- .../ros_publisher_system.hpp | 28 +++++++-- .../module_drake_ros_systems.cpp | 6 +- .../src/ros_publisher_system.cpp | 61 +++++++++++++++---- 3 files changed, 79 insertions(+), 16 deletions(-) diff --git a/drake_ros_systems/include/drake_ros_systems/ros_publisher_system.hpp b/drake_ros_systems/include/drake_ros_systems/ros_publisher_system.hpp index 86b83e4..051b8f8 100644 --- a/drake_ros_systems/include/drake_ros_systems/ros_publisher_system.hpp +++ b/drake_ros_systems/include/drake_ros_systems/ros_publisher_system.hpp @@ -21,6 +21,7 @@ #include #include +#include #include "drake_ros_systems/drake_ros_interface.hpp" #include "drake_ros_systems/serializer.hpp" @@ -44,17 +45,36 @@ class RosPublisherSystem : public drake::systems::LeafSystem 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); + 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); + std::shared_ptr ros_interface, + const std::unordered_set & publish_triggers, + double publish_period = 0.0); virtual ~RosPublisherSystem(); @@ -72,8 +92,8 @@ class RosPublisherSystem : public drake::systems::LeafSystem publish(const rclcpp::SerializedMessage & serialized_msg); protected: - void - publish_input(const drake::systems::Context & context); + drake::systems::EventStatus + publish_input(const drake::systems::Context & context) const; std::unique_ptr impl_; }; diff --git a/drake_ros_systems/src/python_bindings/module_drake_ros_systems.cpp b/drake_ros_systems/src/python_bindings/module_drake_ros_systems.cpp index 1701258..acf21e3 100644 --- a/drake_ros_systems/src/python_bindings/module_drake_ros_systems.cpp +++ b/drake_ros_systems/src/python_bindings/module_drake_ros_systems.cpp @@ -16,6 +16,7 @@ #include #include +#include #include "drake_ros_systems/drake_ros.hpp" #include "drake_ros_systems/ros_interface_system.hpp" @@ -27,6 +28,7 @@ namespace py = pybind11; using drake::systems::LeafSystem; +using drake::systems::TriggerType; using drake_ros_systems::DrakeRos; using drake_ros_systems::DrakeRosInterface; @@ -65,7 +67,9 @@ PYBIND11_MODULE(drake_ros_systems, m) { serializer, topic_name, rclcpp::QoS(10), // TODO(sloretz) Custom cast for rclpy.QoSProfile <--> rclcpp::Qos - ros_interface); + ros_interface, + std::unordered_set{TriggerType::kPerStep, TriggerType::kForced}, + 0.0); // TODO(sloretz) Expose Publish triggers to python })); py::class_>(m, "RosSubscriberSystem") diff --git a/drake_ros_systems/src/ros_publisher_system.cpp b/drake_ros_systems/src/ros_publisher_system.cpp index c4ac11b..a69f978 100644 --- a/drake_ros_systems/src/ros_publisher_system.cpp +++ b/drake_ros_systems/src/ros_publisher_system.cpp @@ -14,6 +14,7 @@ #include #include +#include #include #include "drake_ros_systems/ros_publisher_system.hpp" @@ -34,7 +35,9 @@ RosPublisherSystem::RosPublisherSystem( std::unique_ptr & serializer, const std::string & topic_name, const rclcpp::QoS & qos, - std::shared_ptr ros) + std::shared_ptr ros, + const std::unordered_set & publish_triggers, + double publish_period) : impl_(new RosPublisherSystemPrivate()) { impl_->serializer_ = std::move(serializer); @@ -43,14 +46,49 @@ RosPublisherSystem::RosPublisherSystem( DeclareAbstractInputPort("message", *(impl_->serializer_->create_default_value())); - // TODO(sloretz) customizable triggers like lcm system - DeclarePerStepEvent( - drake::systems::PublishEvent( - [this]( - const drake::systems::Context & context, - const drake::systems::PublishEvent &) { - publish_input(context); - })); + // vvv Mostly copied from LcmPublisherSystem vvv + // Check that publish_triggers does not contain an unsupported trigger. + for (const auto & trigger : publish_triggers) { + if ( + (trigger != drake::systems::TriggerType::kForced) && + (trigger != drake::systems::TriggerType::kPeriodic) && + (trigger != drake::systems::TriggerType::kPerStep)) + { + throw std::invalid_argument("Only kForced, kPeriodic, or kPerStep are supported"); + } + } + + // Declare a forced publish so that any time Publish(.) is called on this + // system (or a Diagram containing it), a message is emitted. + if (publish_triggers.find(drake::systems::TriggerType::kForced) != publish_triggers.end()) { + this->DeclareForcedPublishEvent( + &RosPublisherSystem::publish_input); + } + + if (publish_triggers.find(drake::systems::TriggerType::kPeriodic) != publish_triggers.end()) { + if (publish_period <= 0.0) { + throw std::invalid_argument("kPeriodic requires publish_period > 0"); + } + const double offset = 0.0; + this->DeclarePeriodicPublishEvent( + publish_period, offset, + &RosPublisherSystem::publish_input); + } else if (publish_period > 0) { + // publish_period > 0 without drake::systems::TriggerType::kPeriodic has no meaning and is + // likely a mistake. + throw std::invalid_argument("publish_period > 0 requires kPeriodic"); + } + + if (publish_triggers.find(drake::systems::TriggerType::kPerStep) != publish_triggers.end()) { + DeclarePerStepEvent( + drake::systems::PublishEvent( + [this]( + const drake::systems::Context & context, + const drake::systems::PublishEvent &) { + publish_input(context); + })); + } + // ^^^ Mostly copied from LcmPublisherSystem ^^^ } RosPublisherSystem::~RosPublisherSystem() @@ -63,10 +101,11 @@ RosPublisherSystem::publish(const rclcpp::SerializedMessage & serialized_msg) impl_->pub_->publish(serialized_msg); } -void -RosPublisherSystem::publish_input(const drake::systems::Context & context) +drake::systems::EventStatus +RosPublisherSystem::publish_input(const drake::systems::Context & context) const { const drake::AbstractValue & input = get_input_port().Eval(context); impl_->pub_->publish(impl_->serializer_->serialize(input)); + return drake::systems::EventStatus::Succeeded(); } } // namespace drake_ros_systems From 8fdf98391e99b2877a0df44bc9e27f204e1d82c6 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Tue, 15 Dec 2020 14:23:18 -0800 Subject: [PATCH 28/39] Subscriber already unsubs on destruction Signed-off-by: Shane Loretz --- drake_ros_systems/src/ros_subscriber_system.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/drake_ros_systems/src/ros_subscriber_system.cpp b/drake_ros_systems/src/ros_subscriber_system.cpp index e5aba99..4ead3cd 100644 --- a/drake_ros_systems/src/ros_subscriber_system.cpp +++ b/drake_ros_systems/src/ros_subscriber_system.cpp @@ -47,7 +47,6 @@ class RosSubscriberSystemPrivate std::unique_ptr serializer_; // A handle to a subscription - // TODO(sloretz) unique_ptr that unsubscribes in destructor std::shared_ptr sub_; // Mutex to prevent multiple threads from modifying this class std::mutex mutex_; From 6327d6796b15d2845d293883d76934c172fe9555 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 16 Dec 2020 15:50:51 -0800 Subject: [PATCH 29/39] Allow setting QoS on Python systems Adds half a type caster (Python -> C++) for rclpy.qos.QoSProfile to rclcpp::QoS. Adds QoS arguments to Python publisher and subscriber systems. Updates the Python example to pass QoS. Signed-off-by: Shane Loretz Signed-off-by: Shane Loretz --- drake_ros_systems/example/rs_flip_flop.py | 12 +- .../module_drake_ros_systems.cpp | 7 +- .../src/python_bindings/qos_type_caster.hpp | 269 ++++++++++++++++++ 3 files changed, 282 insertions(+), 6 deletions(-) create mode 100644 drake_ros_systems/src/python_bindings/qos_type_caster.hpp diff --git a/drake_ros_systems/example/rs_flip_flop.py b/drake_ros_systems/example/rs_flip_flop.py index 344cfa9..708aee2 100755 --- a/drake_ros_systems/example/rs_flip_flop.py +++ b/drake_ros_systems/example/rs_flip_flop.py @@ -25,6 +25,8 @@ from std_msgs.msg import Bool +from rclpy.qos import QoSProfile + class NorGate(LeafSystem): @@ -84,15 +86,17 @@ def main(): sys_ros_interface = builder.AddSystem(RosInterfaceSystem()) + qos = QoSProfile(depth=10) + sys_pub_Q = builder.AddSystem( - RosPublisherSystem(Bool, "Q", sys_ros_interface.get_ros_interface())) + RosPublisherSystem(Bool, "Q", qos, sys_ros_interface.get_ros_interface())) sys_pub_Q_not = builder.AddSystem( - RosPublisherSystem(Bool, "Q_not", sys_ros_interface.get_ros_interface())) + RosPublisherSystem(Bool, "Q_not", qos, sys_ros_interface.get_ros_interface())) sys_sub_S = builder.AddSystem( - RosSubscriberSystem(Bool, "S", sys_ros_interface.get_ros_interface())) + RosSubscriberSystem(Bool, "S", qos, sys_ros_interface.get_ros_interface())) sys_sub_R = builder.AddSystem( - RosSubscriberSystem(Bool, "R", sys_ros_interface.get_ros_interface())) + RosSubscriberSystem(Bool, "R", qos, sys_ros_interface.get_ros_interface())) sys_nor_gate_1 = builder.AddSystem(NorGate()) sys_nor_gate_2 = builder.AddSystem(NorGate()) diff --git a/drake_ros_systems/src/python_bindings/module_drake_ros_systems.cpp b/drake_ros_systems/src/python_bindings/module_drake_ros_systems.cpp index acf21e3..94b8c21 100644 --- a/drake_ros_systems/src/python_bindings/module_drake_ros_systems.cpp +++ b/drake_ros_systems/src/python_bindings/module_drake_ros_systems.cpp @@ -24,6 +24,7 @@ #include "drake_ros_systems/ros_subscriber_system.hpp" #include "py_serializer.hpp" +#include "qos_type_caster.hpp" namespace py = pybind11; @@ -60,13 +61,14 @@ PYBIND11_MODULE(drake_ros_systems, m) { []( pybind11::object type, const char * topic_name, + drake_ros_systems::QoS qos, std::shared_ptr ros_interface) { std::unique_ptr serializer = std::make_unique(type); return std::make_unique( serializer, topic_name, - rclcpp::QoS(10), // TODO(sloretz) Custom cast for rclpy.QoSProfile <--> rclcpp::Qos + qos, ros_interface, std::unordered_set{TriggerType::kPerStep, TriggerType::kForced}, 0.0); // TODO(sloretz) Expose Publish triggers to python @@ -78,13 +80,14 @@ PYBIND11_MODULE(drake_ros_systems, m) { []( pybind11::object type, const char * topic_name, + drake_ros_systems::QoS qos, std::shared_ptr ros_interface) { std::unique_ptr serializer = std::make_unique(type); return std::make_unique( serializer, topic_name, - rclcpp::QoS(10), // TODO(sloretz) Custom cast for rclpy.QoSProfile <--> rclcpp::Qos + qos, ros_interface); })); } diff --git a/drake_ros_systems/src/python_bindings/qos_type_caster.hpp b/drake_ros_systems/src/python_bindings/qos_type_caster.hpp new file mode 100644 index 0000000..217c226 --- /dev/null +++ b/drake_ros_systems/src/python_bindings/qos_type_caster.hpp @@ -0,0 +1,269 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#ifndef PYTHON_BINDINGS__QOS_TYPE_CASTER_HPP_ +#define PYTHON_BINDINGS__QOS_TYPE_CASTER_HPP_ + +#include + +#include + +#include + +namespace drake_ros_systems +{ +// Thin wrapper that adds default constructor +class QoS : public rclcpp::QoS +{ +public: + QoS() + : rclcpp::QoS(1) {} +}; +} // namespace drake_ros_systems + +namespace pybind11 +{ +namespace detail +{ +template<> +struct type_caster +{ +public: + // declares local 'value' of type rclcpp::QoS + PYBIND11_TYPE_CASTER(drake_ros_systems::QoS, _("rclpy.qos.QoSProfile")); + + // Convert from Python rclpy.qos.QoSProfile to rclcpp::QoS + bool load(handle src, bool) + { + /* Extract PyObject from handle */ + PyObject * source = src.ptr(); + + // history : enum int + // depth : int + // reliability : enum int + // durability : enum int + // lifespan : rclpy.Duration + // deadline : rclpy.Duration + // liveliness : enum int + // liveliness_lease_duration : rclpy.Duration + // avoid_ros_namespace_conventions : bool + + PyObject * py_history = nullptr; + PyObject * py_depth = nullptr; + PyObject * py_reliability = nullptr; + PyObject * py_durability = nullptr; + PyObject * py_lifespan = nullptr; + PyObject * py_lifespan_ns = nullptr; + PyObject * py_deadline = nullptr; + PyObject * py_deadline_ns = nullptr; + PyObject * py_liveliness = nullptr; + PyObject * py_liveliness_lease_duration = nullptr; + PyObject * py_liveliness_lease_duration_ns = nullptr; + PyObject * py_avoid_ros_namespace_conventions = nullptr; + + // Clean up references when function exits. + std::unique_ptr> scope_exit( + nullptr, [&](void *) { + Py_XDECREF(py_avoid_ros_namespace_conventions); + Py_XDECREF(py_liveliness_lease_duration_ns); + Py_XDECREF(py_liveliness_lease_duration); + Py_XDECREF(py_liveliness); + Py_XDECREF(py_deadline_ns); + Py_XDECREF(py_deadline); + Py_XDECREF(py_lifespan_ns); + Py_XDECREF(py_lifespan); + Py_XDECREF(py_durability); + Py_XDECREF(py_reliability); + Py_XDECREF(py_depth); + Py_XDECREF(py_history); + }); + + size_t history; + size_t depth; + size_t reliability; + size_t durability; + size_t lifespan_ns; + size_t deadline_ns; + size_t liveliness; + size_t liveliness_lease_duration_ns; + int avoid_ros_namespace_conventions; + + // Get all the QoS Attributes + py_history = PyObject_GetAttrString(source, "history"); + if (!py_history) { + return false; + } + py_depth = PyObject_GetAttrString(source, "depth"); + if (!py_depth) { + return false; + } + py_reliability = PyObject_GetAttrString(source, "reliability"); + if (!py_reliability) { + return false; + } + py_durability = PyObject_GetAttrString(source, "durability"); + if (!py_durability) { + return false; + } + py_lifespan = PyObject_GetAttrString(source, "lifespan"); + if (!py_lifespan) { + return false; + } + py_deadline = PyObject_GetAttrString(source, "deadline"); + if (!py_deadline) { + return false; + } + py_liveliness = PyObject_GetAttrString(source, "liveliness"); + if (!py_liveliness) { + return false; + } + py_liveliness_lease_duration = + PyObject_GetAttrString(source, "liveliness_lease_duration"); + if (!py_liveliness_lease_duration) { + return false; + } + py_avoid_ros_namespace_conventions = + PyObject_GetAttrString(source, "avoid_ros_namespace_conventions"); + if (!py_avoid_ros_namespace_conventions) { + return false; + } + + // Do Type Conversions + // History and depth if history is keep_last + history = PyNumber_AsSsize_t(py_history, NULL); + switch (history) { + case RMW_QOS_POLICY_HISTORY_KEEP_LAST: + depth = PyNumber_AsSsize_t(py_depth, PyExc_OverflowError); + if (PyErr_Occurred()) { + return false; + } + value.keep_last(depth); + break; + case RMW_QOS_POLICY_HISTORY_KEEP_ALL: + case RMW_QOS_POLICY_HISTORY_SYSTEM_DEFAULT: + case RMW_QOS_POLICY_HISTORY_UNKNOWN: + value.history(static_cast(history)); + break; + default: + if (!PyErr_Occurred()) { + PyErr_Format(PyExc_ValueError, "Unsupported history policy %zu", history); + } + return false; + } + + // Reliability + reliability = PyNumber_AsSsize_t(py_reliability, NULL); + switch (reliability) { + case RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT: + case RMW_QOS_POLICY_RELIABILITY_RELIABLE: + case RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT: + case RMW_QOS_POLICY_RELIABILITY_UNKNOWN: + value.reliability(static_cast(reliability)); + break; + default: + if (!PyErr_Occurred()) { + PyErr_Format(PyExc_ValueError, "Unsupported reliability policy %zu", reliability); + } + return false; + } + + // Durability + durability = PyNumber_AsSsize_t(py_durability, NULL); + switch (durability) { + case RMW_QOS_POLICY_DURABILITY_SYSTEM_DEFAULT: + case RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL: + case RMW_QOS_POLICY_DURABILITY_VOLATILE: + case RMW_QOS_POLICY_DURABILITY_UNKNOWN: + value.durability(static_cast(durability)); + break; + default: + if (!PyErr_Occurred()) { + PyErr_Format(PyExc_ValueError, "Unsupported durability policy %zu", durability); + } + return false; + } + + // lifespan + py_lifespan_ns = PyObject_GetAttrString(py_lifespan, "nanoseconds"); + if (!py_lifespan_ns) { + return false; + } + lifespan_ns = PyNumber_AsSsize_t(py_lifespan_ns, PyExc_OverflowError); + if (PyErr_Occurred()) { + return false; + } + value.lifespan(rclcpp::Duration::from_nanoseconds(lifespan_ns)); + + // deadline + py_deadline_ns = PyObject_GetAttrString(py_deadline, "nanoseconds"); + if (!py_deadline_ns) { + return false; + } + deadline_ns = PyNumber_AsSsize_t(py_deadline_ns, PyExc_OverflowError); + if (PyErr_Occurred()) { + return false; + } + value.deadline(rclcpp::Duration::from_nanoseconds(deadline_ns)); + + // liveliness + liveliness = PyNumber_AsSsize_t(py_liveliness, NULL); + switch (liveliness) { + case RMW_QOS_POLICY_LIVELINESS_AUTOMATIC: + case RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC: + case RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT: + case RMW_QOS_POLICY_LIVELINESS_UNKNOWN: + value.liveliness(static_cast(liveliness)); + break; + default: + if (!PyErr_Occurred()) { + PyErr_Format(PyExc_ValueError, "Unsupported liveliness policy %zu", liveliness); + } + return false; + } + + // liveliness_lease_duration + py_liveliness_lease_duration_ns = PyObject_GetAttrString( + py_liveliness_lease_duration, "nanoseconds"); + if (!py_liveliness_lease_duration_ns) { + return false; + } + liveliness_lease_duration_ns = PyNumber_AsSsize_t( + py_liveliness_lease_duration_ns, PyExc_OverflowError); + if (PyErr_Occurred()) { + return false; + } + value.liveliness_lease_duration( + rclcpp::Duration::from_nanoseconds(liveliness_lease_duration_ns)); + + // avoid_ros_namespace_conventions + avoid_ros_namespace_conventions = PyObject_IsTrue(py_avoid_ros_namespace_conventions); + if (-1 == avoid_ros_namespace_conventions) { + return false; + } + value.avoid_ros_namespace_conventions(avoid_ros_namespace_conventions); + + return true; + } + + // Convert from Python rclcpp::QoS to rclpy.qos.QoSProfile + static handle cast(drake_ros_systems::QoS src, return_value_policy policy, handle parent) + { + (void) src; + (void) policy; + (void) parent; + Py_RETURN_NOTIMPLEMENTED; + } +}; +} // namespace detail +} // namespace pybind11 +#endif // PYTHON_BINDINGS__QOS_TYPE_CASTER_HPP_ From 5478e50bcbe16f2db60b4e7413c7a9f943992b55 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 16 Dec 2020 16:11:18 -0800 Subject: [PATCH 30/39] Expose publish triggers to Python systems Signed-off-by: Shane Loretz --- .../module_drake_ros_systems.cpp | 21 ++++++++++++++++++- 1 file changed, 20 insertions(+), 1 deletion(-) diff --git a/drake_ros_systems/src/python_bindings/module_drake_ros_systems.cpp b/drake_ros_systems/src/python_bindings/module_drake_ros_systems.cpp index 94b8c21..977395e 100644 --- a/drake_ros_systems/src/python_bindings/module_drake_ros_systems.cpp +++ b/drake_ros_systems/src/python_bindings/module_drake_ros_systems.cpp @@ -71,7 +71,26 @@ PYBIND11_MODULE(drake_ros_systems, m) { qos, ros_interface, std::unordered_set{TriggerType::kPerStep, TriggerType::kForced}, - 0.0); // TODO(sloretz) Expose Publish triggers to python + 0.0); + })) + .def( + py::init( + []( + pybind11::object type, + const char * topic_name, + drake_ros_systems::QoS qos, + std::shared_ptr ros_interface, + std::unordered_set publish_triggers, + double publish_period) + { + std::unique_ptr serializer = std::make_unique(type); + return std::make_unique( + serializer, + topic_name, + qos, + ros_interface, + publish_triggers, + publish_period); })); py::class_>(m, "RosSubscriberSystem") From 89b607adb9253f7011074418af6834a26b145ec8 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 16 Dec 2020 16:39:36 -0800 Subject: [PATCH 31/39] Throw exception if Serialization fails Signed-off-by: Shane Loretz --- drake_ros_systems/src/python_bindings/py_serializer.hpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/drake_ros_systems/src/python_bindings/py_serializer.hpp b/drake_ros_systems/src/python_bindings/py_serializer.hpp index 859f89f..3871335 100644 --- a/drake_ros_systems/src/python_bindings/py_serializer.hpp +++ b/drake_ros_systems/src/python_bindings/py_serializer.hpp @@ -83,8 +83,7 @@ class PySerializer : public SerializerInterface // Convert the Python message to a C ROS message if (!convert_from_py_(message.ptr(), c_ros_message.get())) { - // TODO(sloretz) Error handling? Throw? - return rclcpp::SerializedMessage(); + throw std::runtime_error("Failed to convert Python message to C ROS message"); } // Serialize the C message @@ -94,8 +93,7 @@ class PySerializer : public SerializerInterface type_support_, &serialized_msg.get_rcl_serialized_message()); if (ret != RMW_RET_OK) { - // TODO(sloretz) do something if serialization fails - return rclcpp::SerializedMessage(); + throw std::runtime_error("Failed to serialize C ROS message"); } return serialized_msg; } From 41d8f8a30f731875e06476b8291fd58d4b7f95f5 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 16 Dec 2020 16:53:30 -0800 Subject: [PATCH 32/39] Remove obsolete comment Signed-off-by: Shane Loretz --- drake_ros_systems/src/drake_ros.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/drake_ros_systems/src/drake_ros.cpp b/drake_ros_systems/src/drake_ros.cpp index d607bb4..7b9ca43 100644 --- a/drake_ros_systems/src/drake_ros.cpp +++ b/drake_ros_systems/src/drake_ros.cpp @@ -95,9 +95,6 @@ DrakeRos::create_subscription( impl_->node_->get_node_base_interface().get(), ts, topic_name, qos, callback); impl_->node_->get_node_topics_interface()->add_subscription(sub, nullptr); - // TODO(sloretz) return unique pointer to subscription and make subscription - // automatically unsubscribe when it's deleted - return sub; } From 32b26bcf86876243824e36d6b93e28abd6718dfa Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Thu, 17 Dec 2020 15:35:41 -0800 Subject: [PATCH 33/39] NodeOptions defaults to global context Signed-off-by: Shane Loretz --- drake_ros_systems/src/drake_ros.cpp | 12 ++++-------- 1 file changed, 4 insertions(+), 8 deletions(-) diff --git a/drake_ros_systems/src/drake_ros.cpp b/drake_ros_systems/src/drake_ros.cpp index 7b9ca43..ff856ec 100644 --- a/drake_ros_systems/src/drake_ros.cpp +++ b/drake_ros_systems/src/drake_ros.cpp @@ -38,15 +38,11 @@ DrakeRos::DrakeRos( rclcpp::NodeOptions node_options) : impl_(new DrakeRosPrivate()) { - if (node_options.context()) { - // Already given a context - don't create a new one - impl_->context_ = node_options.context(); - } else { - // Need a context - create one - impl_->context_ = std::make_shared(); - node_options.context(impl_->context_); + 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; From 265e404aff824b86d397835d8bf47e31e02c87d0 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Thu, 17 Dec 2020 15:36:19 -0800 Subject: [PATCH 34/39] Add integration test Signed-off-by: Shane Loretz --- drake_ros_systems/CMakeLists.txt | 9 +++ drake_ros_systems/package.xml | 5 +- drake_ros_systems/test/integration.cpp | 95 ++++++++++++++++++++++++++ 3 files changed, 108 insertions(+), 1 deletion(-) create mode 100644 drake_ros_systems/test/integration.cpp diff --git a/drake_ros_systems/CMakeLists.txt b/drake_ros_systems/CMakeLists.txt index c6a1c45..673f17d 100644 --- a/drake_ros_systems/CMakeLists.txt +++ b/drake_ros_systems/CMakeLists.txt @@ -87,7 +87,16 @@ install( if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) + find_package(test_msgs REQUIRED) ament_lint_auto_find_test_dependencies() + + ament_add_gtest(test_integration test/integration.cpp) + message(STATUS "Got test_msgs targets ${test_msgs_TARGETS}") + target_link_libraries(test_integration + drake::drake + drake_ros_systems + ${test_msgs_TARGETS} + ) endif() ament_package() diff --git a/drake_ros_systems/package.xml b/drake_ros_systems/package.xml index 180c83b..0f5d0d2 100644 --- a/drake_ros_systems/package.xml +++ b/drake_ros_systems/package.xml @@ -6,14 +6,17 @@ Shane Loretz Apache License 2.0 + ament_cmake_ros + rclcpp rosidl_runtime_c rosidl_typesupport_cpp + ament_cmake_gtest ament_lint_auto ament_lint_common - cmake + ament_cmake diff --git a/drake_ros_systems/test/integration.cpp b/drake_ros_systems/test/integration.cpp new file mode 100644 index 0000000..6aadd6d --- /dev/null +++ b/drake_ros_systems/test/integration.cpp @@ -0,0 +1,95 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include + +#include +#include + +#include +#include +#include + +#include "drake_ros_systems/drake_ros.hpp" +#include "drake_ros_systems/ros_interface_system.hpp" +#include "drake_ros_systems/ros_publisher_system.hpp" +#include "drake_ros_systems/ros_subscriber_system.hpp" + +using drake_ros_systems::DrakeRos; +using drake_ros_systems::RosInterfaceSystem; +using drake_ros_systems::RosPublisherSystem; +using drake_ros_systems::RosSubscriberSystem; + + +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()); +} From 2776642e666f567848094f5ca7db1c89de370277 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Thu, 17 Dec 2020 16:55:54 -0800 Subject: [PATCH 35/39] Add Python integration test Signed-off-by: Shane Loretz --- drake_ros_systems/CMakeLists.txt | 7 +++ drake_ros_systems/test/integration.cpp | 84 ++++++++++++++++++++++++++ 2 files changed, 91 insertions(+) diff --git a/drake_ros_systems/CMakeLists.txt b/drake_ros_systems/CMakeLists.txt index 673f17d..fa00206 100644 --- a/drake_ros_systems/CMakeLists.txt +++ b/drake_ros_systems/CMakeLists.txt @@ -91,11 +91,18 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() ament_add_gtest(test_integration test/integration.cpp) + + # TODO(sloretz) Why isn't pybind11::embed including python libs? + find_package(PythonLibs REQUIRED) + message(STATUS "Got test_msgs targets ${test_msgs_TARGETS}") target_link_libraries(test_integration drake::drake drake_ros_systems ${test_msgs_TARGETS} + pybind11::embed + # TODO(sloretz) Remove when this is included in pybind11::embed + ${PYTHON_LIBRARIES} ) endif() diff --git a/drake_ros_systems/test/integration.cpp b/drake_ros_systems/test/integration.cpp index 6aadd6d..63aab3d 100644 --- a/drake_ros_systems/test/integration.cpp +++ b/drake_ros_systems/test/integration.cpp @@ -15,6 +15,7 @@ #include #include #include +#include #include #include @@ -33,6 +34,7 @@ using drake_ros_systems::RosInterfaceSystem; using drake_ros_systems::RosPublisherSystem; using drake_ros_systems::RosSubscriberSystem; +namespace py = pybind11; TEST(Integration, sub_to_pub) { drake::systems::DiagramBuilder builder; @@ -93,3 +95,85 @@ TEST(Integration, sub_to_pub) { // Make sure same number of messages got out ASSERT_EQ(num_msgs, rx_msgs.size()); } + +TEST(Integration, sub_to_pub_py) { + py::scoped_interpreter guard{}; + + py::dict scope; + py::exec( + R"delim( +from drake_ros_systems import RosInterfaceSystem +from drake_ros_systems import RosPublisherSystem +from drake_ros_systems import RosSubscriberSystem + +from pydrake.systems.analysis import Simulator +from pydrake.systems.framework import DiagramBuilder + +from rclpy.qos import DurabilityPolicy +from rclpy.qos import HistoryPolicy +from rclpy.qos import QoSProfile +from rclpy.qos import ReliabilityPolicy + +from test_msgs.msg import BasicTypes + +builder = DiagramBuilder() + +sys_ros_interface = builder.AddSystem(RosInterfaceSystem()) + +qos = QoSProfile( + depth=10, + history=HistoryPolicy.KEEP_LAST, + reliability=ReliabilityPolicy.RELIABLE, + durability=DurabilityPolicy.TRANSIENT_LOCAL) + +sys_pub = builder.AddSystem( + RosPublisherSystem(BasicTypes, 'out_py', qos, sys_ros_interface.get_ros_interface())) + +sys_sub = builder.AddSystem( + RosSubscriberSystem(BasicTypes, 'in_py', qos, sys_ros_interface.get_ros_interface())) + +builder.Connect(sys_sub.get_output_port(0), sys_pub.get_input_port(0)) +diagram = builder.Build() +simulator = Simulator(diagram) +simulator_context = simulator.get_mutable_context() +simulator.set_target_realtime_rate(1.0) +)delim", + py::globals(), scope); + + const size_t num_msgs = 5; + rclcpp::QoS qos{rclcpp::KeepLast(num_msgs)}; + qos.transient_local().reliable(); + + // Don't need to rclcpp::init because DrakeRos uses global rclcpp::Context by default + auto node = rclcpp::Node::make_shared("sub_to_pub_py"); + + // Create publisher talking to subscriber system. + auto publisher = node->create_publisher("in_py", qos); + + // Create subscription listening to publisher system + std::vector> rx_msgs; + auto rx_callback = [&](std::unique_ptr msg) + { + rx_msgs.push_back(std::move(msg)); + }; + auto subscription = + node->create_subscription("out_py", qos, rx_callback); + + // Send messages into the drake system + for (size_t p = 0; p < num_msgs; ++p) { + publisher->publish(std::make_unique()); + } + + const int timeout_sec = 5; + const int spins_per_sec = 10; + const float time_delta = 1.0f / spins_per_sec; + scope["time_delta"] = time_delta; + for (int i = 0; i < timeout_sec * spins_per_sec && rx_msgs.size() < num_msgs; ++i) { + rclcpp::spin_some(node); + py::exec( + "simulator.AdvanceTo(simulator_context.get_time() + time_delta)", py::globals(), scope); + } + + // Make sure same number of messages got out + ASSERT_EQ(num_msgs, rx_msgs.size()); +} From dac964be7df0097bf5e189183e334eb4fb42f324 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Thu, 17 Dec 2020 17:18:21 -0800 Subject: [PATCH 36/39] Remove debug print Signed-off-by: Shane Loretz --- drake_ros_systems/CMakeLists.txt | 1 - 1 file changed, 1 deletion(-) diff --git a/drake_ros_systems/CMakeLists.txt b/drake_ros_systems/CMakeLists.txt index fa00206..97c73d3 100644 --- a/drake_ros_systems/CMakeLists.txt +++ b/drake_ros_systems/CMakeLists.txt @@ -95,7 +95,6 @@ if(BUILD_TESTING) # TODO(sloretz) Why isn't pybind11::embed including python libs? find_package(PythonLibs REQUIRED) - message(STATUS "Got test_msgs targets ${test_msgs_TARGETS}") target_link_libraries(test_integration drake::drake drake_ros_systems From 56bed38a03bf629608ff86c2525ed4982209e961 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Fri, 18 Dec 2020 09:30:14 -0800 Subject: [PATCH 37/39] Fix bug where context wasn't being shut down Signed-off-by: Shane Loretz --- drake_ros_systems/src/drake_ros.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/drake_ros_systems/src/drake_ros.cpp b/drake_ros_systems/src/drake_ros.cpp index ff856ec..ca0b374 100644 --- a/drake_ros_systems/src/drake_ros.cpp +++ b/drake_ros_systems/src/drake_ros.cpp @@ -64,7 +64,7 @@ DrakeRos::DrakeRos( DrakeRos::~DrakeRos() { - if (impl_->externally_init_) { + if (!impl_->externally_init_) { // This system init'd the context, so this system will shut it down too. impl_->context_->shutdown("~DrakeRos()"); } From 82ee8fb6160aec76cf9f6c8ab9513595253b0f54 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Fri, 18 Dec 2020 09:30:31 -0800 Subject: [PATCH 38/39] Add tests for DrakeRos Signed-off-by: Shane Loretz --- drake_ros_systems/CMakeLists.txt | 5 ++++ drake_ros_systems/test/drake_ros.cpp | 44 ++++++++++++++++++++++++++++ 2 files changed, 49 insertions(+) create mode 100644 drake_ros_systems/test/drake_ros.cpp diff --git a/drake_ros_systems/CMakeLists.txt b/drake_ros_systems/CMakeLists.txt index 97c73d3..ef3bbcf 100644 --- a/drake_ros_systems/CMakeLists.txt +++ b/drake_ros_systems/CMakeLists.txt @@ -103,6 +103,11 @@ if(BUILD_TESTING) # TODO(sloretz) Remove when this is included in pybind11::embed ${PYTHON_LIBRARIES} ) + + ament_add_gtest(test_drake_ros test/drake_ros.cpp) + target_link_libraries(test_drake_ros + drake_ros_systems + ) endif() ament_package() diff --git a/drake_ros_systems/test/drake_ros.cpp b/drake_ros_systems/test/drake_ros.cpp new file mode 100644 index 0000000..3b974f1 --- /dev/null +++ b/drake_ros_systems/test/drake_ros.cpp @@ -0,0 +1,44 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include + +#include +#include +#include + +#include "drake_ros_systems/drake_ros.hpp" + +using drake_ros_systems::DrakeRos; + +TEST(DrakeRos, default_construct) +{ + EXPECT_NO_THROW(std::make_unique()); +} + +TEST(DrakeRos, local_context) +{ + auto context = std::make_shared(); + rclcpp::NodeOptions node_options; + node_options.context(context); + + auto drake_ros = std::make_unique("local_ctx_node", node_options); + (void) drake_ros; + + // Should not have initialized global context + EXPECT_FALSE(rclcpp::contexts::get_global_default_context()->is_valid()); + EXPECT_TRUE(context->is_valid()); +} From 22880a87db95f7ef97faeaae9ebb05545eee3477 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Fri, 18 Dec 2020 14:31:15 -0800 Subject: [PATCH 39/39] Do more in Python Signed-off-by: Shane Loretz --- .../src/python_bindings/py_serializer.hpp | 45 ++++++++++++------- 1 file changed, 28 insertions(+), 17 deletions(-) diff --git a/drake_ros_systems/src/python_bindings/py_serializer.hpp b/drake_ros_systems/src/python_bindings/py_serializer.hpp index 3871335..f5fd6d3 100644 --- a/drake_ros_systems/src/python_bindings/py_serializer.hpp +++ b/drake_ros_systems/src/python_bindings/py_serializer.hpp @@ -33,37 +33,51 @@ class PySerializer : public SerializerInterface explicit PySerializer(py::object message_type) : message_type_(message_type) { - // Check if message_type.__class__._TYPE_SUPPORT is None, - py::object metaclass = message_type.attr("__class__"); - if (metaclass.attr("_TYPE_SUPPORT").is_none()) { - // call message_type.__class__.__import_type_support__() - metaclass.attr("__import_type_support__")(); - } + py::dict scope; + py::exec( + R"delim( +def get_typesupport(msg_type, attribute_name): + metaclass = msg_type.__class__ + if metaclass._TYPE_SUPPORT is None: + # Import typesupport if not done already + metaclass.__import_type_support__() + return getattr(metaclass, attribute_name) + + +def make_abstract_value(some_type): + from pydrake.common.value import AbstractValue + return AbstractValue.Make(some_type()) + )delim", + py::globals(), scope); + + py_make_abstract_value_ = scope["make_abstract_value"]; + py::object py_get_typesupport = scope["get_typesupport"]; + // Get type support capsule and pointer - auto typesupport = py::cast(metaclass.attr("_TYPE_SUPPORT")); + auto typesupport = py::cast(py_get_typesupport(message_type, "_TYPE_SUPPORT")); // TODO(sloretz) use get_pointer() in py 2.6+ type_support_ = static_cast(typesupport); auto convert_from_py = - py::cast(metaclass.attr("_CONVERT_FROM_PY")); + py::cast(py_get_typesupport(message_type, "_CONVERT_FROM_PY")); // TODO(sloretz) use get_pointer() in py 2.6+ convert_from_py_ = reinterpret_cast( static_cast(convert_from_py)); auto convert_to_py = - py::cast(metaclass.attr("_CONVERT_TO_PY")); + py::cast(py_get_typesupport(message_type, "_CONVERT_TO_PY")); // TODO(sloretz) use get_pointer() in py 2.6+ convert_to_py_ = reinterpret_cast( static_cast(convert_to_py)); auto create_ros_message = - py::cast(metaclass.attr("_CREATE_ROS_MESSAGE")); + py::cast(py_get_typesupport(message_type, "_CREATE_ROS_MESSAGE")); // TODO(sloretz) use get_pointer() in py 2.6+ create_ros_message_ = reinterpret_cast( static_cast(create_ros_message)); auto destroy_ros_message = - py::cast(metaclass.attr("_DESTROY_ROS_MESSAGE")); + py::cast(py_get_typesupport(message_type, "_DESTROY_ROS_MESSAGE")); // TODO(sloretz) use get_pointer() in py 2.6+ destroy_ros_message_ = reinterpret_cast( static_cast(destroy_ros_message)); @@ -143,12 +157,7 @@ class PySerializer : public SerializerInterface std::unique_ptr create_default_value() const override { - // convert to inaccessible drake::pydrake::Object type - py::object scope = py::module::import("pydrake.common.value").attr("__dict__"); - py::object lambda = py::eval("lambda msg: AbstractValue.Make(msg)", scope); - py::object result = lambda(message_type_()); - - return py::cast>(result); + return py::cast>(py_make_abstract_value_(message_type_)); } const rosidl_message_type_support_t * @@ -161,6 +170,8 @@ class PySerializer : public SerializerInterface py::object message_type_; rosidl_message_type_support_t * type_support_; + py::object py_make_abstract_value_; + bool (* convert_from_py_)(PyObject *, void *); PyObject * (* convert_to_py_)(void *); void * (* create_ros_message_)(void);