mirror of
https://github.com/bjoernellens1/cps_rmp220_support.git
synced 2025-01-18 23:56:59 +00:00
update
This commit is contained in:
parent
edb23af234
commit
df832c64b7
@ -1,38 +1,38 @@
|
||||
#!/usr/bin/env python
|
||||
|
||||
import rospy
|
||||
from tf.transform_broadcaster import TransformBroadcaster
|
||||
import tf2_ros
|
||||
from nav_msgs.msg import Odometry
|
||||
from geometry_msgs.msg import TransformStamped
|
||||
|
||||
def odom_callback(msg):
|
||||
# Create TransformStamped message
|
||||
# Create a TransformStamped message
|
||||
odom_trans = TransformStamped()
|
||||
odom_trans.header.stamp = msg.header.stamp
|
||||
odom_trans.header.frame_id = msg.header.frame_id
|
||||
odom_trans.child_frame_id = msg.child_frame_id
|
||||
|
||||
# Set translation
|
||||
# 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 rotation
|
||||
# Set the rotation
|
||||
odom_trans.transform.rotation = msg.pose.pose.orientation
|
||||
|
||||
# Broadcast the transform
|
||||
tf_broadcaster.sendTransformMessage(odom_trans)
|
||||
# Publish the transform
|
||||
tf_broadcaster.sendTransform(odom_trans)
|
||||
|
||||
if __name__ == "__main__":
|
||||
rospy.init_node("tf_broadcaster")
|
||||
rospy.init_node("tf2_broadcaster")
|
||||
|
||||
# Create a TransformBroadcaster
|
||||
tf_broadcaster = TransformBroadcaster()
|
||||
tf_broadcaster = tf2_ros.TransformBroadcaster()
|
||||
|
||||
# Subscribe to the /odom topic
|
||||
rospy.Subscriber("/odom", Odometry, odom_callback)
|
||||
|
||||
rospy.loginfo("TF broadcaster node is running...")
|
||||
rospy.loginfo("TF2 broadcaster node is running...")
|
||||
|
||||
# Keep the node running
|
||||
rospy.spin()
|
||||
|
Loading…
Reference in New Issue
Block a user