compute.py
Go to the documentation of this file.
1 #
2 # @author Mike Purvis <mpurvis@clearpathrobotics.com>
3 # @copyright Copyright (c) 2013, Clearpath Robotics, Inc.
4 #
5 # Redistribution and use in source and binary forms, with or without
6 # modification, are permitted provided that the following conditions are met:
7 # # Redistributions of source code must retain the above copyright
8 # notice, this list of conditions and the following disclaimer.
9 # # Redistributions in binary form must reproduce the above copyright
10 # notice, this list of conditions and the following disclaimer in the
11 # documentation and/or other materials provided with the distribution.
12 # # Neither the name of Clearpath Robotics, Inc. nor the
13 # names of its contributors may be used to endorse or promote products
14 # derived from this software without specific prior written permission.
15 #
16 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17 # ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 # WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 # DISCLAIMED. IN NO EVENT SHALL CLEARPATH ROBOTICS, INC. BE LIABLE FOR ANY
20 # DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 # (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 # ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 # (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 # SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 #
27 # Please send comments, questions, or patches to code@clearpathrobotics.com
28 
29 
30 from roslib.packages import get_pkg_dir
31 import rospy
32 
33 from std_msgs.msg import Float32
34 from sensor_msgs.msg import NavSatFix
35 
36 from geomag.geomag import GeoMag
37 from math import radians
38 from os import path
39 
40 
41 class Compute:
42  def __init__(self):
43  rospy.init_node("declination_provider")
44  wmm_path = path.join(get_pkg_dir('declination'), "wmm", "WMM.COF")
45  self.gm = GeoMag(wmm_path)
46 
47  self.fix = None
48  self.sub = rospy.Subscriber("fix", NavSatFix, self._fix)
49  self.pub = rospy.Publisher("declination", Float32, latch=True)
50 
51  def _fix(self, msg):
52  if self.fix:
53  # For now, return. Later, figure out conditions under which to recompute.
54  return
55 
56  if msg.latitude and msg.longitude:
57  self.fix = msg
58  if not msg.altitude: msg.altitude = 0
59  result = self.gm.calc(msg.latitude, msg.longitude, msg.altitude)
60  result_rad = radians(result.dec)
61  self.pub.publish(result_rad)
62  rospy.loginfo("Computed magnetic declination to be %f rad (%f degrees)" \
63  % (result_rad, result.dec))
64 
65  def spin(self):
66  rospy.spin()
def _fix(self, msg)
Definition: compute.py:51


declination
Author(s):
autogenerated on Mon Jun 10 2019 13:01:42