def SetHeadFocus(self,pos,speed):
msg = Target()
msg.x = pos.x
msg.y = pos.y
msg.z = pos.z
msg.speed = speed
self.head_focus_pub.publish(msg)
def SetGazeFocus(self,pos,speed):
msg = Target()
msg.x = pos.x
msg.y = pos.y
msg.z = pos.z
msg.speed = speed
self.gaze_focus_pub.publish(msg)
def SetHeadFocus(self, pos):
# publish head focus message
msg = Target()
msg.x = pos.x
msg.y = pos.y
msg.z = pos.z
msg.speed = 5.0
self.head_focus_pub.publish(msg)
def face_toward_point(self, x, y, z, speed):
""" Turn the robot's face towards the given target point.
:param float x: metres forward
:param float y: metres to robots left
:param float z:
:param float speed:
:return: None
"""
msg = Target()
msg.x = x
msg.y = y
msg.z = z
msg.speed = speed
self.face_target_pub.publish(msg)
rospy.logdebug("published face_(x={}, y={}, z={}, speed={})".format(
x, y, z, speed))
