def fusion(self):
#self._maskjoints[41] = 0
# designate means
_sum = 2.0
self._covs = [
float(self._temp[0]) / _sum,
float(self._temp[1]) / _sum,
float(self._temp[2]) / _sum,
float(self._temp[3]) / _sum
]
# take means
self._jointmeans = list(self._covs[0]*self.joint_idle+ self._covs[1]*self.joint_reflex +\
self._covs[2]*self.joint_slave + self._covs[3]*self.joint_auto)
# get mask
for i in range(len(self._jointmeans)):
self._jointmeans[i] = self._maskjoints[i] * self._jointmeans[i]
self._payload = list(np.add(self._default, self._jointmeans))
# compare if the value exceed maxmin
self.compare_bounds(self._payload)
# make message
tform.make_message(self._pub_msg, 0, 'fusion', 0, self._payload)
# update state message
self._state_msg.data = self._covs
return None
def start(self):
loop_rate = rospy.Rate(20)
while not rospy.is_shutdown():
tform.make_message(self.msg, 1, 'gait', 1, self._payload)
self.pub.publish(self.msg)
print self._payload
loop_rate.sleep()
def start(self):
rospy.loginfo("DEMO")
loop_rate = rospy.Rate(50)
while not rospy.is_shutdown():
"make message"
tform.make_message(self._pub_msg, 0, 'demo', 0, self._payload)
"publish message"
self.pub.publish(self._pub_msg)
loop_rate.sleep()
def start(self):
loop_rate = rospy.Rate(_RATE)
while not rospy.is_shutdown():
self.react()
self._payload_neck = [0, 0, 0, self._neck, 0]
tform.make_message(self._pub_msg, 4, 'neck', 2, self._payload_neck)
self.pub.publish(self._pub_msg)
#print(self._payload_neck)
print self._direction
loop_rate.sleep()
def start(self):
rospy.loginfo("In attesa")
while not rospy.is_shutdown():
# make message
self._payload = [self.neck_pos[0], self.neck_pos[1], self.neck_pos[2], self.neck_pos[3] + self.eye_width, self.neck_pos[4] + self.eye_height]
tform.make_message(self._pub_msg, 0, 'neck', 0, self._payload)
# publish
self.pub.publish(self._pub_msg)
#self.pub.publish(message)
self.loop_rate.sleep()
def start(self):
rospy.loginfo("In attesa")
while not rospy.is_shutdown():
# make message
self._payload = [200, 200, self.speed_r, self.speed_l, 600]
tform.make_message(self._pub_msg, 4, 'wheel', 5, self._payload)
# publish
self.pub.publish(self._pub_msg)
#self.pub.publish(message)
self.loop_rate.sleep()
def speech_cb(self, msg):
if msg.data == 'hello':
self._content = 'こんにちは'
filename = 'hello'
elif msg.data == 'agree':
self._content = 'そうですね。'
filename = 'agree'
elif msg.data == 'goodbye':
self._content = 'さようならー'
filename = 'goodbye'
elif msg.data == 'selfintro1':
self._content = 'はじめまして。ー。ー。なまえは、いぶきです。いきる、っていういみがあります。ー。としわ、じっさい。'
filename = 'selfintro1'
elif msg.data == 'selfintro2':
self._content = 'すきなことは、いろんなばしょに、いくこと。ー。ー。ー。やまや、うみ、たくさんのどうぶつや、むしたち、。ー。いろんなものをみて、。ー。さわって、。ー。かんじる。ー。いろんなばしょで、いっしょに、あそんでくれるともだちがほしいです。'
filename = 'selfintro2'
elif msg.data == 'selfintro3':
self._content = 'どうぞよろしくおねがいします。'
filename = 'selfintro3'
elif msg.data == 'thankyou':
self._content = 'ありがとうございます'
filename = 'thankyou'
# because we only want contents be carried out once, so do commu tts here
WAVNAME = self.getTTS() # get wave name
self._data = self.load_presets(filename)
print(self._data[4])
run_event = threading.Event()
run_event.set()
# send_w = threading.Thread(target = self.tcplink, args = (self.sock, self.addr, WAVNAME, run_eventw))
move_t = threading.Thread(target=self.player_i,
args=(WAVNAME, run_event))
# send_w.start()
move_t.start()
time.sleep(0.19) # threshold
# make the message, and send the message to slave operation
for index in range(0, len(self._data[4])):
self.make_carlos_message(self._data, index)
tform.make_message(self._pub_msg, 3, 'slave', 3, self._payload)
print(self._payload)
self.pub.publish(self._pub_msg)
time.sleep(INTERVAL)
def start(self):
rospy.loginfo("silva_REFLEX")
loop_rate = rospy.Rate(_RATE)
while not rospy.is_shutdown():
# set message from callback
self.set_msg_from_pos()
# make the message
tform.make_message(self._pub_msg, 2, 'reflex', 0, self._payload)
# publish the message
self.pub.publish(self._pub_msg)
loop_rate.sleep()
def start(self):
rospy.loginfo("silva_AUTO")
# publish the /joint_local/auto message
loop_rate = rospy.Rate(_RATE)
while not rospy.is_shutdown():
# set message from callback
self.set_msg_from_pos()
# make the message
tform.make_message(self._pub_msg, 4, 'auto', 0, self._payload)
# publish the message
self.pub.publish(self._pub_msg)
# print type(self._payload)
loop_rate.sleep()
def start(self):
rospy.loginfo("silva_SLAVE")
loop_rate = rospy.Rate(_RATE)
while not rospy.is_shutdown():
# set message from callback
self.set_msg_from_pos()
# make the message
tform.make_message(self._pub_msg, 3, 'slave', 0, self._payload)
# publish the message
self.pub.publish(self._pub_msg)
# debug
# print type(self._rel)
loop_rate.sleep()
def start(self):
rospy.loginfo("HANDIDLE")
loop_rate = rospy.Rate(_RATE)
# # signal flag for running threads
# run_event = threading.Event()
# run_event.set()
#
# # thread that changes blinks
# move_blink = threading.Thread(target = self.blink, args = \
# (2, run_event))
#
# move_blink.start()
while not rospy.is_shutdown():
self._count += 1
# print self._count
# do rythem
self.rythem_d(self._count)
#print self._physical
#print self._eyelid
# make message
self.make_message()
self.make_message_a()
# tform method use hand r, I want to see difference
tform.make_message(self._pub_msg_r, 1, 'handr', 9, self._payload)
tform.make_message(self._pub_msg_ra, 4, 'handr', 9,
self._payload_r)
# publish
self.pub.publish(self._pub_msg)
self.pub_a.publish(self._pub_msg_a)
self.pub.publish(self._pub_msg_r)
self.pub_a.publish(self._pub_msg_a)
if self._count > 15 * _RATE: #every 10 seconds
self._count = 0
self._bias = int(3 * _RATE * rd.rand()) #bias within 2 seconds
loop_rate.sleep()
def start(self):
rospy.loginfo("Joystick")
loop_rate = rospy.Rate(_RATE)
while not rospy.is_shutdown():
# make payload
self._payload[0] = 0
self._payload[1] = 0
self._payload[2] = int(self._speed_right * 350) # right
self._payload[3] = int(self._speed_left * 350) # left
if self._break == 1:
self._payload[4] = 0
else:
self._payload[4] = 100 # break
print("left", self._payload)
tform.make_message(self._pub_msg, 3, 'wheel', 4, self._payload)
self.pub.publish(self._pub_msg)
loop_rate.sleep()
def start(self):
loop_rate = rospy.Rate(_RATE)
while not rospy.is_shutdown():
self._payload_neck = self._neck
self._payload_headc = self._headc
self._payload_hip = self._hip
tform.make_message(self._pub_neck, 4, 'neck', 2,
self._payload_neck)
self.pub.publish(self._pub_neck)
tform.make_message(self._pub_headc, 4, 'headc', 2,
self._payload_headc)
self.pub.publish(self._pub_headc)
tform.make_message(self._pub_hip, 4, 'hip', 2, self._payload_hip)
self.pub.publish(self._pub_hip)
#print(self._payload_neck)
print self._payload_hip
loop_rate.sleep()
def pub_pos(self, msg):
self.counter += 1
if self.counter % 10 != 0:
return
print(rospy.Time.now())
count = msg.scan_time / msg.time_increment
upper_angle1 = 218.5
upper_angle2 = 180
upper_angle3 = 270
upper_angle4 = 135
lower_angle1 = 180
lower_angle2 = 141
lower_angle3 = 225
lower_angle4 = 90
s_range = 0.40
l_range = 1.5
l_range2 = 0.80
thr1 = 0.20
thr2 = 0.20
thr3 = 0.10
thr4 = 0.35
thr5 = 0.35
ibegin = 0
iend = 0
num = int(0)
total = float(0)
total_angle = int(0)
near_point = int(0)
minimum_list_x = []
minimum_list_y = []
pair_minimum_x = []
pair_minimum_y = []
for i in range(int(count)):
if s_range < msg.ranges[i] < l_range2:
angle = msg.angle_increment * i * 180.0 / 3.14159265358979
if (lower_angle1 < angle < upper_angle1) or (
lower_angle2 < angle <= upper_angle2) or (
lower_angle3 < angle <= upper_angle3) or (
lower_angle4 < angle <= upper_angle4):
if (i > 1) and (i < int(count) - 1):
if (msg.ranges[i - 1] - msg.ranges[i] > thr1):
ibegin = i
iend = i
self.pos_r1 = msg.ranges[i]
self.pos_theta1 = angle
self.pos_x1 = -1 * self.pos_r1 * math.cos(
self.pos_theta1 * 3.14159265358979 / 180.0)
self.pos_y1 = -1 * self.pos_r1 * math.sin(
self.pos_theta1 * 3.14159265358979 / 180.0)
if msg.ranges[i + 1] - msg.ranges[i] > thr2:
iend = i
self.pos_r2 = msg.ranges[i]
self.pos_theta2 = angle
self.pos_x2 = -1 * self.pos_r2 * math.cos(
self.pos_theta2 * 3.14159265358979 / 180.0)
self.pos_y2 = -1 * self.pos_r2 * math.sin(
self.pos_theta2 * 3.14159265358979 / 180.0)
dist = ((self.pos_x2 - self.pos_x1)**2 +
(self.pos_y2 - self.pos_y1)**2)**(1.0 /
2.0)
if (thr3 < dist < thr4):
minimum_list_x.append(
(self.pos_x2 + self.pos_x1) / 2.0)
minimum_list_y.append(
(self.pos_y2 + self.pos_y1) / 2.0)
elif l_range2 < msg.ranges[i] < l_range:
angle = msg.angle_increment * i * 180.0 / 3.14159265358979
if (lower_angle1 < angle < upper_angle1) or (
lower_angle2 < angle <= upper_angle2):
if (i > 1) and (i < int(count) - 1):
if (msg.ranges[i - 1] - msg.ranges[i] > thr1):
ibegin2 = i
iend2 = i
self.pos_r1 = msg.ranges[i]
self.pos_theta1 = angle
self.pos_x1 = -1 * self.pos_r1 * math.cos(
self.pos_theta1 * 3.14159265358979 / 180.0)
self.pos_y1 = -1 * self.pos_r1 * math.sin(
self.pos_theta1 * 3.14159265358979 / 180.0)
if msg.ranges[i + 1] - msg.ranges[i] > thr2:
iend2 = i
self.pos_r2 = msg.ranges[i]
self.pos_theta2 = angle
self.pos_x2 = -1 * self.pos_r2 * math.cos(
self.pos_theta2 * 3.14159265358979 / 180.0)
self.pos_y2 = -1 * self.pos_r2 * math.sin(
self.pos_theta2 * 3.14159265358979 / 180.0)
dist = ((self.pos_x2 - self.pos_x1)**2 +
(self.pos_y2 - self.pos_y1)**2)**(1.0 /
2.0)
if (thr3 < dist < thr4):
minimum_list_x.append(
(self.pos_x2 + self.pos_x1) / 2.0)
minimum_list_y.append(
(self.pos_y2 + self.pos_y1) / 2.0)
if len(minimum_list_x) >= 2:
for i in range(len(minimum_list_x) - 1):
if ((minimum_list_x[i] - minimum_list_x[i + 1])**2 +
(minimum_list_y[i] - minimum_list_y[i + 1])**2)**(
1.0 / 2.0) < thr5:
pair_minimum_x.append(
(minimum_list_x[i] + minimum_list_x[i + 1]) / 2.0)
pair_minimum_y.append(
(minimum_list_y[i] + minimum_list_y[i + 1]) / 2.0)
minimum_pos = PoseStamped()
minimum_pos.header.stamp = rospy.Time.now()
minimum_pos.header.frame_id = 'laser'
minimum_pos.pose.position.x = pair_minimum_x[0]
minimum_pos.pose.position.y = pair_minimum_y[0]
if pair_minimum_y[0] < 0:
minimum_pos.pose.position.y = pair_minimum_y[0] + 0.4
else:
minimum_pos.pose.position.y = pair_minimum_y[0] - 0.4
minimum_pos.pose.position.z = 0.0
q = quaternion_from_euler(0, 0, 0)
minimum_pos.pose.orientation = Quaternion(*q)
self.minimum_pub.publish(minimum_pos)
angle_temp = math.atan2(pair_minimum_y[0],
pair_minimum_x[0])
if angle_temp > 0:
angle_send = int(
((-angle_temp + math.pi) / math.pi) * 180)
else:
angle_send = int(
((-angle_temp - math.pi) / math.pi) * 180)
self.direction_pub.publish(angle_send)
self._payload_headc[2] = -self.eye_gaze
self._payload_headc[3] = -self.eye_gaze
tform.make_message(self._pub_headc, 4, 'headc', 2,
self._payload_headc)
self.pub.publish(self._pub_headc)
# print 'x:',minimum_list_x
# print 'y:',minimum_list_y
elif len(minimum_list_x) > 0:
minimum_pos = PoseStamped()
minimum_pos.header.stamp = rospy.Time.now()
minimum_pos.header.frame_id = 'laser'
minimum_pos.pose.position.x = minimum_list_x[0]
minimum_pos.pose.position.y = minimum_list_y[0]
# if minimum_list_y[0] < 0:
# minimum_pos.pose.position.y = minimum_list_y[0] + 0.4
# else:
# minimum_pos.pose.position.y = minimum_list_y[0] - 0.4
minimum_pos.pose.position.z = 0.0
q = quaternion_from_euler(0, 0, 0)
minimum_pos.pose.orientation = Quaternion(*q)
self.minimum_pub.publish(minimum_pos)
angle_temp = math.atan2(minimum_list_y[0], minimum_list_x[0])
angle_send = 180
if angle_temp > 0:
angle_send = int(((-angle_temp + math.pi) / math.pi) * 180)
else:
angle_send = int(((-angle_temp - math.pi) / math.pi) * 180)
self.direction_pub.publish(angle_send)
self._headc[2] = -self.eye_gaze
self._headc[3] = -self.eye_gaze
tform.make_message(self._pub_headc, 4, 'headc', 2,
self._payload_headc)
self.pub.publish(self._pub_headc)
else:
minimum_pos = PoseStamped()
minimum_pos.header.stamp = rospy.Time.now()
minimum_pos.header.frame_id = 'laser'
minimum_pos.pose.position.x = 0
minimum_pos.pose.position.y = 0
minimum_pos.pose.position.z = 0.0
q = quaternion_from_euler(0, 0, 0)
minimum_pos.pose.orientation = Quaternion(*q)
self.minimum_pub.publish(minimum_pos)
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]
for idx in range(0, 5):
payload_p.append(int(p_pos[idx * 5:(idx + 1) * 5]))
for idx in range(0, 5):
payload_c.append(int(p_cur[idx * 5:(idx + 1) * 5]) * 2 - 10000)
# make message
tform.make_message(pub_msg_p, 2, dev_name, 3, payload_p)
tform.make_message(pub_msg_c, 2, dev_name, 4, payload_c)
#print payload_p
#print payload_c
print position
# publish
pub_p.publish(pub_msg_p)
pub_c.publish(pub_msg_c)
rate.sleep()
# If the value cannot be obtain, try again until get the value
__version = "1.1.0"
(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
#---------------------------------------------------------------------------
try:
"UDP send launch"
motorsock.sendto(otm, (ip[dev_name], port[dev_name]))
except socket.error as error:
if error.errno == errno.ENETUNREACH:
rospy.WARN("connection to mbed lost.")
else:
raise
if dev_name == 'wheel':
tform.make_message(pub_msg, 2, dev_name, 2, joint._payload_w)
pub.publish(pub_msg)
#---------------------------------------------------------------------------
rate.sleep()
if dev_name == 'wheel':
move_t.join()
__version = "1.1.0"
"generate one time message"
otm = tform.merge(joint._payload)
#---------------------------------------------------------------------------
try:
"UDP send launch"
motorsock.sendto(otm, (ip[dev_name], port[dev_name]))
except socket.error as error:
if error.errno == errno.ENETUNREACH:
rospy.WARN("connection to mbed lost.")
else:
raise
if dev_name == 'wheel':
pub_msg = make_message(2, dev_name, 2, joint._payload_w)
pub.publish(pub_msg)
# make some message
tform.make_message(joint._pub_msg_p, 2, dev_name, 3, joint._payload_p)
tform.make_message(joint._pub_msg_c, 2, dev_name, 4, joint._payload_c)
pub_p.publish(joint._pub_msg_p)
pub_c.publish(joint._pub_msg_c)
#---------------------------------------------------------------------------
rate.sleep()
move_t.join()
__version = "1.0.1"
