publish_external_position_vrpn.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 import rospy
4 import tf
5 from geometry_msgs.msg import PointStamped, TransformStamped, PoseStamped #PoseStamped added to support vrpn_client
6 from crazyflie_driver.srv import UpdateParams
7 
8 def onNewTransform(pose):
9  global msg
10  global pub
11  global firstTransform
12 
13  if firstTransform:
14  # initialize kalman filter
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"])
19 
20  rospy.set_param("kalman/resetEstimation", 1)
21  update_params(["kalman/resetEstimation"])
22  firstTransform = False
23 
24  else:
25  msg.header.frame_id = pose.header.frame_id
26  msg.header.stamp = pose.header.stamp
27  msg.header.seq += 1
28  msg.point.x = pose.pose.position.x
29  msg.point.y = pose.pose.position.y
30  msg.point.z = pose.pose.position.z
31  pub.publish(msg)
32 
33 
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")
37 
38  rospy.wait_for_service('update_params')
39  rospy.loginfo("found update_params service")
40  update_params = rospy.ServiceProxy('update_params', UpdateParams)
41 
42  firstTransform = True
43 
44  msg = PointStamped()
45  msg.header.seq = 0
46  msg.header.stamp = rospy.Time.now()
47 
48  pub = rospy.Publisher("external_position", PointStamped, queue_size=1)
49  rospy.Subscriber(topic, PoseStamped, onNewTransform)
50 
51  rospy.spin()


crazyflie_demo
Author(s): Wolfgang Hoenig
autogenerated on Mon Sep 28 2020 03:40:12