Added publishing to LidarNode as well

carnegiemellonracing · Oct 26, 2023 · 787677c · 787677c
1 parent ed05d95
commit 787677c
Show file tree

Hide file tree

Showing 2 changed files with 22 additions and 8 deletions.
diff --git a/driverless_ws/src/perceptions/perceptions/predict/LidarNode.py b/driverless_ws/src/perceptions/perceptions/predict/LidarNode.py
@@ -4,18 +4,29 @@
 # for subscribing to sensor data
 from perceptions.utils.DataNode import DataNode
 
+# for converting predictor output to cone message type
+from eufs_msgs.msg import ConeArrayWithCovariance
+import perceptions.utils.conversions as conversions
+
 # for doing prediction on sensor data
 from perc22a.predictors import LidarPredictor
 
+NODE_NAME = "lidar_node"
+
 class LidarNode(DataNode):
 
     def __init__(self):
-        super().__init__(name="stereo_node")
+        super().__init__(name=NODE_NAME)
 
         # do prediction on a timer
-        self.interval = 1
+        self.interval = 0.5
         self.predict_timer = self.create_timer(self.interval, self.predict_callback)
 
+        # create publisher
+        self.cone_topic = f"/{NODE_NAME}_cones"
+        self.qos_profile = 10
+        self.cone_publisher = self.create_publisher(ConeArrayWithCovariance, self.cone_topic, self.qos_profile)
+
         # create predictor
         self.predictor = LidarPredictor()
 
@@ -24,13 +35,16 @@ def predict_callback(self):
             self.get_logger().warn("Not got all data")
             return
 
-        # otherwise, do prediction on data
+        # otherwise, do prediction on data and display
         data = {
             "points": self.points
         }
         cones = self.predictor.predict(data)
-
         self.predictor.display()
+
+        # publish messages
+        msg = conversions.cones_to_msg(cones)
+        self.cone_publisher.publish(msg)
         print(cones)
 
 

diff --git a/driverless_ws/src/perceptions/perceptions/predict/StereoNode.py b/driverless_ws/src/perceptions/perceptions/predict/StereoNode.py
@@ -4,7 +4,7 @@
 # for subscribing to sensor data
 from perceptions.utils.DataNode import DataNode
 
-# for convetring predictor output to cone message type
+# for converting predictor output to cone message type
 from eufs_msgs.msg import ConeArrayWithCovariance
 import perceptions.utils.conversions as conversions
 
@@ -16,7 +16,7 @@
 class StereoNode(DataNode):
 
     def __init__(self):
-        super().__init__(name="stereo_node")
+        super().__init__(name=NODE_NAME)
 
         # do prediction on a timer
         # TODO: figure out what the best way is to deal with this?
@@ -38,7 +38,7 @@ def predict_callback(self):
             self.get_logger().warn("Not got all data")
             return
 
-        # otherwise, do prediction on data
+        # otherwise, do prediction on data and display
         data = {
             "left_color": self.left_color,
             "xyz_image": self.xyz_image,
@@ -50,7 +50,7 @@ def predict_callback(self):
         msg = conversions.cones_to_msg(cones)
         self.cone_publisher.publish(msg)
 
-        print("cones", cones)
+        print(cones)
 
 
 def main(args=None):