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)
|
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
|
||||||
################################################################################
|
################################################################################
|
||||||
|
@ -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