def __init__(self):
"""
Initialize OptoforceDriver object
"""
port = rospy.get_param("~port", "/dev/ttyACM0")
sensor_type = rospy.get_param("~type", "m-ch/3-axis")
starting_index = rospy.get_param("~starting_index", 0)
scaling_factors = rospy.get_param("~scale")
# Initialize optoforce driver
try:
self._driver = optoforce.OptoforceDriver(port,
sensor_type,
scaling_factors,
starting_index)
except serial.SerialException as e:
rospy.logfatal("Cannot connect to the sensor " + port
+ (e.message if e.message else ''))
rospy.signal_shutdown("Serial connection failure")
raise
# Create and advertise publishers for each connected sensor
self._publishers = []
self._wrenches = []
topic_basename = "optoforce_"
for i in range(self._driver.nb_sensors()):
self._publishers.append(rospy.Publisher(topic_basename +
str(starting_index + i),
WrenchStamped,
queue_size=100))
wrench = WrenchStamped()
wrench.header.frame_id = topic_basename + str(starting_index + i)
self._wrenches.append(wrench)
def _initialize_driver(self, port, sensor_type, scaling_factors,
starting_index):
try:
self._driver = optoforce.OptoforceDriver(port, sensor_type,
scaling_factors,
starting_index)
except serial.SerialException as e:
rospy.logfatal("Cannot connect to the sensor " + port +
(e.message if e.message else ''))
rospy.signal_shutdown("Serial connection failure")
raise
def getForceData():
test = optoforce.OptoforceDriver('/dev/ttyACM0',"s-ch/6-axis",
[[41.1398,41.0959,7.60483,1158,1082,1513.5]],starting_index = 0)
test.config("100Hz","15Hz",zero = False)
while True:
# print(getForceData())
data = test.read()
if isinstance(data, optoforce.OptoforceData):
# print(data)
for i in range(test.nb_sensors()):
# print('fx=',float(data.force[i][0]))
# print('fy=',float(data.force[i][1]))
# print('fz=',float(data.force[i][2]))
# print('Tx=',float(data.force[i][3]))
# print('Ty=',float(data.force[i][4]))
# print('Tz=',float(data.force[i][5]))
return(data.force[i][0:6])
# ch.setLevel(logging.DEBUG)
#
# # create formatter
# formatter = logging.Formatter('%(asctime)s :: %(name)s :: %(levelname)s :: %(message)s')
#
# # add formatter to ch
# ch.setFormatter(formatter)
#
# # add ch to logger
# logger.addHandler(ch)
if len(sys.argv) > 1:
port = "/dev/" + sys.argv[1]
sensor_type = "s-ch/3-axis"
scale = [[100, 100, 100]]
driver = optoforce.OptoforceDriver(port, sensor_type, scale)
driver.request_serial_number()
while not signal_handle.stop:
try:
data = driver.read()
if isinstance(data, optoforce.OptoforceData):
print '.',
elif isinstance(data, optoforce.OptoforceSerialNumber):
print("optoforce_" + str(data))
sys.exit(0)
except optoforce.OptoforceError:
pass
import random as r