From 6d9d00ffc516b5e8469a68674aaef4309022bb04 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Mon, 9 Nov 2020 17:03:21 -0800 Subject: [PATCH 01/87] 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 00000000..681dcbbd --- /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 00000000..36374763 --- /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 00000000..9ef8b4a5 --- /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 00000000..88bb0d53 --- /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 00000000..b0d0fd71 --- /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 00000000..0a5d045b --- /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 00000000..242e8ced --- /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 00000000..a7d9593c --- /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 00000000..e69de29b diff --git a/drake_ros_systems/src/publisher.cpp b/drake_ros_systems/src/publisher.cpp new file mode 100644 index 00000000..1a67a6e1 --- /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 00000000..366d47f1 --- /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 00000000..e69de29b 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 00000000..e69de29b 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 00000000..e69de29b diff --git a/drake_ros_systems/src/subscription.cpp b/drake_ros_systems/src/subscription.cpp new file mode 100644 index 00000000..17078f48 --- /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 00000000..6fd0ce59 --- /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 d253479156f178f3d589aa1fec8306cc75a69ed6 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Tue, 10 Nov 2020 13:49:59 -0800 Subject: [PATCH 02/87] 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 9ef8b4a5..39926c7e 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 88bb0d53..9daccd50 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 b0d0fd71..0d5c5582 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 0a5d045b..6cbeb353 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 242e8ced..df7720fd 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 96ddcfdf0b18639b64db1734da9f44c773852098 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Tue, 10 Nov 2020 13:53:02 -0800 Subject: [PATCH 03/87] 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 6cbeb353..393eb9b9 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 8f3f925ffc610c0ae8c0c97c4a008a1f89d2b6b2 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Tue, 10 Nov 2020 15:38:29 -0800 Subject: [PATCH 04/87] 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 39926c7e..f3b3b61d 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 393eb9b9..da39611d 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 df7720fd..416b19fb 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 e69de29b..6486258f 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 17078f48..e70d7788 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 6fd0ce59..289fbc6a 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 10a347b91e60931e3f5c7d362041fd2a0e9381b6 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Tue, 10 Nov 2020 16:39:13 -0800 Subject: [PATCH 05/87] 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 9daccd50..7600def5 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 0d5c5582..1eef43e8 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 e69de29b..a9a27dbc 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 810d6f0302c570c62e6931fec71dc169b2d38b10 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 11 Nov 2020 10:33:06 -0800 Subject: [PATCH 06/87] 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 416b19fb..1039f564 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 e69de29b..6f8abe77 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 ae95a7fcc471945c94b0e906548bedf9ecd3ee70 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 11 Nov 2020 13:01:32 -0800 Subject: [PATCH 07/87] 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 da39611d..2a6aa7dd 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 e69de29b..0627e960 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 78be6812c6dfe4958ea353a0eb0f53fbf280dae7 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Fri, 13 Nov 2020 15:08:08 -0800 Subject: [PATCH 08/87] 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 2a6aa7dd..936e60bd 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 00000000..fea7832e --- /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 00000000..f0669af3 --- /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 0627e960..a5aa52a4 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 9947161ad40ddb1b209ec95bdf788764b473b0d2 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Fri, 13 Nov 2020 16:34:16 -0800 Subject: [PATCH 09/87] 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 f3b3b61d..02ea012b 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 7600def5..c86ef60a 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 1039f564..469ff87a 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 fea7832e..9e0dd129 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 f0669af3..66b11e05 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 6486258f..2cbf31f4 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 6f8abe77..7ccf8e9d 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 289fbc6a..03d74af3 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 2965a9712724620cc43c7996e3c3cc4ae021ab63 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Fri, 13 Nov 2020 16:35:23 -0800 Subject: [PATCH 10/87] 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 681dcbbd..c8733700 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 00000000..284db04e --- /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 00000000..7c330024 --- /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 a5bc7d97202cd92841fe5f2a0d1d9b48283cdbc2 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Thu, 19 Nov 2020 09:35:03 -0800 Subject: [PATCH 11/87] 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 936e60bd..03f19b1a 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 469ff87a..2e3c0084 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 9e0dd129..a370e98f 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 66b11e05..a9df7e50 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 a5aa52a4..176fdcee 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 7ccf8e9d..02b43cc4 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 2db7ba48a5cafbd62ece567bbd25c08818b9966a Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 2 Dec 2020 11:10:35 -0800 Subject: [PATCH 12/87] 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 c8733700..e3caf42c 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 00000000..acf763a7 --- /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 00000000..3fd8cc53 --- /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 00000000..6c9026d5 --- /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 768eff70411810bb95442d2cce7c3577f9ed05e3 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 2 Dec 2020 11:23:26 -0800 Subject: [PATCH 13/87] 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 3fd8cc53..e8190b74 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 21f358f6d5ecd8b268fe023ba19becd2d50520f7 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 2 Dec 2020 14:41:32 -0800 Subject: [PATCH 14/87] 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 acf763a7..7f8cd63f 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 e8190b74..8bd4b179 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 6c9026d5..7b4a96ca 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 26d60b2b9744569e32ebcf6789a1e410b7d41f8e Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 2 Dec 2020 16:19:27 -0800 Subject: [PATCH 15/87] 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 7f8cd63f..6dfbfe04 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 d88de33c4e92410f0b7ea4a189d253e21ecf5c66 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 2 Dec 2020 18:47:20 -0800 Subject: [PATCH 16/87] 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 36374763..51250810 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 0ab7e709f1ba9310a3051a3834ea2efb061f144b Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 2 Dec 2020 18:48:41 -0800 Subject: [PATCH 17/87] 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 51250810..cea77c2a 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 63f8e82d9a6056b073e7e2ec3872b3909585d946 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 2 Dec 2020 18:49:24 -0800 Subject: [PATCH 18/87] 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 cea77c2a..e78f13d2 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 87d62d1dc2e952b9bed0ab1ffc7015c26c7cf842 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 2 Dec 2020 18:53:31 -0800 Subject: [PATCH 19/87] 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 e78f13d2..231f8377 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 5e1509d55254afac6e0fb7319620ec7b0b0b5f81 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Mon, 14 Dec 2020 15:01:55 -0800 Subject: [PATCH 20/87] 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 e3caf42c..1cc7b0a1 100644 --- a/drake_ros_systems/CMakeLists.txt +++ b/drake_ros_systems/CMakeLists.txt @@ -86,4 +86,3 @@ install( ### End Python bindings ament_package() - From 23898ae44d611c81d4b233073bb1d46b659f6850 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Mon, 14 Dec 2020 15:24:38 -0800 Subject: [PATCH 21/87] 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 1cc7b0a1..c6a1c454 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 a7d9593c..180c83ba 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 5ef563d9a0d956d343caecf528fe3550dd3492bb Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Mon, 14 Dec 2020 15:30:47 -0800 Subject: [PATCH 22/87] Add LICENSE file and header comments Signed-off-by: Shane Loretz --- 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 +++++++++++++ .../drake_ros_systems/ros_interface_system.hpp | 13 +++++++++++++ .../drake_ros_systems/ros_publisher_system.hpp | 13 +++++++++++++ .../drake_ros_systems/ros_subscriber_system.hpp | 13 +++++++++++++ .../include/drake_ros_systems/serializer.hpp | 13 +++++++++++++ .../drake_ros_systems/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 +++++++++++++ .../python_bindings/module_drake_ros_systems.cpp | 13 +++++++++++++ .../src/python_bindings/py_serializer.hpp | 13 +++++++++++++ drake_ros_systems/src/ros_interface_system.cpp | 13 +++++++++++++ drake_ros_systems/src/ros_publisher_system.cpp | 13 +++++++++++++ drake_ros_systems/src/ros_subscriber_system.cpp | 13 +++++++++++++ drake_ros_systems/src/subscription.cpp | 13 +++++++++++++ drake_ros_systems/src/subscription.hpp | 13 +++++++++++++ 19 files changed, 247 insertions(+) diff --git a/drake_ros_systems/example/rs_flip_flop.cpp b/drake_ros_systems/example/rs_flip_flop.cpp index 7c330024..a8c3b349 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 6dfbfe04..5e2b3fd3 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 02ea012b..503245b4 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 c86ef60a..7504724d 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 1eef43e8..b4a35c7f 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 03f19b1a..fc5ca11b 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 2e3c0084..5aac2bef 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 a370e98f..87458919 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 a9df7e50..67102a93 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 2cbf31f4..0db52af9 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 1a67a6e1..8a7c9afa 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 366d47f1..0009f454 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 8bd4b179..ad22d87c 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 7b4a96ca..4097ec86 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 a9a27dbc..00e0dff1 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 176fdcee..5895e55b 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 02b43cc4..cecaf0f7 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 e70d7788..d0ed88d9 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 03d74af3..b9e41734 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 2af50239b41dc024a469d8caa533bda80f56fcab Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Mon, 14 Dec 2020 15:31:43 -0800 Subject: [PATCH 23/87] 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 5e2b3fd3..344cfa9a 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 3c9f79e3c9e2c015fdbecb49652afd44094c575c Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Mon, 14 Dec 2020 15:56:09 -0800 Subject: [PATCH 24/87] 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 a8c3b349..e80615e5 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 503245b4..a59003b9 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 b4a35c7f..6638c44e 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 fc5ca11b..683606ce 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 5aac2bef..8daeb0fa 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 87458919..add15b06 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 0db52af9..e02c7f93 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 ad22d87c..fcb2cc8b 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 4097ec86..c7b59055 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 5895e55b..0a3ab95a 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 cecaf0f7..0cd49f05 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 d0ed88d9..1d719cde 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/87] 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 e80615e5..16c38da0 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 a59003b9..c1799b29 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 7504724d..775477d1 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 6638c44e..2d9d3f56 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 683606ce..86b83e44 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 8daeb0fa..e29eb28e 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 add15b06..60301e9f 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 67102a93..7f3354e1 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 e02c7f93..90ac30dc 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 8a7c9afa..f726e00f 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 0009f454..623a3571 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 fcb2cc8b..17012585 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 c7b59055..859f89fd 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 00e0dff1..28dc97dd 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 0a3ab95a..c4ac11bb 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 0cd49f05..e5aba99e 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 1d719cde..585ae430 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 b9e41734..199834fd 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 2dc31ce39bdf65d41b3a97eb18ea211ee6ab0299 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Mon, 14 Dec 2020 17:14:48 -0800 Subject: [PATCH 26/87] 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 c1799b29..b0d07a4e 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 90ac30dc..d607bb46 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 914f9879ce7a53eb0d6c7d2d986cf25e68104efe Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Mon, 14 Dec 2020 18:31:59 -0800 Subject: [PATCH 27/87] 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 86b83e44..051b8f81 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 17012585..acf21e33 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 c4ac11bb..a69f978b 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 dffa0e8ae4c74fcae20506d6ea973f1ccccecc6f Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Tue, 15 Dec 2020 14:23:18 -0800 Subject: [PATCH 28/87] 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 e5aba99e..4ead3cd7 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 8513a6a5ff0c260115855b6d58a2932ad17ec6e9 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 16 Dec 2020 15:50:51 -0800 Subject: [PATCH 29/87] 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 344cfa9a..708aee21 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 acf21e33..94b8c213 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 00000000..217c226a --- /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 ff7497fbadd7220751341c87bc99305eb30b2a2c Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 16 Dec 2020 16:11:18 -0800 Subject: [PATCH 30/87] 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 94b8c213..977395e3 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 a4010dacdce2c7acd834b583f3e71d4b04896ccf Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 16 Dec 2020 16:39:36 -0800 Subject: [PATCH 31/87] 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 859f89fd..38713350 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 49054cadbf317c4b739893d86e2bc1fb875838ef Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 16 Dec 2020 16:53:30 -0800 Subject: [PATCH 32/87] 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 d607bb46..7b9ca433 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 4bdf9dc5e7ae0baaf1d2b01b8285fded9ae71717 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Thu, 17 Dec 2020 15:35:41 -0800 Subject: [PATCH 33/87] 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 7b9ca433..ff856ec2 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 16f033b388cc9ad811a603fe31c3754d2425e860 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Thu, 17 Dec 2020 15:36:19 -0800 Subject: [PATCH 34/87] 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 c6a1c454..673f17d5 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 180c83ba..0f5d0d28 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 00000000..6aadd6dc --- /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 52388adb3c2db087822c458fab5cef8f6fa5f61d Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Thu, 17 Dec 2020 16:55:54 -0800 Subject: [PATCH 35/87] 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 673f17d5..fa002066 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 6aadd6dc..63aab3d8 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 6866d041e8c33c5d3c0a2c482a3dfa3918d427ab Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Thu, 17 Dec 2020 17:18:21 -0800 Subject: [PATCH 36/87] 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 fa002066..97c73d3e 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 09825208ac9633f945085723e225cde5f633df9e Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Fri, 18 Dec 2020 09:30:14 -0800 Subject: [PATCH 37/87] 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 ff856ec2..ca0b374f 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 957b6180f88246a19d20a0fe357cfd941dcfec91 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Fri, 18 Dec 2020 09:30:31 -0800 Subject: [PATCH 38/87] 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 97c73d3e..ef3bbcf2 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 00000000..3b974f1f --- /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 e7759ac5cfa03ec1eb5994c720e5c78f3efa00ff Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Fri, 18 Dec 2020 14:31:15 -0800 Subject: [PATCH 39/87] 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 38713350..f5fd6d3d 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); From f008032e9e6d249faa94b1eae5aca6ebf853cf08 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Mon, 19 Apr 2021 18:12:17 -0300 Subject: [PATCH 40/87] WIP: Add TfBroadcaster in C++ and Python. Signed-off-by: Michel Hidalgo --- drake_ros_systems/CMakeLists.txt | 3 + .../include/drake_ros_systems/drake_ros.hpp | 6 + .../drake_ros_systems/drake_ros_interface.hpp | 8 ++ .../tf_broadcaster_system.hpp | 42 ++++++ drake_ros_systems/src/drake_ros.cpp | 13 ++ .../module_drake_ros_systems.cpp | 25 ++++ .../src/tf_broadcaster_system.cpp | 132 ++++++++++++++++++ 7 files changed, 229 insertions(+) create mode 100644 drake_ros_systems/include/drake_ros_systems/tf_broadcaster_system.hpp create mode 100644 drake_ros_systems/src/tf_broadcaster_system.cpp diff --git a/drake_ros_systems/CMakeLists.txt b/drake_ros_systems/CMakeLists.txt index ef3bbcf2..6c0645c0 100644 --- a/drake_ros_systems/CMakeLists.txt +++ b/drake_ros_systems/CMakeLists.txt @@ -22,6 +22,7 @@ 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) +find_package(tf2_ros REQUIRED) add_library(drake_ros_systems src/drake_ros.cpp @@ -30,7 +31,9 @@ add_library(drake_ros_systems src/ros_publisher_system.cpp src/ros_subscriber_system.cpp src/subscription.cpp + src/tf_broadcaster_system.cpp ) + target_link_libraries(drake_ros_systems PUBLIC drake::drake rclcpp::rclcpp 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 b0d07a4e..39d9498b 100644 --- a/drake_ros_systems/include/drake_ros_systems/drake_ros.hpp +++ b/drake_ros_systems/include/drake_ros_systems/drake_ros.hpp @@ -58,6 +58,12 @@ class DrakeRos final : public DrakeRosInterface const rclcpp::QoS & qos, std::function)> callback) final; + std::unique_ptr + create_tf_broadcaster() final; + + std::shared_ptr + get_clock() final; + void spin( int timeout_millis) final; 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 775477d1..6b07321a 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 @@ -47,6 +47,14 @@ class DrakeRosInterface const rclcpp::QoS & qos, std::function)> callback) = 0; + virtual + std::unique_ptr + create_tf_broadcaster() = 0; + + virtual + std::shared_ptr + get_clock() = 0; + virtual void spin( diff --git a/drake_ros_systems/include/drake_ros_systems/tf_broadcaster_system.hpp b/drake_ros_systems/include/drake_ros_systems/tf_broadcaster_system.hpp new file mode 100644 index 00000000..3bba65d7 --- /dev/null +++ b/drake_ros_systems/include/drake_ros_systems/tf_broadcaster_system.hpp @@ -0,0 +1,42 @@ +// Copyright 2021 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#ifndef DRAKE_ROS_SYSTEMS__TF_BROADCASTER_SYSTEM_HPP_ +#define DRAKE_ROS_SYSTEMS__TF_BROADCASTER_SYSTEM_HPP_ + +#include + +#include + +#include "drake_ros_systems/drake_ros_interface.hpp" + + +namespace drake_ros_systems +{ +// PIMPL forward declaration +class TfBroadcasterSystemPrivate; + +class TfBroadcasterSystem : public drake::systems::LeafSystem +{ +public: + TfBroadcasterSystem( + DrakeRosInterface * ros_interface + const std::unordered_set & publish_triggers, + double publish_period = 0.0); + virtual ~TfBroadcasterSystem() = default; + +private: + std::unique_ptr impl_; +}; +} // namespace drake_ros_systems +#endif // DRAKE_ROS_SYSTEMS__TF_BROADCASTER_SYSTEM_HPP_ diff --git a/drake_ros_systems/src/drake_ros.cpp b/drake_ros_systems/src/drake_ros.cpp index ca0b374f..a6a544f1 100644 --- a/drake_ros_systems/src/drake_ros.cpp +++ b/drake_ros_systems/src/drake_ros.cpp @@ -94,6 +94,19 @@ DrakeRos::create_subscription( return sub; } +std::unique_ptr +DrakeRos::create_tf_broadcaster() +{ + return std::make_unique(impl_->node_); +} + +std::shared_ptr +DrakeRos::get_clock() +{ + return impl_->node_->get_clock(); +} + + void DrakeRos::spin( int timeout_millis) 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 977395e3..f6fd7f10 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 @@ -22,6 +22,7 @@ #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 "drake_ros_systems/tf_broadcaster_system.hpp" #include "py_serializer.hpp" #include "qos_type_caster.hpp" @@ -38,6 +39,7 @@ using drake_ros_systems::RosInterfaceSystem; using drake_ros_systems::RosPublisherSystem; using drake_ros_systems::RosSubscriberSystem; using drake_ros_systems::SerializerInterface; +using drake_ros_systems::TfBroadcasterSystem; PYBIND11_MODULE(drake_ros_systems, m) { @@ -109,4 +111,27 @@ PYBIND11_MODULE(drake_ros_systems, m) { qos, ros_interface); })); + + py::class_>(m, "TfBroadcasterSystem") + .def( + py::init( + [](DrakeRosInterface* ros_interface) + { + constexpr double kZeroPublishPeriod{0.0}; + return std::make_unique( + ros_interface, + std::unordered_set{ + TriggerType::kPerStep, TriggerType::kForced}, + kZeroPublishPeriod); + })) + .def( + py::init( + []( + DrakeRosInterface* ros_interface + std::unordered_set publish_triggers, + double publish_period) + { + return std::make_unique( + ros_interface, publish_triggers, publish_period); + })); } diff --git a/drake_ros_systems/src/tf_broadcaster_system.cpp b/drake_ros_systems/src/tf_broadcaster_system.cpp new file mode 100644 index 00000000..6e6b0dc3 --- /dev/null +++ b/drake_ros_systems/src/tf_broadcaster_system.cpp @@ -0,0 +1,132 @@ +// 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 // NOLINT + +#include "drake_ros_systems/tf_broadcaster_system.hpp" + + +namespace drake_ros_systems +{ +class TfBroadcasterSystemPrivate +{ +public: + std::unique_ptr tf_broadcaster_; + std::shared_ptr clock_; +}; + +TfBroadcasterSystem::TfBroadcasterSystem( + DrakeRosInterface * ros, + const std::unordered_set & publish_triggers, + double publish_period) +: impl_(new TFBroadcasterSystemPrivate()) +{ + impl_->clock_ = ros->get_clock(); + impl_->tf_broadcaster_ = ros->create_tf_broadcaster(); + + DeclareAbstractInputPort( + "graph_query", drake::Value>{}); + + // vvv Mostly copied from LcmPublisherSystem vvv + using TriggerType = drake::systems::TriggerType; + // Check that publish_triggers does not contain an unsupported trigger. + for (const auto & trigger : publish_triggers) { + if ( + (trigger != TriggerType::kForced) && + (trigger != TriggerType::kPeriodic) && + (trigger != 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(TriggerType::kForced) != publish_triggers.end()) { + this->DeclareForcedPublishEvent(&TFBroadcasterSystem::DoPublishFrames); + } + + if (publish_triggers.find(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, + &TfBroadcasterSystem::DoPublishFrames); + } 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(TriggerType::kPerStep) != publish_triggers.end()) { + DeclarePerStepEvent( + drake::systems::PublishEvent( + [this]( + const drake::systems::Context & context, + const drake::systems::PublishEvent &) { + DoPublishFrames(context); + })); + } + // ^^^ Mostly copied from LcmPublisherSystem ^^^ +} + +drake::systems::EventStatus +TfBroadcasterSystem::DoPublishFrames( + const drake::systems::Context & context) const +{ + const drake::geometry::QueryObject & query_object = + get_input_port().Eval>(context); + const drake::geometry::SceneGraphInspector & inspector = + query_object.inspector(); + // TODO(hidmic): publish frame transforms w.r.t. to their parent frame + // instead of the world frame when an API is made available. + std::vector all_messages; + all_messages.reserve(inspector.num_frames() - 1); + const std::string & world_frame_name = + inspector.Getname(inspector.world_frame_id()); + geometry_msgs::msg::TransformStamped message; + message.header.frame_id = world_frame_name; + message.header.stamp = impl_->clock_->now(); + for (const drake::geometry::FrameId & frame_id : inspector.all_frame_ids()) { + if (frame_id != inspector.world_frame_id()) { + continue; + } + const drake::math::RigidTransform & X_FW = + query_object.GetPoseInWord(frame_id); + message.child_frame_id = inspector.GetName(frame_id); + const drake::math::Vector3 & p_FW = X_FW.translation(); + message.transform.translation.x = p_FW.x(); + message.transform.translation.y = p_FW.y(); + message.transform.translation.z = p_FW.z(); + const Eigen::Quaternion R_FW = X_FW.rotation().ToQuaternion(); + message.transform.rotation.x = R_FW.x(); + message.transform.rotation.y = R_FW.y(); + message.transform.rotation.z = R_FW.z(); + message.transform.rotation.w = R_FW.w(); + all_messages.push_back(message); + } + impl_->tf_broadcaster_->sendTransform(all_messages); + return drake::systems::EventStatus::Succeeded(); +} + +} // namespace drake_ros_systems From b875a68315ec549a8b071852fca9580201979553 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Tue, 20 Apr 2021 12:38:30 -0300 Subject: [PATCH 41/87] Add missing deps to drake_ros_systems package.xml Signed-off-by: Michel Hidalgo --- drake_ros_systems/package.xml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/drake_ros_systems/package.xml b/drake_ros_systems/package.xml index 0f5d0d28..79c9e4ad 100644 --- a/drake_ros_systems/package.xml +++ b/drake_ros_systems/package.xml @@ -11,10 +11,12 @@ rclcpp rosidl_runtime_c rosidl_typesupport_cpp + tf2_ros ament_cmake_gtest ament_lint_auto ament_lint_common + test_msgs ament_cmake From 14149a9bc8d860f802902d4d8eb9c85a17f5dc41 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Tue, 20 Apr 2021 12:40:11 -0300 Subject: [PATCH 42/87] Wrap up TfBroadcater in C++ and Python. Also, feed clock separately via a RosClockSystem. Signed-off-by: Michel Hidalgo --- drake_ros_systems/CMakeLists.txt | 1 + .../include/drake_ros_systems/drake_ros.hpp | 2 + .../drake_ros_systems/drake_ros_interface.hpp | 2 + .../drake_ros_systems/ros_clock_system.hpp | 44 ++++++++++++++++ .../tf_broadcaster_system.hpp | 5 +- drake_ros_systems/src/drake_ros.cpp | 2 +- .../module_drake_ros_systems.cpp | 51 +++++++++++-------- drake_ros_systems/src/ros_clock_system.cpp | 49 ++++++++++++++++++ .../src/tf_broadcaster_system.cpp | 41 +++++++++------ 9 files changed, 159 insertions(+), 38 deletions(-) create mode 100644 drake_ros_systems/include/drake_ros_systems/ros_clock_system.hpp create mode 100644 drake_ros_systems/src/ros_clock_system.cpp diff --git a/drake_ros_systems/CMakeLists.txt b/drake_ros_systems/CMakeLists.txt index 6c0645c0..9e910aab 100644 --- a/drake_ros_systems/CMakeLists.txt +++ b/drake_ros_systems/CMakeLists.txt @@ -27,6 +27,7 @@ find_package(tf2_ros REQUIRED) add_library(drake_ros_systems src/drake_ros.cpp src/publisher.cpp + src/ros_clock_system.cpp src/ros_interface_system.cpp src/ros_publisher_system.cpp src/ros_subscriber_system.cpp 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 39d9498b..64e68fb6 100644 --- a/drake_ros_systems/include/drake_ros_systems/drake_ros.hpp +++ b/drake_ros_systems/include/drake_ros_systems/drake_ros.hpp @@ -15,9 +15,11 @@ #define DRAKE_ROS_SYSTEMS__DRAKE_ROS_HPP_ #include +#include #include #include #include +#include #include #include 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 6b07321a..07439312 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,8 +15,10 @@ #define DRAKE_ROS_SYSTEMS__DRAKE_ROS_INTERFACE_HPP_ #include +#include #include #include +#include #include #include diff --git a/drake_ros_systems/include/drake_ros_systems/ros_clock_system.hpp b/drake_ros_systems/include/drake_ros_systems/ros_clock_system.hpp new file mode 100644 index 00000000..e11a8751 --- /dev/null +++ b/drake_ros_systems/include/drake_ros_systems/ros_clock_system.hpp @@ -0,0 +1,44 @@ +// Copyright 2021 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#ifndef DRAKE_ROS_SYSTEMS__ROS_CLOCK_SYSTEM_HPP_ +#define DRAKE_ROS_SYSTEMS__ROS_CLOCK_SYSTEM_HPP_ + +#include + +#include + +#include "drake_ros_systems/drake_ros_interface.hpp" + + +namespace drake_ros_systems +{ +// PIMPL forward declaration +class RosClockSystemPrivate; + +class RosClockSystem : public drake::systems::LeafSystem +{ +public: + RosClockSystem(DrakeRosInterface * ros_interface); + virtual ~RosClockSystem() = default; + +private: + void + DoCalcOutput( + const drake::systems::Context & context, + drake::AbstractValue * output_value) const; + + std::unique_ptr impl_; +}; +} // namespace drake_ros_systems +#endif // DRAKE_ROS_SYSTEMS__ROS_CLOCK_SYSTEM_HPP_ diff --git a/drake_ros_systems/include/drake_ros_systems/tf_broadcaster_system.hpp b/drake_ros_systems/include/drake_ros_systems/tf_broadcaster_system.hpp index 3bba65d7..fd9544c6 100644 --- a/drake_ros_systems/include/drake_ros_systems/tf_broadcaster_system.hpp +++ b/drake_ros_systems/include/drake_ros_systems/tf_broadcaster_system.hpp @@ -30,12 +30,15 @@ class TfBroadcasterSystem : public drake::systems::LeafSystem { public: TfBroadcasterSystem( - DrakeRosInterface * ros_interface + DrakeRosInterface * ros_interface, const std::unordered_set & publish_triggers, double publish_period = 0.0); virtual ~TfBroadcasterSystem() = default; private: + drake::systems::EventStatus + DoPublishFrames(const drake::systems::Context & context) const; + std::unique_ptr impl_; }; } // namespace drake_ros_systems diff --git a/drake_ros_systems/src/drake_ros.cpp b/drake_ros_systems/src/drake_ros.cpp index a6a544f1..8234076d 100644 --- a/drake_ros_systems/src/drake_ros.cpp +++ b/drake_ros_systems/src/drake_ros.cpp @@ -97,7 +97,7 @@ DrakeRos::create_subscription( std::unique_ptr DrakeRos::create_tf_broadcaster() { - return std::make_unique(impl_->node_); + return std::make_unique(*impl_->node_); } std::shared_ptr 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 f6fd7f10..ba20c430 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 @@ -19,6 +19,7 @@ #include #include "drake_ros_systems/drake_ros.hpp" +#include "drake_ros_systems/ros_clock_system.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" @@ -51,6 +52,14 @@ PYBIND11_MODULE(drake_ros_systems, m) { // get_ros_interface py::class_>(m, "DrakeRosInterface"); + py::class_>(m, "RosClockSystem") + .def( + py::init( + [](DrakeRosInterface * ros_interface) + { + return std::make_unique(ros_interface); + })); + py::class_>(m, "RosInterfaceSystem") .def( py::init( @@ -113,25 +122,25 @@ PYBIND11_MODULE(drake_ros_systems, m) { })); py::class_>(m, "TfBroadcasterSystem") - .def( - py::init( - [](DrakeRosInterface* ros_interface) - { - constexpr double kZeroPublishPeriod{0.0}; - return std::make_unique( - ros_interface, - std::unordered_set{ - TriggerType::kPerStep, TriggerType::kForced}, - kZeroPublishPeriod); - })) - .def( - py::init( - []( - DrakeRosInterface* ros_interface - std::unordered_set publish_triggers, - double publish_period) - { - return std::make_unique( - ros_interface, publish_triggers, publish_period); - })); + .def( + py::init( + [](DrakeRosInterface * ros_interface) + { + constexpr double kZeroPublishPeriod{0.0}; + return std::make_unique( + ros_interface, + std::unordered_set{ + TriggerType::kPerStep, TriggerType::kForced}, + kZeroPublishPeriod); + })) + .def( + py::init( + []( + DrakeRosInterface * ros_interface, + std::unordered_set publish_triggers, + double publish_period) + { + return std::make_unique( + ros_interface, publish_triggers, publish_period); + })); } diff --git a/drake_ros_systems/src/ros_clock_system.cpp b/drake_ros_systems/src/ros_clock_system.cpp new file mode 100644 index 00000000..23ccbccd --- /dev/null +++ b/drake_ros_systems/src/ros_clock_system.cpp @@ -0,0 +1,49 @@ +// Copyright 2021 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include + +#include "drake_ros_systems/ros_clock_system.hpp" + + +namespace drake_ros_systems +{ +class RosClockSystemPrivate +{ +public: + std::shared_ptr clock_; +}; + +RosClockSystem::RosClockSystem(DrakeRosInterface * ros) +: impl_(new RosClockSystemPrivate()) +{ + impl_->clock_ = ros->get_clock(); + + DeclareAbstractOutputPort("clock", &RosClockSystem::DoCalcOutput); +} + +void +RosClockSystem::DoCalcOutput( + const drake::systems::Context &, + drake::AbstractValue * output_value) const +{ + // Transfer message from state to output port + output_value->set_value(std::chrono::nanoseconds( + impl_->clock_->now().nanoseconds())); +} + +} // namespace drake_ros_systems diff --git a/drake_ros_systems/src/tf_broadcaster_system.cpp b/drake_ros_systems/src/tf_broadcaster_system.cpp index 6e6b0dc3..cc46644b 100644 --- a/drake_ros_systems/src/tf_broadcaster_system.cpp +++ b/drake_ros_systems/src/tf_broadcaster_system.cpp @@ -1,4 +1,4 @@ -// Copyright 2020 Open Source Robotics Foundation, Inc. +// Copyright 2021 Open Source Robotics Foundation, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -17,8 +17,13 @@ #include #include +#include +#include +#include +#include + #include -#include // NOLINT +#include #include "drake_ros_systems/tf_broadcaster_system.hpp" @@ -29,20 +34,23 @@ class TfBroadcasterSystemPrivate { public: std::unique_ptr tf_broadcaster_; - std::shared_ptr clock_; + const drake::systems::InputPort * query_object_port_{nullptr}; + const drake::systems::InputPort * clock_port_{nullptr}; }; TfBroadcasterSystem::TfBroadcasterSystem( DrakeRosInterface * ros, const std::unordered_set & publish_triggers, double publish_period) -: impl_(new TFBroadcasterSystemPrivate()) +: impl_(new TfBroadcasterSystemPrivate()) { - impl_->clock_ = ros->get_clock(); impl_->tf_broadcaster_ = ros->create_tf_broadcaster(); - DeclareAbstractInputPort( - "graph_query", drake::Value>{}); + impl_->query_object_port_ = &DeclareAbstractInputPort( + "query_object", drake::Value>{}); + + impl_->clock_port_ = &DeclareAbstractInputPort( + "clock", drake::Value{}); // vvv Mostly copied from LcmPublisherSystem vvv using TriggerType = drake::systems::TriggerType; @@ -61,7 +69,7 @@ TfBroadcasterSystem::TfBroadcasterSystem( // 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(TriggerType::kForced) != publish_triggers.end()) { - this->DeclareForcedPublishEvent(&TFBroadcasterSystem::DoPublishFrames); + this->DeclareForcedPublishEvent(&TfBroadcasterSystem::DoPublishFrames); } if (publish_triggers.find(TriggerType::kPeriodic) != publish_triggers.end()) { @@ -95,26 +103,29 @@ TfBroadcasterSystem::DoPublishFrames( const drake::systems::Context & context) const { const drake::geometry::QueryObject & query_object = - get_input_port().Eval>(context); - const drake::geometry::SceneGraphInspector & inspector = - query_object.inspector(); + impl_->query_object_port_->Eval>(context); + const drake::geometry::SceneGraphInspector & inspector = query_object.inspector(); // TODO(hidmic): publish frame transforms w.r.t. to their parent frame // instead of the world frame when an API is made available. std::vector all_messages; all_messages.reserve(inspector.num_frames() - 1); const std::string & world_frame_name = - inspector.Getname(inspector.world_frame_id()); + inspector.GetName(inspector.world_frame_id()); geometry_msgs::msg::TransformStamped message; message.header.frame_id = world_frame_name; - message.header.stamp = impl_->clock_->now(); + const std::chrono::nanoseconds & time = + impl_->clock_port_->Eval(context); + auto sec = std::chrono::duration_cast(time); + message.header.stamp.sec = sec.count(); + message.header.stamp.nanosec = (time - sec).count(); for (const drake::geometry::FrameId & frame_id : inspector.all_frame_ids()) { if (frame_id != inspector.world_frame_id()) { continue; } const drake::math::RigidTransform & X_FW = - query_object.GetPoseInWord(frame_id); + query_object.GetPoseInWorld(frame_id); message.child_frame_id = inspector.GetName(frame_id); - const drake::math::Vector3 & p_FW = X_FW.translation(); + const drake::Vector3 & p_FW = X_FW.translation(); message.transform.translation.x = p_FW.x(); message.transform.translation.y = p_FW.y(); message.transform.translation.z = p_FW.z(); From fa227ff8717b14ad75d1415a99ffede73832c4b3 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Tue, 20 Apr 2021 13:00:35 -0300 Subject: [PATCH 43/87] drake_ros_systems library, bindings, and example building Signed-off-by: Michel Hidalgo --- drake_ros_systems/CMakeLists.txt | 1 + drake_ros_systems/example/rs_flip_flop.cpp | 2 +- .../include/drake_ros_systems/ros_clock_system.hpp | 9 +++++---- .../drake_ros_systems/tf_broadcaster_system.hpp | 2 +- .../src/python_bindings/module_drake_ros_systems.cpp | 1 + drake_ros_systems/src/ros_clock_system.cpp | 10 +++++++--- drake_ros_systems/src/ros_subscriber_system.cpp | 4 ++-- drake_ros_systems/src/tf_broadcaster_system.cpp | 4 ++++ 8 files changed, 22 insertions(+), 11 deletions(-) diff --git a/drake_ros_systems/CMakeLists.txt b/drake_ros_systems/CMakeLists.txt index 9e910aab..738ec080 100644 --- a/drake_ros_systems/CMakeLists.txt +++ b/drake_ros_systems/CMakeLists.txt @@ -40,6 +40,7 @@ target_link_libraries(drake_ros_systems PUBLIC rclcpp::rclcpp rosidl_runtime_c::rosidl_runtime_c rosidl_typesupport_cpp::rosidl_typesupport_cpp + tf2_ros::tf2_ros ) target_include_directories(drake_ros_systems PUBLIC diff --git a/drake_ros_systems/example/rs_flip_flop.cpp b/drake_ros_systems/example/rs_flip_flop.cpp index 16c38da0..bbb2afb7 100644 --- a/drake_ros_systems/example/rs_flip_flop.cpp +++ b/drake_ros_systems/example/rs_flip_flop.cpp @@ -66,7 +66,7 @@ class Memory : public drake::systems::LeafSystem DeclareAbstractInputPort("value", *drake::AbstractValue::Make(T())); // State for value - DeclareAbstractState(drake::AbstractValue::Make(initial_value)); + DeclareAbstractState(drake::Value(initial_value)); // Output depends only on the previous state DeclareAbstractOutputPort("value", &Memory::calc_output_value, {all_state_ticket()}); diff --git a/drake_ros_systems/include/drake_ros_systems/ros_clock_system.hpp b/drake_ros_systems/include/drake_ros_systems/ros_clock_system.hpp index e11a8751..b7f463ca 100644 --- a/drake_ros_systems/include/drake_ros_systems/ros_clock_system.hpp +++ b/drake_ros_systems/include/drake_ros_systems/ros_clock_system.hpp @@ -14,10 +14,11 @@ #ifndef DRAKE_ROS_SYSTEMS__ROS_CLOCK_SYSTEM_HPP_ #define DRAKE_ROS_SYSTEMS__ROS_CLOCK_SYSTEM_HPP_ -#include - +#include #include +#include + #include "drake_ros_systems/drake_ros_interface.hpp" @@ -30,13 +31,13 @@ class RosClockSystem : public drake::systems::LeafSystem { public: RosClockSystem(DrakeRosInterface * ros_interface); - virtual ~RosClockSystem() = default; + virtual ~RosClockSystem(); private: void DoCalcOutput( const drake::systems::Context & context, - drake::AbstractValue * output_value) const; + std::chrono::nanoseconds * output_value) const; std::unique_ptr impl_; }; diff --git a/drake_ros_systems/include/drake_ros_systems/tf_broadcaster_system.hpp b/drake_ros_systems/include/drake_ros_systems/tf_broadcaster_system.hpp index fd9544c6..766af72e 100644 --- a/drake_ros_systems/include/drake_ros_systems/tf_broadcaster_system.hpp +++ b/drake_ros_systems/include/drake_ros_systems/tf_broadcaster_system.hpp @@ -33,7 +33,7 @@ class TfBroadcasterSystem : public drake::systems::LeafSystem DrakeRosInterface * ros_interface, const std::unordered_set & publish_triggers, double publish_period = 0.0); - virtual ~TfBroadcasterSystem() = default; + virtual ~TfBroadcasterSystem(); private: drake::systems::EventStatus 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 ba20c430..bcfea9dc 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 @@ -36,6 +36,7 @@ using drake::systems::TriggerType; using drake_ros_systems::DrakeRos; using drake_ros_systems::DrakeRosInterface; using drake_ros_systems::PySerializer; +using drake_ros_systems::RosClockSystem; using drake_ros_systems::RosInterfaceSystem; using drake_ros_systems::RosPublisherSystem; using drake_ros_systems::RosSubscriberSystem; diff --git a/drake_ros_systems/src/ros_clock_system.cpp b/drake_ros_systems/src/ros_clock_system.cpp index 23ccbccd..6e3c1c2a 100644 --- a/drake_ros_systems/src/ros_clock_system.cpp +++ b/drake_ros_systems/src/ros_clock_system.cpp @@ -36,14 +36,18 @@ RosClockSystem::RosClockSystem(DrakeRosInterface * ros) DeclareAbstractOutputPort("clock", &RosClockSystem::DoCalcOutput); } +RosClockSystem::~RosClockSystem() +{ +} + void RosClockSystem::DoCalcOutput( const drake::systems::Context &, - drake::AbstractValue * output_value) const + std::chrono::nanoseconds * output_value) const { // Transfer message from state to output port - output_value->set_value(std::chrono::nanoseconds( - impl_->clock_->now().nanoseconds())); + *output_value = std::chrono::nanoseconds( + impl_->clock_->now().nanoseconds()); } } // namespace drake_ros_systems diff --git a/drake_ros_systems/src/ros_subscriber_system.cpp b/drake_ros_systems/src/ros_subscriber_system.cpp index 4ead3cd7..1feb72dd 100644 --- a/drake_ros_systems/src/ros_subscriber_system.cpp +++ b/drake_ros_systems/src/ros_subscriber_system.cpp @@ -67,14 +67,14 @@ RosSubscriberSystem::RosSubscriberSystem( std::bind(&RosSubscriberSystemPrivate::handle_message, impl_.get(), std::placeholders::_1)); DeclareAbstractOutputPort( - [serializer{impl_->serializer_.get()}]() {return serializer->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(impl_->serializer_->create_default_value()); + DeclareAbstractState(*(impl_->serializer_->create_default_value())); } RosSubscriberSystem::~RosSubscriberSystem() diff --git a/drake_ros_systems/src/tf_broadcaster_system.cpp b/drake_ros_systems/src/tf_broadcaster_system.cpp index cc46644b..6586ad3a 100644 --- a/drake_ros_systems/src/tf_broadcaster_system.cpp +++ b/drake_ros_systems/src/tf_broadcaster_system.cpp @@ -98,6 +98,10 @@ TfBroadcasterSystem::TfBroadcasterSystem( // ^^^ Mostly copied from LcmPublisherSystem ^^^ } +TfBroadcasterSystem::~TfBroadcasterSystem() +{ +} + drake::systems::EventStatus TfBroadcasterSystem::DoPublishFrames( const drake::systems::Context & context) const From 6182d43a29a8c48f1fea700620811ab4a86e063b Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Tue, 20 Apr 2021 14:55:35 -0300 Subject: [PATCH 44/87] Tweak TfBroadcasterSystem port names Signed-off-by: Michel Hidalgo --- drake_ros_systems/src/tf_broadcaster_system.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/drake_ros_systems/src/tf_broadcaster_system.cpp b/drake_ros_systems/src/tf_broadcaster_system.cpp index 6586ad3a..216cf877 100644 --- a/drake_ros_systems/src/tf_broadcaster_system.cpp +++ b/drake_ros_systems/src/tf_broadcaster_system.cpp @@ -34,7 +34,7 @@ class TfBroadcasterSystemPrivate { public: std::unique_ptr tf_broadcaster_; - const drake::systems::InputPort * query_object_port_{nullptr}; + const drake::systems::InputPort * graph_query_port_{nullptr}; const drake::systems::InputPort * clock_port_{nullptr}; }; @@ -46,8 +46,8 @@ TfBroadcasterSystem::TfBroadcasterSystem( { impl_->tf_broadcaster_ = ros->create_tf_broadcaster(); - impl_->query_object_port_ = &DeclareAbstractInputPort( - "query_object", drake::Value>{}); + impl_->graph_query_port_ = &DeclareAbstractInputPort( + "graph_query", drake::Value>{}); impl_->clock_port_ = &DeclareAbstractInputPort( "clock", drake::Value{}); @@ -107,7 +107,7 @@ TfBroadcasterSystem::DoPublishFrames( const drake::systems::Context & context) const { const drake::geometry::QueryObject & query_object = - impl_->query_object_port_->Eval>(context); + impl_->graph_query_port_->Eval>(context); const drake::geometry::SceneGraphInspector & inspector = query_object.inspector(); // TODO(hidmic): publish frame transforms w.r.t. to their parent frame // instead of the world frame when an API is made available. From 02821b2d4385777dc828f83bcf33171d1157adff Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Tue, 20 Apr 2021 18:50:14 -0300 Subject: [PATCH 45/87] WIP: Work on iiwa_manipulator demo Signed-off-by: Michel Hidalgo --- drake_ros_systems/example/iiwa_manipulator.py | 103 ++++++++++++++++++ .../drake_ros_systems/ros_clock_system.hpp | 3 +- .../tf_broadcaster_system.hpp | 4 + .../module_drake_ros_systems.cpp | 7 +- drake_ros_systems/src/ros_clock_system.cpp | 5 +- .../src/tf_broadcaster_system.cpp | 73 ++++++++----- 6 files changed, 158 insertions(+), 37 deletions(-) create mode 100755 drake_ros_systems/example/iiwa_manipulator.py diff --git a/drake_ros_systems/example/iiwa_manipulator.py b/drake_ros_systems/example/iiwa_manipulator.py new file mode 100755 index 00000000..ea44cdc0 --- /dev/null +++ b/drake_ros_systems/example/iiwa_manipulator.py @@ -0,0 +1,103 @@ +#!/usr/bin/env python + +import numpy as np + +from drake_ros_systems import RosClockSystem +from drake_ros_systems import RosInterfaceSystem +from drake_ros_systems import RosPublisherSystem +from drake_ros_systems import TfBroadcasterSystem + +from pydrake.common import FindResourceOrThrow +from pydrake.common.value import AbstractValue +from pydrake.examples.manipulation_station import ManipulationStation +from pydrake.systems.analysis import Simulator +from pydrake.systems.framework import DiagramBuilder +from pydrake.systems.framework import TriggerType +from pydrake.systems.primitives import ConstantValueSource + +from rclpy.qos import DurabilityPolicy +from rclpy.qos import HistoryPolicy +from rclpy.qos import ReliabilityPolicy +from rclpy.qos import QoSProfile +from std_msgs.msg import String + + +def main(): + builder = DiagramBuilder() + + ros_interface_system = builder.AddSystem(RosInterfaceSystem()) + + manipulation_station = builder.AddSystem(ManipulationStation()) + manipulation_station.SetupClutterClearingStation() + manipulation_station.Finalize() + + clock_system = builder.AddSystem( + RosClockSystem(ros_interface_system.get_ros_interface())) + + tf_broadcaster_system = builder.AddSystem(TfBroadcasterSystem( + ros_interface_system.get_ros_interface(), + {'iiwa': ''}, # drop iiwa namespace from frame names + {TriggerType.kForced, TriggerType.kPeriodic}, + 0.1)) + + builder.Connect( + clock_system.GetOutputPort('clock'), + tf_broadcaster_system.GetInputPort('clock') + ) + + builder.Connect( + manipulation_station.GetOutputPort('query_object'), + tf_broadcaster_system.GetInputPort('graph_query') + ) + + # TODO(hidmic): fetch SDF path from ManipulationStation when/if it is exposed + manipulator_sdf_path = FindResourceOrThrow( + 'drake/manipulation/models/iiwa_description/iiwa7/' + 'iiwa7_no_collision.sdf') + with open(manipulator_sdf_path, 'r') as f: + manipulator_sdf_string = f.read() + + robot_description_source = builder.AddSystem( + ConstantValueSource(AbstractValue.Make( + String(data=manipulator_sdf_string)))) + + qos = QoSProfile(depth=1) + qos.history = HistoryPolicy.KEEP_LAST + qos.durability = DurabilityPolicy.TRANSIENT_LOCAL + qos.reliability = ReliabilityPolicy.RELIABLE + + robot_description_publisher = builder.AddSystem( + RosPublisherSystem(String, '/robot_description', qos, + ros_interface_system.get_ros_interface(), + {TriggerType.kForced}, 0)) + + builder.Connect( + robot_description_source.get_output_port(), + robot_description_publisher.get_input_port() + ) + + diagram = builder.Build() + + simulator = Simulator(diagram) + simulator.Initialize() + simulator.set_target_realtime_rate(1.0) + context = simulator.get_mutable_context() + + manipulation_station_context = \ + diagram.GetMutableSubsystemContext(manipulation_station, context) + manipulation_station.GetInputPort('iiwa_position').FixValue( + manipulation_station_context, + manipulation_station.GetIiwaPosition(manipulation_station_context)) + manipulation_station.GetInputPort('wsg_position').FixValue( + manipulation_station_context, np.zeros(1)) + + try: + diagram.Publish(context) + while True: + simulator.AdvanceTo(context.get_time() + 0.1) + except KeyboardInterrupt: + pass + + +if __name__ == '__main__': + main() diff --git a/drake_ros_systems/include/drake_ros_systems/ros_clock_system.hpp b/drake_ros_systems/include/drake_ros_systems/ros_clock_system.hpp index b7f463ca..11f754fd 100644 --- a/drake_ros_systems/include/drake_ros_systems/ros_clock_system.hpp +++ b/drake_ros_systems/include/drake_ros_systems/ros_clock_system.hpp @@ -14,7 +14,6 @@ #ifndef DRAKE_ROS_SYSTEMS__ROS_CLOCK_SYSTEM_HPP_ #define DRAKE_ROS_SYSTEMS__ROS_CLOCK_SYSTEM_HPP_ -#include #include #include @@ -37,7 +36,7 @@ class RosClockSystem : public drake::systems::LeafSystem void DoCalcOutput( const drake::systems::Context & context, - std::chrono::nanoseconds * output_value) const; + double * output_value) const; std::unique_ptr impl_; }; diff --git a/drake_ros_systems/include/drake_ros_systems/tf_broadcaster_system.hpp b/drake_ros_systems/include/drake_ros_systems/tf_broadcaster_system.hpp index 766af72e..7e491b8e 100644 --- a/drake_ros_systems/include/drake_ros_systems/tf_broadcaster_system.hpp +++ b/drake_ros_systems/include/drake_ros_systems/tf_broadcaster_system.hpp @@ -17,6 +17,9 @@ #include #include +#include +#include +#include #include "drake_ros_systems/drake_ros_interface.hpp" @@ -31,6 +34,7 @@ class TfBroadcasterSystem : public drake::systems::LeafSystem public: TfBroadcasterSystem( DrakeRosInterface * ros_interface, + const std::unordered_map & remappings, const std::unordered_set & publish_triggers, double publish_period = 0.0); virtual ~TfBroadcasterSystem(); 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 bcfea9dc..057cbddd 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 @@ -14,6 +14,7 @@ #include #include +#include #include #include @@ -127,9 +128,10 @@ PYBIND11_MODULE(drake_ros_systems, m) { py::init( [](DrakeRosInterface * ros_interface) { + const std::unordered_map kNoRemappings{}; constexpr double kZeroPublishPeriod{0.0}; return std::make_unique( - ros_interface, + ros_interface, kNoRemappings, std::unordered_set{ TriggerType::kPerStep, TriggerType::kForced}, kZeroPublishPeriod); @@ -138,10 +140,11 @@ PYBIND11_MODULE(drake_ros_systems, m) { py::init( []( DrakeRosInterface * ros_interface, + std::unordered_map remappings, std::unordered_set publish_triggers, double publish_period) { return std::make_unique( - ros_interface, publish_triggers, publish_period); + ros_interface, remappings, publish_triggers, publish_period); })); } diff --git a/drake_ros_systems/src/ros_clock_system.cpp b/drake_ros_systems/src/ros_clock_system.cpp index 6e3c1c2a..c74f677d 100644 --- a/drake_ros_systems/src/ros_clock_system.cpp +++ b/drake_ros_systems/src/ros_clock_system.cpp @@ -43,11 +43,10 @@ RosClockSystem::~RosClockSystem() void RosClockSystem::DoCalcOutput( const drake::systems::Context &, - std::chrono::nanoseconds * output_value) const + double * output_value) const { // Transfer message from state to output port - *output_value = std::chrono::nanoseconds( - impl_->clock_->now().nanoseconds()); + *output_value = impl_->clock_->now().seconds(); } } // namespace drake_ros_systems diff --git a/drake_ros_systems/src/tf_broadcaster_system.cpp b/drake_ros_systems/src/tf_broadcaster_system.cpp index 216cf877..382539ae 100644 --- a/drake_ros_systems/src/tf_broadcaster_system.cpp +++ b/drake_ros_systems/src/tf_broadcaster_system.cpp @@ -21,6 +21,7 @@ #include #include #include +#include #include #include @@ -34,23 +35,25 @@ class TfBroadcasterSystemPrivate { public: std::unique_ptr tf_broadcaster_; + std::unordered_map remappings_; const drake::systems::InputPort * graph_query_port_{nullptr}; const drake::systems::InputPort * clock_port_{nullptr}; }; TfBroadcasterSystem::TfBroadcasterSystem( DrakeRosInterface * ros, + const std::unordered_map & remappings, const std::unordered_set & publish_triggers, double publish_period) : impl_(new TfBroadcasterSystemPrivate()) { impl_->tf_broadcaster_ = ros->create_tf_broadcaster(); + impl_->remappings_ = remappings; impl_->graph_query_port_ = &DeclareAbstractInputPort( "graph_query", drake::Value>{}); - impl_->clock_port_ = &DeclareAbstractInputPort( - "clock", drake::Value{}); + impl_->clock_port_ = &DeclareAbstractInputPort("clock", drake::Value{}); // vvv Mostly copied from LcmPublisherSystem vvv using TriggerType = drake::systems::TriggerType; @@ -111,36 +114,46 @@ TfBroadcasterSystem::DoPublishFrames( const drake::geometry::SceneGraphInspector & inspector = query_object.inspector(); // TODO(hidmic): publish frame transforms w.r.t. to their parent frame // instead of the world frame when an API is made available. - std::vector all_messages; - all_messages.reserve(inspector.num_frames() - 1); - const std::string & world_frame_name = - inspector.GetName(inspector.world_frame_id()); - geometry_msgs::msg::TransformStamped message; - message.header.frame_id = world_frame_name; - const std::chrono::nanoseconds & time = - impl_->clock_port_->Eval(context); - auto sec = std::chrono::duration_cast(time); - message.header.stamp.sec = sec.count(); - message.header.stamp.nanosec = (time - sec).count(); - for (const drake::geometry::FrameId & frame_id : inspector.all_frame_ids()) { - if (frame_id != inspector.world_frame_id()) { - continue; + if (inspector.num_frames() > 1) { + std::vector all_messages; + all_messages.reserve(inspector.num_frames() - 1); + const std::string & world_frame_name = + inspector.GetName(inspector.world_frame_id()); + geometry_msgs::msg::TransformStamped message; + message.header.frame_id = world_frame_name; + const double time = impl_->clock_port_->Eval(context); + message.header.stamp = rclcpp::Time() + rclcpp::Duration::from_seconds(time); + for (const drake::geometry::FrameId & frame_id : inspector.all_frame_ids()) { + if (frame_id == inspector.world_frame_id()) { + continue; + } + const drake::math::RigidTransform & X_FW = + query_object.GetPoseInWorld(frame_id); + std::string frame_name = inspector.GetName(frame_id); + if (impl_->remappings_.count(frame_name) > 0) { + frame_name = impl_->remappings_[frame_name]; + } + drake::multibody::parsing::ScopedName scoped_frame_name = + drake::multibody::parsing::ParseScopedName(frame_name); + if (impl_->remappings_.count(scoped_frame_name.instance_name) > 0) { + scoped_frame_name.instance_name = + impl_->remappings_[scoped_frame_name.instance_name]; + } + message.child_frame_id = drake::multibody::parsing::PrefixName( + scoped_frame_name.instance_name, scoped_frame_name.name); + const drake::Vector3 & p_FW = X_FW.translation(); + message.transform.translation.x = p_FW.x(); + message.transform.translation.y = p_FW.y(); + message.transform.translation.z = p_FW.z(); + const Eigen::Quaternion R_FW = X_FW.rotation().ToQuaternion(); + message.transform.rotation.x = R_FW.x(); + message.transform.rotation.y = R_FW.y(); + message.transform.rotation.z = R_FW.z(); + message.transform.rotation.w = R_FW.w(); + all_messages.push_back(message); } - const drake::math::RigidTransform & X_FW = - query_object.GetPoseInWorld(frame_id); - message.child_frame_id = inspector.GetName(frame_id); - const drake::Vector3 & p_FW = X_FW.translation(); - message.transform.translation.x = p_FW.x(); - message.transform.translation.y = p_FW.y(); - message.transform.translation.z = p_FW.z(); - const Eigen::Quaternion R_FW = X_FW.rotation().ToQuaternion(); - message.transform.rotation.x = R_FW.x(); - message.transform.rotation.y = R_FW.y(); - message.transform.rotation.z = R_FW.z(); - message.transform.rotation.w = R_FW.w(); - all_messages.push_back(message); + impl_->tf_broadcaster_->sendTransform(all_messages); } - impl_->tf_broadcaster_->sendTransform(all_messages); return drake::systems::EventStatus::Succeeded(); } From 4c3e1c7df244dbc9669e9a24ee3a0673494b4469 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Wed, 21 Apr 2021 20:09:28 -0300 Subject: [PATCH 46/87] Add type conversion utilities Pose & Transform messages Signed-off-by: Michel Hidalgo --- .../utilities/type_conversion.hpp | 34 ++++++++++ .../src/utilities/type_conversion.cpp | 66 +++++++++++++++++++ 2 files changed, 100 insertions(+) create mode 100644 drake_ros_systems/include/drake_ros_systems/utilities/type_conversion.hpp create mode 100644 drake_ros_systems/src/utilities/type_conversion.cpp diff --git a/drake_ros_systems/include/drake_ros_systems/utilities/type_conversion.hpp b/drake_ros_systems/include/drake_ros_systems/utilities/type_conversion.hpp new file mode 100644 index 00000000..63f67783 --- /dev/null +++ b/drake_ros_systems/include/drake_ros_systems/utilities/type_conversion.hpp @@ -0,0 +1,34 @@ +// Copyright 2021 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#ifndef DRAKE_ROS_SYSTEMS__UTILITIES__TYPE_CONVERSION_HPP_ +#define DRAKE_ROS_SYSTEMS__UTILITIES__TYPE_CONVERSION_HPP_ + +#include + +#include +#include + +namespace drake_ros_systems { +namespace utilities { + +geometry_msgs::msg::Pose +ToPoseMsg(const drake::math::RigidTransform X_AB); + +geometry_msgs::msg::Transform +ToTransformMsg(const drake::math::RigidTransform X_AB); + +} // namespace utilities +} // namespace drake_ros_systems + +#endif // DRAKE_ROS_SYSTEMS__UTILITIES__TYPE_CONVERSION_HPP_ diff --git a/drake_ros_systems/src/utilities/type_conversion.cpp b/drake_ros_systems/src/utilities/type_conversion.cpp new file mode 100644 index 00000000..1b58ba9a --- /dev/null +++ b/drake_ros_systems/src/utilities/type_conversion.cpp @@ -0,0 +1,66 @@ +// Copyright 2021 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include + +#include "drake_ros_systems/utilities/type_conversion.hpp" + +namespace drake_ros_systems { +namespace utilities { + +geometry_msgs::msg::Pose +ToPoseMsg(const drake::math::RigidTransform X_AB) +{ + geometry_msgs::msg::Pose msg; + + const drake::Vector3 & p_AB = + X_AB.translation(); + msg.position.x = p_AB.x(); + msg.position.y = p_AB.y(); + msg.position.z = p_AB.z(); + const drake::Quaternion R_AB = + X_AB.rotation().ToQuaternion(); + msg.orientation.x = R_AB.x(); + msg.orientation.y = R_AB.y(); + msg.orientation.z = R_AB.z(); + msg.orientation.w = R_AB.w(); + + return msg; +} + +geometry_msgs::msg::Transform +ToTransformMsg(const drake::math::RigidTransform X_AB) +{ + geometry_msgs::msg::Transform msg; + + const drake::Vector3 & p_AB = + X_AB.translation(); + msg.translation.x = p_AB.x(); + msg.translation.y = p_AB.y(); + msg.translation.z = p_AB.z(); + const Eigen::Quaternion R_AB = + X_AB.rotation().ToQuaternion(); + msg.rotation.x = R_AB.x(); + msg.rotation.y = R_AB.y(); + msg.rotation.z = R_AB.z(); + msg.rotation.w = R_AB.w(); + + return msg; +} + +} // namespace utilities +} // namespace drake_ros_systems From 8982f840083e5787809dd10de56d0ac295dc4578 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Wed, 21 Apr 2021 20:10:05 -0300 Subject: [PATCH 47/87] Tweak TfBroadcaster system Signed-off-by: Michel Hidalgo --- .../tf_broadcaster_system.hpp | 3 - .../src/tf_broadcaster_system.cpp | 65 ++++++------------- 2 files changed, 21 insertions(+), 47 deletions(-) diff --git a/drake_ros_systems/include/drake_ros_systems/tf_broadcaster_system.hpp b/drake_ros_systems/include/drake_ros_systems/tf_broadcaster_system.hpp index 7e491b8e..e2682dc9 100644 --- a/drake_ros_systems/include/drake_ros_systems/tf_broadcaster_system.hpp +++ b/drake_ros_systems/include/drake_ros_systems/tf_broadcaster_system.hpp @@ -17,8 +17,6 @@ #include #include -#include -#include #include #include "drake_ros_systems/drake_ros_interface.hpp" @@ -34,7 +32,6 @@ class TfBroadcasterSystem : public drake::systems::LeafSystem public: TfBroadcasterSystem( DrakeRosInterface * ros_interface, - const std::unordered_map & remappings, const std::unordered_set & publish_triggers, double publish_period = 0.0); virtual ~TfBroadcasterSystem(); diff --git a/drake_ros_systems/src/tf_broadcaster_system.cpp b/drake_ros_systems/src/tf_broadcaster_system.cpp index 382539ae..fa0f0627 100644 --- a/drake_ros_systems/src/tf_broadcaster_system.cpp +++ b/drake_ros_systems/src/tf_broadcaster_system.cpp @@ -20,13 +20,12 @@ #include #include #include -#include -#include #include #include #include "drake_ros_systems/tf_broadcaster_system.hpp" +#include "drake_ros_systems/utilities/type_conversion.hpp" namespace drake_ros_systems @@ -35,25 +34,24 @@ class TfBroadcasterSystemPrivate { public: std::unique_ptr tf_broadcaster_; - std::unordered_map remappings_; - const drake::systems::InputPort * graph_query_port_{nullptr}; - const drake::systems::InputPort * clock_port_{nullptr}; + drake::systems::InputPortIndex graph_query_port_index; + drake::systems::InputPortIndex clock_port_index; }; TfBroadcasterSystem::TfBroadcasterSystem( DrakeRosInterface * ros, - const std::unordered_map & remappings, const std::unordered_set & publish_triggers, double publish_period) : impl_(new TfBroadcasterSystemPrivate()) { impl_->tf_broadcaster_ = ros->create_tf_broadcaster(); - impl_->remappings_ = remappings; - impl_->graph_query_port_ = &DeclareAbstractInputPort( - "graph_query", drake::Value>{}); + impl_->graph_query_port_index = + this->DeclareAbstractInputPort( + "graph_query", drake::Value>{}).get_index(); - impl_->clock_port_ = &DeclareAbstractInputPort("clock", drake::Value{}); + impl_->clock_port_index = + this->DeclareAbstractInputPort("clock", drake::Value{}).get_index(); // vvv Mostly copied from LcmPublisherSystem vvv using TriggerType = drake::systems::TriggerType; @@ -110,49 +108,28 @@ TfBroadcasterSystem::DoPublishFrames( const drake::systems::Context & context) const { const drake::geometry::QueryObject & query_object = - impl_->graph_query_port_->Eval>(context); + get_input_port(impl_->graph_query_port_index) + .Eval>(context); const drake::geometry::SceneGraphInspector & inspector = query_object.inspector(); // TODO(hidmic): publish frame transforms w.r.t. to their parent frame // instead of the world frame when an API is made available. if (inspector.num_frames() > 1) { - std::vector all_messages; - all_messages.reserve(inspector.num_frames() - 1); - const std::string & world_frame_name = - inspector.GetName(inspector.world_frame_id()); - geometry_msgs::msg::TransformStamped message; - message.header.frame_id = world_frame_name; - const double time = impl_->clock_port_->Eval(context); - message.header.stamp = rclcpp::Time() + rclcpp::Duration::from_seconds(time); + std::vector transforms; + transforms.reserve(inspector.num_frames() - 1); + geometry_msgs::msg::TransformStamped transform; + transform.header.frame_id = inspector.GetName(inspector.world_frame_id()); + const double time = get_input_port(impl_->clock_port_index).Eval(context); + transform.header.stamp = rclcpp::Time() + rclcpp::Duration::from_seconds(time); for (const drake::geometry::FrameId & frame_id : inspector.all_frame_ids()) { if (frame_id == inspector.world_frame_id()) { continue; } - const drake::math::RigidTransform & X_FW = - query_object.GetPoseInWorld(frame_id); - std::string frame_name = inspector.GetName(frame_id); - if (impl_->remappings_.count(frame_name) > 0) { - frame_name = impl_->remappings_[frame_name]; - } - drake::multibody::parsing::ScopedName scoped_frame_name = - drake::multibody::parsing::ParseScopedName(frame_name); - if (impl_->remappings_.count(scoped_frame_name.instance_name) > 0) { - scoped_frame_name.instance_name = - impl_->remappings_[scoped_frame_name.instance_name]; - } - message.child_frame_id = drake::multibody::parsing::PrefixName( - scoped_frame_name.instance_name, scoped_frame_name.name); - const drake::Vector3 & p_FW = X_FW.translation(); - message.transform.translation.x = p_FW.x(); - message.transform.translation.y = p_FW.y(); - message.transform.translation.z = p_FW.z(); - const Eigen::Quaternion R_FW = X_FW.rotation().ToQuaternion(); - message.transform.rotation.x = R_FW.x(); - message.transform.rotation.y = R_FW.y(); - message.transform.rotation.z = R_FW.z(); - message.transform.rotation.w = R_FW.w(); - all_messages.push_back(message); + transform.child_frame_id = inspector.GetName(frame_id); + transform.transform = + utilities::ToTransformMsg(query_object.GetPoseInWorld(frame_id)); + transforms.push_back(transform); } - impl_->tf_broadcaster_->sendTransform(all_messages); + impl_->tf_broadcaster_->sendTransform(transforms); } return drake::systems::EventStatus::Succeeded(); } From 38b36c4fe1c6f33eb10e41ec464e2ac176c04778 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Wed, 21 Apr 2021 20:10:42 -0300 Subject: [PATCH 48/87] Add C++ SceneMarkersSystem. To populate a visualization_msgs::msg::MarkerArray from geometry data in the connected SceneGraph. Signed-off-by: Michel Hidalgo --- .../scene_markers_system.hpp | 59 ++++ .../src/scene_markers_system.cpp | 328 ++++++++++++++++++ 2 files changed, 387 insertions(+) create mode 100644 drake_ros_systems/include/drake_ros_systems/scene_markers_system.hpp create mode 100644 drake_ros_systems/src/scene_markers_system.cpp diff --git a/drake_ros_systems/include/drake_ros_systems/scene_markers_system.hpp b/drake_ros_systems/include/drake_ros_systems/scene_markers_system.hpp new file mode 100644 index 00000000..b95e7d4a --- /dev/null +++ b/drake_ros_systems/include/drake_ros_systems/scene_markers_system.hpp @@ -0,0 +1,59 @@ +// Copyright 2021 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#ifndef DRAKE_ROS_SYSTEMS__SCENE_MARKERS_SYSTEM_HPP_ +#define DRAKE_ROS_SYSTEMS__SCENE_MARKERS_SYSTEM_HPP_ + +#include + +#include +#include +#include + +#include + + +namespace drake_ros_systems +{ + +class SceneMarkersSystem : public drake::systems::LeafSystem +{ +public: + SceneMarkersSystem( + const drake::geometry::Role & role = drake::geometry::Role::kIllustration, + const drake::geometry::Rgba & default_color = {0.9, 0.9, 0.9, 1.0}); + virtual ~SceneMarkersSystem(); + +private: + void + PopulateSceneMarkersMessage( + const drake::systems::Context & context, + visualization_msgs::msg::MarkerArray * output_value) const; + + const visualization_msgs::msg::MarkerArray & + EvalSceneMarkers(const drake::systems::Context & context) const; + + void + CalcSceneMarkers( + const drake::systems::Context & context, + visualization_msgs::msg::MarkerArray * output_value) const; + + // PIMPL forward declaration + class SceneMarkersSystemPrivate; + + std::unique_ptr impl_; +}; + +} // namespace drake_ros_systems + +#endif // DRAKE_ROS_SYSTEMS__SCENE_MARKERS_SYSTEM_HPP_ diff --git a/drake_ros_systems/src/scene_markers_system.cpp b/drake_ros_systems/src/scene_markers_system.cpp new file mode 100644 index 00000000..8314c83e --- /dev/null +++ b/drake_ros_systems/src/scene_markers_system.cpp @@ -0,0 +1,328 @@ +// Copyright 2021 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include + +#include "drake_ros_systems/scene_markers_system.hpp" +#include "drake_ros_systems/utilities/type_conversion.hpp" + + +namespace drake_ros_systems { + +namespace { + +class SceneGeometryToMarkers : public drake::geometry::ShapeReifier { +public: + DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(SceneGeometryToMarkers) + + SceneGeometryToMarkers( + const drake::geometry::Role & role, + const drake::geometry::Rgba & default_color) + : role_(role), default_color_(default_color) + { + } + + ~SceneGeometryToMarkers() override = default; + + void Populate( + const drake::geometry::SceneGraphInspector & inspector, + const drake::geometry::GeometryId & geometry_id, + visualization_msgs::msg::MarkerArray * marker_array) + { + DRAKE_ASSERT(nullptr != marker_array); + + marker_array_ = marker_array; + + prototype_marker_.header.frame_id = + inspector.GetName(inspector.GetFrameId(geometry_id)); + prototype_marker_.ns = inspector.GetOwningSourceName(geometry_id) + + "::" + inspector.GetName(geometry_id); + prototype_marker_.id = 0; + prototype_marker_.action = visualization_msgs::msg::Marker::MODIFY; + + prototype_marker_.lifetime = rclcpp::Duration::from_nanoseconds(0); + prototype_marker_.frame_locked = true; + + const drake::geometry::GeometryProperties * props = + inspector.GetProperties(geometry_id, role_); + DRAKE_ASSERT(nullptr != props); + drake::geometry::Rgba default_color = default_color_; + if (drake::geometry::Role::kIllustration != role_) { + const drake::geometry::IllustrationProperties * illustration_props = + inspector.GetIllustrationProperties(geometry_id); + if (illustration_props) { + default_color = + illustration_props->GetPropertyOrDefault( + "phong", "diffuse", default_color); + } + } + const drake::geometry::Rgba & color = + props->GetPropertyOrDefault("phong", "diffuse", default_color); + prototype_marker_.color.r = color.r(); + prototype_marker_.color.g = color.g(); + prototype_marker_.color.b = color.b(); + prototype_marker_.color.a = color.a(); + + X_FG_ = inspector.GetPoseInFrame(geometry_id); + + inspector.GetShape(geometry_id).Reify(this); + } + +private: + using drake::geometry::ShapeReifier::ImplementGeometry; + + void ImplementGeometry(const drake::geometry::Sphere & sphere, void *) override + { + prototype_marker_.type = visualization_msgs::msg::Marker::SPHERE; + prototype_marker_.scale.x = sphere.radius(); + prototype_marker_.scale.y = sphere.radius(); + prototype_marker_.scale.z = sphere.radius(); + prototype_marker_.pose = utilities::ToPoseMsg(X_FG_); + marker_array_->markers.push_back(prototype_marker_); + } + + void ImplementGeometry(const drake::geometry::Ellipsoid & ellipsoid, void *) override + { + prototype_marker_.type = visualization_msgs::msg::Marker::SPHERE; + prototype_marker_.scale.x = ellipsoid.a(); + prototype_marker_.scale.y = ellipsoid.b(); + prototype_marker_.scale.z = ellipsoid.c(); + prototype_marker_.pose = utilities::ToPoseMsg(X_FG_); + + marker_array_->markers.push_back(prototype_marker_); + } + + void ImplementGeometry(const drake::geometry::Cylinder & cylinder, void *) override + { + prototype_marker_.type = visualization_msgs::msg::Marker::CYLINDER; + prototype_marker_.scale.x = cylinder.radius(); + prototype_marker_.scale.y = cylinder.radius(); + prototype_marker_.scale.z = cylinder.length(); + prototype_marker_.pose = utilities::ToPoseMsg(X_FG_); + + marker_array_->markers.push_back(prototype_marker_); + } + + void ImplementGeometry(const drake::geometry::HalfSpace &, void *) override + { + constexpr double kHalfSpaceLength = 50.; + constexpr double kHalfSpaceThickness = 1.; + + prototype_marker_.type = visualization_msgs::msg::Marker::CUBE; + prototype_marker_.scale.x = kHalfSpaceLength; + prototype_marker_.scale.y = kHalfSpaceLength; + prototype_marker_.scale.z = kHalfSpaceThickness; + const drake::math::RigidTransform X_GH{ + drake::Vector3{0., 0., -kHalfSpaceThickness / 2.}}; + const drake::math::RigidTransform X_FH = X_FG_ * X_GH; + prototype_marker_.pose = utilities::ToPoseMsg(X_FH); + + marker_array_->markers.push_back(prototype_marker_); + } + + void ImplementGeometry(const drake::geometry::Box & box, void *) override + { + prototype_marker_.type = visualization_msgs::msg::Marker::CUBE; + prototype_marker_.scale.x = box.width(); + prototype_marker_.scale.y = box.depth(); + prototype_marker_.scale.z = box.height(); + prototype_marker_.pose = utilities::ToPoseMsg(X_FG_); + + marker_array_->markers.push_back(prototype_marker_); + } + + void ImplementGeometry(const drake::geometry::Capsule & capsule, void *) override + { + visualization_msgs::msg::Marker body_marker = prototype_marker_; + body_marker.type = visualization_msgs::msg::Marker::CYLINDER; + body_marker.scale.x = capsule.radius(); + body_marker.scale.y = capsule.radius(); + body_marker.scale.z = capsule.length(); + prototype_marker_.pose = utilities::ToPoseMsg(X_FG_); + + marker_array_->markers.push_back(body_marker); + + visualization_msgs::msg::Marker upper_cap_marker = prototype_marker_; + upper_cap_marker.id = 1; + upper_cap_marker.type = visualization_msgs::msg::Marker::SPHERE; + upper_cap_marker.scale.x = capsule.radius(); + upper_cap_marker.scale.y = capsule.radius(); + upper_cap_marker.scale.z = capsule.radius(); + const drake::math::RigidTransform X_GU{ + drake::Vector3{0., 0., capsule.length() / 2.}}; + const drake::math::RigidTransform X_FU = X_FG_ * X_GU; + upper_cap_marker.pose = utilities::ToPoseMsg(X_FU); + + marker_array_->markers.push_back(upper_cap_marker); + + visualization_msgs::msg::Marker lower_cap_marker = upper_cap_marker; + lower_cap_marker.id = 2; + const drake::math::RigidTransform X_GL{ + drake::Vector3{0., 0., -capsule.length() / 2.}}; + const drake::math::RigidTransform X_FL = X_FG_ * X_GL; + lower_cap_marker.pose = utilities::ToPoseMsg(X_FL); + + marker_array_->markers.push_back(lower_cap_marker); + } + + void ImplementGeometry(const drake::geometry::Convex & convex, void *) override + { + prototype_marker_.type = visualization_msgs::msg::Marker::MESH_RESOURCE; + prototype_marker_.scale.x = convex.scale(); + prototype_marker_.scale.y = convex.scale(); + prototype_marker_.scale.z = convex.scale(); + // Assume it is an absolute path and turn it into a file URL. + prototype_marker_.mesh_resource = "file://" + convex.filename(); + prototype_marker_.pose = utilities::ToPoseMsg(X_FG_); + + marker_array_->markers.push_back(prototype_marker_); + } + + void ImplementGeometry(const drake::geometry::Mesh & mesh, void *) override + { + prototype_marker_.type = visualization_msgs::msg::Marker::MESH_RESOURCE; + prototype_marker_.scale.x = mesh.scale(); + prototype_marker_.scale.y = mesh.scale(); + prototype_marker_.scale.z = mesh.scale(); + // Assume it is an absolute path and turn it into a file URL. + prototype_marker_.mesh_resource = "file://" + mesh.filename(); + prototype_marker_.pose = utilities::ToPoseMsg(X_FG_); + + marker_array_->markers.push_back(prototype_marker_); + } + + const drake::geometry::Role role_; + const drake::geometry::Rgba default_color_; + visualization_msgs::msg::MarkerArray * marker_array_{nullptr}; + visualization_msgs::msg::Marker prototype_marker_{}; + drake::math::RigidTransform X_FG_{}; +}; + +} // namespace + +class SceneMarkersSystem::SceneMarkersSystemPrivate +{ +public: + SceneMarkersSystemPrivate( + const drake::geometry::Role & _role, + const drake::geometry::Rgba & _default_color) + : role(_role), default_color(_default_color) + { + } + + const drake::geometry::Role role; + const drake::geometry::Rgba default_color; + mutable drake::geometry::GeometryVersion version; + drake::systems::CacheIndex scene_markers_cache_index; + drake::systems::InputPortIndex graph_query_port_index; + drake::systems::InputPortIndex clock_port_index; +}; + +SceneMarkersSystem::SceneMarkersSystem( + const drake::geometry::Role & role, + const drake::geometry::Rgba & default_color) +: impl_(new SceneMarkersSystemPrivate(role, default_color)) +{ + impl_->graph_query_port_index = + this->DeclareAbstractInputPort( + "graph_query", drake::Value>{}).get_index(); + + impl_->clock_port_index = + this->DeclareAbstractInputPort("clock", drake::Value{}).get_index(); + + impl_->scene_markers_cache_index = + this->DeclareCacheEntry("scene_markers_cache", + &SceneMarkersSystem::CalcSceneMarkers, + {nothing_ticket()}).cache_index(); + + this->DeclareAbstractOutputPort( + "scene_markers", &SceneMarkersSystem::PopulateSceneMarkersMessage); +} + +SceneMarkersSystem::~SceneMarkersSystem() +{ +} + +void +SceneMarkersSystem::PopulateSceneMarkersMessage( + const drake::systems::Context & context, + visualization_msgs::msg::MarkerArray * output_value) const +{ + *output_value = this->EvalSceneMarkers(context); + + rclcpp::Time current_time; + current_time += rclcpp::Duration::from_seconds( + get_input_port(impl_->clock_port_index).Eval(context)); + builtin_interfaces::msg::Time stamp = current_time; + for (visualization_msgs::msg::Marker & marker : output_value->markers) + { + marker.header.stamp = stamp; + } +} + +const visualization_msgs::msg::MarkerArray & +SceneMarkersSystem::EvalSceneMarkers( + const drake::systems::Context & context) const +{ + const drake::geometry::QueryObject & query_object = + get_input_port(impl_->graph_query_port_index) + .Eval>(context); + const drake::geometry::GeometryVersion & current_version = + query_object.inspector().geometry_version(); + if (!impl_->version.IsSameAs(current_version, impl_->role)) { + // Invalidate scene markers cache + get_cache_entry(impl_->scene_markers_cache_index) + .get_mutable_cache_entry_value(context) + .mark_out_of_date(); + impl_->version = current_version; + } + return get_cache_entry(impl_->scene_markers_cache_index) + .Eval(context); +} + +void +SceneMarkersSystem::CalcSceneMarkers( + const drake::systems::Context & context, + visualization_msgs::msg::MarkerArray * output_value) const +{ + const drake::geometry::QueryObject & query_object = + get_input_port(impl_->graph_query_port_index) + .Eval>(context); + const drake::geometry::SceneGraphInspector & inspector = query_object.inspector(); + output_value->markers.reserve(inspector.NumGeometriesWithRole(impl_->role)); + for (const drake::geometry::FrameId & frame_id : inspector.all_frame_ids()) { + for (const drake::geometry::GeometryId & geometry_id : + inspector.GetGeometries(frame_id, impl_->role)) { + SceneGeometryToMarkers(impl_->role, impl_->default_color) + .Populate(inspector, geometry_id, output_value); + } + } +} + +} // drake_ros_systems From d8f0806ed1b911722cc299ddde32f3668a604ede Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Wed, 21 Apr 2021 20:11:55 -0300 Subject: [PATCH 49/87] Add C++ RvizVisualizer system to wire all up. Signed-off-by: Michel Hidalgo --- .../drake_ros_systems/rviz_visualizer.hpp | 40 ++++++++++++ drake_ros_systems/src/rviz_visualizer.cpp | 65 +++++++++++++++++++ 2 files changed, 105 insertions(+) create mode 100644 drake_ros_systems/include/drake_ros_systems/rviz_visualizer.hpp create mode 100644 drake_ros_systems/src/rviz_visualizer.cpp diff --git a/drake_ros_systems/include/drake_ros_systems/rviz_visualizer.hpp b/drake_ros_systems/include/drake_ros_systems/rviz_visualizer.hpp new file mode 100644 index 00000000..ec1864d3 --- /dev/null +++ b/drake_ros_systems/include/drake_ros_systems/rviz_visualizer.hpp @@ -0,0 +1,40 @@ +// Copyright 2021 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#ifndef DRAKE_ROS_SYSTEMS__RVIZ_VISUALIZER_HPP_ +#define DRAKE_ROS_SYSTEMS__RVIZ_VISUALIZER_HPP_ + +#include + +#include +#include + +#include "drake_ros_systems/drake_ros_interface.hpp" + + +namespace drake_ros_systems { + +class RvizVisualizer : public drake::systems::Diagram +{ +public: + RvizVisualizer( + std::shared_ptr ros_interface, + const std::unordered_set & publish_triggers = { + drake::systems::TriggerType::kForced, drake::systems::TriggerType::kPeriodic}, + double publish_period = 0.05, + bool publish_tf = true); +}; + +} // namespace drake_ros_systems + +#endif // DRAKE_ROS_SYSTEMS__RVIZ_VISUALIZER_HPP_ diff --git a/drake_ros_systems/src/rviz_visualizer.cpp b/drake_ros_systems/src/rviz_visualizer.cpp new file mode 100644 index 00000000..8886119c --- /dev/null +++ b/drake_ros_systems/src/rviz_visualizer.cpp @@ -0,0 +1,65 @@ +// Copyright 2021 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include + +#include + +#include + +#include "drake_ros_systems/ros_publisher_system.hpp" +#include "drake_ros_systems/scene_markers_system.hpp" +#include "drake_ros_systems/tf_broadcaster_system.hpp" + +#include "drake_ros_systems/rviz_visualizer.hpp" + + +namespace drake_ros_systems { + +RvizVisualizer::RvizVisualizer( + std::shared_ptr ros_interface, + const std::unordered_set & publish_triggers, + double publish_period, bool publish_tf) +{ + drake::systems::DiagramBuilder builder; + + auto scene_markers = builder.AddSystem(); + + auto scene_markers_publisher = builder.AddSystem( + RosPublisherSystem::Make( + "/scene_markers", rclcpp::QoS(1), ros_interface, publish_triggers, publish_period)); + + builder.Connect( + scene_markers->GetOutputPort("scene_markers"), + scene_markers_publisher->GetInputPort("message")); + + builder.ExportInput(scene_markers->GetInputPort("clock"), "clock"); + builder.ExportInput(scene_markers->GetInputPort("graph_query"), "graph_query"); + + if (publish_tf) { + auto tf_broadcaster = builder.AddSystem( + ros_interface.get(), publish_triggers, publish_period); + + builder.ConnectInput("clock", tf_broadcaster->GetInputPort("clock")); + builder.ConnectInput("graph_query", tf_broadcaster->GetInputPort("graph_query")); + } + + builder.BuildInto(this); +} + +} // namespace drake_ros_systems + From 0082091509c80e86b4aa06cba62cd83189a1c27d Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Wed, 21 Apr 2021 20:12:37 -0300 Subject: [PATCH 50/87] Add Python bindings for RvizVisualizer and SceneMarkersSystem Signed-off-by: Michel Hidalgo --- .../module_drake_ros_systems.cpp | 38 +++++++++++++++++-- 1 file changed, 34 insertions(+), 4 deletions(-) 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 057cbddd..649687bd 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,8 @@ #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 "drake_ros_systems/rviz_visualizer.hpp" +#include "drake_ros_systems/scene_markers_system.hpp" #include "drake_ros_systems/tf_broadcaster_system.hpp" #include "py_serializer.hpp" @@ -31,6 +33,7 @@ namespace py = pybind11; +using drake::systems::Diagram; using drake::systems::LeafSystem; using drake::systems::TriggerType; @@ -41,6 +44,8 @@ using drake_ros_systems::RosClockSystem; using drake_ros_systems::RosInterfaceSystem; using drake_ros_systems::RosPublisherSystem; using drake_ros_systems::RosSubscriberSystem; +using drake_ros_systems::RvizVisualizer; +using drake_ros_systems::SceneMarkersSystem; using drake_ros_systems::SerializerInterface; using drake_ros_systems::TfBroadcasterSystem; @@ -128,10 +133,9 @@ PYBIND11_MODULE(drake_ros_systems, m) { py::init( [](DrakeRosInterface * ros_interface) { - const std::unordered_map kNoRemappings{}; constexpr double kZeroPublishPeriod{0.0}; return std::make_unique( - ros_interface, kNoRemappings, + ros_interface, std::unordered_set{ TriggerType::kPerStep, TriggerType::kForced}, kZeroPublishPeriod); @@ -140,11 +144,37 @@ PYBIND11_MODULE(drake_ros_systems, m) { py::init( []( DrakeRosInterface * ros_interface, - std::unordered_map remappings, std::unordered_set publish_triggers, double publish_period) { return std::make_unique( - ros_interface, remappings, publish_triggers, publish_period); + ros_interface, publish_triggers, publish_period); + })); + + py::class_>(m, "SceneMarkersSystem") + .def(py::init([]() { return std::make_unique(); })) + .def( + py::init( + [](const drake::geometry::Role & role, + const drake::geometry::Rgba & default_color) + { + return std::make_unique(role, default_color); + })); + + py::class_>(m, "RvizVisualizer") + .def( + py::init( + [](std::shared_ptr ros_interface) + { + return std::make_unique(ros_interface); + })) + .def( + py::init( + [](std::shared_ptr ros_interface, + std::unordered_set publish_triggers, + double publish_period, bool publish_tf) + { + return std::make_unique( + ros_interface, publish_triggers, publish_period, publish_tf); })); } From 9d3d5cf15e33e22e0d0561c118d83694bb9f5f12 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Wed, 21 Apr 2021 20:12:52 -0300 Subject: [PATCH 51/87] Update build system Signed-off-by: Michel Hidalgo --- drake_ros_systems/CMakeLists.txt | 6 ++++++ drake_ros_systems/package.xml | 1 + 2 files changed, 7 insertions(+) diff --git a/drake_ros_systems/CMakeLists.txt b/drake_ros_systems/CMakeLists.txt index 738ec080..a3c81338 100644 --- a/drake_ros_systems/CMakeLists.txt +++ b/drake_ros_systems/CMakeLists.txt @@ -23,6 +23,7 @@ find_package(rclcpp REQUIRED) find_package(rosidl_runtime_c REQUIRED) find_package(rosidl_typesupport_cpp REQUIRED) find_package(tf2_ros REQUIRED) +find_package(visualization_msgs REQUIRED) add_library(drake_ros_systems src/drake_ros.cpp @@ -31,8 +32,11 @@ add_library(drake_ros_systems src/ros_interface_system.cpp src/ros_publisher_system.cpp src/ros_subscriber_system.cpp + src/rviz_visualizer.cpp + src/scene_markers_system.cpp src/subscription.cpp src/tf_broadcaster_system.cpp + src/utilities/type_conversion.cpp ) target_link_libraries(drake_ros_systems PUBLIC @@ -41,7 +45,9 @@ target_link_libraries(drake_ros_systems PUBLIC rosidl_runtime_c::rosidl_runtime_c rosidl_typesupport_cpp::rosidl_typesupport_cpp tf2_ros::tf2_ros + ${visualization_msgs_TARGETS} ) + target_include_directories(drake_ros_systems PUBLIC "$" diff --git a/drake_ros_systems/package.xml b/drake_ros_systems/package.xml index 79c9e4ad..9b6dc9d5 100644 --- a/drake_ros_systems/package.xml +++ b/drake_ros_systems/package.xml @@ -12,6 +12,7 @@ rosidl_runtime_c rosidl_typesupport_cpp tf2_ros + visualization_msgs ament_cmake_gtest ament_lint_auto From c9f392cc0b4b0229bc80ac15e9abe33b4a685311 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Wed, 21 Apr 2021 20:13:12 -0300 Subject: [PATCH 52/87] Complete iiwa_manipulator example in C++ & Python. Signed-off-by: Michel Hidalgo --- drake_ros_systems/example/CMakeLists.txt | 6 ++ .../example/iiwa_manipulator.cpp | 85 +++++++++++++++++++ drake_ros_systems/example/iiwa_manipulator.py | 53 ++---------- 3 files changed, 96 insertions(+), 48 deletions(-) create mode 100644 drake_ros_systems/example/iiwa_manipulator.cpp diff --git a/drake_ros_systems/example/CMakeLists.txt b/drake_ros_systems/example/CMakeLists.txt index 284db04e..d7733920 100644 --- a/drake_ros_systems/example/CMakeLists.txt +++ b/drake_ros_systems/example/CMakeLists.txt @@ -6,3 +6,9 @@ target_link_libraries(rs_flip_flop drake_ros_systems ${std_msgs_TARGETS} ) + +add_executable(iiwa_manipulator iiwa_manipulator.cpp) +target_link_libraries(iiwa_manipulator + drake::drake + drake_ros_systems +) diff --git a/drake_ros_systems/example/iiwa_manipulator.cpp b/drake_ros_systems/example/iiwa_manipulator.cpp new file mode 100644 index 00000000..cd714b83 --- /dev/null +++ b/drake_ros_systems/example/iiwa_manipulator.cpp @@ -0,0 +1,85 @@ +// Copyright 2021 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#include +#include + +#include + +#include +#include +#include +#include + +#include +#include + +using drake_ros_systems::DrakeRos; +using drake_ros_systems::RosClockSystem; +using drake_ros_systems::RosInterfaceSystem; +using drake_ros_systems::RvizVisualizer; + +using drake::examples::manipulation_station::ManipulationStation; + + +int main() { + drake::systems::DiagramBuilder builder; + + auto ros_interface_system = + builder.AddSystem(std::make_unique()); + + auto manipulation_station = builder.AddSystem(); + manipulation_station->SetupClutterClearingStation(); + manipulation_station->Finalize(); + + auto clock_system = builder.AddSystem( + ros_interface_system->get_ros_interface().get()); + + auto rviz_visualizer = builder.AddSystem( + ros_interface_system->get_ros_interface()); + + builder.Connect( + clock_system->GetOutputPort("clock"), + rviz_visualizer->GetInputPort("clock")); + + builder.Connect( + manipulation_station->GetOutputPort("query_object"), + rviz_visualizer->GetInputPort("graph_query")); + + 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(); + + auto & manipulation_station_context = + diagram->GetMutableSubsystemContext(*manipulation_station, &simulator_context); + + manipulation_station->GetInputPort("iiwa_position") + .FixValue( + &manipulation_station_context, + manipulation_station->GetIiwaPosition(manipulation_station_context)); + + manipulation_station->GetInputPort("wsg_position") + .FixValue(&manipulation_station_context, 0.); + + while (true) { + simulator->AdvanceTo(simulator_context.get_time() + 0.1); + } + return 0; + +} diff --git a/drake_ros_systems/example/iiwa_manipulator.py b/drake_ros_systems/example/iiwa_manipulator.py index ea44cdc0..5e535c69 100755 --- a/drake_ros_systems/example/iiwa_manipulator.py +++ b/drake_ros_systems/example/iiwa_manipulator.py @@ -1,25 +1,12 @@ #!/usr/bin/env python -import numpy as np - from drake_ros_systems import RosClockSystem from drake_ros_systems import RosInterfaceSystem -from drake_ros_systems import RosPublisherSystem -from drake_ros_systems import TfBroadcasterSystem +from drake_ros_systems import RvizVisualizer -from pydrake.common import FindResourceOrThrow -from pydrake.common.value import AbstractValue from pydrake.examples.manipulation_station import ManipulationStation from pydrake.systems.analysis import Simulator from pydrake.systems.framework import DiagramBuilder -from pydrake.systems.framework import TriggerType -from pydrake.systems.primitives import ConstantValueSource - -from rclpy.qos import DurabilityPolicy -from rclpy.qos import HistoryPolicy -from rclpy.qos import ReliabilityPolicy -from rclpy.qos import QoSProfile -from std_msgs.msg import String def main(): @@ -34,46 +21,17 @@ def main(): clock_system = builder.AddSystem( RosClockSystem(ros_interface_system.get_ros_interface())) - tf_broadcaster_system = builder.AddSystem(TfBroadcasterSystem( - ros_interface_system.get_ros_interface(), - {'iiwa': ''}, # drop iiwa namespace from frame names - {TriggerType.kForced, TriggerType.kPeriodic}, - 0.1)) + rviz_visualizer = builder.AddSystem( + RvizVisualizer(ros_interface_system.get_ros_interface())) builder.Connect( clock_system.GetOutputPort('clock'), - tf_broadcaster_system.GetInputPort('clock') + rviz_visualizer.GetInputPort('clock') ) builder.Connect( manipulation_station.GetOutputPort('query_object'), - tf_broadcaster_system.GetInputPort('graph_query') - ) - - # TODO(hidmic): fetch SDF path from ManipulationStation when/if it is exposed - manipulator_sdf_path = FindResourceOrThrow( - 'drake/manipulation/models/iiwa_description/iiwa7/' - 'iiwa7_no_collision.sdf') - with open(manipulator_sdf_path, 'r') as f: - manipulator_sdf_string = f.read() - - robot_description_source = builder.AddSystem( - ConstantValueSource(AbstractValue.Make( - String(data=manipulator_sdf_string)))) - - qos = QoSProfile(depth=1) - qos.history = HistoryPolicy.KEEP_LAST - qos.durability = DurabilityPolicy.TRANSIENT_LOCAL - qos.reliability = ReliabilityPolicy.RELIABLE - - robot_description_publisher = builder.AddSystem( - RosPublisherSystem(String, '/robot_description', qos, - ros_interface_system.get_ros_interface(), - {TriggerType.kForced}, 0)) - - builder.Connect( - robot_description_source.get_output_port(), - robot_description_publisher.get_input_port() + rviz_visualizer.GetInputPort('graph_query') ) diagram = builder.Build() @@ -92,7 +50,6 @@ def main(): manipulation_station_context, np.zeros(1)) try: - diagram.Publish(context) while True: simulator.AdvanceTo(context.get_time() + 0.1) except KeyboardInterrupt: From 483e437827f276e3856a94fa215dd9cb0345cc7d Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Tue, 27 Apr 2021 12:24:31 -0300 Subject: [PATCH 53/87] Update drake_ros_systems README.md Signed-off-by: Michel Hidalgo --- drake_ros_systems/README.md | 103 ++++++++++++++++++++++++++---------- 1 file changed, 74 insertions(+), 29 deletions(-) diff --git a/drake_ros_systems/README.md b/drake_ros_systems/README.md index 231f8377..6cab6aae 100644 --- a/drake_ros_systems/README.md +++ b/drake_ros_systems/README.md @@ -2,18 +2,18 @@ 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`. +It explores ROS 2 equivalents of `LcmInterfaceSystem`, `LcmPublisherSystem`, `LcmSubscriberSystem`, and `DrakeVisualizer`. # Building -This package has been built and tested on Ubuntu Focal with ROS Rolling, using a Drake nightly from November 2020. +This package has been built and tested on Ubuntu Focal with ROS Rolling, using a Drake nightly from April 2021. It may work on other versions of ROS and Drake, but it hasn't been tested. To build it: 1. [Install ROS Rolling](https://index.ros.org/doc/ros2/Installation/Rolling/) 1. Source your ROS installation `. /opt/ros/rolling/setup.bash` -1. [Install drake from November-ish 2020](https://drake.mit.edu/from_binary.html) +1. [Download April-ish 2021 Drake binary](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 @@ -21,23 +21,49 @@ To build it: **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. Get this code `git clone https://github.com/RobotLocomotion/drake-ros.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. Get this code `git clone https://github.com/RobotLocomotion/drake-ros.git` + 1. `cd drake-ros/drake_ros_systems` 1. Make a build and install folder to avoid installing to the whole system `mkdir build install` 1. `cd build` 1. Configure the project `cmake -DCMAKE_INSTALL_PREFIX=$(pwd)/../install ..` 1. Build the project `make && make install` -# Example +# Running -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. +If you built with `colcon`, then source your workspace. + +``` +. ./ws/install/setup.bash +# Also make sure to activate drake virtual environment +``` + +If you built with plain CMake, then source the ROS workspace and set these variables. + +``` +. /opt/ros/rolling/setup.bash +# Also make sure to activate drake virtual environment +# CD to folder containing `build` and `install` created earlier, commands below use PWD to find correct path +export LD_LIBRARY_PATH=$(pwd)/install/lib:$LD_LIBRARY_PATH +export PYTHONPATH=$(pwd)/install/lib/$(python -c 'import sys; print(f"python{sys.version_info[0]}.{sys.version_info[1]}")')/site-packages:$PYTHONPATH +``` + +Now you can run C++ examples from the build folder and Python examples from the source folder. + +# Examples + +Some examples of using these systems are given in the [`example`](./example) folder in two languages: Python and C++. + +## RS flip flop + +### Overview + +Both `rs_flip_flop` and `rs_flip_flop.py` implement an RS flip flop using NOR gates. They subscribe to the following topics: * `/R` @@ -48,6 +74,27 @@ And publish to the following topics * `/Q` * `/Q_not` +### How To + +If built with **colcon** using an isolated build (default), run the C++ example as follows: + +``` +./build/drake_ros_systems/example/rs_flip_flop +``` + +If built with **cmake** or a non-isolated build of **colcon**, run the C++ example as follows: + +``` +./build/example/rs_flip_flop +``` + + +The Python example can always be run from the source folder as follows: + +``` +python3 ./example/rs_flip_flop.py +``` + Run these commands in different terminals with your ROS installation sourced to echo the output topics: ``` @@ -70,41 +117,39 @@ ros2 topic pub /R std_msgs/msg/Bool "data: false" ros2 topic pub /R std_msgs/msg/Bool "data: true" ``` -## Running the Example +## IIWA Manipulator -If you built with `colcon`, then source your workspace. +### Overview -``` -. ./ws/install/setup.bash -# Also make sure to activate drake virtual environment -``` +Both `iiwa_manipulator` and `iiwa_manipulator.py` enable RViz visualization of a static `ManipulationStation` example. +They publish the following topics: -If you built with plain CMake, then source the ROS workspace and set these variables. +* `/tf` (all scene frames) +* `/scene_markers` (all scene geometries, including the robot model) + +### How To + +If built with **colcon** using an isolated build (default), run the C++ example as follows: ``` -. /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 +./build/drake_ros_systems/example/iiwa_manipulator ``` -Now you can run the C++ example from the build folder - -If built with **colcon** using an isolated build (default) +If built with **cmake** or a non-isolated build of **colcon**, run the C++ example as follows: ``` -./build/drake_ros_systems/example/rs_flip_flop +./build/example/iiwa_manipulator ``` -If built with **cmake** or a non-isolated build of **colcon** + +The Python example can always be run from the source folder as follows: ``` -./build/example/rs_flip_flop +python3 ./example/iiwa_manipulator.py ``` -The Python example can be run from the source folder in either case. +Run RViz in a different terminal with your ROS installation sourced to visualize the station: ``` -python3 ./example/rs_flip_flop.py +ros2 run rviz2 rviz2 -d ./example/iiwa_manipulator.rviz ``` From 1c4693ee73247d42ce10c7495e22dc4991092414 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Tue, 27 Apr 2021 12:24:51 -0300 Subject: [PATCH 54/87] Add RViz configuration for iiwa_manipulator example Signed-off-by: Michel Hidalgo --- .../example/iiwa_manipulator.rviz | 165 ++++++++++++++++++ 1 file changed, 165 insertions(+) create mode 100644 drake_ros_systems/example/iiwa_manipulator.rviz diff --git a/drake_ros_systems/example/iiwa_manipulator.rviz b/drake_ros_systems/example/iiwa_manipulator.rviz new file mode 100644 index 00000000..31a9d215 --- /dev/null +++ b/drake_ros_systems/example/iiwa_manipulator.rviz @@ -0,0 +1,165 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + Splitter Ratio: 0.5 + Tree Height: 555 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MarkerArray + Namespaces: + Source_3::bin1::back: true + Source_3::bin1::bottom: true + Source_3::bin1::left: true + Source_3::bin1::right: true + Source_3::bin1::slope: true + Source_3::bin1::visual: true + Source_3::bin2::back: true + Source_3::bin2::bottom: true + Source_3::bin2::left: true + Source_3::bin2::right: true + Source_3::bin2::slope: true + Source_3::bin2::visual: true + Source_3::gripper::visual: true + Source_3::iiwa::iiwa_link_0_visual: true + Source_3::iiwa::iiwa_link_1_visual: true + Source_3::iiwa::iiwa_link_2_visual: true + Source_3::iiwa::iiwa_link_3_visual: true + Source_3::iiwa::iiwa_link_4_visual: true + Source_3::iiwa::iiwa_link_5_visual: true + Source_3::iiwa::iiwa_link_6_visual: true + Source_3::iiwa::iiwa_link_7_visual: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /scene_markers + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: world + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 2.3739840984344482 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.07645688951015472 + Y: -0.3666839599609375 + Z: 0.10706909000873566 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.7403980493545532 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.5653908252716064 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 846 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd000000040000000000000156000002b4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000002b4000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000002b4000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000024400fffffffb0000000800540069006d006501000000000000045000000000000000000000023f000002b400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1200 + X: 67 + Y: 60 From 101e8c7fc1b4bc9905db9067d3341bebe7e3d2d5 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Tue, 27 Apr 2021 13:55:35 -0300 Subject: [PATCH 55/87] Document drake_ros_systems package systems - RosClockSystem - TfBroadcasterSystem - SceneMarkersSystem - RvizVisualizer Signed-off-by: Michel Hidalgo --- .../drake_ros_systems/ros_clock_system.hpp | 12 ++++++--- .../drake_ros_systems/rviz_visualizer.hpp | 12 +++++++++ .../scene_markers_system.hpp | 26 +++++++++++++++++++ .../tf_broadcaster_system.hpp | 17 +++++++++--- drake_ros_systems/src/ros_clock_system.cpp | 6 ++--- .../src/tf_broadcaster_system.cpp | 10 +++---- 6 files changed, 67 insertions(+), 16 deletions(-) diff --git a/drake_ros_systems/include/drake_ros_systems/ros_clock_system.hpp b/drake_ros_systems/include/drake_ros_systems/ros_clock_system.hpp index 11f754fd..ba9c43e3 100644 --- a/drake_ros_systems/include/drake_ros_systems/ros_clock_system.hpp +++ b/drake_ros_systems/include/drake_ros_systems/ros_clock_system.hpp @@ -23,9 +23,10 @@ namespace drake_ros_systems { -// PIMPL forward declaration -class RosClockSystemPrivate; - +/// Source system that abstracts the ROS clock. +/// +/// It has one output port: +/// - *clock* (abstract): clock time in seconds, as a double. class RosClockSystem : public drake::systems::LeafSystem { public: @@ -34,10 +35,13 @@ class RosClockSystem : public drake::systems::LeafSystem private: void - DoCalcOutput( + CalcOutput( const drake::systems::Context & context, double * output_value) const; + // PIMPL forward declaration + class RosClockSystemPrivate; + std::unique_ptr impl_; }; } // namespace drake_ros_systems diff --git a/drake_ros_systems/include/drake_ros_systems/rviz_visualizer.hpp b/drake_ros_systems/include/drake_ros_systems/rviz_visualizer.hpp index ec1864d3..cd8b4be7 100644 --- a/drake_ros_systems/include/drake_ros_systems/rviz_visualizer.hpp +++ b/drake_ros_systems/include/drake_ros_systems/rviz_visualizer.hpp @@ -24,6 +24,18 @@ namespace drake_ros_systems { +/// System for SceneGraph visualization in RViz. +/// +/// This system is a subdiagram aggregating a SceneMarkersSystem, a +/// RosPublisherSystem, and optionally a TfBroadcasterSystem to enable +/// SceneGraph visualization in RViz. All scene geometries are published +/// as a visualization_msgs/msg/MarkerArray message to a `/scene_markers` +/// ROS topic. If `publish_tf` is `true`, all SceneGraph frames are +/// broadcasted as tf2 transforms. +/// +/// It exports two input ports: +/// - *graph_query* (abstract): expects a QueryObject from the SceneGraph. +/// - *clock* (abstract): expects clock time in seconds, as a double. class RvizVisualizer : public drake::systems::Diagram { public: diff --git a/drake_ros_systems/include/drake_ros_systems/scene_markers_system.hpp b/drake_ros_systems/include/drake_ros_systems/scene_markers_system.hpp index b95e7d4a..8d7e2614 100644 --- a/drake_ros_systems/include/drake_ros_systems/scene_markers_system.hpp +++ b/drake_ros_systems/include/drake_ros_systems/scene_markers_system.hpp @@ -26,6 +26,25 @@ namespace drake_ros_systems { +/// System for SceneGraph depiction as a ROS markers array. +/// +/// This system outputs a `visualization_msg/msg/MarkerArray` populated with +/// all geometries found in a SceneGraph, using an external clock signal +/// to timestamp `geometry_msgs/msg/TransformStamped` messages. +/// +/// It has two input ports: +/// - *graph_query* (abstract): expects a QueryObject from the SceneGraph. +/// - *clock* (abstract): expects clock time in seconds, as a double. +/// +/// It has one output port: +/// - *scene_markers* (abstract): all scene geometries, as a +/// visualization_msg::msg::MarkerArray message. +/// +/// This system provides the same base functionality in terms of SceneGraph +/// geometries lookup and message conversion for ROS-based applications as +/// the DrakeVisualizer system does for LCM-based applications. +/// Thus, discussions in DrakeVisualizer's documentation about geometry +/// versioning, geometry roles, and so on are equally applicable here. class SceneMarkersSystem : public drake::systems::LeafSystem { public: @@ -35,14 +54,21 @@ class SceneMarkersSystem : public drake::systems::LeafSystem virtual ~SceneMarkersSystem(); private: + // Outputs visualization_msgs::msg::MarkerArray message, + // timestamping the most up-to-date version. void PopulateSceneMarkersMessage( const drake::systems::Context & context, visualization_msgs::msg::MarkerArray * output_value) const; + // Returns cached visualization_msgs::msg::MarkerArray message, + // which is invalidated (and thus recomputed) upon a SceneGraph + // geometry version change. const visualization_msgs::msg::MarkerArray & EvalSceneMarkers(const drake::systems::Context & context) const; + // Inspects the SceneGraph and carries out the conversion + // to visualization_msgs::msg::MarkerArray message unconditionally. void CalcSceneMarkers( const drake::systems::Context & context, diff --git a/drake_ros_systems/include/drake_ros_systems/tf_broadcaster_system.hpp b/drake_ros_systems/include/drake_ros_systems/tf_broadcaster_system.hpp index e2682dc9..094af95e 100644 --- a/drake_ros_systems/include/drake_ros_systems/tf_broadcaster_system.hpp +++ b/drake_ros_systems/include/drake_ros_systems/tf_broadcaster_system.hpp @@ -24,9 +24,15 @@ namespace drake_ros_systems { -// PIMPL forward declaration -class TfBroadcasterSystemPrivate; - +/// System for tf2 transform broadcasting. +/// +/// This system publishes all frame transforms found in a SceneGraph +/// to the `/tf` ROS topic, using an external clock signal to timestamp +/// `geometry_msgs/msg/TransformStamped` messages. +/// +/// It has two input ports: +/// - *graph_query* (abstract): expects a QueryObject from the SceneGraph. +/// - *clock* (abstract): expects clock time in seconds, as a double. class TfBroadcasterSystem : public drake::systems::LeafSystem { public: @@ -38,7 +44,10 @@ class TfBroadcasterSystem : public drake::systems::LeafSystem private: drake::systems::EventStatus - DoPublishFrames(const drake::systems::Context & context) const; + PublishFrames(const drake::systems::Context & context) const; + + // PIMPL forward declaration + class TfBroadcasterSystemPrivate; std::unique_ptr impl_; }; diff --git a/drake_ros_systems/src/ros_clock_system.cpp b/drake_ros_systems/src/ros_clock_system.cpp index c74f677d..0251d1e0 100644 --- a/drake_ros_systems/src/ros_clock_system.cpp +++ b/drake_ros_systems/src/ros_clock_system.cpp @@ -22,7 +22,7 @@ namespace drake_ros_systems { -class RosClockSystemPrivate +class RosClockSystem::RosClockSystemPrivate { public: std::shared_ptr clock_; @@ -33,7 +33,7 @@ RosClockSystem::RosClockSystem(DrakeRosInterface * ros) { impl_->clock_ = ros->get_clock(); - DeclareAbstractOutputPort("clock", &RosClockSystem::DoCalcOutput); + DeclareAbstractOutputPort("clock", &RosClockSystem::CalcOutput); } RosClockSystem::~RosClockSystem() @@ -41,7 +41,7 @@ RosClockSystem::~RosClockSystem() } void -RosClockSystem::DoCalcOutput( +RosClockSystem::CalcOutput( const drake::systems::Context &, double * output_value) const { diff --git a/drake_ros_systems/src/tf_broadcaster_system.cpp b/drake_ros_systems/src/tf_broadcaster_system.cpp index fa0f0627..dd286a8a 100644 --- a/drake_ros_systems/src/tf_broadcaster_system.cpp +++ b/drake_ros_systems/src/tf_broadcaster_system.cpp @@ -30,7 +30,7 @@ namespace drake_ros_systems { -class TfBroadcasterSystemPrivate +class TfBroadcasterSystem::TfBroadcasterSystemPrivate { public: std::unique_ptr tf_broadcaster_; @@ -70,7 +70,7 @@ TfBroadcasterSystem::TfBroadcasterSystem( // 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(TriggerType::kForced) != publish_triggers.end()) { - this->DeclareForcedPublishEvent(&TfBroadcasterSystem::DoPublishFrames); + this->DeclareForcedPublishEvent(&TfBroadcasterSystem::PublishFrames); } if (publish_triggers.find(TriggerType::kPeriodic) != publish_triggers.end()) { @@ -80,7 +80,7 @@ TfBroadcasterSystem::TfBroadcasterSystem( const double offset = 0.0; this->DeclarePeriodicPublishEvent( publish_period, offset, - &TfBroadcasterSystem::DoPublishFrames); + &TfBroadcasterSystem::PublishFrames); } else if (publish_period > 0) { // publish_period > 0 without drake::systems::TriggerType::kPeriodic has no meaning and is // likely a mistake. @@ -93,7 +93,7 @@ TfBroadcasterSystem::TfBroadcasterSystem( [this]( const drake::systems::Context & context, const drake::systems::PublishEvent &) { - DoPublishFrames(context); + PublishFrames(context); })); } // ^^^ Mostly copied from LcmPublisherSystem ^^^ @@ -104,7 +104,7 @@ TfBroadcasterSystem::~TfBroadcasterSystem() } drake::systems::EventStatus -TfBroadcasterSystem::DoPublishFrames( +TfBroadcasterSystem::PublishFrames( const drake::systems::Context & context) const { const drake::geometry::QueryObject & query_object = From 33e443468d391d775fd16f53af856dba70cb5a8c Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Wed, 28 Apr 2021 14:19:48 -0300 Subject: [PATCH 56/87] Drop clock system concept. Signed-off-by: Michel Hidalgo --- drake_ros_systems/CMakeLists.txt | 1 - .../example/iiwa_manipulator.cpp | 7 --- drake_ros_systems/example/iiwa_manipulator.py | 8 --- .../drake_ros_systems/ros_clock_system.hpp | 48 ----------------- .../drake_ros_systems/rviz_visualizer.hpp | 3 +- .../scene_markers_system.hpp | 7 ++- .../tf_broadcaster_system.hpp | 5 +- .../module_drake_ros_systems.cpp | 10 ---- drake_ros_systems/src/ros_clock_system.cpp | 52 ------------------- drake_ros_systems/src/rviz_visualizer.cpp | 2 - .../src/scene_markers_system.cpp | 10 +--- .../src/tf_broadcaster_system.cpp | 8 +-- 12 files changed, 10 insertions(+), 151 deletions(-) delete mode 100644 drake_ros_systems/include/drake_ros_systems/ros_clock_system.hpp delete mode 100644 drake_ros_systems/src/ros_clock_system.cpp diff --git a/drake_ros_systems/CMakeLists.txt b/drake_ros_systems/CMakeLists.txt index a3c81338..2112328e 100644 --- a/drake_ros_systems/CMakeLists.txt +++ b/drake_ros_systems/CMakeLists.txt @@ -28,7 +28,6 @@ find_package(visualization_msgs REQUIRED) add_library(drake_ros_systems src/drake_ros.cpp src/publisher.cpp - src/ros_clock_system.cpp src/ros_interface_system.cpp src/ros_publisher_system.cpp src/ros_subscriber_system.cpp diff --git a/drake_ros_systems/example/iiwa_manipulator.cpp b/drake_ros_systems/example/iiwa_manipulator.cpp index cd714b83..e53f1453 100644 --- a/drake_ros_systems/example/iiwa_manipulator.cpp +++ b/drake_ros_systems/example/iiwa_manipulator.cpp @@ -42,16 +42,9 @@ int main() { manipulation_station->SetupClutterClearingStation(); manipulation_station->Finalize(); - auto clock_system = builder.AddSystem( - ros_interface_system->get_ros_interface().get()); - auto rviz_visualizer = builder.AddSystem( ros_interface_system->get_ros_interface()); - builder.Connect( - clock_system->GetOutputPort("clock"), - rviz_visualizer->GetInputPort("clock")); - builder.Connect( manipulation_station->GetOutputPort("query_object"), rviz_visualizer->GetInputPort("graph_query")); diff --git a/drake_ros_systems/example/iiwa_manipulator.py b/drake_ros_systems/example/iiwa_manipulator.py index 5e535c69..cf6e7517 100755 --- a/drake_ros_systems/example/iiwa_manipulator.py +++ b/drake_ros_systems/example/iiwa_manipulator.py @@ -18,17 +18,9 @@ def main(): manipulation_station.SetupClutterClearingStation() manipulation_station.Finalize() - clock_system = builder.AddSystem( - RosClockSystem(ros_interface_system.get_ros_interface())) - rviz_visualizer = builder.AddSystem( RvizVisualizer(ros_interface_system.get_ros_interface())) - builder.Connect( - clock_system.GetOutputPort('clock'), - rviz_visualizer.GetInputPort('clock') - ) - builder.Connect( manipulation_station.GetOutputPort('query_object'), rviz_visualizer.GetInputPort('graph_query') diff --git a/drake_ros_systems/include/drake_ros_systems/ros_clock_system.hpp b/drake_ros_systems/include/drake_ros_systems/ros_clock_system.hpp deleted file mode 100644 index ba9c43e3..00000000 --- a/drake_ros_systems/include/drake_ros_systems/ros_clock_system.hpp +++ /dev/null @@ -1,48 +0,0 @@ -// Copyright 2021 Open Source Robotics Foundation, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -#ifndef DRAKE_ROS_SYSTEMS__ROS_CLOCK_SYSTEM_HPP_ -#define DRAKE_ROS_SYSTEMS__ROS_CLOCK_SYSTEM_HPP_ - -#include - -#include - -#include "drake_ros_systems/drake_ros_interface.hpp" - - -namespace drake_ros_systems -{ -/// Source system that abstracts the ROS clock. -/// -/// It has one output port: -/// - *clock* (abstract): clock time in seconds, as a double. -class RosClockSystem : public drake::systems::LeafSystem -{ -public: - RosClockSystem(DrakeRosInterface * ros_interface); - virtual ~RosClockSystem(); - -private: - void - CalcOutput( - const drake::systems::Context & context, - double * output_value) const; - - // PIMPL forward declaration - class RosClockSystemPrivate; - - std::unique_ptr impl_; -}; -} // namespace drake_ros_systems -#endif // DRAKE_ROS_SYSTEMS__ROS_CLOCK_SYSTEM_HPP_ diff --git a/drake_ros_systems/include/drake_ros_systems/rviz_visualizer.hpp b/drake_ros_systems/include/drake_ros_systems/rviz_visualizer.hpp index cd8b4be7..af8dcdfb 100644 --- a/drake_ros_systems/include/drake_ros_systems/rviz_visualizer.hpp +++ b/drake_ros_systems/include/drake_ros_systems/rviz_visualizer.hpp @@ -33,9 +33,8 @@ namespace drake_ros_systems { /// ROS topic. If `publish_tf` is `true`, all SceneGraph frames are /// broadcasted as tf2 transforms. /// -/// It exports two input ports: +/// It exports one input port: /// - *graph_query* (abstract): expects a QueryObject from the SceneGraph. -/// - *clock* (abstract): expects clock time in seconds, as a double. class RvizVisualizer : public drake::systems::Diagram { public: diff --git a/drake_ros_systems/include/drake_ros_systems/scene_markers_system.hpp b/drake_ros_systems/include/drake_ros_systems/scene_markers_system.hpp index 8d7e2614..4020ff4e 100644 --- a/drake_ros_systems/include/drake_ros_systems/scene_markers_system.hpp +++ b/drake_ros_systems/include/drake_ros_systems/scene_markers_system.hpp @@ -29,12 +29,11 @@ namespace drake_ros_systems /// System for SceneGraph depiction as a ROS markers array. /// /// This system outputs a `visualization_msg/msg/MarkerArray` populated with -/// all geometries found in a SceneGraph, using an external clock signal -/// to timestamp `geometry_msgs/msg/TransformStamped` messages. +/// all geometries found in a SceneGraph, using Context time to timestamp +/// `geometry_msgs/msg/TransformStamped` messages. /// -/// It has two input ports: +/// It has one input port: /// - *graph_query* (abstract): expects a QueryObject from the SceneGraph. -/// - *clock* (abstract): expects clock time in seconds, as a double. /// /// It has one output port: /// - *scene_markers* (abstract): all scene geometries, as a diff --git a/drake_ros_systems/include/drake_ros_systems/tf_broadcaster_system.hpp b/drake_ros_systems/include/drake_ros_systems/tf_broadcaster_system.hpp index 094af95e..349a9075 100644 --- a/drake_ros_systems/include/drake_ros_systems/tf_broadcaster_system.hpp +++ b/drake_ros_systems/include/drake_ros_systems/tf_broadcaster_system.hpp @@ -27,12 +27,11 @@ namespace drake_ros_systems /// System for tf2 transform broadcasting. /// /// This system publishes all frame transforms found in a SceneGraph -/// to the `/tf` ROS topic, using an external clock signal to timestamp +/// to the `/tf` ROS topic, using Context time to timestamp /// `geometry_msgs/msg/TransformStamped` messages. /// -/// It has two input ports: +/// It has one input port: /// - *graph_query* (abstract): expects a QueryObject from the SceneGraph. -/// - *clock* (abstract): expects clock time in seconds, as a double. class TfBroadcasterSystem : public drake::systems::LeafSystem { public: 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 649687bd..32b14b02 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 @@ -20,7 +20,6 @@ #include #include "drake_ros_systems/drake_ros.hpp" -#include "drake_ros_systems/ros_clock_system.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" @@ -40,7 +39,6 @@ using drake::systems::TriggerType; using drake_ros_systems::DrakeRos; using drake_ros_systems::DrakeRosInterface; using drake_ros_systems::PySerializer; -using drake_ros_systems::RosClockSystem; using drake_ros_systems::RosInterfaceSystem; using drake_ros_systems::RosPublisherSystem; using drake_ros_systems::RosSubscriberSystem; @@ -59,14 +57,6 @@ PYBIND11_MODULE(drake_ros_systems, m) { // get_ros_interface py::class_>(m, "DrakeRosInterface"); - py::class_>(m, "RosClockSystem") - .def( - py::init( - [](DrakeRosInterface * ros_interface) - { - return std::make_unique(ros_interface); - })); - py::class_>(m, "RosInterfaceSystem") .def( py::init( diff --git a/drake_ros_systems/src/ros_clock_system.cpp b/drake_ros_systems/src/ros_clock_system.cpp deleted file mode 100644 index 0251d1e0..00000000 --- a/drake_ros_systems/src/ros_clock_system.cpp +++ /dev/null @@ -1,52 +0,0 @@ -// Copyright 2021 Open Source Robotics Foundation, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include - -#include - -#include "drake_ros_systems/ros_clock_system.hpp" - - -namespace drake_ros_systems -{ -class RosClockSystem::RosClockSystemPrivate -{ -public: - std::shared_ptr clock_; -}; - -RosClockSystem::RosClockSystem(DrakeRosInterface * ros) -: impl_(new RosClockSystemPrivate()) -{ - impl_->clock_ = ros->get_clock(); - - DeclareAbstractOutputPort("clock", &RosClockSystem::CalcOutput); -} - -RosClockSystem::~RosClockSystem() -{ -} - -void -RosClockSystem::CalcOutput( - const drake::systems::Context &, - double * output_value) const -{ - // Transfer message from state to output port - *output_value = impl_->clock_->now().seconds(); -} - -} // namespace drake_ros_systems diff --git a/drake_ros_systems/src/rviz_visualizer.cpp b/drake_ros_systems/src/rviz_visualizer.cpp index 8886119c..9d172985 100644 --- a/drake_ros_systems/src/rviz_visualizer.cpp +++ b/drake_ros_systems/src/rviz_visualizer.cpp @@ -47,14 +47,12 @@ RvizVisualizer::RvizVisualizer( scene_markers->GetOutputPort("scene_markers"), scene_markers_publisher->GetInputPort("message")); - builder.ExportInput(scene_markers->GetInputPort("clock"), "clock"); builder.ExportInput(scene_markers->GetInputPort("graph_query"), "graph_query"); if (publish_tf) { auto tf_broadcaster = builder.AddSystem( ros_interface.get(), publish_triggers, publish_period); - builder.ConnectInput("clock", tf_broadcaster->GetInputPort("clock")); builder.ConnectInput("graph_query", tf_broadcaster->GetInputPort("graph_query")); } diff --git a/drake_ros_systems/src/scene_markers_system.cpp b/drake_ros_systems/src/scene_markers_system.cpp index 8314c83e..81797c8b 100644 --- a/drake_ros_systems/src/scene_markers_system.cpp +++ b/drake_ros_systems/src/scene_markers_system.cpp @@ -241,7 +241,6 @@ class SceneMarkersSystem::SceneMarkersSystemPrivate mutable drake::geometry::GeometryVersion version; drake::systems::CacheIndex scene_markers_cache_index; drake::systems::InputPortIndex graph_query_port_index; - drake::systems::InputPortIndex clock_port_index; }; SceneMarkersSystem::SceneMarkersSystem( @@ -253,9 +252,6 @@ SceneMarkersSystem::SceneMarkersSystem( this->DeclareAbstractInputPort( "graph_query", drake::Value>{}).get_index(); - impl_->clock_port_index = - this->DeclareAbstractInputPort("clock", drake::Value{}).get_index(); - impl_->scene_markers_cache_index = this->DeclareCacheEntry("scene_markers_cache", &SceneMarkersSystem::CalcSceneMarkers, @@ -276,10 +272,8 @@ SceneMarkersSystem::PopulateSceneMarkersMessage( { *output_value = this->EvalSceneMarkers(context); - rclcpp::Time current_time; - current_time += rclcpp::Duration::from_seconds( - get_input_port(impl_->clock_port_index).Eval(context)); - builtin_interfaces::msg::Time stamp = current_time; + const builtin_interfaces::msg::Time stamp = + rclcpp::Time() + rclcpp::Duration::from_seconds(context.get_time()); for (visualization_msgs::msg::Marker & marker : output_value->markers) { marker.header.stamp = stamp; diff --git a/drake_ros_systems/src/tf_broadcaster_system.cpp b/drake_ros_systems/src/tf_broadcaster_system.cpp index dd286a8a..40621862 100644 --- a/drake_ros_systems/src/tf_broadcaster_system.cpp +++ b/drake_ros_systems/src/tf_broadcaster_system.cpp @@ -35,7 +35,6 @@ class TfBroadcasterSystem::TfBroadcasterSystemPrivate public: std::unique_ptr tf_broadcaster_; drake::systems::InputPortIndex graph_query_port_index; - drake::systems::InputPortIndex clock_port_index; }; TfBroadcasterSystem::TfBroadcasterSystem( @@ -50,9 +49,6 @@ TfBroadcasterSystem::TfBroadcasterSystem( this->DeclareAbstractInputPort( "graph_query", drake::Value>{}).get_index(); - impl_->clock_port_index = - this->DeclareAbstractInputPort("clock", drake::Value{}).get_index(); - // vvv Mostly copied from LcmPublisherSystem vvv using TriggerType = drake::systems::TriggerType; // Check that publish_triggers does not contain an unsupported trigger. @@ -117,9 +113,9 @@ TfBroadcasterSystem::PublishFrames( std::vector transforms; transforms.reserve(inspector.num_frames() - 1); geometry_msgs::msg::TransformStamped transform; + transform.header.stamp = + rclcpp::Time() + rclcpp::Duration::from_seconds(context.get_time()); transform.header.frame_id = inspector.GetName(inspector.world_frame_id()); - const double time = get_input_port(impl_->clock_port_index).Eval(context); - transform.header.stamp = rclcpp::Time() + rclcpp::Duration::from_seconds(time); for (const drake::geometry::FrameId & frame_id : inspector.all_frame_ids()) { if (frame_id == inspector.world_frame_id()) { continue; From 9764fe4beb69376cea2c255b028b2e9647397258 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Wed, 28 Apr 2021 17:56:15 -0300 Subject: [PATCH 57/87] Please linters Signed-off-by: Michel Hidalgo --- .../example/iiwa_manipulator.cpp | 15 +++---- drake_ros_systems/example/iiwa_manipulator.py | 18 +++++++- .../drake_ros_systems/rviz_visualizer.hpp | 3 +- .../scene_markers_system.hpp | 4 +- .../utilities/type_conversion.hpp | 6 ++- .../src/ros_subscriber_system.cpp | 2 +- drake_ros_systems/src/rviz_visualizer.cpp | 11 +++-- .../src/scene_markers_system.cpp | 44 ++++++++++--------- .../src/utilities/type_conversion.cpp | 6 ++- 9 files changed, 65 insertions(+), 44 deletions(-) diff --git a/drake_ros_systems/example/iiwa_manipulator.cpp b/drake_ros_systems/example/iiwa_manipulator.cpp index e53f1453..e96c9445 100644 --- a/drake_ros_systems/example/iiwa_manipulator.cpp +++ b/drake_ros_systems/example/iiwa_manipulator.cpp @@ -11,13 +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 #include @@ -25,14 +25,14 @@ #include using drake_ros_systems::DrakeRos; -using drake_ros_systems::RosClockSystem; using drake_ros_systems::RosInterfaceSystem; using drake_ros_systems::RvizVisualizer; using drake::examples::manipulation_station::ManipulationStation; -int main() { +int main() +{ drake::systems::DiagramBuilder builder; auto ros_interface_system = @@ -63,16 +63,15 @@ int main() { diagram->GetMutableSubsystemContext(*manipulation_station, &simulator_context); manipulation_station->GetInputPort("iiwa_position") - .FixValue( - &manipulation_station_context, - manipulation_station->GetIiwaPosition(manipulation_station_context)); + .FixValue( + &manipulation_station_context, + manipulation_station->GetIiwaPosition(manipulation_station_context)); manipulation_station->GetInputPort("wsg_position") - .FixValue(&manipulation_station_context, 0.); + .FixValue(&manipulation_station_context, 0.); while (true) { simulator->AdvanceTo(simulator_context.get_time() + 0.1); } return 0; - } diff --git a/drake_ros_systems/example/iiwa_manipulator.py b/drake_ros_systems/example/iiwa_manipulator.py index cf6e7517..b608bbb8 100755 --- a/drake_ros_systems/example/iiwa_manipulator.py +++ b/drake_ros_systems/example/iiwa_manipulator.py @@ -1,6 +1,20 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 +# Copyright 2021 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import numpy as np -from drake_ros_systems import RosClockSystem from drake_ros_systems import RosInterfaceSystem from drake_ros_systems import RvizVisualizer diff --git a/drake_ros_systems/include/drake_ros_systems/rviz_visualizer.hpp b/drake_ros_systems/include/drake_ros_systems/rviz_visualizer.hpp index af8dcdfb..f1bb6e8b 100644 --- a/drake_ros_systems/include/drake_ros_systems/rviz_visualizer.hpp +++ b/drake_ros_systems/include/drake_ros_systems/rviz_visualizer.hpp @@ -22,7 +22,8 @@ #include "drake_ros_systems/drake_ros_interface.hpp" -namespace drake_ros_systems { +namespace drake_ros_systems +{ /// System for SceneGraph visualization in RViz. /// diff --git a/drake_ros_systems/include/drake_ros_systems/scene_markers_system.hpp b/drake_ros_systems/include/drake_ros_systems/scene_markers_system.hpp index 4020ff4e..9a0b7dff 100644 --- a/drake_ros_systems/include/drake_ros_systems/scene_markers_system.hpp +++ b/drake_ros_systems/include/drake_ros_systems/scene_markers_system.hpp @@ -14,14 +14,14 @@ #ifndef DRAKE_ROS_SYSTEMS__SCENE_MARKERS_SYSTEM_HPP_ #define DRAKE_ROS_SYSTEMS__SCENE_MARKERS_SYSTEM_HPP_ -#include - #include #include #include #include +#include + namespace drake_ros_systems { diff --git a/drake_ros_systems/include/drake_ros_systems/utilities/type_conversion.hpp b/drake_ros_systems/include/drake_ros_systems/utilities/type_conversion.hpp index 63f67783..c73792cb 100644 --- a/drake_ros_systems/include/drake_ros_systems/utilities/type_conversion.hpp +++ b/drake_ros_systems/include/drake_ros_systems/utilities/type_conversion.hpp @@ -19,8 +19,10 @@ #include #include -namespace drake_ros_systems { -namespace utilities { +namespace drake_ros_systems +{ +namespace utilities +{ geometry_msgs::msg::Pose ToPoseMsg(const drake::math::RigidTransform X_AB); diff --git a/drake_ros_systems/src/ros_subscriber_system.cpp b/drake_ros_systems/src/ros_subscriber_system.cpp index 1feb72dd..643c269b 100644 --- a/drake_ros_systems/src/ros_subscriber_system.cpp +++ b/drake_ros_systems/src/ros_subscriber_system.cpp @@ -67,7 +67,7 @@ RosSubscriberSystem::RosSubscriberSystem( std::bind(&RosSubscriberSystemPrivate::handle_message, impl_.get(), std::placeholders::_1)); DeclareAbstractOutputPort( - [serializer{impl_->serializer_.get()}]() { return serializer->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)); diff --git a/drake_ros_systems/src/rviz_visualizer.cpp b/drake_ros_systems/src/rviz_visualizer.cpp index 9d172985..59d5d315 100644 --- a/drake_ros_systems/src/rviz_visualizer.cpp +++ b/drake_ros_systems/src/rviz_visualizer.cpp @@ -12,15 +12,14 @@ // 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_publisher_system.hpp" #include "drake_ros_systems/scene_markers_system.hpp" #include "drake_ros_systems/tf_broadcaster_system.hpp" @@ -28,7 +27,8 @@ #include "drake_ros_systems/rviz_visualizer.hpp" -namespace drake_ros_systems { +namespace drake_ros_systems +{ RvizVisualizer::RvizVisualizer( std::shared_ptr ros_interface, @@ -60,4 +60,3 @@ RvizVisualizer::RvizVisualizer( } } // namespace drake_ros_systems - diff --git a/drake_ros_systems/src/scene_markers_system.cpp b/drake_ros_systems/src/scene_markers_system.cpp index 81797c8b..8a23fadd 100644 --- a/drake_ros_systems/src/scene_markers_system.cpp +++ b/drake_ros_systems/src/scene_markers_system.cpp @@ -33,11 +33,14 @@ #include "drake_ros_systems/utilities/type_conversion.hpp" -namespace drake_ros_systems { +namespace drake_ros_systems +{ -namespace { +namespace +{ -class SceneGeometryToMarkers : public drake::geometry::ShapeReifier { +class SceneGeometryToMarkers : public drake::geometry::ShapeReifier +{ public: DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(SceneGeometryToMarkers) @@ -61,8 +64,8 @@ class SceneGeometryToMarkers : public drake::geometry::ShapeReifier { prototype_marker_.header.frame_id = inspector.GetName(inspector.GetFrameId(geometry_id)); - prototype_marker_.ns = inspector.GetOwningSourceName(geometry_id) - + "::" + inspector.GetName(geometry_id); + prototype_marker_.ns = inspector.GetOwningSourceName(geometry_id) + + "::" + inspector.GetName(geometry_id); prototype_marker_.id = 0; prototype_marker_.action = visualization_msgs::msg::Marker::MODIFY; @@ -79,7 +82,7 @@ class SceneGeometryToMarkers : public drake::geometry::ShapeReifier { if (illustration_props) { default_color = illustration_props->GetPropertyOrDefault( - "phong", "diffuse", default_color); + "phong", "diffuse", default_color); } } const drake::geometry::Rgba & color = @@ -250,12 +253,13 @@ SceneMarkersSystem::SceneMarkersSystem( { impl_->graph_query_port_index = this->DeclareAbstractInputPort( - "graph_query", drake::Value>{}).get_index(); + "graph_query", drake::Value>{}).get_index(); impl_->scene_markers_cache_index = - this->DeclareCacheEntry("scene_markers_cache", - &SceneMarkersSystem::CalcSceneMarkers, - {nothing_ticket()}).cache_index(); + this->DeclareCacheEntry( + "scene_markers_cache", + &SceneMarkersSystem::CalcSceneMarkers, + {nothing_ticket()}).cache_index(); this->DeclareAbstractOutputPort( "scene_markers", &SceneMarkersSystem::PopulateSceneMarkersMessage); @@ -274,8 +278,7 @@ SceneMarkersSystem::PopulateSceneMarkersMessage( const builtin_interfaces::msg::Time stamp = rclcpp::Time() + rclcpp::Duration::from_seconds(context.get_time()); - for (visualization_msgs::msg::Marker & marker : output_value->markers) - { + for (visualization_msgs::msg::Marker & marker : output_value->markers) { marker.header.stamp = stamp; } } @@ -286,18 +289,18 @@ SceneMarkersSystem::EvalSceneMarkers( { const drake::geometry::QueryObject & query_object = get_input_port(impl_->graph_query_port_index) - .Eval>(context); + .Eval>(context); const drake::geometry::GeometryVersion & current_version = query_object.inspector().geometry_version(); if (!impl_->version.IsSameAs(current_version, impl_->role)) { // Invalidate scene markers cache get_cache_entry(impl_->scene_markers_cache_index) - .get_mutable_cache_entry_value(context) - .mark_out_of_date(); + .get_mutable_cache_entry_value(context) + .mark_out_of_date(); impl_->version = current_version; } return get_cache_entry(impl_->scene_markers_cache_index) - .Eval(context); + .Eval(context); } void @@ -307,16 +310,17 @@ SceneMarkersSystem::CalcSceneMarkers( { const drake::geometry::QueryObject & query_object = get_input_port(impl_->graph_query_port_index) - .Eval>(context); + .Eval>(context); const drake::geometry::SceneGraphInspector & inspector = query_object.inspector(); output_value->markers.reserve(inspector.NumGeometriesWithRole(impl_->role)); for (const drake::geometry::FrameId & frame_id : inspector.all_frame_ids()) { for (const drake::geometry::GeometryId & geometry_id : - inspector.GetGeometries(frame_id, impl_->role)) { + inspector.GetGeometries(frame_id, impl_->role)) + { SceneGeometryToMarkers(impl_->role, impl_->default_color) - .Populate(inspector, geometry_id, output_value); + .Populate(inspector, geometry_id, output_value); } } } -} // drake_ros_systems +} // namespace drake_ros_systems diff --git a/drake_ros_systems/src/utilities/type_conversion.cpp b/drake_ros_systems/src/utilities/type_conversion.cpp index 1b58ba9a..3117461c 100644 --- a/drake_ros_systems/src/utilities/type_conversion.cpp +++ b/drake_ros_systems/src/utilities/type_conversion.cpp @@ -19,8 +19,10 @@ #include "drake_ros_systems/utilities/type_conversion.hpp" -namespace drake_ros_systems { -namespace utilities { +namespace drake_ros_systems +{ +namespace utilities +{ geometry_msgs::msg::Pose ToPoseMsg(const drake::math::RigidTransform X_AB) From 2986f07f44aa2c0fc0814bb7ebf661c55dab3e36 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Wed, 28 Apr 2021 17:57:19 -0300 Subject: [PATCH 58/87] Improve TfBroadcasterSystem API Signed-off-by: Michel Hidalgo --- .../tf_broadcaster_system.hpp | 4 ++- .../module_drake_ros_systems.cpp | 33 +++++-------------- .../src/tf_broadcaster_system.cpp | 24 +++++++++----- 3 files changed, 27 insertions(+), 34 deletions(-) diff --git a/drake_ros_systems/include/drake_ros_systems/tf_broadcaster_system.hpp b/drake_ros_systems/include/drake_ros_systems/tf_broadcaster_system.hpp index 349a9075..621770a1 100644 --- a/drake_ros_systems/include/drake_ros_systems/tf_broadcaster_system.hpp +++ b/drake_ros_systems/include/drake_ros_systems/tf_broadcaster_system.hpp @@ -37,10 +37,12 @@ class TfBroadcasterSystem : public drake::systems::LeafSystem public: TfBroadcasterSystem( DrakeRosInterface * ros_interface, - const std::unordered_set & publish_triggers, + const std::unordered_set & publish_triggers = { + drake::systems::TriggerType::kPerStep, drake::systems::kForced}, double publish_period = 0.0); virtual ~TfBroadcasterSystem(); + const drake::systems::InputPort & get_graph_query_port() const; private: drake::systems::EventStatus PublishFrames(const drake::systems::Context & context) const; 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 32b14b02..c41afc26 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 @@ -120,33 +120,17 @@ PYBIND11_MODULE(drake_ros_systems, m) { py::class_>(m, "TfBroadcasterSystem") .def( - py::init( - [](DrakeRosInterface * ros_interface) - { - constexpr double kZeroPublishPeriod{0.0}; - return std::make_unique( - ros_interface, - std::unordered_set{ - TriggerType::kPerStep, TriggerType::kForced}, - kZeroPublishPeriod); - })) - .def( - py::init( - []( - DrakeRosInterface * ros_interface, - std::unordered_set publish_triggers, - double publish_period) - { - return std::make_unique( - ros_interface, publish_triggers, publish_period); - })); + py::init, double>(), + py::arg("ros_interface"), + py::arg("publish_triggers") = {TriggerType::kPerStep, TriggerType::kForced}, + py::arg("publish_period") = 0.0) + .def("get_graph_query_port", &TfBroadcasterSystem::get_graph_query_port); py::class_>(m, "SceneMarkersSystem") - .def(py::init([]() { return std::make_unique(); })) + .def(py::init([]() {return std::make_unique();})) .def( py::init( - [](const drake::geometry::Role & role, - const drake::geometry::Rgba & default_color) + [](const drake::geometry::Role & role, const drake::geometry::Rgba & default_color) { return std::make_unique(role, default_color); })); @@ -160,7 +144,8 @@ PYBIND11_MODULE(drake_ros_systems, m) { })) .def( py::init( - [](std::shared_ptr ros_interface, + []( + std::shared_ptr ros_interface, std::unordered_set publish_triggers, double publish_period, bool publish_tf) { diff --git a/drake_ros_systems/src/tf_broadcaster_system.cpp b/drake_ros_systems/src/tf_broadcaster_system.cpp index 40621862..7b4936ab 100644 --- a/drake_ros_systems/src/tf_broadcaster_system.cpp +++ b/drake_ros_systems/src/tf_broadcaster_system.cpp @@ -12,11 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include -#include -#include - #include #include #include @@ -24,6 +19,12 @@ #include #include +#include +#include +#include +#include +#include + #include "drake_ros_systems/tf_broadcaster_system.hpp" #include "drake_ros_systems/utilities/type_conversion.hpp" @@ -47,7 +48,7 @@ TfBroadcasterSystem::TfBroadcasterSystem( impl_->graph_query_port_index = this->DeclareAbstractInputPort( - "graph_query", drake::Value>{}).get_index(); + "graph_query", drake::Value>{}).get_index(); // vvv Mostly copied from LcmPublisherSystem vvv using TriggerType = drake::systems::TriggerType; @@ -59,7 +60,7 @@ TfBroadcasterSystem::TfBroadcasterSystem( (trigger != TriggerType::kPerStep)) { throw std::invalid_argument( - "Only kForced, kPeriodic, or kPerStep are supported"); + "Only kForced, kPeriodic, or kPerStep are supported"); } } @@ -99,13 +100,18 @@ TfBroadcasterSystem::~TfBroadcasterSystem() { } +const drake::systems::InputPort & +TfBroadcasterSystem::get_graph_query_port() const +{ + return get_input_port(impl_->graph_query_port_index); +} + drake::systems::EventStatus TfBroadcasterSystem::PublishFrames( const drake::systems::Context & context) const { const drake::geometry::QueryObject & query_object = - get_input_port(impl_->graph_query_port_index) - .Eval>(context); + get_graph_query_port().Eval>(context); const drake::geometry::SceneGraphInspector & inspector = query_object.inspector(); // TODO(hidmic): publish frame transforms w.r.t. to their parent frame // instead of the world frame when an API is made available. From 944029dff50fb041e5deaf95a64b2fdf163fe929 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Wed, 28 Apr 2021 18:01:49 -0300 Subject: [PATCH 59/87] Add TfBroadcasterSystem tests. Signed-off-by: Michel Hidalgo --- drake_ros_systems/CMakeLists.txt | 15 ++ .../test/test_tf_broadcaster.cpp | 131 ++++++++++++++++++ drake_ros_systems/test/test_tf_broadcaster.py | 109 +++++++++++++++ 3 files changed, 255 insertions(+) create mode 100644 drake_ros_systems/test/test_tf_broadcaster.cpp create mode 100644 drake_ros_systems/test/test_tf_broadcaster.py diff --git a/drake_ros_systems/CMakeLists.txt b/drake_ros_systems/CMakeLists.txt index 2112328e..64df4cc9 100644 --- a/drake_ros_systems/CMakeLists.txt +++ b/drake_ros_systems/CMakeLists.txt @@ -16,6 +16,7 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") endif() find_package(ament_cmake_ros REQUIRED) +find_package(geometry_msgs REQUIRED) find_package(drake REQUIRED) # Must use Drake's fork of Pybind11 find_package(pybind11 REQUIRED HINTS "${drake_DIR}/../pybind11" NO_DEFAULT_PATH) @@ -100,6 +101,9 @@ if(BUILD_TESTING) find_package(test_msgs REQUIRED) ament_lint_auto_find_test_dependencies() + find_package(ament_cmake_gtest REQUIRED) + find_package(ament_cmake_pytest REQUIRED) + ament_add_gtest(test_integration test/integration.cpp) # TODO(sloretz) Why isn't pybind11::embed including python libs? @@ -118,6 +122,17 @@ if(BUILD_TESTING) target_link_libraries(test_drake_ros drake_ros_systems ) + + ament_add_gtest(test_tf_broadcaster test/test_tf_broadcaster.cpp) + target_link_libraries(test_tf_broadcaster + drake::drake + drake_ros_systems + ${geometry_msgs_TARGETS} + rclcpp::rclcpp + tf2_ros::tf2_ros + ) + + ament_add_pytest_test(test_tf_broadcaster_py test/test_tf_broadcaster.py) endif() ament_package() diff --git a/drake_ros_systems/test/test_tf_broadcaster.cpp b/drake_ros_systems/test/test_tf_broadcaster.cpp new file mode 100644 index 00000000..b0c041da --- /dev/null +++ b/drake_ros_systems/test/test_tf_broadcaster.cpp @@ -0,0 +1,131 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#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/tf_broadcaster_system.hpp" + +using drake_ros_systems::DrakeRos; +using drake_ros_systems::RosInterfaceSystem; +using drake_ros_systems::TfBroadcasterSystem; + +TEST(TfBroadcasting, nominal_case) { + drake::systems::DiagramBuilder builder; + + auto sys_ros_interface = + builder.AddSystem(std::make_unique()); + + auto scene_graph = builder.AddSystem(); + const drake::geometry::SourceId source_id = + scene_graph->RegisterSource("test_source"); + const drake::geometry::FrameId odom_frame = scene_graph->RegisterFrame( + source_id, drake::geometry::GeometryFrame("odom")); + const drake::geometry::FrameId base_frame = scene_graph->RegisterFrame( + source_id, odom_frame, drake::geometry::GeometryFrame("base_link")); + + const drake::math::RigidTransform X_WO{ // World to Odom + drake::math::RotationMatrix::Identity(), + drake::Vector3{1., 1., 0.}}; + const drake::math::RigidTransform X_OB{ // Odom to Base link + drake::math::RollPitchYaw(0., 0., M_PI / 2.), + drake::Vector3{0., 0., 0.1}}; + + const drake::geometry::FramePoseVector pose_vector{ + {odom_frame, X_WO}, {base_frame, X_OB}}; + + auto pose_vector_value = drake::AbstractValue::Make(pose_vector); + auto pose_vector_source = + builder.AddSystem(*pose_vector_value); + + builder.Connect( + pose_vector_source->get_output_port(), + scene_graph->get_source_pose_port(source_id)); + + const std::unordered_set + publish_triggers{drake::systems::TriggerType::kForced}; + auto broadcaster = builder.AddSystem( + sys_ros_interface->get_ros_interface().get(), publish_triggers); + + builder.Connect( + scene_graph->get_query_output_port(), + broadcaster->GetInputPort("graph_query")); + + auto diagram = builder.Build(); + auto context = diagram->CreateDefaultContext(); + + // Don't need to rclcpp::init because DrakeRos uses global rclcpp::Context by default + auto node = rclcpp::Node::make_shared("tf_listener"); + + constexpr bool kNoSpinThread{false}; + tf2_ros::Buffer buffer(node->get_clock()); + tf2_ros::TransformListener listener(buffer, node, kNoSpinThread); + + const rclcpp::Time time = node->get_clock()->now(); + const builtin_interfaces::msg::Time stamp = time; + + context->SetTime(time.seconds()); + diagram->Publish(*context); + rclcpp::spin_some(node); + + const geometry_msgs::msg::TransformStamped world_to_odom = + buffer.lookupTransform("world", "odom", time); + EXPECT_EQ(world_to_odom.header.stamp.sec, stamp.sec); + EXPECT_EQ(world_to_odom.header.stamp.nanosec, stamp.nanosec); + const drake::Vector3 & p_WO = X_WO.translation(); + EXPECT_DOUBLE_EQ(world_to_odom.transform.translation.x, p_WO.x()); + EXPECT_DOUBLE_EQ(world_to_odom.transform.translation.y, p_WO.y()); + EXPECT_DOUBLE_EQ(world_to_odom.transform.translation.z, p_WO.z()); + const Eigen::Quaternion R_WO = X_WO.rotation().ToQuaternion(); + EXPECT_DOUBLE_EQ(world_to_odom.transform.rotation.x, R_WO.x()); + EXPECT_DOUBLE_EQ(world_to_odom.transform.rotation.y, R_WO.y()); + EXPECT_DOUBLE_EQ(world_to_odom.transform.rotation.z, R_WO.z()); + EXPECT_DOUBLE_EQ(world_to_odom.transform.rotation.w, R_WO.w()); + + const geometry_msgs::msg::TransformStamped odom_to_base_link = + buffer.lookupTransform("odom", "base_link", time); + EXPECT_EQ(odom_to_base_link.header.stamp.sec, stamp.sec); + EXPECT_EQ(odom_to_base_link.header.stamp.nanosec, stamp.nanosec); + const drake::Vector3 & p_OB = X_OB.translation(); + EXPECT_DOUBLE_EQ(odom_to_base_link.transform.translation.x, p_OB.x()); + EXPECT_DOUBLE_EQ(odom_to_base_link.transform.translation.y, p_OB.y()); + EXPECT_DOUBLE_EQ(odom_to_base_link.transform.translation.z, p_OB.z()); + const Eigen::Quaternion R_OB = X_OB.rotation().ToQuaternion(); + EXPECT_DOUBLE_EQ(odom_to_base_link.transform.rotation.x, R_OB.x()); + EXPECT_DOUBLE_EQ(odom_to_base_link.transform.rotation.y, R_OB.y()); + EXPECT_DOUBLE_EQ(odom_to_base_link.transform.rotation.z, R_OB.z()); + EXPECT_DOUBLE_EQ(odom_to_base_link.transform.rotation.w, R_OB.w()); +} diff --git a/drake_ros_systems/test/test_tf_broadcaster.py b/drake_ros_systems/test/test_tf_broadcaster.py new file mode 100644 index 00000000..010139e0 --- /dev/null +++ b/drake_ros_systems/test/test_tf_broadcaster.py @@ -0,0 +1,109 @@ +# Copyright 2021 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import numpy as np + +from drake_ros_systems import TfBroadcasterSystem + +from pydrake.common import AbstractValue +from pydrake.math import RigidTransform +from pydrake.geometry import SceneGraph +from pydrake.geometry import FramePoseVector +from pydrake.systems.analysis import Simulator +from pydrake.systems.framework import DiagramBuilder +from pydrake.systems.primitives import ConstantValueSource + +import rclpy +import tf2_ros + + +def test_nominal_case(): + builder = DiagramBuilder() + + sys_ros_interface = builder.AddSystem(RosInterfaceSystem()) + + scene_graph = builder.AddSystem(SceneGraph()) + source_id = scene_graph.RegisterSource('test_source') + odom_frame = scene_graph.RegisterFrame( + source_id, GeometryFrame('odom')) + base_link_frame = scene_graph.RegisterFrame( + source_id, odom_frame, GeometryFrame('base_link')) + + X_WO = RigidTransform( + R=RotationMatrix.Identity(), + p=np.array([1., 1., 0.])) + X_OB = RigidTransform( + rpy=np.array([0., 0., np.pi / 2.]), + p=np.array([1., 1., 0.])) + + pose_vector = FramePoseVector() + pose_vector.set_value(odom_frame, X_WO) + pose_vector.set_value(base_link_frame, X_OB) + + pose_vector_source = builder.AddSystem( + ConstantValueSource(AbstractValue.Make(pose_vector))) + + builder.Connect( + pose_vector_source.get_output_port(), + scene_graph.get_source_pose_port(source_id)) + + tf_broadcaster = builder.AddSystem(TfBroadcasterSystem( + sys_ros_interface, publish_triggers={TriggerType.kForced})) + + builder.Connect( + scene_graph.get_query_object_port(), + tf_broadcaster.get_graph_query_port()) + + diagram = builder.Build() + context = diagram.CreateDefaultContext() + + rclpy.init() + node = rclpy.create_node('tf_listener') + + buffer_ = tf2_ros.Buffer() + listener = tf2_ros.TransformListener( + buffer_, node, spin_thread=False) + + time = node.get_clock().now() + stamp = time.to_msg(); + + context.SetTime(time.nanoseconds / 1e9) + diagram.Publish(context) + rclpy.spin_once(node) + + world_to_odom = buffer_.lookupTransform('world', 'odom', time) + assert world_to_odom.header.stamp.sec == stamp.sec + assert world_to_odom.header.stamp.nanosec == stamp.nanosec + p_WO = X_WO.translation() + assert math.isclose(world_to_odom.transform.translation.x, p_WO[0]) + assert math.isclose(world_to_odom.transform.translation.y, p_WO[1]) + assert math.isclose(world_to_odom.transform.translation.z, p_WO[2]) + R_WO = X_WO.rotation().ToQuaternion() + assert math.isclose(world_to_odom.transform.rotation.x, R_WO.x()) + assert math.isclose(world_to_odom.transform.rotation.y, R_WO.y()) + assert math.isclose(world_to_odom.transform.rotation.z, R_WO.z()) + assert math.isclose(world_to_odom.transform.rotation.w, R_WO.w()) + + odom_to_base_link = buffer_.lookupTransform('odom', 'base_link', time) + assert odom_to_base_link.header.stamp.sec == stamp.sec + assert odom_to_base_link.header.stamp.nanosec == stamp.nanosec + p_OB = X_OB.translation() + assert math.isclose(odom_to_base_link.transform.translation.x, p_OB[0]) + assert math.isclose(odom_to_base_link.transform.translation.y, p_OB[1]) + assert math.isclose(odom_to_base_link.transform.translation.z, p_OB[2]) + R_OB = X_OB.rotation().ToQuaternion() + assert math.isclose(odom_to_base_link.transform.rotation.x, R_OB.x()) + assert math.isclose(odom_to_base_link.transform.rotation.y, R_OB.y()) + assert math.isclose(odom_to_base_link.transform.rotation.z, R_OB.z()) + assert math.isclose(odom_to_base_link.transform.rotation.w, R_OB.w()) From beabc3c4bb781be2a008a48b63f5706cb7b85915 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Mon, 3 May 2021 12:51:00 -0300 Subject: [PATCH 60/87] Fix TfBroadcasterSystem default arguments. Signed-off-by: Michel Hidalgo --- .../include/drake_ros_systems/tf_broadcaster_system.hpp | 2 +- .../src/python_bindings/module_drake_ros_systems.cpp | 3 ++- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/drake_ros_systems/include/drake_ros_systems/tf_broadcaster_system.hpp b/drake_ros_systems/include/drake_ros_systems/tf_broadcaster_system.hpp index 621770a1..3430cc72 100644 --- a/drake_ros_systems/include/drake_ros_systems/tf_broadcaster_system.hpp +++ b/drake_ros_systems/include/drake_ros_systems/tf_broadcaster_system.hpp @@ -38,7 +38,7 @@ class TfBroadcasterSystem : public drake::systems::LeafSystem TfBroadcasterSystem( DrakeRosInterface * ros_interface, const std::unordered_set & publish_triggers = { - drake::systems::TriggerType::kPerStep, drake::systems::kForced}, + drake::systems::TriggerType::kPerStep, drake::systems::TriggerType::kForced}, double publish_period = 0.0); virtual ~TfBroadcasterSystem(); 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 c41afc26..6a2f5109 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 @@ -122,7 +122,8 @@ PYBIND11_MODULE(drake_ros_systems, m) { .def( py::init, double>(), py::arg("ros_interface"), - py::arg("publish_triggers") = {TriggerType::kPerStep, TriggerType::kForced}, + py::arg("publish_triggers") = std::unordered_set{ + TriggerType::kPerStep, TriggerType::kForced}, py::arg("publish_period") = 0.0) .def("get_graph_query_port", &TfBroadcasterSystem::get_graph_query_port); From 34985177a3b83f4dd81492c6fcbe58b7b27cb020 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Mon, 3 May 2021 12:51:31 -0300 Subject: [PATCH 61/87] Drop unnecessary semicolon in Python Signed-off-by: Michel Hidalgo --- drake_ros_systems/test/test_tf_broadcaster.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/drake_ros_systems/test/test_tf_broadcaster.py b/drake_ros_systems/test/test_tf_broadcaster.py index 010139e0..6723d832 100644 --- a/drake_ros_systems/test/test_tf_broadcaster.py +++ b/drake_ros_systems/test/test_tf_broadcaster.py @@ -76,7 +76,7 @@ def test_nominal_case(): buffer_, node, spin_thread=False) time = node.get_clock().now() - stamp = time.to_msg(); + stamp = time.to_msg() context.SetTime(time.nanoseconds / 1e9) diagram.Publish(context) From c19e5c2b0be43b89ee619ac06ff2d898f2d32b56 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Mon, 3 May 2021 12:51:58 -0300 Subject: [PATCH 62/87] Please uncrustify Signed-off-by: Michel Hidalgo --- .../include/drake_ros_systems/tf_broadcaster_system.hpp | 1 + .../src/python_bindings/module_drake_ros_systems.cpp | 8 ++++---- drake_ros_systems/test/test_tf_broadcaster.cpp | 6 +++--- 3 files changed, 8 insertions(+), 7 deletions(-) diff --git a/drake_ros_systems/include/drake_ros_systems/tf_broadcaster_system.hpp b/drake_ros_systems/include/drake_ros_systems/tf_broadcaster_system.hpp index 3430cc72..333e76ba 100644 --- a/drake_ros_systems/include/drake_ros_systems/tf_broadcaster_system.hpp +++ b/drake_ros_systems/include/drake_ros_systems/tf_broadcaster_system.hpp @@ -43,6 +43,7 @@ class TfBroadcasterSystem : public drake::systems::LeafSystem virtual ~TfBroadcasterSystem(); const drake::systems::InputPort & get_graph_query_port() const; + private: drake::systems::EventStatus PublishFrames(const drake::systems::Context & context) const; 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 6a2f5109..165a2897 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 @@ -123,7 +123,7 @@ PYBIND11_MODULE(drake_ros_systems, m) { py::init, double>(), py::arg("ros_interface"), py::arg("publish_triggers") = std::unordered_set{ - TriggerType::kPerStep, TriggerType::kForced}, + TriggerType::kPerStep, TriggerType::kForced}, py::arg("publish_period") = 0.0) .def("get_graph_query_port", &TfBroadcasterSystem::get_graph_query_port); @@ -146,9 +146,9 @@ PYBIND11_MODULE(drake_ros_systems, m) { .def( py::init( []( - std::shared_ptr ros_interface, - std::unordered_set publish_triggers, - double publish_period, bool publish_tf) + std::shared_ptr ros_interface, + std::unordered_set publish_triggers, + double publish_period, bool publish_tf) { return std::make_unique( ros_interface, publish_triggers, publish_period, publish_tf); diff --git a/drake_ros_systems/test/test_tf_broadcaster.cpp b/drake_ros_systems/test/test_tf_broadcaster.cpp index b0c041da..17de7cb2 100644 --- a/drake_ros_systems/test/test_tf_broadcaster.cpp +++ b/drake_ros_systems/test/test_tf_broadcaster.cpp @@ -51,11 +51,11 @@ TEST(TfBroadcasting, nominal_case) { auto scene_graph = builder.AddSystem(); const drake::geometry::SourceId source_id = - scene_graph->RegisterSource("test_source"); + scene_graph->RegisterSource("test_source"); const drake::geometry::FrameId odom_frame = scene_graph->RegisterFrame( - source_id, drake::geometry::GeometryFrame("odom")); + source_id, drake::geometry::GeometryFrame("odom")); const drake::geometry::FrameId base_frame = scene_graph->RegisterFrame( - source_id, odom_frame, drake::geometry::GeometryFrame("base_link")); + source_id, odom_frame, drake::geometry::GeometryFrame("base_link")); const drake::math::RigidTransform X_WO{ // World to Odom drake::math::RotationMatrix::Identity(), From c70e539436712e50b0c0c1c00f07ba474bcac217 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Mon, 3 May 2021 12:54:29 -0300 Subject: [PATCH 63/87] Please flake8 Signed-off-by: Michel Hidalgo --- drake_ros_systems/test/test_tf_broadcaster.py | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/drake_ros_systems/test/test_tf_broadcaster.py b/drake_ros_systems/test/test_tf_broadcaster.py index 6723d832..dd93d9ff 100644 --- a/drake_ros_systems/test/test_tf_broadcaster.py +++ b/drake_ros_systems/test/test_tf_broadcaster.py @@ -12,16 +12,20 @@ # See the License for the specific language governing permissions and # limitations under the License. +import math import numpy as np +from drake_ros_systems import RosInterfaceSystem from drake_ros_systems import TfBroadcasterSystem from pydrake.common import AbstractValue from pydrake.math import RigidTransform -from pydrake.geometry import SceneGraph +from pydrake.math import RotationMatrix from pydrake.geometry import FramePoseVector -from pydrake.systems.analysis import Simulator +from pydrake.geometry import GeometryFrame +from pydrake.geometry import SceneGraph from pydrake.systems.framework import DiagramBuilder +from pydrake.systems.framework import TriggerType from pydrake.systems.primitives import ConstantValueSource import rclpy @@ -72,8 +76,7 @@ def test_nominal_case(): node = rclpy.create_node('tf_listener') buffer_ = tf2_ros.Buffer() - listener = tf2_ros.TransformListener( - buffer_, node, spin_thread=False) + listener = tf2_ros.TransformListener(buffer_, node, spin_thread=False) # noqa time = node.get_clock().now() stamp = time.to_msg() From 830dacbedb224df7e2055edb6ef7219bcb04620c Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Mon, 3 May 2021 14:30:12 -0300 Subject: [PATCH 64/87] Unit tests for TfBroadcasterSystem passing. Signed-off-by: Michel Hidalgo --- .../module_drake_ros_systems.cpp | 4 +++- .../test/test_tf_broadcaster.cpp | 10 +++++++--- drake_ros_systems/test/test_tf_broadcaster.py | 19 ++++++++++++------- 3 files changed, 22 insertions(+), 11 deletions(-) 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 165a2897..cb83cc18 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 @@ -125,7 +125,9 @@ PYBIND11_MODULE(drake_ros_systems, m) { py::arg("publish_triggers") = std::unordered_set{ TriggerType::kPerStep, TriggerType::kForced}, py::arg("publish_period") = 0.0) - .def("get_graph_query_port", &TfBroadcasterSystem::get_graph_query_port); + .def( + "get_graph_query_port", &TfBroadcasterSystem::get_graph_query_port, + py::return_value_policy::reference_internal); py::class_>(m, "SceneMarkersSystem") .def(py::init([]() {return std::make_unique();})) diff --git a/drake_ros_systems/test/test_tf_broadcaster.cpp b/drake_ros_systems/test/test_tf_broadcaster.cpp index 17de7cb2..e7dd4c5c 100644 --- a/drake_ros_systems/test/test_tf_broadcaster.cpp +++ b/drake_ros_systems/test/test_tf_broadcaster.cpp @@ -90,19 +90,22 @@ TEST(TfBroadcasting, nominal_case) { // Don't need to rclcpp::init because DrakeRos uses global rclcpp::Context by default auto node = rclcpp::Node::make_shared("tf_listener"); - constexpr bool kNoSpinThread{false}; tf2_ros::Buffer buffer(node->get_clock()); + buffer.setUsingDedicatedThread(true); // Shut tf2 warnings. + + constexpr bool kNoSpinThread{false}; tf2_ros::TransformListener listener(buffer, node, kNoSpinThread); - const rclcpp::Time time = node->get_clock()->now(); - const builtin_interfaces::msg::Time stamp = time; + const auto time = rclcpp::Time() + rclcpp::Duration::from_seconds(13.); context->SetTime(time.seconds()); diagram->Publish(*context); rclcpp::spin_some(node); + EXPECT_TRUE(buffer.canTransform("world", "odom", time)); const geometry_msgs::msg::TransformStamped world_to_odom = buffer.lookupTransform("world", "odom", time); + const builtin_interfaces::msg::Time stamp = time; EXPECT_EQ(world_to_odom.header.stamp.sec, stamp.sec); EXPECT_EQ(world_to_odom.header.stamp.nanosec, stamp.nanosec); const drake::Vector3 & p_WO = X_WO.translation(); @@ -115,6 +118,7 @@ TEST(TfBroadcasting, nominal_case) { EXPECT_DOUBLE_EQ(world_to_odom.transform.rotation.z, R_WO.z()); EXPECT_DOUBLE_EQ(world_to_odom.transform.rotation.w, R_WO.w()); + EXPECT_TRUE(buffer.canTransform("odom", "base_link", time)); const geometry_msgs::msg::TransformStamped odom_to_base_link = buffer.lookupTransform("odom", "base_link", time); EXPECT_EQ(odom_to_base_link.header.stamp.sec, stamp.sec); diff --git a/drake_ros_systems/test/test_tf_broadcaster.py b/drake_ros_systems/test/test_tf_broadcaster.py index dd93d9ff..9328437b 100644 --- a/drake_ros_systems/test/test_tf_broadcaster.py +++ b/drake_ros_systems/test/test_tf_broadcaster.py @@ -18,8 +18,9 @@ from drake_ros_systems import RosInterfaceSystem from drake_ros_systems import TfBroadcasterSystem -from pydrake.common import AbstractValue +from pydrake.common.value import AbstractValue from pydrake.math import RigidTransform +from pydrake.math import RollPitchYaw from pydrake.math import RotationMatrix from pydrake.geometry import FramePoseVector from pydrake.geometry import GeometryFrame @@ -29,6 +30,7 @@ from pydrake.systems.primitives import ConstantValueSource import rclpy +import rclpy.time import tf2_ros @@ -48,7 +50,7 @@ def test_nominal_case(): R=RotationMatrix.Identity(), p=np.array([1., 1., 0.])) X_OB = RigidTransform( - rpy=np.array([0., 0., np.pi / 2.]), + rpy=RollPitchYaw(0., 0., math.pi / 2.), p=np.array([1., 1., 0.])) pose_vector = FramePoseVector() @@ -63,10 +65,11 @@ def test_nominal_case(): scene_graph.get_source_pose_port(source_id)) tf_broadcaster = builder.AddSystem(TfBroadcasterSystem( - sys_ros_interface, publish_triggers={TriggerType.kForced})) + sys_ros_interface.get_ros_interface(), + publish_triggers={TriggerType.kForced})) builder.Connect( - scene_graph.get_query_object_port(), + scene_graph.get_query_output_port(), tf_broadcaster.get_graph_query_port()) diagram = builder.Build() @@ -78,14 +81,15 @@ def test_nominal_case(): buffer_ = tf2_ros.Buffer() listener = tf2_ros.TransformListener(buffer_, node, spin_thread=False) # noqa - time = node.get_clock().now() + time = rclpy.time.Time(seconds=13.) stamp = time.to_msg() context.SetTime(time.nanoseconds / 1e9) diagram.Publish(context) rclpy.spin_once(node) - world_to_odom = buffer_.lookupTransform('world', 'odom', time) + assert buffer_.can_transform('world', 'odom', time) + world_to_odom = buffer_.lookup_transform('world', 'odom', time) assert world_to_odom.header.stamp.sec == stamp.sec assert world_to_odom.header.stamp.nanosec == stamp.nanosec p_WO = X_WO.translation() @@ -98,7 +102,8 @@ def test_nominal_case(): assert math.isclose(world_to_odom.transform.rotation.z, R_WO.z()) assert math.isclose(world_to_odom.transform.rotation.w, R_WO.w()) - odom_to_base_link = buffer_.lookupTransform('odom', 'base_link', time) + assert buffer_.can_transform('odom', 'base_link', time) + odom_to_base_link = buffer_.lookup_transform('odom', 'base_link', time) assert odom_to_base_link.header.stamp.sec == stamp.sec assert odom_to_base_link.header.stamp.nanosec == stamp.nanosec p_OB = X_OB.translation() From 1804887755830e22e66f1e7c72948b26efb65551 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Mon, 3 May 2021 18:22:14 -0300 Subject: [PATCH 65/87] Improve SceneMarkersSystem API Signed-off-by: Michel Hidalgo --- .../scene_markers_system.hpp | 10 +++++++ .../src/scene_markers_system.cpp | 30 +++++++++++++++++-- 2 files changed, 38 insertions(+), 2 deletions(-) diff --git a/drake_ros_systems/include/drake_ros_systems/scene_markers_system.hpp b/drake_ros_systems/include/drake_ros_systems/scene_markers_system.hpp index 9a0b7dff..52a39abf 100644 --- a/drake_ros_systems/include/drake_ros_systems/scene_markers_system.hpp +++ b/drake_ros_systems/include/drake_ros_systems/scene_markers_system.hpp @@ -52,6 +52,16 @@ class SceneMarkersSystem : public drake::systems::LeafSystem const drake::geometry::Rgba & default_color = {0.9, 0.9, 0.9, 1.0}); virtual ~SceneMarkersSystem(); + /// Role of the geometries this system targets. + const drake::geometry::Role & role() const; + + /// Default color used when a phong/diffuse property cannot be found. + const drake::geometry::Rgba & default_color() const; + + const drake::systems::InputPort & get_graph_query_port() const; + + const drake::systems::OutputPort & get_markers_output_port() const; + private: // Outputs visualization_msgs::msg::MarkerArray message, // timestamping the most up-to-date version. diff --git a/drake_ros_systems/src/scene_markers_system.cpp b/drake_ros_systems/src/scene_markers_system.cpp index 8a23fadd..ad5680bd 100644 --- a/drake_ros_systems/src/scene_markers_system.cpp +++ b/drake_ros_systems/src/scene_markers_system.cpp @@ -17,6 +17,7 @@ #include #include #include +#include #include #include #include @@ -244,6 +245,7 @@ class SceneMarkersSystem::SceneMarkersSystemPrivate mutable drake::geometry::GeometryVersion version; drake::systems::CacheIndex scene_markers_cache_index; drake::systems::InputPortIndex graph_query_port_index; + drake::systems::OutputPortIndex scene_markers_port_index; }; SceneMarkersSystem::SceneMarkersSystem( @@ -261,8 +263,8 @@ SceneMarkersSystem::SceneMarkersSystem( &SceneMarkersSystem::CalcSceneMarkers, {nothing_ticket()}).cache_index(); - this->DeclareAbstractOutputPort( - "scene_markers", &SceneMarkersSystem::PopulateSceneMarkersMessage); + impl_->scene_markers_port_index = this->DeclareAbstractOutputPort( + "scene_markers", &SceneMarkersSystem::PopulateSceneMarkersMessage).get_index(); } SceneMarkersSystem::~SceneMarkersSystem() @@ -323,4 +325,28 @@ SceneMarkersSystem::CalcSceneMarkers( } } +const drake::geometry::Role & +SceneMarkersSystem::role() const +{ + return impl_->role; +} + +const drake::geometry::Rgba & +SceneMarkersSystem::default_color() const +{ + return impl_->default_color; +} + +const drake::systems::InputPort & +SceneMarkersSystem::get_graph_query_port() const +{ + return get_input_port(impl_->graph_query_port_index); +} + +const drake::systems::OutputPort & +SceneMarkersSystem::get_markers_output_port() const +{ + return get_output_port(impl_->scene_markers_port_index); +} + } // namespace drake_ros_systems From a650723b51d402bf6031e9b3d5e6d620b0401582 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Mon, 3 May 2021 18:22:30 -0300 Subject: [PATCH 66/87] Improve RvizVisualizer API Signed-off-by: Michel Hidalgo --- .../include/drake_ros_systems/rviz_visualizer.hpp | 2 ++ drake_ros_systems/src/rviz_visualizer.cpp | 14 ++++++++++---- 2 files changed, 12 insertions(+), 4 deletions(-) diff --git a/drake_ros_systems/include/drake_ros_systems/rviz_visualizer.hpp b/drake_ros_systems/include/drake_ros_systems/rviz_visualizer.hpp index f1bb6e8b..eefc1b0c 100644 --- a/drake_ros_systems/include/drake_ros_systems/rviz_visualizer.hpp +++ b/drake_ros_systems/include/drake_ros_systems/rviz_visualizer.hpp @@ -45,6 +45,8 @@ class RvizVisualizer : public drake::systems::Diagram drake::systems::TriggerType::kForced, drake::systems::TriggerType::kPeriodic}, double publish_period = 0.05, bool publish_tf = true); + + const drake::systems::InputPort & get_graph_query_port() const; }; } // namespace drake_ros_systems diff --git a/drake_ros_systems/src/rviz_visualizer.cpp b/drake_ros_systems/src/rviz_visualizer.cpp index 59d5d315..8c2615ae 100644 --- a/drake_ros_systems/src/rviz_visualizer.cpp +++ b/drake_ros_systems/src/rviz_visualizer.cpp @@ -44,19 +44,25 @@ RvizVisualizer::RvizVisualizer( "/scene_markers", rclcpp::QoS(1), ros_interface, publish_triggers, publish_period)); builder.Connect( - scene_markers->GetOutputPort("scene_markers"), - scene_markers_publisher->GetInputPort("message")); + scene_markers->get_markers_output_port(), + scene_markers_publisher->get_input_port()); - builder.ExportInput(scene_markers->GetInputPort("graph_query"), "graph_query"); + builder.ExportInput(scene_markers->get_graph_query_port(), "graph_query"); if (publish_tf) { auto tf_broadcaster = builder.AddSystem( ros_interface.get(), publish_triggers, publish_period); - builder.ConnectInput("graph_query", tf_broadcaster->GetInputPort("graph_query")); + builder.ConnectInput("graph_query", tf_broadcaster->get_graph_query_port()); } builder.BuildInto(this); } +const drake::systems::InputPort & +RvizVisualizer::get_graph_query_port() const +{ + return get_input_port(); +} + } // namespace drake_ros_systems From 68500c3ab21f95800595eec151948c6ccd94ab92 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Mon, 3 May 2021 18:22:50 -0300 Subject: [PATCH 67/87] Update Python bindings Signed-off-by: Michel Hidalgo --- .../module_drake_ros_systems.cpp | 37 +++++-------------- 1 file changed, 10 insertions(+), 27 deletions(-) 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 cb83cc18..7db4d191 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,7 +24,6 @@ #include "drake_ros_systems/ros_publisher_system.hpp" #include "drake_ros_systems/ros_subscriber_system.hpp" #include "drake_ros_systems/rviz_visualizer.hpp" -#include "drake_ros_systems/scene_markers_system.hpp" #include "drake_ros_systems/tf_broadcaster_system.hpp" #include "py_serializer.hpp" @@ -43,7 +42,6 @@ using drake_ros_systems::RosInterfaceSystem; using drake_ros_systems::RosPublisherSystem; using drake_ros_systems::RosSubscriberSystem; using drake_ros_systems::RvizVisualizer; -using drake_ros_systems::SceneMarkersSystem; using drake_ros_systems::SerializerInterface; using drake_ros_systems::TfBroadcasterSystem; @@ -122,37 +120,22 @@ PYBIND11_MODULE(drake_ros_systems, m) { .def( py::init, double>(), py::arg("ros_interface"), - py::arg("publish_triggers") = std::unordered_set{ - TriggerType::kPerStep, TriggerType::kForced}, + py::arg("publish_triggers") = + std::unordered_set{TriggerType::kPerStep, TriggerType::kForced}, py::arg("publish_period") = 0.0) .def( "get_graph_query_port", &TfBroadcasterSystem::get_graph_query_port, py::return_value_policy::reference_internal); - py::class_>(m, "SceneMarkersSystem") - .def(py::init([]() {return std::make_unique();})) - .def( - py::init( - [](const drake::geometry::Role & role, const drake::geometry::Rgba & default_color) - { - return std::make_unique(role, default_color); - })); - py::class_>(m, "RvizVisualizer") .def( - py::init( - [](std::shared_ptr ros_interface) - { - return std::make_unique(ros_interface); - })) + py::init, std::unordered_set, double, bool>(), + py::arg("ros_interface"), + py::arg("publish_triggers") = + std::unordered_set{TriggerType::kPerStep, TriggerType::kForced}, + py::arg("publish_period") = 0.0, + py::arg("publish_tf") = false) .def( - py::init( - []( - std::shared_ptr ros_interface, - std::unordered_set publish_triggers, - double publish_period, bool publish_tf) - { - return std::make_unique( - ros_interface, publish_triggers, publish_period, publish_tf); - })); + "get_graph_query_port", &RvizVisualizer::get_graph_query_port, + py::return_value_policy::reference_internal); } From f9064f7d74d8b3d2e04db8cc00d5b2f7d9f42da6 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Mon, 3 May 2021 18:23:29 -0300 Subject: [PATCH 68/87] Add SceneMarkersSystem tests and polish the rest Signed-off-by: Michel Hidalgo --- drake_ros_systems/CMakeLists.txt | 13 +- .../{drake_ros.cpp => test_drake_ros.cpp} | 0 .../{integration.cpp => test_pub_sub.cpp} | 0 drake_ros_systems/test/test_scene_markers.cpp | 544 ++++++++++++++++++ .../test/test_tf_broadcaster.cpp | 2 +- 5 files changed, 555 insertions(+), 4 deletions(-) rename drake_ros_systems/test/{drake_ros.cpp => test_drake_ros.cpp} (100%) rename drake_ros_systems/test/{integration.cpp => test_pub_sub.cpp} (100%) create mode 100644 drake_ros_systems/test/test_scene_markers.cpp diff --git a/drake_ros_systems/CMakeLists.txt b/drake_ros_systems/CMakeLists.txt index 64df4cc9..0639cac9 100644 --- a/drake_ros_systems/CMakeLists.txt +++ b/drake_ros_systems/CMakeLists.txt @@ -104,12 +104,12 @@ if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) find_package(ament_cmake_pytest REQUIRED) - ament_add_gtest(test_integration test/integration.cpp) + ament_add_gtest(test_pub_sub test/test_pub_sub.cpp) # TODO(sloretz) Why isn't pybind11::embed including python libs? find_package(PythonLibs REQUIRED) - target_link_libraries(test_integration + target_link_libraries(test_pub_sub drake::drake drake_ros_systems ${test_msgs_TARGETS} @@ -118,7 +118,7 @@ if(BUILD_TESTING) ${PYTHON_LIBRARIES} ) - ament_add_gtest(test_drake_ros test/drake_ros.cpp) + ament_add_gtest(test_drake_ros test/test_drake_ros.cpp) target_link_libraries(test_drake_ros drake_ros_systems ) @@ -133,6 +133,13 @@ if(BUILD_TESTING) ) ament_add_pytest_test(test_tf_broadcaster_py test/test_tf_broadcaster.py) + + ament_add_gtest(test_scene_markers test/test_scene_markers.cpp) + target_link_libraries(test_scene_markers + drake::drake + drake_ros_systems + ${visualization_msgs_TARGETS} + ) endif() ament_package() diff --git a/drake_ros_systems/test/drake_ros.cpp b/drake_ros_systems/test/test_drake_ros.cpp similarity index 100% rename from drake_ros_systems/test/drake_ros.cpp rename to drake_ros_systems/test/test_drake_ros.cpp diff --git a/drake_ros_systems/test/integration.cpp b/drake_ros_systems/test/test_pub_sub.cpp similarity index 100% rename from drake_ros_systems/test/integration.cpp rename to drake_ros_systems/test/test_pub_sub.cpp diff --git a/drake_ros_systems/test/test_scene_markers.cpp b/drake_ros_systems/test/test_scene_markers.cpp new file mode 100644 index 00000000..148abd67 --- /dev/null +++ b/drake_ros_systems/test/test_scene_markers.cpp @@ -0,0 +1,544 @@ +// Copyright 2021 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include + +#include +#include +#include + +#include "drake_ros_systems/scene_markers_system.hpp" + +using drake_ros_systems::SceneMarkersSystem; + +static constexpr char kSourceName[] = "test"; +static constexpr double kTolerance{1e-6}; + +struct SingleSphereSceneTestDetails +{ + static constexpr double kRadius{1.}; + + static + drake::geometry::FramePoseVector + PopulateSceneGraph( + const drake::geometry::SourceId & source_id, + drake::geometry::SceneGraph * scene_graph) + { + const drake::geometry::GeometryId geometry_id = + scene_graph->RegisterAnchoredGeometry( + source_id, std::make_unique( + drake::math::RigidTransform::Identity(), + std::make_unique(kRadius), "sphere")); + scene_graph->AssignRole( + source_id, geometry_id, drake::geometry::IllustrationProperties()); + return {}; + } + + static + void + CheckSceneMarkers( + const visualization_msgs::msg::MarkerArray & marker_array, + SceneMarkersSystem * scene_markers_system) + { + ASSERT_EQ(marker_array.markers.size(), 1u); + const visualization_msgs::msg::Marker & marker = marker_array.markers[0]; + EXPECT_EQ(marker.header.frame_id, "world"); + EXPECT_EQ(marker.header.stamp.sec, 0); + EXPECT_EQ(marker.header.stamp.nanosec, 0u); + EXPECT_EQ(marker.ns, std::string(kSourceName) + "::sphere"); + EXPECT_EQ(marker.id, 0); + EXPECT_EQ(marker.action, visualization_msgs::msg::Marker::MODIFY); + EXPECT_EQ(marker.type, visualization_msgs::msg::Marker::SPHERE); + EXPECT_EQ(marker.lifetime.sec, 0); + EXPECT_EQ(marker.lifetime.nanosec, 0u); + EXPECT_TRUE(marker.frame_locked); + EXPECT_DOUBLE_EQ(marker.scale.x, kRadius); + EXPECT_DOUBLE_EQ(marker.scale.y, kRadius); + EXPECT_DOUBLE_EQ(marker.scale.z, kRadius); + const drake::geometry::Rgba & default_color = + scene_markers_system->default_color(); + EXPECT_NEAR(marker.color.r, default_color.r(), kTolerance); + EXPECT_NEAR(marker.color.g, default_color.g(), kTolerance); + EXPECT_NEAR(marker.color.b, default_color.b(), kTolerance); + EXPECT_NEAR(marker.color.a, default_color.a(), kTolerance); + EXPECT_DOUBLE_EQ(marker.pose.position.x, 0.); + EXPECT_DOUBLE_EQ(marker.pose.position.y, 0.); + EXPECT_DOUBLE_EQ(marker.pose.position.z, 0.); + EXPECT_DOUBLE_EQ(marker.pose.orientation.x, 0.); + EXPECT_DOUBLE_EQ(marker.pose.orientation.y, 0.); + EXPECT_DOUBLE_EQ(marker.pose.orientation.z, 0.); + EXPECT_DOUBLE_EQ(marker.pose.orientation.w, 1.); + } +}; + +struct SingleEllipsoidSceneTestDetails +{ + static constexpr double kLengthA{0.3}; + static constexpr double kLengthB{0.4}; + static constexpr double kLengthC{0.5}; + + static + drake::geometry::FramePoseVector + PopulateSceneGraph( + const drake::geometry::SourceId & source_id, + drake::geometry::SceneGraph * scene_graph) + { + const drake::geometry::GeometryId geometry_id = + scene_graph->RegisterAnchoredGeometry( + source_id, std::make_unique( + drake::math::RigidTransform::Identity(), + std::make_unique( + kLengthA, kLengthB, kLengthC), "ellipsoid")); + scene_graph->AssignRole( + source_id, geometry_id, drake::geometry::IllustrationProperties()); + return {}; + } + + static + void + CheckSceneMarkers( + const visualization_msgs::msg::MarkerArray & marker_array, + SceneMarkersSystem * scene_markers_system) + { + ASSERT_EQ(marker_array.markers.size(), 1u); + const visualization_msgs::msg::Marker & marker = marker_array.markers[0]; + EXPECT_EQ(marker.header.frame_id, "world"); + EXPECT_EQ(marker.header.stamp.sec, 0); + EXPECT_EQ(marker.header.stamp.nanosec, 0u); + EXPECT_EQ(marker.ns, std::string(kSourceName) + "::ellipsoid"); + EXPECT_EQ(marker.id, 0); + EXPECT_EQ(marker.action, visualization_msgs::msg::Marker::MODIFY); + EXPECT_EQ(marker.type, visualization_msgs::msg::Marker::SPHERE); + EXPECT_EQ(marker.lifetime.sec, 0); + EXPECT_EQ(marker.lifetime.nanosec, 0u); + EXPECT_TRUE(marker.frame_locked); + EXPECT_DOUBLE_EQ(marker.scale.x, kLengthA); + EXPECT_DOUBLE_EQ(marker.scale.y, kLengthB); + EXPECT_DOUBLE_EQ(marker.scale.z, kLengthC); + const drake::geometry::Rgba & default_color = + scene_markers_system->default_color(); + EXPECT_NEAR(marker.color.r, default_color.r(), kTolerance); + EXPECT_NEAR(marker.color.g, default_color.g(), kTolerance); + EXPECT_NEAR(marker.color.b, default_color.b(), kTolerance); + EXPECT_NEAR(marker.color.a, default_color.a(), kTolerance); + EXPECT_DOUBLE_EQ(marker.pose.position.x, 0.); + EXPECT_DOUBLE_EQ(marker.pose.position.y, 0.); + EXPECT_DOUBLE_EQ(marker.pose.position.z, 0.); + EXPECT_DOUBLE_EQ(marker.pose.orientation.x, 0.); + EXPECT_DOUBLE_EQ(marker.pose.orientation.y, 0.); + EXPECT_DOUBLE_EQ(marker.pose.orientation.z, 0.); + EXPECT_DOUBLE_EQ(marker.pose.orientation.w, 1.); + } +}; + +struct SingleCylinderSceneTestDetails +{ + static constexpr double kRadius{0.5}; + static constexpr double kLength{1.0}; + + static + drake::geometry::FramePoseVector + PopulateSceneGraph( + const drake::geometry::SourceId & source_id, + drake::geometry::SceneGraph * scene_graph) + { + const drake::geometry::GeometryId geometry_id = + scene_graph->RegisterAnchoredGeometry( + source_id, std::make_unique( + drake::math::RigidTransform::Identity(), + std::make_unique(kRadius, kLength), "cylinder")); + scene_graph->AssignRole( + source_id, geometry_id, drake::geometry::IllustrationProperties()); + return {}; + } + + static + void + CheckSceneMarkers( + const visualization_msgs::msg::MarkerArray & marker_array, + SceneMarkersSystem * scene_markers_system) + { + ASSERT_EQ(marker_array.markers.size(), 1u); + const visualization_msgs::msg::Marker & marker = marker_array.markers[0]; + EXPECT_EQ(marker.header.frame_id, "world"); + EXPECT_EQ(marker.header.stamp.sec, 0); + EXPECT_EQ(marker.header.stamp.nanosec, 0u); + EXPECT_EQ(marker.ns, std::string(kSourceName) + "::cylinder"); + EXPECT_EQ(marker.id, 0); + EXPECT_EQ(marker.action, visualization_msgs::msg::Marker::MODIFY); + EXPECT_EQ(marker.type, visualization_msgs::msg::Marker::CYLINDER); + EXPECT_EQ(marker.lifetime.sec, 0); + EXPECT_EQ(marker.lifetime.nanosec, 0u); + EXPECT_TRUE(marker.frame_locked); + EXPECT_DOUBLE_EQ(marker.scale.x, kRadius); + EXPECT_DOUBLE_EQ(marker.scale.y, kRadius); + EXPECT_DOUBLE_EQ(marker.scale.z, kLength); + const drake::geometry::Rgba & default_color = + scene_markers_system->default_color(); + EXPECT_NEAR(marker.color.r, default_color.r(), kTolerance); + EXPECT_NEAR(marker.color.g, default_color.g(), kTolerance); + EXPECT_NEAR(marker.color.b, default_color.b(), kTolerance); + EXPECT_NEAR(marker.color.a, default_color.a(), kTolerance); + EXPECT_DOUBLE_EQ(marker.pose.position.x, 0.); + EXPECT_DOUBLE_EQ(marker.pose.position.y, 0.); + EXPECT_DOUBLE_EQ(marker.pose.position.z, 0.); + EXPECT_DOUBLE_EQ(marker.pose.orientation.x, 0.); + EXPECT_DOUBLE_EQ(marker.pose.orientation.y, 0.); + EXPECT_DOUBLE_EQ(marker.pose.orientation.z, 0.); + EXPECT_DOUBLE_EQ(marker.pose.orientation.w, 1.); + } +}; + +struct SingleHalfSpaceSceneTestDetails +{ + static + drake::geometry::FramePoseVector + PopulateSceneGraph( + const drake::geometry::SourceId & source_id, + drake::geometry::SceneGraph * scene_graph) + { + const drake::geometry::GeometryId geometry_id = + scene_graph->RegisterAnchoredGeometry( + source_id, std::make_unique( + drake::math::RigidTransform::Identity(), + std::make_unique(), "hspace")); + scene_graph->AssignRole( + source_id, geometry_id, drake::geometry::IllustrationProperties()); + return {}; + } + + static + void + CheckSceneMarkers( + const visualization_msgs::msg::MarkerArray & marker_array, + SceneMarkersSystem * scene_markers_system) + { + ASSERT_EQ(marker_array.markers.size(), 1u); + const visualization_msgs::msg::Marker & marker = marker_array.markers[0]; + EXPECT_EQ(marker.header.frame_id, "world"); + EXPECT_EQ(marker.header.stamp.sec, 0); + EXPECT_EQ(marker.header.stamp.nanosec, 0u); + EXPECT_EQ(marker.ns, std::string(kSourceName) + "::hspace"); + EXPECT_EQ(marker.id, 0); + EXPECT_EQ(marker.action, visualization_msgs::msg::Marker::MODIFY); + EXPECT_EQ(marker.type, visualization_msgs::msg::Marker::CUBE); + EXPECT_EQ(marker.lifetime.sec, 0); + EXPECT_EQ(marker.lifetime.nanosec, 0u); + EXPECT_TRUE(marker.frame_locked); + EXPECT_GT(marker.scale.x, 10.0); + EXPECT_GT(marker.scale.y, 10.0); + const drake::geometry::Rgba & default_color = + scene_markers_system->default_color(); + EXPECT_NEAR(marker.color.r, default_color.r(), kTolerance); + EXPECT_NEAR(marker.color.g, default_color.g(), kTolerance); + EXPECT_NEAR(marker.color.b, default_color.b(), kTolerance); + EXPECT_NEAR(marker.color.a, default_color.a(), kTolerance); + } +}; + +struct SingleBoxSceneTestDetails +{ + static constexpr double kWidth{0.5}; + static constexpr double kDepth{0.25}; + static constexpr double kHeight{1.0}; + + static + drake::geometry::FramePoseVector + PopulateSceneGraph( + const drake::geometry::SourceId & source_id, + drake::geometry::SceneGraph * scene_graph) + { + const drake::geometry::GeometryId geometry_id = + scene_graph->RegisterAnchoredGeometry( + source_id, std::make_unique( + drake::math::RigidTransform::Identity(), + std::make_unique(kWidth, kDepth, kHeight), "box")); + scene_graph->AssignRole( + source_id, geometry_id, drake::geometry::IllustrationProperties()); + return {}; + } + + static + void + CheckSceneMarkers( + const visualization_msgs::msg::MarkerArray & marker_array, + SceneMarkersSystem * scene_markers_system) + { + ASSERT_EQ(marker_array.markers.size(), 1u); + const visualization_msgs::msg::Marker & marker = marker_array.markers[0]; + EXPECT_EQ(marker.header.frame_id, "world"); + EXPECT_EQ(marker.header.stamp.sec, 0); + EXPECT_EQ(marker.header.stamp.nanosec, 0u); + EXPECT_EQ(marker.ns, std::string(kSourceName) + "::box"); + EXPECT_EQ(marker.id, 0); + EXPECT_EQ(marker.action, visualization_msgs::msg::Marker::MODIFY); + EXPECT_EQ(marker.type, visualization_msgs::msg::Marker::CUBE); + EXPECT_EQ(marker.lifetime.sec, 0); + EXPECT_EQ(marker.lifetime.nanosec, 0u); + EXPECT_TRUE(marker.frame_locked); + EXPECT_DOUBLE_EQ(marker.scale.x, kWidth); + EXPECT_DOUBLE_EQ(marker.scale.y, kDepth); + EXPECT_DOUBLE_EQ(marker.scale.z, kHeight); + const drake::geometry::Rgba & default_color = + scene_markers_system->default_color(); + EXPECT_NEAR(marker.color.r, default_color.r(), kTolerance); + EXPECT_NEAR(marker.color.g, default_color.g(), kTolerance); + EXPECT_NEAR(marker.color.b, default_color.b(), kTolerance); + EXPECT_NEAR(marker.color.a, default_color.a(), kTolerance); + EXPECT_DOUBLE_EQ(marker.pose.position.x, 0.); + EXPECT_DOUBLE_EQ(marker.pose.position.y, 0.); + EXPECT_DOUBLE_EQ(marker.pose.position.z, 0.); + EXPECT_DOUBLE_EQ(marker.pose.orientation.x, 0.); + EXPECT_DOUBLE_EQ(marker.pose.orientation.y, 0.); + EXPECT_DOUBLE_EQ(marker.pose.orientation.z, 0.); + EXPECT_DOUBLE_EQ(marker.pose.orientation.w, 1.); + } +}; + +struct SingleCapsuleSceneTestDetails +{ + static constexpr double kRadius{0.25}; + static constexpr double kLength{0.5}; + + static + drake::geometry::FramePoseVector + PopulateSceneGraph( + const drake::geometry::SourceId & source_id, + drake::geometry::SceneGraph * scene_graph) + { + const drake::geometry::GeometryId geometry_id = + scene_graph->RegisterAnchoredGeometry( + source_id, std::make_unique( + drake::math::RigidTransform::Identity(), + std::make_unique(kRadius, kLength), "capsule")); + scene_graph->AssignRole( + source_id, geometry_id, drake::geometry::IllustrationProperties()); + return {}; + } + + static + void + CheckSceneMarkers( + const visualization_msgs::msg::MarkerArray & marker_array, + SceneMarkersSystem * scene_markers_system) + { + ASSERT_EQ(marker_array.markers.size(), 3u); + const drake::geometry::Rgba & default_color = scene_markers_system->default_color(); + + const visualization_msgs::msg::Marker & body_marker = marker_array.markers[0]; + EXPECT_EQ(body_marker.header.frame_id, "world"); + EXPECT_EQ(body_marker.header.stamp.sec, 0); + EXPECT_EQ(body_marker.header.stamp.nanosec, 0u); + EXPECT_EQ(body_marker.ns, std::string(kSourceName) + "::capsule"); + EXPECT_EQ(body_marker.id, 0); + EXPECT_EQ(body_marker.action, visualization_msgs::msg::Marker::MODIFY); + EXPECT_EQ(body_marker.type, visualization_msgs::msg::Marker::CYLINDER); + EXPECT_EQ(body_marker.lifetime.sec, 0); + EXPECT_EQ(body_marker.lifetime.nanosec, 0u); + EXPECT_TRUE(body_marker.frame_locked); + EXPECT_DOUBLE_EQ(body_marker.scale.x, kRadius); + EXPECT_DOUBLE_EQ(body_marker.scale.y, kRadius); + EXPECT_DOUBLE_EQ(body_marker.scale.z, kLength); + EXPECT_NEAR(body_marker.color.r, default_color.r(), kTolerance); + EXPECT_NEAR(body_marker.color.g, default_color.g(), kTolerance); + EXPECT_NEAR(body_marker.color.b, default_color.b(), kTolerance); + EXPECT_NEAR(body_marker.color.a, default_color.a(), kTolerance); + EXPECT_DOUBLE_EQ(body_marker.pose.position.x, 0.); + EXPECT_DOUBLE_EQ(body_marker.pose.position.y, 0.); + EXPECT_DOUBLE_EQ(body_marker.pose.position.z, 0.); + EXPECT_DOUBLE_EQ(body_marker.pose.orientation.x, 0.); + EXPECT_DOUBLE_EQ(body_marker.pose.orientation.y, 0.); + EXPECT_DOUBLE_EQ(body_marker.pose.orientation.z, 0.); + EXPECT_DOUBLE_EQ(body_marker.pose.orientation.w, 1.); + + const visualization_msgs::msg::Marker & upper_cap_marker = marker_array.markers[1]; + EXPECT_EQ(upper_cap_marker.header.frame_id, "world"); + EXPECT_EQ(upper_cap_marker.header.stamp.sec, 0); + EXPECT_EQ(upper_cap_marker.header.stamp.nanosec, 0u); + EXPECT_EQ(upper_cap_marker.ns, std::string(kSourceName) + "::capsule"); + EXPECT_EQ(upper_cap_marker.id, 1); + EXPECT_EQ(upper_cap_marker.action, visualization_msgs::msg::Marker::MODIFY); + EXPECT_EQ(upper_cap_marker.type, visualization_msgs::msg::Marker::SPHERE); + EXPECT_EQ(upper_cap_marker.lifetime.sec, 0); + EXPECT_EQ(upper_cap_marker.lifetime.nanosec, 0u); + EXPECT_TRUE(upper_cap_marker.frame_locked); + EXPECT_DOUBLE_EQ(upper_cap_marker.scale.x, kRadius); + EXPECT_DOUBLE_EQ(upper_cap_marker.scale.y, kRadius); + EXPECT_DOUBLE_EQ(upper_cap_marker.scale.z, kRadius); + EXPECT_NEAR(upper_cap_marker.color.r, default_color.r(), kTolerance); + EXPECT_NEAR(upper_cap_marker.color.g, default_color.g(), kTolerance); + EXPECT_NEAR(upper_cap_marker.color.b, default_color.b(), kTolerance); + EXPECT_NEAR(upper_cap_marker.color.a, default_color.a(), kTolerance); + EXPECT_DOUBLE_EQ(upper_cap_marker.pose.position.x, 0.); + EXPECT_DOUBLE_EQ(upper_cap_marker.pose.position.y, 0.); + EXPECT_DOUBLE_EQ(upper_cap_marker.pose.position.z, kLength / 2); + EXPECT_DOUBLE_EQ(upper_cap_marker.pose.orientation.x, 0.); + EXPECT_DOUBLE_EQ(upper_cap_marker.pose.orientation.y, 0.); + EXPECT_DOUBLE_EQ(upper_cap_marker.pose.orientation.z, 0.); + EXPECT_DOUBLE_EQ(upper_cap_marker.pose.orientation.w, 1.); + + const visualization_msgs::msg::Marker & lower_cap_marker = marker_array.markers[2]; + EXPECT_EQ(lower_cap_marker.header.frame_id, "world"); + EXPECT_EQ(lower_cap_marker.header.stamp.sec, 0); + EXPECT_EQ(lower_cap_marker.header.stamp.nanosec, 0u); + EXPECT_EQ(lower_cap_marker.ns, std::string(kSourceName) + "::capsule"); + EXPECT_EQ(lower_cap_marker.id, 2); + EXPECT_EQ(lower_cap_marker.action, visualization_msgs::msg::Marker::MODIFY); + EXPECT_EQ(lower_cap_marker.type, visualization_msgs::msg::Marker::SPHERE); + EXPECT_EQ(lower_cap_marker.lifetime.sec, 0); + EXPECT_EQ(lower_cap_marker.lifetime.nanosec, 0u); + EXPECT_TRUE(lower_cap_marker.frame_locked); + EXPECT_DOUBLE_EQ(lower_cap_marker.scale.x, kRadius); + EXPECT_DOUBLE_EQ(lower_cap_marker.scale.y, kRadius); + EXPECT_DOUBLE_EQ(lower_cap_marker.scale.z, kRadius); + EXPECT_NEAR(lower_cap_marker.color.r, default_color.r(), kTolerance); + EXPECT_NEAR(lower_cap_marker.color.g, default_color.g(), kTolerance); + EXPECT_NEAR(lower_cap_marker.color.b, default_color.b(), kTolerance); + EXPECT_NEAR(lower_cap_marker.color.a, default_color.a(), kTolerance); + EXPECT_DOUBLE_EQ(lower_cap_marker.pose.position.x, 0.); + EXPECT_DOUBLE_EQ(lower_cap_marker.pose.position.y, 0.); + EXPECT_DOUBLE_EQ(lower_cap_marker.pose.position.z, -kLength / 2); + EXPECT_DOUBLE_EQ(lower_cap_marker.pose.orientation.x, 0.); + EXPECT_DOUBLE_EQ(lower_cap_marker.pose.orientation.y, 0.); + EXPECT_DOUBLE_EQ(lower_cap_marker.pose.orientation.z, 0.); + EXPECT_DOUBLE_EQ(lower_cap_marker.pose.orientation.w, 1.); + } +}; + +template +struct SingleMeshSceneTestDetails +{ + static constexpr char kFilename[] = "/tmp/dummy.obj"; + static constexpr double kScale{0.1}; + + static + drake::geometry::FramePoseVector + PopulateSceneGraph( + const drake::geometry::SourceId & source_id, + drake::geometry::SceneGraph * scene_graph) + { + const drake::geometry::GeometryId geometry_id = + scene_graph->RegisterAnchoredGeometry( + source_id, std::make_unique( + drake::math::RigidTransform::Identity(), + std::make_unique(kFilename, kScale), "mesh")); + scene_graph->AssignRole( + source_id, geometry_id, drake::geometry::IllustrationProperties()); + return {}; + } + + static + void + CheckSceneMarkers( + const visualization_msgs::msg::MarkerArray & marker_array, + SceneMarkersSystem * scene_markers_system) + { + ASSERT_EQ(marker_array.markers.size(), 1u); + const visualization_msgs::msg::Marker & marker = marker_array.markers[0]; + EXPECT_EQ(marker.header.frame_id, "world"); + EXPECT_EQ(marker.header.stamp.sec, 0); + EXPECT_EQ(marker.header.stamp.nanosec, 0u); + EXPECT_EQ(marker.ns, std::string(kSourceName) + "::mesh"); + EXPECT_EQ(marker.id, 0); + EXPECT_EQ(marker.action, visualization_msgs::msg::Marker::MODIFY); + EXPECT_EQ(marker.type, visualization_msgs::msg::Marker::MESH_RESOURCE); + EXPECT_EQ(marker.lifetime.sec, 0); + EXPECT_EQ(marker.lifetime.nanosec, 0u); + EXPECT_TRUE(marker.frame_locked); + EXPECT_EQ(marker.mesh_resource, std::string("file://") + kFilename); + EXPECT_DOUBLE_EQ(marker.scale.x, kScale); + EXPECT_DOUBLE_EQ(marker.scale.y, kScale); + EXPECT_DOUBLE_EQ(marker.scale.z, kScale); + const drake::geometry::Rgba & default_color = + scene_markers_system->default_color(); + EXPECT_NEAR(marker.color.r, default_color.r(), kTolerance); + EXPECT_NEAR(marker.color.g, default_color.g(), kTolerance); + EXPECT_NEAR(marker.color.b, default_color.b(), kTolerance); + EXPECT_NEAR(marker.color.a, default_color.a(), kTolerance); + EXPECT_DOUBLE_EQ(marker.pose.position.x, 0.); + EXPECT_DOUBLE_EQ(marker.pose.position.y, 0.); + EXPECT_DOUBLE_EQ(marker.pose.position.z, 0.); + EXPECT_DOUBLE_EQ(marker.pose.orientation.x, 0.); + EXPECT_DOUBLE_EQ(marker.pose.orientation.y, 0.); + EXPECT_DOUBLE_EQ(marker.pose.orientation.z, 0.); + EXPECT_DOUBLE_EQ(marker.pose.orientation.w, 1.); + } +}; + +template +class SceneMarkersTest : public ::testing::Test +{ +}; + +TYPED_TEST_SUITE_P(SceneMarkersTest); + +TYPED_TEST_P(SceneMarkersTest, nominal) +{ + using TestDetails = TypeParam; + + drake::systems::DiagramBuilder builder; + auto scene_graph = builder.AddSystem(); + auto source_id = scene_graph->RegisterSource(kSourceName); + auto pose_vector_value = drake::AbstractValue::Make( + TestDetails::PopulateSceneGraph(source_id, scene_graph)); + auto pose_vector_source = + builder.AddSystem(*pose_vector_value); + builder.Connect( + pose_vector_source->get_output_port(), + scene_graph->get_source_pose_port(source_id)); + + auto scene_markers = builder.AddSystem(); + builder.Connect( + scene_graph->get_query_output_port(), + scene_markers->get_graph_query_port()); + + builder.ExportOutput(scene_markers->get_markers_output_port()); + + std::unique_ptr> diagram = builder.Build(); + std::unique_ptr> context = diagram->CreateDefaultContext(); + + const drake::systems::OutputPort & markers_port = diagram->get_output_port(); + auto marker_array = markers_port.Eval(*context); + + TestDetails::CheckSceneMarkers(marker_array, scene_markers); +} + +REGISTER_TYPED_TEST_SUITE_P(SceneMarkersTest, nominal); + +using SingleGeometrySceneTestDetails = ::testing::Types< + SingleSphereSceneTestDetails, + SingleEllipsoidSceneTestDetails, + SingleCylinderSceneTestDetails, + SingleHalfSpaceSceneTestDetails, + SingleBoxSceneTestDetails, + SingleCapsuleSceneTestDetails, + SingleMeshSceneTestDetails, + SingleMeshSceneTestDetails +>; + +INSTANTIATE_TYPED_TEST_SUITE_P( + SingleGeometrySceneMarkersTests, + SceneMarkersTest, + SingleGeometrySceneTestDetails); diff --git a/drake_ros_systems/test/test_tf_broadcaster.cpp b/drake_ros_systems/test/test_tf_broadcaster.cpp index e7dd4c5c..64a92bd8 100644 --- a/drake_ros_systems/test/test_tf_broadcaster.cpp +++ b/drake_ros_systems/test/test_tf_broadcaster.cpp @@ -1,4 +1,4 @@ -// Copyright 2020 Open Source Robotics Foundation, Inc. +// Copyright 2021 Open Source Robotics Foundation, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. From 76649f69346084ee75c6a50dda713d3bd6d4a956 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Mon, 3 May 2021 18:23:45 -0300 Subject: [PATCH 69/87] Update iiwa_manipulator examples Signed-off-by: Michel Hidalgo --- drake_ros_systems/example/iiwa_manipulator.cpp | 2 +- drake_ros_systems/example/iiwa_manipulator.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/drake_ros_systems/example/iiwa_manipulator.cpp b/drake_ros_systems/example/iiwa_manipulator.cpp index e96c9445..d6bf41cf 100644 --- a/drake_ros_systems/example/iiwa_manipulator.cpp +++ b/drake_ros_systems/example/iiwa_manipulator.cpp @@ -47,7 +47,7 @@ int main() builder.Connect( manipulation_station->GetOutputPort("query_object"), - rviz_visualizer->GetInputPort("graph_query")); + rviz_visualizer->get_graph_query_port()); auto diagram = builder.Build(); auto context = diagram->CreateDefaultContext(); diff --git a/drake_ros_systems/example/iiwa_manipulator.py b/drake_ros_systems/example/iiwa_manipulator.py index b608bbb8..57f5e88e 100755 --- a/drake_ros_systems/example/iiwa_manipulator.py +++ b/drake_ros_systems/example/iiwa_manipulator.py @@ -37,7 +37,7 @@ def main(): builder.Connect( manipulation_station.GetOutputPort('query_object'), - rviz_visualizer.GetInputPort('graph_query') + rviz_visualizer.get_graph_query_port() ) diagram = builder.Build() From abb1a79a0ce9d42b2536b00059fbc6046d06ea6f Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Mon, 3 May 2021 19:18:51 -0300 Subject: [PATCH 70/87] Make RvizVisualizer publish /tf by default Signed-off-by: Michel Hidalgo --- .../src/python_bindings/module_drake_ros_systems.cpp | 2 +- 1 file changed, 1 insertion(+), 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 7db4d191..122d816f 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 @@ -134,7 +134,7 @@ PYBIND11_MODULE(drake_ros_systems, m) { py::arg("publish_triggers") = std::unordered_set{TriggerType::kPerStep, TriggerType::kForced}, py::arg("publish_period") = 0.0, - py::arg("publish_tf") = false) + py::arg("publish_tf") = true) .def( "get_graph_query_port", &RvizVisualizer::get_graph_query_port, py::return_value_policy::reference_internal); From 1e092f6502cf0ecc5a1231bbff7571b9dcbef380 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Mon, 3 May 2021 19:19:52 -0300 Subject: [PATCH 71/87] Move examples out of drake_ros_systems. Also export drake_ros_systems targets properly. Signed-off-by: Michel Hidalgo --- drake_ros_systems/CMakeLists.txt | 23 ++- drake_ros_systems/README.md | 122 +------------ drake_ros_systems/example/CMakeLists.txt | 14 -- .../example/iiwa_manipulator.cpp | 77 -------- drake_ros_systems/example/iiwa_manipulator.py | 66 ------- .../example/iiwa_manipulator.rviz | 165 ------------------ drake_ros_systems/example/rs_flip_flop.cpp | 154 ---------------- drake_ros_systems/example/rs_flip_flop.py | 148 ---------------- 8 files changed, 12 insertions(+), 757 deletions(-) delete mode 100644 drake_ros_systems/example/CMakeLists.txt delete mode 100644 drake_ros_systems/example/iiwa_manipulator.cpp delete mode 100755 drake_ros_systems/example/iiwa_manipulator.py delete mode 100644 drake_ros_systems/example/iiwa_manipulator.rviz delete mode 100644 drake_ros_systems/example/rs_flip_flop.cpp delete mode 100755 drake_ros_systems/example/rs_flip_flop.py diff --git a/drake_ros_systems/CMakeLists.txt b/drake_ros_systems/CMakeLists.txt index 0639cac9..71a0e08e 100644 --- a/drake_ros_systems/CMakeLists.txt +++ b/drake_ros_systems/CMakeLists.txt @@ -1,11 +1,6 @@ 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) @@ -40,12 +35,12 @@ add_library(drake_ros_systems ) target_link_libraries(drake_ros_systems PUBLIC - drake::drake - rclcpp::rclcpp - rosidl_runtime_c::rosidl_runtime_c - rosidl_typesupport_cpp::rosidl_typesupport_cpp - tf2_ros::tf2_ros - ${visualization_msgs_TARGETS} + drake::drake + rclcpp::rclcpp + rosidl_runtime_c::rosidl_runtime_c + rosidl_typesupport_cpp::rosidl_typesupport_cpp + tf2_ros::tf2_ros + ${visualization_msgs_TARGETS} ) target_include_directories(drake_ros_systems @@ -55,11 +50,15 @@ target_include_directories(drake_ros_systems "$" ) -add_subdirectory(example) +ament_export_include_directories(include) +ament_export_libraries(drake_ros_systems) +ament_export_targets(${PROJECT_NAME}) ament_export_dependencies(drake) ament_export_dependencies(rclcpp) ament_export_dependencies(rosidl_generator_c) +ament_export_dependencies(tf2_ros) +ament_export_dependencies(visualization_msgs) install(TARGETS ${PROJECT_NAME} EXPORT ${PROJECT_NAME} ARCHIVE DESTINATION lib diff --git a/drake_ros_systems/README.md b/drake_ros_systems/README.md index 6cab6aae..6f5ec8af 100644 --- a/drake_ros_systems/README.md +++ b/drake_ros_systems/README.md @@ -4,7 +4,7 @@ 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`, `LcmSubscriberSystem`, and `DrakeVisualizer`. -# Building +## Building This package has been built and tested on Ubuntu Focal with ROS Rolling, using a Drake nightly from April 2021. It may work on other versions of ROS and Drake, but it hasn't been tested. @@ -33,123 +33,3 @@ To build it: 1. `cd build` 1. Configure the project `cmake -DCMAKE_INSTALL_PREFIX=$(pwd)/../install ..` 1. Build the project `make && make install` - -# Running - -If you built with `colcon`, then source your workspace. - -``` -. ./ws/install/setup.bash -# Also make sure to activate drake virtual environment -``` - -If you built with plain CMake, then source the ROS workspace and set these variables. - -``` -. /opt/ros/rolling/setup.bash -# Also make sure to activate drake virtual environment -# CD to folder containing `build` and `install` created earlier, commands below use PWD to find correct path -export LD_LIBRARY_PATH=$(pwd)/install/lib:$LD_LIBRARY_PATH -export PYTHONPATH=$(pwd)/install/lib/$(python -c 'import sys; print(f"python{sys.version_info[0]}.{sys.version_info[1]}")')/site-packages:$PYTHONPATH -``` - -Now you can run C++ examples from the build folder and Python examples from the source folder. - -# Examples - -Some examples of using these systems are given in the [`example`](./example) folder in two languages: Python and C++. - -## RS flip flop - -### Overview - -Both `rs_flip_flop` and `rs_flip_flop.py` 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` - -### How To - -If built with **colcon** using an isolated build (default), run the C++ example as follows: - -``` -./build/drake_ros_systems/example/rs_flip_flop -``` - -If built with **cmake** or a non-isolated build of **colcon**, run the C++ example as follows: - -``` -./build/example/rs_flip_flop -``` - - -The Python example can always be run from the source folder as follows: - -``` -python3 ./example/rs_flip_flop.py -``` - -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" -``` - -## IIWA Manipulator - -### Overview - -Both `iiwa_manipulator` and `iiwa_manipulator.py` enable RViz visualization of a static `ManipulationStation` example. -They publish the following topics: - -* `/tf` (all scene frames) -* `/scene_markers` (all scene geometries, including the robot model) - -### How To - -If built with **colcon** using an isolated build (default), run the C++ example as follows: - -``` -./build/drake_ros_systems/example/iiwa_manipulator -``` - -If built with **cmake** or a non-isolated build of **colcon**, run the C++ example as follows: - -``` -./build/example/iiwa_manipulator -``` - - -The Python example can always be run from the source folder as follows: - -``` -python3 ./example/iiwa_manipulator.py -``` - -Run RViz in a different terminal with your ROS installation sourced to visualize the station: - -``` -ros2 run rviz2 rviz2 -d ./example/iiwa_manipulator.rviz -``` diff --git a/drake_ros_systems/example/CMakeLists.txt b/drake_ros_systems/example/CMakeLists.txt deleted file mode 100644 index d7733920..00000000 --- a/drake_ros_systems/example/CMakeLists.txt +++ /dev/null @@ -1,14 +0,0 @@ -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} -) - -add_executable(iiwa_manipulator iiwa_manipulator.cpp) -target_link_libraries(iiwa_manipulator - drake::drake - drake_ros_systems -) diff --git a/drake_ros_systems/example/iiwa_manipulator.cpp b/drake_ros_systems/example/iiwa_manipulator.cpp deleted file mode 100644 index d6bf41cf..00000000 --- a/drake_ros_systems/example/iiwa_manipulator.cpp +++ /dev/null @@ -1,77 +0,0 @@ -// Copyright 2021 Open Source Robotics Foundation, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include - -#include - -#include -#include -#include - -#include -#include - -using drake_ros_systems::DrakeRos; -using drake_ros_systems::RosInterfaceSystem; -using drake_ros_systems::RvizVisualizer; - -using drake::examples::manipulation_station::ManipulationStation; - - -int main() -{ - drake::systems::DiagramBuilder builder; - - auto ros_interface_system = - builder.AddSystem(std::make_unique()); - - auto manipulation_station = builder.AddSystem(); - manipulation_station->SetupClutterClearingStation(); - manipulation_station->Finalize(); - - auto rviz_visualizer = builder.AddSystem( - ros_interface_system->get_ros_interface()); - - builder.Connect( - manipulation_station->GetOutputPort("query_object"), - rviz_visualizer->get_graph_query_port()); - - 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(); - - auto & manipulation_station_context = - diagram->GetMutableSubsystemContext(*manipulation_station, &simulator_context); - - manipulation_station->GetInputPort("iiwa_position") - .FixValue( - &manipulation_station_context, - manipulation_station->GetIiwaPosition(manipulation_station_context)); - - manipulation_station->GetInputPort("wsg_position") - .FixValue(&manipulation_station_context, 0.); - - while (true) { - simulator->AdvanceTo(simulator_context.get_time() + 0.1); - } - return 0; -} diff --git a/drake_ros_systems/example/iiwa_manipulator.py b/drake_ros_systems/example/iiwa_manipulator.py deleted file mode 100755 index 57f5e88e..00000000 --- a/drake_ros_systems/example/iiwa_manipulator.py +++ /dev/null @@ -1,66 +0,0 @@ -#!/usr/bin/env python3 -# Copyright 2021 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -import numpy as np - -from drake_ros_systems import RosInterfaceSystem -from drake_ros_systems import RvizVisualizer - -from pydrake.examples.manipulation_station import ManipulationStation -from pydrake.systems.analysis import Simulator -from pydrake.systems.framework import DiagramBuilder - - -def main(): - builder = DiagramBuilder() - - ros_interface_system = builder.AddSystem(RosInterfaceSystem()) - - manipulation_station = builder.AddSystem(ManipulationStation()) - manipulation_station.SetupClutterClearingStation() - manipulation_station.Finalize() - - rviz_visualizer = builder.AddSystem( - RvizVisualizer(ros_interface_system.get_ros_interface())) - - builder.Connect( - manipulation_station.GetOutputPort('query_object'), - rviz_visualizer.get_graph_query_port() - ) - - diagram = builder.Build() - - simulator = Simulator(diagram) - simulator.Initialize() - simulator.set_target_realtime_rate(1.0) - context = simulator.get_mutable_context() - - manipulation_station_context = \ - diagram.GetMutableSubsystemContext(manipulation_station, context) - manipulation_station.GetInputPort('iiwa_position').FixValue( - manipulation_station_context, - manipulation_station.GetIiwaPosition(manipulation_station_context)) - manipulation_station.GetInputPort('wsg_position').FixValue( - manipulation_station_context, np.zeros(1)) - - try: - while True: - simulator.AdvanceTo(context.get_time() + 0.1) - except KeyboardInterrupt: - pass - - -if __name__ == '__main__': - main() diff --git a/drake_ros_systems/example/iiwa_manipulator.rviz b/drake_ros_systems/example/iiwa_manipulator.rviz deleted file mode 100644 index 31a9d215..00000000 --- a/drake_ros_systems/example/iiwa_manipulator.rviz +++ /dev/null @@ -1,165 +0,0 @@ -Panels: - - Class: rviz_common/Displays - Help Height: 78 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Status1 - Splitter Ratio: 0.5 - Tree Height: 555 - - Class: rviz_common/Selection - Name: Selection - - Class: rviz_common/Tool Properties - Expanded: - - /2D Goal Pose1 - - /Publish Point1 - Name: Tool Properties - Splitter Ratio: 0.5886790156364441 - - Class: rviz_common/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: rviz_common/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: "" -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz_default_plugins/Grid - Color: 160; 160; 164 - Enabled: true - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 10 - Reference Frame: - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: MarkerArray - Namespaces: - Source_3::bin1::back: true - Source_3::bin1::bottom: true - Source_3::bin1::left: true - Source_3::bin1::right: true - Source_3::bin1::slope: true - Source_3::bin1::visual: true - Source_3::bin2::back: true - Source_3::bin2::bottom: true - Source_3::bin2::left: true - Source_3::bin2::right: true - Source_3::bin2::slope: true - Source_3::bin2::visual: true - Source_3::gripper::visual: true - Source_3::iiwa::iiwa_link_0_visual: true - Source_3::iiwa::iiwa_link_1_visual: true - Source_3::iiwa::iiwa_link_2_visual: true - Source_3::iiwa::iiwa_link_3_visual: true - Source_3::iiwa::iiwa_link_4_visual: true - Source_3::iiwa::iiwa_link_5_visual: true - Source_3::iiwa::iiwa_link_6_visual: true - Source_3::iiwa::iiwa_link_7_visual: true - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /scene_markers - Value: true - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Fixed Frame: world - Frame Rate: 30 - Name: root - Tools: - - Class: rviz_default_plugins/Interact - Hide Inactive Objects: true - - Class: rviz_default_plugins/MoveCamera - - Class: rviz_default_plugins/Select - - Class: rviz_default_plugins/FocusCamera - - Class: rviz_default_plugins/Measure - Line color: 128; 128; 0 - - Class: rviz_default_plugins/SetInitialPose - Covariance x: 0.25 - Covariance y: 0.25 - Covariance yaw: 0.06853891909122467 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /initialpose - - Class: rviz_default_plugins/SetGoal - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /goal_pose - - Class: rviz_default_plugins/PublishPoint - Single click: true - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /clicked_point - Transformation: - Current: - Class: rviz_default_plugins/TF - Value: true - Views: - Current: - Class: rviz_default_plugins/Orbit - Distance: 2.3739840984344482 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: 0.07645688951015472 - Y: -0.3666839599609375 - Z: 0.10706909000873566 - Focal Shape Fixed Size: true - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.7403980493545532 - Target Frame: - Value: Orbit (rviz) - Yaw: 0.5653908252716064 - Saved: ~ -Window Geometry: - Displays: - collapsed: false - Height: 846 - Hide Left Dock: false - Hide Right Dock: false - QMainWindow State: 000000ff00000000fd000000040000000000000156000002b4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000002b4000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000002b4000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000024400fffffffb0000000800540069006d006501000000000000045000000000000000000000023f000002b400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: false - Width: 1200 - X: 67 - Y: 60 diff --git a/drake_ros_systems/example/rs_flip_flop.cpp b/drake_ros_systems/example/rs_flip_flop.cpp deleted file mode 100644 index bbb2afb7..00000000 --- a/drake_ros_systems/example/rs_flip_flop.cpp +++ /dev/null @@ -1,154 +0,0 @@ -// Copyright 2020 Open Source Robotics Foundation, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -#include -#include -#include - -#include -#include -#include -#include - -#include - -#include -#include - -using drake_ros_systems::DrakeRos; -using drake_ros_systems::RosInterfaceSystem; -using drake_ros_systems::RosPublisherSystem; -using drake_ros_systems::RosSubscriberSystem; - -class NorGate : public drake::systems::LeafSystem -{ -public: - NorGate() - { - DeclareAbstractInputPort("A", *drake::AbstractValue::Make(std_msgs::msg::Bool())); - DeclareAbstractInputPort("B", *drake::AbstractValue::Make(std_msgs::msg::Bool())); - - DeclareAbstractOutputPort("Q", &NorGate::calc_output_value); - } - - virtual ~NorGate() = default; - -private: - void - calc_output_value( - const drake::systems::Context & context, - std_msgs::msg::Bool * output) const - { - const bool a = GetInputPort("A").Eval(context).data; - const bool b = GetInputPort("B").Eval(context).data; - output->data = !(a || b); - } -}; - -// Delay's input port by one timestep to avoid algebraic loop error -// Inspired by Simulink's Memory block -template -class Memory : public drake::systems::LeafSystem -{ -public: - explicit Memory(const T & initial_value) - { - DeclareAbstractInputPort("value", *drake::AbstractValue::Make(T())); - - // State for value - DeclareAbstractState(drake::Value(initial_value)); - - // Output depends only on the previous state - DeclareAbstractOutputPort("value", &Memory::calc_output_value, {all_state_ticket()}); - - DeclarePerStepEvent( - drake::systems::UnrestrictedUpdateEvent( - [this]( - const drake::systems::Context & context, - const drake::systems::UnrestrictedUpdateEvent &, - drake::systems::State * state) { - // Copy input value to state - drake::systems::AbstractValues & abstract_state = state->get_mutable_abstract_state(); - abstract_state.get_mutable_value(0).SetFrom( - get_input_port().Eval(context)); - })); - } - - virtual ~Memory() = default; - -private: - void - calc_output_value(const drake::systems::Context & context, T * output) const - { - *output = context.get_abstract_state().get_value(0).get_value(); - } -}; - -int main() -{ - // NOR gate RS flip flop example - // Input topics /S and /R are active high (true is logic 1 and false is logic 0) - // Output topics /Q and /Q_not are active low (true is logic 0 and false is logic 1) - - // Input/Output table - // S: false R: false | Q: no change Q_not: no change - // S: true R: false | Q: false Q_not: true - // S: false R: true | Q: true Q_not: false - // S: true R: true | Q: invalid Q_not: invalid - drake::systems::DiagramBuilder builder; - - rclcpp::QoS qos{10}; - - auto sys_ros_interface = builder.AddSystem(std::make_unique()); - auto sys_pub_Q = builder.AddSystem( - RosPublisherSystem::Make( - "Q", qos, sys_ros_interface->get_ros_interface())); - auto sys_pub_Q_not = builder.AddSystem( - RosPublisherSystem::Make( - "Q_not", qos, sys_ros_interface->get_ros_interface())); - auto sys_sub_S = builder.AddSystem( - RosSubscriberSystem::Make( - "S", qos, sys_ros_interface->get_ros_interface())); - auto sys_sub_R = builder.AddSystem( - RosSubscriberSystem::Make( - "R", qos, sys_ros_interface->get_ros_interface())); - auto sys_nor_gate_1 = builder.AddSystem(); - auto sys_nor_gate_2 = builder.AddSystem(); - auto sys_memory = builder.AddSystem>(std_msgs::msg::Bool()); - - builder.Connect(sys_nor_gate_1->GetOutputPort("Q"), sys_memory->get_input_port(0)); - - builder.Connect(sys_sub_S->get_output_port(0), sys_nor_gate_1->GetInputPort("A")); - builder.Connect(sys_nor_gate_2->GetOutputPort("Q"), sys_nor_gate_1->GetInputPort("B")); - - builder.Connect(sys_memory->get_output_port(0), sys_nor_gate_2->GetInputPort("A")); - builder.Connect(sys_sub_R->get_output_port(0), sys_nor_gate_2->GetInputPort("B")); - - builder.Connect(sys_nor_gate_1->GetOutputPort("Q"), sys_pub_Q->get_input_port(0)); - builder.Connect(sys_nor_gate_2->GetOutputPort("Q"), sys_pub_Q_not->get_input_port(0)); - - auto diagram = builder.Build(); - auto context = diagram->CreateDefaultContext(); - - auto simulator = - std::make_unique>(*diagram, std::move(context)); - simulator->set_target_realtime_rate(1.0); - simulator->Initialize(); - - auto & simulator_context = simulator->get_mutable_context(); - - while (true) { - simulator->AdvanceTo(simulator_context.get_time() + 0.1); - } - return 0; -} diff --git a/drake_ros_systems/example/rs_flip_flop.py b/drake_ros_systems/example/rs_flip_flop.py deleted file mode 100755 index 708aee21..00000000 --- a/drake_ros_systems/example/rs_flip_flop.py +++ /dev/null @@ -1,148 +0,0 @@ -#!/usr/bin/env python3 -# Copyright 2020 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from drake_ros_systems import RosInterfaceSystem -from drake_ros_systems import RosPublisherSystem -from drake_ros_systems import RosSubscriberSystem - -from pydrake.systems.analysis import Simulator -from pydrake.systems.framework import DiagramBuilder -from pydrake.systems.framework import UnrestrictedUpdateEvent -from pydrake.common.value import AbstractValue -from pydrake.systems.framework import LeafSystem - -from std_msgs.msg import Bool - -from rclpy.qos import QoSProfile - - -class NorGate(LeafSystem): - - def __init__(self): - super().__init__() - self._a = self.DeclareAbstractInputPort("A", AbstractValue.Make(Bool())) - self._b = self.DeclareAbstractInputPort("B", AbstractValue.Make(Bool())) - - self.DeclareAbstractOutputPort( - 'Q', - lambda: AbstractValue.Make(Bool()), - self._calc_output_value) - - def _calc_output_value(self, context, data): - a = self._a.Eval(context) - b = self._b.Eval(context) - data.get_mutable_value().data = not (a.data or b.data) - - -class Memory(LeafSystem): - """Delay input port by one time step to avoid algebraic loop error.""" - - def __init__(self, initial_value): - super().__init__() - - self._input = self.DeclareAbstractInputPort("A", AbstractValue.Make(initial_value)) - - self.DeclareAbstractState(AbstractValue.Make(initial_value)) - - self.DeclareAbstractOutputPort( - 'Q', - lambda: AbstractValue.Make(initial_value), - self._calc_output_value, - {self.all_state_ticket()}) - - self.DeclarePerStepEvent(UnrestrictedUpdateEvent(self._move_input_to_state)) - - def _move_input_to_state(self, context, event, state): - state.get_mutable_abstract_state().get_mutable_value(0).SetFrom( - AbstractValue.Make(self._input.Eval(context))) - - def _calc_output_value(self, context, output): - output.SetFrom(context.get_abstract_state().get_value(0)) - - -def main(): - # NOR gate RS flip flop example - # Input topics /S and /R are active high (true is logic 1 and false is logic 0) - # Output topics /Q and /Q_not are active low (true is logic 0 and false is logic 1) - - # Input/Output table - # S: false R: false | Q: no change Q_not: no change - # S: true R: false | Q: false Q_not: true - # S: false R: true | Q: true Q_not: false - # S: true R: true | Q: invalid Q_not: invalid - builder = DiagramBuilder() - - sys_ros_interface = builder.AddSystem(RosInterfaceSystem()) - - qos = QoSProfile(depth=10) - - sys_pub_Q = builder.AddSystem( - RosPublisherSystem(Bool, "Q", qos, sys_ros_interface.get_ros_interface())) - sys_pub_Q_not = builder.AddSystem( - RosPublisherSystem(Bool, "Q_not", qos, sys_ros_interface.get_ros_interface())) - - sys_sub_S = builder.AddSystem( - RosSubscriberSystem(Bool, "S", qos, sys_ros_interface.get_ros_interface())) - sys_sub_R = builder.AddSystem( - RosSubscriberSystem(Bool, "R", qos, sys_ros_interface.get_ros_interface())) - - sys_nor_gate_1 = builder.AddSystem(NorGate()) - sys_nor_gate_2 = builder.AddSystem(NorGate()) - - sys_memory = builder.AddSystem(Memory(Bool())) - - builder.Connect( - sys_nor_gate_1.GetOutputPort('Q'), - sys_memory.get_input_port(0) - ) - - builder.Connect( - sys_sub_S.get_output_port(0), - sys_nor_gate_1.GetInputPort('A'), - ) - builder.Connect( - sys_nor_gate_2.GetOutputPort('Q'), - sys_nor_gate_1.GetInputPort('B'), - ) - - builder.Connect( - sys_memory.get_output_port(0), - sys_nor_gate_2.GetInputPort('A'), - ) - builder.Connect( - sys_sub_R.get_output_port(0), - sys_nor_gate_2.GetInputPort('B'), - ) - - builder.Connect( - sys_nor_gate_1.GetOutputPort('Q'), - sys_pub_Q.get_input_port(0) - ) - builder.Connect( - sys_nor_gate_2.GetOutputPort('Q'), - sys_pub_Q_not.get_input_port(0) - ) - - diagram = builder.Build() - simulator = Simulator(diagram) - simulator_context = simulator.get_mutable_context() - simulator.set_target_realtime_rate(1.0) - - while True: - simulator.AdvanceTo(simulator_context.get_time() + 0.1) - - -if __name__ == '__main__': - main() From 731e82b61703bb6af424326d76aa6b9896207252 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Mon, 3 May 2021 19:20:24 -0300 Subject: [PATCH 72/87] Add drake_ros_examples package Signed-off-by: Michel Hidalgo --- drake_ros_examples/CMakeLists.txt | 24 +++ drake_ros_examples/README.md | 65 +++++++ drake_ros_examples/examples/CMakeLists.txt | 2 + .../examples/iiwa_manipulator/CMakeLists.txt | 16 ++ .../examples/iiwa_manipulator/README.md | 21 +++ .../iiwa_manipulator/iiwa_manipulator.cpp | 77 ++++++++ .../iiwa_manipulator/iiwa_manipulator.py | 66 +++++++ .../iiwa_manipulator/iiwa_manipulator.rviz | 165 ++++++++++++++++++ .../examples/rs_flip_flop/CMakeLists.txt | 18 ++ .../examples/rs_flip_flop/README.md | 40 +++++ .../examples/rs_flip_flop/rs_flip_flop.cpp | 154 ++++++++++++++++ .../examples/rs_flip_flop/rs_flip_flop.py | 143 +++++++++++++++ drake_ros_examples/package.xml | 21 +++ 13 files changed, 812 insertions(+) create mode 100644 drake_ros_examples/CMakeLists.txt create mode 100644 drake_ros_examples/README.md create mode 100644 drake_ros_examples/examples/CMakeLists.txt create mode 100644 drake_ros_examples/examples/iiwa_manipulator/CMakeLists.txt create mode 100644 drake_ros_examples/examples/iiwa_manipulator/README.md create mode 100644 drake_ros_examples/examples/iiwa_manipulator/iiwa_manipulator.cpp create mode 100755 drake_ros_examples/examples/iiwa_manipulator/iiwa_manipulator.py create mode 100644 drake_ros_examples/examples/iiwa_manipulator/iiwa_manipulator.rviz create mode 100644 drake_ros_examples/examples/rs_flip_flop/CMakeLists.txt create mode 100644 drake_ros_examples/examples/rs_flip_flop/README.md create mode 100644 drake_ros_examples/examples/rs_flip_flop/rs_flip_flop.cpp create mode 100755 drake_ros_examples/examples/rs_flip_flop/rs_flip_flop.py create mode 100644 drake_ros_examples/package.xml diff --git a/drake_ros_examples/CMakeLists.txt b/drake_ros_examples/CMakeLists.txt new file mode 100644 index 00000000..5f4cfca0 --- /dev/null +++ b/drake_ros_examples/CMakeLists.txt @@ -0,0 +1,24 @@ +cmake_minimum_required(VERSION 3.10) +project(drake_ros_examples) + +# 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(drake_ros_systems REQUIRED) + +add_subdirectory(examples) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/drake_ros_examples/README.md b/drake_ros_examples/README.md new file mode 100644 index 00000000..6f6e557f --- /dev/null +++ b/drake_ros_examples/README.md @@ -0,0 +1,65 @@ +# Drake ROS Examples + +This is a collection of examples built around `drake_ros_systems` C++ and Python APIs. + +## Building + +This package has been built and tested on Ubuntu Focal with ROS Rolling, using a Drake nightly from April 2021. +It may work on other versions of ROS and Drake, but it hasn't been tested. + +To build it: + +1. [Install ROS Rolling](https://index.ros.org/doc/ros2/Installation/Rolling/) +1. Source your ROS installation `. /opt/ros/rolling/setup.bash` +1. [Download April-ish 2021 Drake binary](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/RobotLocomotion/drake-ros.git` + 1. `cd ..` + 1. Build this package and its dependencies `colcon build --packages-up-to drake_ros_systems` + + **CMake** + 1. Get this code `git clone https://github.com/RobotLocomotion/drake-ros.git` + 1. Build the [`drake_ros_systems`](../drake_ros_systems/README.md#building) package using CMake first. + 1. Manually set `CMAKE_PREFIX_PATH`: `export CMAKE_PREFIX_PATH=$CMAKE_PREFIX_PATH:$(pwd)/drake-ros/drake_ros_systems/install` + 1. `cd drake-ros/drake_ros_examples` + 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 + +* If you built with `colcon`, then source your workspace. + +``` +. ./ws/install/setup.bash +# Also make sure to activate drake virtual environment +``` + + Now you can run C++ and Python examples using `ros2 run drake_ros_examples `. + + +* If you built with plain CMake, then source the ROS workspace and set these variables. + +``` +. /opt/ros/rolling/setup.bash +# Also make sure to activate drake virtual environment +# CD to repository root +export LD_LIBRARY_PATH=$(pwd)/drake_ros_systems/install/lib:$LD_LIBRARY_PATH +export LD_LIBRARY_PATH=$(pwd)/drake_ros_examples/install/lib:$LD_LIBRARY_PATH +export PYTHONPATH=$(pwd)/drake_ros_systems/install/lib/$(python -c 'import sys; print(f"python{sys.version_info[0]}.{sys.version_info[1]}")')/site-packages:$PYTHONPATH +export PYTHONPATH=$(pwd)/drake_ros_examples/install/lib/$(python -c 'import sys; print(f"python{sys.version_info[0]}.{sys.version_info[1]}")')/site-packages:$PYTHONPATH +``` + + Now you can run C++ and Python examples from the install folder using `./install/lib/drake_ros_examples/`. + +## List of examples + +- [RS flip flop](./examples/rs_flip_flop): a latch with a ROS 2 topic interface. +- [IIWA manipulator](./examples/iiwa_manipulator): an RViz visualization of a static IIWA arm. diff --git a/drake_ros_examples/examples/CMakeLists.txt b/drake_ros_examples/examples/CMakeLists.txt new file mode 100644 index 00000000..10d18967 --- /dev/null +++ b/drake_ros_examples/examples/CMakeLists.txt @@ -0,0 +1,2 @@ +add_subdirectory(iiwa_manipulator) +add_subdirectory(rs_flip_flop) diff --git a/drake_ros_examples/examples/iiwa_manipulator/CMakeLists.txt b/drake_ros_examples/examples/iiwa_manipulator/CMakeLists.txt new file mode 100644 index 00000000..296efde3 --- /dev/null +++ b/drake_ros_examples/examples/iiwa_manipulator/CMakeLists.txt @@ -0,0 +1,16 @@ +add_executable(iiwa_manipulator iiwa_manipulator.cpp) +target_link_libraries(iiwa_manipulator + drake::drake + drake_ros_systems::drake_ros_systems +) + +install( + PROGRAMS iiwa_manipulator.py + DESTINATION lib/${PROJECT_NAME} +) + +install( + TARGETS + iiwa_manipulator + DESTINATION lib/${PROJECT_NAME} +) diff --git a/drake_ros_examples/examples/iiwa_manipulator/README.md b/drake_ros_examples/examples/iiwa_manipulator/README.md new file mode 100644 index 00000000..fe64f6fa --- /dev/null +++ b/drake_ros_examples/examples/iiwa_manipulator/README.md @@ -0,0 +1,21 @@ +# IIWA Manipulator + +## Overview + +Both `iiwa_manipulator` and `iiwa_manipulator.py` enable RViz visualization of a static [`ManipulationStation`](https://github.com/RobotLocomotion/drake/tree/master/examples/manipulation_station) example. +They publish the following topics: + +* `/tf` (all scene frames) +* `/scene_markers` (all scene geometries, including the robot model) + +## How To + +Run either the C++ `iiwa_manipulator` executable or the Python `iiwa_manipulator.py` script as explained [here](../../README.md#running). + +Run RViz in a different terminal with your ROS installation sourced to visualize the station: + +``` +ros2 run rviz2 rviz2 -d iiwa_manipulator.rviz +``` + + diff --git a/drake_ros_examples/examples/iiwa_manipulator/iiwa_manipulator.cpp b/drake_ros_examples/examples/iiwa_manipulator/iiwa_manipulator.cpp new file mode 100644 index 00000000..d6bf41cf --- /dev/null +++ b/drake_ros_examples/examples/iiwa_manipulator/iiwa_manipulator.cpp @@ -0,0 +1,77 @@ +// Copyright 2021 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include + +#include +#include +#include + +#include +#include + +using drake_ros_systems::DrakeRos; +using drake_ros_systems::RosInterfaceSystem; +using drake_ros_systems::RvizVisualizer; + +using drake::examples::manipulation_station::ManipulationStation; + + +int main() +{ + drake::systems::DiagramBuilder builder; + + auto ros_interface_system = + builder.AddSystem(std::make_unique()); + + auto manipulation_station = builder.AddSystem(); + manipulation_station->SetupClutterClearingStation(); + manipulation_station->Finalize(); + + auto rviz_visualizer = builder.AddSystem( + ros_interface_system->get_ros_interface()); + + builder.Connect( + manipulation_station->GetOutputPort("query_object"), + rviz_visualizer->get_graph_query_port()); + + 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(); + + auto & manipulation_station_context = + diagram->GetMutableSubsystemContext(*manipulation_station, &simulator_context); + + manipulation_station->GetInputPort("iiwa_position") + .FixValue( + &manipulation_station_context, + manipulation_station->GetIiwaPosition(manipulation_station_context)); + + manipulation_station->GetInputPort("wsg_position") + .FixValue(&manipulation_station_context, 0.); + + while (true) { + simulator->AdvanceTo(simulator_context.get_time() + 0.1); + } + return 0; +} diff --git a/drake_ros_examples/examples/iiwa_manipulator/iiwa_manipulator.py b/drake_ros_examples/examples/iiwa_manipulator/iiwa_manipulator.py new file mode 100755 index 00000000..57f5e88e --- /dev/null +++ b/drake_ros_examples/examples/iiwa_manipulator/iiwa_manipulator.py @@ -0,0 +1,66 @@ +#!/usr/bin/env python3 +# Copyright 2021 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import numpy as np + +from drake_ros_systems import RosInterfaceSystem +from drake_ros_systems import RvizVisualizer + +from pydrake.examples.manipulation_station import ManipulationStation +from pydrake.systems.analysis import Simulator +from pydrake.systems.framework import DiagramBuilder + + +def main(): + builder = DiagramBuilder() + + ros_interface_system = builder.AddSystem(RosInterfaceSystem()) + + manipulation_station = builder.AddSystem(ManipulationStation()) + manipulation_station.SetupClutterClearingStation() + manipulation_station.Finalize() + + rviz_visualizer = builder.AddSystem( + RvizVisualizer(ros_interface_system.get_ros_interface())) + + builder.Connect( + manipulation_station.GetOutputPort('query_object'), + rviz_visualizer.get_graph_query_port() + ) + + diagram = builder.Build() + + simulator = Simulator(diagram) + simulator.Initialize() + simulator.set_target_realtime_rate(1.0) + context = simulator.get_mutable_context() + + manipulation_station_context = \ + diagram.GetMutableSubsystemContext(manipulation_station, context) + manipulation_station.GetInputPort('iiwa_position').FixValue( + manipulation_station_context, + manipulation_station.GetIiwaPosition(manipulation_station_context)) + manipulation_station.GetInputPort('wsg_position').FixValue( + manipulation_station_context, np.zeros(1)) + + try: + while True: + simulator.AdvanceTo(context.get_time() + 0.1) + except KeyboardInterrupt: + pass + + +if __name__ == '__main__': + main() diff --git a/drake_ros_examples/examples/iiwa_manipulator/iiwa_manipulator.rviz b/drake_ros_examples/examples/iiwa_manipulator/iiwa_manipulator.rviz new file mode 100644 index 00000000..31a9d215 --- /dev/null +++ b/drake_ros_examples/examples/iiwa_manipulator/iiwa_manipulator.rviz @@ -0,0 +1,165 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + Splitter Ratio: 0.5 + Tree Height: 555 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MarkerArray + Namespaces: + Source_3::bin1::back: true + Source_3::bin1::bottom: true + Source_3::bin1::left: true + Source_3::bin1::right: true + Source_3::bin1::slope: true + Source_3::bin1::visual: true + Source_3::bin2::back: true + Source_3::bin2::bottom: true + Source_3::bin2::left: true + Source_3::bin2::right: true + Source_3::bin2::slope: true + Source_3::bin2::visual: true + Source_3::gripper::visual: true + Source_3::iiwa::iiwa_link_0_visual: true + Source_3::iiwa::iiwa_link_1_visual: true + Source_3::iiwa::iiwa_link_2_visual: true + Source_3::iiwa::iiwa_link_3_visual: true + Source_3::iiwa::iiwa_link_4_visual: true + Source_3::iiwa::iiwa_link_5_visual: true + Source_3::iiwa::iiwa_link_6_visual: true + Source_3::iiwa::iiwa_link_7_visual: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /scene_markers + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: world + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 2.3739840984344482 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.07645688951015472 + Y: -0.3666839599609375 + Z: 0.10706909000873566 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.7403980493545532 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.5653908252716064 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 846 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd000000040000000000000156000002b4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000002b4000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000002b4000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000024400fffffffb0000000800540069006d006501000000000000045000000000000000000000023f000002b400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1200 + X: 67 + Y: 60 diff --git a/drake_ros_examples/examples/rs_flip_flop/CMakeLists.txt b/drake_ros_examples/examples/rs_flip_flop/CMakeLists.txt new file mode 100644 index 00000000..f6125a25 --- /dev/null +++ b/drake_ros_examples/examples/rs_flip_flop/CMakeLists.txt @@ -0,0 +1,18 @@ +find_package(geometry_msgs REQUIRED) + +add_executable(rs_flip_flop rs_flip_flop.cpp) +target_link_libraries(rs_flip_flop + drake::drake + drake_ros_systems::drake_ros_systems + ${std_msgs_TARGETS} +) + +install( + PROGRAMS rs_flip_flop.py + DESTINATION lib/${PROJECT_NAME} +) + +install( + TARGETS rs_flip_flop + DESTINATION lib/${PROJECT_NAME} +) diff --git a/drake_ros_examples/examples/rs_flip_flop/README.md b/drake_ros_examples/examples/rs_flip_flop/README.md new file mode 100644 index 00000000..c8b23083 --- /dev/null +++ b/drake_ros_examples/examples/rs_flip_flop/README.md @@ -0,0 +1,40 @@ +# RS flip flop + +## Overview + +Both `rs_flip_flop` and `rs_flip_flop.py` implement an [RS flip flop using NOR gates](https://en.wikipedia.org/wiki/Flip-flop_(electronics)#SR_NOR_latch). +They subscribe to the following topics: + +* `/R` +* `/S` + +And publish to the following topics + +* `/Q` +* `/Q_not` + +## How To + +Run either the C++ `rs_flip_flop` executable or the Python `rs_flip_flop.py` script as explained [here](../../README.md#running). + +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" +``` diff --git a/drake_ros_examples/examples/rs_flip_flop/rs_flip_flop.cpp b/drake_ros_examples/examples/rs_flip_flop/rs_flip_flop.cpp new file mode 100644 index 00000000..bbb2afb7 --- /dev/null +++ b/drake_ros_examples/examples/rs_flip_flop/rs_flip_flop.cpp @@ -0,0 +1,154 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#include +#include +#include + +#include +#include +#include +#include + +#include + +#include +#include + +using drake_ros_systems::DrakeRos; +using drake_ros_systems::RosInterfaceSystem; +using drake_ros_systems::RosPublisherSystem; +using drake_ros_systems::RosSubscriberSystem; + +class NorGate : public drake::systems::LeafSystem +{ +public: + NorGate() + { + DeclareAbstractInputPort("A", *drake::AbstractValue::Make(std_msgs::msg::Bool())); + DeclareAbstractInputPort("B", *drake::AbstractValue::Make(std_msgs::msg::Bool())); + + DeclareAbstractOutputPort("Q", &NorGate::calc_output_value); + } + + virtual ~NorGate() = default; + +private: + void + calc_output_value( + const drake::systems::Context & context, + std_msgs::msg::Bool * output) const + { + const bool a = GetInputPort("A").Eval(context).data; + const bool b = GetInputPort("B").Eval(context).data; + output->data = !(a || b); + } +}; + +// Delay's input port by one timestep to avoid algebraic loop error +// Inspired by Simulink's Memory block +template +class Memory : public drake::systems::LeafSystem +{ +public: + explicit Memory(const T & initial_value) + { + DeclareAbstractInputPort("value", *drake::AbstractValue::Make(T())); + + // State for value + DeclareAbstractState(drake::Value(initial_value)); + + // Output depends only on the previous state + DeclareAbstractOutputPort("value", &Memory::calc_output_value, {all_state_ticket()}); + + DeclarePerStepEvent( + drake::systems::UnrestrictedUpdateEvent( + [this]( + const drake::systems::Context & context, + const drake::systems::UnrestrictedUpdateEvent &, + drake::systems::State * state) { + // Copy input value to state + drake::systems::AbstractValues & abstract_state = state->get_mutable_abstract_state(); + abstract_state.get_mutable_value(0).SetFrom( + get_input_port().Eval(context)); + })); + } + + virtual ~Memory() = default; + +private: + void + calc_output_value(const drake::systems::Context & context, T * output) const + { + *output = context.get_abstract_state().get_value(0).get_value(); + } +}; + +int main() +{ + // NOR gate RS flip flop example + // Input topics /S and /R are active high (true is logic 1 and false is logic 0) + // Output topics /Q and /Q_not are active low (true is logic 0 and false is logic 1) + + // Input/Output table + // S: false R: false | Q: no change Q_not: no change + // S: true R: false | Q: false Q_not: true + // S: false R: true | Q: true Q_not: false + // S: true R: true | Q: invalid Q_not: invalid + drake::systems::DiagramBuilder builder; + + rclcpp::QoS qos{10}; + + auto sys_ros_interface = builder.AddSystem(std::make_unique()); + auto sys_pub_Q = builder.AddSystem( + RosPublisherSystem::Make( + "Q", qos, sys_ros_interface->get_ros_interface())); + auto sys_pub_Q_not = builder.AddSystem( + RosPublisherSystem::Make( + "Q_not", qos, sys_ros_interface->get_ros_interface())); + auto sys_sub_S = builder.AddSystem( + RosSubscriberSystem::Make( + "S", qos, sys_ros_interface->get_ros_interface())); + auto sys_sub_R = builder.AddSystem( + RosSubscriberSystem::Make( + "R", qos, sys_ros_interface->get_ros_interface())); + auto sys_nor_gate_1 = builder.AddSystem(); + auto sys_nor_gate_2 = builder.AddSystem(); + auto sys_memory = builder.AddSystem>(std_msgs::msg::Bool()); + + builder.Connect(sys_nor_gate_1->GetOutputPort("Q"), sys_memory->get_input_port(0)); + + builder.Connect(sys_sub_S->get_output_port(0), sys_nor_gate_1->GetInputPort("A")); + builder.Connect(sys_nor_gate_2->GetOutputPort("Q"), sys_nor_gate_1->GetInputPort("B")); + + builder.Connect(sys_memory->get_output_port(0), sys_nor_gate_2->GetInputPort("A")); + builder.Connect(sys_sub_R->get_output_port(0), sys_nor_gate_2->GetInputPort("B")); + + builder.Connect(sys_nor_gate_1->GetOutputPort("Q"), sys_pub_Q->get_input_port(0)); + builder.Connect(sys_nor_gate_2->GetOutputPort("Q"), sys_pub_Q_not->get_input_port(0)); + + auto diagram = builder.Build(); + auto context = diagram->CreateDefaultContext(); + + auto simulator = + std::make_unique>(*diagram, std::move(context)); + simulator->set_target_realtime_rate(1.0); + simulator->Initialize(); + + auto & simulator_context = simulator->get_mutable_context(); + + while (true) { + simulator->AdvanceTo(simulator_context.get_time() + 0.1); + } + return 0; +} diff --git a/drake_ros_examples/examples/rs_flip_flop/rs_flip_flop.py b/drake_ros_examples/examples/rs_flip_flop/rs_flip_flop.py new file mode 100755 index 00000000..d2e4e4ec --- /dev/null +++ b/drake_ros_examples/examples/rs_flip_flop/rs_flip_flop.py @@ -0,0 +1,143 @@ +# Copyright 2020 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from drake_ros_systems import RosInterfaceSystem +from drake_ros_systems import RosPublisherSystem +from drake_ros_systems import RosSubscriberSystem + +from pydrake.systems.analysis import Simulator +from pydrake.systems.framework import DiagramBuilder +from pydrake.systems.framework import UnrestrictedUpdateEvent +from pydrake.common.value import AbstractValue +from pydrake.systems.framework import LeafSystem + +from std_msgs.msg import Bool + +from rclpy.qos import QoSProfile + + +class NorGate(LeafSystem): + + def __init__(self): + super().__init__() + self._a = self.DeclareAbstractInputPort("A", AbstractValue.Make(Bool())) + self._b = self.DeclareAbstractInputPort("B", AbstractValue.Make(Bool())) + + self.DeclareAbstractOutputPort( + 'Q', + lambda: AbstractValue.Make(Bool()), + self._calc_output_value) + + def _calc_output_value(self, context, data): + a = self._a.Eval(context) + b = self._b.Eval(context) + data.get_mutable_value().data = not (a.data or b.data) + + +class Memory(LeafSystem): + """Delay input port by one time step to avoid algebraic loop error.""" + + def __init__(self, initial_value): + super().__init__() + + self._input = self.DeclareAbstractInputPort("A", AbstractValue.Make(initial_value)) + + self.DeclareAbstractState(AbstractValue.Make(initial_value)) + + self.DeclareAbstractOutputPort( + 'Q', + lambda: AbstractValue.Make(initial_value), + self._calc_output_value, + {self.all_state_ticket()}) + + self.DeclarePerStepEvent(UnrestrictedUpdateEvent(self._move_input_to_state)) + + def _move_input_to_state(self, context, event, state): + state.get_mutable_abstract_state().get_mutable_value(0).SetFrom( + AbstractValue.Make(self._input.Eval(context))) + + def _calc_output_value(self, context, output): + output.SetFrom(context.get_abstract_state().get_value(0)) + + +def main(): + # NOR gate RS flip flop example + # Input topics /S and /R are active high (true is logic 1 and false is logic 0) + # Output topics /Q and /Q_not are active low (true is logic 0 and false is logic 1) + + # Input/Output table + # S: false R: false | Q: no change Q_not: no change + # S: true R: false | Q: false Q_not: true + # S: false R: true | Q: true Q_not: false + # S: true R: true | Q: invalid Q_not: invalid + builder = DiagramBuilder() + + sys_ros_interface = builder.AddSystem(RosInterfaceSystem()) + + qos = QoSProfile(depth=10) + + sys_pub_Q = builder.AddSystem( + RosPublisherSystem(Bool, "Q", qos, sys_ros_interface.get_ros_interface())) + sys_pub_Q_not = builder.AddSystem( + RosPublisherSystem(Bool, "Q_not", qos, sys_ros_interface.get_ros_interface())) + + sys_sub_S = builder.AddSystem( + RosSubscriberSystem(Bool, "S", qos, sys_ros_interface.get_ros_interface())) + sys_sub_R = builder.AddSystem( + RosSubscriberSystem(Bool, "R", qos, sys_ros_interface.get_ros_interface())) + + sys_nor_gate_1 = builder.AddSystem(NorGate()) + sys_nor_gate_2 = builder.AddSystem(NorGate()) + + sys_memory = builder.AddSystem(Memory(Bool())) + + builder.Connect( + sys_nor_gate_1.GetOutputPort('Q'), + sys_memory.get_input_port(0) + ) + + builder.Connect( + sys_sub_S.get_output_port(0), + sys_nor_gate_1.GetInputPort('A'), + ) + builder.Connect( + sys_nor_gate_2.GetOutputPort('Q'), + sys_nor_gate_1.GetInputPort('B'), + ) + + builder.Connect( + sys_memory.get_output_port(0), + sys_nor_gate_2.GetInputPort('A'), + ) + builder.Connect( + sys_sub_R.get_output_port(0), + sys_nor_gate_2.GetInputPort('B'), + ) + + builder.Connect( + sys_nor_gate_1.GetOutputPort('Q'), + sys_pub_Q.get_input_port(0) + ) + builder.Connect( + sys_nor_gate_2.GetOutputPort('Q'), + sys_pub_Q_not.get_input_port(0) + ) + + diagram = builder.Build() + simulator = Simulator(diagram) + simulator_context = simulator.get_mutable_context() + simulator.set_target_realtime_rate(1.0) + + while True: + simulator.AdvanceTo(simulator_context.get_time() + 0.1) diff --git a/drake_ros_examples/package.xml b/drake_ros_examples/package.xml new file mode 100644 index 00000000..ae49bbb1 --- /dev/null +++ b/drake_ros_examples/package.xml @@ -0,0 +1,21 @@ + + + drake_ros_examples + 0.1.0 + Examples using Drake systems that interact with ROS 2. + Shane Loretz + Michel Hidalgo + Apache License 2.0 + + ament_cmake_ros + + drake_ros_systems + geometry_msgs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + From 66f4e1d190efcb411c3868149216d57fdf228102 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Mon, 3 May 2021 19:35:29 -0300 Subject: [PATCH 73/87] Add type conversion tests. Signed-off-by: Michel Hidalgo --- drake_ros_systems/CMakeLists.txt | 8 +++ .../test/test_type_conversion.cpp | 52 +++++++++++++++++++ 2 files changed, 60 insertions(+) create mode 100644 drake_ros_systems/test/test_type_conversion.cpp diff --git a/drake_ros_systems/CMakeLists.txt b/drake_ros_systems/CMakeLists.txt index 71a0e08e..fe4e3f4f 100644 --- a/drake_ros_systems/CMakeLists.txt +++ b/drake_ros_systems/CMakeLists.txt @@ -40,6 +40,7 @@ target_link_libraries(drake_ros_systems PUBLIC rosidl_runtime_c::rosidl_runtime_c rosidl_typesupport_cpp::rosidl_typesupport_cpp tf2_ros::tf2_ros + ${geometry_msgs_TARGETS} ${visualization_msgs_TARGETS} ) @@ -55,6 +56,7 @@ ament_export_libraries(drake_ros_systems) ament_export_targets(${PROJECT_NAME}) ament_export_dependencies(drake) +ament_export_dependencies(geometry_msgs) ament_export_dependencies(rclcpp) ament_export_dependencies(rosidl_generator_c) ament_export_dependencies(tf2_ros) @@ -139,6 +141,12 @@ if(BUILD_TESTING) drake_ros_systems ${visualization_msgs_TARGETS} ) + + ament_add_gtest(test_type_conversion test/test_type_conversion.cpp) + target_link_libraries(test_type_conversion + drake::drake + drake_ros_systems + ) endif() ament_package() diff --git a/drake_ros_systems/test/test_type_conversion.cpp b/drake_ros_systems/test/test_type_conversion.cpp new file mode 100644 index 00000000..7428d7cf --- /dev/null +++ b/drake_ros_systems/test/test_type_conversion.cpp @@ -0,0 +1,52 @@ +// Copyright 2021 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include + +#include "drake_ros_systems/utilities/type_conversion.hpp" + + +TEST(TypeConversion, ToPoseMsg) +{ + const drake::Vector3 position{1., 2., 3.}; + const drake::Quaternion orientation{0., 0., sin(M_PI / 4.), cos(M_PI / 4.)}; + const drake::math::RigidTransform pose{orientation, position}; + const geometry_msgs::msg::Pose message = + drake_ros_systems::utilities::ToPoseMsg(pose); + EXPECT_DOUBLE_EQ(message.position.x, position.x()); + EXPECT_DOUBLE_EQ(message.position.y, position.y()); + EXPECT_DOUBLE_EQ(message.position.z, position.z()); + EXPECT_DOUBLE_EQ(message.orientation.x, orientation.x()); + EXPECT_DOUBLE_EQ(message.orientation.y, orientation.y()); + EXPECT_DOUBLE_EQ(message.orientation.z, orientation.z()); + EXPECT_DOUBLE_EQ(message.orientation.w, orientation.w()); +} + +TEST(TypeConversion, ToTransformMsg) +{ + const drake::Vector3 translation{3., 2., 1.}; + const drake::Quaternion rotation{0., 0., sin(-M_PI / 4.), cos(-M_PI / 4.)}; + const drake::math::RigidTransform transform{rotation, translation}; + const geometry_msgs::msg::Transform message = + drake_ros_systems::utilities::ToTransformMsg(transform); + EXPECT_DOUBLE_EQ(message.translation.x, translation.x()); + EXPECT_DOUBLE_EQ(message.translation.y, translation.y()); + EXPECT_DOUBLE_EQ(message.translation.z, translation.z()); + EXPECT_DOUBLE_EQ(message.rotation.x, rotation.x()); + EXPECT_DOUBLE_EQ(message.rotation.y, rotation.y()); + EXPECT_DOUBLE_EQ(message.rotation.z, rotation.z()); + EXPECT_DOUBLE_EQ(message.rotation.w, rotation.w()); +} From 4bd2953c54ed9b7715493e7ddadf3c3c5f6366d3 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Mon, 3 May 2021 19:43:05 -0300 Subject: [PATCH 74/87] Tweak drake_ros_examples README.md Signed-off-by: Michel Hidalgo --- drake_ros_examples/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/drake_ros_examples/README.md b/drake_ros_examples/README.md index 6f6e557f..a7aa33f2 100644 --- a/drake_ros_examples/README.md +++ b/drake_ros_examples/README.md @@ -57,7 +57,7 @@ export PYTHONPATH=$(pwd)/drake_ros_systems/install/lib/$(python -c 'import sys; export PYTHONPATH=$(pwd)/drake_ros_examples/install/lib/$(python -c 'import sys; print(f"python{sys.version_info[0]}.{sys.version_info[1]}")')/site-packages:$PYTHONPATH ``` - Now you can run C++ and Python examples from the install folder using `./install/lib/drake_ros_examples/`. + Now you can run C++ and Python examples from the install folder using `./drake-ros/drake_ros_examples/install/lib/drake_ros_examples/`. ## List of examples From 67ed30b1cc865ee72567ab477ec89bcf11bf6040 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Tue, 4 May 2021 13:49:30 -0300 Subject: [PATCH 75/87] Add sinusoidal motion to iiwa_manipulator demos Signed-off-by: Michel Hidalgo --- .../iiwa_manipulator/iiwa_manipulator.cpp | 53 ++++++++++++++++--- .../iiwa_manipulator/iiwa_manipulator.py | 36 +++++++++++-- 2 files changed, 80 insertions(+), 9 deletions(-) diff --git a/drake_ros_examples/examples/iiwa_manipulator/iiwa_manipulator.cpp b/drake_ros_examples/examples/iiwa_manipulator/iiwa_manipulator.cpp index d6bf41cf..7e5da5e4 100644 --- a/drake_ros_examples/examples/iiwa_manipulator/iiwa_manipulator.cpp +++ b/drake_ros_examples/examples/iiwa_manipulator/iiwa_manipulator.cpp @@ -12,8 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include #include +#include +#include +#include + #include @@ -21,6 +26,7 @@ #include #include +#include #include #include @@ -28,6 +34,10 @@ using drake_ros_systems::DrakeRos; using drake_ros_systems::RosInterfaceSystem; using drake_ros_systems::RvizVisualizer; +using drake::systems::Adder; +using drake::systems::ConstantVectorSource; +using drake::systems::Sine; +using drake::systems::Simulator; using drake::examples::manipulation_station::ManipulationStation; @@ -42,6 +52,32 @@ int main() manipulation_station->SetupClutterClearingStation(); manipulation_station->Finalize(); + // Make the base joint swing sinusoidally. + auto constant_term = builder.AddSystem( + drake::VectorX::Zero(manipulation_station->num_iiwa_joints())); + + drake::VectorX amplitudes = + drake::VectorX::Zero(manipulation_station->num_iiwa_joints()); + amplitudes[0] = M_PI / 4.; // == 45 degrees + const drake::VectorX frequencies = + drake::VectorX::Constant(manipulation_station->num_iiwa_joints(), 1.); // Hz + const drake::VectorX phases = + drake::VectorX::Zero(manipulation_station->num_iiwa_joints()); + auto variable_term = builder.AddSystem(amplitudes, frequencies, phases); + + auto joint_trajectory_generator = + builder.AddSystem(2, manipulation_station->num_iiwa_joints()); + + builder.Connect( + constant_term->get_output_port(), + joint_trajectory_generator->get_input_port(0)); + builder.Connect( + variable_term->get_output_port(0), + joint_trajectory_generator->get_input_port(1)); + builder.Connect( + joint_trajectory_generator->get_output_port(), + manipulation_station->GetInputPort("iiwa_position")); + auto rviz_visualizer = builder.AddSystem( ros_interface_system->get_ros_interface()); @@ -52,8 +88,7 @@ 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(); @@ -62,14 +97,20 @@ int main() auto & manipulation_station_context = diagram->GetMutableSubsystemContext(*manipulation_station, &simulator_context); - manipulation_station->GetInputPort("iiwa_position") - .FixValue( - &manipulation_station_context, - manipulation_station->GetIiwaPosition(manipulation_station_context)); + auto & constant_term_context = + diagram->GetMutableSubsystemContext(*constant_term, &simulator_context); + // Fix gripper joints' position. manipulation_station->GetInputPort("wsg_position") .FixValue(&manipulation_station_context, 0.); + // Use default positions for every joint but the base joint. + drake::systems::BasicVector & constants = + constant_term->get_mutable_source_value(&constant_term_context); + constants.set_value( + manipulation_station->GetIiwaPosition(manipulation_station_context)); + constants.get_mutable_value()[0] = -M_PI / 4.; + while (true) { simulator->AdvanceTo(simulator_context.get_time() + 0.1); } diff --git a/drake_ros_examples/examples/iiwa_manipulator/iiwa_manipulator.py b/drake_ros_examples/examples/iiwa_manipulator/iiwa_manipulator.py index 57f5e88e..e3e57956 100755 --- a/drake_ros_examples/examples/iiwa_manipulator/iiwa_manipulator.py +++ b/drake_ros_examples/examples/iiwa_manipulator/iiwa_manipulator.py @@ -21,6 +21,9 @@ from pydrake.examples.manipulation_station import ManipulationStation from pydrake.systems.analysis import Simulator from pydrake.systems.framework import DiagramBuilder +from pydrake.systems.primitives import Adder +from pydrake.systems.primitives import ConstantVectorSource +from pydrake.systems.primitives import Sine def main(): @@ -32,6 +35,26 @@ def main(): manipulation_station.SetupClutterClearingStation() manipulation_station.Finalize() + # Make the base joint swing sinusoidally. + constant_term = builder.AddSystem(ConstantVectorSource( + np.zeros(manipulation_station.num_iiwa_joints()))) + + amplitudes = np.zeros(manipulation_station.num_iiwa_joints()) + amplitudes[0] = np.pi / 4. # == 45 degrees + frequencies = np.ones(manipulation_station.num_iiwa_joints()) + phases = np.zeros(manipulation_station.num_iiwa_joints()) + variable_term = builder.AddSystem(Sine(amplitudes, frequencies, phases)) + + joint_trajectory_generator = builder.AddSystem( + Adder(2, manipulation_station.num_iiwa_joints())) + + builder.Connect(constant_term.get_output_port(), + joint_trajectory_generator.get_input_port(0)) + builder.Connect(variable_term.get_output_port(0), + joint_trajectory_generator.get_input_port(1)) + builder.Connect(joint_trajectory_generator.get_output_port(), + manipulation_station.GetInputPort('iiwa_position')) + rviz_visualizer = builder.AddSystem( RvizVisualizer(ros_interface_system.get_ros_interface())) @@ -49,12 +72,19 @@ def main(): manipulation_station_context = \ diagram.GetMutableSubsystemContext(manipulation_station, context) - manipulation_station.GetInputPort('iiwa_position').FixValue( - manipulation_station_context, - manipulation_station.GetIiwaPosition(manipulation_station_context)) + constant_term_context = \ + diagram.GetMutableSubsystemContext(constant_term, context) + + # Fix gripper joints' position. manipulation_station.GetInputPort('wsg_position').FixValue( manipulation_station_context, np.zeros(1)) + # Use default positions for every joint but the base joint. + constants = constant_term.get_mutable_source_value(constant_term_context) + constants.set_value( + manipulation_station.GetIiwaPosition(manipulation_station_context)) + constants.get_mutable_value()[0] = -np.pi / 4. + try: while True: simulator.AdvanceTo(context.get_time() + 0.1) From 6e6a4e9d0f1254c33f894bacecc2ed3d2cd525ec Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Fri, 7 May 2021 13:17:06 -0300 Subject: [PATCH 76/87] Update maintainers in package.xml Signed-off-by: Michel Hidalgo --- drake_ros_examples/package.xml | 1 + drake_ros_systems/package.xml | 2 ++ 2 files changed, 3 insertions(+) diff --git a/drake_ros_examples/package.xml b/drake_ros_examples/package.xml index ae49bbb1..045eabeb 100644 --- a/drake_ros_examples/package.xml +++ b/drake_ros_examples/package.xml @@ -3,6 +3,7 @@ drake_ros_examples 0.1.0 Examples using Drake systems that interact with ROS 2. + Ian McMahon Shane Loretz Michel Hidalgo Apache License 2.0 diff --git a/drake_ros_systems/package.xml b/drake_ros_systems/package.xml index 9b6dc9d5..97683dd4 100644 --- a/drake_ros_systems/package.xml +++ b/drake_ros_systems/package.xml @@ -3,7 +3,9 @@ drake_ros_systems 0.1.0 Drake systems for interacting with ROS 2. + Ian McMahon Shane Loretz + Michel Hidalgo Apache License 2.0 ament_cmake_ros From 5e711c2e9808eeb9cd18098ba09759e4b3b7b014 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Tue, 11 May 2021 15:04:08 -0300 Subject: [PATCH 77/87] Split TfBroadcasterSystem - Split conversion logic into a SceneTfSystem. - Use a RosPublisherSystem with proper QoS settings. Signed-off-by: Michel Hidalgo --- drake_ros_systems/CMakeLists.txt | 1 + .../drake_ros_systems/scene_tf_system.hpp | 62 ++++++++++ .../tf_broadcaster_system.hpp | 22 +--- .../module_drake_ros_systems.cpp | 4 +- drake_ros_systems/src/rviz_visualizer.cpp | 2 +- drake_ros_systems/src/scene_tf_system.cpp | 91 ++++++++++++++ .../src/tf_broadcaster_system.cpp | 116 +++--------------- .../test/test_tf_broadcaster.cpp | 2 +- 8 files changed, 183 insertions(+), 117 deletions(-) create mode 100644 drake_ros_systems/include/drake_ros_systems/scene_tf_system.hpp create mode 100644 drake_ros_systems/src/scene_tf_system.cpp diff --git a/drake_ros_systems/CMakeLists.txt b/drake_ros_systems/CMakeLists.txt index fe4e3f4f..888e4ef2 100644 --- a/drake_ros_systems/CMakeLists.txt +++ b/drake_ros_systems/CMakeLists.txt @@ -29,6 +29,7 @@ add_library(drake_ros_systems src/ros_subscriber_system.cpp src/rviz_visualizer.cpp src/scene_markers_system.cpp + src/scene_tf_system.cpp src/subscription.cpp src/tf_broadcaster_system.cpp src/utilities/type_conversion.cpp diff --git a/drake_ros_systems/include/drake_ros_systems/scene_tf_system.hpp b/drake_ros_systems/include/drake_ros_systems/scene_tf_system.hpp new file mode 100644 index 00000000..676f2995 --- /dev/null +++ b/drake_ros_systems/include/drake_ros_systems/scene_tf_system.hpp @@ -0,0 +1,62 @@ +// Copyright 2021 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#ifndef DRAKE_ROS_SYSTEMS__SCENE_TF_SYSTEM_HPP_ +#define DRAKE_ROS_SYSTEMS__SCENE_TF_SYSTEM_HPP_ + +#include +#include + +#include + +#include + + +namespace drake_ros_systems +{ +/// System for SceneGraph frame transforms aggregation as a ROS tf2 message. +/// +/// This system outputs a `tf2_msgs/msg/TFMessage` populated with the +/// rigid transforms from the world frame to each other frame in the scene, +/// using Context time to timestamp each `geometry_msgs/msg/TransformStamped` +/// message. +/// +/// It has one input port: +/// - *graph_query* (abstract): expects a QueryObject from the SceneGraph. +/// +/// It has one output port: +/// - *scene_tf* (abstract): rigid transforms w.r.t. the world frame for all +/// frames in the scene, as a tf2_msgs::msg::TFMessage message. +class SceneTfSystem : public drake::systems::LeafSystem +{ +public: + SceneTfSystem(); + virtual ~SceneTfSystem(); + + const drake::systems::InputPort & get_graph_query_port() const; + + const drake::systems::OutputPort & get_scene_tf_output_port() const; + +private: + void + CalcSceneTf( + const drake::systems::Context & context, + tf2_msgs::msg::TFMessage * output_value) const; + + // PIMPL forward declaration + class SceneTfSystemPrivate; + + std::unique_ptr impl_; +}; +} // namespace drake_ros_systems +#endif // DRAKE_ROS_SYSTEMS__SCENE_TF_SYSTEM_HPP_ diff --git a/drake_ros_systems/include/drake_ros_systems/tf_broadcaster_system.hpp b/drake_ros_systems/include/drake_ros_systems/tf_broadcaster_system.hpp index 333e76ba..b430aca4 100644 --- a/drake_ros_systems/include/drake_ros_systems/tf_broadcaster_system.hpp +++ b/drake_ros_systems/include/drake_ros_systems/tf_broadcaster_system.hpp @@ -26,32 +26,22 @@ namespace drake_ros_systems { /// System for tf2 transform broadcasting. /// -/// This system publishes all frame transforms found in a SceneGraph -/// to the `/tf` ROS topic, using Context time to timestamp -/// `geometry_msgs/msg/TransformStamped` messages. +/// This system is a subdiagram aggregating a SceneTFSystem and a +/// RosPublisherSystem to broadcast SceneGraph frame transforms. +/// Messages are published to the `/tf` ROS topic. /// -/// It has one input port: +/// It exports one input port: /// - *graph_query* (abstract): expects a QueryObject from the SceneGraph. -class TfBroadcasterSystem : public drake::systems::LeafSystem +class TfBroadcasterSystem : public drake::systems::Diagram { public: TfBroadcasterSystem( - DrakeRosInterface * ros_interface, + std::shared_ptr ros_interface, const std::unordered_set & publish_triggers = { drake::systems::TriggerType::kPerStep, drake::systems::TriggerType::kForced}, double publish_period = 0.0); - virtual ~TfBroadcasterSystem(); const drake::systems::InputPort & get_graph_query_port() const; - -private: - drake::systems::EventStatus - PublishFrames(const drake::systems::Context & context) const; - - // PIMPL forward declaration - class TfBroadcasterSystemPrivate; - - std::unique_ptr impl_; }; } // namespace drake_ros_systems #endif // DRAKE_ROS_SYSTEMS__TF_BROADCASTER_SYSTEM_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 122d816f..9393a6c4 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 @@ -116,9 +116,9 @@ PYBIND11_MODULE(drake_ros_systems, m) { ros_interface); })); - py::class_>(m, "TfBroadcasterSystem") + py::class_>(m, "TfBroadcasterSystem") .def( - py::init, double>(), + py::init, std::unordered_set, double>(), py::arg("ros_interface"), py::arg("publish_triggers") = std::unordered_set{TriggerType::kPerStep, TriggerType::kForced}, diff --git a/drake_ros_systems/src/rviz_visualizer.cpp b/drake_ros_systems/src/rviz_visualizer.cpp index 8c2615ae..f92e9ba3 100644 --- a/drake_ros_systems/src/rviz_visualizer.cpp +++ b/drake_ros_systems/src/rviz_visualizer.cpp @@ -51,7 +51,7 @@ RvizVisualizer::RvizVisualizer( if (publish_tf) { auto tf_broadcaster = builder.AddSystem( - ros_interface.get(), publish_triggers, publish_period); + ros_interface, publish_triggers, publish_period); builder.ConnectInput("graph_query", tf_broadcaster->get_graph_query_port()); } diff --git a/drake_ros_systems/src/scene_tf_system.cpp b/drake_ros_systems/src/scene_tf_system.cpp new file mode 100644 index 00000000..f1fb5fa4 --- /dev/null +++ b/drake_ros_systems/src/scene_tf_system.cpp @@ -0,0 +1,91 @@ +// Copyright 2021 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include +#include + +#include "drake_ros_systems/scene_tf_system.hpp" +#include "drake_ros_systems/utilities/type_conversion.hpp" + + +namespace drake_ros_systems +{ +class SceneTfSystem::SceneTfSystemPrivate +{ +public: + drake::systems::InputPortIndex graph_query_port_index; + drake::systems::OutputPortIndex scene_tf_port_index; +}; + +SceneTfSystem::SceneTfSystem() +: impl_(new SceneTfSystemPrivate()) +{ + impl_->graph_query_port_index = + this->DeclareAbstractInputPort( + "graph_query", drake::Value>{}).get_index(); + + impl_->scene_tf_port_index = + this->DeclareAbstractOutputPort( + "scene_tf", &SceneTfSystem::CalcSceneTf).get_index(); +} + +SceneTfSystem::~SceneTfSystem() +{ +} + +const drake::systems::InputPort & +SceneTfSystem::get_graph_query_port() const +{ + return get_input_port(impl_->graph_query_port_index); +} + +const drake::systems::OutputPort & +SceneTfSystem::get_scene_tf_output_port() const +{ + return get_output_port(impl_->scene_tf_port_index); +} + +void +SceneTfSystem::CalcSceneTf( + const drake::systems::Context & context, + tf2_msgs::msg::TFMessage * output_value) const +{ + const drake::geometry::QueryObject & query_object = + get_graph_query_port().Eval>(context); + const drake::geometry::SceneGraphInspector & inspector = query_object.inspector(); + // TODO(hidmic): publish frame transforms w.r.t. to their parent frame + // instead of the world frame when an API is made available. + if (inspector.num_frames() > 1) { + output_value->transforms.reserve(inspector.num_frames() - 1); + + geometry_msgs::msg::TransformStamped transform; + transform.header.stamp = + rclcpp::Time() + rclcpp::Duration::from_seconds(context.get_time()); + transform.header.frame_id = inspector.GetName(inspector.world_frame_id()); + for (const drake::geometry::FrameId & frame_id : inspector.all_frame_ids()) { + if (frame_id == inspector.world_frame_id()) { + continue; + } + transform.child_frame_id = inspector.GetName(frame_id); + transform.transform = + utilities::ToTransformMsg(query_object.GetPoseInWorld(frame_id)); + output_value->transforms.push_back(transform); + } + } +} + +} // namespace drake_ros_systems diff --git a/drake_ros_systems/src/tf_broadcaster_system.cpp b/drake_ros_systems/src/tf_broadcaster_system.cpp index 7b4936ab..638198c7 100644 --- a/drake_ros_systems/src/tf_broadcaster_system.cpp +++ b/drake_ros_systems/src/tf_broadcaster_system.cpp @@ -12,128 +12,50 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include -#include +#include -#include -#include +#include +#include #include -#include #include -#include -#include +#include "drake_ros_systems/drake_ros_interface.hpp" +#include "drake_ros_systems/ros_publisher_system.hpp" +#include "drake_ros_systems/scene_tf_system.hpp" #include "drake_ros_systems/tf_broadcaster_system.hpp" -#include "drake_ros_systems/utilities/type_conversion.hpp" namespace drake_ros_systems { -class TfBroadcasterSystem::TfBroadcasterSystemPrivate -{ -public: - std::unique_ptr tf_broadcaster_; - drake::systems::InputPortIndex graph_query_port_index; -}; TfBroadcasterSystem::TfBroadcasterSystem( - DrakeRosInterface * ros, + std::shared_ptr ros_interface, const std::unordered_set & publish_triggers, double publish_period) -: impl_(new TfBroadcasterSystemPrivate()) { - impl_->tf_broadcaster_ = ros->create_tf_broadcaster(); - - impl_->graph_query_port_index = - this->DeclareAbstractInputPort( - "graph_query", drake::Value>{}).get_index(); + drake::systems::DiagramBuilder builder; - // vvv Mostly copied from LcmPublisherSystem vvv - using TriggerType = drake::systems::TriggerType; - // Check that publish_triggers does not contain an unsupported trigger. - for (const auto & trigger : publish_triggers) { - if ( - (trigger != TriggerType::kForced) && - (trigger != TriggerType::kPeriodic) && - (trigger != TriggerType::kPerStep)) - { - throw std::invalid_argument( - "Only kForced, kPeriodic, or kPerStep are supported"); - } - } + auto scene_tf = builder.AddSystem(); - // 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(TriggerType::kForced) != publish_triggers.end()) { - this->DeclareForcedPublishEvent(&TfBroadcasterSystem::PublishFrames); - } + auto scene_tf_publisher = builder.AddSystem( + RosPublisherSystem::Make( + "/tf", tf2_ros::DynamicBroadcasterQoS(), + ros_interface, publish_triggers, publish_period)); - if (publish_triggers.find(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, - &TfBroadcasterSystem::PublishFrames); - } 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"); - } + builder.Connect( + scene_tf->get_scene_tf_output_port(), + scene_tf_publisher->get_input_port()); - if (publish_triggers.find(TriggerType::kPerStep) != publish_triggers.end()) { - DeclarePerStepEvent( - drake::systems::PublishEvent( - [this]( - const drake::systems::Context & context, - const drake::systems::PublishEvent &) { - PublishFrames(context); - })); - } - // ^^^ Mostly copied from LcmPublisherSystem ^^^ -} + builder.ExportInput(scene_tf->get_graph_query_port(), "graph_query"); -TfBroadcasterSystem::~TfBroadcasterSystem() -{ + builder.BuildInto(this); } const drake::systems::InputPort & TfBroadcasterSystem::get_graph_query_port() const { - return get_input_port(impl_->graph_query_port_index); -} - -drake::systems::EventStatus -TfBroadcasterSystem::PublishFrames( - const drake::systems::Context & context) const -{ - const drake::geometry::QueryObject & query_object = - get_graph_query_port().Eval>(context); - const drake::geometry::SceneGraphInspector & inspector = query_object.inspector(); - // TODO(hidmic): publish frame transforms w.r.t. to their parent frame - // instead of the world frame when an API is made available. - if (inspector.num_frames() > 1) { - std::vector transforms; - transforms.reserve(inspector.num_frames() - 1); - geometry_msgs::msg::TransformStamped transform; - transform.header.stamp = - rclcpp::Time() + rclcpp::Duration::from_seconds(context.get_time()); - transform.header.frame_id = inspector.GetName(inspector.world_frame_id()); - for (const drake::geometry::FrameId & frame_id : inspector.all_frame_ids()) { - if (frame_id == inspector.world_frame_id()) { - continue; - } - transform.child_frame_id = inspector.GetName(frame_id); - transform.transform = - utilities::ToTransformMsg(query_object.GetPoseInWorld(frame_id)); - transforms.push_back(transform); - } - impl_->tf_broadcaster_->sendTransform(transforms); - } - return drake::systems::EventStatus::Succeeded(); + return get_input_port(); } } // namespace drake_ros_systems diff --git a/drake_ros_systems/test/test_tf_broadcaster.cpp b/drake_ros_systems/test/test_tf_broadcaster.cpp index 64a92bd8..549d6bc0 100644 --- a/drake_ros_systems/test/test_tf_broadcaster.cpp +++ b/drake_ros_systems/test/test_tf_broadcaster.cpp @@ -78,7 +78,7 @@ TEST(TfBroadcasting, nominal_case) { const std::unordered_set publish_triggers{drake::systems::TriggerType::kForced}; auto broadcaster = builder.AddSystem( - sys_ros_interface->get_ros_interface().get(), publish_triggers); + sys_ros_interface->get_ros_interface(), publish_triggers); builder.Connect( scene_graph->get_query_output_port(), From c6005c69d5d44f7fb374543c172f1567b7ce7dbd Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Tue, 11 May 2021 15:14:19 -0300 Subject: [PATCH 78/87] Drop unnecessary DrakeRosInterface APIs. Signed-off-by: Michel Hidalgo --- .../include/drake_ros_systems/drake_ros.hpp | 6 ------ .../drake_ros_systems/drake_ros_interface.hpp | 8 -------- drake_ros_systems/src/drake_ros.cpp | 13 ------------- 3 files changed, 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 64e68fb6..fdbfcec6 100644 --- a/drake_ros_systems/include/drake_ros_systems/drake_ros.hpp +++ b/drake_ros_systems/include/drake_ros_systems/drake_ros.hpp @@ -60,12 +60,6 @@ class DrakeRos final : public DrakeRosInterface const rclcpp::QoS & qos, std::function)> callback) final; - std::unique_ptr - create_tf_broadcaster() final; - - std::shared_ptr - get_clock() final; - void spin( int timeout_millis) final; 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 07439312..9ceeabf8 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 @@ -49,14 +49,6 @@ class DrakeRosInterface const rclcpp::QoS & qos, std::function)> callback) = 0; - virtual - std::unique_ptr - create_tf_broadcaster() = 0; - - virtual - std::shared_ptr - get_clock() = 0; - virtual void spin( diff --git a/drake_ros_systems/src/drake_ros.cpp b/drake_ros_systems/src/drake_ros.cpp index 8234076d..ca0b374f 100644 --- a/drake_ros_systems/src/drake_ros.cpp +++ b/drake_ros_systems/src/drake_ros.cpp @@ -94,19 +94,6 @@ DrakeRos::create_subscription( return sub; } -std::unique_ptr -DrakeRos::create_tf_broadcaster() -{ - return std::make_unique(*impl_->node_); -} - -std::shared_ptr -DrakeRos::get_clock() -{ - return impl_->node_->get_clock(); -} - - void DrakeRos::spin( int timeout_millis) From 123259cb9349c1221a13ee3675510553cd4e3eb3 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Tue, 11 May 2021 15:14:36 -0300 Subject: [PATCH 79/87] Update SceneMarkersSystem documentation. Signed-off-by: Michel Hidalgo --- .../include/drake_ros_systems/scene_markers_system.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/drake_ros_systems/include/drake_ros_systems/scene_markers_system.hpp b/drake_ros_systems/include/drake_ros_systems/scene_markers_system.hpp index 52a39abf..886b7789 100644 --- a/drake_ros_systems/include/drake_ros_systems/scene_markers_system.hpp +++ b/drake_ros_systems/include/drake_ros_systems/scene_markers_system.hpp @@ -28,9 +28,9 @@ namespace drake_ros_systems /// System for SceneGraph depiction as a ROS markers array. /// -/// This system outputs a `visualization_msg/msg/MarkerArray` populated with +/// This system outputs a `visualization_msgs/msg/MarkerArray` populated with /// all geometries found in a SceneGraph, using Context time to timestamp -/// `geometry_msgs/msg/TransformStamped` messages. +/// each `visualization_msgs/msg/Marker` message. /// /// It has one input port: /// - *graph_query* (abstract): expects a QueryObject from the SceneGraph. From 4fea1a4379226ffdbea7c8064df133b35d7a74b8 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Mon, 17 May 2021 16:28:46 -0300 Subject: [PATCH 80/87] Use source name as scene marker namespace. Signed-off-by: Michel Hidalgo --- .../src/scene_markers_system.cpp | 23 ++++++++++++++----- 1 file changed, 17 insertions(+), 6 deletions(-) diff --git a/drake_ros_systems/src/scene_markers_system.cpp b/drake_ros_systems/src/scene_markers_system.cpp index ad5680bd..e508e86f 100644 --- a/drake_ros_systems/src/scene_markers_system.cpp +++ b/drake_ros_systems/src/scene_markers_system.cpp @@ -65,9 +65,8 @@ class SceneGeometryToMarkers : public drake::geometry::ShapeReifier prototype_marker_.header.frame_id = inspector.GetName(inspector.GetFrameId(geometry_id)); - prototype_marker_.ns = inspector.GetOwningSourceName(geometry_id) + - "::" + inspector.GetName(geometry_id); - prototype_marker_.id = 0; + prototype_marker_.ns = inspector.GetOwningSourceName(geometry_id); + prototype_marker_.id = marker_array_->markers.size(); prototype_marker_.action = visualization_msgs::msg::Marker::MODIFY; prototype_marker_.lifetime = rclcpp::Duration::from_nanoseconds(0); @@ -173,7 +172,7 @@ class SceneGeometryToMarkers : public drake::geometry::ShapeReifier marker_array_->markers.push_back(body_marker); visualization_msgs::msg::Marker upper_cap_marker = prototype_marker_; - upper_cap_marker.id = 1; + upper_cap_marker.id = marker_array_->markers.size(); upper_cap_marker.type = visualization_msgs::msg::Marker::SPHERE; upper_cap_marker.scale.x = capsule.radius(); upper_cap_marker.scale.y = capsule.radius(); @@ -186,7 +185,7 @@ class SceneGeometryToMarkers : public drake::geometry::ShapeReifier marker_array_->markers.push_back(upper_cap_marker); visualization_msgs::msg::Marker lower_cap_marker = upper_cap_marker; - lower_cap_marker.id = 2; + lower_cap_marker.id = marker_array_->markers.size(); const drake::math::RigidTransform X_GL{ drake::Vector3{0., 0., -capsule.length() / 2.}}; const drake::math::RigidTransform X_FL = X_FG_ * X_GL; @@ -305,6 +304,17 @@ SceneMarkersSystem::EvalSceneMarkers( .Eval(context); } +namespace { + +visualization_msgs::msg::Marker MakeDeleteAllMarker() +{ + visualization_msgs::msg::Marker marker; + marker.action = visualization_msgs::msg::Marker::DELETEALL; + return marker; +} + +} // namespace + void SceneMarkersSystem::CalcSceneMarkers( const drake::systems::Context & context, @@ -314,7 +324,8 @@ SceneMarkersSystem::CalcSceneMarkers( get_input_port(impl_->graph_query_port_index) .Eval>(context); const drake::geometry::SceneGraphInspector & inspector = query_object.inspector(); - output_value->markers.reserve(inspector.NumGeometriesWithRole(impl_->role)); + output_value->markers.reserve(inspector.NumGeometriesWithRole(impl_->role) + 1); + output_value->markers.push_back(MakeDeleteAllMarker()); // always delete all first for (const drake::geometry::FrameId & frame_id : inspector.all_frame_ids()) { for (const drake::geometry::GeometryId & geometry_id : inspector.GetGeometries(frame_id, impl_->role)) From b365190165eee7d24751b273fe432b9d96609b22 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Wed, 19 May 2021 09:28:44 -0300 Subject: [PATCH 81/87] Add name conventions based on SceneGraph and MultibodyPlant metadata Signed-off-by: Michel Hidalgo --- .../utilities/name_conventions.hpp | 50 ++++++++ .../src/utilities/name_conventions.cpp | 121 ++++++++++++++++++ 2 files changed, 171 insertions(+) create mode 100644 drake_ros_systems/include/drake_ros_systems/utilities/name_conventions.hpp create mode 100644 drake_ros_systems/src/utilities/name_conventions.cpp diff --git a/drake_ros_systems/include/drake_ros_systems/utilities/name_conventions.hpp b/drake_ros_systems/include/drake_ros_systems/utilities/name_conventions.hpp new file mode 100644 index 00000000..92e3a9bd --- /dev/null +++ b/drake_ros_systems/include/drake_ros_systems/utilities/name_conventions.hpp @@ -0,0 +1,50 @@ +// Copyright 2021 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#ifndef DRAKE_ROS_SYSTEMS__UTILITIES__NAME_CONVENTIONS_HPP_ +#define DRAKE_ROS_SYSTEMS__UTILITIES__NAME_CONVENTIONS_HPP_ + +#include +#include + +#include +#include + + +namespace drake_ros_systems +{ +namespace utilities +{ + +std::string +GetTfFrameName( + const drake::geometry::SceneGraphInspector & inspector, + const std::unordered_set *> & plants, + const drake::geometry::FrameId & frame_id); + +std::string +GetTfFrameName( + const drake::geometry::SceneGraphInspector & inspector, + const std::unordered_set *> & plants, + const drake::geometry::GeometryId & geometry_id); + +std::string +GetMarkerNamespacePrefix( + const drake::geometry::SceneGraphInspector & inspector, + const std::unordered_set *> & plants, + const drake::geometry::GeometryId & geometry_id); + +} // namespace utilities +} // namespace drake_ros_systems + +#endif // DRAKE_ROS_SYSTEMS__UTILITIES__NAME_CONVENTIONS_HPP_ diff --git a/drake_ros_systems/src/utilities/name_conventions.cpp b/drake_ros_systems/src/utilities/name_conventions.cpp new file mode 100644 index 00000000..fde71e99 --- /dev/null +++ b/drake_ros_systems/src/utilities/name_conventions.cpp @@ -0,0 +1,121 @@ +// Copyright 2021 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include + +#include "drake_ros_systems/utilities/name_conventions.hpp" + + +namespace drake_ros_systems +{ +namespace utilities +{ + +namespace +{ + +std::string +ReplaceAllOccurrences( + std::string string, + const std::string & target, + const std::string & replacement) +{ + std::string::size_type n = 0; + while ((n = string.find(target, n)) != std::string::npos) { + string.replace(n, target.size(), replacement); + n += replacement.size(); + } + return string; +} + +} // namespace + +std::string +GetTfFrameName( + const drake::geometry::SceneGraphInspector & inspector, + const std::unordered_set *> & plants, + const drake::geometry::FrameId & frame_id) +{ + std::stringstream ss; + ss << "/"; + for (auto * plant : plants) { + const drake::multibody::Body * body = + plant->GetBodyFromFrameId(frame_id); + if (!body) { + continue; + } + const std::string & model_instance_name = + plant->GetModelInstanceName(body->model_instance()); + ss << model_instance_name << "/"; + const std::string & body_name = body->name(); + if (body_name.empty()) { + ss << "unnamed_body_" << body->index(); + } else { + ss << ReplaceAllOccurrences(body_name, "::", "/"); + } + return ss.str(); + } + const std::string & frame_name = inspector.GetName(frame_id); + if (frame_name.empty()) { + ss << "unnamed_frame_" << frame_id; + } else { + ss << ReplaceAllOccurrences(frame_name, "::", "/"); + } + return ss.str(); +} + +std::string +GetTfFrameName( + const drake::geometry::SceneGraphInspector & inspector, + const std::unordered_set *> & plants, + const drake::geometry::GeometryId & geometry_id) +{ + return GetTfFrameName(inspector, plants, inspector.GetFrameId(geometry_id)); +} + +std::string +GetMarkerNamespacePrefix( + const drake::geometry::SceneGraphInspector & inspector, + const std::unordered_set *> & plants, + const drake::geometry::GeometryId & geometry_id) +{ + std::stringstream ss; + for (auto * plant : plants) { + const drake::multibody::Body * body = + plant->GetBodyFromFrameId(inspector.GetFrameId(geometry_id)); + if (body) { + ss << "/" << plant->GetModelInstanceName(body->model_instance()); + const std::string & body_name = body->name(); + if (body_name.empty()) { + ss << "/unnamed_body_" << body->index(); + } else { + ss << "/" << ReplaceAllOccurrences(body_name, "::", "/"); + } + return ss.str(); + } + } + ss << "/" << inspector.GetOwningSourceName(geometry_id); + const std::string & geometry_name = inspector.GetName(geometry_id); + if (geometry_name.empty()) { + ss << "/unnamed_geometry_" << geometry_id.get_value(); + } else { + ss << "/" << ReplaceAllOccurrences(geometry_name, "::", "/"); + } + return ss.str(); +} + +} // namespace utilities +} // namespace drake_ros_systems From 1f69048dd13d918cbdaed04471272fba7d786f8d Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Wed, 19 May 2021 09:30:10 -0300 Subject: [PATCH 82/87] Use name conventions everywhere. Signed-off-by: Michel Hidalgo --- drake_ros_systems/CMakeLists.txt | 5 +- .../drake_ros_systems/rviz_visualizer.hpp | 35 ++- .../scene_markers_system.hpp | 48 +++- ...em.hpp => scene_tf_broadcaster_system.hpp} | 42 ++- .../drake_ros_systems/scene_tf_system.hpp | 10 + .../include/drake_ros_systems/serializer.hpp | 5 + .../module_drake_ros_systems.cpp | 57 ++++- drake_ros_systems/src/rviz_visualizer.cpp | 74 +++++- .../src/scene_markers_system.cpp | 239 ++++++++++-------- ...em.cpp => scene_tf_broadcaster_system.cpp} | 37 ++- drake_ros_systems/src/scene_tf_system.cpp | 16 +- drake_ros_systems/src/subscription.cpp | 14 +- drake_ros_systems/src/subscription.hpp | 5 + drake_ros_systems/test/test_scene_markers.cpp | 36 +-- .../test/test_tf_broadcaster.cpp | 16 +- drake_ros_systems/test/test_tf_broadcaster.py | 16 +- 16 files changed, 452 insertions(+), 203 deletions(-) rename drake_ros_systems/include/drake_ros_systems/{tf_broadcaster_system.hpp => scene_tf_broadcaster_system.hpp} (52%) rename drake_ros_systems/src/{tf_broadcaster_system.cpp => scene_tf_broadcaster_system.cpp} (62%) diff --git a/drake_ros_systems/CMakeLists.txt b/drake_ros_systems/CMakeLists.txt index 888e4ef2..01a6deca 100644 --- a/drake_ros_systems/CMakeLists.txt +++ b/drake_ros_systems/CMakeLists.txt @@ -3,7 +3,7 @@ project(drake_ros_systems) # Default to C++14 if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD 17) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") @@ -31,7 +31,8 @@ add_library(drake_ros_systems src/scene_markers_system.cpp src/scene_tf_system.cpp src/subscription.cpp - src/tf_broadcaster_system.cpp + src/scene_tf_broadcaster_system.cpp + src/utilities/name_conventions.cpp src/utilities/type_conversion.cpp ) diff --git a/drake_ros_systems/include/drake_ros_systems/rviz_visualizer.hpp b/drake_ros_systems/include/drake_ros_systems/rviz_visualizer.hpp index eefc1b0c..5093c66a 100644 --- a/drake_ros_systems/include/drake_ros_systems/rviz_visualizer.hpp +++ b/drake_ros_systems/include/drake_ros_systems/rviz_visualizer.hpp @@ -14,6 +14,7 @@ #ifndef DRAKE_ROS_SYSTEMS__RVIZ_VISUALIZER_HPP_ #define DRAKE_ROS_SYSTEMS__RVIZ_VISUALIZER_HPP_ +#include #include #include @@ -25,6 +26,20 @@ namespace drake_ros_systems { +/// Set of parameters that configure an RvizVisualizer. +struct RvizVisualizerParams +{ + /// Publish triggers for scene markers and tf broadcasting. + std::unordered_set publish_triggers{ + drake::systems::TriggerType::kForced, drake::systems::TriggerType::kPeriodic}; + + /// Period for periodic scene markers and tf broadcasting. + double publish_period{0.05}; + + /// Whether to perform tf broadcasting or not. + bool publish_tf{true}; +}; + /// System for SceneGraph visualization in RViz. /// /// This system is a subdiagram aggregating a SceneMarkersSystem, a @@ -39,14 +54,24 @@ namespace drake_ros_systems class RvizVisualizer : public drake::systems::Diagram { public: - RvizVisualizer( + explicit RvizVisualizer( std::shared_ptr ros_interface, - const std::unordered_set & publish_triggers = { - drake::systems::TriggerType::kForced, drake::systems::TriggerType::kPeriodic}, - double publish_period = 0.05, - bool publish_tf = true); + RvizVisualizerParams params = {}); + + ~RvizVisualizer() override; + + // Forwarded to SceneTfSystem::RegisterMultibodyPlant + // and SceneTfBroadcasterSystem::RegisterMultibodyPlant + void + RegisterMultibodyPlant( + const drake::multibody::MultibodyPlant * plant); const drake::systems::InputPort & get_graph_query_port() const; + +private: + class RvizVisualizerPrivate; + + std::unique_ptr impl_; }; } // namespace drake_ros_systems diff --git a/drake_ros_systems/include/drake_ros_systems/scene_markers_system.hpp b/drake_ros_systems/include/drake_ros_systems/scene_markers_system.hpp index 886b7789..15be1434 100644 --- a/drake_ros_systems/include/drake_ros_systems/scene_markers_system.hpp +++ b/drake_ros_systems/include/drake_ros_systems/scene_markers_system.hpp @@ -16,16 +16,44 @@ #include #include +#include #include #include #include +#include // NOLINT(build/include_order) +#include namespace drake_ros_systems { +/// Set of parameters that configure a SceneMarkersSystem. +struct SceneMarkersParams +{ + /// Configure SceneMarkersSystem to depict illustration geometries. + static SceneMarkersParams Illustration() { return {}; } + + /// Configure SceneMarkersSystem to depict proximity geometries. + static SceneMarkersParams Proximity() { + SceneMarkersParams params; + params.role = drake::geometry::Role::kProximity; + params.default_color.set(0.5, 0.5, 0.5, 1.0); + return params; + } + + /// Optional marker namespace prefix, defaults to + /// utilities::GetMarkerNamespacePrefix() outcome. + std::optional marker_namespace_prefix{std::nullopt}; + + /// Role of the geometries to depict. + drake::geometry::Role role{drake::geometry::Role::kIllustration}; + + /// Default marker color if no ("phong", "diffuse") property is found. + drake::geometry::Rgba default_color{0.9, 0.9, 0.9, 1.0}; +}; + /// System for SceneGraph depiction as a ROS markers array. /// /// This system outputs a `visualization_msgs/msg/MarkerArray` populated with @@ -47,16 +75,19 @@ namespace drake_ros_systems class SceneMarkersSystem : public drake::systems::LeafSystem { public: - SceneMarkersSystem( - const drake::geometry::Role & role = drake::geometry::Role::kIllustration, - const drake::geometry::Rgba & default_color = {0.9, 0.9, 0.9, 1.0}); + explicit SceneMarkersSystem(SceneMarkersParams params = {}); virtual ~SceneMarkersSystem(); - /// Role of the geometries this system targets. - const drake::geometry::Role & role() const; + /// Register a MultibodyPlant present in the scene + /** + * This provides the system with additional information to generate + * semantically meaningful frame string IDs and marker namespaces. + */ + void + RegisterMultibodyPlant( + const drake::multibody::MultibodyPlant * plant); - /// Default color used when a phong/diffuse property cannot be found. - const drake::geometry::Rgba & default_color() const; + const SceneMarkersParams & params() const; const drake::systems::InputPort & get_graph_query_port() const; @@ -74,7 +105,8 @@ class SceneMarkersSystem : public drake::systems::LeafSystem // which is invalidated (and thus recomputed) upon a SceneGraph // geometry version change. const visualization_msgs::msg::MarkerArray & - EvalSceneMarkers(const drake::systems::Context & context) const; + EvalSceneMarkers(const drake::systems::Context & context, + bool * cached = nullptr) const; // Inspects the SceneGraph and carries out the conversion // to visualization_msgs::msg::MarkerArray message unconditionally. diff --git a/drake_ros_systems/include/drake_ros_systems/tf_broadcaster_system.hpp b/drake_ros_systems/include/drake_ros_systems/scene_tf_broadcaster_system.hpp similarity index 52% rename from drake_ros_systems/include/drake_ros_systems/tf_broadcaster_system.hpp rename to drake_ros_systems/include/drake_ros_systems/scene_tf_broadcaster_system.hpp index b430aca4..73326962 100644 --- a/drake_ros_systems/include/drake_ros_systems/tf_broadcaster_system.hpp +++ b/drake_ros_systems/include/drake_ros_systems/scene_tf_broadcaster_system.hpp @@ -11,9 +11,10 @@ // 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__TF_BROADCASTER_SYSTEM_HPP_ -#define DRAKE_ROS_SYSTEMS__TF_BROADCASTER_SYSTEM_HPP_ +#ifndef DRAKE_ROS_SYSTEMS__SCENE_TF_BROADCASTER_SYSTEM_HPP_ +#define DRAKE_ROS_SYSTEMS__SCENE_TF_BROADCASTER_SYSTEM_HPP_ +#include #include #include @@ -24,24 +25,47 @@ namespace drake_ros_systems { + +/// Set of parameters that configure a SceneTfBroadcasterParams. +struct SceneTfBroadcasterParams +{ + /// Publish triggers for tf broadcasting. + std::unordered_set + publish_triggers{ + drake::systems::TriggerType::kForced, + drake::systems::TriggerType::kPerStep}; + + /// Period for periodic tf broadcasting. + double publish_period{0.0}; +}; + /// System for tf2 transform broadcasting. /// -/// This system is a subdiagram aggregating a SceneTFSystem and a +/// This system is a subdiagram aggregating a SceneTfSystem and a /// RosPublisherSystem to broadcast SceneGraph frame transforms. /// Messages are published to the `/tf` ROS topic. /// /// It exports one input port: /// - *graph_query* (abstract): expects a QueryObject from the SceneGraph. -class TfBroadcasterSystem : public drake::systems::Diagram +class SceneTfBroadcasterSystem : public drake::systems::Diagram { public: - TfBroadcasterSystem( + explicit SceneTfBroadcasterSystem( std::shared_ptr ros_interface, - const std::unordered_set & publish_triggers = { - drake::systems::TriggerType::kPerStep, drake::systems::TriggerType::kForced}, - double publish_period = 0.0); + SceneTfBroadcasterParams params = {}); + ~SceneTfBroadcasterSystem() override; + + // Forwarded to SceneTfSystem::RegisterMultibodyPlant + void + RegisterMultibodyPlant( + const drake::multibody::MultibodyPlant * plant); const drake::systems::InputPort & get_graph_query_port() const; + +private: + class SceneTfBroadcasterSystemPrivate; + + std::unique_ptr impl_; }; } // namespace drake_ros_systems -#endif // DRAKE_ROS_SYSTEMS__TF_BROADCASTER_SYSTEM_HPP_ +#endif // DRAKE_ROS_SYSTEMS__SCENE_TF_BROADCASTER_SYSTEM_HPP_ diff --git a/drake_ros_systems/include/drake_ros_systems/scene_tf_system.hpp b/drake_ros_systems/include/drake_ros_systems/scene_tf_system.hpp index 676f2995..67ee3631 100644 --- a/drake_ros_systems/include/drake_ros_systems/scene_tf_system.hpp +++ b/drake_ros_systems/include/drake_ros_systems/scene_tf_system.hpp @@ -14,6 +14,7 @@ #ifndef DRAKE_ROS_SYSTEMS__SCENE_TF_SYSTEM_HPP_ #define DRAKE_ROS_SYSTEMS__SCENE_TF_SYSTEM_HPP_ +#include #include #include @@ -43,6 +44,15 @@ class SceneTfSystem : public drake::systems::LeafSystem SceneTfSystem(); virtual ~SceneTfSystem(); + /// Register a MultibodyPlant present in the scene + /** + * This provides the system with additional information + * to generate semantically meaningful frame string IDs. + */ + void + RegisterMultibodyPlant( + const drake::multibody::MultibodyPlant * plant); + const drake::systems::InputPort & get_graph_query_port() const; const drake::systems::OutputPort & get_scene_tf_output_port() const; diff --git a/drake_ros_systems/include/drake_ros_systems/serializer.hpp b/drake_ros_systems/include/drake_ros_systems/serializer.hpp index 60301e9f..8704e452 100644 --- a/drake_ros_systems/include/drake_ros_systems/serializer.hpp +++ b/drake_ros_systems/include/drake_ros_systems/serializer.hpp @@ -20,10 +20,15 @@ #include #include +#include +#include + #include #include "drake_ros_systems/serializer_interface.hpp" + + namespace drake_ros_systems { template 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 9393a6c4..3ce50726 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,7 +24,7 @@ #include "drake_ros_systems/ros_publisher_system.hpp" #include "drake_ros_systems/ros_subscriber_system.hpp" #include "drake_ros_systems/rviz_visualizer.hpp" -#include "drake_ros_systems/tf_broadcaster_system.hpp" +#include "drake_ros_systems/scene_tf_broadcaster_system.hpp" #include "py_serializer.hpp" #include "qos_type_caster.hpp" @@ -42,14 +42,17 @@ using drake_ros_systems::RosInterfaceSystem; using drake_ros_systems::RosPublisherSystem; using drake_ros_systems::RosSubscriberSystem; using drake_ros_systems::RvizVisualizer; +using drake_ros_systems::RvizVisualizerParams; using drake_ros_systems::SerializerInterface; -using drake_ros_systems::TfBroadcasterSystem; +using drake_ros_systems::SceneTfBroadcasterSystem; +using drake_ros_systems::SceneTfBroadcasterParams; PYBIND11_MODULE(drake_ros_systems, m) { m.doc() = "Python wrapper for drake_ros_systems"; py::module::import("pydrake.systems.framework"); + py::module::import("pydrake.multibody.plant"); // Use std::shared_ptr holder so pybind11 doesn't try to delete interfaces returned from // get_ros_interface @@ -116,25 +119,53 @@ PYBIND11_MODULE(drake_ros_systems, m) { ros_interface); })); - py::class_>(m, "TfBroadcasterSystem") + py::class_(m, "SceneTfBroadcasterParams") .def( - py::init, std::unordered_set, double>(), + py::init( + [](py::kwargs kwargs) { + SceneTfBroadcasterParams obj{}; + py::object pyobj = py::cast( + &obj, py::return_value_policy::reference); + for (auto & item : kwargs) { + py::setattr(pyobj, item.first, item.second); + } + return obj; + })) + .def_readwrite("publish_triggers", &SceneTfBroadcasterParams::publish_triggers) + .def_readwrite("publish_period", &SceneTfBroadcasterParams::publish_period); + + py::class_>(m, "SceneTfBroadcasterSystem") + .def( + py::init, SceneTfBroadcasterParams>(), py::arg("ros_interface"), - py::arg("publish_triggers") = - std::unordered_set{TriggerType::kPerStep, TriggerType::kForced}, - py::arg("publish_period") = 0.0) + py::arg("params") = SceneTfBroadcasterParams{}) + .def("RegisterMultibodyPlant", &SceneTfBroadcasterSystem::RegisterMultibodyPlant) .def( - "get_graph_query_port", &TfBroadcasterSystem::get_graph_query_port, + "get_graph_query_port", &SceneTfBroadcasterSystem::get_graph_query_port, py::return_value_policy::reference_internal); + py::class_(m, "RvizVisualizerParams") + .def( + py::init( + [](py::kwargs kwargs) { + RvizVisualizerParams obj{}; + py::object pyobj = py::cast( + &obj, py::return_value_policy::reference); + for (auto & item : kwargs) { + py::setattr(pyobj, item.first, item.second); + } + return obj; + })) + .def_readwrite("publish_triggers", &RvizVisualizerParams::publish_triggers) + .def_readwrite("publish_period", &RvizVisualizerParams::publish_period) + .def_readwrite("publish_tf", &RvizVisualizerParams::publish_tf); + py::class_>(m, "RvizVisualizer") .def( - py::init, std::unordered_set, double, bool>(), + py::init, RvizVisualizerParams>(), py::arg("ros_interface"), - py::arg("publish_triggers") = - std::unordered_set{TriggerType::kPerStep, TriggerType::kForced}, - py::arg("publish_period") = 0.0, - py::arg("publish_tf") = true) + py::arg("params") = RvizVisualizerParams{}) + .def("RegisterMultibodyPlant", &RvizVisualizer::RegisterMultibodyPlant) .def( "get_graph_query_port", &RvizVisualizer::get_graph_query_port, py::return_value_policy::reference_internal); diff --git a/drake_ros_systems/src/rviz_visualizer.cpp b/drake_ros_systems/src/rviz_visualizer.cpp index f92e9ba3..852affc0 100644 --- a/drake_ros_systems/src/rviz_visualizer.cpp +++ b/drake_ros_systems/src/rviz_visualizer.cpp @@ -19,10 +19,12 @@ #include #include +#include +#include "drake_ros_systems/constant_message_source.hpp" #include "drake_ros_systems/ros_publisher_system.hpp" #include "drake_ros_systems/scene_markers_system.hpp" -#include "drake_ros_systems/tf_broadcaster_system.hpp" +#include "drake_ros_systems/scene_tf_broadcaster_system.hpp" #include "drake_ros_systems/rviz_visualizer.hpp" @@ -30,35 +32,81 @@ namespace drake_ros_systems { +class RvizVisualizer::RvizVisualizerPrivate +{ +public: + SceneMarkersSystem * scene_visual_markers; + SceneMarkersSystem * scene_collision_markers; + SceneTfBroadcasterSystem * scene_tf_broadcaster{nullptr}; +}; + RvizVisualizer::RvizVisualizer( std::shared_ptr ros_interface, - const std::unordered_set & publish_triggers, - double publish_period, bool publish_tf) + RvizVisualizerParams params) +: impl_(new RvizVisualizerPrivate()) { drake::systems::DiagramBuilder builder; - auto scene_markers = builder.AddSystem(); + auto scene_visual_markers_publisher = builder.AddSystem( + RosPublisherSystem::Make( + "/scene_markers/visual", rclcpp::QoS(1), ros_interface, + params.publish_triggers, params.publish_period)); + + impl_->scene_visual_markers = + builder.AddSystem( + SceneMarkersParams::Illustration()); - auto scene_markers_publisher = builder.AddSystem( + builder.Connect( + impl_->scene_visual_markers->get_markers_output_port(), + scene_visual_markers_publisher->get_input_port()); + + builder.ExportInput( + impl_->scene_visual_markers->get_graph_query_port(), "graph_query"); + + auto scene_collision_markers_publisher = builder.AddSystem( RosPublisherSystem::Make( - "/scene_markers", rclcpp::QoS(1), ros_interface, publish_triggers, publish_period)); + "/scene_markers/collision", rclcpp::QoS(1), ros_interface, + params.publish_triggers, params.publish_period)); + + impl_->scene_collision_markers = + builder.AddSystem( + SceneMarkersParams::Proximity()); builder.Connect( - scene_markers->get_markers_output_port(), - scene_markers_publisher->get_input_port()); + impl_->scene_collision_markers->get_markers_output_port(), + scene_collision_markers_publisher->get_input_port()); - builder.ExportInput(scene_markers->get_graph_query_port(), "graph_query"); + builder.ConnectInput( + "graph_query", impl_->scene_collision_markers->get_graph_query_port()); - if (publish_tf) { - auto tf_broadcaster = builder.AddSystem( - ros_interface, publish_triggers, publish_period); + if (params.publish_tf) { + impl_->scene_tf_broadcaster = + builder.AddSystem( + ros_interface, SceneTfBroadcasterParams{ + params.publish_triggers, params.publish_period}); - builder.ConnectInput("graph_query", tf_broadcaster->get_graph_query_port()); + builder.ConnectInput( + "graph_query", impl_->scene_tf_broadcaster->get_graph_query_port()); } builder.BuildInto(this); } +RvizVisualizer::~RvizVisualizer() +{ +} + +void +RvizVisualizer::RegisterMultibodyPlant( + const drake::multibody::MultibodyPlant * plant) +{ + impl_->scene_visual_markers->RegisterMultibodyPlant(plant); + impl_->scene_collision_markers->RegisterMultibodyPlant(plant); + if (impl_->scene_tf_broadcaster) { + impl_->scene_tf_broadcaster->RegisterMultibodyPlant(plant); + } +} + const drake::systems::InputPort & RvizVisualizer::get_graph_query_port() const { diff --git a/drake_ros_systems/src/scene_markers_system.cpp b/drake_ros_systems/src/scene_markers_system.cpp index e508e86f..3bfd75b5 100644 --- a/drake_ros_systems/src/scene_markers_system.cpp +++ b/drake_ros_systems/src/scene_markers_system.cpp @@ -30,7 +30,11 @@ #include #include +#include +#include + #include "drake_ros_systems/scene_markers_system.hpp" +#include "drake_ros_systems/utilities/name_conventions.hpp" #include "drake_ros_systems/utilities/type_conversion.hpp" @@ -45,10 +49,8 @@ class SceneGeometryToMarkers : public drake::geometry::ShapeReifier public: DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(SceneGeometryToMarkers) - SceneGeometryToMarkers( - const drake::geometry::Role & role, - const drake::geometry::Rgba & default_color) - : role_(role), default_color_(default_color) + explicit SceneGeometryToMarkers(const SceneMarkersParams & params) + : params_(params) { } @@ -56,16 +58,17 @@ class SceneGeometryToMarkers : public drake::geometry::ShapeReifier void Populate( const drake::geometry::SceneGraphInspector & inspector, + const std::unordered_set *> plants, const drake::geometry::GeometryId & geometry_id, visualization_msgs::msg::MarkerArray * marker_array) { DRAKE_ASSERT(nullptr != marker_array); - marker_array_ = marker_array; prototype_marker_.header.frame_id = - inspector.GetName(inspector.GetFrameId(geometry_id)); - prototype_marker_.ns = inspector.GetOwningSourceName(geometry_id); + utilities::GetTfFrameName(inspector, plants, geometry_id); + prototype_marker_.ns = params_.marker_namespace_prefix.value_or( + utilities::GetMarkerNamespacePrefix(inspector, plants, geometry_id)); prototype_marker_.id = marker_array_->markers.size(); prototype_marker_.action = visualization_msgs::msg::Marker::MODIFY; @@ -73,15 +76,14 @@ class SceneGeometryToMarkers : public drake::geometry::ShapeReifier prototype_marker_.frame_locked = true; const drake::geometry::GeometryProperties * props = - inspector.GetProperties(geometry_id, role_); + inspector.GetProperties(geometry_id, params_.role); DRAKE_ASSERT(nullptr != props); - drake::geometry::Rgba default_color = default_color_; - if (drake::geometry::Role::kIllustration != role_) { + drake::geometry::Rgba default_color = params_.default_color; + if (drake::geometry::Role::kIllustration != params_.role) { const drake::geometry::IllustrationProperties * illustration_props = inspector.GetIllustrationProperties(geometry_id); if (illustration_props) { - default_color = - illustration_props->GetPropertyOrDefault( + default_color = illustration_props->GetPropertyOrDefault( "phong", "diffuse", default_color); } } @@ -102,77 +104,84 @@ class SceneGeometryToMarkers : public drake::geometry::ShapeReifier void ImplementGeometry(const drake::geometry::Sphere & sphere, void *) override { - prototype_marker_.type = visualization_msgs::msg::Marker::SPHERE; - prototype_marker_.scale.x = sphere.radius(); - prototype_marker_.scale.y = sphere.radius(); - prototype_marker_.scale.z = sphere.radius(); - prototype_marker_.pose = utilities::ToPoseMsg(X_FG_); marker_array_->markers.push_back(prototype_marker_); + + visualization_msgs::msg::Marker & marker = marker_array_->markers.back(); + marker.type = visualization_msgs::msg::Marker::SPHERE; + marker.scale.x = sphere.radius(); + marker.scale.y = sphere.radius(); + marker.scale.z = sphere.radius(); + marker.pose = utilities::ToPoseMsg(X_FG_); } void ImplementGeometry(const drake::geometry::Ellipsoid & ellipsoid, void *) override { - prototype_marker_.type = visualization_msgs::msg::Marker::SPHERE; - prototype_marker_.scale.x = ellipsoid.a(); - prototype_marker_.scale.y = ellipsoid.b(); - prototype_marker_.scale.z = ellipsoid.c(); - prototype_marker_.pose = utilities::ToPoseMsg(X_FG_); - marker_array_->markers.push_back(prototype_marker_); + + visualization_msgs::msg::Marker & marker = marker_array_->markers.back(); + marker.type = visualization_msgs::msg::Marker::SPHERE; + marker.scale.x = ellipsoid.a(); + marker.scale.y = ellipsoid.b(); + marker.scale.z = ellipsoid.c(); + marker.pose = utilities::ToPoseMsg(X_FG_); } void ImplementGeometry(const drake::geometry::Cylinder & cylinder, void *) override { - prototype_marker_.type = visualization_msgs::msg::Marker::CYLINDER; - prototype_marker_.scale.x = cylinder.radius(); - prototype_marker_.scale.y = cylinder.radius(); - prototype_marker_.scale.z = cylinder.length(); - prototype_marker_.pose = utilities::ToPoseMsg(X_FG_); - marker_array_->markers.push_back(prototype_marker_); + + visualization_msgs::msg::Marker & marker = marker_array_->markers.back(); + marker.type = visualization_msgs::msg::Marker::CYLINDER; + marker.scale.x = cylinder.radius(); + marker.scale.y = cylinder.radius(); + marker.scale.z = cylinder.length(); + marker.pose = utilities::ToPoseMsg(X_FG_); } void ImplementGeometry(const drake::geometry::HalfSpace &, void *) override { + marker_array_->markers.push_back(prototype_marker_); + + visualization_msgs::msg::Marker & marker = marker_array_->markers.back(); + marker.type = visualization_msgs::msg::Marker::CUBE; constexpr double kHalfSpaceLength = 50.; constexpr double kHalfSpaceThickness = 1.; - - prototype_marker_.type = visualization_msgs::msg::Marker::CUBE; - prototype_marker_.scale.x = kHalfSpaceLength; - prototype_marker_.scale.y = kHalfSpaceLength; - prototype_marker_.scale.z = kHalfSpaceThickness; + marker.scale.x = kHalfSpaceLength; + marker.scale.y = kHalfSpaceLength; + marker.scale.z = kHalfSpaceThickness; const drake::math::RigidTransform X_GH{ drake::Vector3{0., 0., -kHalfSpaceThickness / 2.}}; const drake::math::RigidTransform X_FH = X_FG_ * X_GH; - prototype_marker_.pose = utilities::ToPoseMsg(X_FH); - - marker_array_->markers.push_back(prototype_marker_); + marker.pose = utilities::ToPoseMsg(X_FH); } void ImplementGeometry(const drake::geometry::Box & box, void *) override { - prototype_marker_.type = visualization_msgs::msg::Marker::CUBE; - prototype_marker_.scale.x = box.width(); - prototype_marker_.scale.y = box.depth(); - prototype_marker_.scale.z = box.height(); - prototype_marker_.pose = utilities::ToPoseMsg(X_FG_); - marker_array_->markers.push_back(prototype_marker_); + + visualization_msgs::msg::Marker & marker = marker_array_->markers.back(); + marker.type = visualization_msgs::msg::Marker::CUBE; + marker.scale.x = box.width(); + marker.scale.y = box.depth(); + marker.scale.z = box.height(); + marker.pose = utilities::ToPoseMsg(X_FG_); } void ImplementGeometry(const drake::geometry::Capsule & capsule, void *) override { - visualization_msgs::msg::Marker body_marker = prototype_marker_; + marker_array_->markers.push_back(prototype_marker_); + + visualization_msgs::msg::Marker & body_marker = marker_array_->markers.back(); body_marker.type = visualization_msgs::msg::Marker::CYLINDER; body_marker.scale.x = capsule.radius(); body_marker.scale.y = capsule.radius(); body_marker.scale.z = capsule.length(); - prototype_marker_.pose = utilities::ToPoseMsg(X_FG_); + body_marker.pose = utilities::ToPoseMsg(X_FG_); - marker_array_->markers.push_back(body_marker); + marker_array_->markers.push_back(prototype_marker_); - visualization_msgs::msg::Marker upper_cap_marker = prototype_marker_; - upper_cap_marker.id = marker_array_->markers.size(); + visualization_msgs::msg::Marker & upper_cap_marker = marker_array_->markers.back(); + upper_cap_marker.id = body_marker.id + 1; upper_cap_marker.type = visualization_msgs::msg::Marker::SPHERE; upper_cap_marker.scale.x = capsule.radius(); upper_cap_marker.scale.y = capsule.radius(); @@ -184,44 +193,43 @@ class SceneGeometryToMarkers : public drake::geometry::ShapeReifier marker_array_->markers.push_back(upper_cap_marker); - visualization_msgs::msg::Marker lower_cap_marker = upper_cap_marker; - lower_cap_marker.id = marker_array_->markers.size(); + visualization_msgs::msg::Marker & lower_cap_marker = marker_array_->markers.back(); + lower_cap_marker.id = upper_cap_marker.id + 1; const drake::math::RigidTransform X_GL{ drake::Vector3{0., 0., -capsule.length() / 2.}}; const drake::math::RigidTransform X_FL = X_FG_ * X_GL; lower_cap_marker.pose = utilities::ToPoseMsg(X_FL); - - marker_array_->markers.push_back(lower_cap_marker); } void ImplementGeometry(const drake::geometry::Convex & convex, void *) override { - prototype_marker_.type = visualization_msgs::msg::Marker::MESH_RESOURCE; - prototype_marker_.scale.x = convex.scale(); - prototype_marker_.scale.y = convex.scale(); - prototype_marker_.scale.z = convex.scale(); - // Assume it is an absolute path and turn it into a file URL. - prototype_marker_.mesh_resource = "file://" + convex.filename(); - prototype_marker_.pose = utilities::ToPoseMsg(X_FG_); - marker_array_->markers.push_back(prototype_marker_); + + visualization_msgs::msg::Marker & marker = marker_array_->markers.back(); + marker.type = visualization_msgs::msg::Marker::MESH_RESOURCE; + marker.scale.x = convex.scale(); + marker.scale.y = convex.scale(); + marker.scale.z = convex.scale(); + // Assume it is an absolute path and turn it into a file URL. + marker.mesh_resource = "file://" + convex.filename(); + marker.pose = utilities::ToPoseMsg(X_FG_); } void ImplementGeometry(const drake::geometry::Mesh & mesh, void *) override { - prototype_marker_.type = visualization_msgs::msg::Marker::MESH_RESOURCE; - prototype_marker_.scale.x = mesh.scale(); - prototype_marker_.scale.y = mesh.scale(); - prototype_marker_.scale.z = mesh.scale(); - // Assume it is an absolute path and turn it into a file URL. - prototype_marker_.mesh_resource = "file://" + mesh.filename(); - prototype_marker_.pose = utilities::ToPoseMsg(X_FG_); - marker_array_->markers.push_back(prototype_marker_); + + visualization_msgs::msg::Marker & marker = marker_array_->markers.back(); + marker.type = visualization_msgs::msg::Marker::MESH_RESOURCE; + marker.scale.x = mesh.scale(); + marker.scale.y = mesh.scale(); + marker.scale.z = mesh.scale(); + // Assume it is an absolute path and turn it into a file URL. + marker.mesh_resource = "file://" + mesh.filename(); + marker.pose = utilities::ToPoseMsg(X_FG_); } - const drake::geometry::Role role_; - const drake::geometry::Rgba default_color_; + const SceneMarkersParams & params_; visualization_msgs::msg::MarkerArray * marker_array_{nullptr}; visualization_msgs::msg::Marker prototype_marker_{}; drake::math::RigidTransform X_FG_{}; @@ -232,25 +240,21 @@ class SceneGeometryToMarkers : public drake::geometry::ShapeReifier class SceneMarkersSystem::SceneMarkersSystemPrivate { public: - SceneMarkersSystemPrivate( - const drake::geometry::Role & _role, - const drake::geometry::Rgba & _default_color) - : role(_role), default_color(_default_color) + explicit SceneMarkersSystemPrivate(SceneMarkersParams _params) + : params(std::move(_params)) { } - const drake::geometry::Role role; - const drake::geometry::Rgba default_color; - mutable drake::geometry::GeometryVersion version; + const SceneMarkersParams params; drake::systems::CacheIndex scene_markers_cache_index; drake::systems::InputPortIndex graph_query_port_index; drake::systems::OutputPortIndex scene_markers_port_index; + std::unordered_set *> plants; + mutable drake::geometry::GeometryVersion version; }; -SceneMarkersSystem::SceneMarkersSystem( - const drake::geometry::Role & role, - const drake::geometry::Rgba & default_color) -: impl_(new SceneMarkersSystemPrivate(role, default_color)) +SceneMarkersSystem::SceneMarkersSystem(SceneMarkersParams params) +: impl_(new SceneMarkersSystemPrivate(std::move(params))) { impl_->graph_query_port_index = this->DeclareAbstractInputPort( @@ -270,13 +274,40 @@ SceneMarkersSystem::~SceneMarkersSystem() { } +void +SceneMarkersSystem::RegisterMultibodyPlant( + const drake::multibody::MultibodyPlant * plant) +{ + DRAKE_THROW_UNLESS(plant != nullptr); + impl_->plants.insert(plant); +} + +namespace +{ + +visualization_msgs::msg::Marker MakeDeleteAllMarker() +{ + visualization_msgs::msg::Marker marker; + marker.action = visualization_msgs::msg::Marker::DELETEALL; + return marker; +} + +} // namespace + void SceneMarkersSystem::PopulateSceneMarkersMessage( const drake::systems::Context & context, visualization_msgs::msg::MarkerArray * output_value) const { - *output_value = this->EvalSceneMarkers(context); - + bool cached; + *output_value = this->EvalSceneMarkers(context, &cached); + if (!cached) { + // Cache invalidated after scene change. + // Delete all pre-existing markers before an update. + output_value->markers.insert( + output_value->markers.begin(), + MakeDeleteAllMarker()); + } const builtin_interfaces::msg::Time stamp = rclcpp::Time() + rclcpp::Duration::from_seconds(context.get_time()); for (visualization_msgs::msg::Marker & marker : output_value->markers) { @@ -286,35 +317,30 @@ SceneMarkersSystem::PopulateSceneMarkersMessage( const visualization_msgs::msg::MarkerArray & SceneMarkersSystem::EvalSceneMarkers( - const drake::systems::Context & context) const + const drake::systems::Context & context, + bool * cached) const { const drake::geometry::QueryObject & query_object = get_input_port(impl_->graph_query_port_index) .Eval>(context); const drake::geometry::GeometryVersion & current_version = query_object.inspector().geometry_version(); - if (!impl_->version.IsSameAs(current_version, impl_->role)) { + const bool same_version = + impl_->version.IsSameAs(current_version, impl_->params.role); + if (!same_version) { // Invalidate scene markers cache get_cache_entry(impl_->scene_markers_cache_index) .get_mutable_cache_entry_value(context) .mark_out_of_date(); impl_->version = current_version; } + if (cached) { + *cached = same_version; + } return get_cache_entry(impl_->scene_markers_cache_index) .Eval(context); } -namespace { - -visualization_msgs::msg::Marker MakeDeleteAllMarker() -{ - visualization_msgs::msg::Marker marker; - marker.action = visualization_msgs::msg::Marker::DELETEALL; - return marker; -} - -} // namespace - void SceneMarkersSystem::CalcSceneMarkers( const drake::systems::Context & context, @@ -324,28 +350,21 @@ SceneMarkersSystem::CalcSceneMarkers( get_input_port(impl_->graph_query_port_index) .Eval>(context); const drake::geometry::SceneGraphInspector & inspector = query_object.inspector(); - output_value->markers.reserve(inspector.NumGeometriesWithRole(impl_->role) + 1); - output_value->markers.push_back(MakeDeleteAllMarker()); // always delete all first + output_value->markers.reserve(inspector.NumGeometriesWithRole(impl_->params.role)); for (const drake::geometry::FrameId & frame_id : inspector.all_frame_ids()) { for (const drake::geometry::GeometryId & geometry_id : - inspector.GetGeometries(frame_id, impl_->role)) + inspector.GetGeometries(frame_id, impl_->params.role)) { - SceneGeometryToMarkers(impl_->role, impl_->default_color) - .Populate(inspector, geometry_id, output_value); + SceneGeometryToMarkers(impl_->params).Populate( + inspector, impl_->plants, geometry_id, output_value); } } } -const drake::geometry::Role & -SceneMarkersSystem::role() const -{ - return impl_->role; -} - -const drake::geometry::Rgba & -SceneMarkersSystem::default_color() const +const SceneMarkersParams & +SceneMarkersSystem::params() const { - return impl_->default_color; + return impl_->params; } const drake::systems::InputPort & diff --git a/drake_ros_systems/src/tf_broadcaster_system.cpp b/drake_ros_systems/src/scene_tf_broadcaster_system.cpp similarity index 62% rename from drake_ros_systems/src/tf_broadcaster_system.cpp rename to drake_ros_systems/src/scene_tf_broadcaster_system.cpp index 638198c7..52a57574 100644 --- a/drake_ros_systems/src/tf_broadcaster_system.cpp +++ b/drake_ros_systems/src/scene_tf_broadcaster_system.cpp @@ -22,38 +22,57 @@ #include "drake_ros_systems/drake_ros_interface.hpp" #include "drake_ros_systems/ros_publisher_system.hpp" +#include "drake_ros_systems/scene_tf_broadcaster_system.hpp" #include "drake_ros_systems/scene_tf_system.hpp" -#include "drake_ros_systems/tf_broadcaster_system.hpp" namespace drake_ros_systems { -TfBroadcasterSystem::TfBroadcasterSystem( +class SceneTfBroadcasterSystem::SceneTfBroadcasterSystemPrivate +{ +public: + SceneTfSystem * scene_tf; +}; + +SceneTfBroadcasterSystem::SceneTfBroadcasterSystem( std::shared_ptr ros_interface, - const std::unordered_set & publish_triggers, - double publish_period) + SceneTfBroadcasterParams params) +: impl_(new SceneTfBroadcasterSystemPrivate()) { drake::systems::DiagramBuilder builder; - auto scene_tf = builder.AddSystem(); + impl_->scene_tf = builder.AddSystem(); auto scene_tf_publisher = builder.AddSystem( RosPublisherSystem::Make( "/tf", tf2_ros::DynamicBroadcasterQoS(), - ros_interface, publish_triggers, publish_period)); + ros_interface, params.publish_triggers, + params.publish_period)); builder.Connect( - scene_tf->get_scene_tf_output_port(), + impl_->scene_tf->get_scene_tf_output_port(), scene_tf_publisher->get_input_port()); - builder.ExportInput(scene_tf->get_graph_query_port(), "graph_query"); + builder.ExportInput( + impl_->scene_tf->get_graph_query_port(), "graph_query"); builder.BuildInto(this); } +SceneTfBroadcasterSystem::~SceneTfBroadcasterSystem() +{ +} + +void +SceneTfBroadcasterSystem::RegisterMultibodyPlant( + const drake::multibody::MultibodyPlant * plant) +{ + impl_->scene_tf->RegisterMultibodyPlant(plant); +} + const drake::systems::InputPort & -TfBroadcasterSystem::get_graph_query_port() const +SceneTfBroadcasterSystem::get_graph_query_port() const { return get_input_port(); } diff --git a/drake_ros_systems/src/scene_tf_system.cpp b/drake_ros_systems/src/scene_tf_system.cpp index f1fb5fa4..25dce217 100644 --- a/drake_ros_systems/src/scene_tf_system.cpp +++ b/drake_ros_systems/src/scene_tf_system.cpp @@ -18,7 +18,10 @@ #include #include +#include + #include "drake_ros_systems/scene_tf_system.hpp" +#include "drake_ros_systems/utilities/name_conventions.hpp" #include "drake_ros_systems/utilities/type_conversion.hpp" @@ -27,6 +30,7 @@ namespace drake_ros_systems class SceneTfSystem::SceneTfSystemPrivate { public: + std::unordered_set *> plants; drake::systems::InputPortIndex graph_query_port_index; drake::systems::OutputPortIndex scene_tf_port_index; }; @@ -47,6 +51,14 @@ SceneTfSystem::~SceneTfSystem() { } +void +SceneTfSystem::RegisterMultibodyPlant( + const drake::multibody::MultibodyPlant * plant) +{ + DRAKE_THROW_UNLESS(plant != nullptr); + impl_->plants.insert(plant); +} + const drake::systems::InputPort & SceneTfSystem::get_graph_query_port() const { @@ -70,6 +82,7 @@ SceneTfSystem::CalcSceneTf( // TODO(hidmic): publish frame transforms w.r.t. to their parent frame // instead of the world frame when an API is made available. if (inspector.num_frames() > 1) { + output_value->transforms.clear(); output_value->transforms.reserve(inspector.num_frames() - 1); geometry_msgs::msg::TransformStamped transform; @@ -80,7 +93,8 @@ SceneTfSystem::CalcSceneTf( if (frame_id == inspector.world_frame_id()) { continue; } - transform.child_frame_id = inspector.GetName(frame_id); + transform.child_frame_id = + utilities::GetTfFrameName(inspector, impl_->plants, frame_id); transform.transform = utilities::ToTransformMsg(query_object.GetPoseInWorld(frame_id)); output_value->transforms.push_back(transform); diff --git a/drake_ros_systems/src/subscription.cpp b/drake_ros_systems/src/subscription.cpp index 585ae430..7641da33 100644 --- a/drake_ros_systems/src/subscription.cpp +++ b/drake_ros_systems/src/subscription.cpp @@ -59,8 +59,9 @@ Subscription::handle_message( std::shared_ptr & message, const rclcpp::MessageInfo & message_info) { - (void) message_info; - callback_(std::static_pointer_cast(message)); + handle_serialized_message( + std::static_pointer_cast(message), + message_info); } void @@ -72,6 +73,15 @@ Subscription::handle_loaned_message( throw std::runtime_error("handle_loaned_message() not supported by drake_ros_systems"); } +void +Subscription::handle_serialized_message( + const std::shared_ptr & message, + const rclcpp::MessageInfo & message_info) +{ + (void)message_info; + callback_(message); +} + void Subscription::return_message(std::shared_ptr & message) { diff --git a/drake_ros_systems/src/subscription.hpp b/drake_ros_systems/src/subscription.hpp index 199834fd..84d4ba8f 100644 --- a/drake_ros_systems/src/subscription.hpp +++ b/drake_ros_systems/src/subscription.hpp @@ -62,6 +62,11 @@ class Subscription final : public rclcpp::SubscriptionBase handle_loaned_message( void * loaned_message, const rclcpp::MessageInfo & message_info) override; + void + handle_serialized_message( + const std::shared_ptr & message, + const rclcpp::MessageInfo & message_info) override; + /// Return the message borrowed in create_message. /** \param[in] message Shared pointer to the returned message. */ void diff --git a/drake_ros_systems/test/test_scene_markers.cpp b/drake_ros_systems/test/test_scene_markers.cpp index 148abd67..09de89de 100644 --- a/drake_ros_systems/test/test_scene_markers.cpp +++ b/drake_ros_systems/test/test_scene_markers.cpp @@ -67,7 +67,7 @@ struct SingleSphereSceneTestDetails EXPECT_EQ(marker.header.frame_id, "world"); EXPECT_EQ(marker.header.stamp.sec, 0); EXPECT_EQ(marker.header.stamp.nanosec, 0u); - EXPECT_EQ(marker.ns, std::string(kSourceName) + "::sphere"); + EXPECT_EQ(marker.ns, kSourceName); EXPECT_EQ(marker.id, 0); EXPECT_EQ(marker.action, visualization_msgs::msg::Marker::MODIFY); EXPECT_EQ(marker.type, visualization_msgs::msg::Marker::SPHERE); @@ -78,7 +78,7 @@ struct SingleSphereSceneTestDetails EXPECT_DOUBLE_EQ(marker.scale.y, kRadius); EXPECT_DOUBLE_EQ(marker.scale.z, kRadius); const drake::geometry::Rgba & default_color = - scene_markers_system->default_color(); + scene_markers_system->params().default_color; EXPECT_NEAR(marker.color.r, default_color.r(), kTolerance); EXPECT_NEAR(marker.color.g, default_color.g(), kTolerance); EXPECT_NEAR(marker.color.b, default_color.b(), kTolerance); @@ -127,7 +127,7 @@ struct SingleEllipsoidSceneTestDetails EXPECT_EQ(marker.header.frame_id, "world"); EXPECT_EQ(marker.header.stamp.sec, 0); EXPECT_EQ(marker.header.stamp.nanosec, 0u); - EXPECT_EQ(marker.ns, std::string(kSourceName) + "::ellipsoid"); + EXPECT_EQ(marker.ns, kSourceName); EXPECT_EQ(marker.id, 0); EXPECT_EQ(marker.action, visualization_msgs::msg::Marker::MODIFY); EXPECT_EQ(marker.type, visualization_msgs::msg::Marker::SPHERE); @@ -138,7 +138,7 @@ struct SingleEllipsoidSceneTestDetails EXPECT_DOUBLE_EQ(marker.scale.y, kLengthB); EXPECT_DOUBLE_EQ(marker.scale.z, kLengthC); const drake::geometry::Rgba & default_color = - scene_markers_system->default_color(); + scene_markers_system->params().default_color; EXPECT_NEAR(marker.color.r, default_color.r(), kTolerance); EXPECT_NEAR(marker.color.g, default_color.g(), kTolerance); EXPECT_NEAR(marker.color.b, default_color.b(), kTolerance); @@ -185,7 +185,7 @@ struct SingleCylinderSceneTestDetails EXPECT_EQ(marker.header.frame_id, "world"); EXPECT_EQ(marker.header.stamp.sec, 0); EXPECT_EQ(marker.header.stamp.nanosec, 0u); - EXPECT_EQ(marker.ns, std::string(kSourceName) + "::cylinder"); + EXPECT_EQ(marker.ns, kSourceName); EXPECT_EQ(marker.id, 0); EXPECT_EQ(marker.action, visualization_msgs::msg::Marker::MODIFY); EXPECT_EQ(marker.type, visualization_msgs::msg::Marker::CYLINDER); @@ -196,7 +196,7 @@ struct SingleCylinderSceneTestDetails EXPECT_DOUBLE_EQ(marker.scale.y, kRadius); EXPECT_DOUBLE_EQ(marker.scale.z, kLength); const drake::geometry::Rgba & default_color = - scene_markers_system->default_color(); + scene_markers_system->params().default_color; EXPECT_NEAR(marker.color.r, default_color.r(), kTolerance); EXPECT_NEAR(marker.color.g, default_color.g(), kTolerance); EXPECT_NEAR(marker.color.b, default_color.b(), kTolerance); @@ -240,7 +240,7 @@ struct SingleHalfSpaceSceneTestDetails EXPECT_EQ(marker.header.frame_id, "world"); EXPECT_EQ(marker.header.stamp.sec, 0); EXPECT_EQ(marker.header.stamp.nanosec, 0u); - EXPECT_EQ(marker.ns, std::string(kSourceName) + "::hspace"); + EXPECT_EQ(marker.ns, kSourceName); EXPECT_EQ(marker.id, 0); EXPECT_EQ(marker.action, visualization_msgs::msg::Marker::MODIFY); EXPECT_EQ(marker.type, visualization_msgs::msg::Marker::CUBE); @@ -250,7 +250,7 @@ struct SingleHalfSpaceSceneTestDetails EXPECT_GT(marker.scale.x, 10.0); EXPECT_GT(marker.scale.y, 10.0); const drake::geometry::Rgba & default_color = - scene_markers_system->default_color(); + scene_markers_system->params().default_color; EXPECT_NEAR(marker.color.r, default_color.r(), kTolerance); EXPECT_NEAR(marker.color.g, default_color.g(), kTolerance); EXPECT_NEAR(marker.color.b, default_color.b(), kTolerance); @@ -291,7 +291,7 @@ struct SingleBoxSceneTestDetails EXPECT_EQ(marker.header.frame_id, "world"); EXPECT_EQ(marker.header.stamp.sec, 0); EXPECT_EQ(marker.header.stamp.nanosec, 0u); - EXPECT_EQ(marker.ns, std::string(kSourceName) + "::box"); + EXPECT_EQ(marker.ns, kSourceName); EXPECT_EQ(marker.id, 0); EXPECT_EQ(marker.action, visualization_msgs::msg::Marker::MODIFY); EXPECT_EQ(marker.type, visualization_msgs::msg::Marker::CUBE); @@ -302,7 +302,7 @@ struct SingleBoxSceneTestDetails EXPECT_DOUBLE_EQ(marker.scale.y, kDepth); EXPECT_DOUBLE_EQ(marker.scale.z, kHeight); const drake::geometry::Rgba & default_color = - scene_markers_system->default_color(); + scene_markers_system->params().default_color; EXPECT_NEAR(marker.color.r, default_color.r(), kTolerance); EXPECT_NEAR(marker.color.g, default_color.g(), kTolerance); EXPECT_NEAR(marker.color.b, default_color.b(), kTolerance); @@ -345,13 +345,13 @@ struct SingleCapsuleSceneTestDetails SceneMarkersSystem * scene_markers_system) { ASSERT_EQ(marker_array.markers.size(), 3u); - const drake::geometry::Rgba & default_color = scene_markers_system->default_color(); + const drake::geometry::Rgba & default_color = scene_markers_system->params().default_color; const visualization_msgs::msg::Marker & body_marker = marker_array.markers[0]; EXPECT_EQ(body_marker.header.frame_id, "world"); EXPECT_EQ(body_marker.header.stamp.sec, 0); EXPECT_EQ(body_marker.header.stamp.nanosec, 0u); - EXPECT_EQ(body_marker.ns, std::string(kSourceName) + "::capsule"); + EXPECT_EQ(body_marker.ns, kSourceName); EXPECT_EQ(body_marker.id, 0); EXPECT_EQ(body_marker.action, visualization_msgs::msg::Marker::MODIFY); EXPECT_EQ(body_marker.type, visualization_msgs::msg::Marker::CYLINDER); @@ -377,7 +377,7 @@ struct SingleCapsuleSceneTestDetails EXPECT_EQ(upper_cap_marker.header.frame_id, "world"); EXPECT_EQ(upper_cap_marker.header.stamp.sec, 0); EXPECT_EQ(upper_cap_marker.header.stamp.nanosec, 0u); - EXPECT_EQ(upper_cap_marker.ns, std::string(kSourceName) + "::capsule"); + EXPECT_EQ(upper_cap_marker.ns, kSourceName); EXPECT_EQ(upper_cap_marker.id, 1); EXPECT_EQ(upper_cap_marker.action, visualization_msgs::msg::Marker::MODIFY); EXPECT_EQ(upper_cap_marker.type, visualization_msgs::msg::Marker::SPHERE); @@ -403,7 +403,7 @@ struct SingleCapsuleSceneTestDetails EXPECT_EQ(lower_cap_marker.header.frame_id, "world"); EXPECT_EQ(lower_cap_marker.header.stamp.sec, 0); EXPECT_EQ(lower_cap_marker.header.stamp.nanosec, 0u); - EXPECT_EQ(lower_cap_marker.ns, std::string(kSourceName) + "::capsule"); + EXPECT_EQ(lower_cap_marker.ns, kSourceName); EXPECT_EQ(lower_cap_marker.id, 2); EXPECT_EQ(lower_cap_marker.action, visualization_msgs::msg::Marker::MODIFY); EXPECT_EQ(lower_cap_marker.type, visualization_msgs::msg::Marker::SPHERE); @@ -460,7 +460,7 @@ struct SingleMeshSceneTestDetails EXPECT_EQ(marker.header.frame_id, "world"); EXPECT_EQ(marker.header.stamp.sec, 0); EXPECT_EQ(marker.header.stamp.nanosec, 0u); - EXPECT_EQ(marker.ns, std::string(kSourceName) + "::mesh"); + EXPECT_EQ(marker.ns, kSourceName); EXPECT_EQ(marker.id, 0); EXPECT_EQ(marker.action, visualization_msgs::msg::Marker::MODIFY); EXPECT_EQ(marker.type, visualization_msgs::msg::Marker::MESH_RESOURCE); @@ -472,7 +472,7 @@ struct SingleMeshSceneTestDetails EXPECT_DOUBLE_EQ(marker.scale.y, kScale); EXPECT_DOUBLE_EQ(marker.scale.z, kScale); const drake::geometry::Rgba & default_color = - scene_markers_system->default_color(); + scene_markers_system->params().default_color; EXPECT_NEAR(marker.color.r, default_color.r(), kTolerance); EXPECT_NEAR(marker.color.g, default_color.g(), kTolerance); EXPECT_NEAR(marker.color.b, default_color.b(), kTolerance); @@ -494,7 +494,7 @@ class SceneMarkersTest : public ::testing::Test TYPED_TEST_SUITE_P(SceneMarkersTest); -TYPED_TEST_P(SceneMarkersTest, nominal) +TYPED_TEST_P(SceneMarkersTest, NominalCase) { using TestDetails = TypeParam; @@ -525,7 +525,7 @@ TYPED_TEST_P(SceneMarkersTest, nominal) TestDetails::CheckSceneMarkers(marker_array, scene_markers); } -REGISTER_TYPED_TEST_SUITE_P(SceneMarkersTest, nominal); +REGISTER_TYPED_TEST_SUITE_P(SceneMarkersTest, NominalCase); using SingleGeometrySceneTestDetails = ::testing::Types< SingleSphereSceneTestDetails, diff --git a/drake_ros_systems/test/test_tf_broadcaster.cpp b/drake_ros_systems/test/test_tf_broadcaster.cpp index 549d6bc0..67d046ee 100644 --- a/drake_ros_systems/test/test_tf_broadcaster.cpp +++ b/drake_ros_systems/test/test_tf_broadcaster.cpp @@ -37,13 +37,14 @@ #include "drake_ros_systems/drake_ros.hpp" #include "drake_ros_systems/ros_interface_system.hpp" -#include "drake_ros_systems/tf_broadcaster_system.hpp" +#include "drake_ros_systems/scene_tf_broadcaster_system.hpp" using drake_ros_systems::DrakeRos; using drake_ros_systems::RosInterfaceSystem; -using drake_ros_systems::TfBroadcasterSystem; +using drake_ros_systems::SceneTfBroadcasterSystem; +using drake_ros_systems::SceneTfBroadcasterParams; -TEST(TfBroadcasting, nominal_case) { +TEST(SceneTfBroadcasting, NominalCase) { drake::systems::DiagramBuilder builder; auto sys_ros_interface = @@ -75,14 +76,13 @@ TEST(TfBroadcasting, nominal_case) { pose_vector_source->get_output_port(), scene_graph->get_source_pose_port(source_id)); - const std::unordered_set - publish_triggers{drake::systems::TriggerType::kForced}; - auto broadcaster = builder.AddSystem( - sys_ros_interface->get_ros_interface(), publish_triggers); + auto scene_tf_broadcaster = builder.AddSystem( + sys_ros_interface->get_ros_interface(), + SceneTfBroadcasterParams{{drake::systems::TriggerType::kForced}, 0.}); builder.Connect( scene_graph->get_query_output_port(), - broadcaster->GetInputPort("graph_query")); + scene_tf_broadcaster->GetInputPort("graph_query")); auto diagram = builder.Build(); auto context = diagram->CreateDefaultContext(); diff --git a/drake_ros_systems/test/test_tf_broadcaster.py b/drake_ros_systems/test/test_tf_broadcaster.py index 9328437b..90330946 100644 --- a/drake_ros_systems/test/test_tf_broadcaster.py +++ b/drake_ros_systems/test/test_tf_broadcaster.py @@ -16,7 +16,8 @@ import numpy as np from drake_ros_systems import RosInterfaceSystem -from drake_ros_systems import TfBroadcasterSystem +from drake_ros_systems import SceneTfBroadcasterSystem +from drake_ros_systems import SceneTfBroadcasterParams from pydrake.common.value import AbstractValue from pydrake.math import RigidTransform @@ -64,13 +65,18 @@ def test_nominal_case(): pose_vector_source.get_output_port(), scene_graph.get_source_pose_port(source_id)) - tf_broadcaster = builder.AddSystem(TfBroadcasterSystem( - sys_ros_interface.get_ros_interface(), - publish_triggers={TriggerType.kForced})) + scene_tf_broadcaster = builder.AddSystem( + SceneTfBroadcasterSystem( + sys_ros_interface.get_ros_interface(), + params=SceneTfBroadcasterParams( + publish_triggers={TriggerType.kForced} + ) + ) + ) builder.Connect( scene_graph.get_query_output_port(), - tf_broadcaster.get_graph_query_port()) + scene_tf_broadcaster.get_graph_query_port()) diagram = builder.Build() context = diagram.CreateDefaultContext() From 06485665c7658366426900473aa6252dc684c303 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Wed, 19 May 2021 09:30:27 -0300 Subject: [PATCH 83/87] Update iiwa_manipulator example Signed-off-by: Michel Hidalgo --- .../iiwa_manipulator/iiwa_manipulator.cpp | 3 + .../iiwa_manipulator/iiwa_manipulator.py | 3 + .../iiwa_manipulator/iiwa_manipulator.rviz | 64 ++++++++++--------- 3 files changed, 41 insertions(+), 29 deletions(-) diff --git a/drake_ros_examples/examples/iiwa_manipulator/iiwa_manipulator.cpp b/drake_ros_examples/examples/iiwa_manipulator/iiwa_manipulator.cpp index 7e5da5e4..f9b8a030 100644 --- a/drake_ros_examples/examples/iiwa_manipulator/iiwa_manipulator.cpp +++ b/drake_ros_examples/examples/iiwa_manipulator/iiwa_manipulator.cpp @@ -81,6 +81,9 @@ int main() auto rviz_visualizer = builder.AddSystem( ros_interface_system->get_ros_interface()); + rviz_visualizer->RegisterMultibodyPlant( + &manipulation_station->get_multibody_plant()); + builder.Connect( manipulation_station->GetOutputPort("query_object"), rviz_visualizer->get_graph_query_port()); diff --git a/drake_ros_examples/examples/iiwa_manipulator/iiwa_manipulator.py b/drake_ros_examples/examples/iiwa_manipulator/iiwa_manipulator.py index e3e57956..87c93ebf 100755 --- a/drake_ros_examples/examples/iiwa_manipulator/iiwa_manipulator.py +++ b/drake_ros_examples/examples/iiwa_manipulator/iiwa_manipulator.py @@ -58,6 +58,9 @@ def main(): rviz_visualizer = builder.AddSystem( RvizVisualizer(ros_interface_system.get_ros_interface())) + rviz_visualizer.RegisterMultibodyPlant( + manipulation_station.get_multibody_plant()) + builder.Connect( manipulation_station.GetOutputPort('query_object'), rviz_visualizer.get_graph_query_port() diff --git a/drake_ros_examples/examples/iiwa_manipulator/iiwa_manipulator.rviz b/drake_ros_examples/examples/iiwa_manipulator/iiwa_manipulator.rviz index 31a9d215..f9bc7e63 100644 --- a/drake_ros_examples/examples/iiwa_manipulator/iiwa_manipulator.rviz +++ b/drake_ros_examples/examples/iiwa_manipulator/iiwa_manipulator.rviz @@ -6,8 +6,10 @@ Panels: Expanded: - /Global Options1 - /Status1 - Splitter Ratio: 0.5 - Tree Height: 555 + - /Scene Visual Geometries1 + - /Scene Visual Geometries1/Namespaces1 + Splitter Ratio: 0.6705882549285889 + Tree Height: 734 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -49,36 +51,40 @@ Visualization Manager: Value: true - Class: rviz_default_plugins/MarkerArray Enabled: true - Name: MarkerArray + Name: Scene Visual Geometries Namespaces: - Source_3::bin1::back: true - Source_3::bin1::bottom: true - Source_3::bin1::left: true - Source_3::bin1::right: true - Source_3::bin1::slope: true - Source_3::bin1::visual: true - Source_3::bin2::back: true - Source_3::bin2::bottom: true - Source_3::bin2::left: true - Source_3::bin2::right: true - Source_3::bin2::slope: true - Source_3::bin2::visual: true - Source_3::gripper::visual: true - Source_3::iiwa::iiwa_link_0_visual: true - Source_3::iiwa::iiwa_link_1_visual: true - Source_3::iiwa::iiwa_link_2_visual: true - Source_3::iiwa::iiwa_link_3_visual: true - Source_3::iiwa::iiwa_link_4_visual: true - Source_3::iiwa::iiwa_link_5_visual: true - Source_3::iiwa::iiwa_link_6_visual: true - Source_3::iiwa::iiwa_link_7_visual: true + /bin1/bin_base: true + /bin2/bin_base: true + /gripper/body: true + /gripper/left_finger: true + /gripper/right_finger: true + /iiwa/iiwa_link_0: true + /iiwa/iiwa_link_1: true + /iiwa/iiwa_link_2: true + /iiwa/iiwa_link_3: true + /iiwa/iiwa_link_4: true + /iiwa/iiwa_link_5: true + /iiwa/iiwa_link_6: true + /iiwa/iiwa_link_7: true Topic: Depth: 5 Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /scene_markers + Value: /scene_markers/visual Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Scene Collision Geometries + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /scene_markers/collision + Value: false Enabled: true Global Options: Background Color: 48; 48; 48 @@ -148,10 +154,10 @@ Visualization Manager: Window Geometry: Displays: collapsed: false - Height: 846 + Height: 1025 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd000000040000000000000156000002b4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000002b4000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000002b4000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000024400fffffffb0000000800540069006d006501000000000000045000000000000000000000023f000002b400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000400000000000001c200000367fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b00000367000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000367fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b00000367000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073d0000003efc0100000002fb0000000800540069006d006501000000000000073d0000024400fffffffb0000000800540069006d00650100000000000004500000000000000000000004600000036700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -160,6 +166,6 @@ Window Geometry: collapsed: false Views: collapsed: false - Width: 1200 + Width: 1853 X: 67 - Y: 60 + Y: 27 From 07c0906ed7b24b10ac734bf81a814e7736fa4ae7 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Wed, 19 May 2021 09:31:03 -0300 Subject: [PATCH 84/87] Please uncrustify Signed-off-by: Michel Hidalgo --- .../include/drake_ros_systems/scene_markers_system.hpp | 10 ++++++---- .../include/drake_ros_systems/serializer.hpp | 1 - drake_ros_systems/src/rviz_visualizer.cpp | 4 ++-- 3 files changed, 8 insertions(+), 7 deletions(-) diff --git a/drake_ros_systems/include/drake_ros_systems/scene_markers_system.hpp b/drake_ros_systems/include/drake_ros_systems/scene_markers_system.hpp index 15be1434..bee7178d 100644 --- a/drake_ros_systems/include/drake_ros_systems/scene_markers_system.hpp +++ b/drake_ros_systems/include/drake_ros_systems/scene_markers_system.hpp @@ -33,10 +33,11 @@ namespace drake_ros_systems struct SceneMarkersParams { /// Configure SceneMarkersSystem to depict illustration geometries. - static SceneMarkersParams Illustration() { return {}; } + static SceneMarkersParams Illustration() {return {};} /// Configure SceneMarkersSystem to depict proximity geometries. - static SceneMarkersParams Proximity() { + static SceneMarkersParams Proximity() + { SceneMarkersParams params; params.role = drake::geometry::Role::kProximity; params.default_color.set(0.5, 0.5, 0.5, 1.0); @@ -105,8 +106,9 @@ class SceneMarkersSystem : public drake::systems::LeafSystem // which is invalidated (and thus recomputed) upon a SceneGraph // geometry version change. const visualization_msgs::msg::MarkerArray & - EvalSceneMarkers(const drake::systems::Context & context, - bool * cached = nullptr) const; + EvalSceneMarkers( + const drake::systems::Context & context, + bool * cached = nullptr) const; // Inspects the SceneGraph and carries out the conversion // to visualization_msgs::msg::MarkerArray message unconditionally. diff --git a/drake_ros_systems/include/drake_ros_systems/serializer.hpp b/drake_ros_systems/include/drake_ros_systems/serializer.hpp index 8704e452..a308f12f 100644 --- a/drake_ros_systems/include/drake_ros_systems/serializer.hpp +++ b/drake_ros_systems/include/drake_ros_systems/serializer.hpp @@ -28,7 +28,6 @@ #include "drake_ros_systems/serializer_interface.hpp" - namespace drake_ros_systems { template diff --git a/drake_ros_systems/src/rviz_visualizer.cpp b/drake_ros_systems/src/rviz_visualizer.cpp index 852affc0..2948fbfc 100644 --- a/drake_ros_systems/src/rviz_visualizer.cpp +++ b/drake_ros_systems/src/rviz_visualizer.cpp @@ -54,7 +54,7 @@ RvizVisualizer::RvizVisualizer( impl_->scene_visual_markers = builder.AddSystem( - SceneMarkersParams::Illustration()); + SceneMarkersParams::Illustration()); builder.Connect( impl_->scene_visual_markers->get_markers_output_port(), @@ -70,7 +70,7 @@ RvizVisualizer::RvizVisualizer( impl_->scene_collision_markers = builder.AddSystem( - SceneMarkersParams::Proximity()); + SceneMarkersParams::Proximity()); builder.Connect( impl_->scene_collision_markers->get_markers_output_port(), From e3805010198d79912488e7c37ec057ec9e7dc976 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Wed, 19 May 2021 09:50:08 -0300 Subject: [PATCH 85/87] Tweak name conventions Signed-off-by: Michel Hidalgo --- .../include/drake_ros_systems/scene_markers_system.hpp | 6 +++--- .../drake_ros_systems/utilities/name_conventions.hpp | 2 +- drake_ros_systems/src/scene_markers_system.cpp | 4 ++-- drake_ros_systems/src/utilities/name_conventions.cpp | 3 +-- 4 files changed, 7 insertions(+), 8 deletions(-) diff --git a/drake_ros_systems/include/drake_ros_systems/scene_markers_system.hpp b/drake_ros_systems/include/drake_ros_systems/scene_markers_system.hpp index bee7178d..476f8a69 100644 --- a/drake_ros_systems/include/drake_ros_systems/scene_markers_system.hpp +++ b/drake_ros_systems/include/drake_ros_systems/scene_markers_system.hpp @@ -44,9 +44,9 @@ struct SceneMarkersParams return params; } - /// Optional marker namespace prefix, defaults to - /// utilities::GetMarkerNamespacePrefix() outcome. - std::optional marker_namespace_prefix{std::nullopt}; + /// Optional namespace for all markers, defaults + /// to utilities::GetMarkerNamespace() outcome. + std::optional marker_namespace{std::nullopt}; /// Role of the geometries to depict. drake::geometry::Role role{drake::geometry::Role::kIllustration}; diff --git a/drake_ros_systems/include/drake_ros_systems/utilities/name_conventions.hpp b/drake_ros_systems/include/drake_ros_systems/utilities/name_conventions.hpp index 92e3a9bd..3bfb0adb 100644 --- a/drake_ros_systems/include/drake_ros_systems/utilities/name_conventions.hpp +++ b/drake_ros_systems/include/drake_ros_systems/utilities/name_conventions.hpp @@ -39,7 +39,7 @@ GetTfFrameName( const drake::geometry::GeometryId & geometry_id); std::string -GetMarkerNamespacePrefix( +GetMarkerNamespace( const drake::geometry::SceneGraphInspector & inspector, const std::unordered_set *> & plants, const drake::geometry::GeometryId & geometry_id); diff --git a/drake_ros_systems/src/scene_markers_system.cpp b/drake_ros_systems/src/scene_markers_system.cpp index 3bfd75b5..22d1504f 100644 --- a/drake_ros_systems/src/scene_markers_system.cpp +++ b/drake_ros_systems/src/scene_markers_system.cpp @@ -67,8 +67,8 @@ class SceneGeometryToMarkers : public drake::geometry::ShapeReifier prototype_marker_.header.frame_id = utilities::GetTfFrameName(inspector, plants, geometry_id); - prototype_marker_.ns = params_.marker_namespace_prefix.value_or( - utilities::GetMarkerNamespacePrefix(inspector, plants, geometry_id)); + prototype_marker_.ns = params_.marker_namespace.value_or( + utilities::GetMarkerNamespace(inspector, plants, geometry_id)); prototype_marker_.id = marker_array_->markers.size(); prototype_marker_.action = visualization_msgs::msg::Marker::MODIFY; diff --git a/drake_ros_systems/src/utilities/name_conventions.cpp b/drake_ros_systems/src/utilities/name_conventions.cpp index fde71e99..9031e817 100644 --- a/drake_ros_systems/src/utilities/name_conventions.cpp +++ b/drake_ros_systems/src/utilities/name_conventions.cpp @@ -50,7 +50,6 @@ GetTfFrameName( const drake::geometry::FrameId & frame_id) { std::stringstream ss; - ss << "/"; for (auto * plant : plants) { const drake::multibody::Body * body = plant->GetBodyFromFrameId(frame_id); @@ -87,7 +86,7 @@ GetTfFrameName( } std::string -GetMarkerNamespacePrefix( +GetMarkerNamespace( const drake::geometry::SceneGraphInspector & inspector, const std::unordered_set *> & plants, const drake::geometry::GeometryId & geometry_id) From 7e760a7e8bbd105386bb969139a6883beef570e5 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Wed, 19 May 2021 09:50:29 -0300 Subject: [PATCH 86/87] Update SceneMarkersSystem test Signed-off-by: Michel Hidalgo --- drake_ros_systems/test/test_scene_markers.cpp | 101 ++++++++++++------ 1 file changed, 69 insertions(+), 32 deletions(-) diff --git a/drake_ros_systems/test/test_scene_markers.cpp b/drake_ros_systems/test/test_scene_markers.cpp index 09de89de..cc9efcec 100644 --- a/drake_ros_systems/test/test_scene_markers.cpp +++ b/drake_ros_systems/test/test_scene_markers.cpp @@ -38,6 +38,7 @@ static constexpr double kTolerance{1e-6}; struct SingleSphereSceneTestDetails { + static constexpr char kName[] = "sphere"; static constexpr double kRadius{1.}; static @@ -50,7 +51,7 @@ struct SingleSphereSceneTestDetails scene_graph->RegisterAnchoredGeometry( source_id, std::make_unique( drake::math::RigidTransform::Identity(), - std::make_unique(kRadius), "sphere")); + std::make_unique(kRadius), kName)); scene_graph->AssignRole( source_id, geometry_id, drake::geometry::IllustrationProperties()); return {}; @@ -62,12 +63,16 @@ struct SingleSphereSceneTestDetails const visualization_msgs::msg::MarkerArray & marker_array, SceneMarkersSystem * scene_markers_system) { - ASSERT_EQ(marker_array.markers.size(), 1u); - const visualization_msgs::msg::Marker & marker = marker_array.markers[0]; + ASSERT_EQ(marker_array.markers.size(), 2u); + const visualization_msgs::msg::Marker & control_marker = marker_array.markers[0]; + EXPECT_EQ(control_marker.action, visualization_msgs::msg::Marker::DELETEALL); + const visualization_msgs::msg::Marker & marker = marker_array.markers[1]; EXPECT_EQ(marker.header.frame_id, "world"); EXPECT_EQ(marker.header.stamp.sec, 0); EXPECT_EQ(marker.header.stamp.nanosec, 0u); - EXPECT_EQ(marker.ns, kSourceName); + std::stringstream ss; + ss << "/" << kSourceName << "/" << kName; + EXPECT_EQ(marker.ns, ss.str()); EXPECT_EQ(marker.id, 0); EXPECT_EQ(marker.action, visualization_msgs::msg::Marker::MODIFY); EXPECT_EQ(marker.type, visualization_msgs::msg::Marker::SPHERE); @@ -95,6 +100,7 @@ struct SingleSphereSceneTestDetails struct SingleEllipsoidSceneTestDetails { + static constexpr char kName[] = "ellipsoid"; static constexpr double kLengthA{0.3}; static constexpr double kLengthB{0.4}; static constexpr double kLengthC{0.5}; @@ -110,7 +116,7 @@ struct SingleEllipsoidSceneTestDetails source_id, std::make_unique( drake::math::RigidTransform::Identity(), std::make_unique( - kLengthA, kLengthB, kLengthC), "ellipsoid")); + kLengthA, kLengthB, kLengthC), kName)); scene_graph->AssignRole( source_id, geometry_id, drake::geometry::IllustrationProperties()); return {}; @@ -122,12 +128,16 @@ struct SingleEllipsoidSceneTestDetails const visualization_msgs::msg::MarkerArray & marker_array, SceneMarkersSystem * scene_markers_system) { - ASSERT_EQ(marker_array.markers.size(), 1u); - const visualization_msgs::msg::Marker & marker = marker_array.markers[0]; + ASSERT_EQ(marker_array.markers.size(), 2u); + const visualization_msgs::msg::Marker & control_marker = marker_array.markers[0]; + EXPECT_EQ(control_marker.action, visualization_msgs::msg::Marker::DELETEALL); + const visualization_msgs::msg::Marker & marker = marker_array.markers[1]; EXPECT_EQ(marker.header.frame_id, "world"); EXPECT_EQ(marker.header.stamp.sec, 0); EXPECT_EQ(marker.header.stamp.nanosec, 0u); - EXPECT_EQ(marker.ns, kSourceName); + std::stringstream ss; + ss << "/" << kSourceName << "/" << kName; + EXPECT_EQ(marker.ns, ss.str()); EXPECT_EQ(marker.id, 0); EXPECT_EQ(marker.action, visualization_msgs::msg::Marker::MODIFY); EXPECT_EQ(marker.type, visualization_msgs::msg::Marker::SPHERE); @@ -155,6 +165,7 @@ struct SingleEllipsoidSceneTestDetails struct SingleCylinderSceneTestDetails { + static constexpr char kName[] = "cylinder"; static constexpr double kRadius{0.5}; static constexpr double kLength{1.0}; @@ -168,7 +179,7 @@ struct SingleCylinderSceneTestDetails scene_graph->RegisterAnchoredGeometry( source_id, std::make_unique( drake::math::RigidTransform::Identity(), - std::make_unique(kRadius, kLength), "cylinder")); + std::make_unique(kRadius, kLength), kName)); scene_graph->AssignRole( source_id, geometry_id, drake::geometry::IllustrationProperties()); return {}; @@ -180,12 +191,16 @@ struct SingleCylinderSceneTestDetails const visualization_msgs::msg::MarkerArray & marker_array, SceneMarkersSystem * scene_markers_system) { - ASSERT_EQ(marker_array.markers.size(), 1u); - const visualization_msgs::msg::Marker & marker = marker_array.markers[0]; + ASSERT_EQ(marker_array.markers.size(), 2u); + const visualization_msgs::msg::Marker & control_marker = marker_array.markers[0]; + EXPECT_EQ(control_marker.action, visualization_msgs::msg::Marker::DELETEALL); + const visualization_msgs::msg::Marker & marker = marker_array.markers[1]; EXPECT_EQ(marker.header.frame_id, "world"); EXPECT_EQ(marker.header.stamp.sec, 0); EXPECT_EQ(marker.header.stamp.nanosec, 0u); - EXPECT_EQ(marker.ns, kSourceName); + std::stringstream ss; + ss << "/" << kSourceName << "/" << kName; + EXPECT_EQ(marker.ns, ss.str()); EXPECT_EQ(marker.id, 0); EXPECT_EQ(marker.action, visualization_msgs::msg::Marker::MODIFY); EXPECT_EQ(marker.type, visualization_msgs::msg::Marker::CYLINDER); @@ -213,6 +228,8 @@ struct SingleCylinderSceneTestDetails struct SingleHalfSpaceSceneTestDetails { + static constexpr char kName[] = "hspace"; + static drake::geometry::FramePoseVector PopulateSceneGraph( @@ -223,7 +240,7 @@ struct SingleHalfSpaceSceneTestDetails scene_graph->RegisterAnchoredGeometry( source_id, std::make_unique( drake::math::RigidTransform::Identity(), - std::make_unique(), "hspace")); + std::make_unique(), kName)); scene_graph->AssignRole( source_id, geometry_id, drake::geometry::IllustrationProperties()); return {}; @@ -235,12 +252,16 @@ struct SingleHalfSpaceSceneTestDetails const visualization_msgs::msg::MarkerArray & marker_array, SceneMarkersSystem * scene_markers_system) { - ASSERT_EQ(marker_array.markers.size(), 1u); - const visualization_msgs::msg::Marker & marker = marker_array.markers[0]; + ASSERT_EQ(marker_array.markers.size(), 2u); + const visualization_msgs::msg::Marker & control_marker = marker_array.markers[0]; + EXPECT_EQ(control_marker.action, visualization_msgs::msg::Marker::DELETEALL); + const visualization_msgs::msg::Marker & marker = marker_array.markers[1]; EXPECT_EQ(marker.header.frame_id, "world"); EXPECT_EQ(marker.header.stamp.sec, 0); EXPECT_EQ(marker.header.stamp.nanosec, 0u); - EXPECT_EQ(marker.ns, kSourceName); + std::stringstream ss; + ss << "/" << kSourceName << "/" << kName; + EXPECT_EQ(marker.ns, ss.str()); EXPECT_EQ(marker.id, 0); EXPECT_EQ(marker.action, visualization_msgs::msg::Marker::MODIFY); EXPECT_EQ(marker.type, visualization_msgs::msg::Marker::CUBE); @@ -260,6 +281,7 @@ struct SingleHalfSpaceSceneTestDetails struct SingleBoxSceneTestDetails { + static constexpr char kName[] = "box"; static constexpr double kWidth{0.5}; static constexpr double kDepth{0.25}; static constexpr double kHeight{1.0}; @@ -274,7 +296,7 @@ struct SingleBoxSceneTestDetails scene_graph->RegisterAnchoredGeometry( source_id, std::make_unique( drake::math::RigidTransform::Identity(), - std::make_unique(kWidth, kDepth, kHeight), "box")); + std::make_unique(kWidth, kDepth, kHeight), kName)); scene_graph->AssignRole( source_id, geometry_id, drake::geometry::IllustrationProperties()); return {}; @@ -286,12 +308,16 @@ struct SingleBoxSceneTestDetails const visualization_msgs::msg::MarkerArray & marker_array, SceneMarkersSystem * scene_markers_system) { - ASSERT_EQ(marker_array.markers.size(), 1u); - const visualization_msgs::msg::Marker & marker = marker_array.markers[0]; + ASSERT_EQ(marker_array.markers.size(), 2u); + const visualization_msgs::msg::Marker & control_marker = marker_array.markers[0]; + EXPECT_EQ(control_marker.action, visualization_msgs::msg::Marker::DELETEALL); + const visualization_msgs::msg::Marker & marker = marker_array.markers[1]; EXPECT_EQ(marker.header.frame_id, "world"); EXPECT_EQ(marker.header.stamp.sec, 0); EXPECT_EQ(marker.header.stamp.nanosec, 0u); - EXPECT_EQ(marker.ns, kSourceName); + std::stringstream ss; + ss << "/" << kSourceName << "/" << kName; + EXPECT_EQ(marker.ns, ss.str()); EXPECT_EQ(marker.id, 0); EXPECT_EQ(marker.action, visualization_msgs::msg::Marker::MODIFY); EXPECT_EQ(marker.type, visualization_msgs::msg::Marker::CUBE); @@ -319,6 +345,7 @@ struct SingleBoxSceneTestDetails struct SingleCapsuleSceneTestDetails { + static constexpr char kName[] = "capsule"; static constexpr double kRadius{0.25}; static constexpr double kLength{0.5}; @@ -332,7 +359,7 @@ struct SingleCapsuleSceneTestDetails scene_graph->RegisterAnchoredGeometry( source_id, std::make_unique( drake::math::RigidTransform::Identity(), - std::make_unique(kRadius, kLength), "capsule")); + std::make_unique(kRadius, kLength), kName)); scene_graph->AssignRole( source_id, geometry_id, drake::geometry::IllustrationProperties()); return {}; @@ -344,14 +371,19 @@ struct SingleCapsuleSceneTestDetails const visualization_msgs::msg::MarkerArray & marker_array, SceneMarkersSystem * scene_markers_system) { - ASSERT_EQ(marker_array.markers.size(), 3u); + ASSERT_EQ(marker_array.markers.size(), 4u); + const visualization_msgs::msg::Marker & control_marker = marker_array.markers[0]; + EXPECT_EQ(control_marker.action, visualization_msgs::msg::Marker::DELETEALL); + const drake::geometry::Rgba & default_color = scene_markers_system->params().default_color; - const visualization_msgs::msg::Marker & body_marker = marker_array.markers[0]; + const visualization_msgs::msg::Marker & body_marker = marker_array.markers[1]; EXPECT_EQ(body_marker.header.frame_id, "world"); EXPECT_EQ(body_marker.header.stamp.sec, 0); EXPECT_EQ(body_marker.header.stamp.nanosec, 0u); - EXPECT_EQ(body_marker.ns, kSourceName); + std::stringstream ss; + ss << "/" << kSourceName << "/" << kName; + EXPECT_EQ(body_marker.ns, ss.str()); EXPECT_EQ(body_marker.id, 0); EXPECT_EQ(body_marker.action, visualization_msgs::msg::Marker::MODIFY); EXPECT_EQ(body_marker.type, visualization_msgs::msg::Marker::CYLINDER); @@ -373,11 +405,11 @@ struct SingleCapsuleSceneTestDetails EXPECT_DOUBLE_EQ(body_marker.pose.orientation.z, 0.); EXPECT_DOUBLE_EQ(body_marker.pose.orientation.w, 1.); - const visualization_msgs::msg::Marker & upper_cap_marker = marker_array.markers[1]; + const visualization_msgs::msg::Marker & upper_cap_marker = marker_array.markers[2]; EXPECT_EQ(upper_cap_marker.header.frame_id, "world"); EXPECT_EQ(upper_cap_marker.header.stamp.sec, 0); EXPECT_EQ(upper_cap_marker.header.stamp.nanosec, 0u); - EXPECT_EQ(upper_cap_marker.ns, kSourceName); + EXPECT_EQ(upper_cap_marker.ns, ss.str()); EXPECT_EQ(upper_cap_marker.id, 1); EXPECT_EQ(upper_cap_marker.action, visualization_msgs::msg::Marker::MODIFY); EXPECT_EQ(upper_cap_marker.type, visualization_msgs::msg::Marker::SPHERE); @@ -399,11 +431,11 @@ struct SingleCapsuleSceneTestDetails EXPECT_DOUBLE_EQ(upper_cap_marker.pose.orientation.z, 0.); EXPECT_DOUBLE_EQ(upper_cap_marker.pose.orientation.w, 1.); - const visualization_msgs::msg::Marker & lower_cap_marker = marker_array.markers[2]; + const visualization_msgs::msg::Marker & lower_cap_marker = marker_array.markers[3]; EXPECT_EQ(lower_cap_marker.header.frame_id, "world"); EXPECT_EQ(lower_cap_marker.header.stamp.sec, 0); EXPECT_EQ(lower_cap_marker.header.stamp.nanosec, 0u); - EXPECT_EQ(lower_cap_marker.ns, kSourceName); + EXPECT_EQ(lower_cap_marker.ns, ss.str()); EXPECT_EQ(lower_cap_marker.id, 2); EXPECT_EQ(lower_cap_marker.action, visualization_msgs::msg::Marker::MODIFY); EXPECT_EQ(lower_cap_marker.type, visualization_msgs::msg::Marker::SPHERE); @@ -430,6 +462,7 @@ struct SingleCapsuleSceneTestDetails template struct SingleMeshSceneTestDetails { + static constexpr char kName[] = "mesh"; static constexpr char kFilename[] = "/tmp/dummy.obj"; static constexpr double kScale{0.1}; @@ -443,7 +476,7 @@ struct SingleMeshSceneTestDetails scene_graph->RegisterAnchoredGeometry( source_id, std::make_unique( drake::math::RigidTransform::Identity(), - std::make_unique(kFilename, kScale), "mesh")); + std::make_unique(kFilename, kScale), kName)); scene_graph->AssignRole( source_id, geometry_id, drake::geometry::IllustrationProperties()); return {}; @@ -455,12 +488,16 @@ struct SingleMeshSceneTestDetails const visualization_msgs::msg::MarkerArray & marker_array, SceneMarkersSystem * scene_markers_system) { - ASSERT_EQ(marker_array.markers.size(), 1u); - const visualization_msgs::msg::Marker & marker = marker_array.markers[0]; + ASSERT_EQ(marker_array.markers.size(), 2u); + const visualization_msgs::msg::Marker & control_marker = marker_array.markers[0]; + EXPECT_EQ(control_marker.action, visualization_msgs::msg::Marker::DELETEALL); + const visualization_msgs::msg::Marker & marker = marker_array.markers[1]; EXPECT_EQ(marker.header.frame_id, "world"); EXPECT_EQ(marker.header.stamp.sec, 0); EXPECT_EQ(marker.header.stamp.nanosec, 0u); - EXPECT_EQ(marker.ns, kSourceName); + std::stringstream ss; + ss << "/" << kSourceName << "/" << kName; + EXPECT_EQ(marker.ns, ss.str()); EXPECT_EQ(marker.id, 0); EXPECT_EQ(marker.action, visualization_msgs::msg::Marker::MODIFY); EXPECT_EQ(marker.type, visualization_msgs::msg::Marker::MESH_RESOURCE); From f01c6c3c6326c193e3fd0f174ba55b4d371f59cd Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Wed, 19 May 2021 10:39:50 -0300 Subject: [PATCH 87/87] Fix visualization_msgs/msg/Marker scales. Scale of a sphere marker is its diameter, not its radius. Signed-off-by: Michel Hidalgo --- .../src/scene_markers_system.cpp | 29 ++++++++------- drake_ros_systems/test/test_scene_markers.cpp | 35 ++++++++++--------- 2 files changed, 35 insertions(+), 29 deletions(-) diff --git a/drake_ros_systems/src/scene_markers_system.cpp b/drake_ros_systems/src/scene_markers_system.cpp index 22d1504f..f142d762 100644 --- a/drake_ros_systems/src/scene_markers_system.cpp +++ b/drake_ros_systems/src/scene_markers_system.cpp @@ -108,9 +108,10 @@ class SceneGeometryToMarkers : public drake::geometry::ShapeReifier visualization_msgs::msg::Marker & marker = marker_array_->markers.back(); marker.type = visualization_msgs::msg::Marker::SPHERE; - marker.scale.x = sphere.radius(); - marker.scale.y = sphere.radius(); - marker.scale.z = sphere.radius(); + const double diameter = 2. * sphere.radius(); + marker.scale.x = diameter; + marker.scale.y = diameter; + marker.scale.z = diameter; marker.pose = utilities::ToPoseMsg(X_FG_); } @@ -120,9 +121,9 @@ class SceneGeometryToMarkers : public drake::geometry::ShapeReifier visualization_msgs::msg::Marker & marker = marker_array_->markers.back(); marker.type = visualization_msgs::msg::Marker::SPHERE; - marker.scale.x = ellipsoid.a(); - marker.scale.y = ellipsoid.b(); - marker.scale.z = ellipsoid.c(); + marker.scale.x = 2. * ellipsoid.a(); + marker.scale.y = 2. * ellipsoid.b(); + marker.scale.z = 2. * ellipsoid.c(); marker.pose = utilities::ToPoseMsg(X_FG_); } @@ -132,8 +133,9 @@ class SceneGeometryToMarkers : public drake::geometry::ShapeReifier visualization_msgs::msg::Marker & marker = marker_array_->markers.back(); marker.type = visualization_msgs::msg::Marker::CYLINDER; - marker.scale.x = cylinder.radius(); - marker.scale.y = cylinder.radius(); + const double diameter = 2. * cylinder.radius(); + marker.scale.x = diameter; + marker.scale.y = diameter; marker.scale.z = cylinder.length(); marker.pose = utilities::ToPoseMsg(X_FG_); } @@ -173,8 +175,9 @@ class SceneGeometryToMarkers : public drake::geometry::ShapeReifier visualization_msgs::msg::Marker & body_marker = marker_array_->markers.back(); body_marker.type = visualization_msgs::msg::Marker::CYLINDER; - body_marker.scale.x = capsule.radius(); - body_marker.scale.y = capsule.radius(); + const double diameter = 2. * capsule.radius(); + body_marker.scale.x = diameter; + body_marker.scale.y = diameter; body_marker.scale.z = capsule.length(); body_marker.pose = utilities::ToPoseMsg(X_FG_); @@ -183,9 +186,9 @@ class SceneGeometryToMarkers : public drake::geometry::ShapeReifier visualization_msgs::msg::Marker & upper_cap_marker = marker_array_->markers.back(); upper_cap_marker.id = body_marker.id + 1; upper_cap_marker.type = visualization_msgs::msg::Marker::SPHERE; - upper_cap_marker.scale.x = capsule.radius(); - upper_cap_marker.scale.y = capsule.radius(); - upper_cap_marker.scale.z = capsule.radius(); + upper_cap_marker.scale.x = diameter; + upper_cap_marker.scale.y = diameter; + upper_cap_marker.scale.z = diameter; const drake::math::RigidTransform X_GU{ drake::Vector3{0., 0., capsule.length() / 2.}}; const drake::math::RigidTransform X_FU = X_FG_ * X_GU; diff --git a/drake_ros_systems/test/test_scene_markers.cpp b/drake_ros_systems/test/test_scene_markers.cpp index cc9efcec..be133b3d 100644 --- a/drake_ros_systems/test/test_scene_markers.cpp +++ b/drake_ros_systems/test/test_scene_markers.cpp @@ -79,9 +79,10 @@ struct SingleSphereSceneTestDetails EXPECT_EQ(marker.lifetime.sec, 0); EXPECT_EQ(marker.lifetime.nanosec, 0u); EXPECT_TRUE(marker.frame_locked); - EXPECT_DOUBLE_EQ(marker.scale.x, kRadius); - EXPECT_DOUBLE_EQ(marker.scale.y, kRadius); - EXPECT_DOUBLE_EQ(marker.scale.z, kRadius); + constexpr double kDiameter = 2. * kRadius; + EXPECT_DOUBLE_EQ(marker.scale.x, kDiameter); + EXPECT_DOUBLE_EQ(marker.scale.y, kDiameter); + EXPECT_DOUBLE_EQ(marker.scale.z, kDiameter); const drake::geometry::Rgba & default_color = scene_markers_system->params().default_color; EXPECT_NEAR(marker.color.r, default_color.r(), kTolerance); @@ -144,9 +145,9 @@ struct SingleEllipsoidSceneTestDetails EXPECT_EQ(marker.lifetime.sec, 0); EXPECT_EQ(marker.lifetime.nanosec, 0u); EXPECT_TRUE(marker.frame_locked); - EXPECT_DOUBLE_EQ(marker.scale.x, kLengthA); - EXPECT_DOUBLE_EQ(marker.scale.y, kLengthB); - EXPECT_DOUBLE_EQ(marker.scale.z, kLengthC); + EXPECT_DOUBLE_EQ(marker.scale.x, 2. * kLengthA); + EXPECT_DOUBLE_EQ(marker.scale.y, 2. * kLengthB); + EXPECT_DOUBLE_EQ(marker.scale.z, 2. * kLengthC); const drake::geometry::Rgba & default_color = scene_markers_system->params().default_color; EXPECT_NEAR(marker.color.r, default_color.r(), kTolerance); @@ -207,8 +208,9 @@ struct SingleCylinderSceneTestDetails EXPECT_EQ(marker.lifetime.sec, 0); EXPECT_EQ(marker.lifetime.nanosec, 0u); EXPECT_TRUE(marker.frame_locked); - EXPECT_DOUBLE_EQ(marker.scale.x, kRadius); - EXPECT_DOUBLE_EQ(marker.scale.y, kRadius); + constexpr double kDiameter = 2. * kRadius; + EXPECT_DOUBLE_EQ(marker.scale.x, kDiameter); + EXPECT_DOUBLE_EQ(marker.scale.y, kDiameter); EXPECT_DOUBLE_EQ(marker.scale.z, kLength); const drake::geometry::Rgba & default_color = scene_markers_system->params().default_color; @@ -390,8 +392,9 @@ struct SingleCapsuleSceneTestDetails EXPECT_EQ(body_marker.lifetime.sec, 0); EXPECT_EQ(body_marker.lifetime.nanosec, 0u); EXPECT_TRUE(body_marker.frame_locked); - EXPECT_DOUBLE_EQ(body_marker.scale.x, kRadius); - EXPECT_DOUBLE_EQ(body_marker.scale.y, kRadius); + constexpr double kDiameter = 2. * kRadius; + EXPECT_DOUBLE_EQ(body_marker.scale.x, kDiameter); + EXPECT_DOUBLE_EQ(body_marker.scale.y, kDiameter); EXPECT_DOUBLE_EQ(body_marker.scale.z, kLength); EXPECT_NEAR(body_marker.color.r, default_color.r(), kTolerance); EXPECT_NEAR(body_marker.color.g, default_color.g(), kTolerance); @@ -416,9 +419,9 @@ struct SingleCapsuleSceneTestDetails EXPECT_EQ(upper_cap_marker.lifetime.sec, 0); EXPECT_EQ(upper_cap_marker.lifetime.nanosec, 0u); EXPECT_TRUE(upper_cap_marker.frame_locked); - EXPECT_DOUBLE_EQ(upper_cap_marker.scale.x, kRadius); - EXPECT_DOUBLE_EQ(upper_cap_marker.scale.y, kRadius); - EXPECT_DOUBLE_EQ(upper_cap_marker.scale.z, kRadius); + EXPECT_DOUBLE_EQ(upper_cap_marker.scale.x, kDiameter); + EXPECT_DOUBLE_EQ(upper_cap_marker.scale.y, kDiameter); + EXPECT_DOUBLE_EQ(upper_cap_marker.scale.z, kDiameter); EXPECT_NEAR(upper_cap_marker.color.r, default_color.r(), kTolerance); EXPECT_NEAR(upper_cap_marker.color.g, default_color.g(), kTolerance); EXPECT_NEAR(upper_cap_marker.color.b, default_color.b(), kTolerance); @@ -442,9 +445,9 @@ struct SingleCapsuleSceneTestDetails EXPECT_EQ(lower_cap_marker.lifetime.sec, 0); EXPECT_EQ(lower_cap_marker.lifetime.nanosec, 0u); EXPECT_TRUE(lower_cap_marker.frame_locked); - EXPECT_DOUBLE_EQ(lower_cap_marker.scale.x, kRadius); - EXPECT_DOUBLE_EQ(lower_cap_marker.scale.y, kRadius); - EXPECT_DOUBLE_EQ(lower_cap_marker.scale.z, kRadius); + EXPECT_DOUBLE_EQ(lower_cap_marker.scale.x, kDiameter); + EXPECT_DOUBLE_EQ(lower_cap_marker.scale.y, kDiameter); + EXPECT_DOUBLE_EQ(lower_cap_marker.scale.z, kDiameter); EXPECT_NEAR(lower_cap_marker.color.r, default_color.r(), kTolerance); EXPECT_NEAR(lower_cap_marker.color.g, default_color.g(), kTolerance); EXPECT_NEAR(lower_cap_marker.color.b, default_color.b(), kTolerance);