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()