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 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, 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 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()
