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())
import lcm
import sys
lc = lcm.LCM()
sys.path.insert(0, '../')
sys.path.insert(0, '../lcmt')
sys.path.insert(0, '../mav')
from pose_t import pose_t
msg = pose_t()
msg.utime = 42
lc.publish("test", msg.encode())
import lcm
import sys
lc = lcm.LCM()
sys.path.insert(0, '../')
sys.path.insert(0, '../lcmt')
sys.path.insert(0, '../mav')
from pose_t import pose_t
msg = pose_t();
msg.utime = 42;
lc.publish("test", msg.encode())
