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