def my_handler(channel, data):
msg = pos_gps_t.decode(data)
print("Received message on channel \"%s\"" % channel)
print("")
print "robotid: ", msg.robotid
print "longitude: ", msg.longitude
print "latitude: ", msg.latitude
print "altitude: ", msg.altitude
def my_handler(channel, data):
msg = pos_gps_t.decode(data)
print("Received message on channel \"%s\"" % channel)
print("")
print "robotid: ", msg.robotid
print "longitude: ", msg.longitude
print "latitude: ", msg.latitude
print "altitude: ", msg.altitude
print "transform"
p1 = pyproj.Proj(init='epsg:4326')
p2 = pyproj.Proj(init='epsg:21781')
mx, my = pyproj.transform(p1, p2, msg.longitude ,msg.latitude)
print mx, my
msg_pose = pose_t()
msg_pose.robotid = msg.robotid
msg_pose.position = [mx+offset_x,my+offset_y,msg.altitude]
print msg_pose.position
msg_pose.orientation = [0,0,0,0]
lc.publish(pose_channel , msg_pose.encode())
def my_handler(channel, data):
msg = pos_gps_t.decode(data)
send_poselist(msg.robotid, msg.longitude, msg.latitude, msg.altitude)
