Skip to content
Snippets Groups Projects
Commit b50d7ce6 authored by Manuel Hun's avatar Manuel Hun
Browse files

format

parent 57878b82
No related branches found
No related tags found
1 merge request!7Dev/geometry wrapper example
Hide whitespace changes
Inline Side-by-side
src/homer_gesture_recognition/gesture_node.py
+ 35
− 21
View file @ b50d7ce6
......@@ -67,8 +67,8 @@ class GestureNode:
first_call = False
try:
msg = rospy.wait_for_message(
"/openpose_node/human_poses", HumanPoses, timeout=5
)
"/openpose_node/human_poses", HumanPoses, timeout=5
)
self.human_poses_callback(msg)
except Exception as e:
rospy.logwarn(e)
......@@ -139,7 +139,7 @@ class GestureNode:
def __pointing(self, forearms, upperarms, i):
if forearms[i] is not None and upperarms[i] is not None:
angle_fore_upper = 180 - GeometryWrapper(forearms[i]).angle_between(
upperarms[i]
)
......@@ -185,9 +185,9 @@ class GestureNode:
def human_poses_callback(self, msg):
starting_time = rospy.Time.now()
TARGET_FRAME = "map"
rospy.logwarn("---------GESTURE_RECOGNITION_CALLBACK--------")
# TODO: Get transform to base_link
if len(msg.human_poses) == 0:
return
......@@ -232,26 +232,37 @@ class GestureNode:
+ "'! Aborting."
)
return
transf = quaternion_matrix(q)
t = transf_msg.transform.translation
t = np.array([t.x, t.y, t.z])
transf[:-1, -1] = t
waving_persons = PersonGestures()
pointing_persons = PersonGestures()
for person in msg.human_poses:
store_msg = PersonGesture()
openpose_extractor = OpenPoseExtractor(person,transf)
openpose_extractor = OpenPoseExtractor(person, transf)
store_msg.human_pose = openpose_extractor.human_pose
if self.subs_on_waving:
self.waving(openpose_extractor.ellbows, openpose_extractor.shoulders, openpose_extractor.forearms, openpose_extractor.upperarms, store_msg)
self.waving(
openpose_extractor.ellbows,
openpose_extractor.shoulders,
openpose_extractor.forearms,
openpose_extractor.upperarms,
store_msg,
)
if self.subs_on_pointing:
self.pointing(openpose_extractor.hands, openpose_extractor.forearms, openpose_extractor.upperarms, store_msg)
self.pointing(
openpose_extractor.hands,
openpose_extractor.forearms,
openpose_extractor.upperarms,
store_msg,
)
# fill message only if gestures are detected
......@@ -272,21 +283,24 @@ class GestureNode:
if count_pointing > 0:
self.pointing_pub.publish(pointing_persons)
correct_form = "" if count_pointing == 1 else "s"
info_msg = "Published message of {} pointing person{}!".format(count_pointing,correct_form)
correct_form = "" if count_pointing == 1 else "s"
info_msg = "Published message of {} pointing person{}!".format(
count_pointing, correct_form
)
rospy.loginfo(info_msg)
if count_waving > 0:
if count_waving > 0:
self.waving_pub.publish(waving_persons)
correct_form = "" if count_waving == 1 else "s"
info_msg = "Published message of {} waving person{}!".format(count_waving,correct_form)
correct_form = "" if count_waving == 1 else "s"
info_msg = "Published message of {} waving person{}!".format(
count_waving, correct_form
)
rospy.loginfo(info_msg)
ending_time = rospy.Time.now()
delta_time = ending_time - starting_time
if(self.benchmark):
if self.benchmark:
rospy.loginfo(
"gesture_recognition_callback took: " + str(delta_time.to_sec()) + "s"
)
......
src/homer_gesture_recognition/openpose_extractor.py
+ 110
− 55
View file @ b50d7ce6
......@@ -14,13 +14,12 @@ from tf2_ros import TransformListener, Buffer
from tf.transformations import quaternion_matrix
class OpenPoseExtractor():
def __init__(self,human_pose, transform, frame_id = "map"):
class OpenPoseExtractor:
def __init__(self, human_pose, transform, frame_id="map"):
self.human_pose = human_pose
self.transform = transform
self.frame_id = frame_id
self.transform_human_pose()
self.head = self.get_head(check_frame_id=self.frame_id)
......@@ -34,7 +33,7 @@ class OpenPoseExtractor():
self.hip = self.get_hip(check_frame_id=self.frame_id)
self.right_hip = self.get_right_hip(check_frame_id=frame_id)
self.left_hip = self.get_left_hip(check_frame_id=frame_id)
self.right_spine = self.get_right_spine(check_frame_id = frame_id)
self.right_spine = self.get_right_spine(check_frame_id=frame_id)
self.left_spine = self.get_left_spine(check_frame_id=frame_id)
@staticmethod
......@@ -55,91 +54,147 @@ class OpenPoseExtractor():
self.human_pose.body_parts[i].point = p
self.human_pose.body_parts[i].header.frame_id = self.frame_id
def get_upperarms(self,transform=None,check_frame_id=""):
def get_upperarms(self, transform=None, check_frame_id=""):
return [
OpenPoseExtractor.__get_vec(self.human_pose, transform, 2, 3,check_frame_id),
OpenPoseExtractor.__get_vec(self.human_pose, transform, 5, 6,check_frame_id),
OpenPoseExtractor.__get_vec(
self.human_pose, transform, 2, 3, check_frame_id
),
OpenPoseExtractor.__get_vec(
self.human_pose, transform, 5, 6, check_frame_id
),
]
def get_forearms(self,transform=None,check_frame_id=""):
def get_forearms(self, transform=None, check_frame_id=""):
return [
OpenPoseExtractor.__get_vec(self.human_pose, transform, 3, 4,check_frame_id),
OpenPoseExtractor.__get_vec(self.human_pose, transform, 6, 7,check_frame_id),
OpenPoseExtractor.__get_vec(
self.human_pose, transform, 3, 4, check_frame_id
),
OpenPoseExtractor.__get_vec(
self.human_pose, transform, 6, 7, check_frame_id
),
]
def get_shoulders(self,transform=None,check_frame_id=""):
def get_shoulders(self, transform=None, check_frame_id=""):
return [
OpenPoseExtractor.__get_point(self.human_pose, transform, 2,check_frame_id),
OpenPoseExtractor.__get_point(self.human_pose, transform, 5,check_frame_id),
OpenPoseExtractor.__get_point(
self.human_pose, transform, 2, check_frame_id
),
OpenPoseExtractor.__get_point(
self.human_pose, transform, 5, check_frame_id
),
]
def get_ellbows(self,transform=None,check_frame_id=""):
def get_ellbows(self, transform=None, check_frame_id=""):
return [
OpenPoseExtractor.__get_point(self.human_pose, transform, 3,check_frame_id),
OpenPoseExtractor.__get_point(self.human_pose, transform, 6,check_frame_id),
OpenPoseExtractor.__get_point(
self.human_pose, transform, 3, check_frame_id
),
OpenPoseExtractor.__get_point(
self.human_pose, transform, 6, check_frame_id
),
]
def get_hands(self,transform=None,check_frame_id=""):
def get_hands(self, transform=None, check_frame_id=""):
return [
OpenPoseExtractor.__get_point(self.human_pose, transform, 4,check_frame_id),
OpenPoseExtractor.__get_point(self.human_pose, transform, 7,check_frame_id),
OpenPoseExtractor.__get_point(
self.human_pose, transform, 4, check_frame_id
),
OpenPoseExtractor.__get_point(
self.human_pose, transform, 7, check_frame_id
),
]
def get_collarbones(self,transform=None,check_frame_id=""):
def get_collarbones(self, transform=None, check_frame_id=""):
return [
OpenPoseExtractor.__get_vec(self.human_pose, transform, 1, 2,check_frame_id),
OpenPoseExtractor.__get_vec(self.human_pose, transform, 1, 5,check_frame_id),
OpenPoseExtractor.__get_vec(
self.human_pose, transform, 1, 2, check_frame_id
),
OpenPoseExtractor.__get_vec(
self.human_pose, transform, 1, 5, check_frame_id
),
]
def get_head(self,transform=None,check_frame_id=""):
return OpenPoseExtractor.__get_point(self.human_pose, transform, 0,check_frame_id)
def get_head(self, transform=None, check_frame_id=""):
return OpenPoseExtractor.__get_point(
self.human_pose, transform, 0, check_frame_id
)
def get_neck(self,transform=None,check_frame_id=""):
return OpenPoseExtractor.__get_point(self.human_pose, transform, 1,check_frame_id)
def get_neck(self, transform=None, check_frame_id=""):
return OpenPoseExtractor.__get_point(
self.human_pose, transform, 1, check_frame_id
)
def get_left_spine(self,transform=None,check_frame_id=""):
return OpenPoseExtractor.__get_vec(self.human_pose, transform, 1, 8,check_frame_id)
def get_left_spine(self, transform=None, check_frame_id=""):
return OpenPoseExtractor.__get_vec(
self.human_pose, transform, 1, 8, check_frame_id
)
def get_right_spine(self, transform=None,check_frame_id=""):
return OpenPoseExtractor.__get_vec(self.human_pose, transform, 1, 11,check_frame_id)
def get_right_spine(self, transform=None, check_frame_id=""):
return OpenPoseExtractor.__get_vec(
self.human_pose, transform, 1, 11, check_frame_id
)
def get_right_hip(self, transform=None,check_frame_id=""):
return OpenPoseExtractor.__get_point(self.human_pose, transform, 8,check_frame_id)
def get_right_hip(self, transform=None, check_frame_id=""):
return OpenPoseExtractor.__get_point(
self.human_pose, transform, 8, check_frame_id
)
def get_left_hip(self, transform=None,check_frame_id=""):
return OpenPoseExtractor.__get_point(self.human_pose, transform, 11,check_frame_id)
def get_left_hip(self, transform=None, check_frame_id=""):
return OpenPoseExtractor.__get_point(
self.human_pose, transform, 11, check_frame_id
)
def get_hip(self, transform=None,check_frame_id=""):
return OpenPoseExtractor.__get_vec(self.human_pose, transform, 8, 11,check_frame_id)
def get_hip(self, transform=None, check_frame_id=""):
return OpenPoseExtractor.__get_vec(
self.human_pose, transform, 8, 11, check_frame_id
)
def get_knees(self, transform=None,check_frame_id=""):
def get_knees(self, transform=None, check_frame_id=""):
return [
OpenPoseExtractor.__get_point(self.human_pose, transform, 9,check_frame_id),
OpenPoseExtractor.__get_point(self.human_pose, transform, 12,check_frame_id),
OpenPoseExtractor.__get_point(
self.human_pose, transform, 9, check_frame_id
),
OpenPoseExtractor.__get_point(
self.human_pose, transform, 12, check_frame_id
),
]
def get_feet(self, transform=None,check_frame_id=""):
def get_feet(self, transform=None, check_frame_id=""):
return [
OpenPoseExtractor.__get_point(self.human_pose, transform, 10,check_frame_id),
OpenPoseExtractor.__get_point(self.human_pose, transform, 13,check_frame_id),
OpenPoseExtractor.__get_point(
self.human_pose, transform, 10, check_frame_id
),
OpenPoseExtractor.__get_point(
self.human_pose, transform, 13, check_frame_id
),
]
def get_thighs(self, transform=None,check_frame_id=""):
def get_thighs(self, transform=None, check_frame_id=""):
return [
OpenPoseExtractor.__get_vec(self.human_pose, transform, 8, 9,check_frame_id),
OpenPoseExtractor.__get_vec(self.human_pose, transform, 11, 12,check_frame_id),
OpenPoseExtractor.__get_vec(
self.human_pose, transform, 8, 9, check_frame_id
),
OpenPoseExtractor.__get_vec(
self.human_pose, transform, 11, 12, check_frame_id
),
]
def get_shins(self, transform=None,check_frame_id=""):
def get_shins(self, transform=None, check_frame_id=""):
return [
OpenPoseExtractor.__get_vec(self.human_pose, transform, 9, 10,check_frame_id),
OpenPoseExtractor.__get_vec(self.human_pose, transform, 12, 13,check_frame_id),
OpenPoseExtractor.__get_vec(
self.human_pose, transform, 9, 10, check_frame_id
),
OpenPoseExtractor.__get_vec(
self.human_pose, transform, 12, 13, check_frame_id
),
]
@staticmethod
def __get_vec(person, transform, i, j,check_frame_id):
if person.body_parts[i].header.frame_id == check_frame_id and \
person.body_parts[j].header.frame_id == check_frame_id:
def __get_vec(person, transform, i, j, check_frame_id):
if (
person.body_parts[i].header.frame_id == check_frame_id
and person.body_parts[j].header.frame_id == check_frame_id
):
if transform is not None:
if (
person.keypoints_image[i].confidence < 0.6
......@@ -165,10 +220,10 @@ class OpenPoseExtractor():
if np.sum(np.isnan(a)) > 0 or np.sum(np.isnan(b)) > 0:
return None
return b - a
return None
return None
@staticmethod
def __get_point(person, transform, i,check_frame_id=""):
def __get_point(person, transform, i, check_frame_id=""):
if person.body_parts[i].header.frame_id == check_frame_id:
if transform is not None:
if person.keypoints_image[i].confidence < 0.6:
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment