add pointcloud base_reference_frame lookup

Author: AaronGBoswell

formant_ros2_adapter/scripts/main.py

@@ -145,6 +145,8 @@
 # Seconds
 SERVICE_CALL_TIMEOUT = 5
 
+BASE_REFERENCE_FRAME = os.getenv("FORMANT_BASE_REFERENCE_FRAME","base_link")
+
 
 class ROS2Adapter:
     """
@@ -1041,45 +1043,30 @@ def handle_ros2_message(self, msg, subscriber_config):
                         timestamp=msg_timestamp,
                     )
 
-                elif msg_type == LaserScan:
-                    # Convert LaserScan to a Formant pointcloud
+                elif msg_type == LaserScan or msg_type == PointCloud2:
                     try:
-                        self.fclient.agent_stub.PostData(
-                            Datapoint(
-                                stream=formant_stream,
-                                point_cloud=FPointCloud.from_ros_laserscan(
-                                    msg
-                                ).to_proto(),
-                                tags=subscriber_config["tags"],
-                                timestamp=msg_timestamp,
-                            )
-                        )
-                    except grpc.RpcError as e:
-                        return
-                    except Exception as e:
-                        print(
-                            "ERROR: Could not ingest " + formant_stream + ": " + str(e)
+                        formant_point_cloud = None
+                        if msg_type == LaserScan:
+                            formant_point_cloud = FPointCloud.from_ros_laserscan(
+                                        msg
+                                    )
+                        if msg_type == PointCloud2:
+                            formant_point_cloud = FPointCloud.from_ros(msg)
+
+                        if formant_point_cloud is None:
+                            raise Exception("%s not PointCloud2 or LaserScan" % msg_type)
+                        
+                        formant_point_cloud.transform_to_world = self.lookup_transform(
+                            msg, BASE_REFERENCE_FRAME
                         )
-                        return
-
-                elif msg_type == PointCloud2:
-                    try:
                         self.fclient.agent_stub.PostData(
                             Datapoint(
                                 stream=formant_stream,
-                                point_cloud=FPointCloud.from_ros(msg).to_proto(),
+                                point_cloud=formant_point_cloud.to_proto(),
                                 tags=subscriber_config["tags"],
                                 timestamp=msg_timestamp,
                             )
                         )
-                    except grpc.RpcError as e:
-                        return
-                    except Exception as e:
-                        print(
-                            "ERROR: Could not ingest " + formant_stream + ": " + str(e)
-                        )
-                        return
-
                 else:
                     # Ingest any messages without a direct mapping to a Formant type as JSON
                     self.fclient.post_json(