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

Refactoring / Rename PointWrapper to GeometryWrapper / new structure of wrapper

parent 467adff6
No related branches found
No related tags found
1 merge request!7Dev/geometry wrapper example
Hide whitespace changes
Inline Side-by-side
config/param.yaml
+ 1
− 0
View file @ 57878b82
lengthArrow_pointing: 5
angle_fore_upper_pointing: 150
angle_upperarm_pointing: 30
benchmark: false
src/homer_gesture_recognition/gesture_node.py
+ 52
− 49
View file @ 57878b82
......@@ -13,7 +13,7 @@ from tf2_ros import TransformListener, Buffer
from tf.transformations import quaternion_matrix
from openpose_extractor import OpenPoseExtractor
from homer_utils.visualization_utils import set_marker_topic_prefix, publish_marker
from homer_utils.msg_wrapper import PointWrapper
from homer_utils.geometry_wrapper import GeometryWrapper
class GestureNode:
......@@ -45,6 +45,7 @@ class GestureNode:
self.angle_upperarm_pointing = rospy.get_param(
"gesture/angle_upperarm_pointing"
)
self.benchmark = rospy.get_param("gesture/benchmark")
except:
rospy.loginfo("no yaml-file found")
self.lengthArrow_pointing = 2
......@@ -66,11 +67,11 @@ class GestureNode:
first_call = False
try:
msg = rospy.wait_for_message(
"/openpose_node/human_poses", HumanPoses, timeout=120
)
"/openpose_node/human_poses", HumanPoses, timeout=5
)
self.human_poses_callback(msg)
except:
rospy.logwarn("Timeout Openpose")
except Exception as e:
rospy.logwarn(e)
if (
self.waving_pub.get_num_connections() == 0
and self.pointing_pub.get_num_connections() == 0
......@@ -86,8 +87,8 @@ class GestureNode:
def __waving(self, forearms, upperarms, i):
if forearms[i] is not None and upperarms[i] is not None:
angle_fore = PointWrapper([0, 0, 1]).angle_between(forearms[i])
angle_upper = PointWrapper([0, 0, -1]).angle_between(upperarms[i])
angle_fore = GeometryWrapper([0, 0, 1]).angle_between(forearms[i])
angle_upper = GeometryWrapper([0, 0, -1]).angle_between(upperarms[i])
if angle_fore < 60.0 and angle_upper >= 80.0:
return True
return False
......@@ -95,8 +96,8 @@ class GestureNode:
def waving(self, ellbow, shoulders, forearms, upperarms, msg):
if self.__waving(forearms, upperarms, 0):
msg.gestures.waving.is_waving_right = True
ellbow = PointWrapper(ellbow[0])
forearm = PointWrapper(forearms[0]) + ellbow
ellbow = GeometryWrapper(ellbow[0])
forearm = GeometryWrapper(forearms[0]) + ellbow
publish_marker(
(ellbow.point, forearm.point),
......@@ -105,8 +106,8 @@ class GestureNode:
ns="waving",
lifetime=10,
)
shoulder = PointWrapper(shoulders[0])
upperarm = PointWrapper(upperarms[0]) + shoulder
shoulder = GeometryWrapper(shoulders[0])
upperarm = GeometryWrapper(upperarms[0]) + shoulder
publish_marker(
(shoulder.point, upperarm.point),
frame_id="map",
......@@ -117,8 +118,8 @@ class GestureNode:
if self.__waving(forearms, upperarms, 1):
msg.gestures.waving.is_waving_left = True
ellbow = PointWrapper(ellbow[1])
forearm = PointWrapper(forearms[1]) + ellbow
ellbow = GeometryWrapper(ellbow[1])
forearm = GeometryWrapper(forearms[1]) + ellbow
publish_marker(
(ellbow.point, forearm.point),
frame_id="map",
......@@ -126,8 +127,8 @@ class GestureNode:
ns="waving",
lifetime=10,
)
shoulder = PointWrapper(shoulders[1])
upperarm = PointWrapper(upperarms[1]) + shoulder
shoulder = GeometryWrapper(shoulders[1])
upperarm = GeometryWrapper(upperarms[1]) + shoulder
publish_marker(
(shoulder.point, upperarm.point),
frame_id="map",
......@@ -138,10 +139,11 @@ class GestureNode:
def __pointing(self, forearms, upperarms, i):
if forearms[i] is not None and upperarms[i] is not None:
angle_fore_upper = 180 - PointWrapper(forearms[i]).angle_between(
angle_fore_upper = 180 - GeometryWrapper(forearms[i]).angle_between(
upperarms[i]
)
angle_upper = PointWrapper([0, 0, -1]).angle_between(upperarms[i])
angle_upper = GeometryWrapper([0, 0, -1]).angle_between(upperarms[i])
if (
angle_fore_upper > self.angle_threshold_pointing
and angle_upper >= self.angle_upperarm_pointing
......@@ -151,8 +153,8 @@ class GestureNode:
def pointing(self, hands, forearms, upperarms, msg):
if self.__pointing(forearms, upperarms, 0):
hand_vec = PointWrapper(hands[0])
direction = PointWrapper(forearms[0]).norm()
hand_vec = GeometryWrapper(hands[0])
direction = GeometryWrapper(forearms[0]).normalized()
directional_vec_hand = hand_vec + direction
msg.gestures.pointing.ray_right.append(hand_vec.point)
msg.gestures.pointing.ray_right.append(directional_vec_hand.point)
......@@ -165,8 +167,8 @@ class GestureNode:
)
if self.__pointing(forearms, upperarms, 1):
hand_vec = PointWrapper(hands[1])
direction = PointWrapper(forearms[1]).norm()
hand_vec = GeometryWrapper(hands[1])
direction = GeometryWrapper(forearms[1]).normalized()
directional_vec_hand = hand_vec + direction
msg.gestures.pointing.ray_left.append(hand_vec.point)
......@@ -183,7 +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
......@@ -228,29 +232,26 @@ 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()
OpenPoseExtractor.transform_human_pose(person, transf)
store_msg.human_pose = person
forearms = OpenPoseExtractor.get_forearms(person, check_frame_id="map")
upperarms = OpenPoseExtractor.get_upperarms(person, check_frame_id="map")
hands = OpenPoseExtractor.get_hands(person, check_frame_id="map")
head = OpenPoseExtractor.get_head(person, check_frame_id="map")
ellbows = OpenPoseExtractor.get_ellbows(person, check_frame_id="map")
shoulders = OpenPoseExtractor.get_shoulders(person, check_frame_id="map")
openpose_extractor = OpenPoseExtractor(person,transf)
store_msg.human_pose = openpose_extractor.human_pose
if self.subs_on_waving:
self.waving(ellbows, shoulders, forearms, 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(hands, forearms, upperarms, store_msg)
self.pointing(openpose_extractor.hands, openpose_extractor.forearms, openpose_extractor.upperarms, store_msg)
# fill message only if gestures are detected
......@@ -266,27 +267,29 @@ class GestureNode:
):
waving_persons.person_arr.append(store_msg)
if len(pointing_persons.person_arr) > 0:
count_pointing = len(pointing_persons.person_arr)
count_waving = len(waving_persons.person_arr)
if count_pointing > 0:
self.pointing_pub.publish(pointing_persons)
rospy.loginfo(
"Published poses of {} pointing persons!".format(
len(pointing_persons.person_arr)
)
)
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 len(waving_persons.person_arr) > 0:
if count_waving > 0:
self.waving_pub.publish(waving_persons)
rospy.loginfo(
"Published poses of {} waving persons!".format(
len(waving_persons.person_arr)
)
)
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
rospy.loginfo(
"gesture_recognition_callback took: " + str(delta_time.to_sec()) + "s"
)
if(self.benchmark):
rospy.loginfo(
"gesture_recognition_callback took: " + str(delta_time.to_sec()) + "s"
)
# ---------------------------------------------------------------------------------------------------
......
src/homer_gesture_recognition/openpose_extractor.py
+ 72
− 149
View file @ 57878b82
......@@ -14,7 +14,29 @@ from tf2_ros import TransformListener, Buffer
from tf.transformations import quaternion_matrix
class OpenPoseExtractor:
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)
self.ellbows = self.get_ellbows(check_frame_id=self.frame_id)
self.forearms = self.get_forearms(check_frame_id=self.frame_id)
self.upperarms = self.get_upperarms(check_frame_id=self.frame_id)
self.shoulders = self.get_shoulders(check_frame_id=self.frame_id)
self.hands = self.get_shoulders(check_frame_id=self.frame_id)
self.neck = self.get_neck(check_frame_id=self.frame_id)
self.thighs = self.get_thighs(check_frame_id=self.frame_id)
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.left_spine = self.get_left_spine(check_frame_id=frame_id)
@staticmethod
def __to_np_array(pt):
return np.array([pt.point.x, pt.point.y, pt.point.z])
......@@ -25,196 +47,97 @@ class OpenPoseExtractor:
v_ = transf.dot(v_)
return v_[:3] / v_[-1]
@staticmethod
def transform_human_pose(person, transform):
for i in range(0, len(person.body_parts)):
store = OpenPoseExtractor.__get_point_for_transform(person, transform, i)
def transform_human_pose(self):
for i in range(0, len(self.human_pose.body_parts)):
store = self.__get_point_for_transform(self.human_pose, self.transform, i)
if store is not None:
p = Point(float(store[0]), float(store[1]), float(store[2]))
person.body_parts[i].point = p
person.body_parts[i].header.frame_id = "map"
@staticmethod
def get_right_upperarm(person, transform=None,check_frame_id=""):
return OpenPoseExtractor.__get_vec(person, transform, 2, 3,check_frame_id)
@staticmethod
def get_left_upperarm(person, transform=None,check_frame_id=""):
return OpenPoseExtractor.__get_vec(person, transform, 5, 6,check_frame_id)
self.human_pose.body_parts[i].point = p
self.human_pose.body_parts[i].header.frame_id = self.frame_id
@staticmethod
def get_upperarms(person, transform=None,check_frame_id=""):
def get_upperarms(self,transform=None,check_frame_id=""):
return [
OpenPoseExtractor.__get_vec(person, transform, 2, 3,check_frame_id),
OpenPoseExtractor.__get_vec(person, 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),
]
@staticmethod
def get_right_forearm(person, transform=None,check_frame_id=""):
return OpenPoseExtractor.__get_vec(person, transform, 3, 4,check_frame_id)
@staticmethod
def get_left_forearm(person, transform=None,check_frame_id=""):
return OpenPoseExtractor.__get_vec(person, transform, 6, 7,check_frame_id)
@staticmethod
def get_forearms(person, transform=None,check_frame_id=""):
def get_forearms(self,transform=None,check_frame_id=""):
return [
OpenPoseExtractor.__get_vec(person, transform, 3, 4,check_frame_id),
OpenPoseExtractor.__get_vec(person, 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),
]
@staticmethod
def get_right_shoulder(person, transform=None,check_frame_id=""):
return OpenPoseExtractor.__get_point(person, transform, 2,check_frame_id)
@staticmethod
def get_left_shoulder(person, transform=None,check_frame_id=""):
return OpenPoseExtractor.__get_point(person, transform, 5,check_frame_id)
@staticmethod
def get_shoulders(person, transform=None,check_frame_id=""):
def get_shoulders(self,transform=None,check_frame_id=""):
return [
OpenPoseExtractor.__get_point(person, transform, 2,check_frame_id),
OpenPoseExtractor.__get_point(person, 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),
]
@staticmethod
def get_right_ellbow(person, transform=None,check_frame_id=""):
return OpenPoseExtractor.__get_point(person, transform, 3,check_frame_id)
@staticmethod
def get_left_ellbow(person, transform=None,check_frame_id=""):
return OpenPoseExtractor.__get_point(person, transform, 6,check_frame_id)
@staticmethod
def get_ellbows(person, transform=None,check_frame_id=""):
def get_ellbows(self,transform=None,check_frame_id=""):
return [
OpenPoseExtractor.__get_point(person, transform, 3,check_frame_id),
OpenPoseExtractor.__get_point(person, 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),
]
@staticmethod
def get_right_hand(person, transform=None,check_frame_id=""):
return OpenPoseExtractor.__get_point(person, transform, 4,check_frame_id)
@staticmethod
def get_left_hand(person, transform=None,check_frame_id=""):
return OpenPoseExtractor.__get_point(person, transform, 7,check_frame_id)
@staticmethod
def get_hands(person, transform=None,check_frame_id=""):
def get_hands(self,transform=None,check_frame_id=""):
return [
OpenPoseExtractor.__get_point(person, transform, 4,check_frame_id),
OpenPoseExtractor.__get_point(person, 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),
]
@staticmethod
def get_right_collarbone(person, transform=None,check_frame_id=""):
return OpenPoseExtractor.__get_vec(person, transform, 1, 2,check_frame_id)
@staticmethod
def get_left_collarbone(person, transform=None,check_frame_id=""):
return OpenPoseExtractor.__get_vec(person, transform, 1, 5,check_frame_id)
@staticmethod
def get_collarbones(person, transform=None,check_frame_id=""):
def get_collarbones(self,transform=None,check_frame_id=""):
return [
OpenPoseExtractor.__get_vec(person, transform, 1, 2,check_frame_id),
OpenPoseExtractor.__get_vec(person, 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),
]
@staticmethod
def get_head(person, transform=None,check_frame_id=""):
return OpenPoseExtractor.__get_point(person, 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)
@staticmethod
def get_neck(person, transform=None,check_frame_id=""):
return OpenPoseExtractor.__get_point(person, 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)
@staticmethod
def get_left_spine(person, transform=None,check_frame_id=""):
return OpenPoseExtractor.__get_vec(person, 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)
@staticmethod
def get_right_spine(person, transform=None,check_frame_id=""):
return OpenPoseExtractor.__get_vec(person, 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)
@staticmethod
def get_right_hip(person, transform=None,check_frame_id=""):
return OpenPoseExtractor.__get_point(person, 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)
@staticmethod
def get_left_hip(person, transform=None,check_frame_id=""):
return OpenPoseExtractor.__get_point(person, transform, 11,check_frame_id)
@staticmethod
def get_hip(person, transform=None,check_frame_id=""):
return OpenPoseExtractor.__get_vec(person, transform, 8, 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)
@staticmethod
def get_right_knee(person, transform=None,check_frame_id=""):
return OpenPoseExtractor.__get_point(person, transform, 9,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_left_knee(person, transform=None,check_frame_id=""):
return OpenPoseExtractor.__get_point(person, transform, 12,check_frame_id)
@staticmethod
def get_knees(person, transform=None,check_frame_id=""):
def get_knees(self, transform=None,check_frame_id=""):
return [
OpenPoseExtractor.__get_point(person, transform, 9,check_frame_id),
OpenPoseExtractor.__get_point(person, 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),
]
@staticmethod
def get_right_foot(person, transform=None,check_frame_id=""):
return OpenPoseExtractor.__get_point(person, transform, 10,check_frame_id)
@staticmethod
def get_left_foot(person, transform=None,check_frame_id=""):
return OpenPoseExtractor.__get_point(person, transform, 13,check_frame_id)
@staticmethod
def get_foots(person, transform=None,check_frame_id=""):
def get_feet(self, transform=None,check_frame_id=""):
return [
OpenPoseExtractor.__get_point(person, transform, 10,check_frame_id),
OpenPoseExtractor.__get_point(person, 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),
]
@staticmethod
def get_right_thigh(person, transform=None,check_frame_id=""):
return OpenPoseExtractor.__get_vec(person, transform, 8, 9,check_frame_id)
@staticmethod
def get_left_thigh(person, transform=None,check_frame_id=""):
return OpenPoseExtractor.__get_vec(person, transform, 11, 12,check_frame_id)
@staticmethod
def get_thighs(person, transform=None,check_frame_id=""):
def get_thighs(self, transform=None,check_frame_id=""):
return [
OpenPoseExtractor.__get_vec(person, transform, 8, 9,check_frame_id),
OpenPoseExtractor.__get_vec(person, 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),
]
@staticmethod
def get_right_shin(person, transform=None,check_frame_id=""):
return OpenPoseExtractor.__get_vec(person, transform, 9, 10,check_frame_id)
@staticmethod
def get_left_shin(person, transform=None,check_frame_id=""):
return OpenPoseExtractor.__get_vec(person, transform, 12, 13,check_frame_id)
@staticmethod
def get_shins(person, transform=None,check_frame_id=""):
def get_shins(self, transform=None,check_frame_id=""):
return [
OpenPoseExtractor.__get_vec(person, transform, 9, 10,check_frame_id),
OpenPoseExtractor.__get_vec(person, 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:
if transform is not None:
......
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