def __init__(self):
# var
self._direction = 180
self._neck = [0.0, 0.0, 0.0, 0.0, 0.0]
self._headc = [0.0, 0.0, 0.0, 0.0, 0.0]
self._hip = [0.0, 0.0, 0.0, 0.0, 0.0]
self._payload_headc = [0.0, 0.0, 0.0, 0.0, 0.0]
self._payload_neck = [0, 0, 0.0, 0.0, 0.0, 0.0]
self._payload_hip = [0.0, 0.0, 0.0, 0.0, 0.0]
self.human = 0
# messages
self._pub_neck = Evans()
self._pub_hip = Evans()
self._pub_headc = Evans()
# publisher
self.pub = rospy.Publisher('/silva/auto_local/ch1',
Evans,
queue_size=10)
# subscriber
self.sub = rospy.Subscriber('/sound_direction', Int32, self.direct_cb)
self.human_sub = rospy.Subscriber('/isHuman', Int32, self.updown)
def __init__(self, physical=0, mental=0):
self._physical = physical
self._mental = mental
self._tiredness = 0
self._eyelid = [0, 0]
self._count = 0
self._payload = [0,0,0,0,0]
self._bias = 0
self._flag = 'noblink'
# message publish
self._pub_msg = Evans()
self._pub_msg_a = Evans()
self._pub_msg_b = Evans()
# physical gives a bias on the rythem decider
self._decider = physical + mental
# mental decides the period and the amplitude
# publishers
self.pub = rospy.Publisher('/silva/idle_local/intention', Evans, queue_size=10)
self.pub_a = rospy.Publisher('/silva/auto_local/ch0', Evans, queue_size=10)
self.pub_b = rospy.Publisher('/silva/reflex_local/ch0', Evans, queue_size=10)
# subscribers
self.sub_auto = rospy.Subscriber('/silva/auto_local/intention', Evans, self.state_cb)
self.sub_joy = rospy.Subscriber('/joy', Joy, self.joy_cb)
def __init__(self):
# number of driveunits
# global variables
self._rel = []
self._bias = []
self._payload = []
self._default = []
# messages
self._default_rec = Evans()
self._pub_msg = Evans()
self._intention = Evans()
# publishers
self.pub = rospy.Publisher('/silva/joint_local/idle',
Evans,
queue_size=10)
# subscribers
self.sub_int = rospy.Subscriber('/silva/idle_local/intention', Evans,
self.intention_cb)
self.sub_default = rospy.Subscriber('/silva/joint_local/default',
Evans, self.default_cb)
tform.set_zeros(self._default)
tform.set_zeros(self._rel)
def __init__(self):
self.loop_rate = rospy.Rate(10)
self.pos_r1 = 0
self.pos_theta1 = 0
self.pos_x1 = 0
self.pos_y1 = 0
self.pos_r2 = 0
self.pos_theta2 = 0
self.pos_x2 = 0
self.pos_y2 = 0
self.counter = 0
self.eye_gaze = 0.0
self._headc = [0.0, 0.0, 0.0, 0.0, 0.0]
self._payload_headc = [0.0, 0.0, 0.0, 0.0, 0.0]
self._pub_headc = Evans()
self.minimum_pub = rospy.Publisher('/move_base_simple/goal',
PoseStamped,
queue_size=1,
latch=True)
self.direction_pub = rospy.Publisher('/sound_direction',
Int32,
queue_size=1)
self.pub = rospy.Publisher('/silva/auto_local/ch2',
Evans,
queue_size=10)
rospy.Subscriber('/scan', LaserScan, self.pub_pos)
rospy.Subscriber('/silva/auto_local/ch1', Evans, self.gaze)
def __init__(self):
self._neck = [0, 0, 0, 0, 0]
self._neck_slave = [0, 0, 0]
self._count = 0
self._ct = [1, 1, 1, 1, 1]
self._timebias = 0
self._payload = [0, 0, 0, 0, 0]
# random variables
self._length = [0, 0, 0, 0, 0]
self._startpos = [0.0, 0.0, 0.0, 0.0, 0.0]
self._starttime = [0.0, 0.0, 0.0, 0.0, 0.0]
self._sinscope = [0.0, 0.0, 0.0, 0.0, 0.0]
self._nextpos = [0.0, 0.0, 0.0, 0.0, 0.0]
self._gototime = [0.0, 0.0, 0.0, 0.0, 0.0]
self._sinscopeB = [0.0, 0.0, 0.0, 0.0, 0.0]
# values
self._rel = [0.0, 0.0, 0.0, 0.0, 0.0]
#message publish
self._pub_msg = Evans()
# publishers
self.pub = rospy.Publisher('/silva/idle_local/intention',
Evans,
queue_size=10)
# subscribers
self.sub_joy = rospy.Subscriber('/joy', Joy, self.joy_cb)
def __init__(self):
# env var
self._pub_msg = Evans()
# payload
self._payload = [0, 0, 0, 0, 0]
self.last_payload = [0, 0, 0, 0, 0]
self.last_ax = 0.0
self.key = None
self.string = ''
self.jointnow1, self.jointnow2, self.jointnow3 = 0, 0, 0
self.loop_rate = rospy.Rate(20)
self.Kp = 4
self.Kd = 3.5
self.Ki = 0.3
self.integral = 0
#Publisher
self.pub = rospy.Publisher('/silva/auto_local/ch3',
Evans,
queue_size=25)
self.deviation = [0.0, 0.0, 0.0, 0.0, 0.0]
#Subscriber
rospy.Subscriber('/imu/data_raw1', Imu, self.imu_callback)
def __init__(self):
_filename = sys.argv[1]
#_filename = 'lookaround'
# open the .csv
rospack = rospkg.RosPack()
csvpath = rospack.get_path(
'silva_beta') + '/src/csv/' + _filename + '.csv'
df = pd.read_csv(csvpath, delimiter=',')
# csv
self._df = df
self._timelist = []
self._motionlist = []
self._lastmotion = []
self._payload = []
self._payload_float = []
self._margin_to_target = []
self._time_interval = 0.0
#initilization
tform.set_zeros(self._payload)
tform.set_zeros(self._payload_float)
tform.set_zeros(self._lastmotion)
# messages
self._pub_msg = Evans()
# publishers
self.pub = rospy.Publisher('/silva/slave_local/hsm',
Evans,
queue_size=10)
def __init__(self):
self._gene = [0, 0, 0, 0, 0]
self._count = 0
self._ct = [1, 1, 1, 1, 1]
self._timebias = 0
self._payload = [0, 0, 0, 0, 0]
# random variables
self._length = [0, 0, 0, 0, 0]
self._startpos = [0.0, 0.0, 0.0, 0.0, 0.0]
self._starttime = [0.0, 0.0, 0.0, 0.0, 0.0]
self._sinscope = [0.0, 0.0, 0.0, 0.0, 0.0]
self._nextpos = [0.0, 0.0, 0.0, 0.0, 0.0]
self._gototime = [0.0, 0.0, 0.0, 0.0, 0.0]
self._sinscopeB = [0.0, 0.0, 0.0, 0.0, 0.0]
# values
self._rel = [0.0, 0.0, 0.0, 0.0, 0.0]
#message publish
self._pub_msg = Evans()
# publishers
self.pub = rospy.Publisher('/silva/idle_local/ch0',
Evans,
queue_size=10)
self.pub_a = rospy.Publisher('/silva/auto_local/ch0',
Evans,
queue_size=10)
self.pub_s = rospy.Publisher('/silva/slave_local/intention',
Evans,
queue_size=10)
def __init__(self, name='void', dev='mbed', withfb=False, silence=False):
self._name = name
self._dev = dev
self._withfb = withfb
self._silence = silence
self._msgid = 0
self._seq = 0
self._payload = []
self._payload_p = []
self._payload_c = []
self._payload_w = [0, 0, 0, 0, 0]
self._position = ''
self._current = ''
self._pub_msg = Evans()
self._pub_msg_p = Evans()
self._pub_msg_c = Evans()
self.tmp = [0, 0, 0, 0, 0]
def __init__(self):
# global variables
self._rel = []
self._bias = [[], [], [], [], [], []]
self._payload = []
self._default = []
self._gain = 1.0
# messages
self._default_rec = Evans()
self._pub_msg = Evans()
self._intention = Evans()
# publishers
self.pub = rospy.Publisher('/silva/joint_local/slave',
Evans,
queue_size=10)
# subscribers
self.sub_int = rospy.Subscriber('/silva/slave_local/intention', Evans,
self.intention_cb)
self.sub_opt = rospy.Subscriber('/silva/slave_local/operation', Evans,
self.operation_cb)
self.sub_dec = rospy.Subscriber('/silva/slave_local/decision', Evans,
self.decision_cb)
self.sub_wlk = rospy.Subscriber('/silva/slave_local/walking', Evans,
self.walking_cb)
self.sub_hsm = rospy.Subscriber('/silva/slave_local/hsm', Evans,
self.hsm_cb)
self.sub_dem = rospy.Subscriber('/silva/slave_local/demo', Evans,
self.demo_cb)
self.sub_default = rospy.Subscriber('/silva/joint_local/default',
Evans, self.default_cb)
self.sub_gain = rospy.Subscriber('/joy', Joy, self.joy_cb)
tform.set_zeros(self._default)
tform.set_zeros(self._rel)
for i in range(len(self._bias)):
tform.set_zeros(self._bias[i])
def __init__(self, dev_name):
# robot name #
self._name = dev_name
self._params_length = 0 # the number of DOFs
self._covs = [0, 0, 1, 0, 0] # cov matrix, 1 is slave ctrl
self._params_serial = [] # serial number of each DoF
self._params_name = [] # name of each DoF
self._params_value = [] # value of each DoF
self._default = [] # default value
self._payload = [] # payload value
self._dict_name_value = {} # default name-value dict
self._dict_serial_name = {} # default serial-name dict
self._dict_serial_value = {} # defaule serial-value dict
self._default_msg = Evans() # default msg used to pub
self._pub_msg = Evans() # memeory msg used to pub
def __init__(self):
# global variables
self._rel = []
self._payload = []
self._default = []
self._content = 'ss'
self._data = {}
tform.set_zeros(self._default)
tform.set_zeros(self._rel, 5)
tform.set_zeros(self._rel)
tform.set_zeros(self._payload)
self.sock = ''
self.addr = ''
self.sockmot = ''
self.addrmot = ''
self.moveaxis = ''
self._data = [[] for row in range(_driveunits)]
# set zeros
print self._payload
self._tmp = np.array(self._payload)
# messages
self._default_rec = Evans()
self._pub_msg = Evans()
# publishers
self.pub = rospy.Publisher('/silva/slave_local/decision',
Evans,
queue_size=10)
# subscribers
self.sub_default = rospy.Subscriber('/silva/joint_local/default',
Evans, self.default_cb)
self.sub_speech = rospy.Subscriber('/silva/speech_global/jp', String,
self.speech_cb)
def __init__(self):
# global variables
self._rel = []
self._bias = [[], [], []]
self._payload = []
self._default = []
self._neck = [0.0, 0.0, 0.0, 0.0, 0.0]
self._armr = [0.0, 0.0, 0.0, 0.0, 0.0]
self._arml = [0.0, 0.0, 0.0, 0.0, 0.0]
self._hip = [0.0, 0.0, 0.0, 0.0, 0.0]
# messages
self._default_rec = Evans()
self._pub_msg = Evans()
self._intention = Evans()
# publishers
self.pub = rospy.Publisher('/silva/joint_local/reflex',
Evans,
queue_size=10)
# subscribers
self.sub_int = rospy.Subscriber('/silva/reflex_local/feedback', Evans,
self.intention_cb)
self.sub_default = rospy.Subscriber('/silva/joint_local/default',
Evans, self.default_cb)
# 2 channels
self.sub_ch0 = rospy.Subscriber('/silva/reflex_local/ch0', Evans,
self.ch0_cb)
self.sub_ch1 = rospy.Subscriber('/silva/reflex_local/ch1', Evans,
self.ch1_cb)
self.sub_ch2 = rospy.Subscriber('/silva/reflex_local/ch2', Evans,
self.ch2_cb)
tform.set_zeros(self._default)
tform.set_zeros(self._rel)
for i in range(len(self._bias)):
tform.set_zeros(self._bias[i]) # set zeros of bias
def __init__(self):
# global variables
self._rel = [] # relative value
self._bias = [[], [], [], []] # bias value for subposition
self._payload = [] # final payload
self._default = [] # origin default
# messages
self._default_rec = Evans()
self._pub_msg = Evans()
self._intention = Evans()
# publishers
self.pub = rospy.Publisher('/silva/joint_local/auto',
Evans,
queue_size=10)
# subscribers
self.sub_ch0 = rospy.Subscriber(
'/silva/auto_local/ch0', Evans,
self.ch0_cb) # ch0 for blink message (headl)
self.sub_ch1 = rospy.Subscriber(
'/silva/auto_local/ch1', Evans,
self.ch1_cb) # ch1 for realsense (neck)
self.sub_ch2 = rospy.Subscriber(
'/silva/auto_local/ch2', Evans,
self.ch2_cb) # ch2 for respeaker (neck)
self.sub_ch3 = rospy.Subscriber('/silva/auto_local/ch3', Evans,
self.ch3_cb) # ch3 for rplidar (hip?)
self.sub_default = rospy.Subscriber('/silva/joint_local/default',
Evans, self.default_cb)
#init the default message
tform.set_zeros(self._default)
tform.set_zeros(self._rel) # set zeros of rel
for i in range(len(self._bias)):
tform.set_zeros(self._bias[i]) # set zeros of bias
def __init__(self):
self._payload = []
# zero
tform.set_zeros(self._payload)
# message
self.msg = Evans()
# publisher
self.pub = rospy.Publisher('/silva/reflex_local/ch2',
Evans,
queue_size=10)
# subs
self.sub = rospy.Subscriber('/silva/reflex_local/feedback', Evans,
self.callback)
def make_message(seq, name, msgid, payload):
# make message
msg = Evans()
msg.header.stamp = rospy.Time.now()
msg.seq = seq
msg.name = name
msg.msgid = msgid
msg.payload = payload
return msg
def __init__(self):
self._forward = 0.0
self._steering = 0.0
self._speed_right = 0.0
self._speed_left = 0.0
self._break = 1
self._payload = []
tform.set_zeros(self._payload, 5)
# message
self._pub_msg = Evans()
# publishers
self.pub = rospy.Publisher('/silva/slave_local/walking',
Evans,
queue_size=10)
self.sub = rospy.Subscriber('/joy', Joy, self.joy_cb)
def __init__(self):
# env var
self._pub_msg = Evans()
# payload
self._payload = [0, 0, 0, 0, 0]
self.key = None
self.string = ''
self.eye_width = 0
self.eye_height = 0
self.neck_pos = [0,0,0,0,0]#shoulder*2,r,y,p
self.loop_rate = rospy.Rate(50)
#Publisher
self.pub = rospy.Publisher('/silva/auto_local/ch3', Evans, queue_size =25)
rospy.Subscriber('/face_pos',Int32MultiArray, self.callback)
rospy.Subscriber('/silva/reflex_local/feedback',Evans,self.position)
def __init__(self):
# env var
self._pub_msg = Evans()
self.cmd_vel = Twist()
# payload
self._payload = [0, 0, 0, 0, 0]
self.maxspeed = 2600
self.key = None
self.string = ''
self.jointnow1, self.jointnow2, self.jointnow3 = 0, 0, 0
self.loop_rate = rospy.Rate(50)
#Publisher
# self.pub = rospy.Publisher('/silva/auto_local/ch2', Evans, queue_size =25)
self.vel_pub = rospy.Publisher(
'/my_robo/diff_drive_controller/cmd_vel', Twist, queue_size=32)
self.movingstate = 0
self.laststate = 0
self.deviation = [0.0, 0.0, 0.0, 0.0, 0.0]
self.direction = 0.0
self.flag_back = 0
self.stop_flag = 1
self.rot_l = 0.0
self.rot_r = 0.0
self.speed_l = 0.0
self.speed_r = 0.0
self.goal_x = 0.0
self.goal_y = 0.0
self.a1 = _CENTER
self.a2 = _CENTER
self.a3 = _CENTER
self.delta_x = 0.1
self.delta_y = 0.1
#Subscriber
# rospy.Subscriber('/lrf_pub',Int32, self.callback2) #changed by ise
# rospy.Subscriber('/my_robo/diff_drive_controller/cmd_vel',Twist,self.callback3)
rospy.Subscriber('/move_base_simple/goal', PoseStamped, self.set_goal)
# rospy.Subscriber('/angle_info_arml',String,self.forward_or_back)
# rospy.Subscriber('/scan',LaserScan, self.callback2) #changed by ise
rospy.Subscriber('/scan', LaserScan, self.callback3)
rospy.Subscriber('/joy', Joy, self.joy_cb)
def __init__(self, _df):
# csv
self._df =df
self._timelist = []
self._motionlist = []
self._lastmotion = []
self._payload = []
self._payload_float = []
self._margin_to_target = []
self._time_interval = 0.0
#initilization
tform.set_zeros(self._payload)
tform.set_zeros(self._payload_float)
tform.set_zeros(self._lastmotion)
# messages
self._pub_msg = Evans()
# publishers
self.pub = rospy.Publisher('/silva/slave_local/hsm', Evans, queue_size = 10)
def __init__(self):
# env var
self._pub_msg = Evans()
# payload
self._payload = [0, 0, 0, 0, 0]
self.maxspeed = 2600
self.key = None
self.string = ''
self.jointnow1, self.jointnow2, self.jointnow3 = 0, 0, 0
self.loop_rate = rospy.Rate(20)
#Publisher
self.pub = rospy.Publisher('/silva/auto_local/ch2',
Evans,
queue_size=25)
self.speed_pub = rospy.Publisher('/speed', Twist, queue_size=25)
self.speed = Twist()
self.movingstate = 0
self.laststate = 0
self.deviation = [0.0, 0.0, 0.0, 0.0, 0.0]
self.direction = 0.0
self.flag_back = 0
self.stop_flag = 1
self.rot_l = 0.0
self.rot_r = 0.0
self.speed_l = 0.0
self.speed_r = 0.0
self.human_flag = 1
#Subscriber
# rospy.Subscriber('/lrf_pub',Int32, self.callback2) #changed by ise
rospy.Subscriber('/my_robo/diff_drive_controller/cmd_vel', Twist,
self.callback3)
rospy.Subscriber('/isHuman', Int32, self.human_detect)
# rospy.Subscriber('/angle_info_arml',String,self.forward_or_back)
rospy.Subscriber('/scan', LaserScan, self.callback2) #changed by ise
cur_client = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
rospy.init_node('Act_' + dev_name, anonymous=True)
rate = rospy.Rate(_RATE)
#---------------------------------------------------------------------------
"define this joint"
joint = Joint(dev_name)
#---------------------------------------------------------------------------
"read Evans message and write into the joint"
sub = rospy.Subscriber('/silva/joint_local/fusion', Evans, callback, joint)
pub = rospy.Publisher('/silva/reflex_local/feedback', Evans, queue_size=10)
pub_msg = Evans()
"thread"
if dev_name == 'wheel':
run_event = threading.Event()
run_event.set()
move_t = threading.Thread(target = mbed_cb, args = \
(rtclient,cur_client, 'h', run_event, joint))
move_t.start()
#---------------------------------------------------------------------------
while not rospy.is_shutdown():
"generate one time message"
otm = tform.merge(joint._payload)
print otm
def __init__(self, dev_name):
# robot name #
self._name = dev_name # device name.
self._params_length = 0 # the number of DOFs. Read from param [driveunits].
self._params_serial = [] # serial number of each DoF
self._params_name = [] # name of each DoF
self._params_value = [] # value of each DoF
self._default = [] # default value
self._jointmeans = [] # joint means
self._initlist = [] # init lists
self._payload = [] # final payload
self._dict_name_value = {} # default name-value dict
self._dict_serial_name = {} # default serial-name dict
self._dict_serial_value = {} # defaule serial-value dict
# --- messages --- #
self._default_msg = Evans() # default msg used to announce.
self._state_msg = Float32MultiArray() # state msg to debug GUI.
self._pub_msg = Evans(
) # memeory msg used to publish to JOINT INTERFACES.
# channel masks to all of the joints
self._maskjoints = tform.set_ones()
# math operators #
self._covs = [0, 0, 1, 0] # cov matrix of 4 blocks, 1 is slave ctrl
self._temp = [0, 0, 1, 0]
self._state_msg.data = [0.0, 0.0, 1.0,
0.0] # a copy of the covs, inform GUI
# maximum and minimum
self._joint_max = [] # joint maximum
self._joint_min = [] # joint minimum
### global attributes ###
self.joint_idle = Evans() # idle motions, mostly from generator
self.joint_reflex = Evans(
) # reflex motions, from whole-body sensor feedbacks
self.joint_slave = Evans(
) # slave motions, from multiple human operations
self.joint_auto = Evans() # auto motions, from multiple proceptions
self.joint_balance = Evans() # balance, from imu feedbacks
# publishers
self.pub = rospy.Publisher('/silva/joint_local/fusion',
Evans,
queue_size=20)
self.pub_d = rospy.Publisher('/silva/joint_local/default',
Evans,
queue_size=10)
self.pub_c = rospy.Publisher('/silva/states',
Float32MultiArray,
queue_size=20)
# subscribers
self.sub_idle = rospy.Subscriber('/silva/joint_local/idle', Evans,
self.joint_idle_cb)
self.sub_reflex = rospy.Subscriber('/silva/joint_local/reflex', Evans,
self.joint_reflex_cb)
self.sub_slave = rospy.Subscriber('/silva/joint_local/slave', Evans,
self.joint_slave_cb)
self.sub_auto = rospy.Subscriber('/silva/joint_local/auto', Evans,
self.joint_auto_cb)
self.sub_auto = rospy.Subscriber('/silva/balance', Evans,
self.joint_balance_cb)
## midi subscriber nano Kontrol2
self.sub_joy = rospy.Subscriber('/joy', Joy, self.joy_cb)
self.sub_mask = rospy.Subscriber('/silva/joint_local/mask',
Float32MultiArray, self.mask_cb)
def __init__(self):
self._payload = [0]*50
self._pub_msg = Evans()
self.pub = rospy.Publisher('/silva/slave_local/demo', Evans, queue_size=3)
rospy.Subscriber('keys', String, self.keys_cb)
def __init__(self, dev_name):
# robot name #
self._name = dev_name
self._params_length = 0 # the number of DOFs
self._params_serial = [] # serial number of each DoF
self._params_name = [] # name of each DoF
self._params_value = [] # value of each DoF
self._default = [] # default value
self._jointmeans = [] # joint means
self._initlist = [] # init lists
self._payload = [] # final payload
self._dict_name_value = {} # default name-value dict
self._dict_serial_name = {} # default serial-name dict
self._dict_serial_value = {} # defaule serial-value dict
self._default_msg = Evans() # default msg used to
self._state_msg = Float32MultiArray() # state msg to GUI
self._pub_msg = Evans() # memeory msg used to pub
# math operators #
self._covs = [0, 0, 1, 0] # cov matrix, 1 is slave ctrl
self._temp = [0, 0, 0, 0]
# maximum and minimum
self._joint_max = [] # joint maximum
self._joint_min = [] # joint minimum
### global attributes ###
self.joint_idle = Evans() # idle motions, mostly from generator
self.joint_reflex = Evans(
) # reflex motions, from whole-body sensor feedbacks
self.joint_slave = Evans(
) # slave motions, from multiple human operations
self.joint_auto = Evans() # auto motions, from multiple proceptions
self.joint_balance = Evans() # balance, from imu feedbacks
# publishers
# publishers are devided into 2 parts,
# /fusion used to publish fusion to joint interface,
# /default used to publish the default value to 5 blocks.
self.pub = rospy.Publisher('/silva/joint_local/fusion',
Evans,
queue_size=10)
self.pub_d = rospy.Publisher('/silva/joint_local/default',
Evans,
queue_size=10)
self.pub_c = rospy.Publisher('/silva/states',
Float32MultiArray,
queue_size=10)
# subscribers
self.sub_idle = rospy.Subscriber('/silva/joint_local/idle', Evans,
self.joint_idle_cb)
self.sub_reflex = rospy.Subscriber('/silva/joint_local/reflex', Evans,
self.joint_reflex_cb)
self.sub_slave = rospy.Subscriber('/silva/joint_local/slave', Evans,
self.joint_slave_cb)
self.sub_auto = rospy.Subscriber('/silva/joint_local/auto', Evans,
self.joint_auto_cb)
self.sub_auto = rospy.Subscriber('/silva/balance', Evans,
self.joint_balance_cb)
## midi subscriber nano Kontrol2
self.sub_joy = rospy.Subscriber('/joy', Joy, self.joy_cb)
if __name__ == "__main__":
### use UDP to read FB_thread except wheel ###
# vars
position = ''
# socket initialization
fb_client = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
# node init
rospy.init_node("FB_" + dev_name, anonymous=True)
rate = rospy.Rate(_RATE)
pub_p = rospy.Publisher('/silva/reflex_local/ch0', Evans, queue_size=10)
pub_c = rospy.Publisher('/silva/reflex_local/ch1', Evans, queue_size=10)
pub_msg_p = Evans()
pub_msg_c = Evans()
while not rospy.is_shutdown():
# GET FB_thread by asking
fb_client.sendto('h', (ip[dev_name], portf))
position, addr = fb_client.recvfrom(4096)
payload_p = []
payload_c = []
# split the position and currents
pac = position.split(',')
p_pos = pac[0]
p_cur = pac[1]