ellensohn/cps_rmp220_support
1
0
Fork 0
mirror of https://github.com/bjoernellens1/cps_rmp220_support.git synced 2025-01-18 23:56:59 +00:00
Code Issues Packages Projects Releases Wiki Activity

update

Browse Source
This commit is contained in:
Your Name 2025-01-14 09:30:25 +01:00
parent 14c43a5567
commit 5dd8b2c27b
1 changed files with 12 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
nodes/odom_publisher_python.py
View File

@ -3,17 +3,27 @@
import rospy
from nav_msgs.msg import Odometry
import tf
import math
def normalize_quaternion(x, y, z, w):
norm = math.sqrt(x**2 + y**2 + z**2 + w**2)
return x / norm, y / norm, z / norm, w / norm
def odom_callback(msg):
# Extract position and orientation from the odometry message
position = msg.pose.pose.position
orientation = msg.pose.pose.orientation
# Normalize the quaternion
qx, qy, qz, qw = normalize_quaternion(
orientation.x, orientation.y, orientation.z, orientation.w
)
# Broadcast the transformation
br = tf.TransformBroadcaster()
br.sendTransform(
(position.x, position.y, position.z), # Translation
(orientation.x, orientation.y, orientation.z, orientation.w), # Rotation (quaternion)
(qx, qy, qz, qw), # Normalized rotation (quaternion)
rospy.Time.now(), # Timestamp
msg.child_frame_id if msg.child_frame_id else "base_link", # Child frame ID
msg.header.frame_id if msg.header.frame_id else "odom" # Parent frame ID