def createPointHeadActionGoal(self):
phag = PointHeadGoal()
phag.pointing_frame = 'stereo_link'
phag.pointing_axis = Vector3(1.0, 0.0, 0.0)
phag.min_duration = rospy.Duration(0.5)
phag.target.header = Header(frame_id='oculus', stamp=rospy.Time.now())
phag.target.point = Point(1.0, 0.0, 0.0)
return phag
def createPointHeadActionGoal(self):
phag = PointHeadGoal()
phag.pointing_frame = 'stereo_link'
phag.pointing_axis = Vector3(1.0, 0.0, 0.0)
phag.min_duration = rospy.Duration(0.5)
phag.target.header = Header(frame_id='oculus', stamp=rospy.Time.now())
phag.target.point = Point(1.0, 0.0, 0.0)
return phag
def look_at(self, target_point, frame='base_footprint'):
# make tiago look at a point
point_head_goal = PointHeadGoal()
point_head_goal.target.header.frame_id = frame
point_head_goal.max_velocity = 1
point_head_goal.min_duration = rospy.Duration(2.0)
point_head_goal.target.header.stamp = rospy.Time(0)
point_head_goal.target.point = target_point
point_head_goal.pointing_frame = 'head_2_link'
point_head_goal.pointing_axis = Vector3(1, 0, 0)
self.point_head.send_goal_and_wait(point_head_goal)
rospy.loginfo(
'Sent point_head goal and waiting for robot to carry it out... ')
def tiago_point_head(x, y, wait=False):
point_head_client = actionlib.SimpleActionClient(
'/head_controller/point_head_action', PointHeadAction)
point_head_client.wait_for_server()
chair_point = Point(x, y, 1)
# create head goal
ph_goal = PointHeadGoal()
ph_goal.target.header.frame_id = 'map'
ph_goal.max_velocity = 1
ph_goal.min_duration = rospy.Duration(0.5)
ph_goal.target.header.stamp = rospy.Time(0)
ph_goal.target.point = chair_point
ph_goal.pointing_frame = 'head_2_link'
ph_goal.pointing_axis = Vector3(1, 0, 0)
point_head_client.send_goal(ph_goal)
if wait:
point_head_client.wait_for_result()
