def callback(point):
goal = PointHeadGoal()
goal.target = point
goal.max_velocity = 0.2
goal.pointing_axis.z = 1.0
goal.pointing_frame = point.header.frame_id
client.send_goal(goal)
def lookAtLocation(userdata):
head_goal = PointHeadGoal()
head_goal.target.header.frame_id = "base_link"
# Iterate until we find the object we are searching for
index_of_object = 0
while index_of_object < 10:
if userdata.object_found.object_list[index_of_object].name == userdata.object_to_search_for:
break;
index_of_object += 1
head_goal.target.point.x = userdata.object_found.object_list[index_of_object].pose.position.x
head_goal.target.point.y = userdata.object_found.object_list[index_of_object].pose.position.y
head_goal.target.point.z = userdata.object_found.object_list[index_of_object].pose.position.z
head_goal.pointing_frame = "stereo_link"
head_goal.pointing_axis.x = 1.0
head_goal.pointing_axis.y = 0.0
head_goal.pointing_axis.z = 0.0
head_goal.max_velocity = 1.5
head_goal.min_duration.secs = 1.5
client = SimpleActionClient("/head_traj_controller/point_head_action", PointHeadAction)
client.wait_for_server(rospy.Duration(0.5))
client.send_goal(head_goal)
return succeeded
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 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 move_head(self,
x,
y,
z,
min_dur=0.0,
max_velocity=1.0,
frame_id='base_link',
timeout=5.0):
point = PointStamped()
point.header.frame_id = frame_id
point.header.stamp = rospy.Time.now()
point.point.x, point.point.y, point.point.z = x, y, z
goal = PointHeadGoal()
goal.pointing_frame = 'head_plate_frame'
goal.max_velocity = max_velocity
goal.min_duration = rospy.Duration.from_sec(min_dur)
goal.target = point
self.head_client.send_goal(goal)
self.head_client.wait_for_result(
timeout=rospy.Duration.from_sec(timeout))
if not self.head_client.get_state() == GoalStatus.SUCCEEDED:
rospy.logerr('can not move head to:\n %s' % (goal))
return False
return True
def showStartMessage(userdata):
head_goal = PointHeadGoal()
head_goal.target.header.frame_id = "base_link"
rospy.loginfo("SPECIAL CODE: write e to enable the face tracking or d to disable it in the x coordinate")
x = raw_input("X: ")
if x == "e":
return "enableFaceTracking"
elif x == "d":
return "disableFaceTracking"
y = raw_input("Y: ")
z = raw_input("Z: ")
head_goal.target.point.x = float(x)
head_goal.target.point.y = float(y)
head_goal.target.point.z = float(z)
head_goal.pointing_frame = "stereo_link"
head_goal.pointing_axis.x = 1.0
head_goal.pointing_axis.y = 0.0
head_goal.pointing_axis.z = 0.0
head_goal.max_velocity = 1.5
head_goal.min_duration.secs = 1.5
client = actionlib.SimpleActionClient("/head_traj_controller/point_head_action", PointHeadAction)
client.wait_for_server(rospy.Duration(0.3))
client.send_goal(head_goal)
return succeeded
def look_at_cb(self, where):
""" where: PoseStamped."""
goal = PointHeadGoal()
goal.target = where
goal.pointing_frame = "high_def_frame"
goal.min_duration = rospy.Duration(0.5)
goal.max_velocity = 0.5
self.head_action_client.send_goal_and_wait(goal, rospy.Duration(5))
def look_at_cb(self, where):
""" where: PoseStamped."""
goal = PointHeadGoal()
goal.target = where
goal.pointing_frame = "high_def_frame"
goal.min_duration = rospy.Duration(0.5)
goal.max_velocity = 0.5
self.head_action_client.send_goal_and_wait(goal, rospy.Duration(5))
def lookAt(self, pos, sim=False):
goal = PointHeadGoal()
point = PointStamped()
point.header.frame_id = self.frame
point.point.x = pos[0]
point.point.y = pos[1]
point.point.z = pos[2]
goal.target = point
# Point using kinect frame
goal.pointing_frame = 'head_mount_kinect_rgb_link'
if sim:
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(1.0)
goal.max_velocity = 1.0
self.pointHeadClient.send_goal_and_wait(goal)
def _point_head(self, p, frame, execute_timeout = rospy.Duration(3.0), preempt_timeout = rospy.Duration(3.0)):
p = gm.Point(*p)
ps = gm.PointStamped(point=p)
ps.header.frame_id = frame
ps.header.stamp = rospy.Time.now()
goal = PointHeadGoal(target=ps)
goal.pointing_axis = gm.Vector3(0.0, 0.0, 1.0)
goal.pointing_frame = self.pointing_frame
goal.max_velocity = 1.0
rospy.loginfo('Sending goal to head and waiting for result.')
self._ac.send_goal_and_wait(goal,execute_timeout=execute_timeout,preempt_timeout=preempt_timeout)
def lookAt (self, pose):
goal = PointHeadGoal()
point = PointStamped()
point.header.frame_id = 'torso_lift_link'
point.point.x = pose.x
point.point.y = pose.y
point.point.z = pose.z
goal.target = point
goal.pointing_frame = "head_mount_kinect_rgb_link"
goal.pointing_frame = "head_mount_link"
goal.min_duration = rospy.Duration(0.5)
goal.max_velocity = 1.0
self.client.send_goal(goal)
def configure_head():
head_client = actionlib.SimpleActionClient('head_traj_controller/point_head_action', PointHeadAction)
head_client.wait_for_server()
point_head_goal = PointHeadGoal()
point_head_goal.target.header.frame_id = 'base_link'
point_head_goal.target.point.x = 3.0
point_head_goal.target.point.y = 0.0
point_head_goal.target.point.z = 1.0
point_head_goal.pointing_frame = 'head_tilt_link'
point_head_goal.pointing_axis.x = 1
point_head_goal.pointing_axis.y = 0
point_head_goal.pointing_axis.z = 0
head_client.send_goal(point_head_goal)
head_client.wait_for_result(rospy.Duration(5.0))
def configure_head():
head_client = actionlib.SimpleActionClient(
'head_traj_controller/point_head_action', PointHeadAction)
head_client.wait_for_server()
point_head_goal = PointHeadGoal()
point_head_goal.target.header.frame_id = 'base_link'
point_head_goal.target.point.x = 3.0
point_head_goal.target.point.y = 0.0
point_head_goal.target.point.z = 1.0
point_head_goal.pointing_frame = 'head_tilt_link'
point_head_goal.pointing_axis.x = 1
point_head_goal.pointing_axis.y = 0
point_head_goal.pointing_axis.z = 0
head_client.send_goal(point_head_goal)
head_client.wait_for_result(rospy.Duration(5.0))
def _point_head(self,
p,
frame,
execute_timeout=rospy.Duration(3.0),
preempt_timeout=rospy.Duration(3.0)):
p = gm.Point(*p)
ps = gm.PointStamped(point=p)
ps.header.frame_id = frame
ps.header.stamp = rospy.Time.now()
goal = PointHeadGoal(target=ps)
goal.pointing_axis = gm.Vector3(0.0, 0.0, 1.0)
goal.pointing_frame = self.pointing_frame
goal.max_velocity = 1.0
rospy.loginfo('Sending goal to head and waiting for result.')
self._ac.send_goal_and_wait(goal,
execute_timeout=execute_timeout,
preempt_timeout=preempt_timeout)
def execute(self, userdata):
goal = PointHeadGoal()
goal.target.point.x = userdata['target_x']
goal.target.point.y = userdata['target_y']
goal.target.point.z = userdata['target_z']
goal.target.header.stamp = rospy.Time.now()
if not userdata['target_frame']:
goal.target.header.frame_id = 'base_link'
else:
goal.target.header.frame_id = userdata['target_frame']
goal.pointing_frame = userdata['pointing_frame']
goal.pointing_axis.x = userdata['pointing_x']
goal.pointing_axis.y = userdata['pointing_y']
goal.pointing_axis.z = userdata['pointing_z']
rospy.loginfo("Sending head pointing goal (%f, %f, %f) and waiting for result" % (
goal.target.point.x, goal.target.point.y, goal.target.point.z))
# send the goal
self.point_head_client.send_goal(goal)
start_time = rospy.get_rostime()
r = rospy.Rate(10)
while not rospy.is_shutdown():
now = rospy.get_rostime()
if now - start_time > rospy.Duration(self.head_timeout):
rospy.loginfo("head timed out!")
self.point_head_client.cancel_goal()
return 'failed'
if self.preempt_requested():
rospy.loginfo("point head goal preempted!")
self.point_head_client.cancel_goal()
self.service_preempt()
return 'preempted'
state = self.point_head_client.get_state()
if state == GoalStatus.SUCCEEDED:
break
r.sleep()
# finished_within_time = self.point_head_client.wait_for_result(rospy.Duration(self.head_timeout))
# if not finished_within_time:
# self.point_head_client.cancel_goal()
# return 'failed'
rospy.loginfo("point head succeeded")
return 'succeeded'
def fixHeadPosition(userdata):
head_goal = PointHeadGoal()
head_goal.target.header.frame_id = "base_link"
head_goal.target.point.x = 1.0
head_goal.target.point.y = 0.0
head_goal.target.point.z = 1.65
head_goal.pointing_frame = "stereo_link"
head_goal.pointing_axis.x = 1.0
head_goal.pointing_axis.y = 0.0
head_goal.pointing_axis.z = 0.0
head_goal.max_velocity = 1.5
head_goal.min_duration.secs = 1.5
client = SimpleActionClient("/head_traj_controller/point_head_action", PointHeadAction)
client.wait_for_server(rospy.Duration(5.0))
client.send_goal(head_goal)
return succeeded
def point_head(self, point, velocity = 0.6, frame="/torso_lift_link", block = True):
head_action_client = actionlib.SimpleActionClient("/head_traj_controller/point_head_action", PointHeadAction)
head_action_client.wait_for_server()
goal = PointHeadGoal()
goal.target = cf.create_point_stamped(point, frame)
goal.pointing_frame = "/openni_rgb_optical_frame"
goal.max_velocity = velocity
head_action_client.send_goal(goal)
if not block:
return 0
finished_within_time = head_action_client.wait_for_result(rospy.Duration(10))
if not finished_within_time:
head_action_client.cancel_goal()
rospy.logerr("timed out when asking the head to point to a new goal")
return 0
return 1
def move_head(self):
if self.head_goal.point.x == 0.0:
return None
goal = PointHeadGoal()
goal.pointing_frame = POINTING_FRAME_RH2
goal.pointing_axis.x = 1.0
goal.pointing_axis.y = 0.0
goal.pointing_axis.z = 0.0
print "MOVEMENT TIME" + str(self.period)
goal.min_duration = rospy.Duration(self.period)
#goal.min_duration = rospy.Duration(2.0)
goal.max_velocity = MAX_HEAD_VEL
goal.target = self.head_goal
rospy.loginfo('Goal frame id [%s]', goal.target.header.frame_id)
goal.target.header.stamp = rospy.Time().now()
if check_correct_goal(goal):
#rospy.loginfo('GOOOOOAAAAL SEEENT [%s]', str(goal))
self.client.send_goal(goal)
else:
rospy.loginfo("Not sending goal because it's malformed: \n" + str(goal))
return None
def execute(self, userdata):
camera_info_topic = userdata['camera_info_topic']
camera_info = rospy.wait_for_message(camera_info_topic, CameraInfo)
goal = PointHeadGoal()
x = userdata['target_x']
y = userdata['target_y']
goal.target.point = self.get_target_point(x, y, camera_info)
goal.target.header.frame_id = camera_info.header.frame_id
goal.target.header.stamp = rospy.Time.now()
goal.pointing_frame = camera_info.header.frame_id
goal.pointing_axis.x = 0
goal.pointing_axis.y = 0
goal.pointing_axis.z = 1
rospy.loginfo(
"Sending head pointing goal (%f, %f, %f) and waiting for result" %
(goal.target.point.x, goal.target.point.y, goal.target.point.z))
# send the goal
self.point_head_client.send_goal(goal)
start_time = rospy.get_rostime()
r = rospy.Rate(10)
while not rospy.is_shutdown():
now = rospy.get_rostime()
if now - start_time > rospy.Duration(self.head_timeout):
rospy.loginfo("head timed out!")
self.point_head_client.cancel_goal()
return 'failed'
if self.preempt_requested():
rospy.loginfo("point head goal preempted!")
self.point_head_client.cancel_goal()
self.service_preempt()
return 'preempted'
state = self.point_head_client.get_state()
if state == GoalStatus.SUCCEEDED:
break
r.sleep()
rospy.loginfo("point head succeeded")
return 'succeeded'
def lookAt(self, frame_id, x, y, z):
self._action_client.wait_for_server()
goal = PointHeadGoal()
point = PointStamped()
point.header.frame_id = frame_id
point.point.x = x
point.point.y = y
point.point.z = z
goal.target = point
#pointing high-def camera frame
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(1.0)
goal.max_velocity = 1.0
self._action_client.send_goal(goal)
self._action_client.wait_for_result()
return self._action_client.get_result()
def execute(self, userdata):
camera_info_topic = userdata['camera_info_topic']
camera_info = rospy.wait_for_message(camera_info_topic, CameraInfo)
goal = PointHeadGoal()
x = userdata['target_x']
y = userdata['target_y']
goal.target.point = self.get_target_point(x, y, camera_info)
goal.target.header.frame_id = camera_info.header.frame_id
goal.target.header.stamp = rospy.Time.now()
goal.pointing_frame = camera_info.header.frame_id
goal.pointing_axis.x = 0
goal.pointing_axis.y = 0
goal.pointing_axis.z = 1
rospy.loginfo("Sending head pointing goal (%f, %f, %f) and waiting for result" % (
goal.target.point.x, goal.target.point.y, goal.target.point.z))
# send the goal
self.point_head_client.send_goal(goal)
start_time = rospy.get_rostime()
r = rospy.Rate(10)
while not rospy.is_shutdown():
now = rospy.get_rostime()
if now - start_time > rospy.Duration(self.head_timeout):
rospy.loginfo("head timed out!")
self.point_head_client.cancel_goal()
return 'failed'
if self.preempt_requested():
rospy.loginfo("point head goal preempted!")
self.point_head_client.cancel_goal()
self.service_preempt()
return 'preempted'
state = self.point_head_client.get_state()
if state == GoalStatus.SUCCEEDED:
break
r.sleep()
rospy.loginfo("point head succeeded")
return 'succeeded'
def move_head(self, x, y, z,
min_dur = 0.0,
max_velocity = 1.0,
frame_id = 'base_link',
timeout = 5.0):
point = PointStamped()
point.header.frame_id = frame_id
point.header.stamp = rospy.Time.now()
point.point.x, point.point.y, point.point.z = x, y, z
goal = PointHeadGoal()
goal.pointing_frame = 'head_plate_frame'
goal.max_velocity = max_velocity
goal.min_duration = rospy.Duration.from_sec( min_dur )
goal.target = point
self.head_client.send_goal( goal )
self.head_client.wait_for_result( timeout =
rospy.Duration.from_sec( timeout ) )
if not self.head_client.get_state() == GoalStatus.SUCCEEDED:
rospy.logerr( 'can not move head to:\n %s'%( goal ) )
return False
return True
goal = TuckArmsGoal()
goal.tuck_left = True
goal.tuck_right = True
tuck_arm_client.wait_for_server(rospy.Duration(5.0))
tuck_arm_client.send_goal(goal)
tuck_arm_client.wait_for_result(rospy.Duration.from_sec(30.0))
#tuck_arm_client.send_goal_and_wait(goal, rospy.Duration(30.0), rospy.Duration(5.0))
rospy.loginfo("Head scanning... ")
goalH = PointHeadGoal()
goalH.target.header.frame_id = "base_link"
goalH.target.point.x = 1.7
goalH.target.point.y = -3.0
goalH.target.point.z = 0.0
goalH.pointing_frame = "high_def_frame"
goalH.pointing_axis.x = 1
goalH.pointing_axis.y = 0
goalH.pointing_axis.z = 0
point_head_client = actionlib.SimpleActionClient(
"/head_traj_controller/point_head_action", PointHeadAction)
point_head_client.wait_for_server()
point_head_client.send_goal(goalH)
point_head_client.wait_for_result(rospy.Duration.from_sec(5.0))
rospy.sleep(1.0)
goalH.target.point.y = -1.0
point_head_client.send_goal(goalH)
point_head_client.wait_for_result(rospy.Duration.from_sec(5.0))
rospy.sleep(1.0)