def look_at(self, x, y, z, frame): #The goal message will be sending goal = PointHeadGoal() point = PointStamped() #The target point, expressed in the requested frame point.header.frame_id = frame point.point.x = x point.point.y = y point.point.z = z goal.target = point #We are pointing the high-def camera frame to default X-axis goal.pointing_frame = "base_link" goal.pointing_axis.x = 1 goal.pointing_axis.y = 0 goal.pointing_axis.z = 0 #Take at least 0.5 seconds to get there goal.min_duration = rospy.Duration(0.5) #Go no faster than 1 rad/s goal.max_velocity = 1.0 #send the goal self.client.send_goal_and_wait(goal, rospy.Duration(3))
def point_head_to(self, position, frame, pointing_frame=None, pointing_axis=None): """Point the head to the (x,y,z) tuple specified by position in the passed frame. Default pointing frame is head_tilt_link with axis [1,0,0]. """ goal = PointHeadGoal() if pointing_frame is None: pointing_frame = "head_tilt_link" if pointing_axis is None: pointing_axis = [1, 0, 0] goal.target.header.frame_id = frame goal.target.point.x = position[0] goal.target.point.y = position[1] goal.target.point.z = position[2] goal.pointing_frame = pointing_frame goal.pointing_axis.x = pointing_axis[0] goal.pointing_axis.y = pointing_axis[1] goal.pointing_axis.z = pointing_axis[2] goal.min_duration = rospy.Duration(self.time_to_reach) self.head_pointer_client.send_goal_and_wait(goal, rospy.Duration(self.time_to_reach))
def execute(self, userdata): rospy.loginfo('Executing Move head') frame_id = self.s_frame_id if self.s_frame_id else userdata.target_frame_id point_to_look = self.s_point_to_look if self.s_point_to_look else userdata.target_point look_goal = PointHeadGoal() look_goal.target.header.frame_id = frame_id look_goal.target.point.x = point_to_look[0] look_goal.target.point.y = point_to_look[1] look_goal.target.point.z = point_to_look[2] look_goal.pointing_frame = POINTING_FRAME if POINTING_FRAME == 'head_mount_xtion_rgb_optical_frame': look_goal.pointing_axis.x = 0.0 look_goal.pointing_axis.y = 0.0 look_goal.pointing_axis.z = 1.0 elif POINTING_FRAME == 'stereo_link': look_goal.pointing_axis.x = 1.0 look_goal.pointing_axis.y = 0.0 look_goal.pointing_axis.z = 0.0 look_goal.min_duration.secs = 1.0 rospy.loginfo('Sending this goal: ' + str(look_goal)) userdata.point_goal = look_goal return 'succeeded'
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, x, y, z, frame, duration=1.0): goal = PointHeadGoal() goal.target.header.stamp = rospy.Time.now() goal.target.header.frame_id = frame goal.target.point.x = x goal.target.point.y = y goal.target.point.z = z goal.min_duration = rospy.Duration(duration) goal.pointing_frame = "head_tilt_link" self.client.send_goal(goal) self.client.wait_for_result()
def lookAt(self, point): # Create tf transformer for z point transformation to keep TIAGo's head on the same level tfBuffer = tf2_ros.Buffer() tf = tf2_ros.TransformListener(tfBuffer) # Wait for the server to come up rospy.loginfo('Waiting for the point head server to come up') self.point_head_client.wait_for_server(rospy.Duration(10.0)) # Create the goal print('Looking at: ', point) 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.point.x = point[0] ph_goal.target.point.y = point[1] ph_goal.pointing_frame = 'head_2_link' ph_goal.pointing_axis.x = 1 ph_goal.pointing_axis.y = 0 ph_goal.pointing_axis.z = 0 ps = PointStamped() ps.header.stamp = rospy.Time(0) ps.header.frame_id = 'head_2_link' transform_ok = False while not transform_ok and not rospy.is_shutdown(): try: transform = tfBuffer.lookup_transform('base_link', 'head_2_link', rospy.Time(0)) get_z_ps = do_transform_point(ps, transform) transform_ok = True # This usually happens only on startup except tf2_ros.ExtrapolationException as e: rospy.sleep(1.0 / 4) ps.header.stamp = rospy.Time(0) rospy.logwarn( "Exception on transforming point... trying again \n(" + str(e) + ") at time " + str(ps.header.stamp)) except tf2_ros.LookupException: pass except tf2_ros.ConnectivityException: pass ph_goal.target.point.z = get_z_ps.point.z print(get_z_ps.point.z) # Send the goal rospy.loginfo("Sending the goal...") self.point_head_client.send_goal(ph_goal) rospy.loginfo("Goal sent!!") rospy.sleep(3)
def lookAt(self, point): rospy.loginfo("sending look command %s" % point.point) goal = PointHeadGoal() goal.target = point goal.pointing_axis.x = 1.0 goal.pointing_frame = "kinect2_link" goal.min_duration.secs = 2.0 goal.max_velocity = 1.0 action_goal = PointHeadActionGoal() action_goal.goal = goal self.pub.publish(action_goal)
def look_at(self, x, y, z): phg = PointHeadGoal() phg.target.header.frame_id = '/stereo_optical_frame' phg.target.point.x = x phg.target.point.y = y phg.target.point.z = z phg.pointing_axis.z = 1.0 phg.pointing_frame = phg.target.header.frame_id phg.min_duration = rospy.Duration(1.0) phg.max_velocity = 1.0 self.point_head_ac.send_goal_and_wait(phg)
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 look_at(self, frame_id, x, y, z): goal = PointHeadGoal() goal.target.header.stamp = rospy.Time.now() goal.target.header.frame_id = frame_id goal.target.point.x = x goal.target.point.y = y goal.target.point.z = z goal.pointing_frame = "pointing_frame" goal.pointing_axis.x = 1 goal.pointing_axis.y = 0 goal.pointing_axis.z = 0 # send goal self.point_head_client.send_goal(goal)
def look_at(parent_frame, x, y, z): goal = PointHeadGoal() #the point to be looking at is expressed in the "base_link" frame point = PointStamped() point.header.frame_id = parent_frame point.header.stamp = rospy.Time.now() point.point.x = x point.point.y = y point.point.z = z goal.target = point #we want the X axis of the camera frame to be pointing at the target goal.pointing_frame = "high_def_frame" goal.pointing_axis.x = 1 goal.pointing_axis.y = 0 goal.pointing_axis.z = 0 goal.min_duration = rospy.Duration(0.5) client.send_goal(goal)
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()
def execute(self, userdata): if userdata.point_to_look == None and self.point_to_look == None: rospy.logerr("No point to look at! Error at SM Look_to_point") return 'aborted' print str(self.direction) self.point_to_look = self.point_to_look if self.point_to_look else userdata.point_to_look phg = PointHeadGoal() phg.min_duration = rospy.Duration(self.min_duration) # adapt for as far as the detection is?? phg.target.header.frame_id = self.frame_id if self.frame_id else self.point_to_look.header.frame_id phg.target.header.stamp = rospy.Time.now() phg.target.point.x = self.point_to_look.point.x phg.target.point.y = self.point_to_look.point.y phg.target.point.z = self.point_to_look.point.z phg.pointing_axis.x = 1.0 phg.pointing_frame = 'head_mount_xtion_rgb_frame' if self.direction!="" : print "I HAVE A NEW ORDER" phg.target.point.z=UP if self.direction =="left" : phg.target.point.y = 1 elif self.direction=="right": phg.target.point.y = -1 elif self.direction=="front": phg.target.point.y = 0 elif self.direction=="up": phg.target.point.y = 0 phg.target.point.z=1.6 userdata.point_head_goal = phg print phg return 'succeeded'
def look_at(self, frame_id, x, y, z): ''' Description: sends the ROS node a goal position to move the head to face Input: self <Object>, frame_id <Int>, x <Int>, y <Int>, z <Int> Return: None ''' goal = PointHeadGoal() goal.target.header.stamp = rospy.Time.now() goal.target.header.frame_id = frame_id goal.target.point.x = x goal.target.point.y = y goal.target.point.z = z goal.pointing_frame = "pointing_frame" goal.pointing_axis.x = 1 goal.pointing_axis.y = 0 goal.pointing_axis.z = 0 # send goal self.point_head_client.send_goal(goal)
def execute(self, userdata): if userdata.point_to_look == None and self.point_to_look == None: rospy.logerr("No point to look at! Error at SM Look_to_point") return 'aborted' print str(self.direction) self.point_to_look = self.point_to_look if self.point_to_look else userdata.point_to_look phg = PointHeadGoal() phg.min_duration = rospy.Duration( self.min_duration) # adapt for as far as the detection is?? phg.target.header.frame_id = self.frame_id if self.frame_id else self.point_to_look.header.frame_id phg.target.header.stamp = rospy.Time.now() phg.target.point.x = self.point_to_look.point.x phg.target.point.y = self.point_to_look.point.y phg.target.point.z = self.point_to_look.point.z phg.pointing_axis.x = 1.0 phg.pointing_frame = 'head_mount_xtion_rgb_frame' if self.direction != "": print "I HAVE A NEW ORDER" phg.target.point.z = UP if self.direction == "left": phg.target.point.y = 1 elif self.direction == "right": phg.target.point.y = -1 elif self.direction == "front": phg.target.point.y = 0 elif self.direction == "up": phg.target.point.y = 0 phg.target.point.z = 1.6 userdata.point_head_goal = phg print phg return 'succeeded'
def run_demo(self): try: if not self.published: point = PointStamped() point.header.frame_id = 'base_footprint' point.point.x = 1.44 point.point.y = 0.79 point.point.z = 0.85 goal = PointHeadGoal() goal.pointing_frame = "high_def_frame" goal.pointing_axis.x = 1 goal.pointing_axis.y = 0 goal.pointing_axis.z = 0 goal.target = point self.ph.send_goal_and_wait(goal) print goal rospy.sleep(1.) self.published = True rospy.sleep(0.1) except KeyboardInterrupt: print('interrupted!') sys.exit(0)
marker.action = Marker.ADD marker.pose.position.x = 1 marker.pose.position.y = 0 marker.pose.position.z = 0 marker.scale.x = 0.1 marker.scale.y = 0.1 marker.scale.z = 0.1 marker.color.a = 1.0 marker.color.r = 1.0 marker.color.g = 0.0 marker.color.b = 0.0 rate = rospy.Rate(10) while not rospy.is_shutdown(): gaze_point = PointHeadGoal() gaze_point.target.header.frame_id = 'glass' gaze_point.pointing_frame = 'high_def_optical_frame' gaze_point.min_duration = rospy.Duration(0.01) gaze_point.max_velocity = 1.0 gaze_point.target.header.stamp = rospy.Time.now() gaze_point.target.point.x = 1 gaze_point.target.point.y = 0 gaze_point.target.point.z = 0 head_client.send_goal(gaze_point) pub.publish(marker) rate.sleep()