Refactor pybind11 common functionalities into a separate package

Author: JafarAbdi

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,15 +260,30 @@ 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
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/python/include>
+  $<INSTALL_INTERFACE:include/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)
+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
   python/examples/rviz_visual_tools_demo.py
   DESTINATION lib/${PROJECT_NAME}
 )
 
-
 ament_package()

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)]
 )

python/include/rviz_visual_tools/binding_utils.hpp

@@ -0,0 +1,121 @@
+#pragma once
+
+#include <pybind11/pybind11.h>
+#include <rclcpp/executors/single_threaded_executor.hpp>
+#include <rclcpp/node.hpp>
+#include <rclcpp/serialization.hpp>
+#include <rclcpp/serialized_message.hpp>
+#include <rosidl_runtime_cpp/traits.hpp>
+
+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 <typename T>
+struct RosMsgTypeCaster
+{
+  // C++ -> Python
+  static handle cast(const T& src, return_value_policy /* policy */, handle /* parent */)
+  {
+    // serialize src
+    rclcpp::Serialization<T> serializer;
+    rclcpp::SerializedMessage serialized_msg;
+    serializer.serialize_message(&src, &serialized_msg);
+    pybind11::bytes bytes = pybind11::bytes(
+        reinterpret_cast<const char*>(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<T>();
+
+    // 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<T>()))
+      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<uint8_t*>(serialized_buffer);
+    rmw_ret_t rmw_ret = rmw_deserialize(
+        &rcl_serialized_msg, rosidl_typesupport_cpp::get_message_type_support_handle<T>(), &value);
+    if (RMW_RET_OK != rmw_ret)
+    {
+      throw std::runtime_error("failed to deserialize ROS message");
+    }
+    return true;
+  }
+
+  PYBIND11_TYPE_CASTER(T, _<T>());
+};
+
+template <typename T>
+struct type_caster<T, enable_if_t<rosidl_generator_traits::is_message<T>::value>>
+  : RosMsgTypeCaster<T>
+{
+};
+}  // 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

python/src/binding_utils.cpp

@@ -0,0 +1,57 @@
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp/serialization.hpp>
+#include <rviz_visual_tools/binding_utils.hpp>
+
+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<std::string>(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();
+  }
+}