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 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 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 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 point_head(self):
print "Pointing head"
head_goal = PointHeadGoal()
head_goal.target = PoseConverter.to_point_stamped_msg('/torso_lift_link',
np.mat([1., 0.4, 0.]).T,
np.mat(np.eye(3)))
head_goal.target.header.stamp = rospy.Time()
head_goal.min_duration = rospy.Duration(3.)
head_goal.max_velocity = 0.2
self.head_point_sac.send_goal_and_wait(head_goal)
def goal( frame, pos, d ):
g = PointHeadGoal()
g.target.header.frame_id = frame
g.target.point.x = pos[0]
g.target.point.y = pos[1]
g.target.point.z = pos[2]
g.min_duration = rospy.Duration( d )
# Note, looking @ head action source, it seems pointing_frame unimplemented...
# Just account with a static offset (for now) where desired.
return g
def move_head_to_point_and_wait(point):
rospy.loginfo("Moving head to point and waiting for action to finish")
headGoal = PointHeadGoal()
headGoal.target.header.frame_id = 'base_link'
headGoal.min_duration = rospy.Duration(1.0)
headGoal.target.point = Point(1,0,1)
headGoal.target.point = point
headActionClient.send_goal(headGoal)
headActionClient.wait_for_result(rospy.Duration(10))
rospy.loginfo("Head action client finished")
def point_head_to(x, y, z, wait=True, frame="base_footprint"):
msg = PointHeadGoal()
msg.target.header.frame_id = frame
msg.target.point.x = x
msg.target.point.y = y
msg.target.point.z = z
msg.max_velocity = 1.0
head_client.send_goal(msg)
if wait:
head_client.wait_for_result()
def look_at_table_point(self, pt):
point_head_goal = PointHeadGoal()
point = PointStamped()
point.header.frame_id = '/table_nav'
point.point.x = pt[0]
point.point.y = pt[1]
point.point.z = pt[2]
point_head_goal.target = point
self._point_head_client.send_goal(point_head_goal)
self._point_head_client.wait_for_result()
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 PointAdd(x, y, z, dur, state, res):
pgoal = PointHeadGoal()
pgoal.target.header.frame_id = '/torso_lift_link'
pgoal.target.point.x = x
pgoal.target.point.y = y
pgoal.target.point.z = z
pgoal.min_duration = rospy.Duration(dur)
pgoal.max_velocity = 1.0
smach.StateMachine.add(
state,
SimpleActionState('/head_traj_controller/point_head_action',
PointHeadAction,
goal=pgoal),
transitions={'succeeded': res})
return
def PointAdd( x, y, z, dur, state, res ):
pgoal = PointHeadGoal()
pgoal.target.header.frame_id = '/torso_lift_link'
pgoal.target.point.x = x
pgoal.target.point.y = y
pgoal.target.point.z = z
pgoal.min_duration = rospy.Duration( dur )
pgoal.max_velocity = 1.0
smach.StateMachine.add(
state,
SimpleActionState( '/head_traj_controller/point_head_action',
PointHeadAction,
goal = pgoal ),
transitions = { 'succeeded' : res })
return
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 __init__(self):
rospy.init_node('point_head_driving')
self.rad = 1.5
self.k_theta = 5
self.thres_xy = 0.03
self.thres_th = 0.03
self.lastAngle = 0.0
self.goal = PointHeadGoal()
self.goal.target.header.frame_id = "base_link"
self.goal.target.point.x = self.rad
self.goal.target.point.y = 0
self.goal.target.point.z = 0.0
self.goal.pointing_frame = "high_def_frame"
self.goal.pointing_axis.x = 1
self.goal.pointing_axis.y = 0
self.goal.pointing_axis.z = 0
self.goal.max_velocity = 1.0
self.point_head_client = actionlib.SimpleActionClient(
"/head_traj_controller/point_head_action", PointHeadAction)
self.point_head_client.wait_for_server()
self.twistSub = rospy.Subscriber("/base_controller/command",
Twist,
self.twistCallback,
queue_size=1)
self.point_head_client.send_goal(self.goal)
rospy.loginfo("point_head_driving.py initialized")
def test_camera(self):
rospy.init_node(NAME, anonymous=True)
head_action_client = actionlib.action_client.ActionClient(
'/head_traj_controller/point_head_action', PointHeadAction)
def send_head_goal(g):
g.target.header.stamp = rospy.get_rostime()
return head_action_client.send_goal(g)
head_goal = PointHeadGoal()
head_goal.target.header.frame_id = 'base_link'
head_goal.target.point = Point(10, 0, 0.55)
projector_pub = rospy.Publisher(
"/projector_wg6802418_controller/projector", Int32, latch=True)
projector_pub.publish(Int32(0))
print " subscribe stereo left image from ROS "
rospy.Subscriber(
"/narrow_stereo/left/image_raw", image_msg, self.imageInput
) # this is a camera mounted on PR2 head (left stereo camera)
rospy.Subscriber(
"/narrow_stereo/left/camera_info", camerainfo_msg,
self.camerainfoInput
) # this is a camera mounted on PR2 head (left stereo camera)
timeout_t = time.time() + TEST_DURATION
while not rospy.is_shutdown(
) and not self.success and time.time() < timeout_t:
head_goal_status = send_head_goal(head_goal)
time.sleep(1.0)
self.assert_(self.success)
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 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 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 repoint_goal_cb(userdata, goal):
# Convert PoseStamped in userdata to PointHeadGoal
# mobj = userdata.look_points.poses[0] # We'll have head point to first object.
# pgoal = PointHeadGoal()
# pgoal.target.header.frame_id = userdata.look_points.header.frame_id
# pgoal.target.point.x = mobj.pose.position.x
# pgoal.target.point.y = mobj.pose.position.y
# pgoal.target.point.z = mobj.pose.position.z
# pgoal.min_duration = rospy.Duration(0.6)
# pgoal.max_velocity = 1.0
pgoal = PointHeadGoal()
pgoal.target.header.frame_id = '/torso_lift_link'
pgoal.target.point.x = 0.50
pgoal.target.point.y = 0.0
pgoal.target.point.z = -0.35
pgoal.min_duration = rospy.Duration(0.6)
pgoal.max_velocity = 1.0
return pgoal
def repoint_goal_cb(userdata, goal):
# Convert PoseStamped in userdata to PointHeadGoal
# mobj = userdata.look_points.poses[0] # We'll have head point to first object.
# pgoal = PointHeadGoal()
# pgoal.target.header.frame_id = userdata.look_points.header.frame_id
# pgoal.target.point.x = mobj.pose.position.x
# pgoal.target.point.y = mobj.pose.position.y
# pgoal.target.point.z = mobj.pose.position.z
# pgoal.min_duration = rospy.Duration(0.6)
# pgoal.max_velocity = 1.0
pgoal = PointHeadGoal()
pgoal.target.header.frame_id = '/torso_lift_link'
pgoal.target.point.x = 0.50
pgoal.target.point.y = 0.0
pgoal.target.point.z = -0.35
pgoal.min_duration = rospy.Duration(0.6)
pgoal.max_velocity = 1.0
return pgoal
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,
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 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):
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 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 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 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 __init__(self):
self.defaultLookatPoint = Point(1, 0, 1.3)
self.downLookatPoint = Point(0.7, 0, 0.7)
self.targetLookatPoint = Point(1, 0, 1.3)
self.currentLookatPoint = Point(1, 0, 1.3)
self.currentGazeAction = GazeGoal.LOOK_FORWARD
self.prevGazeAction = self.currentGazeAction
self.prevTargetLookatPoint = array(self.defaultLookatPoint)
self.isFrozen = False
# Some constants
self.doNotInterrupt = [GazeGoal.GLANCE_RIGHT_EE,
GazeGoal.GLANCE_LEFT_EE, GazeGoal.NOD,
GazeGoal.SHAKE]
self.nodPositions = [Point(1, 0, 1.05), Point(1, 0, 1.55)]
self.shakePositions = [Point(1, 0.2, 1.35), Point(1, -0.2, 1.35)]
self.nNods = 5
self.nShakes = 5
self.nodCounter = 5
self.shakeCounter = 5
self.facePos = None
## Action client for sending commands to the head.
self.headActionClient = SimpleActionClient(
'/head_traj_controller/point_head_action', PointHeadAction)
self.headActionClient.wait_for_server()
rospy.loginfo('Head action client has responded.')
self.headGoal = PointHeadGoal()
self.headGoal.target.header.frame_id = 'base_link'
self.headGoal.pointing_frame = 'head_mount_kinect_rgb_link'
self.headGoal.pointing_axis.x = 1
self.headGoal.min_duration = rospy.Duration(1.0)
self.headGoal.target.point = Point(1, 0, 1)
## Client for receiving detected faces
#self.faceClient = SimpleActionClient('face_detector_action', FaceDetectorAction)
## Service client for arm states
self.tfListener = TransformListener()
## Server for gaze requested gaze actions
self.gazeActionServer = actionlib.SimpleActionServer(
'gaze_action', GazeAction, self.executeGazeAction, False)
self.gazeActionServer.start()
def look_at(self, obj_name, frame_id='base_link'):
"""
This method will publish coke_location coordinates to PR2's head. This will rotate
PR2's head to point to those coordinates in the environment.
:param: frame_id: str
:param: x: float
:param: y: float
:param: z: float
"""
print("Trying to look at {}".format(obj_name))
gms_func = rospy.ServiceProxy('/gazebo/get_model_state', GetModelState)
can_pose = gms_func(obj_name, "pr2::base_footprint").pose.position
data = PointHeadActionGoal()
point = PointStamped()
point.header.frame_id = frame_id
point.point.x = can_pose.x
point.point.y = can_pose.y
point.point.z = can_pose.z
data.goal = PointHeadGoal()
data.goal.target = point
data.goal.pointing_frame = "high_def_frame"
data.goal.pointing_axis.x = 1
data.goal.pointing_axis.y = 0
data.goal.pointing_axis.z = 0
# Publish goal to PR2's head
self.headpub.publish(data)
print ('Moving head to coordinates: ')
print (can_pose)
# The callback method for this subscriber will only be activated when the head is pointing to the goal.
self.headsub = rospy.Subscriber("/head_traj_controller/point_head_action/result", PointHeadActionResult, self.callback_sense)
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 __init__(self):
self.defaultLookatPoint = Point(1,0,1.35)
self.downLookatPoint = Point(0.5,0,0.5)
self.targetLookatPoint = Point(1,0,1.35)
self.currentLookatPoint = Point(1,0,1.35)
self.currentGazeAction = 'LOOK_FORWARD';
self.prevGazeAction = self.currentGazeAction
self.prevTargetLookatPoint = array(self.defaultLookatPoint);
# Some constants
self.doNotInterrupt = ['GLANCE_RIGHT_EE', 'GLANCE_LEFT_EE', 'NOD', 'SHAKE'];
self.nodPositions = [Point(1,0,1.05), Point(1,0,1.55)]
self.shakePositions = [Point(1,0.2,1.35), Point(1,-0.2,1.35)]
self.nNods = 5
self.nShakes = 5
self.nodCounter = 5
self.shakeCounter = 5
self.facePos = None
## Action client for sending commands to the head.
self.headActionClient = SimpleActionClient('/head_traj_controller/point_head_action', PointHeadAction)
self.headActionClient.wait_for_server()
rospy.loginfo('Head action client has responded.')
self.headGoal = PointHeadGoal()
self.headGoal.target.header.frame_id = 'base_link'
self.headGoal.min_duration = rospy.Duration(1.0)
self.headGoal.target.point = Point(1,0,1)
## Client for receiving detected faces
#self.faceClient = SimpleActionClient('face_detector_action', FaceDetectorAction)
## Service client for arm states
self.tfListener = TransformListener()
def run(self):
while not self.shutdown:
point_head_goal = PointHeadGoal()
point_head_goal.target.header.frame_id = '/l_gripper_r_finger_tip_link'
point_head_action_client.send_goal(point_head_goal)
def sm_fetch_object():
# Create a SMACH state machine
sm = smach.StateMachine(outcomes=['succeeded', 'aborted', 'preempted'],
input_keys=['tagid'])
with sm:
# Servo in towards table
approach = sm_rfid_servo_approach.sm_rfid_servo_approach()
smach.StateMachine.add(
'SERVO_APPROACH', # outcomes: succeeded, aborted, preempted
approach,
remapping={'tagid': 'tagid'}, #input
transitions={'succeeded': 'POINT_HEAD'})
# Point Head Down (eventaully roll this and perceive table into own sm?)
pgoal = PointHeadGoal()
pgoal.target.header.frame_id = '/torso_lift_link'
pgoal.target.point.x = 0.50
pgoal.min_duration = rospy.Duration(0.6)
pgoal.max_velocity = 1.0
smach.StateMachine.add('POINT_HEAD',
SimpleActionState(
'/head_traj_controller/point_head_action',
PointHeadAction,
goal=pgoal),
transitions={'succeeded': 'PERCEIVE_TABLE'})
# Detect table and determine possible approach directions!
# requires:
# roslaunch hrl_pr2_lib openni_kinect.launch
# roslaunch hrl_object_fetching tabletop_detect.launch
smach.StateMachine.add(
'PERCEIVE_TABLE',
ServiceState('/table_detect_inst',
DetectTableInst,
request=DetectTableInstRequest(1.0),
response_slots=['grasp_points']), # PoseArray
transitions={'succeeded': 'PERCEIVE_OBJECT'},
remapping={'grasp_points': 'approach_poses'})
# Setment objects
smach.StateMachine.add(
'PERCEIVE_OBJECT',
ServiceState('/obj_segment_inst',
DetectTableInst,
request=DetectTableInstRequest(1.0),
response_slots=['grasp_points']), # PoseArray
transitions={'succeeded': 'APPROACH_TABLE'},
remapping={'grasp_points': 'object_poses'}) #output
# Move to the desired approach vector
sm_table = sm_approach.sm_approach_table()
smach.StateMachine.add(
'APPROACH_TABLE',
sm_table,
remapping={
'table_edge_poses': 'approach_poses', # input (PoseArray)
'movebase_pose_global':
'approach_movebase_pose', # output (PoseStamped)
'table_edge_global': 'table_edge_global'
}, # output (PoseStamped)
transitions={'succeeded': 'REPOINT_HEAD'})
# Re-Point Head to table's edge.
def repoint_goal_cb(userdata, goal):
# Convert PoseStamped in userdata to PointHeadGoal
# mobj = userdata.look_points.poses[0] # We'll have head point to first object.
# pgoal = PointHeadGoal()
# pgoal.target.header.frame_id = userdata.look_points.header.frame_id
# pgoal.target.point.x = mobj.pose.position.x
# pgoal.target.point.y = mobj.pose.position.y
# pgoal.target.point.z = mobj.pose.position.z
# pgoal.min_duration = rospy.Duration(0.6)
# pgoal.max_velocity = 1.0
pgoal = PointHeadGoal()
pgoal.target.header.frame_id = '/torso_lift_link'
pgoal.target.point.x = 0.50
pgoal.target.point.y = 0.0
pgoal.target.point.z = -0.35
pgoal.min_duration = rospy.Duration(0.6)
pgoal.max_velocity = 1.0
return pgoal
smach.StateMachine.add(
'REPOINT_HEAD',
SimpleActionState('/head_traj_controller/point_head_action',
PointHeadAction,
goal_cb=repoint_goal_cb,
input_keys=['look_points']),
remapping={'look_points': 'object_poses'}, # input (PoseStamped)
transitions={'succeeded': 'PREP_UNFOLD'})
# Prep unfold
smach.StateMachine.add('PREP_UNFOLD',
ServiceState('rfid_handoff/stow',
HandoffSrv,
request=HandoffSrvRequest()),
transitions={'succeeded': 'UNFOLD'})
# Unfold
smach.StateMachine.add('UNFOLD',
ServiceState('traj_playback/unfold',
TrajPlaybackSrv,
request=TrajPlaybackSrvRequest(0)),
transitions={'succeeded': 'MANIPULATE'})
# Manipulate
sm_grasp = sm_overhead_grasp.sm_grasp()
smach.StateMachine.add('MANIPULATE',
sm_grasp,
transitions={'succeeded': 'BACKUP'})
# Backup (60cm)
smach.StateMachine.add('BACKUP',
ServiceState('/rotate_backup',
RotateBackupSrv,
request=RotateBackupSrvRequest(
0.0, -0.50)),
transitions={'succeeded': 'PRE_STOW'})
smach.StateMachine.add('PRE_STOW',
ServiceState('rfid_handoff/stow_grasp',
HandoffSrv,
request=HandoffSrvRequest()),
transitions={'succeeded': 'succeeded'})
# # Setup robot for further navigation:
# # fold arms, position head, position ear antennas.
# smach.StateMachine.add(
# 'PRE_STOW',
# ServiceState( 'rfid_handoff/pre_stow',
# HandoffSrv,
# request = HandoffSrvRequest()),
# transitions = { 'succeeded':'STOW' })
# smach.StateMachine.add(
# 'STOW',
# ServiceState( 'rfid_handoff/stow',
# HandoffSrv,
# request = HandoffSrvRequest()),
# transitions = { 'succeeded':'succeeded' })
return sm
move_gripper = rospy.ServiceProxy('move_gripper', MoveGripper)
request = MoveGripperRequest()
request.which_arm = 'right'
request.stored_goal = 'READY'
request.speed = 0.10 # m/s
offset_zeros = Vector3Stamped()
offset_zeros.header.frame_id = '/base_link'
request.goal_offset = offset_zeros # no offset
response = move_gripper(request)
# Move head to hard-coded position
client = actionlib.SimpleActionClient(
'/head_traj_controller/point_head_action', PointHeadAction)
client.wait_for_server()
g = PointHeadGoal()
g.target.header.frame_id = 'base_link'
g.target.point.x = 1.0
g.target.point.y = -0.45
g.target.point.z = 0.6
g.min_duration = rospy.Duration(1.0)
client.send_goal(g)
client.wait_for_result()
if client.get_state() == GoalStatus.SUCCEEDED:
print "Succeeded"
else:
print "Failed"
rospy.init_node('tuck_arms_and_scan')
rospy.loginfo("Tucking arms...")
tuck_arm_client = actionlib.SimpleActionClient("tuck_arms", TuckArmsAction)
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)
t = dtheta
msg = '%.2f '%x + \
'%.2f '%y + \
'%.2f '%t
socketServer.client.send(msg)
except socket.error, (value, message):
if value != 32:
print 'failed to send message'
print value, message
else:
print 'done sending state estimation to matlab'
print 'closing client'
socketServer.client.close()
socketServer.client = None
point_head_goal = PointHeadGoal()
point_head_goal.target.header.frame_id = '/base_footprint'
# point_head_goal.target.point.x = basefootprint_P_box[0]+dx-.15
# y = basefootprint_P_box[1]+dy
# if abs(y)<.25:
# k = 15
# elif abs(y)<.3:
# k = 10
# elif abs(y)<.35:
# k = 6.67
# else:
# k = 4.44
# point_head_goal.target.point.y = k*y**3
point_head_goal.target.point.x = basefootprint_P_box[0] + dx
point_head_goal.target.point.y = basefootprint_P_box[1] + dy
def sm_fetch_object():
# Create a SMACH state machine
sm = smach.StateMachine(outcomes=['succeeded','aborted','preempted'],
input_keys = [ 'tagid' ])
with sm:
# Servo in towards table
approach = sm_rfid_servo_approach.sm_rfid_servo_approach()
smach.StateMachine.add(
'SERVO_APPROACH', # outcomes: succeeded, aborted, preempted
approach,
remapping = { 'tagid' : 'tagid'}, #input
transitions = { 'succeeded': 'POINT_HEAD' })
# Point Head Down (eventaully roll this and perceive table into own sm?)
pgoal = PointHeadGoal()
pgoal.target.header.frame_id = '/torso_lift_link'
pgoal.target.point.x = 0.50
pgoal.min_duration = rospy.Duration(0.6)
pgoal.max_velocity = 1.0
smach.StateMachine.add(
'POINT_HEAD',
SimpleActionState( '/head_traj_controller/point_head_action',
PointHeadAction,
goal = pgoal ),
transitions = { 'succeeded' : 'PERCEIVE_TABLE' })
# Detect table and determine possible approach directions!
# requires:
# roslaunch hrl_pr2_lib openni_kinect.launch
# roslaunch hrl_object_fetching tabletop_detect.launch
smach.StateMachine.add(
'PERCEIVE_TABLE',
ServiceState( '/table_detect_inst',
DetectTableInst,
request = DetectTableInstRequest( 1.0 ),
response_slots = ['grasp_points']), # PoseArray
transitions = {'succeeded':'PERCEIVE_OBJECT'},
remapping = {'grasp_points':'approach_poses'})
# Setment objects
smach.StateMachine.add(
'PERCEIVE_OBJECT',
ServiceState( '/obj_segment_inst',
DetectTableInst,
request = DetectTableInstRequest( 1.0 ),
response_slots = ['grasp_points']), # PoseArray
transitions = {'succeeded':'APPROACH_TABLE'},
remapping = {'grasp_points':'object_poses'}) #output
# Move to the desired approach vector
sm_table = sm_approach.sm_approach_table()
smach.StateMachine.add(
'APPROACH_TABLE',
sm_table,
remapping = {'table_edge_poses':'approach_poses', # input (PoseArray)
'movebase_pose_global':'approach_movebase_pose', # output (PoseStamped)
'table_edge_global':'table_edge_global'}, # output (PoseStamped)
transitions = {'succeeded':'REPOINT_HEAD'})
# Re-Point Head to table's edge.
def repoint_goal_cb(userdata, goal):
# Convert PoseStamped in userdata to PointHeadGoal
# mobj = userdata.look_points.poses[0] # We'll have head point to first object.
# pgoal = PointHeadGoal()
# pgoal.target.header.frame_id = userdata.look_points.header.frame_id
# pgoal.target.point.x = mobj.pose.position.x
# pgoal.target.point.y = mobj.pose.position.y
# pgoal.target.point.z = mobj.pose.position.z
# pgoal.min_duration = rospy.Duration(0.6)
# pgoal.max_velocity = 1.0
pgoal = PointHeadGoal()
pgoal.target.header.frame_id = '/torso_lift_link'
pgoal.target.point.x = 0.50
pgoal.target.point.y = 0.0
pgoal.target.point.z = -0.35
pgoal.min_duration = rospy.Duration(0.6)
pgoal.max_velocity = 1.0
return pgoal
smach.StateMachine.add(
'REPOINT_HEAD',
SimpleActionState( '/head_traj_controller/point_head_action',
PointHeadAction,
goal_cb = repoint_goal_cb,
input_keys = ['look_points']),
remapping = {'look_points':'object_poses'}, # input (PoseStamped)
transitions = { 'succeeded' : 'PREP_UNFOLD' })
# Prep unfold
smach.StateMachine.add(
'PREP_UNFOLD',
ServiceState( 'rfid_handoff/stow',
HandoffSrv,
request = HandoffSrvRequest()),
transitions = { 'succeeded':'UNFOLD' })
# Unfold
smach.StateMachine.add(
'UNFOLD',
ServiceState( 'traj_playback/unfold',
TrajPlaybackSrv,
request = TrajPlaybackSrvRequest( 0 )),
transitions = { 'succeeded':'MANIPULATE' })
# Manipulate
sm_grasp = sm_overhead_grasp.sm_grasp()
smach.StateMachine.add(
'MANIPULATE',
sm_grasp,
transitions = {'succeeded':'BACKUP'})
# Backup (60cm)
smach.StateMachine.add(
'BACKUP',
ServiceState( '/rotate_backup',
RotateBackupSrv,
request = RotateBackupSrvRequest(0.0, -0.50)),
transitions = { 'succeeded':'PRE_STOW' })
smach.StateMachine.add(
'PRE_STOW',
ServiceState( 'rfid_handoff/stow_grasp',
HandoffSrv,
request = HandoffSrvRequest()),
transitions = { 'succeeded':'succeeded' })
# # Setup robot for further navigation:
# # fold arms, position head, position ear antennas.
# smach.StateMachine.add(
# 'PRE_STOW',
# ServiceState( 'rfid_handoff/pre_stow',
# HandoffSrv,
# request = HandoffSrvRequest()),
# transitions = { 'succeeded':'STOW' })
# smach.StateMachine.add(
# 'STOW',
# ServiceState( 'rfid_handoff/stow',
# HandoffSrv,
# request = HandoffSrvRequest()),
# transitions = { 'succeeded':'succeeded' })
return sm
def cousins_demo():
# Create a SMACH state machine
sm = smach.StateMachine(outcomes=['succeeded','aborted','preempted'],
input_keys = [ 'explore_id', # should be ''
'obj_id',
'person_id',
'explore_radius' ])
with sm:
sm_search = sm_rfid_explore.sm_search()
smach.StateMachine.add(
'RFID_SEARCH', # outcomes: succeded, aborted, preempted
sm_search,
remapping = { 'tagid' : 'explore_id', # input
'explore_radius' : 'explore_radius' },
transitions={'succeeded':'BEST_VANTAGE_OBJ'})
# Get best vantage for obj.
# The NextBestVantage was initialized in the search.
smach.StateMachine.add(
'BEST_VANTAGE_OBJ',
ServiceState( '/rfid_recorder/best_vantage',
NextBestVantage,
request_slots = ['tagid'],
response_slots = ['best_vantage']),
remapping = {'best_vantage':'best_vantage_obj', # output
'tagid':'obj_id'}, # input
transitions = {'succeeded':'MOVE_VANTAGE_OBJ'})
smach.StateMachine.add(
'MOVE_VANTAGE_OBJ',
SimpleActionState( '/move_base',
MoveBaseAction,
goal_slots = [ 'target_pose' ]),
remapping = { 'target_pose' : 'best_vantage_obj' }, # input
transitions = {'aborted':'BEST_VANTAGE_OBJ',
'preempted':'aborted',
'succeeded':'FETCH'})
# # FETCH
# smach.StateMachine.add('FETCH',PrintStr( 'Fetching object' ),
# transitions = { 'succeeded' : 'BEST_VANTAGE_PERSON' })
# Fetch
smach.StateMachine.add(
'FETCH',
sm_fetch.sm_fetch_object(),
remapping = {'tagid':'obj_id'},
transitions = {'succeeded':'POINT_HEAD',
'aborted':'BACKUP'})
# Backup (60cm)
smach.StateMachine.add(
'BACKUP',
ServiceState( '/rotate_backup',
RotateBackupSrv,
request = RotateBackupSrvRequest(0.0, -0.50)),
transitions = { 'succeeded':'PRE_STOW' })
smach.StateMachine.add(
'PRE_STOW',
ServiceState( 'rfid_handoff/stow_grasp',
HandoffSrv,
request = HandoffSrvRequest()),
transitions = { 'succeeded':'POINT_HEAD' })
# Point Head Down (eventaully roll this and perceive table into own sm?)
pgoal = PointHeadGoal()
pgoal.target.header.frame_id = '/torso_lift_link'
pgoal.target.point.x = 0.50
pgoal.target.point.z = 0.30
pgoal.min_duration = rospy.Duration(0.6)
pgoal.max_velocity = 1.0
smach.StateMachine.add(
'POINT_HEAD',
SimpleActionState( '/head_traj_controller/point_head_action',
PointHeadAction,
goal = pgoal ),
transitions = { 'succeeded' : 'BEST_VANTAGE_PERSON' })
# Get best vantage for obj.
# The NextBestVantage was initialized in the search.
smach.StateMachine.add(
'BEST_VANTAGE_PERSON',
ServiceState( '/rfid_recorder/best_vantage',
NextBestVantage,
request_slots = ['tagid'],
response_slots = ['best_vantage']),
remapping = {'best_vantage':'best_vantage_person', # output
'tagid':'person_id'}, # input
transitions = {'succeeded':'MOVE_VANTAGE_PERSON'})
smach.StateMachine.add(
'MOVE_VANTAGE_PERSON',
SimpleActionState( '/move_base',
MoveBaseAction,
goal_slots = [ 'target_pose' ]),
remapping = { 'target_pose' : 'best_vantage_person' }, # input
transitions = {'aborted':'BEST_VANTAGE_PERSON',
'preempted':'aborted',
'succeeded':'DELIVERY'})
sm_delivery = sm_rfid_delivery.sm_delivery()
smach.StateMachine.add(
'DELIVERY', # outcomes: succeeded, aborted, preempted
sm_delivery,
remapping = { 'tagid' : 'person_id'}, #input
transitions = { 'succeeded': 'succeeded' })
return sm
