Go to the documentation of this file.00001
00002
00003
00004 import roslib; roslib.load_manifest('clearpath_sensors')
00005 import rospy
00006 from geometry_msgs.msg import Point
00007
00008
00009 import serial
00010
00011 def convert_to_signed(val):
00012 if val > 2**15:
00013 return val - 2**16
00014 else:
00015 return val
00016
00017 class CMPS09(object):
00018
00019 def __init__(self):
00020 self.x = 0
00021 self.y = 0
00022 self.z = 0
00023 rospy.init_node('cmps09')
00024
00025
00026 self.port = rospy.get_param('~port', '')
00027 self.hz = rospy.get_param('~hz', 20)
00028 self.timeout = rospy.get_param('~timeout', 0.2)
00029
00030 if self.port != '':
00031 rospy.loginfo("CMPS09 using port %s.",self.port)
00032 else:
00033 rospy.logerr("No port specified for CMPS09!")
00034 exit(1)
00035
00036 try:
00037 self.transport = serial.Serial(port=self.port, baudrate=9600, timeout=self.timeout)
00038 self.transport.open()
00039 except serial.serialutil.SerialException as e:
00040 rospy.logerr("CMPS09: %s" % e)
00041 exit(1)
00042
00043
00044 rospy.on_shutdown(self.shutdown_handler)
00045
00046
00047 self.pub = rospy.Publisher('raw_mag', Point)
00048
00049
00050 def shutdown_handler(self):
00051 self.transport.close()
00052 rospy.loginfo("CMPS09 shutdown.")
00053
00054
00055 def run(self):
00056 rate = rospy.Rate(self.hz)
00057 consecutive_errors = 0
00058
00059 while not rospy.is_shutdown():
00060 try:
00061 self.transport.write('\x22')
00062 raw = self.transport.read(6)
00063 except OSError as e:
00064 rospy.logerr("CMPS09: %s" % e)
00065 exit(1)
00066
00067 if len(raw) < 2:
00068
00069 consecutive_errors += 1
00070 if consecutive_errors == 3:
00071 rospy.logerr("CMPS09 receiving no data. Will try again every second.")
00072 elif consecutive_errors > 3:
00073 rospy.sleep(1.0)
00074 continue
00075
00076
00077 if consecutive_errors >= 3:
00078
00079
00080 rospy.loginfo("CMPS09 receiving data successfully.")
00081 consecutive_errors = 0
00082
00083 alpha = 0.0625
00084 self.x = (1-alpha)*self.x + alpha*convert_to_signed(ord(raw[0]) << 8 | ord(raw[1]))
00085 self.y = (1-alpha)*self.y + alpha*convert_to_signed(ord(raw[2]) << 8 | ord(raw[3]))
00086 self.z = (1-alpha)*self.z + alpha*convert_to_signed(ord(raw[4]) << 8 | ord(raw[5]))
00087 self.pub.publish(Point(self.x,self.y,self.z))
00088
00089 rate.sleep()
00090
00091
00092
00093 CMPS09().run()