Go to the documentation of this file.00001
00002
00003 import rospy
00004 import tf
00005 from crazyflie_driver.msg import Position
00006 from std_msgs.msg import Empty
00007 from crazyflie_driver.srv import UpdateParams
00008
00009 if __name__ == '__main__':
00010 rospy.init_node('position', anonymous=True)
00011 worldFrame = rospy.get_param("~worldFrame", "/world")
00012
00013 rate = rospy.Rate(10)
00014 name = "cmd_position"
00015
00016 msg = Position()
00017 msg.header.seq = 0
00018 msg.header.stamp = rospy.Time.now()
00019 msg.header.frame_id = worldFrame
00020 msg.x = 0.0
00021 msg.y = 0.0
00022 msg.z = 0.0
00023 msg.yaw = 0.0
00024
00025 pub = rospy.Publisher(name, Position, queue_size=1)
00026
00027 stop_pub = rospy.Publisher("cmd_stop", Empty, queue_size=1)
00028 stop_msg = Empty()
00029
00030 rospy.wait_for_service('update_params')
00031 rospy.loginfo("found update_params service")
00032 update_params = rospy.ServiceProxy('update_params', UpdateParams)
00033
00034 rospy.set_param("kalman/resetEstimation", 1)
00035 update_params(["kalman/resetEstimation"])
00036 rospy.sleep(0.1)
00037 rospy.set_param("kalman/resetEstimation", 0)
00038 update_params(["kalman/resetEstimation"])
00039 rospy.sleep(0.5)
00040
00041
00042 while not rospy.is_shutdown():
00043 for y in range(10):
00044 msg.x = 0.0
00045 msg.y = 0.0
00046 msg.yaw = 0.0
00047 msg.z = y / 25.0
00048 now = rospy.get_time()
00049 msg.header.seq += 1
00050 msg.header.stamp = rospy.Time.now()
00051 rospy.loginfo("sending...")
00052 rospy.loginfo(msg.x)
00053 rospy.loginfo(msg.y)
00054 rospy.loginfo(msg.z)
00055 rospy.loginfo(msg.yaw)
00056
00057 pub.publish(msg)
00058 rate.sleep()
00059 for y in range(20):
00060 msg.x = 0.0
00061 msg.y = 0.0
00062 msg.yaw = 0.0
00063 msg.z = 0.4
00064 msg.header.seq += 1
00065 msg.header.stamp = rospy.Time.now()
00066 rospy.loginfo("sending...")
00067 rospy.loginfo(msg.x)
00068 rospy.loginfo(msg.y)
00069 rospy.loginfo(msg.z)
00070 rospy.loginfo(msg.yaw)
00071
00072 pub.publish(msg)
00073 rate.sleep()
00074 break
00075
00076
00077 start = rospy.get_time()
00078 while not rospy.is_shutdown():
00079 msg.x = 0.2
00080 msg.y = 0.2
00081 msg.yaw = 0.0
00082 msg.z = 0.4
00083 now = rospy.get_time()
00084 if (now - start > 3.0):
00085 break
00086 msg.header.seq += 1
00087 msg.header.stamp = rospy.Time.now()
00088 rospy.loginfo("sending...")
00089 rospy.loginfo(msg.x)
00090 rospy.loginfo(msg.y)
00091 rospy.loginfo(msg.z)
00092 rospy.loginfo(msg.yaw)
00093 pub.publish(msg)
00094 rate.sleep()
00095
00096
00097 start = rospy.get_time()
00098 while not rospy.is_shutdown():
00099 msg.x = 0.0
00100 msg.y = 0.0
00101 msg.z = 0.0
00102 msg.yaw = 0.0
00103 now = rospy.get_time()
00104 if (now - start > 1.0):
00105 break
00106 msg.header.seq += 1
00107 msg.header.stamp = rospy.Time.now()
00108 rospy.loginfo("sending...")
00109 rospy.loginfo(msg.x)
00110 rospy.loginfo(msg.y)
00111 rospy.loginfo(msg.z)
00112 rospy.loginfo(msg.yaw)
00113 pub.publish(msg)
00114 rate.sleep()
00115
00116 stop_pub.publish(stop_msg)