00001
00002
00003
00004 import roslib; roslib.load_manifest('clearpath_sensors')
00005 import rospy
00006 from geometry_msgs.msg import Pose2D
00007
00008
00009 import serial, time
00010
00011 FIRST_TIMEOUT_SECS = 3.0
00012 TIMEOUT_SECS = 0.3
00013 RETRY_OPEN_SECS = 1.0
00014
00015 class CMPS09(object):
00016 def __init__(self):
00017 rospy.init_node('cmps09')
00018
00019
00020 self.port = rospy.get_param('~port', '/dev/usb-compass')
00021 self.hz = rospy.get_param('~hz', 10)
00022 self.offset = rospy.get_param('~offset', 0.0)
00023 self.serial = None
00024
00025 if self.port != '':
00026 rospy.loginfo("Compass using port %s.",self.port)
00027 else:
00028 rospy.logerr("No port specified for compass!")
00029 exit(1)
00030
00031 self.start_serial()
00032 self.pub = rospy.Publisher('compass', Pose2D)
00033
00034 def start_serial(self):
00035 if self.serial:
00036 self.serial.close()
00037 self.serial = None
00038
00039 error = None
00040 while True:
00041 try:
00042 self.serial = serial.Serial(self.port, 9600, timeout=0.2)
00043 self.timeout = time.clock() + FIRST_TIMEOUT_SECS
00044 rospy.loginfo("Opened compass on port: %s" % self.port)
00045 break;
00046 except serial.SerialException as e:
00047 if not error:
00048 error = e
00049 rospy.logerr("Error opening compass on %s: %s" % (self.port, error))
00050 rospy.logerr("Will retry opening compass every second.")
00051 time.sleep(RETRY_OPEN_SECS)
00052
00053 if rospy.is_shutdown():
00054 exit(1)
00055
00056
00057
00058 def run(self):
00059 rate = rospy.Rate(self.hz)
00060 consecutive_errors = 0
00061
00062 while not rospy.is_shutdown():
00063 try:
00064 self.serial.write('\x13')
00065 raw = self.serial.read(2)
00066 except OSError as e:
00067 rospy.logerr("Compass serial error: %s" % e)
00068 rospy.loginfo("Reopening compass serial port.")
00069 self.start_serial()
00070 continue
00071
00072 if len(raw) < 2:
00073
00074 consecutive_errors += 1
00075 if consecutive_errors >= 3:
00076 rospy.logerr("CMPS09 received no data for 3 consecutive requests.")
00077 time.sleep(RETRY_OPEN_SECS)
00078 rospy.loginfo("Reopening compass serial port.")
00079 self.start_serial()
00080 continue
00081
00082
00083 consecutive_errors = 0
00084
00085 data = (ord(raw[0]) << 8) | ord(raw[1])
00086 data2 = (data / 10.0) + self.offset
00087 if data2 < 0.0: data2 += 360.0
00088 elif data2 > 360.0: data2 -= 360.0
00089
00090 self.pub.publish(Pose2D(theta=(data2)))
00091
00092 rate.sleep()
00093
00094
00095
00096 CMPS09().run()