From d0da29f3494505f660371cf48c412303ddbb06c7 Mon Sep 17 00:00:00 2001 From: Aditya Pande Date: Tue, 14 Mar 2023 02:01:16 +0000 Subject: [PATCH] [py] Add generic type casters Co-Authored-By: Eric Cousineau --- drake_ros/core/CMakeLists.txt | 1 + drake_ros/core/geometry_conversions_pybind.h | 394 +----------------- drake_ros/core/ros_idl_pybind.h | 147 +++++++ drake_ros/drake_ros/BUILD.bazel | 35 ++ .../test/ros_message_type_caster_test.py | 66 +++ ...ros_message_type_caster_test_via_all_py.cc | 34 ++ ..._type_caster_test_via_specific_types_py.cc | 35 ++ 7 files changed, 332 insertions(+), 380 deletions(-) create mode 100644 drake_ros/core/ros_idl_pybind.h create mode 100644 drake_ros/drake_ros/test/ros_message_type_caster_test.py create mode 100644 drake_ros/drake_ros/test/ros_message_type_caster_test_via_all_py.cc create mode 100644 drake_ros/drake_ros/test/ros_message_type_caster_test_via_specific_types_py.cc diff --git a/drake_ros/core/CMakeLists.txt b/drake_ros/core/CMakeLists.txt index 26fb2bc9..b1033fd7 100644 --- a/drake_ros/core/CMakeLists.txt +++ b/drake_ros/core/CMakeLists.txt @@ -3,6 +3,7 @@ set(HEADERS "drake_ros.h" "geometry_conversions.h" "geometry_conversions_pybind.h" + "ros_idl_pybind.h" "publisher.h" "ros_interface_system.h" "ros_publisher_system.h" diff --git a/drake_ros/core/geometry_conversions_pybind.h b/drake_ros/core/geometry_conversions_pybind.h index 307a5103..5d655692 100644 --- a/drake_ros/core/geometry_conversions_pybind.h +++ b/drake_ros/core/geometry_conversions_pybind.h @@ -1,380 +1,14 @@ -#include -#include -#include -#include -#include -#include -#include - -namespace py = pybind11; - -// TODO(Aditya):Use serialization-deserialization based generic typecasting. -namespace PYBIND11_NAMESPACE { -namespace detail { - -// Typecasting between geometry_msgs.msg.Quaternion (Python) and -// geometry_msgs::msg::Quaternion (C++) -template <> -struct type_caster { - public: - PYBIND11_TYPE_CASTER(geometry_msgs::msg::Quaternion, - _("geometry_msgs.msg.Quaternion")); - - // Convert from python geometry_msgs.msg.Quaternion to - // C ++ geometry_msgs::msg::Quaternion - bool load(handle src, bool) { - handle cls = module::import("geometry_msgs.msg").attr("Quaternion"); - if (!isinstance(src, cls)) { - return false; - } - object source = reinterpret_borrow(src); - - value.x = source.attr("x").cast(); - value.y = source.attr("y").cast(); - value.z = source.attr("z").cast(); - value.w = source.attr("w").cast(); - - return true; - } - - // Converting from C++ geometry_msgs::msg::Quaternion to - // Python geometry_msgs.msg.Quaternion - static handle cast(geometry_msgs::msg::Quaternion src, - return_value_policy policy, handle parent) { - (void)policy; - (void)parent; - - object instance = module::import("geometry_msgs.msg").attr("Quaternion")(); - instance.attr("x") = src.x; - instance.attr("y") = src.y; - instance.attr("z") = src.z; - instance.attr("w") = src.w; - - instance.inc_ref(); - return instance; - } -}; - -// Typecasting between geometry_msgs.msg.Point (Python) and -// geometry_msgs::msg::Point (C++) -template <> -struct type_caster { - public: - PYBIND11_TYPE_CASTER(geometry_msgs::msg::Point, _("geometry_msgs.msg.Point")); - - // Convert from python geometry_msgs.msg.Point to - // C ++ geometry_msgs::msg::Point - bool load(handle src, bool) { - handle cls = module::import("geometry_msgs.msg").attr("Point"); - if (!isinstance(src, cls)) { - return false; - } - object source = reinterpret_borrow(src); - - value.x = source.attr("x").cast(); - value.y = source.attr("y").cast(); - value.z = source.attr("z").cast(); - - return true; - } - - // Converting from C++ geometry_msgs::msg::Point to - // Python geometry_msgs.msg.Point - static handle cast(geometry_msgs::msg::Point src, return_value_policy policy, - handle parent) { - (void)policy; - (void)parent; - - object instance = module::import("geometry_msgs.msg").attr("Point")(); - instance.attr("x") = src.x; - instance.attr("y") = src.y; - instance.attr("z") = src.z; - - instance.inc_ref(); - return instance; - } -}; - -// Typecasting between geometry_msgs.msg.Vector3 (Python) and -// geometry_msgs::msg::Vector3 (C++) -template <> -struct type_caster { - public: - PYBIND11_TYPE_CASTER(geometry_msgs::msg::Vector3, - _("geometry_msgs.msg.Vector3")); - - // Convert from python geometry_msgs.msg.Vector3 to - // C ++ geometry_msgs::msg::Vector3 - bool load(handle src, bool) { - handle cls = module::import("geometry_msgs.msg").attr("Vector3"); - if (!isinstance(src, cls)) { - return false; - } - object source = reinterpret_borrow(src); - - value.x = source.attr("x").cast(); - value.y = source.attr("y").cast(); - value.z = source.attr("z").cast(); - - return true; - } - - // Converting from C++ geometry_msgs::msg::Vector3 to - // Python geometry_msgs.msg.Vector3 - static handle cast(geometry_msgs::msg::Vector3 src, - return_value_policy policy, handle parent) { - (void)policy; - (void)parent; - - object instance = module::import("geometry_msgs.msg").attr("Vector3")(); - instance.attr("x") = src.x; - instance.attr("y") = src.y; - instance.attr("z") = src.z; - - instance.inc_ref(); - return instance; - } -}; - -// Typecasting between geometry_msgs.msg.Twist (Python) and -// geometry_msgs::msg::Twist (C++) -template <> -struct type_caster { - public: - PYBIND11_TYPE_CASTER(geometry_msgs::msg::Twist, _("geometry_msgs.msg.Twist")); - - // Convert from python geometry_msgs.msg.Twist to - // C ++ geometry_msgs::msg::Twist - bool load(handle src, bool) { - handle cls = module::import("geometry_msgs.msg").attr("Twist"); - if (!isinstance(src, cls)) { - return false; - } - object source = reinterpret_borrow(src); - - value.linear.x = source.attr("linear").attr("x").cast(); - value.linear.y = source.attr("linear").attr("y").cast(); - value.linear.z = source.attr("linear").attr("z").cast(); - value.angular.x = source.attr("angular").attr("x").cast(); - value.angular.y = source.attr("angular").attr("y").cast(); - value.angular.z = source.attr("angular").attr("z").cast(); - - return true; - } - - // Converting from C++ geometry_msgs::msg::Twist to - // Python geometry_msgs.msg.Twist - static handle cast(geometry_msgs::msg::Twist src, return_value_policy policy, - handle parent) { - (void)policy; - (void)parent; - - object instance = module::import("geometry_msgs.msg").attr("Twist")(); - instance.attr("linear").attr("x") = src.linear.x; - instance.attr("linear").attr("y") = src.linear.y; - instance.attr("linear").attr("z") = src.linear.z; - instance.attr("angular").attr("x") = src.angular.x; - instance.attr("angular").attr("y") = src.angular.y; - instance.attr("angular").attr("z") = src.angular.z; - - instance.inc_ref(); - return instance; - } -}; - -// Typecasting between geometry_msgs.msg.Accel (Python) and -// geometry_msgs::msg::Accel (C++) -template <> -struct type_caster { - public: - PYBIND11_TYPE_CASTER(geometry_msgs::msg::Accel, _("geometry_msgs.msg.Accel")); - - // Convert from python geometry_msgs.msg.Accel to - // C ++ geometry_msgs::msg::Accel - bool load(handle src, bool) { - handle cls = module::import("geometry_msgs.msg").attr("Accel"); - if (!isinstance(src, cls)) { - return false; - } - object source = reinterpret_borrow(src); - - value.linear.x = source.attr("linear").attr("x").cast(); - value.linear.y = source.attr("linear").attr("y").cast(); - value.linear.z = source.attr("linear").attr("z").cast(); - value.angular.x = source.attr("angular").attr("x").cast(); - value.angular.y = source.attr("angular").attr("y").cast(); - value.angular.z = source.attr("angular").attr("z").cast(); - - return true; - } - - // Converting from C++ geometry_msgs::msg::Accel to - // Python geometry_msgs.msg.Accel - static handle cast(geometry_msgs::msg::Accel src, return_value_policy policy, - handle parent) { - (void)policy; - (void)parent; - - object instance = module::import("geometry_msgs.msg").attr("Accel")(); - instance.attr("linear").attr("x") = src.linear.x; - instance.attr("linear").attr("y") = src.linear.y; - instance.attr("linear").attr("z") = src.linear.z; - instance.attr("angular").attr("x") = src.angular.x; - instance.attr("angular").attr("y") = src.angular.y; - instance.attr("angular").attr("z") = src.angular.z; - - instance.inc_ref(); - return instance; - } -}; - -// Typecasting between geometry_msgs.msg.Wrench (Python) and -// geometry_msgs::msg::Wrench (C++) -template <> -struct type_caster { - public: - PYBIND11_TYPE_CASTER(geometry_msgs::msg::Wrench, - _("geometry_msgs.msg.Wrench")); - - // Convert from python geometry_msgs.msg.Wrench to - // C ++ geometry_msgs::msg::Wrench - bool load(handle src, bool) { - handle cls = module::import("geometry_msgs.msg").attr("Wrench"); - if (!isinstance(src, cls)) { - return false; - } - object source = reinterpret_borrow(src); - - value.force.x = source.attr("force").attr("x").cast(); - value.force.y = source.attr("force").attr("y").cast(); - value.force.z = source.attr("force").attr("z").cast(); - value.torque.x = source.attr("torque").attr("x").cast(); - value.torque.y = source.attr("torque").attr("y").cast(); - value.torque.z = source.attr("torque").attr("z").cast(); - - return true; - } - - // Converting from C++ geometry_msgs::msg::Wrench to - // Python geometry_msgs.msg.Wrench - static handle cast(geometry_msgs::msg::Wrench src, return_value_policy policy, - handle parent) { - (void)policy; - (void)parent; - - object instance = module::import("geometry_msgs.msg").attr("Wrench")(); - instance.attr("force").attr("x") = src.force.x; - instance.attr("force").attr("y") = src.force.y; - instance.attr("force").attr("z") = src.force.z; - instance.attr("torque").attr("x") = src.torque.x; - instance.attr("torque").attr("y") = src.torque.y; - instance.attr("torque").attr("z") = src.torque.z; - - instance.inc_ref(); - return instance; - } -}; - -// Typecasting between geometry_msgs.msg.Pose (Python) and -// geometry_msgs::msg::Pose (C++) -template <> -struct type_caster { - public: - PYBIND11_TYPE_CASTER(geometry_msgs::msg::Pose, _("geometry_msgs.msg.Pose")); - - // Convert from python geometry_msgs.msg.Pose to - // C ++ geometry_msgs::msg::Pose - bool load(handle src, bool) { - handle cls = module::import("geometry_msgs.msg").attr("Pose"); - if (!isinstance(src, cls)) { - return false; - } - object source = reinterpret_borrow(src); - - value.position.x = source.attr("position").attr("x").cast(); - value.position.y = source.attr("position").attr("y").cast(); - value.position.z = source.attr("position").attr("z").cast(); - - value.orientation.x = source.attr("orientation").attr("x").cast(); - value.orientation.y = source.attr("orientation").attr("y").cast(); - value.orientation.z = source.attr("orientation").attr("z").cast(); - value.orientation.w = source.attr("orientation").attr("w").cast(); - - return true; - } - - // Converting from C++ geometry_msgs::msg::Pose to - // Python geometry_msgs.msg.Pose - static handle cast(geometry_msgs::msg::Pose src, return_value_policy policy, - handle parent) { - (void)policy; - (void)parent; - - object instance = module::import("geometry_msgs.msg").attr("Pose")(); - instance.attr("position").attr("x") = src.position.x; - instance.attr("position").attr("y") = src.position.y; - instance.attr("position").attr("z") = src.position.z; - - instance.attr("orientation").attr("x") = src.orientation.x; - instance.attr("orientation").attr("y") = src.orientation.y; - instance.attr("orientation").attr("z") = src.orientation.z; - instance.attr("orientation").attr("w") = src.orientation.w; - - instance.inc_ref(); - return instance; - } -}; - -// Typecasting between geometry_msgs.msg.Transform (Python) and -// geometry_msgs::msg::Transform (C++) -template <> -struct type_caster { - public: - PYBIND11_TYPE_CASTER(geometry_msgs::msg::Transform, - _("geometry_msgs.msg.Transform")); - - // Convert from python geometry_msgs.msg.Transform to - // C ++ geometry_msgs::msg::Transform - bool load(handle src, bool) { - handle cls = module::import("geometry_msgs.msg").attr("Transform"); - if (!isinstance(src, cls)) { - return false; - } - object source = reinterpret_borrow(src); - - value.translation.x = source.attr("translation").attr("x").cast(); - value.translation.y = source.attr("translation").attr("y").cast(); - value.translation.z = source.attr("translation").attr("z").cast(); - - value.rotation.x = source.attr("rotation").attr("x").cast(); - value.rotation.y = source.attr("rotation").attr("y").cast(); - value.rotation.z = source.attr("rotation").attr("z").cast(); - value.rotation.w = source.attr("rotation").attr("w").cast(); - - return true; - } - - // Converting from C++ geometry_msgs::msg::Transform to - // Python geometry_msgs.msg.Transform - static handle cast(geometry_msgs::msg::Transform src, - return_value_policy policy, handle parent) { - (void)policy; - (void)parent; - - object instance = module::import("geometry_msgs.msg").attr("Transform")(); - instance.attr("translation").attr("x") = src.translation.x; - instance.attr("translation").attr("y") = src.translation.y; - instance.attr("translation").attr("z") = src.translation.z; - - instance.attr("rotation").attr("x") = src.rotation.x; - instance.attr("rotation").attr("y") = src.rotation.y; - instance.attr("rotation").attr("z") = src.rotation.z; - instance.attr("rotation").attr("w") = src.rotation.w; - - instance.inc_ref(); - return instance; - } -}; - -} // namespace detail -} // namespace PYBIND11_NAMESPACE +#pragma once + +#include "ros_idl_pybind.h" + +// Generic typecaster for specific ROS 2 messages. +// This method can be used instead of the ROS_MSG_PYBIND_TYPECAST_ALL() macro. +ROS_MSG_PYBIND_TYPECAST(geometry_msgs::msg::Quaternion); +ROS_MSG_PYBIND_TYPECAST(geometry_msgs::msg::Point); +ROS_MSG_PYBIND_TYPECAST(geometry_msgs::msg::Vector3); +ROS_MSG_PYBIND_TYPECAST(geometry_msgs::msg::Twist); +ROS_MSG_PYBIND_TYPECAST(geometry_msgs::msg::Accel); +ROS_MSG_PYBIND_TYPECAST(geometry_msgs::msg::Wrench); +ROS_MSG_PYBIND_TYPECAST(geometry_msgs::msg::Pose); +ROS_MSG_PYBIND_TYPECAST(geometry_msgs::msg::Transform); diff --git a/drake_ros/core/ros_idl_pybind.h b/drake_ros/core/ros_idl_pybind.h new file mode 100644 index 00000000..f80b48bd --- /dev/null +++ b/drake_ros/core/ros_idl_pybind.h @@ -0,0 +1,147 @@ +#pragma once + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace py = pybind11; + +// Generic (C++ <-> Python) typecaster for all ROS 2 messages. +#define ROS_MSG_PYBIND_TYPECAST_ALL() \ + namespace pybind11 { \ + namespace detail { \ + template \ + struct type_caster< \ + T, std::enable_if_t::value>> \ + : public drake_ros::drake_ros_py::ros_message_type_caster {}; \ + } \ + } + +// Generic (C++ <-> Python) typecaster for a specific ROS 2 message. +#define ROS_MSG_PYBIND_TYPECAST(T) \ + namespace pybind11 { \ + namespace detail { \ + template <> \ + struct type_caster \ + : public drake_ros::drake_ros_py::ros_message_type_caster {}; \ + } \ + } + +#define DRAKE_ROS_PYBIND_TYPECASTER(T, name) \ + using handle = py::handle; \ + using none = py::none; \ + using return_value_policy = py::return_value_policy; \ + template \ + using enable_if_t = std::enable_if_t; \ + template \ + using remove_cv_t = std::remove_cv_t; \ + PYBIND11_TYPE_CASTER(T, name) + +namespace drake_ros { +namespace drake_ros_py { + +// Get the name of the message as a string. +template +std::string GetPythonMessageName() { + std::string msg_name(rosidl_generator_traits::name()); + auto pos = msg_name.find_last_of("/"); + return msg_name.substr(pos + 1); +} + +// Get the package of the message as a string. +template +std::string GetPythonMessagePackageName() { + std::string msg_name(rosidl_generator_traits::name()); + msg_name = std::regex_replace(msg_name, std::regex("/"), "."); + auto pos = msg_name.find_last_of("."); + return msg_name.substr(0, pos); +} + +// Convert a Python ROS 2 message into a C++ equivalent. +template +bool RosMessagePyToCpp(const py::handle& src, T* cpp_message) { + py::handle cls = py::module::import(GetPythonMessagePackageName().c_str()) + .attr(GetPythonMessageName().c_str()); + if (!isinstance(src, cls)) { + return false; + } + py::object py_message = py::reinterpret_borrow(src); + + // Check for type support + py::object check_for_type_support = + py::module::import("rclpy.type_support").attr("check_for_type_support"); + py::object msg_type = + py::module::import(GetPythonMessagePackageName().c_str()) + .attr(GetPythonMessageName().c_str()); + check_for_type_support(msg_type); + + py::object py_serialize = + py::module::import("rclpy.serialization").attr("serialize_message"); + py::bytes pybytes = py_serialize(py_message); + const std::string content = pybytes; + const auto content_size = content.size() + 1; + + rclcpp::SerializedMessage serialized_message(content_size); + auto& rcl_handle = serialized_message.get_rcl_serialized_message(); + std::memcpy(rcl_handle.buffer, content.c_str(), content.size()); + rcl_handle.buffer[content.size()] = '\0'; + rcl_handle.buffer_length = content_size; + + rclcpp::Serialization protocol; + protocol.deserialize_message(&serialized_message, cpp_message); + + return true; +} + +// Convert a C++ ROS 2 message into a Python equivalent. +template +py::object RosMessageCppToPy(T src) { + rclcpp::SerializedMessage serialized_message; + rclcpp::Serialization protocol; + protocol.serialize_message(&src, &serialized_message); + + auto& rcl_handle = serialized_message.get_rcl_serialized_message(); + py::object py_deserialize = + py::module::import("rclpy.serialization").attr("deserialize_message"); + py::object msg_type = + py::module::import(GetPythonMessagePackageName().c_str()) + .attr(GetPythonMessageName().c_str()); + std::string content_string(reinterpret_cast(rcl_handle.buffer), + serialized_message.size()); + content_string.push_back('\0'); + py::object instance = py_deserialize(py::bytes(content_string), msg_type); + instance.inc_ref(); + return instance; +} + +template +struct ros_message_type_caster { + public: + // TODO(Aditya): Apply constexpr message name to type caster macro + // after https://github.com/ros2/rosidl/issues/734 is resolved. + DRAKE_ROS_PYBIND_TYPECASTER(T, py::detail::_("RosTypeCaster[Unknown]")); + + bool load(py::handle src, bool) { return RosMessagePyToCpp(src, &value); } + + static py::handle cast(T src, py::return_value_policy policy, + py::handle parent) { + (void)policy; + (void)parent; + return RosMessageCppToPy(src); + } +}; + +} // namespace drake_ros_py +} // namespace drake_ros diff --git a/drake_ros/drake_ros/BUILD.bazel b/drake_ros/drake_ros/BUILD.bazel index a4ef580c..1a3408df 100644 --- a/drake_ros/drake_ros/BUILD.bazel +++ b/drake_ros/drake_ros/BUILD.bazel @@ -128,3 +128,38 @@ ros_py_test( main = "test/odr_py_test.py", deps = [":drake_ros_py"], ) + +pybind_py_library( + name = "ros_message_type_caster_test_via_specific_types_py", + testonly = 1, + cc_deps = [ + ":python_bindings_internal_hdrs", + "//:drake_ros_shared_library", + ], + cc_so_name = "ros_message_type_caster_test_via_specific_types", + cc_srcs = ["test/ros_message_type_caster_test_via_specific_types_py.cc"], + py_deps = ["@ros2//:rclpy_py"], +) + +pybind_py_library( + name = "ros_message_type_caster_test_via_all_py", + testonly = 1, + cc_deps = [ + ":python_bindings_internal_hdrs", + "//:drake_ros_shared_library", + ], + cc_so_name = "ros_message_type_caster_test_via_all", + cc_srcs = ["test/ros_message_type_caster_test_via_all_py.cc"], + py_deps = ["@ros2//:rclpy_py"], +) + +ros_py_test( + name = "ros_message_type_caster_test", + srcs = ["test/ros_message_type_caster_test.py"], + main = "test/ros_message_type_caster_test.py", + deps = [ + ":ros_message_type_caster_test_via_all_py", + ":ros_message_type_caster_test_via_specific_types_py", + "@ros2//:geometry_msgs_py", + ], +) diff --git a/drake_ros/drake_ros/test/ros_message_type_caster_test.py b/drake_ros/drake_ros/test/ros_message_type_caster_test.py new file mode 100644 index 00000000..46051908 --- /dev/null +++ b/drake_ros/drake_ros/test/ros_message_type_caster_test.py @@ -0,0 +1,66 @@ +import pytest +import random +import sys + +import drake_ros +from drake_ros.ros_message_type_caster_test_via_specific_types import ( + TestTypecastingViaSpecificMacro, +) +from drake_ros.ros_message_type_caster_test_via_all import ( + TestTypecastingViaAllMacro, +) + +from geometry_msgs.msg import Polygon, Point32, Quaternion + + +def make_quaternion(): + q = Quaternion() + q.x = 1.0 + q.y = 2.0 + q.z = 3.0 + q.w = 4.0 + return q + + +def make_polygon(): + p = Polygon() + for _ in range(10): + point = Point32() + point.x = float(random.randint(1, 100)) + point.y = float(random.randint(1, 100)) + point.z = float(random.randint(1, 100)) + p.points.append(point) + return p + + +def test_msg_quaternion(): + """ + This should convert via ROS_MSG_PYBIND_TYPECAST_ALL() and via + ROS_MSG_PYBIND_TYPECAST() + """ + q = make_quaternion() + + msg_converted = TestTypecastingViaAllMacro(msg=q) + assert q == msg_converted + + msg_converted = TestTypecastingViaSpecificMacro(msg=q) + assert q == msg_converted + + +def test_msg_polygon(): + """ + This should convert via ROS_MSG_PYBIND_TYPECAST_ALL(), but *not* + via ROS_MSG_PYBIND_TYPECAST() because we explicitly did not + declare it so. + """ + p = make_polygon() + + msg_converted = TestTypecastingViaAllMacro(msg=p) + assert p == msg_converted + + with pytest.raises(TypeError): + TestTypecastingViaSpecificMacro(msg=p) + + +if __name__ == '__main__': + sys.exit(pytest.main(sys.argv)) diff --git a/drake_ros/drake_ros/test/ros_message_type_caster_test_via_all_py.cc b/drake_ros/drake_ros/test/ros_message_type_caster_test_via_all_py.cc new file mode 100644 index 00000000..85c14161 --- /dev/null +++ b/drake_ros/drake_ros/test/ros_message_type_caster_test_via_all_py.cc @@ -0,0 +1,34 @@ +#include "geometry_msgs/msg/polygon.hpp" +#include +#include + +#include "drake_ros/core/ros_idl_pybind.h" +#include "drake_ros/drake_ros_pybind.h" + +// This is meant to be a test module, to just test +// the generic typecaster macro. +ROS_MSG_PYBIND_TYPECAST_ALL(); + +namespace drake_ros { +namespace drake_ros_py { +namespace { + +template +T TestTypecastingViaAllMacro(T msg) { + return msg; +} + +PYBIND11_MODULE(ros_message_type_caster_test_via_all, m) { + m.def("TestTypecastingViaAllMacro", + &TestTypecastingViaAllMacro, + py::arg("msg")); + m.def("TestTypecastingViaAllMacro", + &TestTypecastingViaAllMacro, + py::arg("msg")); +} + +} // namespace +// clang-format off +} // namespace drake_ros_py +// clang-format on +} // namespace drake_ros diff --git a/drake_ros/drake_ros/test/ros_message_type_caster_test_via_specific_types_py.cc b/drake_ros/drake_ros/test/ros_message_type_caster_test_via_specific_types_py.cc new file mode 100644 index 00000000..6cdde61d --- /dev/null +++ b/drake_ros/drake_ros/test/ros_message_type_caster_test_via_specific_types_py.cc @@ -0,0 +1,35 @@ +#include "geometry_msgs/msg/polygon.hpp" +#include +#include + +#include "drake_ros/core/ros_idl_pybind.h" +#include "drake_ros/drake_ros_pybind.h" + +// This is meant to be a test module, to just test +// the typecaster macro. +ROS_MSG_PYBIND_TYPECAST(geometry_msgs::msg::Quaternion); +// N.B. We explicitly do not add a typecaster for Polygon. + +namespace drake_ros { +namespace drake_ros_py { +namespace { + +template +T TestTypecastingViaSpecificMacro(T msg) { + return msg; +} + +PYBIND11_MODULE(ros_message_type_caster_test_via_specific_types, m) { + m.def("TestTypecastingViaSpecificMacro", + &TestTypecastingViaSpecificMacro, + py::arg("msg")); + m.def("TestTypecastingViaSpecificMacro", + &TestTypecastingViaSpecificMacro, + py::arg("msg")); +} + +} // namespace +// clang-format off +} // namespace drake_ros_py +// clang-format on +} // namespace drake_ros