Compare commits

..

No commits in common. "91c376089f4e26231ad37c52faa3ec1c34a848b7" and "1943de705d2ab00bb6a0378db9239a07a6bbc8d0" have entirely different histories.

3 changed files with 0 additions and 91 deletions

View File

@ -47,14 +47,6 @@ include_directories(
add_executable(odom_publisher nodes/odom_publisher.cpp) add_executable(odom_publisher nodes/odom_publisher.cpp)
target_link_libraries(odom_publisher ${catkin_LIBRARIES}) 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
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
################################################################################ ################################################################################
# Install # Install
################################################################################ ################################################################################

View File

@ -1,40 +0,0 @@
#!/usr/bin/env python
import rospy
import tf2_ros
from nav_msgs.msg import Odometry
from geometry_msgs.msg import TransformStamped
def odom_callback(msg):
# Create a TransformStamped message
odom_trans = TransformStamped()
odom_trans.header.stamp = msg.header.stamp
odom_trans.header.frame_id = msg.header.frame_id
# Set child_frame_id explicitly (e.g., base_link)
odom_trans.child_frame_id = "base_link"
# Set the translation
odom_trans.transform.translation.x = msg.pose.pose.position.x
odom_trans.transform.translation.y = msg.pose.pose.position.y
odom_trans.transform.translation.z = msg.pose.pose.position.z
# Set the rotation
odom_trans.transform.rotation = msg.pose.pose.orientation
# Publish the transform
tf_broadcaster.sendTransform(odom_trans)
if __name__ == "__main__":
rospy.init_node("tf2_broadcaster")
# Create a TransformBroadcaster
tf_broadcaster = tf2_ros.TransformBroadcaster()
# Subscribe to the /odom topic
rospy.Subscriber("/odom", Odometry, odom_callback)
rospy.loginfo("TF2 broadcaster node is running...")
# Keep the node running
rospy.spin()

View File

@ -1,43 +0,0 @@
#!/usr/bin/env python
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
)
# Get frame IDs from the message
parent_frame_id = msg.header.frame_id if msg.header.frame_id else "Odometry"
child_frame_id = msg.child_frame_id if msg.child_frame_id else "base_link"
# Broadcast the transformation
br = tf.TransformBroadcaster()
br.sendTransform(
(position.x, position.y, position.z), # Translation
(qx, qy, qz, qw), # Normalized rotation (quaternion)
rospy.Time.now(), # Timestamp
child_frame_id, # Child frame ID
parent_frame_id # Parent frame ID
)
def main():
rospy.init_node('odom_tf_publisher', anonymous=True)
rospy.Subscriber('/odom', Odometry, odom_callback)
rospy.loginfo("TF Publisher for /odom topic is running...")
rospy.spin()
if __name__ == '__main__':
main()