From e7c3bd50edd06e393839775f12d3624b05501300 Mon Sep 17 00:00:00 2001 From: Your Name Date: Thu, 16 Jan 2025 09:57:38 +0100 Subject: [PATCH] update --- CMakeLists.txt | 17 +++++++++---- nodes/odom_normalizer.py | 52 ++++++++++++++++++++++++++++++++++++++++ 2 files changed, 65 insertions(+), 4 deletions(-) create mode 100644 nodes/odom_normalizer.py diff --git a/CMakeLists.txt b/CMakeLists.txt index 76d50ae..5d19ba9 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -49,13 +49,22 @@ target_link_libraries(odom_publisher ${catkin_LIBRARIES}) ## Mark executable scripts (Python etc.) for installation ## in contrast to setup.py, you can choose the destination -catkin_install_python(PROGRAMS - nodes/odom_publisher_python.py - nodes/odom_publisher_linus.py - nodes/odom_publisher_fromTwist.py +#catkin_install_python(PROGRAMS +# nodes/odom_publisher_python.py +# nodes/odom_publisher_linus.py +# nodes/odom_publisher_fromTwist.py +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +#) + +# Find all Python scripts in the nodes/ directory +file(GLOB python_scripts nodes/*.py) + +# Mark all found scripts as executable and install them +catkin_install_python(PROGRAMS ${python_scripts} DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) + ################################################################################ # Install ################################################################################ diff --git a/nodes/odom_normalizer.py b/nodes/odom_normalizer.py new file mode 100644 index 0000000..c819bea --- /dev/null +++ b/nodes/odom_normalizer.py @@ -0,0 +1,52 @@ +#!/usr/bin/env python + +import rospy +from nav_msgs.msg import Odometry +import numpy as np + +def normalize_quaternion(qx, qy, qz, qw): + norm = np.sqrt(qx**2 + qy**2 + qz**2 + qw**2) + if norm == 0: + return 0.0, 0.0, 0.0, 1.0 # Default to unit quaternion + return qx / norm, qy / norm, qz / norm, qw / norm + +def odom_callback(msg): + # Normalize quaternion + qx, qy, qz, qw = normalize_quaternion( + msg.pose.pose.orientation.x, + msg.pose.pose.orientation.y, + msg.pose.pose.orientation.z, + msg.pose.pose.orientation.w + ) + + # Create new message + odom_normalized = Odometry() + odom_normalized.header = msg.header + odom_normalized.child_frame_id = msg.child_frame_id + + # Copy position + odom_normalized.pose.pose.position = msg.pose.pose.position + + # Set normalized quaternion + odom_normalized.pose.pose.orientation.x = qx + odom_normalized.pose.pose.orientation.y = qy + odom_normalized.pose.pose.orientation.z = qz + odom_normalized.pose.pose.orientation.w = qw + + # Copy covariance + odom_normalized.pose.covariance = msg.pose.covariance + odom_normalized.twist = msg.twist # Copy twist data + + # Publish normalized odom + pub.publish(odom_normalized) + +if __name__ == "__main__": + rospy.init_node("odom_normalizer") + + # Publisher + pub = rospy.Publisher("/odom_normalized", Odometry, queue_size=10) + + # Subscriber + rospy.Subscriber("/odom", Odometry, odom_callback) + + rospy.spin()