Added point_cloud_transport_py

CMakeLists.txt → point_cloud_transport/CMakeLists.txt

@@ -13,17 +13,11 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
 endif()
 
 find_package(ament_cmake_ros REQUIRED)
-find_package(ament_cmake_python REQUIRED)
 
 find_package(message_filters REQUIRED)
 find_package(pluginlib REQUIRED)
-find_package(pybind11 REQUIRED)
-find_package(pybind11_vendor REQUIRED)
-find_package(python_cmake_module REQUIRED)
-find_package(Python3 REQUIRED COMPONENTS Interpreter Development)
 find_package(rclcpp REQUIRED)
 find_package(rclcpp_components REQUIRED)
-find_package(rclpy REQUIRED)
 find_package(sensor_msgs REQUIRED)
 
 include_directories(
@@ -105,33 +99,6 @@ target_link_libraries(list_transports
   ${PROJECT_NAME}
   pluginlib::pluginlib)
 
-
-# Install Python modules
-ament_python_install_package(${PROJECT_NAME})
-
-# Python bindings
-pybind11_add_module(_codec SHARED
-  src/pybind_codec.cpp
-)
-target_link_libraries(_codec PRIVATE
-  ${PROJECT_NAME}
-  pluginlib::pluginlib
-)
-
-# Install cython modules as sub-modules of the project
-install(
-  TARGETS
-    _codec
-  DESTINATION "${PYTHON_INSTALL_DIR}/${PROJECT_NAME}"
-)
-
-# Install Python executables
-install(PROGRAMS
-  point_cloud_transport/publisher.py
-  point_cloud_transport/subscriber.py
-  DESTINATION lib/${PROJECT_NAME}
-)
-
 # Install plugin descriptions
 pluginlib_export_plugin_description_file(${PROJECT_NAME} default_plugins.xml)
 

include/point_cloud_transport/point_cloud_transport.hpp → point_cloud_transport/include/point_cloud_transport/point_cloud_transport.hpp

@@ -151,12 +151,24 @@ class PointCloudTransport : public PointCloudTransportLoader
     return ret;
   }
 
+  //! Advertise a PointCloud2 topic, simple version.
+  POINT_CLOUD_TRANSPORT_PUBLIC
+  Publisher advertise(
+    const std::string & base_topic,
+    uint32_t queue_size)
+  {
+    rclcpp::PublisherOptions options = rclcpp::PublisherOptions();
+    rmw_qos_profile_t custom_qos = rmw_qos_profile_sensor_data;
+    custom_qos.depth = queue_size;
+    return Publisher(node_, base_topic, pub_loader_, custom_qos, options);
+  }
+
   //! Advertise a PointCloud2 topic, simple version.
   POINT_CLOUD_TRANSPORT_PUBLIC
   Publisher advertise(
     const std::string & base_topic,
     uint32_t queue_size,
-    const rclcpp::PublisherOptions & options = rclcpp::PublisherOptions())
+    const rclcpp::PublisherOptions & options)
   {
     rmw_qos_profile_t custom_qos = rmw_qos_profile_sensor_data;
     custom_qos.depth = queue_size;
@@ -172,6 +184,19 @@ class PointCloudTransport : public PointCloudTransportLoader
     return Publisher(node_, base_topic, pub_loader_, custom_qos, options);
   }
 
+  // //! Subscribe to a point cloud topic, version for arbitrary std::function object.
+  // POINT_CLOUD_TRANSPORT_PUBLIC
+  // point_cloud_transport::Subscriber subscribe(
+  //   const std::string & base_topic,
+  //   uint32_t queue_size,
+  //   const std::function<void(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &)> & callback)
+  // {
+  //   rmw_qos_profile_t custom_qos = rmw_qos_profile_sensor_data;
+  //   custom_qos.depth = queue_size;
+  //   return subscribe(
+  //     base_topic, custom_qos, callback, {}, nullptr);
+  // }
+
   //! Advertise an PointCloud2 topic with subscriber status callbacks.
   // TODO(ros2) Implement when SubscriberStatusCallback is available
   // point_cloud_transport::Publisher advertise(const std::string& base_topic, uint32_t queue_size,

package.xml → point_cloud_transport/package.xml

@@ -14,21 +14,16 @@
 
   <license>BSD</license>
 
-  <url type="repository">https://github.com/ctu-vras/point_cloud_transport</url>
-  <url type="bugtracker">https://github.com/ctu-vras/point_cloud_transport/issues</url>
+  <url type="repository">https://github.com/ros-perception/point_cloud_transport</url>
+  <url type="bugtracker">https://github.com/ros-perception/point_cloud_transport/issues</url>
 
   <buildtool_depend>ament_cmake_ros</buildtool_depend>
-  <buildtool_depend>ament_cmake_python</buildtool_depend>
 
   <depend>message_filters</depend>
-  <depend>pybind11</depend>
-  <depend>pybind11_vendor</depend>
   <depend>pluginlib</depend>
   <depend>rclcpp_components</depend>
   <depend>rclcpp</depend>
-  <depend>rclpy</depend>
   <depend>sensor_msgs</depend>
-  <depend>git</depend>
 
   <test_depend>ament_cmake_gtest</test_depend>
   <test_depend>ament_cmake_copyright</test_depend>

point_cloud_transport_py/CMakeLists.txt

@@ -0,0 +1,70 @@
+cmake_minimum_required(VERSION 3.5)
+project(point_cloud_transport_py)
+
+# Default to C99
+if(NOT CMAKE_C_STANDARD)
+  set(CMAKE_C_STANDARD 99)
+endif()
+
+# Default to C++17
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 17)
+  set(CMAKE_CXX_STANDARD_REQUIRED ON)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# Figure out Python3 debug/release before anything else can find_package it
+if(WIN32 AND CMAKE_BUILD_TYPE STREQUAL "Debug")
+  find_package(python_cmake_module REQUIRED)
+  find_package(PythonExtra REQUIRED)
+
+  # Force FindPython3 to use the debug interpreter where ROS 2 expects it
+  set(Python3_EXECUTABLE "${PYTHON_EXECUTABLE_DEBUG}")
+endif()
+
+
+find_package(ament_cmake REQUIRED)
+find_package(ament_cmake_python REQUIRED)
+find_package(ament_cmake_ros REQUIRED)
+find_package(point_cloud_transport REQUIRED)
+find_package(pluginlib REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(sensor_msgs REQUIRED)
+
+# Find python before pybind11
+find_package(Python3 REQUIRED COMPONENTS Interpreter Development)
+
+find_package(pybind11_vendor REQUIRED)
+find_package(pybind11 REQUIRED)
+
+ament_python_install_package(${PROJECT_NAME})
+
+pybind11_add_module(_point_cloud_transport SHARED
+  src/point_cloud_transport_py/pybind_point_cloud_transport.cpp
+)
+target_link_libraries(_point_cloud_transport PUBLIC
+  point_cloud_transport::point_cloud_transport
+  rclcpp::rclcpp
+  ${sensor_msgs_TARGETS}
+)
+
+pybind11_add_module(_codec SHARED
+  src/point_cloud_transport_py/pybind_codec.cpp
+)
+target_link_libraries(_codec PUBLIC
+  point_cloud_transport::point_cloud_transport
+  pluginlib::pluginlib
+)
+
+# Install cython modules as sub-modules of the project
+install(
+  TARGETS
+    _point_cloud_transport
+    _codec
+  DESTINATION "${PYTHON_INSTALL_DIR}/${PROJECT_NAME}"
+)
+
+ament_package()

point_cloud_transport/publisher.py → point_cloud_transport_py/examples/publisher.py

@@ -75,7 +75,7 @@ def __init__(self):
         print('Topics to publish: \n', self.topics_to_publish)
 
         self.declare_parameter('enable_pub_plugins', Parameter.Type.STRING_ARRAY)
-        whitelist = self.get_parameter('enable_pub_plugins')
+        whitelist = self.get_parameter_or('enable_pub_plugins', ["raw", "draco"])
 
         self.transport_publishers = {}
         for transport in self.topics_to_publish:
@@ -86,15 +86,16 @@ def __init__(self):
                 stringToMsgType(topic_to_publish.data_type), topic_to_publish.topic, 1)
 
     def publish(self, raw: PointCloud2):
-        for transport, transport_info in self.topics_to_publish.items():
-            compressed_buffer = self.codec.encode(
-                transport_info.name, pointCloud2ToString(raw))
-            if compressed_buffer:
-                # rclpy is smart and will publish the correct type even though we
-                # are giving it a serialized array of bytes
-                self.transport_publishers[transport].publish(compressed_buffer)
-            else:
-                self.get_logger().error('Error encoding message!')
+        # for transport, transport_info in self.topics_to_publish.items():
+        self.transport_publishers[transport].publish(raw)
+            # compressed_buffer = self.codec.encode(
+            #     transport_info.name, pointCloud2ToString(raw))
+            # if compressed_buffer:
+            #     # rclpy is smart and will publish the correct type even though we
+            #     # are giving it a serialized array of bytes
+            #     self.transport_publishers[transport].publish(raw)
+            # else:
+            #     self.get_logger().error('Error encoding message!')
 
 
 if __name__ == '__main__':

point_cloud_transport/subscriber.py → point_cloud_transport_py/examples/subscriber.py

@@ -36,6 +36,8 @@
 from point_cloud_transport.common import stringToMsgType, stringToPointCloud2, TransportInfo
 
 from rclpy.node import Node
+from rclpy.qos import qos_profile_sensor_data
+from sensor_msgs.msg import PointCloud2
 
 
 def _get_loadable_transports(codec: PointCloudCodec):
@@ -61,8 +63,8 @@ def __init__(self):
         node_name = 'point_cloud_transport_subscriber'
         super().__init__(node_name)
 
-        self.base_topic = 'point_cloud'
-        self.transport = self.get_parameter_or('transport', 'raw')
+        self.base_topic = '/pct/point_cloud'
+        self.transport = self.get_parameter_or('transport', 'draco')
         self.codec = PointCloudCodec()
 
         transports = _get_loadable_transports(self.codec)
@@ -78,13 +80,10 @@ def __init__(self):
 
         # subscribe to compressed, serialized msg
         self.subscriber = self.create_subscription(stringToMsgType(
-            self.transport_info.data_type), self.transport_info.topic, self.cb, 1, raw=True)
+            self.transport_info.data_type), self.transport_info.topic, self.cb, qos_profile_sensor_data)
 
-    def cb(self, serialized_buffer):
-        cloud_buffer = self.codec.decode(
-            self.transport_info.name, serialized_buffer)
-        cloud = stringToPointCloud2(cloud_buffer)
-        print(cloud)
+    def cb(self, cloud):
+        print(cloud.height * cloud.width)
 
 
 if __name__ == '__main__':

point_cloud_transport_py/package.xml

@@ -0,0 +1,34 @@
+<package format="3">
+  <name>point_cloud_transport_py</name>
+
+  <version>0.0.0</version>
+
+  <description>Python API for point_cloud_transport</description>
+
+  <author>Alejandro Hernandez Cordero</author>
+
+  <maintainer email="[email protected]">Alejandro Hernández</maintainer>
+  <maintainer email="[email protected]">John D'Angelo</maintainer>
+
+  <license>BSD</license>
+
+  <url type="repository">https://github.com/ros-perception/point_cloud_transport</url>
+  <url type="bugtracker">https://github.com/ros-perception/point_cloud_transport/issues</url>
+
+  <buildtool_depend>ament_cmake_ros</buildtool_depend>
+  <buildtool_depend>ament_cmake_python</buildtool_depend>
+  <buildtool_depend>python_cmake_module</buildtool_depend>
+
+  <depend>point_cloud_transport</depend>
+  <depend>pybind11_vendor</depend>
+  <depend>pluginlib</depend>
+  <depend>rclcpp</depend>
+  <depend>sensor_msgs</depend>
+
+  <exec_depend>rpyutils</exec_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+
+</package>

point_cloud_transport_py/point_cloud_transport_py/__init__.py

@@ -0,0 +1,38 @@
+# Copyright 2023 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from rpyutils import add_dll_directories_from_env
+
+# Since Python 3.8, on Windows we should ensure DLL directories are explicitly added
+# to the search path.
+# See https://docs.python.org/3/whatsnew/3.8.html#bpo-36085-whatsnew
+with add_dll_directories_from_env('PATH'):
+    from point_cloud_transport_py._point_cloud_transport import (
+        PointCloudTransport,
+        Publisher,
+        Subscriber,
+    )
+    from point_cloud_transport_py._codec import (
+        PointCloudCodec,
+        VectorString,
+    )
+
+
+__all__ = [
+    'Publisher',
+    'Subscriber',
+    'PointCloudTransport',
+    'PointCloudCodec',
+    'VectorString'
+]