diff --git a/CMakeLists.txt b/CMakeLists.txt index 35c6ca9..2292ca3 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -213,7 +213,7 @@ install(DIRECTORY include/ DESTINATION include) # Install shared resources install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) -ament_export_targets(export_${PROJECT_NAME} rviz_visual_tools_binding_utilsTargets HAS_LIBRARY_TARGET) +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) ament_export_dependencies(geometry_msgs shape_msgs rclcpp @@ -257,27 +257,14 @@ endif() find_package(Python3 REQUIRED COMPONENTS Interpreter Development) find_package(pybind11_vendor REQUIRED) find_package(pybind11 REQUIRED) +find_package(py_binding_tools REQUIRED) ament_python_install_package(rviz_visual_tools PACKAGE_DIR python/rviz_visual_tools) -add_library(rviz_visual_tools_binding_utils SHARED python/src/binding_utils.cpp) -ament_target_dependencies(rviz_visual_tools_binding_utils rclcpp pybind11) -target_include_directories(rviz_visual_tools_binding_utils PUBLIC - $ - $ -) - pybind11_add_module(pyrviz_visual_tools python/src/rviz_visual_tools.cpp) -target_link_libraries(pyrviz_visual_tools PRIVATE rviz_visual_tools rviz_visual_tools_binding_utils) +target_link_libraries(pyrviz_visual_tools PRIVATE rviz_visual_tools py_binding_tools::py_binding_tools) ament_target_dependencies(pyrviz_visual_tools pybind11) -install( - TARGETS - rviz_visual_tools_binding_utils - EXPORT - rviz_visual_tools_binding_utilsTargets - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib -) + install(DIRECTORY python/include/ DESTINATION include/rviz_visual_tools) install(TARGETS pyrviz_visual_tools LIBRARY DESTINATION ${PYTHON_INSTALL_DIR}/rviz_visual_tools) @@ -285,5 +272,6 @@ install(PROGRAMS python/examples/rviz_visual_tools_demo.py DESTINATION lib/${PROJECT_NAME} ) +ament_export_dependencies(py_binding_tools) ament_package() diff --git a/package.xml b/package.xml index dd807fd..71aa3fb 100644 --- a/package.xml +++ b/package.xml @@ -38,6 +38,7 @@ visualization_msgs std_msgs trajectory_msgs + py_binding_tools qtbase5-dev eigen diff --git a/python/examples/rviz_visual_tools_demo.py b/python/examples/rviz_visual_tools_demo.py index c926391..f7a6495 100755 --- a/python/examples/rviz_visual_tools_demo.py +++ b/python/examples/rviz_visual_tools_demo.py @@ -6,11 +6,12 @@ from threading import Thread import numpy as np from copy import deepcopy +import rclcpp LOGGER = get_logger("rviz_visual_tools_demo") -rvt.init() -node = rvt.RvizVisualToolsNode("rviz_visual_tools_demo") +rclcpp.init() +node = rclcpp.Node("rviz_visual_tools_demo") visual_tools = rvt.RvizVisualTools(node, "world", "/rviz_visual_tools") visual_tools.load_marker_publisher(True) visual_tools.delete_all_markers() diff --git a/python/include/rviz_visual_tools/binding_utils.hpp b/python/include/rviz_visual_tools/binding_utils.hpp deleted file mode 100644 index b303e35..0000000 --- a/python/include/rviz_visual_tools/binding_utils.hpp +++ /dev/null @@ -1,121 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include -#include - -PYBIND11_EXPORT pybind11::object createMessage(const std::string& ros_msg_name); - -PYBIND11_EXPORT bool convertible(const pybind11::handle& h, const std::string& ros_msg_name); - -namespace pybind11 -{ -namespace detail -{ -// Base class for type conversion (C++ <-> python) of ROS message types -template -struct RosMsgTypeCaster -{ - // C++ -> Python - static handle cast(const T& src, return_value_policy /* policy */, handle /* parent */) - { - // serialize src - rclcpp::Serialization serializer; - rclcpp::SerializedMessage serialized_msg; - serializer.serialize_message(&src, &serialized_msg); - pybind11::bytes bytes = pybind11::bytes( - reinterpret_cast(serialized_msg.get_rcl_serialized_message().buffer), - serialized_msg.get_rcl_serialized_message().buffer_length); - - // get Python object type - const std::string ros_msg_name = rosidl_generator_traits::name(); - - // find delimiting '/' in ros_msg_name - std::size_t pos1 = ros_msg_name.find('/'); - std::size_t pos2 = ros_msg_name.find('/', pos1 + 1); - pybind11::module m = pybind11::module::import((ros_msg_name.substr(0, pos1) + ".msg").c_str()); - - // retrieve type instance - pybind11::object cls = m.attr(ros_msg_name.substr(pos2 + 1).c_str()); - - // deserialize into python object - pybind11::module rclpy = pybind11::module::import("rclpy.serialization"); - pybind11::object msg = rclpy.attr("deserialize_message")(bytes, cls); - - return msg.release(); - } - - // Python -> C++ - bool load(handle src, bool /*convert*/) - { - // check datatype of src - if (!convertible(src, rosidl_generator_traits::name())) - return false; - - // serialize src into python buffer - pybind11::module rclpy = pybind11::module::import("rclpy.serialization"); - pybind11::bytes bytes = rclpy.attr("serialize_message")(src); - - // deserialize into C++ object - rcl_serialized_message_t rcl_serialized_msg = rmw_get_zero_initialized_serialized_message(); - char* serialized_buffer; - Py_ssize_t length; - if (PYBIND11_BYTES_AS_STRING_AND_SIZE(bytes.ptr(), &serialized_buffer, &length)) - { - throw pybind11::error_already_set(); - } - if (length < 0) - { - throw pybind11::error_already_set(); - } - rcl_serialized_msg.buffer_capacity = length; - rcl_serialized_msg.buffer_length = length; - rcl_serialized_msg.buffer = reinterpret_cast(serialized_buffer); - rmw_ret_t rmw_ret = rmw_deserialize( - &rcl_serialized_msg, rosidl_typesupport_cpp::get_message_type_support_handle(), &value); - if (RMW_RET_OK != rmw_ret) - { - throw std::runtime_error("failed to deserialize ROS message"); - } - return true; - } - - PYBIND11_TYPE_CASTER(T, _()); -}; - -template -struct type_caster::value>> - : RosMsgTypeCaster -{ -}; -} // namespace detail -} // namespace pybind11 - -namespace rviz_visual_tools -{ -// The idea was taken from -// https://github.com/RobotLocomotion/drake-ros/blob/main/drake_ros/core/drake_ros.h -/** A ROS interface that wraps a live rclcpp node.*/ -class RvizVisualToolsNode final : public rclcpp::Node -{ -public: - RCLCPP_SMART_PTR_DEFINITIONS(RvizVisualToolsNode) - - RvizVisualToolsNode(const std::string& node_name); - - ~RvizVisualToolsNode(); - - void spin_all(int timeout_millis = 0); - - void start_spin_thread(); - - void stop_spin_thread(); - -private: - rclcpp::executors::SingleThreadedExecutor executor; - std::thread executor_thread; -}; -} // namespace rviz_visual_tools diff --git a/python/rviz_visual_tools/__init__.py b/python/rviz_visual_tools/__init__.py index 9a6f1e3..76e32fd 100644 --- a/python/rviz_visual_tools/__init__.py +++ b/python/rviz_visual_tools/__init__.py @@ -1,9 +1,6 @@ from rviz_visual_tools.pyrviz_visual_tools import ( - init, - shutdown, Scales, Colors, EulerConvention, RvizVisualTools, - RvizVisualToolsNode, ) diff --git a/python/src/binding_utils.cpp b/python/src/binding_utils.cpp deleted file mode 100644 index 0538bf6..0000000 --- a/python/src/binding_utils.cpp +++ /dev/null @@ -1,57 +0,0 @@ -#include -#include -#include - -namespace py = pybind11; - -PYBIND11_EXPORT py::object createMessage(const std::string& ros_msg_name) -{ - // find delimiting '/' in ros msg name - std::size_t pos = ros_msg_name.find('/'); - // import module - py::module m = py::module::import((ros_msg_name.substr(0, pos) + ".msg").c_str()); - // retrieve type instance - py::object cls = m.attr(ros_msg_name.substr(pos + 1).c_str()); - // create message instance - return cls(); -} - -PYBIND11_EXPORT bool convertible(const pybind11::handle& h, const std::string& ros_msg_name) -{ - PyObject* o = h.attr("__class__").attr("__name__").ptr(); - std::size_t pos = ros_msg_name.find_last_of('/'); - std::string class_name = ros_msg_name.substr(pos + 1); - return py::cast(o) == class_name; -} - -rviz_visual_tools::RvizVisualToolsNode::RvizVisualToolsNode(const std::string& node_name) - : Node(node_name) -{ - executor.add_node(get_node_base_interface()); -} - -rviz_visual_tools::RvizVisualToolsNode::~RvizVisualToolsNode() -{ - executor.remove_node(get_node_base_interface()); - stop_spin_thread(); -} - -void rviz_visual_tools::RvizVisualToolsNode::spin_all(int timeout_millis) -{ - executor.spin_all(std::chrono::milliseconds(timeout_millis)); -} - -void rviz_visual_tools::RvizVisualToolsNode::start_spin_thread() -{ - stop_spin_thread(); - executor_thread = std::thread([this] { executor.spin(); }); -} - -void rviz_visual_tools::RvizVisualToolsNode::stop_spin_thread() -{ - executor.cancel(); - if (executor_thread.joinable()) - { - executor_thread.join(); - } -} diff --git a/python/src/rviz_visual_tools.cpp b/python/src/rviz_visual_tools.cpp index 1821034..8334af5 100644 --- a/python/src/rviz_visual_tools.cpp +++ b/python/src/rviz_visual_tools.cpp @@ -2,7 +2,7 @@ #include #include #include -#include +#include namespace py = pybind11; using py::literals::operator""_a; @@ -11,36 +11,6 @@ namespace rviz_visual_tools { PYBIND11_MODULE(pyrviz_visual_tools, m) { - py::class_(m, "RvizVisualToolsNode") - .def(py::init(), "node_name"_a) - .def("spin_all", &RvizVisualToolsNode::spin_all, "timeout_millis"_a = 0) - .def("start_spin_thread", &RvizVisualToolsNode::start_spin_thread) - .def("stop_spin_thread", &RvizVisualToolsNode::stop_spin_thread); - - /**Initialize ROS 2's global context. - This function decorates a `rclcpp::init` invocation. - */ - m.def( - "init", - [](std::vector args) { - if (args.empty()) - { - args = py::module::import("sys").attr("argv").cast>(); - } - std::vector raw_args; - raw_args.reserve(args.size()); - for (const auto& arg : args) - { - raw_args.push_back(arg.c_str()); - } - if (!rclcpp::ok()) - { - rclcpp::init(raw_args.size(), raw_args.data()); - } - }, - py::arg("args") = std::vector{}); - m.def("shutdown", &rclcpp::shutdown); - py::enum_(m, "Colors") .value("BLACK", Colors::BLACK) .value("BROWN", Colors::BROWN) @@ -83,7 +53,7 @@ PYBIND11_MODULE(pyrviz_visual_tools, m) .value("ZXZ", EulerConvention::ZXZ); py::class_(m, "RvizVisualTools") - .def(py::init([](const RvizVisualToolsNode::SharedPtr& node, const std::string& base_frame, + .def(py::init([](const rclcpp::Node::SharedPtr& node, const std::string& base_frame, const std::string& marker_topic) { return RvizVisualTools(base_frame, marker_topic, node); }))