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 loop(self): while not rospy.is_shutdown(): if self.marker is not None: self.debug.publish(self.marker) # Reset head if no face found in 1 second if rospy.Time.now() - self.lastFaceTimestamp > rospy.Duration(1): self.lastHeadTarget = None # Reset head pos = PointStamped() pos.header.stamp = rospy.Time.now() pos.header.frame_id = "base_link" if self.controllerPosition is None: pos.point.x = 1 pos.point.y = 0 else: pos.point.x = self.controllerPosition.x pos.point.y = self.controllerPosition.y pos.point.z = 1.5 goal = PointHeadGoal() goal.min_duration = rospy.Duration(0.5) goal.target = pos self.head_client.send_goal(goal) rospy.sleep(0.1)
def personCallback(self, person): # Get the distance of this person from the robot person_distance = math.sqrt(person.pose.position.x ** 2 + person.pose.position.y ** 2) person_angle = math.degrees(math.atan2(person.pose.position.y, person.pose.position.x)) # First look at the closest person if we know that the person is being # detected using the face detector if person.detection_context.pose_source == DetectionContext.POSE_FROM_FACE: self.lastFaceTimestamp = rospy.Time.now() # Point to the person if the distance between their current position # and the position we were looking at earlier has changed by more # than 2 cm goal = PointHeadGoal() goal.min_duration = rospy.Duration(0.2) goal.target = PointStamped(header=person.header, point=person.pose.position) head_distance = 999999999 if self.lastHeadTarget is not None: head_distance = math.sqrt( (person.pose.position.x - self.lastHeadTarget.point.x) ** 2 + (person.pose.position.y - self.lastHeadTarget.point.y) ** 2 + (person.pose.position.z - self.lastHeadTarget.point.z) ** 2 ) if head_distance > 0.02: self.lastHeadTarget = goal.target self.head_client.send_goal(goal) # Regardless of whether they were the original controller or not, # this person is the new controller if they are within 1m of us # if person_distance <= 1.0: if self.controllerID != person.id: rospy.loginfo("Setting controller ID to {}".format(person.id)) self.controllerID = person.id self.controllerPosition = person.pose.position # Then check to see if the person we were tracking still in view. if self.controllerID == person.id: self.controllerPosition = person.pose.position # Check to see if we are not in collision. If so, MOVE!!! if self.safeToTrack: MAX_SPEED = 0.7 cmd = Twist() if person_distance > 1: targetSpeed = 0.15 * person_distance + 0.1 cmd.linear.x = min(targetSpeed, MAX_SPEED) elif person_distance < 0.7: cmd.linear.x = -0.3 if abs(person_angle) > 5: cmd.angular.z = person_angle / 50 self.cmdvel.publish(cmd) # Otherwise, we should stop trying to follow the person elif self.controllerID is not None: rospy.loginfo("Lost controller {}".format(self.controllerID)) self.controllerID = None self.controllerPosition = None
def SetPointHead(self, point): goal = PointHeadGoal() corrected_point = self.tf.transformPoint("head_pan_link", point) rospy.loginfo(corrected_point) goal.target = corrected_point goal.min_duration = rospy.Duration(.5) goal.max_velocity = 1 self.client.send_goal(goal) self.last_trigger_time = rospy.Time.now()
def execute(self, point_value): goal_msg = PointHeadGoal() point_st_msg = PointStamped() point_st_msg.header.frame_id = "/base_link" point_st_msg.header.stamp = rospy.Time.now() point_st_msg.point.x = point_value[0] point_st_msg.point.y = point_value[1] point_st_msg.point.z = point_value[2] goal_msg.target = point_st_msg self._client.send_goal(goal_msg)
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 SetPointHead(self, point): goal = PointHeadGoal() now = rospy.Time.now() # point.header.stamp rospy.loginfo( self.tf.waitForTransform('head_pan_link', 'base_link', now, rospy.Duration(4.0))) corrected_point = self.tf.transformPoint('head_pan_link', point) corrected_point.point.z = 0.0 rospy.loginfo(corrected_point) goal.target = corrected_point goal.min_duration = rospy.Duration(.5) goal.max_velocity = 1 self.client.send_goal_and_wait(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 loop(self): while not rospy.is_shutdown(): # Reset head if no face found in 1 second if time.time() - self.lastFaceCallback > 1: self.lastHeadTarget = None # Reset head pos = PointStamped() pos.header.stamp = rospy.Time.now() pos.header.frame_id = "base_link" pos.point.x = 1 pos.point.y = 0 pos.point.z = 1.5 goal = PointHeadGoal() goal.min_duration = rospy.Duration(0.5) goal.target = pos #self.client.cancel_all_goals() self.client.send_goal(goal) rospy.sleep(0.1)
def look_at(self, frame_id, x, y, z): """Moves the head to look at a point in space. Args: frame_id: The name of the frame in which x, y, and z are specified. x: The x value of the point to look at. y: The y value of the point to look at. z: The z value of the point to look at. """ # TODO: Create goal goal = PointHeadGoal() # t = JointTrajectory() # t.header.frame_id = frame_id pointStamped = PointStamped() point = Point() point.x = x point.y = y point.z = z # point.positions.append(y) # point.positions.append(z) pointStamped.point = point # goal.target.append(point) goal.target = pointStamped goal.target.header.frame_id = frame_id # t.points.append(point) # t.joint_names.append(PAN_JOINT) # t.joint_names.append(TILT_JOINT) # TODO: Fill out the goal (we recommend setting min_duration to 1 second) goal.min_duration = rospy.Duration(secs=1, nsecs=0) # TODO: Send the goal # goal.trajectory = t self._look_client.send_goal(goal) # TODO: Wait for result self._look_client.wait_for_result()
def faceCallback(self, msg): self.lastFaceCallback = time.time() # Find closest face to look at closestFace = None closestDistance = 999999999 for face in msg.people: pos = PointStamped() pos.header = face.header pos.point = face.pos try: pos = self.listener.transformPoint("base_link", pos) except ExtrapolationException: return distance = math.sqrt(pos.point.x ** 2 + pos.point.y ** 2 + pos.point.z ** 2) if distance < closestDistance: closestFace = pos closestDistance = distance if closestFace is None: return goal = PointHeadGoal() goal.min_duration = rospy.Duration(0.0) goal.target = closestFace distance = 999999999 if self.lastHeadTarget is not None: distance = math.sqrt( (closestFace.point.x - self.lastHeadTarget.point.x) ** 2 + (closestFace.point.y - self.lastHeadTarget.point.y) ** 2 + (closestFace.point.z - self.lastHeadTarget.point.z) ** 2 ) # Prevents jitter from insignificant face movements if distance > 0.02: self.lastHeadTarget = goal.target self.client.send_goal(goal) self.client.wait_for_result()
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)
def SetPointHead(self, point): goal = PointHeadGoal() corrected_point = self.tf.transformPoint("head_pan_link", point) rospy.loginfo(corrected_point) goal.target = corrected_point goal.min_duration = rospy.Duration(.5) goal.max_velocity = 1 # send the goal to the server self.ignore_inputs = True self.client.send_goal(goal) # block and wait for the turn to be finished # doing this because there are a lot of noises when turning the head # ignore the input during that period by blocking here result = self.client.wait_for_result(rospy.Duration(0.0)) rospy.loginfo("Result:") rospy.loginfo(result) rospy.sleep(2) self.ignore_inputs = False # re-initialized history array, buffer array and counter self.angle = np.array([]) self.buf = np.zeros(BUFFER_LENGTH) self.counter = 0