crazyflie_demo: publish_external_pose

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 import rospy
00004 import tf
00005 from geometry_msgs.msg import PoseStamped, TransformStamped
00006 from crazyflie_driver.srv import UpdateParams, Takeoff, Land
00007 
00008 def onNewTransform(transform):
00009     global msg
00010     global pub
00011     global firstTransform
00012 
00013     if firstTransform:
00014         # initialize kalman filter
00015         rospy.set_param("kalman/initialX", transform.transform.translation.x)
00016         rospy.set_param("kalman/initialY", transform.transform.translation.y)
00017         rospy.set_param("kalman/initialZ", transform.transform.translation.z)
00018         update_params(["kalman/initialX", "kalman/initialY", "kalman/initialZ"])
00019 
00020         rospy.set_param("kalman/resetEstimation", 1)
00021         # rospy.set_param("locSrv/extPosStdDev", 1e-4)
00022         update_params(["kalman/resetEstimation"]) #, "locSrv/extPosStdDev"])
00023         firstTransform = False
00024     else:
00025         msg.header.frame_id = transform.header.frame_id
00026         msg.header.stamp = transform.header.stamp
00027         msg.header.seq += 1
00028         msg.pose.position.x = transform.transform.translation.x
00029         msg.pose.position.y = transform.transform.translation.y
00030         msg.pose.position.z = transform.transform.translation.z
00031         msg.pose.orientation.x = transform.rotation.x
00032         msg.pose.orientation.y = transform.rotation.y
00033         msg.pose.orientation.z = transform.rotation.z
00034         msg.pose.orientation.w = transform.rotation.w
00035         pub.publish(msg)
00036 
00037 
00038 if __name__ == '__main__':
00039     rospy.init_node('publish_external_position_vicon', anonymous=True)
00040     topic = rospy.get_param("~topic", "/vicon/cf/cf")
00041 
00042     rospy.wait_for_service('update_params')
00043     rospy.loginfo("found update_params service")
00044     update_params = rospy.ServiceProxy('update_params', UpdateParams)
00045 
00046     firstTransform = True
00047 
00048     msg = PoseStamped()
00049     msg.header.seq = 0
00050     msg.header.stamp = rospy.Time.now()
00051 
00052     pub = rospy.Publisher("external_pose", PoseStamped, queue_size=1)
00053     rospy.Subscriber(topic, TransformStamped, onNewTransform)
00054 
00055     rospy.spin()