perception: Add PointCloudToLcm system (RobotLocomotion#14990)

Add new lcmt_point_cloud message type for it to encode into.

bindings/pydrake/perception_py.cc

@@ -8,6 +8,7 @@
 #include "drake/bindings/pydrake/pydrake_pybind.h"
 #include "drake/perception/depth_image_to_point_cloud.h"
 #include "drake/perception/point_cloud.h"
+#include "drake/perception/point_cloud_to_lcm.h"
 
 namespace drake {
 namespace pydrake {
@@ -140,6 +141,14 @@ void init_perception(py::module m) {
         .def("point_cloud_output_port", &Class::point_cloud_output_port,
             py_rvp::reference_internal, cls_doc.point_cloud_output_port.doc);
   }
+
+  {
+    using Class = PointCloudToLcm;
+    constexpr auto& cls_doc = doc.PointCloudToLcm;
+    py::class_<Class, LeafSystem<double>>(m, "PointCloudToLcm", cls_doc.doc)
+        .def(py::init<std::string>(), py::arg("frame_name") = std::string(),
+            cls_doc.ctor.doc);
+  }
 }
 
 PYBIND11_MODULE(perception, m) {

bindings/pydrake/test/perception_test.py

@@ -93,3 +93,8 @@ def test_depth_image_to_point_cloud_api(self):
             pixel_type=PixelType.kDepth16U,
             scale=0.001,
             fields=mut.BaseField.kXYZs | mut.BaseField.kRGBs)
+
+    def test_point_cloud_to_lcm(self):
+        dut = mut.PointCloudToLcm(frame_name="world")
+        dut.get_input_port()
+        dut.get_output_port()

lcmtypes/BUILD.bazel

@@ -120,6 +120,15 @@ drake_lcm_cc_library(
     lcm_srcs = ["lcmt_point.lcm"],
 )
 
+drake_lcm_cc_library(
+    name = "point_cloud",
+    lcm_package = "drake",
+    lcm_srcs = [
+        "lcmt_point_cloud.lcm",
+        "lcmt_point_cloud_field.lcm",
+    ],
+)
+
 drake_lcm_cc_library(
     name = "point_pair_contact_info_for_viz",
     lcm_package = "drake",
@@ -264,6 +273,7 @@ LCMTYPES_CC = [
     ":allegro",
     ":call_python",
     ":point",
+    ":point_cloud",
     ":header",
     ":quaternion",
     ":image",

lcmtypes/lcmt_point_cloud.lcm

@@ -0,0 +1,56 @@
+package drake;
+
+// Generic point cloud with runtime typing.
+//
+// Modeled after PCL and ROS conventions:
+// https://pointclouds.org/documentation/structpcl_1_1_p_c_l_point_cloud2.html
+// https://docs.ros.org/en/api/sensor_msgs/html/msg/PointCloud2.html
+//
+struct lcmt_point_cloud {
+  // Timestamp in microseconds.
+  int64_t utime;
+
+  // The name of a frame this data is associated with.
+  string frame_name;
+
+  // If the cloud is structured, this is the 2D width.
+  // If the cloud is unstructured, this is the number of points in the cloud.
+  int64_t width;
+
+  // If the cloud is structured, this is the 2D height.
+  // If the cloud is unstructured, this is set to 1.
+  int64_t height;
+
+  // Descriptions of the fields.
+  int32_t num_fields;
+  lcmt_point_cloud_field fields[num_fields];
+
+  // Bitfield of flags describing this.  See the "IS_..." constants below.
+  int64_t flags;
+
+  // Number of bytes between each successive point.  This is always at least the
+  // sum of all field sizes, but may be larger if the data has internal padding.
+  int32_t point_step;
+
+  // Number of bytes between each successive row.
+  int64_t row_step;
+
+  // Optional filler bytes to allow the raw point data to be aligned in memory.
+  int16_t filler_size;
+  byte filler[filler_size];
+
+  // The raw point data.
+  int64_t data_size;
+  byte data[data_size];
+
+  // === Constants for the flags bits ===
+
+  // Set iff data is big-endian.
+  const int64_t IS_BIGENDIAN = 1;
+
+  // Set iff data contains only finite values.  (ROS sometimes uses "is_dense"
+  // to refer to this concept, but not all implementations agree on whether
+  // "dense" means "strictly finite" or "provides full width x height matrix
+  // with possibly infinite values", so we use a less ambiguous term here.)
+  const int64_t IS_STRICTLY_FINITE = 2;
+}

lcmtypes/lcmt_point_cloud_field.lcm

@@ -0,0 +1,32 @@
+package drake;
+
+// Describes one field (i.e., channel) within an lcmt_point_cloud.
+//
+// Modeled after PCL and ROS conventions:
+// https://pointclouds.org/documentation/structpcl_1_1_p_c_l_point_field.html
+// https://docs.ros.org/en/api/sensor_msgs/html/msg/PointField.html
+// http://wiki.ros.org/pcl/Overview#Common_PointCloud2_field_names
+//
+struct lcmt_point_cloud_field {
+  // Field name.
+  string name;
+
+  // Location of this field after the start of each point's data.
+  int32_t byte_offset;
+
+  // Element type, per the constants shown below.
+  int8_t datatype;
+
+  // Number of elements per field.
+  int32_t count;
+
+  // Allowed values for datatype.
+  const int8_t INT8 = 1;
+  const int8_t UINT8 = 2;
+  const int8_t INT16 = 3;
+  const int8_t UINT16 = 4;
+  const int8_t INT32 = 5;
+  const int8_t UINT32 = 6;
+  const int8_t FLOAT32 = 7;
+  const int8_t FLOAT64 = 8;
+}

perception/BUILD.bazel

@@ -18,6 +18,7 @@ drake_cc_package_library(
         ":depth_image_to_point_cloud",
         ":point_cloud",
         ":point_cloud_flags",
+        ":point_cloud_to_lcm",
     ],
 )
 
@@ -48,15 +49,25 @@ drake_cc_library(
         ":point_cloud",
         "//common:essential",
         "//math:geometric_transform",
-        "//systems/framework",
+        "//systems/framework:leaf_system",
         "//systems/sensors:camera_info",
         "//systems/sensors:image",
     ],
 )
 
+drake_cc_library(
+    name = "point_cloud_to_lcm",
+    srcs = ["point_cloud_to_lcm.cc"],
+    hdrs = ["point_cloud_to_lcm.h"],
+    deps = [
+        ":point_cloud",
+        "//lcmtypes:point_cloud",
+        "//systems/framework:leaf_system",
+    ],
+)
+
 drake_cc_googletest(
     name = "depth_image_to_point_cloud_test",
-    srcs = ["test/depth_image_to_point_cloud_test.cc"],
     deps = [
         ":depth_image_to_point_cloud",
         "//common/test_utilities:eigen_matrix_compare",
@@ -66,7 +77,6 @@ drake_cc_googletest(
 
 drake_cc_googletest(
     name = "point_cloud_flags_test",
-    srcs = ["test/point_cloud_flags_test.cc"],
     deps = [
         ":point_cloud_flags",
         "//common/test_utilities:expect_no_throw",
@@ -75,12 +85,19 @@ drake_cc_googletest(
 
 drake_cc_googletest(
     name = "point_cloud_test",
-    srcs = ["test/point_cloud_test.cc"],
     deps = [
         ":point_cloud",
         "//common/test_utilities:eigen_matrix_compare",
         "//common/test_utilities:expect_no_throw",
     ],
 )
 
+drake_cc_googletest(
+    name = "point_cloud_to_lcm_test",
+    deps = [
+        ":point_cloud_to_lcm",
+        "//common/test_utilities:limit_malloc",
+    ],
+)
+
 add_lint_tests()