def spin(self):
while not rospy.is_shutdown():
now = time()
if(now - self.last_msg_time) < 1.0/self.img_hz:
rospy.loginfo("%s: Drone locked" % rospy.get_name())
# Manipulate drones position
utm_pose_out = self.utm_pose_in
ll_pose_out = self.uc.utm_to_geodetic(self.offset_hemisphere, self.offset_zone, utm_pose_out[0], utm_pose_out[1])
print "Dist from offset: ", hypot(utm_pose_out[0] - self.offset_easting, utm_pose_out[1] - self.offset_northing)
print "drone position: ", ll_pose_out
gps = GPS()
gps.lat = ll_pose_out[0]
gps.lon = ll_pose_out[1]
gps.height = self.height
# gps.DOP = 2
self.pub.publish(gps)
else:
rospy.logwarn("%s:No drone position" % rospy.get_name())
self.rate.sleep()
#!/usr/bin/python
# license removed for brevity
import rospy
from time import sleep
from Communication.msg import GPS
if __name__ == '__main__':
pub = rospy.Publisher('/drone_id', GPS, queue_size=10)
rospy.init_node('talker', anonymous=True)
# rate = rospy.Rate(10) # 10hz
hz = 20.0
while not rospy.is_shutdown():
raw_input("Enter to send 200 msg")
msg = GPS()
msg.DOP = 10
msg.height = 10.43
msg.lat = 10.123
msg.lon = 10.321
for i in range(200):
status = float(i)/200.0*100
rospy.loginfo("msg out %d" % status)
pub.publish(msg)
sleep(1/hz)
