From 63cf9f12bd1ed777396c9bf889cdd68354188179 Mon Sep 17 00:00:00 2001
From: oliverkania <oliver.kania@rwth-aachen.de>
Date: Fri, 9 May 2025 12:26:38 +0200
Subject: [PATCH] added broadcaster node
---
.vscode/c_cpp_properties.json | 20 ++++++++++
.vscode/settings.json | 20 ++++++++++
my_robot_package/tof_tf_caster.py | 62 +++++++++++++++++++++++++++++++
setup.py | 1 +
4 files changed, 103 insertions(+)
create mode 100644 .vscode/c_cpp_properties.json
create mode 100644 .vscode/settings.json
create mode 100644 my_robot_package/tof_tf_caster.py
diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json
new file mode 100644
index 0000000..af5a8df
--- /dev/null
+++ b/.vscode/c_cpp_properties.json
@@ -0,0 +1,20 @@
+{
+ "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
diff --git a/.vscode/settings.json b/.vscode/settings.json
new file mode 100644
index 0000000..cab2135
--- /dev/null
+++ b/.vscode/settings.json
@@ -0,0 +1,20 @@
+{
+ "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
diff --git a/my_robot_package/tof_tf_caster.py b/my_robot_package/tof_tf_caster.py
new file mode 100644
index 0000000..b6853b8
--- /dev/null
+++ b/my_robot_package/tof_tf_caster.py
@@ -0,0 +1,62 @@
+#!/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()
diff --git a/setup.py b/setup.py
index 8f8fdb0..4f07ddf 100644
--- a/setup.py
+++ b/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',
],
},
)
--
GitLab