def __init__(self): self.pm = ml2437a_driver() self.pub_ave_onoff = rospy.Publisher("ml2437a_ave_onoff", Int32, queue_size=1) self.sub_ave_onoff = rospy.Subscriber("ml2437a_ave_onoff_cmd", Int32, self.ave_onoff) self.pub_ave_count = rospy.Publisher("ml2437a_ave_count", Int32, queue_size=1) self.sub_ave_count = rospy.Subscriber("ml2437a_ave_count_cmd", Int32, self.ave_count) self.rate = rospy.get_param('~rate') self.mode = 'diff' self.ch = 17 rsw_id = rospy.get_param('~rsw_id') self.pub_power = rospy.Publisher('cpz3177_rsw{0}_{1}{2}'.format( rsw_id, self.mode, self.ch), Float64, queue_size=1) node_name = 'cpz3177' try: self.ad = pyinterface.open(3177, rsw_id) except OSError as e: rospy.logerr("{e.strerror}. node={node_name}, rsw={rsw_id}".format( **locals())) sys.exit()
def __init__(self): self.rate = rospy.get_param('~rate') self.rsw_id = rospy.get_param('~rsw_id') self.node_name = 'cpz340816' self.flag = 1 self.lock = 0 self.param_buff = [] try: self.da = pyinterface.open(3408, self.rsw_id) except OSError as e: rospy.logerr("{e.strerror}. node={node_name}, rsw={rsw_id}".format( self.node_name, self.rsw_id)) sys.exit() self.topic_list = [('{0}_rsw{1}_{2}'.format(self.node_name, self.rsw_id, _)) for _ in range(1, ch_number + 1)] self.pub_list = [ rospy.Publisher(topic, Float64, queue_size=1) for topic in self.topic_list ] self.sub_list = [ rospy.Subscriber(topic + '_cmd', Float64, self.set_param, callback_args=ch) for ch, topic in enumerate(self.topic_list, start=1) ] self.sub_lock = rospy.Subscriber( '{0}_rsw{1}_lock'.format(self.node_name, self.rsw_id), Int32, self.set_lock)
def __init__(self): board_name = 2724 rsw_id = 0 #rotary switch id self.dio = pyinterface.open(board_name, rsw_id) self.dio.initialize() ut = time.gmtime() self.dir_name = time.strftime( "/home/necst/data/experiment/save_azel/%Y_%m_%d_%H_%M_%S", ut) os.mkdir(self.dir_name)
def __init__(self): board_name = 6204 rsw_id = 0 self.dio = pyinterface.open(board_name, rsw_id) self.initialize() self.sub = rospy.Service("encoder_origin", Bool_srv, self.origin_setting) th = threading.Thread(target=self.origin_flag_check) th.start() pass
def __init__(self, ndev2=1): board_name = 6204 rsw_id = 1 #self.dio = pyinterface.create_gpg6204(ndev2) self.dio = pyinterface.open(board_name, rsw_id) #self.dio = shiotani_pyinterface.create_gpg6204(ndev2) self.dome_enc_initialize() #self.dio.ctrl.set_mode(4, 0, 1, 0) #self.dome_encoder_acq() pass
def __init__(self): self.da = pyinterface.open(3346, rsw_id) self.da.initialize() topic_li = [] for ch in ch_num_li: rospy.Subscriber(name="/dev/pci3346A/rsw%d/ch%d" % (rsw_id, ch), data_class=Float64, callback=self.callback, callback_args=ch, queue_size=1) self.ch_list = [eval(rospy.get_param(i)) for i in _ch_name_li] self.data_li = [] self.data_dic = {} pass
def __init__(self): self.rsw_id = rospy.get_param("~rsw_id") try: self.dio = pyinterface.open(2724, self.rsw_id) self.dio.initialize() except OSError as e: rospy.logerr(e, name, self.rsw_id) sys.exit() self.pub = [rospy.Publisher( name = "cpz2724_rsw{0}/di{1}".format(self.rsw_id, ch), data_class = std_msgs.msg.Bool, latch = True, queue_size = 1 ) for ch in self.ch_list] self.sub = [rospy.Subscriber( name = "cpz2724_rsw{0}/do{1}".format(self.rsw_id, ch), data_class = std_msgs.msg.Bool, callback = self.output_point, callback_args = ch, queue_size = 1 ) for ch in self.ch_list] self.sub_byte = [rospy.Subscriber( name = "cpz2724_rsw{0}/do{1}".format(self.rsw_id, ch), data_class = std_msgs.msg.ByteMultiArray, callback = self.output_byte, callback_args = ch, queue_size = 1 ) for ch in self.ch_list_byte] self.sub_word = [rospy.Subscriber( name = "cpz2724_rsw{0}/do{1}".format(self.rsw_id, ch), data_class = std_msgs.msg.ByteMultiArray, callback = self.output_word, callback_args = ch, queue_size = 1 ) for ch in self.ch_list_word] self.sub_dword = [rospy.Subscriber( name = "cpz2724_rsw{0}/do{1}".format(self.rsw_id, ch), data_class = std_msgs.msg.ByteMultiArray, callback = self.output_dword, queue_size = 1 ) for ch in self.ch_list_dword] pass
def __init__(self): self.rate = rospy.get_param('~rate') self.rsw_id = rospy.get_param('~rsw_id') self.node_name = 'cpz340816' self.flag = 1 self.lock = 0 self.param_buff = [] try: self.da = pyinterface.open(340816, self.rsw_id) except OSError as e: rospy.logerr("{e.strerror}. node={node_name}, rsw={rsw_id}". format(self.node_name, self.rsw_id)) sys.exit() ###=== Define Publisher ===### self.pub = rospy.Publisher(cpz340816, Float64, queue_size=1, latch = True) ###=== Define Subscriber ===### self.sub = rospy.Subscriber(cpz340816_rsw0_ch1, Float64, self.set_param)
def __init__(self,rsw_id, params): self.params = params self.func_queue = queue.Queue() self.use_axis = ''.join([ax for ax in self.params]) self.mode = [self.params[ax]['mode'] for ax in self.use_axis] self.motion = {ax: self.params[ax]['motion'] for ax in self.use_axis} self.current_speed = {ax: 0 for ax in self.use_axis} self.current_step = {ax: 0 for ax in self.use_axis} self.current_moving = {ax: 0 for ax in self.use_axis} self.last_direction = {ax: 0 for ax in self.use_axis} self.do_status = [0, 0, 0, 0] self.move_mode = {ax: p['mode'] for ax, p in self.params.items()} self.default_speed = {ax: p['motion']['speed'] for ax, p in self.params.items()} self.low_speed = {ax: p['motion']['low_speed'] for ax, p in self.params.items()} self.mot = pyinterface.open(7415,rsw_id) #self.mot.initialize() for ax in self.use_axis: self.mot.set_pulse_out(ax, 'method', self.params[ax]['pulse_conf']) self.mot.set_motion(self.use_axis, self.mode, self.motion) self.pub = {} base = '/pyinterface/pci7415/rsw{rsw_id}'.format(**locals()) for ax in self.use_axis: b = '{base}/{ax}/'.format(**locals()) #create publish self.pub[ax+'_speed'] = rospy.Publisher(b+'speed', std_msgs.msg.Float64, queue_size=1) self.pub[ax+'_step'] = rospy.Publisher(b+'step', std_msgs.msg.Int64, queue_size=1) rospy.Subscriber(b+'step_cmd', std_msgs.msg.Int64, self.set_step, callback_args=ax) rospy.Subscriber(b+'speed_cmd', std_msgs.msg.Float64, self.set_speed, callback_args=ax) for do_num in range(1,5): rospy.Subscriber('{}/output_do{}_cmd'.format(base, do_num), std_msgs.msg.Int64, self.set_do, callback_args=do_num) self.th = threading.Thread(target=self.loop) self.th.setDaemon(True) self.th.start() return
def __init__(self, rsw_id, params): self.rsw_id = rsw_id self.use_axis = ''.join([p['axis'] for p in params]) self.params = {} for p in params: self.params[p['axis']] = p continue # initialize motion controller self.mot = pyinterface.open(7415, rsw_id) self.mot.initialize() self.mot.output_do(p['do_conf']) [self.mot.set_pulse_out(p['axis'], 'method', p['pulse_conf']) for p in params] [self.mot.set_motion(p['axis'], [p['mode']], p['motion']) for p in params] self.last_direction_dict = {p['axis']: 0 for p in params} # create publishers base = '/cpz7415/rsw{rsw_id}'.format(**locals()) self.pub = {} for ax in self.use_axis: b = '{base}/{ax}/'.format(**locals()) self.pub[ax+'_step'] = rospy.Publisher('/dev'+b+'step', std_msgs.msg.Int64, queue_size=1) self.pub[ax+'_speed'] = rospy.Publisher('/dev'+b+'speed', std_msgs.msg.Int64, queue_size=1) continue # create subscrivers for ax in self.use_axis: b = '{base}/{ax}/'.format(**locals()) rospy.Subscriber('/dev'+b+'mode_cmd', std_msgs.msg.String, self.regist, callback_args=ax+'_mode') rospy.Subscriber('/dev'+b+'step_cmd', std_msgs.msg.Int64, self.regist, callback_args=ax+'_step') rospy.Subscriber('/dev'+b+'speed_cmd', std_msgs.msg.Int64, self.regist, callback_args=ax+'_speed') continue # create DIO pub/sub #self.pub['di'] = rospy.Publisher(base+'_di', std_msgs.msg.Int64, queue_size=1) rospy.Subscriber('/dev'+base+'/do_cmd', std_msgs.msg.Int64, self.regist, callback_args='do') # start threads time.sleep(0.5) self._start_threads() pass
def __init__(self): self.rate = rospy.get_param('~rate') self.rsw_id = rospy.get_param('~rsw_id') self.node_name = 'cpz340516' self.param_buff = [] try: self.da = pyinterface.open(3405, self.rsw_id) except OSError as e: rospy.logerr( '{e.strerror}. node={self.node_name}, rsw={self.rsw_id}'. format(*locals())) sys.exit() [ self.da.set_outputrange(ch, outputrange) for ch, outputrange in enumerate(select_outputrange(), start=1) ] self.topic_list = [ ('{0}_rsw{1}_{2}_{3}'.format(self.node_name, self.rsw_id, ch, outputrange)) for ch, outputrange in enumerate(select_outputrange(), start=1) ] self.pub_list = [ rospy.Publisher(name=topic, data_class=Float64, latch=True, queue_size=1) for topic in self.topic_list ] self.sub_list = [ rospy.Subscriber( name=topic + '_cmd', data_class=Float64, callback=self.set_param, callback_args=ch, # queue_size = 1 ) for ch, topic in enumerate(self.topic_list, start=1) ]
def __init__(self): self.pub_rate = rospy.get_param("~pub_rate") self.ave_num = rospy.get_param('~ave_num') self.smpl_freq = rospy.get_param("~smpl_freq") self.ad = pyinterface.open(3177, rsw_id) self.ad.stop_sampling() self.ad.initialize() self.ad.set_sampling_config(smpl_ch_req=smpl_ch_req, smpl_num=1000, smpl_freq=self.smpl_freq, single_diff=single_diff, trig_mode='ETERNITY' ) self.ad.start_sampling('ASYNC') self.pub_list = [rospy.Publisher("/dev/cpz3177/rsw%d/ch%d"%(rsw_id,ch), Float64, queue_size=1) for ch in ch_num_li] rospy.Subscriber("/dev/cpz3177/rsw%d/pub_rate"%(rsw_id),Float64, self.pub_rate_set) pass
def __init__(self): self.rsw_id = rospy.get_param("~rsw_id") if self.rsw_id == 0: self.ch_list = [1,2] self.ch_list_set = "" if self.rsw_id == 1: self.ch_list = [1] self.ch_list_set = [1] self.pub = [rospy.Publisher( name = "/cpz6204_rsw{0}/ch0{1}".format(self.rsw_id, ch), data_class = std_msgs.msg.Int64, latch = True, queue_size = 1, ) for ch in self.ch_list] self.sub = [rospy.Subscriber( name = "/cpz6204_rsw{0}/ch0{1}_set_counter".format(self.rsw_id, ch), data_class = std_msgs.msg.Int64, callback = self.set_counter, callback_args = ch, queue_size = 1 ) for ch in self.ch_list_set] if self.rsw_id == 0: self.sub_origin = rospy.Subscriber( name = "/cpz6204_rsw{0}/origin".format(self.rsw_id), data_class = std_msgs.msg.Bool, callback = self.set_origin, queue_size = 1, ) try: self.dio = pyinterface.open(6204, self.rsw_id) self.initialize() except OSError as e: rospy.logerr(e, name, self.rsw_id) sys.exit() pass
def __init__(self): self.rsw_id = rospy.get_param("~rsw_id") self.pub_busy = rospy.Publisher( name="/cpz7204_rsw{0}/busy".format(self.rsw_id), data_class=std_msgs.msg.Bool, latch=True, queue_size=1, ) self.pub_pEL = rospy.Publisher( name="/cpz7204_rsw{0}/p_EL".format(self.rsw_id), data_class=std_msgs.msg.Bool, latch=True, queue_size=1, ) self.pub_mEL = rospy.Publisher( name="/cpz7204_rsw{0}/m_EL".format(self.rsw_id), data_class=std_msgs.msg.Bool, latch=True, queue_size=1, ) self.sub = rospy.Subscriber( name="/cpz7204_rsw{0}/step".format(self.rsw_id), data_class=std_msgs.msg.Bool, callback=self.set_function, queue_size=1, ) try: self.mot = pyinterface.open(7204, self.rsw_id) self.mot.initialize() self.mot.set_limit_config('LOGIC', '+EL -EL', axis=1) except OSError as e: rospy.logerr(e, name, self.rsw_id) sys.exit() pass
def __init__(self): self.rate = rospy.get_param('~rate') self.rsw_id = rospy.get_param('~rsw_id') self.ch_number = rospy.get_param('~ch_number') self.node_name = 'cpz340816' self.param_buff = [] try: self.da = pyinterface.open(3408, self.rsw_id) except OSError as e: rospy.logerr("{e.strerror}. node={node_name}, rsw={rsw_id}".format( self.node_name, self.rsw_id)) sys.exit() self.topic_list = [ ('/dev/cpz340816/rsw{0}/ch{1}'.format(self.rsw_id, _)) for _ in range(1, self.ch_number + 1) ] self.sub_list = [ rospy.Subscriber(topic, Float64, self.set_param, callback_args=ch) for ch, topic in enumerate(self.topic_list, start=1) ]
#!/usr/bin/env python3 import pyinterface from datetime import datetime as dt import time import threading import rospy from necst.msg import Float64_list_msg dio = pyinterface.open(6204, 0) node_name = "encoder_dio" def check_dio(mode, pub, name): while not rospy.is_shutdown(): now = dt.utcnow() date = now.strftime("[UTC] %Y/%m/%d %H:%M:%S") #parameter = dio.get_mode().to_list() if mode == "normal": parameter = dio.get_mode().to_list() elif mode == "z_mode": parameter = dio.get_z_mode().to_list() print(date, " ", parameter) msg = Float64_list_msg() msg.data = parameter msg.from_node = name msg.timestamp = time.time() pub.publish(msg) time.sleep(1.) return
def __init__(self): ###=== Define member-variables ===### self.rsw_id = rospy.get_param('~rsw_id') self.node_name = rospy.get_param('~node_name') self.move_mode['x'] = rospy.get_param('~mode_x') self.move_mode['y'] = rospy.get_param('~mode_y') self.move_mode['z'] = rospy.get_param('~mode_z') self.move_mode['u'] = rospy.get_param('~mode_u') ###=== Create instance ===### try: self.mot = pyinterface.open(7415, self.rsw_id) except OSError as e: rospy.logerr("{e.strerror}. node={node_name}, rsw={rsw_id}".format( self.node_name, self.rsw_id)) sys.exit() ###=== Setting the board ===### self.mot.initialize() self.motion['x']['clock'] = rospy.get_param('~clock_x') self.motion['x']['acc_mode'] = rospy.get_param('~acc_mode_x') self.motion['x']['low_speed'] = rospy.get_param('~low_speed_x') self.motion['x']['speed'] = rospy.get_param('~speed_x') self.motion['x']['acc'] = rospy.get_param('~acc_x') self.motion['x']['dec'] = rospy.get_param('~dec_x') self.motion['x']['step'] = rospy.get_param('~step_x') self.motion['y']['clock'] = rospy.get_param('~clock_y') self.motion['y']['acc_mode'] = rospy.get_param('~acc_mode_y') self.motion['y']['low_speed'] = rospy.get_param('~low_speed_y') self.motion['y']['speed'] = rospy.get_param('~speed_y') self.motion['y']['acc'] = rospy.get_param('~acc_y') self.motion['y']['dec'] = rospy.get_param('~dec_y') self.motion['y']['step'] = rospy.get_param('~step_y') self.motion['z']['clock'] = rospy.get_param('~clock_z') self.motion['z']['acc_mode'] = rospy.get_param('~acc_mode_z') self.motion['z']['low_speed'] = rospy.get_param('~low_speed_z') self.motion['z']['speed'] = rospy.get_param('~speed_z') self.motion['z']['acc'] = rospy.get_param('~acc_z') self.motion['z']['dec'] = rospy.get_param('~dec_z') self.motion['z']['step'] = rospy.get_param('~step_z') self.motion['u']['clock'] = rospy.get_param('~clock_u') self.motion['u']['acc_mode'] = rospy.get_param('~acc_mode_u') self.motion['u']['low_speed'] = rospy.get_param('~low_speed_u') self.motion['u']['speed'] = rospy.get_param('~speed_u') self.motion['u']['acc'] = rospy.get_param('~acc_u') self.motion['u']['dec'] = rospy.get_param('~dec_u') self.motion['u']['step'] = rospy.get_param('~step_u') self.mot.set_motion(axis='x', mode=self.move_mode['x'], motion=self.motion) self.mot.set_motion(axis='y', mode=self.move_mode['y'], motion=self.motion) self.mot.set_motion(axis='z', mode=self.move_mode['z'], motion=self.motion) self.mot.set_motion(axis='u', mode=self.move_mode['u'], motion=self.motion) ###=== Define topic ===### topic_step_x_cmd = '/{0}_rsw{1}_x_step_cmd'.format( self.node_name, self.rsw_id) topic_step_x = '/{0}_rsw{1}_x_step'.format(self.node_name, self.rsw_id) topic_speed_x_cmd = '/{0}_rsw{1}_x_speed_cmd'.format( self.node_name, self.rsw_id) topic_speed_x = '/{0}_rsw{1}_x_speed'.format(self.node_name, self.rsw_id) topic_step_y_cmd = '/{0}_rsw{1}_y_step_cmd'.format( self.node_name, self.rsw_id) topic_step_y = '/{0}_rsw{1}_y_step'.format(self.node_name, self.rsw_id) topic_speed_y_cmd = '/{0}_rsw{1}_y_speed_cmd'.format( self.node_name, self.rsw_id) topic_speed_y = '/{0}_rsw{1}_y_speed'.format(self.node_name, self.rsw_id) topic_step_z_cmd = '/{0}_rsw{1}_z_step_cmd'.format( self.node_name, self.rsw_id) topic_step_z = '/{0}_rsw{1}_z_step'.format(self.node_name, self.rsw_id) topic_speed_z_cmd = '/{0}_rsw{1}_z_speed_cmd'.format( self.node_name, self.rsw_id) topic_speed_z = '/{0}_rsw{1}_z_speed'.format(self.node_name, self.rsw_id) topic_step_u_cmd = '/{0}_rsw{1}_u_step_cmd'.format( self.node_name, self.rsw_id) topic_step_u = '/{0}_rsw{1}_u_step'.format(self.node_name, self.rsw_id) topic_speed_u_cmd = '/{0}_rsw{1}_u_speed_cmd'.format( self.node_name, self.rsw_id) topic_speed_u = '/{0}_rsw{1}_u_speed'.format(self.node_name, self.rsw_id) topic_output_do_cmd = '/{0}_rsw{1}_do_cmd'.format( self.node_name, self.rsw_id) ###=== Define Publisher ===### self.pub_step_x = rospy.Publisher(topic_step_x, Int64, queue_size=1) self.pub_speed_x = rospy.Publisher(topic_speed_x, Int64, queue_size=1) self.pub_step_y = rospy.Publisher(topic_step_y, Int64, queue_size=1) self.pub_speed_y = rospy.Publisher(topic_speed_y, Int64, queue_size=1) self.pub_step_z = rospy.Publisher(topic_step_z, Int64, queue_size=1) self.pub_speed_z = rospy.Publisher(topic_speed_z, Int64, queue_size=1) self.pub_step_u = rospy.Publisher(topic_step_u, Int64, queue_size=1) self.pub_speed_u = rospy.Publisher(topic_speed_u, Int64, queue_size=1) ###=== Define Subscriber ===### self.sub_step_x_cmd = rospy.Subscriber(topic_step_x_cmd, Int64, self.set_step, callback_args='x') self.sub_speed_x_cmd = rospy.Subscriber(topic_speed_x_cmd, Int64, self.set_speed, callback_args='x') self.sub_step_y_cmd = rospy.Subscriber(topic_step_y_cmd, Int64, self.set_step, callback_args='y') self.sub_speed_y_cmd = rospy.Subscriber(topic_speed_y_cmd, Int64, self.set_speed, callback_args='y') self.sub_step_z_cmd = rospy.Subscriber(topic_step_z_cmd, Int64, self.set_step, callback_args='z') self.sub_speed_z_cmd = rospy.Subscriber(topic_speed_z_cmd, Int64, self.set_speed, callback_args='z') self.sub_step_u_cmd = rospy.Subscriber(topic_step_u_cmd, Int64, self.set_step, callback_args='u') self.sub_speed_u_cmd = rospy.Subscriber(topic_speed_u_cmd, Int64, self.set_speed, callback_args='u') self.sub_output_do_cmd = rospy.Subscriber(topic_output_do_cmd, Int64, self.output_do) pass
import pyinterface board_name = 6204 rsw_id = 0 print("board : CPZ-6204") print("rsw : 1") print("\n") yn = input("initialize OK??(Y/n) : ") if yn == "Y": print("initialize start") self.dio = pyinterface.open(board_name, rsw_id) self.dio.initialize() self.dio.set_mode(mode="MD0 SEL1", direction=1, equal=0, latch=0, ch=1) self.dio.set_mode(mode="MD0 SEL1", direction=1, equal=0, latch=0, ch=2) self.dio.set_z_mode(clear_condition="CLS0", latch_condition="", z_polarity=0, ch=1) self.dio.set_z_mode(clear_condition="CLS0", latch_condition="", z_polarity=0, ch=2) print("initialize end") else: print("no initialize, ok") print("see you")
def __init__(self): board_name = 2724 rsw_id = 0 #rotary switch id self.dio = pyinterface.open(board_name, rsw_id) self.dio.initialize()
def __init__(self): self.dio1 = pyinterface.open(2702, 0) pass
#!/usr/bin/env python3 import time import pyinterface board_name = 7204 rsw_id = 0 mtr = pyinterface.open(board_name, rsw_id) mtr.initialize() mtr.set_limit_config('LOGIC', '+EL -EL', axis=1) def get_pos(): print(mtr.get_counter()) status = mtr.get_status() #print(status) """ if status["busy"] == False and status["limit"]["+EL"] == 1: status["limit"]["-EL"] = 0 """ if status["busy"] == True: position = 'MOVE' elif status["limit"]["+EL"] == 0: #status == 0x0004: #OUT position = 'SMART' elif status["limit"]["-EL"] == 0: #IN position = 'NAGOYA' else: print('limit error')
def __init__(self): board_name = 2724 rsw_id = 2 self.dio = pyinterface.open(board_name, rsw_id)
def __init__(self, rsw_id, params): #(__init__における handler の組み込み場所) self.node = rclpy.create_node(node_name) self.node.declare_parameter('~node_name') self.node.declare_parameter('~rsw_id') self.node.declare_parameter('~use_axis') self.name = self.node.get_parameter('~node_name').get_parameter_value().string_value self.rsw_id = self.node.get_parameter('~rsw_id').get_parameter_value().string_value self._use_axis = self.node.get_parameter('~use_axis').get_parameter_value().string_value self.params = {} for ax in self._use_axis: params[ax] = {} #params[ax]['mode'] = rospy.get_param('~{ax}_mode'.format(**locals()), default_mode) #params[ax]['pulse_conf'] = [eval(rospy.get_param('~{ax}_pulse_conf'.format(**locals()), default_pulse_conf))] #evalとは??? self.node.declare_parameter('~{ax}_mode') self.node.declare_parameter('~{ax}_pulse_conf') self.params[ax]['mode'] = self.node.get_parameter('~{ax}_mode').get_parameter_value().string_value self.params[ax]['pulse_conf'] = self.node.get_parameter('~{ax}_pulse_conf').get_parameter_value().string_value self.mp = {} # mp['clock'] = rospy.get_param('~{ax}_clock'.format(**locals()), default_clock) # mp['acc_mode'] = rospy.get_param('~{ax}_acc_mode'.format(**locals()), default_acc_mode) # mp['low_speed'] = rospy.get_param('~{ax}_low_speed'.format(**locals()), default_low_speed) # mp['speed'] = rospy.get_param('~{ax}_speed'.format(**locals()), default_speed) # mp['acc'] = rospy.get_param('~{ax}_acc'.format(**locals()), default_acc) # mp['dec'] = rospy.get_param('~{ax}_dec'.format(**locals()), default_dec) # mp['step'] = rospy.get_param('~{ax}_step'.format(**locals()), default_step) self.node.declare_parameter('~{ax}_clock') self.node.declare_parameter('~{ax}_acc_mode') self.node.declare_parameter('~{ax}_low_speed') self.node.declare_parameter('~{ax}_speed') self.node.declare_parameter('~{ax}_acc') self.node.declare_parameter('~{ax}_dec') self.node.declare_parameter('~{ax}_step') self.mp['clock'] = self.node.get_parameter('~{ax}_clock').get_parameter_value().string_value #mpにはselfはいらない?? paramsに格納するから self.mp['acc_mode'] = self.node.get_parameter('~{ax}_acc_mode').get_parameter_value().string_value self.mp['low_speed'] = self.node.get_parameter('~{ax}_low_speed').get_parameter_value().string_value self.mp['speed'] = self.node.get_parameter('~{ax}_speed').get_parameter_value().string_value self.mp['acc'] = self.node.get_parameter('~{ax}_acc').get_parameter_value().string_value self.mp['dec'] = self.node.get_parameter('~{ax}_dec').get_parameter_value().string_value self.mp['step'] = self.node.get_parameter('~{ax}_step').get_parameter_value().string_value self.params[ax]['motion'] = mp continue #元driver self.func_queue = queue.Queue() self.pub = {} self.use_axis = ''.join([ax for ax in self.params]) self.mode = [self.params[ax]['mode'] for ax in self.use_axis] self.motion = {ax: self.params[ax]['motion'] for ax in self.use_axis} #元handler #use axis self.current_speed = {ax: 0 for ax in self.use_axis} self.current_step = {ax: 0 for ax in self.use_axis} self.current_moving = {ax: 0 for ax in self.use_axis} self.last_direction = {ax: 0 for ax in self.use_axis} self.do_status = [0, 0, 0, 0] self.move_mode = {ax: p['mode'] for ax, p in self.params.items()} self.default_speed = {ax: p['motion']['speed'] for ax, p in self.params.items()} self.low_speed = {ax: p['motion']['low_speed'] for ax, p in self.params.items()} self.mot = pyinterface.open(7415, self.rsw_id) [self.mot.set_pulse_out(ax, 'method', self.params[ax]['pulse_conf']) for ax in self.use_axis] self.mot.set_motion(self.use_axis, self.mode, self.motion) #元handler 命令値を受け取る base = '/pyinterface/pci7415/rsw{self.rsw_id}'.format(**locals()) for ax in self.use_axis: b = '{base}/{ax}/'.format(**locals()) # rospy.Subscriber(b+'step_cmd', std_msgs.msg.Int64, self.set_step, callback_args=ax) self.node.create_subscription(std_msgs.msg.Int64, b+'step_cmd', partial(self.set_step, ax)) # rospy.Subscriber(b+'speed_cmd', std_msgs.msg.Float64, self.set_speed, callback_args=ax) self.node.create_subscription(std_msgs.msg.Int64, b+'speed_cmd', partial(self.set_speed, ax)) for do_num in range(1,5): self.node.create_subscription(std_msgs.msg.Int64, '{}/output_do{}_cmd'.format(base, do_num), partial(self.set_do, do_num)) #rospy.Subscriber('{}/output_do{}_cmd'.format(base, do_num), std_msgs.msg.Int64, self.set_do, callback_args=do_num) # loop start 元driver # self.th = threading.Thread(target=self.loop) # self.th.setDaemon(True) # self.th.start() self.node.create_timer(1e-5,self.loop) return
def __init__(self, dev=0): self.dev = dev self.bname = 3177 self.driver = pyinterface.open(self.bname, self.dev)
pm_ch_list = [i for i in range(10, pm_ch + 10)] sis_pub_list = [ rospy.Publisher('{0}_rsw{1}_diff{2}'.format(node_name, rsw_id, ch), Float64, queue_size=1) for ch in sis_ch_list ] pm_pub_list = [ rospy.Publisher('{0}_rsw{1}_diff{2}'.format(node_name, rsw_id, ch), Float64, queue_size=1) for ch in pm_ch_list ] try: ad = pyinterface.open(3177, rsw_id) except OSError as e: rospy.logerr( "{e.strerror}. node={node_name}, rsw={rsw_id}".format(**locals())) sys.exit() while not rospy.is_shutdown(): for ch, pub in zip(pm_ch_list, pm_pub_list): ret = ad.input_voltage(ch, "diff") msg = Float64() msg.data = ret pub.publish(msg) for ch, pub in zip(sis_ch_list, sis_pub_list): ret = ad.input_voltage(ch, "diff")
#!/usr/bin/env python3 import pyinterface from datetime import datetime as dt import time import rospy from necst.msg import Float64_list_msg dio = pyinterface.open(2724, 2) node_name = "dome_dio" def check_dio(): while not rospy.is_shutdown(): now = dt.utcnow() date = now.strftime("[UTC] %Y/%m/%d %H:%M:%S") parameter = dio.input_point(2, 6) print(date, " ", parameter) msg = Float64_list_msg() msg.data = parameter msg.from_node = node_name msg.timestamp = time.time() pub.publish(msg) time.sleep(1.) return if __name__ == "__main__": print("check start\n") rospy.init_node(node_name)
def __init__(self, dev=0): self.dev = dev self.bname = 3408 self.driver = pyinterface.open(self.bname, self.dev) self.initialize()
#! /usr/bin/env python3 import time import pyinterface board_name = 2724 output_rsw_id = 1 input_rsw_id = 0 dio = pyinterface.open(board_name, output_rsw_id) #dio_input = pyinterface.open(self.board_name, self.input_rsw_id) """ This publish is ROS_limit_check.py def get_position(): pos = self.dio_input.input_byte("IN1_8").to_list() if pos[0] == 1 and pos[1] == 1: self.contactor_pos = "on" else: self.contactor_pos = "off" pass if pos[2] == 1 and pos[3] == 1: self.drive_pos = "on" else: self.drive_pos = "off" pass print("Current position is : ") print("drive : ",self.drive_pos, "contactor : ", self.contactor_pos) return [self.drive_pos, self.contactor_pos] """ def move_drive(position):