5 from geometry_msgs.msg
import PointStamped, TransformStamped, PoseStamped
15 rospy.set_param(
"kalman/initialX", pose.pose.position.x)
16 rospy.set_param(
"kalman/initialY", pose.pose.position.y)
17 rospy.set_param(
"kalman/initialZ", pose.pose.position.z)
18 update_params([
"kalman/initialX",
"kalman/initialY",
"kalman/initialZ"])
20 rospy.set_param(
"kalman/resetEstimation", 1)
22 firstTransform =
False 25 msg.header.frame_id = pose.header.frame_id
26 msg.header.stamp = pose.header.stamp
28 msg.point.x = pose.pose.position.x
29 msg.point.y = pose.pose.position.y
30 msg.point.z = pose.pose.position.z
34 if __name__ ==
'__main__':
35 rospy.init_node(
'publish_external_position_vrpn', anonymous=
True)
36 topic = rospy.get_param(
"~topic",
"/crazyflie1/vrpn_client_node/crazyflie1/pose")
38 rospy.wait_for_service(
'update_params')
39 rospy.loginfo(
"found update_params service")
40 update_params = rospy.ServiceProxy(
'update_params', UpdateParams)
46 msg.header.stamp = rospy.Time.now()
48 pub = rospy.Publisher(
"external_position", PointStamped, queue_size=1)
49 rospy.Subscriber(topic, PoseStamped, onNewTransform)