Add tracking radiation data and position Node

src/rove_radiation/launch/radiation.launch.py

@@ -13,6 +13,12 @@ def generate_launch_description():
             executable='radiation_publisher',
             name='radiation_publisher',
             output='screen'
+        ),
+        Node(
+            package='rove_radiation',
+            executable='radiation_position_tracker',
+            name='radiation_position_tracker',
+            output='screen'
         )
     ])
 

src/rove_radiation/rove_radiation/radiation_position_tracker.py

@@ -0,0 +1,33 @@
+import rclpy
+from rclpy.node import Node
+from std_msgs.msg import Float32
+from geometry_msgs.msg import PoseWithCovarianceStamped, Point
+
+class RadiationPositionTracker(Node):
+    def __init__(self):
+        super().__init__('radiation_position_tracker')
+
+        self.radiation_subscription = self.create_subscription(Float32, '/dose_rate', self.radiation_callback, 10)
+        self.odom_subscription = self.create_subscription(PoseWithCovarianceStamped, '/localization_pose', self.localization_pose_callback, 10)
+
+        self.current_position = Point(x=0.0, y=0.0, z=0.0)
+
+    def radiation_callback(self, msg):
+        self.get_logger().info(f'radiation detected: {msg.data}')
+
+    def localization_pose_callback(self, msg):
+        self.current_position.x = msg.pose.pose.position.x
+        self.current_position.y = msg.pose.pose.position.y
+        self.current_position.z = msg.pose.pose.position.z
+        self.get_logger().info(f'current robot position: x : {self.current_position.x}, y : {self.current_position.y}, z : {self.current_position.z}')
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = RadiationPositionTracker()
+    rclpy.spin(node)
+    node.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()

src/rove_radiation/setup.py

@@ -25,6 +25,7 @@
     entry_points={
         'console_scripts': [
             'radiation_publisher = rove_radiation.radiation_publisher:main',
+            'radiation_position_tracker = rove_radiation.radiation_position_tracker:main',
         ],
     },
 )