def __init__(self): rospy.init_node('decawave_driver') #Init DecaWave port dwPort = rospy.get_param('~port','/dev/ttyUSB0') dwRate = rospy.get_param('~baud',9600) dwID = rospy.get_param('~id','t0') dwPub = rospy.Publisher('dw/' + dwID + '/data', DecaWaveMsg, queue_size = 5) try: ser = serial.Serial( port = dwPort, timeout = 10, baudrate = dwRate ) dwMsg = DecaWaveMsg() dwMsg.header.frame_id = "base_decawave_" + dwID ser.close() ser.open() d0 = 0 d1 = 0 d2 = 0 d3 = 0 while not rospy.is_shutdown(): # Check to make sure the bitstream is at the beginning of a message match = '\x6D' # x6D is the ascii character 'm' sync = ser.read() if sync != match: print "Sync didn't match." continue else: print "Sync matched. Starting Read." i = 0 while i <= 2: if i != 0: sync = ser.read() # read the m from the beginning of the message raw_data = ser.readline() if ((i == 0) and (raw_data[0:3] != b'a00')): print "Advance until at a00." break if raw_data[0:3] == b'a00': d0 = int(raw_data[8:16], 16) / 1000.00 elif raw_data[0:3] == b'a01': d1 = int(raw_data[8:16], 16) / 1000.00 elif raw_data[0:3] == b'a02': d2 = int(raw_data[8:16], 16) / 1000.00 elif raw_data[0:3] == b'a03': d3 = int(raw_data[8:16], 16) / 1000.00 i+=1 dwMsg.dist = (d0, d1, d2, d3) dwMsg.header.stamp = rospy.get_rostime() dwPub.publish(dwMsg) rospy.loginfo(dwMsg.dist) ser.close() except SerialException: print("Could not connect to the serial port")
def __init__(self): rospy.init_node('decawave_driver') # Init DecaWave port self.dwPort = rospy.get_param('~port', 'dev/ttyACM0') self.dwRate = rospy.get_param('~baud', 115200) self.dwID = rospy.get_param('~id', 'a0') self.anchor_angle = rospy.get_param('~anchor_angle', 0) self.tag_angle = rospy.get_param('~tag_angle', 0) self.beacon_distance = rospy.get_param('~beacon_distance', 0) self.dwPub = rospy.Publisher('/ranger_finder/data', DecaWaveMsg, queue_size=5) #filepath = str(rospkg.RosPack().get_path('decawave_driver')) + '/src/ranging_tests/range.csv' #fh = open(filepath, "w") self.last_time = rospy.rostime.Time(0,0) try: ser = serial.Serial( port=self.dwPort, timeout=10, baudrate=self.dwRate ) dwMsg = DecaWaveMsg() dwMsg.header.frame_id = "base_decawave_" + self.dwID ser.close() ser.open() range0 = 0 i=1 while (not rospy.is_shutdown()): raw_data = ser.readline() if raw_data == serial.to_bytes([]): print "serial timeout" else: data = raw_data.split(' ') if data[0] == 'mc': dwMsg.header.stamp = rospy.Time.now() #now = dwMsg.header.stamp rospy.loginfo("UW time %f", rospy.get_time()) mask = int(data[1], 16) if (mask & 0x01): #global range0 range0 = int(data[2], 16) / 1000.0 if range0 != 0: print range0 dwMsg.dist = range0 self.dwPub.publish(dwMsg) # publish the topic #i=i+1 else: print "range0 bad" ser.close() #fh.close() except SerialException: print("Could not connect to the serial port")
def __init__(self): rospy.init_node('decawave_driver') #Init DecaWave port self.dwPort = rospy.get_param('~port', 'dev/ttyACM0') self.dwRate = rospy.get_param('~baud', 115200) self.dwID = rospy.get_param('~id', 'a0') self.anchor_angle = rospy.get_param('~anchor_angle', 0) self.tag_angle = rospy.get_param('~tag_angle', 0) self.beacon_distance = rospy.get_param('~beacon_distance', 0) self.dwPub = rospy.Publisher('/ranger_finder/data', DecaWaveMsg, queue_size=5) try: ser = serial.Serial(port=self.dwPort, timeout=10, baudrate=self.dwRate) dwMsg = DecaWaveMsg() dwMsg.header.frame_id = "base_decawave_" + self.dwID ser.close() ser.open() range0 = 0 loop_rate = rospy.Rate(3.33) time_array = np.array([]) while (not rospy.is_shutdown()): raw_data = ser.readline() if raw_data == serial.to_bytes([]): print "serial timeout" else: data = raw_data.split(' ') if data[0] == 'mc': dwMsg.header.stamp = rospy.Time.now() now = dwMsg.header.stamp rospy.loginfo("UW time %f", rospy.get_time()) mask = int(data[1], 16) if (mask & 0x01): global range0 range0 = int(data[2], 16) / 1000.0 if range0 != 0: print range0 dwMsg.dist = range0 self.dwPub.publish(dwMsg) #publish the topic else: print "range0 bad" ser.close() file.flush() file.close() except SerialException: print("Could not connect to the serial port")
def __init__(self): rospy.init_node('decawave_driver') #Init DecaWave port dwPort = rospy.get_param('~port','/dev/ttyUSB0') dwRate = rospy.get_param('~baud',9600) dwID = rospy.get_param('~id','t0') dwPub = rospy.Publisher('dw/' + dwID + '/data', DecaWaveMsg, queue_size = 5) try: ser = serial.Serial( port = dwPort, timeout = 10, baudrate = dwRate ) dwMsg = DecaWaveMsg() dwMsg.header.frame_id = "base_decawave_" + dwID ser.close() ser.open() # initialize range values in case a measurement is dropped the first time range0 = 0 range1 = 0 range2 = 0 range3 = 0 while not rospy.is_shutdown(): raw_data = ser.readline() if raw_data == serial.to_bytes([]): print "serial timeout" else: data = raw_data.split() if data[0] == 'mc': #print "read data as a list:" #print data mask = int(data[1],16) if (mask & 0x01): #print "range0 good" range0 = int(data[2],16)/1000.0 else: print "range0 bad" # range0 = -1 if (mask & 0x02): #print "range1 good" range1 = int(data[3],16)/1000.0 else: print "range1 bad" # range1 = -1 if (mask & 0x04): #print "range2 good" range2 = int(data[4],16)/1000.0 else: print "range2 bad" # range2 = -1 if (mask & 0x08): #print "range3 good" range3 = int(data[5],16)/1000.0 else: print "range3 bad" # range3 = -1 dwMsg.dist = (range0, range1, range2, range3) dwMsg.header.stamp = rospy.get_rostime() dwPub.publish(dwMsg) rospy.loginfo(dwMsg.dist) ser.close() except SerialException: print("Could not connect to the serial port")