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):
