def callback(self, pos_x):
# get the header from the received message
h = pos_x.keypoints[0].points.header
# get the keypoint of wrist
pos_x = pos_x.keypoints[0].points.point.x
# set the starting point of the wrist
starting_point = self.config['start_keypoint']
offset = self.config['offset']
"""
s: starting point
o/2: offset/2
================================================
-1 | 0 | 1
================================================
LEFT | CENTER | RIGHT
------------------------------------------------
||<---offset--->| | ||
|| | s | ||
|| <-o/2->|<-o/2-><-o/2->|<-o/2-> ||
------------------------------------------------
"""
if pos_x < starting_point - (offset / 2):
action = -1
elif pos_x > starting_point + (offset / 2):
action = 1
else:
action = 0
act = action_msg()
act.action = action
act.header = h
self.action_human_pub.publish(act)
return action
def publish_agent_action(self, agent_act):
"""Publishes Agent's action."""
h = std_msgs.msg.Header()
h.stamp = rospy.Time.now()
act = action_msg()
act.action = agent_act
act.header = h
self.act_agent_pub.publish(act)
def callback(self, data):
h = std_msgs.msg.Header()
h.stamp = rospy.Time.now()
'''
pos_x = data.keypoints[0].points.point.y # yes it is "y" because of the setup in lab
'''
pos_x = -data.keypoints[0].points.point.y
shift = self.getShift(self.normalize(pos_x))
if shift != 0:
act = action_msg()
act.action = shift
act.header = h
self.action_human_pub.publish(act)
def callback(self, data):
h = std_msgs.msg.Header()
h.stamp = rospy.Time.now()
pos_x = data.keypoints[0].points.point.x
pos_x = pos_x + 0.28
pos_x = self.getShift(pos_x)
if pos_x != 0 and (self.prev_x is None or self.prev_x != pos_x):
act = action_msg()
act.action = pos_x
act.header = h
self.action_human_pub.publish(act)
self.prev_x = pos_x
def callback(self, data):
# h = std_msgs.msg.Header()
# h.stamp = rospy.Time.now()
h = data.keypoints[0].points.header
pos_x = data.keypoints[0].points.point.x
pos_x = pos_x + 0.26
if 0.20 < pos_x:
pos_x = 0.20
elif pos_x < -0.20:
pos_x = -0.20
pos_x = pos_x / 2
pos_x = round(pos_x * 100) / 10
act = action_msg()
act.action = pos_x
act.header = h
self.action_human_pub.publish(act)