From 1083ac4b2f1810698453e73da1402db5c80df671 Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Mon, 21 Feb 2022 11:50:54 +0800 Subject: [PATCH] Fixing up inconsistencies in file extensions, rebase errors and documentation Signed-off-by: Aaron Chong --- drake_ros_core/README.md | 10 +- drake_ros_core/package.xml | 1 - .../python_bindings/module_drake_ros_core.cpp | 93 ------ .../src/python_bindings/py_serializer.hpp | 170 ----------- .../src/python_bindings/qos_type_caster.hpp | 267 ------------------ drake_ros_tf2/README.md | 4 +- .../scene_tf_broadcaster_system.hpp | 65 ----- .../include/drake_ros_tf2/scene_tf_system.hpp | 65 ----- .../utilities/name_conventions.hpp | 41 --- .../utilities/type_conversion.hpp | 30 -- .../python_bindings/module_drake_ros_tf2.cpp | 65 ----- .../src/scene_tf_broadcaster_system.cpp | 69 ----- drake_ros_tf2/src/scene_tf_system.cpp | 95 ------- .../src/utilities/name_conventions.cpp | 79 ------ .../src/utilities/type_conversion.cpp | 42 --- drake_ros_tf2/test/test_tf_broadcaster.cpp | 134 --------- drake_ros_tf2/test/test_type_conversion.cpp | 34 --- drake_ros_viz/.clang-format | 2 +- drake_ros_viz/CMakeLists.txt | 15 +- drake_ros_viz/CPPLINT.cfg | 6 + drake_ros_viz/README.md | 12 +- ...ame_conventions.hpp => name_conventions.h} | 15 +- ...{type_conversion.hpp => type_conversion.h} | 5 +- drake_ros_viz/package.xml | 2 +- ...me_conventions.cpp => name_conventions.cc} | 2 +- ...type_conversion.cpp => type_conversion.cc} | 2 +- ...conversion.cpp => test_type_conversion.cc} | 3 +- 27 files changed, 46 insertions(+), 1282 deletions(-) delete mode 100644 drake_ros_core/src/python_bindings/module_drake_ros_core.cpp delete mode 100644 drake_ros_core/src/python_bindings/py_serializer.hpp delete mode 100644 drake_ros_core/src/python_bindings/qos_type_caster.hpp delete mode 100644 drake_ros_tf2/include/drake_ros_tf2/scene_tf_broadcaster_system.hpp delete mode 100644 drake_ros_tf2/include/drake_ros_tf2/scene_tf_system.hpp delete mode 100644 drake_ros_tf2/include/drake_ros_tf2/utilities/name_conventions.hpp delete mode 100644 drake_ros_tf2/include/drake_ros_tf2/utilities/type_conversion.hpp delete mode 100644 drake_ros_tf2/src/python_bindings/module_drake_ros_tf2.cpp delete mode 100644 drake_ros_tf2/src/scene_tf_broadcaster_system.cpp delete mode 100644 drake_ros_tf2/src/scene_tf_system.cpp delete mode 100644 drake_ros_tf2/src/utilities/name_conventions.cpp delete mode 100644 drake_ros_tf2/src/utilities/type_conversion.cpp delete mode 100644 drake_ros_tf2/test/test_tf_broadcaster.cpp delete mode 100644 drake_ros_tf2/test/test_type_conversion.cpp rename drake_ros_viz/include/drake_ros_viz/utilities/{name_conventions.hpp => name_conventions.h} (66%) rename drake_ros_viz/include/drake_ros_viz/utilities/{type_conversion.hpp => type_conversion.h} (84%) rename drake_ros_viz/src/utilities/{name_conventions.cpp => name_conventions.cc} (97%) rename drake_ros_viz/src/utilities/{type_conversion.cpp => type_conversion.cc} (96%) rename drake_ros_viz/test/{test_type_conversion.cpp => test_type_conversion.cc} (96%) diff --git a/drake_ros_core/README.md b/drake_ros_core/README.md index 06996983..457265ff 100644 --- a/drake_ros_core/README.md +++ b/drake_ros_core/README.md @@ -6,17 +6,17 @@ It is similar to this ROS1 prototype [`gizatt/drake_ros_systems`](https://github ## Building -This package has been built and tested on Ubuntu Focal with ROS2 Rolling, using a Drake nightly from April 2021. +This package has been built and tested on Ubuntu Focal with ROS2 Rolling, using a Drake nightly or any stable releases after 14 Jan 2022. 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 +1. [Download Drake binary](https://drake.mit.edu/from_binary.html), nightly or any stable releases after 14 Jan 2022. +1. Extract the Drake binary installation, install it's prerequisites, and [use this Python virutalenv trick](https://drake.mit.edu/from_binary.html). +1. Activate the drake virtual environment. +1. Build it using Colcon, or using CMake directly. **Colcon** 1. Make a workspace `mkdir -p ./ws/src` diff --git a/drake_ros_core/package.xml b/drake_ros_core/package.xml index ed563a94..5d1e3785 100644 --- a/drake_ros_core/package.xml +++ b/drake_ros_core/package.xml @@ -10,7 +10,6 @@ ament_cmake_ros - pybind11-dev rclcpp rclpy rosidl_runtime_c diff --git a/drake_ros_core/src/python_bindings/module_drake_ros_core.cpp b/drake_ros_core/src/python_bindings/module_drake_ros_core.cpp deleted file mode 100644 index db1fe294..00000000 --- a/drake_ros_core/src/python_bindings/module_drake_ros_core.cpp +++ /dev/null @@ -1,93 +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 "py_serializer.hpp" -#include "qos_type_caster.hpp" -#include -#include -#include - -#include "drake_ros_core/drake_ros.hpp" -#include "drake_ros_core/ros_interface_system.hpp" -#include "drake_ros_core/ros_publisher_system.hpp" -#include "drake_ros_core/ros_subscriber_system.hpp" - -namespace py = pybind11; - -using drake::systems::Diagram; -using drake::systems::LeafSystem; -using drake::systems::TriggerType; - -using drake_ros_core::DrakeRos; -using drake_ros_core::DrakeRosInterface; -using drake_ros_core::PySerializer; -using drake_ros_core::RosInterfaceSystem; -using drake_ros_core::RosPublisherSystem; -using drake_ros_core::RosSubscriberSystem; -using drake_ros_core::SerializerInterface; - -PYBIND11_MODULE(drake_ros_core, m) { - m.doc() = "Python wrapper for drake_ros_core"; - - 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 - py::class_>( - m, "DrakeRosInterface"); - - py::class_>(m, "RosInterfaceSystem") - .def(py::init([]() { - return std::make_unique( - std::make_unique()); - })) - .def("get_ros_interface", &RosInterfaceSystem::get_ros_interface); - - py::class_>(m, "RosPublisherSystem") - .def(py::init([](pybind11::object type, const char* topic_name, - drake_ros_core::QoS qos, - std::shared_ptr ros_interface) { - std::unique_ptr serializer = - std::make_unique(type); - return std::make_unique( - serializer, topic_name, qos, ros_interface, - std::unordered_set{TriggerType::kPerStep, - TriggerType::kForced}, - 0.0); - })) - .def(py::init([](pybind11::object type, const char* topic_name, - drake_ros_core::QoS qos, - std::shared_ptr ros_interface, - std::unordered_set publish_triggers, - double publish_period) { - std::unique_ptr serializer = - std::make_unique(type); - return std::make_unique( - serializer, topic_name, qos, ros_interface, publish_triggers, - publish_period); - })); - - py::class_>(m, "RosSubscriberSystem") - .def(py::init([](pybind11::object type, const char* topic_name, - drake_ros_core::QoS qos, - std::shared_ptr ros_interface) { - std::unique_ptr serializer = - std::make_unique(type); - return std::make_unique(serializer, topic_name, - qos, ros_interface); - })); -} diff --git a/drake_ros_core/src/python_bindings/py_serializer.hpp b/drake_ros_core/src/python_bindings/py_serializer.hpp deleted file mode 100644 index ee71c471..00000000 --- a/drake_ros_core/src/python_bindings/py_serializer.hpp +++ /dev/null @@ -1,170 +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. -#ifndef PYTHON_BINDINGS__PY_SERIALIZER_HPP_ -#define PYTHON_BINDINGS__PY_SERIALIZER_HPP_ - -#include - -#include -#include -#include - -#include "drake_ros_core/serializer_interface.hpp" - -namespace py = pybind11; - -namespace drake_ros_core { -// Serialize/Deserialize Python ROS types to rclcpp::SerializedMessage -class PySerializer : public SerializerInterface { - public: - explicit PySerializer(py::object message_type) : message_type_(message_type) { - py::dict scope; - py::exec( - R"delim( -def get_typesupport(msg_type, attribute_name): - metaclass = msg_type.__class__ - if metaclass._TYPE_SUPPORT is None: - # Import typesupport if not done already - metaclass.__import_type_support__() - return getattr(metaclass, attribute_name) - - -def make_abstract_value(some_type): - from pydrake.common.value import AbstractValue - return AbstractValue.Make(some_type()) - )delim", - py::globals(), scope); - - py_make_abstract_value_ = scope["make_abstract_value"]; - py::object py_get_typesupport = scope["get_typesupport"]; - - // Get type support capsule and pointer - auto typesupport = py::cast( - py_get_typesupport(message_type, "_TYPE_SUPPORT")); - // TODO(sloretz) use get_pointer() in py 2.6+ - type_support_ = static_cast(typesupport); - - auto convert_from_py = py::cast( - py_get_typesupport(message_type, "_CONVERT_FROM_PY")); - // TODO(sloretz) use get_pointer() in py 2.6+ - convert_from_py_ = reinterpret_cast( - static_cast(convert_from_py)); - - auto convert_to_py = py::cast( - py_get_typesupport(message_type, "_CONVERT_TO_PY")); - // TODO(sloretz) use get_pointer() in py 2.6+ - convert_to_py_ = reinterpret_cast( - static_cast(convert_to_py)); - - auto create_ros_message = py::cast( - py_get_typesupport(message_type, "_CREATE_ROS_MESSAGE")); - // TODO(sloretz) use get_pointer() in py 2.6+ - create_ros_message_ = reinterpret_cast( - static_cast(create_ros_message)); - - auto destroy_ros_message = py::cast( - py_get_typesupport(message_type, "_DESTROY_ROS_MESSAGE")); - // TODO(sloretz) use get_pointer() in py 2.6+ - destroy_ros_message_ = reinterpret_cast( - static_cast(destroy_ros_message)); - } - - rclcpp::SerializedMessage serialize( - const drake::AbstractValue& abstract_value) const override { - // convert from inaccessible drake::pydrake::Object type - py::dict scope; - scope["av"] = &abstract_value; - py::object message = py::eval("av.Clone().get_mutable_value()", scope); - - // Create C ROS message - auto c_ros_message = std::unique_ptr( - create_ros_message_(), destroy_ros_message_); - - // Convert the Python message to a C ROS message - if (!convert_from_py_(message.ptr(), c_ros_message.get())) { - throw std::runtime_error( - "Failed to convert Python message to C ROS message"); - } - - // Serialize the C message - rclcpp::SerializedMessage serialized_msg; - const auto ret = - rmw_serialize(c_ros_message.get(), type_support_, - &serialized_msg.get_rcl_serialized_message()); - if (ret != RMW_RET_OK) { - throw std::runtime_error("Failed to serialize C ROS message"); - } - return serialized_msg; - } - - bool deserialize(const rclcpp::SerializedMessage& serialized_message, - drake::AbstractValue& abstract_value) const override { - // TODO(sloretz) it would be so much more convenient if I didn't have to - // care that the Python typesupport used the C type support internally. - // Why isn't this encapsulated in the python type support itself? - - // Create a C type support version of the message - auto c_ros_message = std::unique_ptr( - create_ros_message_(), destroy_ros_message_); - if (nullptr == c_ros_message.get()) { - return false; - } - - // Deserialize to C message type - const auto ret = - rmw_deserialize(&serialized_message.get_rcl_serialized_message(), - type_support_, c_ros_message.get()); - - if (RMW_RET_OK != ret) { - return false; - } - - // Convert C type to Python type - PyObject* pymessage = convert_to_py_(c_ros_message.get()); - if (!pymessage) { - return false; - } - - // Store the Python message in the AbstractValue - // convert to inaccessible drake::pydrake::Object type - py::dict scope; - scope["av"] = &abstract_value; - scope["message"] = pymessage; - py::exec("av.set_value(message)", scope); - - return true; - } - - std::unique_ptr create_default_value() const override { - return py::cast>( - py_make_abstract_value_(message_type_)); - } - - const rosidl_message_type_support_t* get_type_support() const override { - return type_support_; - } - - private: - py::object message_type_; - rosidl_message_type_support_t* type_support_; - - py::object py_make_abstract_value_; - - bool (*convert_from_py_)(PyObject*, void*); - PyObject* (*convert_to_py_)(void*); - void* (*create_ros_message_)(void); - void (*destroy_ros_message_)(void*); -}; -} // namespace drake_ros_core -#endif // PYTHON_BINDINGS__PY_SERIALIZER_HPP_ diff --git a/drake_ros_core/src/python_bindings/qos_type_caster.hpp b/drake_ros_core/src/python_bindings/qos_type_caster.hpp deleted file mode 100644 index 4bcc8973..00000000 --- a/drake_ros_core/src/python_bindings/qos_type_caster.hpp +++ /dev/null @@ -1,267 +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. -#ifndef PYTHON_BINDINGS__QOS_TYPE_CASTER_HPP_ -#define PYTHON_BINDINGS__QOS_TYPE_CASTER_HPP_ - -#include - -#include -#include - -namespace drake_ros_core { -// Thin wrapper that adds default constructor -class QoS : public rclcpp::QoS { - public: - QoS() : rclcpp::QoS(1) {} -}; -} // namespace drake_ros_core - -namespace pybind11 { -namespace detail { -template <> -struct type_caster { - public: - // declares local 'value' of type rclcpp::QoS - PYBIND11_TYPE_CASTER(drake_ros_core::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_core::QoS src, return_value_policy policy, - handle parent) { - (void)src; - (void)policy; - (void)parent; - Py_RETURN_NOTIMPLEMENTED; - } -}; -} // namespace detail -} // namespace pybind11 -#endif // PYTHON_BINDINGS__QOS_TYPE_CASTER_HPP_ diff --git a/drake_ros_tf2/README.md b/drake_ros_tf2/README.md index 6daad272..258fe85b 100644 --- a/drake_ros_tf2/README.md +++ b/drake_ros_tf2/README.md @@ -4,7 +4,7 @@ This package provides abstractions to simplify the use of tf2 ROS functionality ## Building -This package has been built and tested on Ubuntu Focal with ROS Rolling, using a Drake nightly from April 2021. +This package has been built and tested on Ubuntu Focal with ROS Rolling, using a Drake nightly or any stable releses after 14 Jan 2022. It may work on other versions of ROS and Drake, but it hasn't been tested. To build it: @@ -12,7 +12,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` 1. [Download Drake binary](https://drake.mit.edu/from_binary.html), nightly or any stable releases after 14 Jan 2022. -1. [Use this Python virtualenv trick](https://drake.mit.edu/from_binary.html), under "Use as a Python Library", to extract the Drake binary installation, install its prerequisites. +1. Extract the Drake binary installation, install it's prerequisites, and [use this Python virutalenv trick](https://drake.mit.edu/from_binary.html). 1. Activate the Drake virtual environment. 1. Build it using Colcon, or using CMake directly. diff --git a/drake_ros_tf2/include/drake_ros_tf2/scene_tf_broadcaster_system.hpp b/drake_ros_tf2/include/drake_ros_tf2/scene_tf_broadcaster_system.hpp deleted file mode 100644 index 2a240154..00000000 --- a/drake_ros_tf2/include/drake_ros_tf2/scene_tf_broadcaster_system.hpp +++ /dev/null @@ -1,65 +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_TF2__SCENE_TF_BROADCASTER_SYSTEM_HPP_ -#define DRAKE_ROS_TF2__SCENE_TF_BROADCASTER_SYSTEM_HPP_ - -#include -#include - -#include -#include - -#include "drake_ros_core/drake_ros_interface.hpp" - -namespace drake_ros_tf2 { - -/// 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 -/// 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 SceneTfBroadcasterSystem : public drake::systems::Diagram { - public: - explicit SceneTfBroadcasterSystem( - std::shared_ptr ros_interface, - 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_tf2 -#endif // DRAKE_ROS_TF2__SCENE_TF_BROADCASTER_SYSTEM_HPP_ diff --git a/drake_ros_tf2/include/drake_ros_tf2/scene_tf_system.hpp b/drake_ros_tf2/include/drake_ros_tf2/scene_tf_system.hpp deleted file mode 100644 index 3aac3eb3..00000000 --- a/drake_ros_tf2/include/drake_ros_tf2/scene_tf_system.hpp +++ /dev/null @@ -1,65 +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_TF2__SCENE_TF_SYSTEM_HPP_ -#define DRAKE_ROS_TF2__SCENE_TF_SYSTEM_HPP_ - -#include - -#include -#include -#include -#include - -namespace drake_ros_tf2 { -/// 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(); - - /// 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; - - 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_tf2 -#endif // DRAKE_ROS_TF2__SCENE_TF_SYSTEM_HPP_ diff --git a/drake_ros_tf2/include/drake_ros_tf2/utilities/name_conventions.hpp b/drake_ros_tf2/include/drake_ros_tf2/utilities/name_conventions.hpp deleted file mode 100644 index 38e94cc2..00000000 --- a/drake_ros_tf2/include/drake_ros_tf2/utilities/name_conventions.hpp +++ /dev/null @@ -1,41 +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_TF2__UTILITIES__NAME_CONVENTIONS_HPP_ -#define DRAKE_ROS_TF2__UTILITIES__NAME_CONVENTIONS_HPP_ - -#include -#include - -#include -#include - -namespace drake_ros_tf2 { -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); - -} // namespace utilities -} // namespace drake_ros_tf2 - -#endif // DRAKE_ROS_TF2__UTILITIES__NAME_CONVENTIONS_HPP_ diff --git a/drake_ros_tf2/include/drake_ros_tf2/utilities/type_conversion.hpp b/drake_ros_tf2/include/drake_ros_tf2/utilities/type_conversion.hpp deleted file mode 100644 index f0d170ef..00000000 --- a/drake_ros_tf2/include/drake_ros_tf2/utilities/type_conversion.hpp +++ /dev/null @@ -1,30 +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_TF2__UTILITIES__TYPE_CONVERSION_HPP_ -#define DRAKE_ROS_TF2__UTILITIES__TYPE_CONVERSION_HPP_ - -#include -#include -#include - -namespace drake_ros_tf2 { -namespace utilities { - -geometry_msgs::msg::Transform ToTransformMsg( - const drake::math::RigidTransform X_AB); - -} // namespace utilities -} // namespace drake_ros_tf2 - -#endif // DRAKE_ROS_TF2__UTILITIES__TYPE_CONVERSION_HPP_ diff --git a/drake_ros_tf2/src/python_bindings/module_drake_ros_tf2.cpp b/drake_ros_tf2/src/python_bindings/module_drake_ros_tf2.cpp deleted file mode 100644 index 41b98aac..00000000 --- a/drake_ros_tf2/src/python_bindings/module_drake_ros_tf2.cpp +++ /dev/null @@ -1,65 +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 "drake_ros_tf2/scene_tf_broadcaster_system.hpp" -#include -#include -#include - -#include "drake_ros_core/ros_interface_system.hpp" - -namespace py = pybind11; - -using drake::systems::Diagram; -using drake::systems::TriggerType; - -using drake_ros_core::DrakeRosInterface; -using drake_ros_tf2::SceneTfBroadcasterParams; -using drake_ros_tf2::SceneTfBroadcasterSystem; - -PYBIND11_MODULE(drake_ros_tf2, m) { - m.doc() = "Python wrapper for drake_ros_tf2"; - - py::module::import("drake_ros_core"); - - py::module::import("pydrake.systems.framework"); - py::module::import("pydrake.multibody.plant"); - - py::class_(m, "SceneTfBroadcasterParams") - .def(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("params") = SceneTfBroadcasterParams{}) - .def("RegisterMultibodyPlant", - &SceneTfBroadcasterSystem::RegisterMultibodyPlant) - .def("get_graph_query_port", - &SceneTfBroadcasterSystem::get_graph_query_port, - py::return_value_policy::reference_internal); -} diff --git a/drake_ros_tf2/src/scene_tf_broadcaster_system.cpp b/drake_ros_tf2/src/scene_tf_broadcaster_system.cpp deleted file mode 100644 index ef936a43..00000000 --- a/drake_ros_tf2/src/scene_tf_broadcaster_system.cpp +++ /dev/null @@ -1,69 +0,0 @@ -// Copyright 2021 Open Source Robotics Foundation, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "drake_ros_tf2/scene_tf_broadcaster_system.hpp" - -#include -#include - -#include "drake_ros_tf2/scene_tf_system.hpp" -#include -#include -#include - -#include "drake_ros_core/drake_ros_interface.hpp" -#include "drake_ros_core/ros_publisher_system.hpp" - -namespace drake_ros_tf2 { - -class SceneTfBroadcasterSystem::SceneTfBroadcasterSystemPrivate { - public: - SceneTfSystem* scene_tf; -}; - -SceneTfBroadcasterSystem::SceneTfBroadcasterSystem( - std::shared_ptr ros_interface, - SceneTfBroadcasterParams params) - : impl_(new SceneTfBroadcasterSystemPrivate()) { - drake::systems::DiagramBuilder builder; - - impl_->scene_tf = builder.AddSystem(); - - using drake_ros_core::RosPublisherSystem; - auto scene_tf_publisher = - builder.AddSystem(RosPublisherSystem::Make( - "/tf", tf2_ros::DynamicBroadcasterQoS(), ros_interface, - params.publish_triggers, params.publish_period)); - - builder.Connect(impl_->scene_tf->get_scene_tf_output_port(), - scene_tf_publisher->get_input_port()); - - 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& -SceneTfBroadcasterSystem::get_graph_query_port() const { - return get_input_port(); -} - -} // namespace drake_ros_tf2 diff --git a/drake_ros_tf2/src/scene_tf_system.cpp b/drake_ros_tf2/src/scene_tf_system.cpp deleted file mode 100644 index 1d1d65a9..00000000 --- a/drake_ros_tf2/src/scene_tf_system.cpp +++ /dev/null @@ -1,95 +0,0 @@ -// Copyright 2021 Open Source Robotics Foundation, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "drake_ros_tf2/scene_tf_system.hpp" - -#include - -#include "drake_ros_tf2/utilities/name_conventions.hpp" -#include "drake_ros_tf2/utilities/type_conversion.hpp" -#include -#include -#include -#include - -namespace drake_ros_tf2 { -class SceneTfSystem::SceneTfSystemPrivate { - public: - std::unordered_set*> plants; - 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() {} - -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 { - 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.clear(); - 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.GetAllFrameIds()) { - if (frame_id == inspector.world_frame_id()) { - continue; - } - 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); - } - } -} - -} // namespace drake_ros_tf2 diff --git a/drake_ros_tf2/src/utilities/name_conventions.cpp b/drake_ros_tf2/src/utilities/name_conventions.cpp deleted file mode 100644 index 31335637..00000000 --- a/drake_ros_tf2/src/utilities/name_conventions.cpp +++ /dev/null @@ -1,79 +0,0 @@ -// Copyright 2021 Open Source Robotics Foundation, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "drake_ros_tf2/utilities/name_conventions.hpp" - -#include -#include -#include - -namespace drake_ros_tf2 { -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; - 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)); -} - -} // namespace utilities -} // namespace drake_ros_tf2 diff --git a/drake_ros_tf2/src/utilities/type_conversion.cpp b/drake_ros_tf2/src/utilities/type_conversion.cpp deleted file mode 100644 index 2879626e..00000000 --- a/drake_ros_tf2/src/utilities/type_conversion.cpp +++ /dev/null @@ -1,42 +0,0 @@ -// Copyright 2021 Open Source Robotics Foundation, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "drake_ros_tf2/utilities/type_conversion.hpp" - -#include -#include -#include - -namespace drake_ros_tf2 { -namespace utilities { - -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_tf2 diff --git a/drake_ros_tf2/test/test_tf_broadcaster.cpp b/drake_ros_tf2/test/test_tf_broadcaster.cpp deleted file mode 100644 index b3e94eb4..00000000 --- a/drake_ros_tf2/test/test_tf_broadcaster.cpp +++ /dev/null @@ -1,134 +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 "drake_ros_tf2/scene_tf_broadcaster_system.hpp" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "drake_ros_core/drake_ros.hpp" -#include "drake_ros_core/ros_interface_system.hpp" - -using drake_ros_core::DrakeRos; -using drake_ros_core::RosInterfaceSystem; -using drake_ros_tf2::SceneTfBroadcasterParams; -using drake_ros_tf2::SceneTfBroadcasterSystem; - -TEST(SceneTfBroadcasting, NominalCase) { - 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)); - - 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(), - scene_tf_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"); - - 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 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(); - 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()); - - 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); - 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_tf2/test/test_type_conversion.cpp b/drake_ros_tf2/test/test_type_conversion.cpp deleted file mode 100644 index 9502b515..00000000 --- a/drake_ros_tf2/test/test_type_conversion.cpp +++ /dev/null @@ -1,34 +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 "drake_ros_tf2/utilities/type_conversion.hpp" -#include - -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_tf2::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()); -} diff --git a/drake_ros_viz/.clang-format b/drake_ros_viz/.clang-format index ff6a7ada..1e5eeb3a 100644 --- a/drake_ros_viz/.clang-format +++ b/drake_ros_viz/.clang-format @@ -38,7 +38,7 @@ IncludeCategories: - Regex: '^<' Priority: 40 # Your project's h files. - - Regex: '^"drake_ros_core' + - Regex: '^"drake_ros_viz' Priority: 50 # Other libraries' h files (with quotes). - Regex: '^"' diff --git a/drake_ros_viz/CMakeLists.txt b/drake_ros_viz/CMakeLists.txt index 79e44b49..3bd3e4e0 100644 --- a/drake_ros_viz/CMakeLists.txt +++ b/drake_ros_viz/CMakeLists.txt @@ -1,7 +1,7 @@ cmake_minimum_required(VERSION 3.10) project(drake_ros_viz) -# Default to C++14 +# Default to C++17 if(NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 17) endif() @@ -9,14 +9,15 @@ 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(ament_cmake REQUIRED) find_package(geometry_msgs REQUIRED) find_package(drake REQUIRED) find_package(visualization_msgs REQUIRED) add_library(drake_ros_viz - src/utilities/name_conventions.cpp - src/utilities/type_conversion.cpp + src/utilities/name_conventions.cc + src/utilities/type_conversion.cc ) target_link_libraries(drake_ros_viz PUBLIC @@ -40,7 +41,9 @@ ament_export_dependencies(drake) ament_export_dependencies(geometry_msgs) ament_export_dependencies(visualization_msgs) -install(TARGETS ${PROJECT_NAME} EXPORT ${PROJECT_NAME} +install( + TARGETS ${PROJECT_NAME} + EXPORT ${PROJECT_NAME} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin @@ -60,7 +63,7 @@ if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) - ament_add_gtest(test_type_conversion test/test_type_conversion.cpp) + ament_add_gtest(test_type_conversion test/test_type_conversion.cc) target_link_libraries(test_type_conversion drake::drake drake_ros_viz diff --git a/drake_ros_viz/CPPLINT.cfg b/drake_ros_viz/CPPLINT.cfg index 98c9cdd4..476dca32 100644 --- a/drake_ros_viz/CPPLINT.cfg +++ b/drake_ros_viz/CPPLINT.cfg @@ -41,6 +41,12 @@ filter=+build/pragma_once # Disable cpplint's include order. We have our own via //tools:drakelint. filter=-build/include_order +# TODO(hidmic): uncomment when ament_cpplint updates its vendored cpplint +# version to support the following filters. Also remove corresponding NOLINT +# codetags in sources. +## Allow private, sibling include files. +#filter=-build/include_subdir + # We do not care about the whitespace details of a TODO comment. It is not # relevant for easy grepping, and the GSG does not specify any particular # whitespace style. (We *do* care what the "TODO(username)" itself looks like diff --git a/drake_ros_viz/README.md b/drake_ros_viz/README.md index e55d311d..ff79ddc4 100644 --- a/drake_ros_viz/README.md +++ b/drake_ros_viz/README.md @@ -1,20 +1,20 @@ -# Drake ROS Systems +# Drake ROS Viz This package provides abstractions to simplify the visualization of a Drake scene using `rviz2`. ## Building -This package has been built and tested on Ubuntu Focal with ROS Rolling, using a Drake nightly from April 2021. +This package has been built and tested on Ubuntu Focal with ROS Rolling, using a Drake nightly or any stable releases after 14 Jan 2022. 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 +1. [Download Drake binary](https://drake.mit.edu/from_binary.html), nightly or any stable releases after 14 Jan 2022. +1. Extract the Drake binary installation, install it's prerequisites, and [use this Python virutalenv trick](https://drake.mit.edu/from_binary.html). +1. Activate the drake virtual environment. +1. Build it using Colcon, or using CMake directly. **Colcon** 1. Make a workspace `mkdir -p ./ws/src` diff --git a/drake_ros_viz/include/drake_ros_viz/utilities/name_conventions.hpp b/drake_ros_viz/include/drake_ros_viz/utilities/name_conventions.h similarity index 66% rename from drake_ros_viz/include/drake_ros_viz/utilities/name_conventions.hpp rename to drake_ros_viz/include/drake_ros_viz/utilities/name_conventions.h index 3cea2675..a3390a7d 100644 --- a/drake_ros_viz/include/drake_ros_viz/utilities/name_conventions.hpp +++ b/drake_ros_viz/include/drake_ros_viz/utilities/name_conventions.h @@ -11,8 +11,7 @@ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. -#ifndef DRAKE_ROS_VIZ__UTILITIES__NAME_CONVENTIONS_HPP_ -#define DRAKE_ROS_VIZ__UTILITIES__NAME_CONVENTIONS_HPP_ +#pragma once #include #include @@ -23,6 +22,16 @@ namespace drake_ros_viz { namespace utilities { +/** Retrieve conventional marker namespace for a given scene geometry. + @param[in] inspector inspector for a given SceneGraph's data. + @param[in] plants a set of MultibodyPlant instances from which to derive + semantically meaningful marker namespaces, if possible. + @param[in] geometry_id target geometry ID. + @returns the namespace for the marker + @pre `geomtry_id` is associated with a registered geometry in the SceneGraph + implied by `inspector`. + @pre all `plants` are associated with the SceneGraph implied by `inspector`. + */ std::string GetMarkerNamespace( const drake::geometry::SceneGraphInspector& inspector, const std::unordered_set*>& @@ -31,5 +40,3 @@ std::string GetMarkerNamespace( } // namespace utilities } // namespace drake_ros_viz - -#endif // DRAKE_ROS_VIZ__UTILITIES__NAME_CONVENTIONS_HPP_ diff --git a/drake_ros_viz/include/drake_ros_viz/utilities/type_conversion.hpp b/drake_ros_viz/include/drake_ros_viz/utilities/type_conversion.h similarity index 84% rename from drake_ros_viz/include/drake_ros_viz/utilities/type_conversion.hpp rename to drake_ros_viz/include/drake_ros_viz/utilities/type_conversion.h index 53e9d715..09c98b47 100644 --- a/drake_ros_viz/include/drake_ros_viz/utilities/type_conversion.hpp +++ b/drake_ros_viz/include/drake_ros_viz/utilities/type_conversion.h @@ -11,8 +11,7 @@ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. -#ifndef DRAKE_ROS_VIZ__UTILITIES__TYPE_CONVERSION_HPP_ -#define DRAKE_ROS_VIZ__UTILITIES__TYPE_CONVERSION_HPP_ +#pragma once #include #include @@ -25,5 +24,3 @@ geometry_msgs::msg::Pose ToPoseMsg( } // namespace utilities } // namespace drake_ros_viz - -#endif // DRAKE_ROS_VIZ__UTILITIES__TYPE_CONVERSION_HPP_ diff --git a/drake_ros_viz/package.xml b/drake_ros_viz/package.xml index c6e2db43..cdcce7f2 100644 --- a/drake_ros_viz/package.xml +++ b/drake_ros_viz/package.xml @@ -8,7 +8,7 @@ Michel Hidalgo Apache License 2.0 - ament_cmake_ros + ament_cmake geometry_msgs visualization_msgs diff --git a/drake_ros_viz/src/utilities/name_conventions.cpp b/drake_ros_viz/src/utilities/name_conventions.cc similarity index 97% rename from drake_ros_viz/src/utilities/name_conventions.cpp rename to drake_ros_viz/src/utilities/name_conventions.cc index e7065706..bec858c5 100644 --- a/drake_ros_viz/src/utilities/name_conventions.cpp +++ b/drake_ros_viz/src/utilities/name_conventions.cc @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "drake_ros_viz/utilities/name_conventions.hpp" +#include "drake_ros_viz/utilities/name_conventions.h" #include #include diff --git a/drake_ros_viz/src/utilities/type_conversion.cpp b/drake_ros_viz/src/utilities/type_conversion.cc similarity index 96% rename from drake_ros_viz/src/utilities/type_conversion.cpp rename to drake_ros_viz/src/utilities/type_conversion.cc index 1324f110..da656487 100644 --- a/drake_ros_viz/src/utilities/type_conversion.cpp +++ b/drake_ros_viz/src/utilities/type_conversion.cc @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "drake_ros_viz/utilities/type_conversion.hpp" +#include "drake_ros_viz/utilities/type_conversion.h" #include #include diff --git a/drake_ros_viz/test/test_type_conversion.cpp b/drake_ros_viz/test/test_type_conversion.cc similarity index 96% rename from drake_ros_viz/test/test_type_conversion.cpp rename to drake_ros_viz/test/test_type_conversion.cc index 54c04c17..9a9155a1 100644 --- a/drake_ros_viz/test/test_type_conversion.cpp +++ b/drake_ros_viz/test/test_type_conversion.cc @@ -14,9 +14,10 @@ #include -#include "drake_ros_viz/utilities/type_conversion.hpp" #include +#include "drake_ros_viz/utilities/type_conversion.h" + TEST(TypeConversion, ToPoseMsg) { const drake::Vector3 position{1., 2., 3.}; const drake::Quaternion orientation{0., 0., sin(M_PI / 4.),