mirror of
https://github.com/bjoernellens1/cps_rmp220_support.git
synced 2025-05-16 09:48:05 +00:00
Compare commits
No commits in common. "91c376089f4e26231ad37c52faa3ec1c34a848b7" and "1943de705d2ab00bb6a0378db9239a07a6bbc8d0" have entirely different histories.
91c376089f
...
1943de705d
@ -47,14 +47,6 @@ include_directories(
|
||||
add_executable(odom_publisher nodes/odom_publisher.cpp)
|
||||
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
|
||||
################################################################################
|
||||
|
@ -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()
|
@ -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()
|
Loading…
Reference in New Issue
Block a user