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