added broadcaster node

.vscode/c_cpp_properties.json

+{
+  "configurations": [
+    {
+      "browse": {
+        "databaseFilename": "${default}",
+        "limitSymbolsToIncludedHeaders": false
+      },
+      "includePath": [
+        "/opt/ros/humble/include/**",
+        "/usr/include/**"
+      ],
+      "name": "ROS",
+      "intelliSenseMode": "gcc-x64",
+      "compilerPath": "/usr/bin/gcc",
+      "cStandard": "gnu11",
+      "cppStandard": "c++14"
+    }
+  ],
+  "version": 4
+}
\ No newline at end of file

.vscode/settings.json

+{
+    "python.autoComplete.extraPaths": [
+        "/home/sochi/40_BA_WS/ros2_ws/build/my_sensor_controller",
+        "/home/sochi/40_BA_WS/ros2_ws/install/my_sensor_controller/lib/python3.10/site-packages",
+        "/home/sochi/40_BA_WS/ros2_ws/build/my_robot_package",
+        "/home/sochi/40_BA_WS/ros2_ws/install/my_robot_package/lib/python3.10/site-packages",
+        "/home/sochi/40_BA_WS/ros2_ws/src/my_sensor_controller/install/my_sensor_controller/lib/python3.10/site-packages",
+        "/opt/ros/humble/lib/python3.10/site-packages",
+        "/opt/ros/humble/local/lib/python3.10/dist-packages"
+    ],
+    "python.analysis.extraPaths": [
+        "/home/sochi/40_BA_WS/ros2_ws/build/my_sensor_controller",
+        "/home/sochi/40_BA_WS/ros2_ws/install/my_sensor_controller/lib/python3.10/site-packages",
+        "/home/sochi/40_BA_WS/ros2_ws/build/my_robot_package",
+        "/home/sochi/40_BA_WS/ros2_ws/install/my_robot_package/lib/python3.10/site-packages",
+        "/home/sochi/40_BA_WS/ros2_ws/src/my_sensor_controller/install/my_sensor_controller/lib/python3.10/site-packages",
+        "/opt/ros/humble/lib/python3.10/site-packages",
+        "/opt/ros/humble/local/lib/python3.10/dist-packages"
+    ]
+}
\ No newline at end of file

my_robot_package/tof_tf_caster.py

+#!/usr/bin/env python3
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import JointState
+from tf2_ros import TransformBroadcaster
+from geometry_msgs.msg import TransformStamped
+import math
+import tf_transformations
+
+
+class SensorTFBroadcaster(Node):
+    def __init__(self):
+        super().__init__('sensor_tf_broadcaster')
+        self.broadcaster = TransformBroadcaster(self)
+        self.subscription = self.create_subscription(
+            JointState,
+            '/joint_states',
+            self.listener_callback,
+            10)
+        self.sensor_offset = (0.1, 0.0, 0.1)  # offset
+
+    def listener_callback(self, msg: JointState):
+        try:
+            idx = msg.name.index("shoulder_pan_joint")
+            angle = msg.position[idx]
+
+            t = TransformStamped()
+            t.header.stamp = self.get_clock().now().to_msg()
+            t.header.frame_id = "shoulder_pan_joint"
+            t.child_frame_id = "vl53_frame"
+
+            # Apply static offset
+            t.transform.translation.x = self.sensor_offset[0]
+            t.transform.translation.y = self.sensor_offset[1]
+            t.transform.translation.z = self.sensor_offset[2]
+
+            # Rotate sensor with shoulder_pan_joint (around Z axis)
+            # Combine static rotation (90° pitch, -90° yaw) with joint angle (around Z)
+            static_quat = tf_transformations.quaternion_from_euler(0, 1.5708, -1.5708)
+            joint_quat = tf_transformations.quaternion_from_euler(angle, 0, 0)
+
+            # Multiply quaternions: final = static * joint
+            quat = tf_transformations.quaternion_multiply(static_quat, joint_quat)
+            t.transform.rotation.x = quat[0]
+            t.transform.rotation.y = quat[1]
+            t.transform.rotation.z = quat[2]
+            t.transform.rotation.w = quat[3]
+
+            self.broadcaster.sendTransform(t)
+        except ValueError:
+            self.get_logger().warn("Joint shoulder_pan_joint not found in JointState")
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = SensorTFBroadcaster()
+    rclpy.spin(node)
+    node.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()

setup.py

@@ -25,6 +25,7 @@ setup(
         'console_scripts': [
         'joint_reader = my_robot_package.joint_reader:main',
         'tof_pico_node = my_robot_package.tof_pico_node:main',
+        'tof_tf_caster = my_robot_package.tof_tf_caster:main',
         ],
     },
 )