mirror of
https://github.com/introlab/rtabmap_ros.git
synced 2026-09-13 06:40:19 +08:00
29 lines
778 B
Python
Executable File
29 lines
778 B
Python
Executable File
#!/usr/bin/env python
|
|
import rospy
|
|
import tf
|
|
from geometry_msgs.msg import PointStamped
|
|
|
|
def callback(point):
|
|
global br
|
|
global frame_id
|
|
local_frame_id = point.header.frame_id
|
|
if not local_frame_id:
|
|
local_frame_id = frame_id
|
|
br.sendTransform(
|
|
(point.point.x, point.point.y, point.point.z),
|
|
tf.transformations.quaternion_from_euler(0,0,0),
|
|
point.header.stamp,
|
|
local_frame_id,
|
|
fixed_frame_id)
|
|
|
|
if __name__ == "__main__":
|
|
|
|
rospy.init_node("point_to_tf", anonymous=True)
|
|
|
|
frame_id = rospy.get_param('~frame_id', 'point')
|
|
fixed_frame_id = rospy.get_param('~fixed_frame_id', 'world')
|
|
|
|
br = tf.TransformBroadcaster()
|
|
rospy.Subscriber("point", PointStamped, callback, queue_size=1)
|
|
rospy.spin()
|