def humans_to_msg(humans):
persons = Persons()
for human in humans:
person = Person()
for k in human.body_parts:
body_part = human.body_parts[k]
body_part_msg = BodyPartElm()
body_part_msg.part_id = body_part.part_idx
body_part_msg.x = body_part.x
body_part_msg.y = body_part.y
body_part_msg.confidence = body_part.score
person.body_part.append(body_part_msg)
persons.persons.append(person)
return persons
def set_body_parts(self, data):
body_parts = BodyPartElm()
body_parts.part_id = -1
body_parts.x = -1
body_parts.y = -1
body_parts.confidence = -1
np_body_part_id = np.full(19, -1)
np_body_parts = np.full(19, body_parts)
for i in range(len(data)):
np_body_part_id[data[i].part_id] = data[i].part_id
np_body_parts[data[i].part_id] = data[i]
# print "----"
# print data[i].part_id
return np_body_part_id, np_body_parts
def humans_to_msg(humans):
persons = Persons()
for human in humans:
person = Person()
for k in human.body_parts:
body_part = human.body_parts[k]
body_part_msg = BodyPartElm()
body_part_msg.part_id = body_part.part_idx
body_part_msg.x = body_part.x
body_part_msg.y = body_part.y
body_part_msg.confidence = body_part.score
person.body_part.append(body_part_msg)
persons.persons.append(person)
return persons
LWrist = 7
RHip = 8
RKnee = 9
RAnkle = 10
LHip = 11
LKnee = 12
LAnkle = 13
REye = 14
LEye = 15
REar = 16
LEar = 17
Background = 18
number_of_person = 0
detected_person = Person()
body_part = BodyPartElm()
minimun_person = 0
maximun_person = 0
person_pose = [0, [0, 0, 0], [0, 0, 0], [0, 0, 0]]
# first is person number : second is wrist, third is elbow, shoulder
sentence = ""
def raise_hand(wrist_y, elbow_y, person_number):
if wrist_y < elbow_y:
rospy.loginfo(str(wrist_y) + " and " + str(elbow_y))
rospy.loginfo(str(person_number) + " is raising his hand")
return True
def action(human, n):
# print(human.person_id)
# print('-----------------')
# print(type(human.body_part))
# print(len(human.body_part))
# print('-----------------')
# print(type(human.body_part[1]))
# l=np.array(human.body_part)
# print('shape', l.shape)
RShoulder = BodyPartElm()
RElbow = BodyPartElm()
RWrist = BodyPartElm()
LShoulder = BodyPartElm()
LElbow = BodyPartElm()
LWrist = BodyPartElm()
for part in human.body_part:
# body right
if part.part_id == 2:
RShoulder.part_id = part.part_id
RShoulder.x = part.x
RShoulder.y = part.y
RShoulder.confidence = part.confidence
if part.part_id == 3:
RElbow.part_id = part.part_id
RElbow.x = part.x
RElbow.y = part.y
RElbow.confidence = part.confidence
if part.part_id == 4:
RWrist.part_id = part.part_id
RWrist.x = part.x
RWrist.y = part.y
RWrist.confidence = part.confidence
# body left
if part.part_id == 5:
LShoulder.part_id = part.part_id
LShoulder.x = part.x
LShoulder.y = part.y
LShoulder.confidence = part.confidence
if part.part_id == 6:
LElbow.part_id = part.part_id
LElbow.x = part.x
LElbow.y = part.y
LElbow.confidence = part.confidence
if part.part_id == 7:
LWrist.part_id = part.part_id
LWrist.x = part.x
LWrist.y = part.y
LWrist.confidence = part.confidence
print('RShoulder', RShoulder)
print('RElbow', RElbow)
print('RWrist', RWrist)
threshold = 0.5
#calculation
# length of the big arm
R_big_arm_len = np.sqrt((RShoulder.x - RElbow.x)**2 +
(RShoulder.y - RElbow.y)**2)
# R_small_arm_len = np.sqrt((RWrist.x-RElbow.x)**2 + (RWrist.y-RElbow.y)**2)
R_small_arm_ylen = abs(RWrist.y - RElbow.y)
L_big_arm_len = np.sqrt((LShoulder.x - LElbow.x)**2 +
(LShoulder.y - LElbow.y)**2)
# L_small_arm_len = np.sqrt((LWrist.x-LElbow.x)**2 + (LWrist.y-LElbow.y)**2)
L_small_arm_ylen = abs(LWrist.y - LElbow.y)
# Action recognize
if RWrist.y < RShoulder.y and (RWrist.confidence > threshold
and RShoulder.confidence > threshold):
action_str = 'come on'
elif R_small_arm_ylen < R_big_arm_len / 3 and (
RWrist.confidence > threshold and RShoulder.confidence > threshold
and RElbow.confidence > threshold):
action_str = 'sit down'
elif LWrist.y < LShoulder.y and (LWrist.confidence > threshold
and LShoulder.confidence > threshold):
action_str = 'stand up'
elif L_small_arm_ylen < L_big_arm_len / 3 and (
LWrist.confidence > threshold and LShoulder.confidence > threshold
and LElbow.confidence > threshold):
action_str = 'stop'
else:
action_str = 'do nothing'
print(action_str)
# last = ''
# if last == action_str and action_str != 'do nothing':
# n += 1
# else:
# n = 0
# last = action_str
#
# if n > 3:
# action_publisher(action_str)
action_publisher(action_str)
