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
|
#!/usr/bin/env python
|
||||||
|
|
||||||
import rospy
|
import rospy
|
||||||
from tf.transform_broadcaster import TransformBroadcaster
|
import tf2_ros
|
||||||
from nav_msgs.msg import Odometry
|
from nav_msgs.msg import Odometry
|
||||||
from geometry_msgs.msg import TransformStamped
|
from geometry_msgs.msg import TransformStamped
|
||||||
|
|
||||||
def odom_callback(msg):
|
def odom_callback(msg):
|
||||||
# Create TransformStamped message
|
# Create a TransformStamped message
|
||||||
odom_trans = TransformStamped()
|
odom_trans = TransformStamped()
|
||||||
odom_trans.header.stamp = msg.header.stamp
|
odom_trans.header.stamp = msg.header.stamp
|
||||||
odom_trans.header.frame_id = msg.header.frame_id
|
odom_trans.header.frame_id = msg.header.frame_id
|
||||||
odom_trans.child_frame_id = msg.child_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.x = msg.pose.pose.position.x
|
||||||
odom_trans.transform.translation.y = msg.pose.pose.position.y
|
odom_trans.transform.translation.y = msg.pose.pose.position.y
|
||||||
odom_trans.transform.translation.z = msg.pose.pose.position.z
|
odom_trans.transform.translation.z = msg.pose.pose.position.z
|
||||||
|
|
||||||
# Set rotation
|
# Set the rotation
|
||||||
odom_trans.transform.rotation = msg.pose.pose.orientation
|
odom_trans.transform.rotation = msg.pose.pose.orientation
|
||||||
|
|
||||||
# Broadcast the transform
|
# Publish the transform
|
||||||
tf_broadcaster.sendTransformMessage(odom_trans)
|
tf_broadcaster.sendTransform(odom_trans)
|
||||||
|
|
||||||
if __name__ == "__main__":
|
if __name__ == "__main__":
|
||||||
rospy.init_node("tf_broadcaster")
|
rospy.init_node("tf2_broadcaster")
|
||||||
|
|
||||||
# Create a TransformBroadcaster
|
# Create a TransformBroadcaster
|
||||||
tf_broadcaster = TransformBroadcaster()
|
tf_broadcaster = tf2_ros.TransformBroadcaster()
|
||||||
|
|
||||||
# Subscribe to the /odom topic
|
# Subscribe to the /odom topic
|
||||||
rospy.Subscriber("/odom", Odometry, odom_callback)
|
rospy.Subscriber("/odom", Odometry, odom_callback)
|
||||||
|
|
||||||
rospy.loginfo("TF broadcaster node is running...")
|
rospy.loginfo("TF2 broadcaster node is running...")
|
||||||
|
|
||||||
# Keep the node running
|
# Keep the node running
|
||||||
rospy.spin()
|
rospy.spin()
|
||||||
|
Loading…
Reference in New Issue
Block a user