Fix transform entity path for PointCloud2, add additional checks

rerun-io · May 24, 2024 · e8cecbe · e8cecbe
1 parent e862720
commit e8cecbe
Show file tree

Hide file tree

Showing 2 changed files with 20 additions and 1 deletion.
diff --git a/rerun_bridge/src/rerun_bridge/rerun_ros_interface.cpp b/rerun_bridge/src/rerun_bridge/rerun_ros_interface.cpp
@@ -308,17 +308,36 @@ void log_point_cloud2(
     // TODO(leo) allow arbitrary color mapping
 
     size_t x_offset, y_offset, z_offset;
+    bool has_x{false}, has_y{false}, has_z{false};
 
     for (const auto& field : msg->fields) {
         if (field.name == "x") {
             x_offset = field.offset;
+            if(field.datatype != sensor_msgs::msg::PointField::FLOAT32) {
+                rec.log(entity_path, rerun::TextLog("Only FLOAT32 x field supported"));
+                return;
+            }
+            has_x = true;
         } else if (field.name == "y") {
             y_offset = field.offset;
+            if(field.datatype != sensor_msgs::msg::PointField::FLOAT32) {
+                rec.log(entity_path, rerun::TextLog("Only FLOAT32 y field supported"));
+                return;
+            }
         } else if (field.name == "z") {
             z_offset = field.offset;
+            if(field.datatype != sensor_msgs::msg::PointField::FLOAT32) {
+                rec.log(entity_path, rerun::TextLog("Only FLOAT32 z field supported"));
+                return;
+            }
         }
     }
 
+    if (!has_x || !has_y || !has_z) {
+        rec.log(entity_path, rerun::TextLog("Currently only PointCloud2 messages with x, y, z fields are supported"));
+        return;
+    }
+
     std::vector<rerun::Position3D> points(msg->width * msg->height);
     std::vector<rerun::Color> colors;
 

diff --git a/rerun_bridge/src/rerun_bridge/visualizer_node.cpp b/rerun_bridge/src/rerun_bridge/visualizer_node.cpp
@@ -542,7 +542,7 @@ std::shared_ptr<rclcpp::Subscription<sensor_msgs::msg::PointCloud2>>
                             100ms
                         )
                     );
-                    log_transform(_rec, parent_entity_path(entity_path), transform);
+                    log_transform(_rec, entity_path, transform);
                 } catch (tf2::TransformException& ex) {
                     RCLCPP_WARN(this->get_logger(), "%s", ex.what());
                 }