~cyphylab/cyphy/bonsai

« back to all changes in this revision

Viewing changes to cyphy_Drivers/cyphy_xsens_mtig/nodes/lla2tf.py

• browse files at revision 92
• compare with another revision
• download diff
• download tarball
• view history from revision 92

Show diffs side-by-side

added

removed

cyphy_Drivers/cyphy_xsens_mtig/nodes/lla2tf.py

 

1

#!/usr/bin/env python

 

2

#Converter for LLA WGS84 to local coord system

 

3

import roslib

 

4

roslib.load_manifest('xsens_mtig')

 

5

import rospy

 

6

import tf

 

7

from sensor_msgs.msg import NavSatFix

 

8

import math

 

9

 

 

10

global long, lat, radius

 

11

 

 

12

 

 

13

def satData(data):

 

14

    global long, lat, radius

 

15

    #print "gotdata"

 

16

    br = tf.TransformBroadcaster()

 

17

    a = 6378137

 

18

    b = 6356752

 

19

    ang_ecc = math.acos(b/a)

 

20

 

 

21

 

 

22

 

 

23

    if radius == 0:

 

24

        radius = a/math.sqrt(1-pow(math.sin(lat)*math.sin(ang_ecc),2))

 

25

        long = math.radians(data.longitude)

 

26

        lat = math.radians(data.latitude)

 

27

        print "Initialised"

 

28

 

 

29

    N = radius

 

30

 

 

31

    x = radius*math.sin(math.radians(data.latitude)-lat)

 

32

    y = radius*math.sin(math.radians(data.longitude)-long)

 

33

    

 

34

 

 

35

 

 

36

    br.sendTransform((x, y, 0),

 

37

                     tf.transformations.quaternion_from_euler(0, 0, 0),

 

38

                     rospy.Time.now(),

 

39

                     "base_link",

 

40

                     "gps")

 

41

 

 

42

 

 

43

if __name__ == '__main__':

 

44

    rospy.init_node('lla2tf', anonymous=True)

 

45

    rospy.Subscriber("/nav", NavSatFix, satData)

 

46

 

 

47

    radius = 0

 

48

    long = 0

 

49

    lat = 0

 

50

 

 

51

    r = rospy.Rate(20) 

 

52

    rospy.spin()

 

53

 

 

54

    print "\nROS shutdown"

• Older »

Loggerhead is a web-based interface for Breezy
Version: 2.0.1