compute.py
Go to the documentation of this file.
00001 #
00002 #  @author     Mike Purvis <mpurvis@clearpathrobotics.com>
00003 #  @copyright  Copyright (c) 2013, Clearpath Robotics, Inc.
00004 # 
00005 # Redistribution and use in source and binary forms, with or without
00006 # modification, are permitted provided that the following conditions are met:
00007 #     # Redistributions of source code must retain the above copyright
00008 #       notice, this list of conditions and the following disclaimer.
00009 #     # Redistributions in binary form must reproduce the above copyright
00010 #       notice, this list of conditions and the following disclaimer in the
00011 #       documentation and/or other materials provided with the distribution.
00012 #     # Neither the name of Clearpath Robotics, Inc. nor the
00013 #       names of its contributors may be used to endorse or promote products
00014 #       derived from this software without specific prior written permission.
00015 # 
00016 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00017 # ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00018 # WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00019 # DISCLAIMED. IN NO EVENT SHALL CLEARPATH ROBOTICS, INC. BE LIABLE FOR ANY
00020 # DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00021 # (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00022 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00023 # ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00024 # (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00025 # SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00026 # 
00027 # Please send comments, questions, or patches to code@clearpathrobotics.com 
00028 
00029 
00030 from roslib.packages import get_pkg_dir
00031 import rospy
00032 
00033 from std_msgs.msg import Float32
00034 from sensor_msgs.msg import NavSatFix
00035 
00036 from geomag.geomag import GeoMag
00037 from math import radians
00038 from os import path
00039 
00040 
00041 class Compute:
00042     def __init__(self):
00043         rospy.init_node("declination_provider")
00044         wmm_path = path.join(get_pkg_dir('declination'), "wmm", "WMM.COF")
00045         self.gm = GeoMag(wmm_path)
00046 
00047         self.fix = None
00048         self.sub = rospy.Subscriber("fix", NavSatFix, self._fix)
00049         self.pub = rospy.Publisher("declination", Float32, latch=True)
00050 
00051     def _fix(self, msg):
00052         if self.fix:
00053             # For now, return. Later, figure out conditions under which to recompute.
00054             return
00055 
00056         if msg.latitude and msg.longitude:
00057             self.fix = msg
00058             if not msg.altitude: msg.altitude = 0
00059             result = self.gm.calc(msg.latitude, msg.longitude, msg.altitude)
00060             result_rad = radians(result.dec)
00061             self.pub.publish(result_rad) 
00062             rospy.loginfo("Computed magnetic declination to be %f rad (%f degrees)" \
00063                 % (result_rad, result.dec))
00064 
00065     def spin(self):
00066         rospy.spin()


declination
Author(s):
autogenerated on Fri Aug 28 2015 10:27:05