From 37cfaf88b044f26529ffd6debc909388e0416d93 Mon Sep 17 00:00:00 2001 From: JafarAbdi Date: Fri, 26 May 2023 19:21:21 +0100 Subject: [PATCH 1/4] Python bindings --- CMakeLists.txt | 21 ++ README.md | 2 +- package.xml | 1 + python/examples/rviz_visual_tools_demo.py | 132 ++++++++ python/rviz_visual_tools/__init__.py | 9 + python/rviz_visual_tools/py.typed | 0 python/src/rviz_visual_tools.cpp | 370 ++++++++++++++++++++++ 7 files changed, 534 insertions(+), 1 deletion(-) create mode 100755 python/examples/rviz_visual_tools_demo.py create mode 100644 python/rviz_visual_tools/__init__.py create mode 100644 python/rviz_visual_tools/py.typed create mode 100644 python/src/rviz_visual_tools.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 8b45187..cc2e5ad 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -256,4 +256,25 @@ if (BUILD_TESTING) ) endif() +#################### +## Python binding ## +#################### + +find_package(Python3 REQUIRED COMPONENTS Interpreter Development) +find_package(pybind11_vendor REQUIRED) +find_package(pybind11 REQUIRED) + +ament_python_install_package(rviz_visual_tools PACKAGE_DIR python/rviz_visual_tools) + +pybind11_add_module(pyrviz_visual_tools python/src/rviz_visual_tools.cpp) +target_link_libraries(pyrviz_visual_tools PRIVATE rviz_visual_tools) +ament_target_dependencies(pyrviz_visual_tools pybind11) +install(TARGETS pyrviz_visual_tools + LIBRARY DESTINATION ${PYTHON_INSTALL_DIR}/rviz_visual_tools) +install(PROGRAMS + python/examples/rviz_visual_tools_demo.py + DESTINATION lib/${PROJECT_NAME} +) + + ament_package() diff --git a/README.md b/README.md index 26c74d2..18f4e04 100644 --- a/README.md +++ b/README.md @@ -54,7 +54,7 @@ To see random shapes generated in Rviz, first launch Rviz: Then start the RViz Visual Tools demo: - ros2 run rviz_visual_tools rviz_visual_tools_demo + ros2 run rviz_visual_tools rviz_visual_tools_demo # Or rviz_visual_tools_demo.py for the python version ## Code API diff --git a/package.xml b/package.xml index baa6449..dd807fd 100644 --- a/package.xml +++ b/package.xml @@ -41,6 +41,7 @@ qtbase5-dev eigen + pybind11_vendor eigen libqt5-widgets diff --git a/python/examples/rviz_visual_tools_demo.py b/python/examples/rviz_visual_tools_demo.py new file mode 100755 index 0000000..efc0c57 --- /dev/null +++ b/python/examples/rviz_visual_tools_demo.py @@ -0,0 +1,132 @@ +#!/usr/bin/env python3 + +import rviz_visual_tools as rvt +from geometry_msgs.msg import Point, Pose, Quaternion +from threading import Thread +import numpy as np +from copy import deepcopy +import time + +rvt.init() +node = rvt.RvizVisualToolsNode("rviz_visual_tools_demo") +visual_tools = rvt.RvizVisualTools(node, "world", "/rviz_visual_tools") +visual_tools.load_marker_publisher(True) +visual_tools.delete_all_markers() +visual_tools.enable_batch_publishing(True) + +point = Point(x=0.0, y=0.0, z=0.0) + +step_x = 0.1 + +visual_tools.publish_text( + Pose(), + "Shpere-Color-Range", + rvt.Colors.WHITE, + rvt.Scales.XXLARGE, + False, +) +for color in (rvt.Colors.BLACK, rvt.Colors.GREY, rvt.Colors.WHITE, rvt.Colors.RED): + visual_tools.publish_sphere(point, scale=rvt.Scales.XLARGE, color=color) + point.x += step_x +visual_tools.trigger() + + +visual_tools.publish_text( + Pose(position=Point(x=0.0, y=-0.2, z=0.0)), + "Coordinate-Axis", + rvt.Colors.WHITE, + rvt.Scales.XXLARGE, + False, +) +visual_tools.publish_axis(Pose(position=Point(x=0.0, y=-0.2, z=0.0))) +visual_tools.publish_axis( + Pose( + position=Point(x=0.25, y=-0.2, z=0.0), + orientation=Quaternion(x=0.0, y=0.0, z=1.0, w=0.0), + ) +) +visual_tools.trigger() + +visual_tools.publish_text( + Pose(position=Point(x=0.0, y=-0.4, z=0.0)), + "Arrows", + rvt.Colors.WHITE, + rvt.Scales.XXLARGE, + False, +) +visual_tools.publish_x_arrow( + Pose( + position=Point(x=0.0, y=-0.4, z=0.0), + orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ) +) +visual_tools.publish_y_arrow( + Pose( + position=Point(x=0.25, y=-0.4, z=0.0), + orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ) +) +visual_tools.publish_z_arrow( + Pose( + position=Point(x=0.5, y=-0.4, z=0.0), + orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ) +) +visual_tools.trigger() + +cuboid_max_size = 0.075 +cuboid_min_size = 0.01 +point1 = Point(x=0.0, y=-0.6, z=0.0) +visual_tools.publish_text( + Pose(position=point1), "Cuboid", rvt.Colors.WHITE, rvt.Scales.XXLARGE, False +) +for i in np.linspace(0, 1.0, 10): + point2 = deepcopy(point1) + point2.x += i * cuboid_max_size + cuboid_min_size + point2.y += i * cuboid_max_size + cuboid_min_size + point2.z += i * cuboid_max_size + cuboid_min_size + visual_tools.publish_cuboid(point1, point2, color=rvt.Colors.RAND) + point1.x += step_x +visual_tools.trigger() + +line_max_size = 0.075 +line_min_size = 0.01 +point1 = Point(x=0.0, y=-0.8, z=0.0) +visual_tools.publish_text( + Pose(position=point1), "Line", rvt.Colors.WHITE, rvt.Scales.XXLARGE, False +) +for i in np.linspace(0, 1.0, 10): + point2 = deepcopy(point1) + point2.x += i * line_max_size + line_min_size + point2.y += i * line_max_size + line_min_size + point2.z += i * line_max_size + line_min_size + visual_tools.publish_line(point1, point2, color=rvt.Colors.RAND) + point1.x += step_x +visual_tools.trigger() + +visual_tools.publish_cylinder( + Pose(position=Point(x=0.0, y=0.2, z=0.0)), color=rvt.Colors.RAND +) +visual_tools.trigger() + +visual_tools.publish_cone( + Pose(position=Point(x=0.0, y=0.4, z=0.0)), + angle=np.pi, + color=rvt.Colors.RAND, + scale=0.1, +) +visual_tools.trigger() + +plane_pose = Pose(position=Point(x=0.0, y=0.6, z=0.0)) +visual_tools.publish_xy_plane(plane_pose, color=rvt.Colors.RED, scale=0.1) +visual_tools.publish_xz_plane(plane_pose, color=rvt.Colors.GREEN, scale=0.1) +visual_tools.publish_yz_plane(plane_pose, color=rvt.Colors.BLUE, scale=0.1) +visual_tools.trigger() + +visual_tools.publish_axis_labeled(Pose(position=Point(x=0.0, y=0.8, z=0.0)), "MyAxis") +visual_tools.trigger() + +visual_tools.publish_path( + [Point(x=0.0, y=1.0, z=0.0), Point(x=0.2, y=1.0, z=0.0), Point(x=0.4, y=1.1, z=0.0)] +) +visual_tools.trigger() diff --git a/python/rviz_visual_tools/__init__.py b/python/rviz_visual_tools/__init__.py new file mode 100644 index 0000000..9a6f1e3 --- /dev/null +++ b/python/rviz_visual_tools/__init__.py @@ -0,0 +1,9 @@ +from rviz_visual_tools.pyrviz_visual_tools import ( + init, + shutdown, + Scales, + Colors, + EulerConvention, + RvizVisualTools, + RvizVisualToolsNode, +) diff --git a/python/rviz_visual_tools/py.typed b/python/rviz_visual_tools/py.typed new file mode 100644 index 0000000..e69de29 diff --git a/python/src/rviz_visual_tools.cpp b/python/src/rviz_visual_tools.cpp new file mode 100644 index 0000000..16a564b --- /dev/null +++ b/python/src/rviz_visual_tools.cpp @@ -0,0 +1,370 @@ +#include +#include +#include +#include +#include + +namespace py = pybind11; +using py::literals::operator""_a; + +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; +} + +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); + py::bytes bytes = + py::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); + py::module m = py::module::import((ros_msg_name.substr(0, pos1) + ".msg").c_str()); + + // retrieve type instance + py::object cls = m.attr(ros_msg_name.substr(pos2 + 1).c_str()); + + // deserialize into python object + py::module rclpy = py::module::import("rclpy.serialization"); + py::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 + py::module rclpy = py::module::import("rclpy.serialization"); + py::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 py::error_already_set(); + } + if (length < 0) + { + throw py::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: + RvizVisualToolsNode(const std::string& node_name) + { + node = rclcpp::Node::make_unique(node_name); + executor.add_node(node->get_node_base_interface()); + } + + ~RvizVisualToolsNode() + { + executor.remove_node(node->get_node_base_interface()); + stop_spin_thread(); + } + + [[nodiscard]] const rclcpp::Node& getNode() const + { + return *node; + } + + [[nodiscard]] rclcpp::Node* getMutableNode() const + { + return node.get(); + } + + void spin_all(int timeout_millis = 0) + { + executor.spin_all(std::chrono::milliseconds(timeout_millis)); + } + + void start_spin_thread() + { + stop_spin_thread(); + executor_thread = std::thread([this] { executor.spin(); }); + } + + void stop_spin_thread() + { + executor.cancel(); + if (executor_thread.joinable()) + { + executor_thread.join(); + } + } + +private: + rclcpp::Node::UniquePtr node; + rclcpp::executors::SingleThreadedExecutor executor; + std::thread executor_thread; +}; + +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) { + std::vector raw_args; + raw_args.reserve(args.size()); + for (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) + .value("BLUE", Colors::BLUE) + .value("CYAN", Colors::CYAN) + .value("GREY", Colors::GREY) + .value("DARK_GREY", Colors::DARK_GREY) + .value("GREEN", Colors::GREEN) + .value("LIME_GREEN", Colors::LIME_GREEN) + .value("MAGENTA", Colors::MAGENTA) + .value("ORANGE", Colors::ORANGE) + .value("PURPLE", Colors::PURPLE) + .value("RED", Colors::RED) + .value("PINK", Colors::PINK) + .value("WHITE", Colors::WHITE) + .value("YELLOW", Colors::YELLOW) + .value("TRANSLUCENT", Colors::TRANSLUCENT) + .value("TRANSLUCENT_LIGHT", Colors::TRANSLUCENT_LIGHT) + .value("TRANSLUCENT_DARK", Colors::TRANSLUCENT_DARK) + .value("RAND", Colors::RAND) + .value("CLEAR", Colors::CLEAR) + .value("DEFAULT", Colors::DEFAULT); + + py::enum_(m, "Scales") + .value("XXXXSMALL", Scales::XXXXSMALL) + .value("XXXSMALL", Scales::XXXSMALL) + .value("XXSMALL", Scales::XXSMALL) + .value("XSMALL", Scales::XSMALL) + .value("SMALL", Scales::SMALL) + .value("MEDIUM", Scales::MEDIUM) + .value("LARGE", Scales::LARGE) + .value("XLARGE", Scales::XLARGE) + .value("XXLARGE", Scales::XXLARGE) + .value("XXXLARGE", Scales::XXXLARGE) + .value("XXXXLARGE", Scales::XXXXLARGE); + + py::enum_(m, "EulerConvention") + .value("XYZ", EulerConvention::XYZ) + .value("ZYX", EulerConvention::ZYX) + .value("ZXZ", EulerConvention::ZXZ); + + py::class_(m, "RvizVisualTools") + .def(py::init([](RvizVisualToolsNode* node, const std::string& base_frame, + const std::string& marker_topic) { + return RvizVisualTools(base_frame, marker_topic, node->getMutableNode()); + })) + .def("delete_marker", &RvizVisualTools::deleteMarker) + .def("delete_all_markers", py::overload_cast<>(&RvizVisualTools::deleteAllMarkers)) + .def("delete_all_markers", + py::overload_cast(&RvizVisualTools::deleteAllMarkers)) + .def("reset_marker_counts", &RvizVisualTools::resetMarkerCounts) + .def("load_rviz_markers", &RvizVisualTools::loadRvizMarkers) + .def("set_marker_topic", &RvizVisualTools::setMarkerTopic) + .def("load_marker_publisher", &RvizVisualTools::loadMarkerPub) + .def("wait_for_marker_subscriber", &RvizVisualTools::waitForMarkerSub) + .def("set_alpha", &RvizVisualTools::setAlpha) + .def("set_lifetime", &RvizVisualTools::setLifetime) + .def("get_random_color", &RvizVisualTools::getRandColor) + .def("get_color", &RvizVisualTools::getColor) + .def("get_color_scale", &RvizVisualTools::getColorScale) + .def("get_scale", &RvizVisualTools::getScale) + .def_property("base_frame", &RvizVisualTools::getBaseFrame, &RvizVisualTools::setBaseFrame) + .def_property("global_scale", &RvizVisualTools::getGlobalScale, + &RvizVisualTools::setGlobalScale) + .def("enable_batch_publishing", &RvizVisualTools::enableBatchPublishing, "enable"_a = true) + .def("enable_frame_locking", &RvizVisualTools::enableFrameLocking) + .def("trigger_every", &RvizVisualTools::triggerEvery) + .def("trigger", &RvizVisualTools::trigger) + .def("publish_markers", &RvizVisualTools::publishMarkers) + .def("publish_cone", + py::overload_cast( + &RvizVisualTools::publishCone), + "pose"_a, "angle"_a, "color"_a = Colors::TRANSLUCENT, "scale"_a = 1.0) + .def("publish_abcd_plane", &RvizVisualTools::publishABCDPlane, "A"_a, "B"_a, "C"_a, "D"_a, + "color"_a = Colors::TRANSLUCENT, "x_width"_a = 1.0, "y_width"_a = 1.0) + .def("publish_normal_and_distance_plane", &RvizVisualTools::publishNormalAndDistancePlane, + "normal"_a, "distance"_a, "color"_a = Colors::TRANSLUCENT, "x_width"_a = 1.0, + "y_width"_a = 1.0) + .def("publish_xy_plane", + py::overload_cast( + &RvizVisualTools::publishXYPlane), + "pose"_a, "color"_a = Colors::TRANSLUCENT, "scale"_a = 1.0) + .def("publish_xz_plane", + py::overload_cast( + &RvizVisualTools::publishXZPlane), + "pose"_a, "color"_a = Colors::TRANSLUCENT, "scale"_a = 1.0) + .def("publish_yz_plane", + py::overload_cast( + &RvizVisualTools::publishYZPlane), + "pose"_a, "color"_a = Colors::TRANSLUCENT, "scale"_a = 1.0) + .def("publish_sphere", + py::overload_cast(&RvizVisualTools::publishSphere), + "point"_a, "color"_a = Colors::BLUE, "scale"_a = Scales::MEDIUM, "ns"_a = "Sphere", + "id"_a = 0) + .def("publish_sphere", + py::overload_cast(&RvizVisualTools::publishSphere), + "pose"_a, "color"_a = Colors::BLUE, "scale"_a = Scales::MEDIUM, "ns"_a = "Sphere", + "id"_a = 0) + .def("publish_spheres", + py::overload_cast&, Colors, Scales, + const std::string&>(&RvizVisualTools::publishSpheres), + "points"_a, "color"_a = Colors::BLUE, "scale"_a = Scales::MEDIUM, "ns"_a = "Spheres") + .def("publish_x_arrow", + py::overload_cast( + &RvizVisualTools::publishXArrow), + "pose"_a, "color"_a = Colors::RED, "scale"_a = Scales::MEDIUM, "length"_a = 0.0) + .def("publish_x_arrow", + py::overload_cast( + &RvizVisualTools::publishXArrow), + "pose_stamped"_a, "color"_a = Colors::RED, "scale"_a = Scales::MEDIUM, "length"_a = 0.0) + .def("publish_y_arrow", + py::overload_cast( + &RvizVisualTools::publishYArrow), + "pose"_a, "color"_a = Colors::GREEN, "scale"_a = Scales::MEDIUM, "length"_a = 0.0) + .def("publish_y_arrow", + py::overload_cast( + &RvizVisualTools::publishYArrow), + "pose_stamped"_a, "color"_a = Colors::GREEN, "scale"_a = Scales::MEDIUM, + "length"_a = 0.0) + .def("publish_z_arrow", + py::overload_cast( + &RvizVisualTools::publishZArrow), + "pose"_a, "color"_a = Colors::BLUE, "scale"_a = Scales::MEDIUM, "length"_a = 0.0) + .def("publish_z_arrow", + py::overload_cast( + &RvizVisualTools::publishZArrow), + "pose_stamped"_a, "color"_a = Colors::BLUE, "scale"_a = Scales::MEDIUM, "length"_a = 0.0) + .def("publish_cuboid", + py::overload_cast( + &RvizVisualTools::publishCuboid), + "point1"_a, "point2"_a, "color"_a = Colors::BLUE, "ns"_a = "Cuboid", "id"_a = 0) + .def("publish_cuboid", + py::overload_cast( + &RvizVisualTools::publishCuboid), + "pose"_a, "depth"_a, "width"_a, "height"_a, "color"_a = Colors::BLUE) + .def("publish_line", + py::overload_cast(&RvizVisualTools::publishLine), + "point1"_a, "point2"_a, "color"_a = Colors::BLUE, "scale"_a = Scales::MEDIUM) + .def("publish_path", + py::overload_cast&, Colors, Scales, + const std::string&>(&RvizVisualTools::publishPath), + "path"_a, "color"_a = Colors::RED, "scale"_a = Scales::MEDIUM, "ns"_a = "Path") + .def("publish_polygon", &RvizVisualTools::publishPolygon, "polygon"_a, + "color"_a = Colors::RED, "scale"_a = Scales::MEDIUM, "ns"_a = "Polygon") + .def("publish_axis_labeled", + py::overload_cast( + &RvizVisualTools::publishAxisLabeled), + "pose"_a, "label"_a, "scale"_a = Scales::MEDIUM, "color"_a = Colors::WHITE) + .def("publish_axis", + py::overload_cast( + &RvizVisualTools::publishAxis), + "pose"_a, "scale"_a = Scales::MEDIUM, "ns"_a = "Axis") + .def("publish_cylinder", + py::overload_cast(&RvizVisualTools::publishCylinder), + "pose"_a, "color"_a = Colors::BLUE, "height"_a = 0.1, "radius"_a = 0.01, + "ns"_a = "Cylinder") + .def("publish_mesh", + py::overload_cast(&RvizVisualTools::publishMesh), + "pose"_a, "filename"_a, "color"_a = Colors::CLEAR, "scale"_a = 1.0, "ns"_a = "Mesh", + "id"_a = 0) + .def("publish_text", + py::overload_cast(&RvizVisualTools::publishText), + "pose"_a, "text"_a, "color"_a = Colors::WHITE, "scale"_a = Scales::MEDIUM, + "static_id"_a = true); +} +} // namespace rviz_visual_tools From c0a3c690371ab1d0d43d5c51dbeff593509ca56a Mon Sep 17 00:00:00 2001 From: JafarAbdi Date: Fri, 2 Jun 2023 20:29:20 +0000 Subject: [PATCH 2/4] Refactor pybind11 common functionalities into a separate package --- CMakeLists.txt | 27 ++- python/examples/rviz_visual_tools_demo.py | 14 +- .../rviz_visual_tools/binding_utils.hpp | 121 ++++++++++++ python/src/binding_utils.cpp | 57 ++++++ python/src/rviz_visual_tools.cpp | 172 +----------------- 5 files changed, 216 insertions(+), 175 deletions(-) create mode 100644 python/include/rviz_visual_tools/binding_utils.hpp create mode 100644 python/src/binding_utils.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index cc2e5ad..35c6ca9 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} HAS_LIBRARY_TARGET) +ament_export_targets(export_${PROJECT_NAME} rviz_visual_tools_binding_utilsTargets HAS_LIBRARY_TARGET) ament_export_dependencies(geometry_msgs shape_msgs rclcpp @@ -231,12 +231,6 @@ ament_export_dependencies(geometry_msgs rviz_ogre_vendor ) ament_export_include_directories(include) -ament_export_libraries( - ${PROJECT_NAME} - ${PROJECT_NAME}_remote_control - ${PROJECT_NAME}_imarker_simple - ${PROJECT_NAME}_gui -) ########## ## TEST ## @@ -266,9 +260,25 @@ find_package(pybind11 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) +target_link_libraries(pyrviz_visual_tools PRIVATE rviz_visual_tools rviz_visual_tools_binding_utils) 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) install(PROGRAMS @@ -276,5 +286,4 @@ install(PROGRAMS DESTINATION lib/${PROJECT_NAME} ) - ament_package() diff --git a/python/examples/rviz_visual_tools_demo.py b/python/examples/rviz_visual_tools_demo.py index efc0c57..c926391 100755 --- a/python/examples/rviz_visual_tools_demo.py +++ b/python/examples/rviz_visual_tools_demo.py @@ -1,11 +1,13 @@ #!/usr/bin/env python3 import rviz_visual_tools as rvt +from rclpy.logging import get_logger from geometry_msgs.msg import Point, Pose, Quaternion from threading import Thread import numpy as np from copy import deepcopy -import time + +LOGGER = get_logger("rviz_visual_tools_demo") rvt.init() node = rvt.RvizVisualToolsNode("rviz_visual_tools_demo") @@ -18,6 +20,7 @@ step_x = 0.1 +LOGGER.info("Displaying range of colors red->green") visual_tools.publish_text( Pose(), "Shpere-Color-Range", @@ -31,6 +34,7 @@ visual_tools.trigger() +LOGGER.info("Displaying Coordinate Axis") visual_tools.publish_text( Pose(position=Point(x=0.0, y=-0.2, z=0.0)), "Coordinate-Axis", @@ -47,6 +51,7 @@ ) visual_tools.trigger() +LOGGER.info("Displaying Arrows") visual_tools.publish_text( Pose(position=Point(x=0.0, y=-0.4, z=0.0)), "Arrows", @@ -77,6 +82,7 @@ cuboid_max_size = 0.075 cuboid_min_size = 0.01 point1 = Point(x=0.0, y=-0.6, z=0.0) +LOGGER.info("Displaying Rectangular Cuboid") visual_tools.publish_text( Pose(position=point1), "Cuboid", rvt.Colors.WHITE, rvt.Scales.XXLARGE, False ) @@ -92,6 +98,7 @@ line_max_size = 0.075 line_min_size = 0.01 point1 = Point(x=0.0, y=-0.8, z=0.0) +LOGGER.info("Displaying Lines") visual_tools.publish_text( Pose(position=point1), "Line", rvt.Colors.WHITE, rvt.Scales.XXLARGE, False ) @@ -104,11 +111,13 @@ point1.x += step_x visual_tools.trigger() +LOGGER.info("Displaying Cylinder") visual_tools.publish_cylinder( Pose(position=Point(x=0.0, y=0.2, z=0.0)), color=rvt.Colors.RAND ) visual_tools.trigger() +LOGGER.info("Displaying Cone") visual_tools.publish_cone( Pose(position=Point(x=0.0, y=0.4, z=0.0)), angle=np.pi, @@ -117,15 +126,18 @@ ) visual_tools.trigger() +LOGGER.info("Displaying Planes") plane_pose = Pose(position=Point(x=0.0, y=0.6, z=0.0)) visual_tools.publish_xy_plane(plane_pose, color=rvt.Colors.RED, scale=0.1) visual_tools.publish_xz_plane(plane_pose, color=rvt.Colors.GREEN, scale=0.1) visual_tools.publish_yz_plane(plane_pose, color=rvt.Colors.BLUE, scale=0.1) visual_tools.trigger() +LOGGER.info("Displaying Labeled Coordinate Axis") visual_tools.publish_axis_labeled(Pose(position=Point(x=0.0, y=0.8, z=0.0)), "MyAxis") visual_tools.trigger() +LOGGER.info("Displaying Path") visual_tools.publish_path( [Point(x=0.0, y=1.0, z=0.0), Point(x=0.2, y=1.0, z=0.0), Point(x=0.4, y=1.1, z=0.0)] ) diff --git a/python/include/rviz_visual_tools/binding_utils.hpp b/python/include/rviz_visual_tools/binding_utils.hpp new file mode 100644 index 0000000..b303e35 --- /dev/null +++ b/python/include/rviz_visual_tools/binding_utils.hpp @@ -0,0 +1,121 @@ +#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/src/binding_utils.cpp b/python/src/binding_utils.cpp new file mode 100644 index 0000000..0538bf6 --- /dev/null +++ b/python/src/binding_utils.cpp @@ -0,0 +1,57 @@ +#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 16a564b..9af4a51 100644 --- a/python/src/rviz_visual_tools.cpp +++ b/python/src/rviz_visual_tools.cpp @@ -2,174 +2,16 @@ #include #include #include -#include +#include namespace py = pybind11; using py::literals::operator""_a; -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; -} - -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); - py::bytes bytes = - py::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); - py::module m = py::module::import((ros_msg_name.substr(0, pos1) + ".msg").c_str()); - - // retrieve type instance - py::object cls = m.attr(ros_msg_name.substr(pos2 + 1).c_str()); - - // deserialize into python object - py::module rclpy = py::module::import("rclpy.serialization"); - py::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 - py::module rclpy = py::module::import("rclpy.serialization"); - py::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 py::error_already_set(); - } - if (length < 0) - { - throw py::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: - RvizVisualToolsNode(const std::string& node_name) - { - node = rclcpp::Node::make_unique(node_name); - executor.add_node(node->get_node_base_interface()); - } - - ~RvizVisualToolsNode() - { - executor.remove_node(node->get_node_base_interface()); - stop_spin_thread(); - } - - [[nodiscard]] const rclcpp::Node& getNode() const - { - return *node; - } - - [[nodiscard]] rclcpp::Node* getMutableNode() const - { - return node.get(); - } - - void spin_all(int timeout_millis = 0) - { - executor.spin_all(std::chrono::milliseconds(timeout_millis)); - } - - void start_spin_thread() - { - stop_spin_thread(); - executor_thread = std::thread([this] { executor.spin(); }); - } - - void stop_spin_thread() - { - executor.cancel(); - if (executor_thread.joinable()) - { - executor_thread.join(); - } - } - -private: - rclcpp::Node::UniquePtr node; - rclcpp::executors::SingleThreadedExecutor executor; - std::thread executor_thread; -}; - PYBIND11_MODULE(pyrviz_visual_tools, m) { - py::class_(m, "RvizVisualToolsNode") + 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) @@ -180,10 +22,10 @@ PYBIND11_MODULE(pyrviz_visual_tools, m) */ m.def( "init", - [](std::vector args) { + [](const std::vector& args) { std::vector raw_args; raw_args.reserve(args.size()); - for (auto& arg : args) + for (const auto& arg : args) { raw_args.push_back(arg.c_str()); } @@ -237,9 +79,9 @@ PYBIND11_MODULE(pyrviz_visual_tools, m) .value("ZXZ", EulerConvention::ZXZ); py::class_(m, "RvizVisualTools") - .def(py::init([](RvizVisualToolsNode* node, const std::string& base_frame, + .def(py::init([](const RvizVisualToolsNode::SharedPtr& node, const std::string& base_frame, const std::string& marker_topic) { - return RvizVisualTools(base_frame, marker_topic, node->getMutableNode()); + return RvizVisualTools(base_frame, marker_topic, node); })) .def("delete_marker", &RvizVisualTools::deleteMarker) .def("delete_all_markers", py::overload_cast<>(&RvizVisualTools::deleteAllMarkers)) @@ -252,7 +94,7 @@ PYBIND11_MODULE(pyrviz_visual_tools, m) .def("wait_for_marker_subscriber", &RvizVisualTools::waitForMarkerSub) .def("set_alpha", &RvizVisualTools::setAlpha) .def("set_lifetime", &RvizVisualTools::setLifetime) - .def("get_random_color", &RvizVisualTools::getRandColor) + .def_static("get_random_color", &RvizVisualTools::getRandColor) .def("get_color", &RvizVisualTools::getColor) .def("get_color_scale", &RvizVisualTools::getColorScale) .def("get_scale", &RvizVisualTools::getScale) From 391e41ac21594cbf8cf404c6df0143dc5cb84d8e Mon Sep 17 00:00:00 2001 From: JafarAbdi Date: Fri, 2 Jun 2023 20:46:37 +0000 Subject: [PATCH 3/4] Use sys.argv for rclcpp::init when no args are passed --- python/src/rviz_visual_tools.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/python/src/rviz_visual_tools.cpp b/python/src/rviz_visual_tools.cpp index 9af4a51..1821034 100644 --- a/python/src/rviz_visual_tools.cpp +++ b/python/src/rviz_visual_tools.cpp @@ -22,7 +22,11 @@ PYBIND11_MODULE(pyrviz_visual_tools, m) */ m.def( "init", - [](const std::vector& args) { + [](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) From a84901bc16464d259bf9b3beb6ce091bb3e9a237 Mon Sep 17 00:00:00 2001 From: JafarAbdi Date: Wed, 13 Nov 2024 21:38:48 +0000 Subject: [PATCH 4/4] Use py_binding_tools --- CMakeLists.txt | 22 +--- package.xml | 1 + python/examples/rviz_visual_tools_demo.py | 5 +- .../rviz_visual_tools/binding_utils.hpp | 121 ------------------ python/rviz_visual_tools/__init__.py | 3 - python/src/binding_utils.cpp | 57 --------- python/src/rviz_visual_tools.cpp | 34 +---- 7 files changed, 11 insertions(+), 232 deletions(-) delete mode 100644 python/include/rviz_visual_tools/binding_utils.hpp delete mode 100644 python/src/binding_utils.cpp 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); }))