## devices that were connected via a serial connection. We can find out if there
## is any 3-Space Sensor devices by using the getSensorInfo function. However,
## we must send commands over the port which could make devices other than
## 3-Space Sensor devices act strangely. So, be sure to know what port your
## 3-Space Sensor device(s) are on when calling this function.
## getSensorInfo returns a list of information that it found from the COM port
## (Friendly name, 3-Space Type, 3-Space ID, 3-Space Firmware Version String,
## 3-Space Hardware Version String, is in bootloader mode)
for device_port in device_list:
com_port, friendly_name, device_type = device_port
if device_type == "USB":
device = ts_api.TSUSBSensor(com_port=com_port)
elif device_type == "DNG":
device = ts_api.TSDongle(com_port=com_port)
elif device_type == "WL":
device = ts_api.TSWLSensor(com_port=com_port)
elif device_type == "EM":
device = ts_api.TSEMSensor(com_port=com_port)
elif device_type == "DL":
device = ts_api.TSDLSensor(com_port=com_port)
elif device_type == "BT":
device = ts_api.TSBTSensor(com_port=com_port)
elif device_type == "BTL":
device = "Device on " + com_port + " is in bootloader mode."
elif device_type == "???":
port_info = ts_api.getDeviceInfoFromComPort(com_port, poll_device=True)
device_type = port_info[1]
is_bootloader = port_info[5]
if is_bootloader:
device = "Device on %s is in bootloader mode." % com_port
elif device_type == "USB":
import math
import string
import sensor_table
import threespace_api as tsa
import find_ports
from threespace_api import *
from socket import *
rospy.init_node("poll_sensors")
result = find_ports.findPorts()
sensor_list = []
for a_port in result:
try:
rospy.loginfo("Checking for sensor in port %s", a_port)
sensor = tsa.TSWLSensor(com_port=a_port, baudrate=115200)
rospy.logwarn("Found Sensor")
rospy.logwarn(sensor)
hwid = None
panid = None
channel = None
att = 0
while hwid is None or panid is None or channel is None:
try:
att += 1
rospy.loginfo("Attempting to get Sensor Data attempt #%d", att)
hwid = convertString(sensor.f7WriteRead('getSerialNumber'))
panid = tsa.TSWLSensor.getWirelessPanID(sensor)
channel = tsa.TSWLSensor.getWirelessChannel(sensor)
rospy.logwarn("HW ID : %s", hwid)
rospy.logwarn("PanID : %s", panid)
def returnDev(arg):
# rospy.init_node("detect")
result = find_ports.findPorts()
dng_list = []
dev_list = []
rospy.logwarn(result)
if arg == "dng":
for a_port in result:
try:
rospy.loginfo("Checking for dongle in port %s", a_port)
dongle = tsa.TSDongle(com_port=a_port, baudrate=115200)
rospy.logwarn(dongle)
hwid = convertString(dongle.f7WriteRead('getSerialNumber'))
panid = None
channel = None
wa = None
att = 0
while panid is None or channel is None or wa is None:
try:
att += 1
if att > 10:
break
rospy.loginfo(
"Attempting to get Dongle data from port %s, attempt #%d",
a_port, att)
channel = tsa.TSDongle.getWirelessChannel(dongle)
panid = tsa.TSDongle.getWirelessPanID(dongle)
wa = tsa.TSDongle.getWirelessAddress(dongle)
rospy.logwarn("HW ID : %s", hwid)
rospy.logwarn("PanID : %s", panid)
rospy.logwarn("Channel : %s", channel)
rospy.logwarn("Wireless Address %s", wa)
rospy.logwarn(dongle.wireless_table)
dng_list.append(dongle)
except:
pass
except:
pass
print dng_list
return dng_list
else:
for a_port in result:
try:
rospy.loginfo("Checking for wireless sensor in port %s",
a_port)
sensor = tsa.TSWLSensor(com_port=a_port, baudrate=115200)
rospy.logwarn(sensor)
hwid = convertString(sensor.f7WriteRead('getSerialNumber'))
# dev_list.append(sensor)
panid = None
channel = None
wa = None
att = 0
while panid is None or channel is None or wa is None:
try:
att += 1
if att > 10:
break
rospy.loginfo(
"Attempting to get Wireless Sensor data from port %s, attempt #%d",
a_port, att)
channel = tsa.TSWLSensor.getWirelessChannel(sensor)
# tsa.TSWLSensor.setWirelessPanID(sensor, channel)
panid = tsa.TSWLSensor.getWirelessPanID(sensor)
wa = tsa.TSWLSensor.getWirelessAddress(sensor)
rospy.logwarn("HW ID : %s", hwid)
rospy.logwarn("PanID : %s", panid)
rospy.logwarn("Channel : %s", channel)
rospy.logwarn("Wireless Address %s", wa)
dev_list.append(sensor)
except:
pass
except:
pass
return dev_list
## (COM port name, friendly name, 3-Space Sensor device type)
## This example makes use of the filter parameter of getComPorts and just
## searches for Wireless devices and Dongle devices.
#device_list = ts_api.getComPorts(filter=ts_api.TSS_FIND_DNG)
## Only the 3-Space Sensor Dongle device is connected so we are just going to
## take the first one from the list.
com_port = '/dev/ttyACM0' #device_list[0]
dng_device = ts_api.TSDongle(com_port=com_port)
## If a connection to the COM port fails, None is returned.
if dng_device is not None:
## Now we need to get our Wireless device from our Dongle device.
## Indexing into the TSDongle instance like it was a list will return a
## TSWLSensor instance.
wl_device = ts_api.TSWLSensor(logical_id=3, dongle=dng_device)
## Now we can start getting information from the Wireless device.
## The class instances have all of the functionality that corresponds to the
## 3-Space Sensor device type it is representing.
print("==================================================")
print("Getting the filtered tared quaternion orientation.")
quat = wl_device.getTaredOrientationAsQuaternion()
if quat is not None:
print(quat)
print("==================================================")
print("Getting the raw sensor data.")
data = wl_device.getAllRawComponentSensorData()
if data is not None:
print(
"[%f, %f, %f] --Gyro\n[%f, %f, %f] --Accel\n[%f, %f, %f] --Comp" %