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