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 move_head_hor (self, hor_val):
head_goal = PointHeadGoal()
head_goal.target.header.frame_id = 'torso_lift_link'
head_goal.max_velocity = .3
self.hor_pos = -(hor_val/20.0 - 2.5)
head_goal.target.point = Point(1.5, self.hor_pos, self.vert_pos)
self.head_client.send_goal(head_goal)
if (self.head_client.get_state() != GoalStatus.SUCCEEDED):
rospy.logwarn('Head action unsuccessful.')
def look_at(self, x, y, z, frame, max_v, duration=0.5):
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.max_velocity = max_v
self.client.send_goal(goal)
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 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 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 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 lookat_goal(self, angle):
head_goal = PointHeadGoal()
head_goal.target.header.frame_id = '/torso_lift_link'
head_goal.max_velocity = 0.8
angle_in_radians = math.radians(angle)
x = math.cos(angle_in_radians) * 5
y = math.sin(angle_in_radians) * 5
z = -0.5
head_goal.target.point = Point(x, y, z)
return head_goal
def point_head(self, x, y, z):
"""
Point the head to the specified point
"""
head_goal = PointHeadGoal()
head_goal.target.header.frame_id = '/torso_lift_link'
head_goal.max_velocity = .3
# The transform seems to aim high. Move it down a little...
head_goal.target.point = Point(x, y, z - .4)
rospy.logdebug('Moving head to\n' + str(head_goal.target.point))
self.head_client.send_goal(head_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(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_r_gripper(self):
name_space = '/head_traj_controller/point_head_action'
head_client = SimpleActionClient(name_space, PointHeadAction)
head_client.wait_for_server()
head_goal = PointHeadGoal()
head_goal.target.header.frame_id = "r_gripper_tool_frame"
head_goal.min_duration = rospy.Duration(0.3)
head_goal.target.point = Point(0, 0, 0)
head_goal.max_velocity = 1.0
head_client.send_goal(head_goal)
head_client.wait_for_result(rospy.Duration(5.0))
if (head_client.get_state() != GoalStatus.SUCCEEDED):
rospy.logwarn('Head action unsuccessful.')
def look_at_r_gripper(self):
name_space = '/head_traj_controller/point_head_action'
head_client = SimpleActionClient(name_space, PointHeadAction)
head_client.wait_for_server()
head_goal = PointHeadGoal()
head_goal.target.header.frame_id = "r_gripper_tool_frame"
head_goal.min_duration = rospy.Duration(0.3)
head_goal.target.point = Point(0, 0, 0)
head_goal.max_velocity = 1.0
head_client.send_goal(head_goal)
head_client.wait_for_result(rospy.Duration(5.0))
if (head_client.get_state() != GoalStatus.SUCCEEDED):
rospy.logwarn('Head action unsuccessful.')
def move_head():
name_space = '/head_traj_controller/point_head_action'
head_client = SimpleActionClient(name_space, PointHeadAction)
head_client.wait_for_server()
head_goal = PointHeadGoal()
head_goal.target.header.frame_id = self.get_frame()
head_goal.min_duration = rospy.Duration(0.3)
head_goal.target.point = self.get_target()
head_goal.max_velocity = 10.0
head_client.send_goal(head_goal)
head_client.wait_for_result(rospy.Duration(2.0))
if (head_client.get_state() != GoalStatus.SUCCEEDED):
rospy.logwarn('Head action unsuccessful.')
self.gui.show_text_in_rviz("Head!")
def move_head():
name_space = '/head_traj_controller/point_head_action'
head_client = SimpleActionClient(name_space, PointHeadAction)
head_client.wait_for_server()
head_goal = PointHeadGoal()
head_goal.target.header.frame_id = self.get_frame()
head_goal.min_duration = rospy.Duration(0.3)
head_goal.target.point = self.get_target()
head_goal.max_velocity = 10.0
head_client.send_goal(head_goal)
head_client.wait_for_result(rospy.Duration(2.0))
if (head_client.get_state() != GoalStatus.SUCCEEDED):
rospy.logwarn('Head action unsuccessful.')
self.gui.show_text_in_rviz("Head!")
def _look_at(self, 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(2.0) goal.max_velocity = 0.85 self.client.send_goal(goal)
def main():
rospy.init_node('ball_tracking', anonymous=True)
head_client = actionlib.SimpleActionClient(
"/head_controller/absolute_point_head_action", PointHeadAction)
head_client.wait_for_server()
goal = PointHeadGoal()
goal.target.header.stamp = rospy.Time.now()
goal.target.header.frame_id = "/camera_color_optical_frame"
goal.pointing_axis.x = 0
goal.pointing_axis.y = 0
goal.pointing_axis.z = 1
goal.target.point.x = 0.7
goal.target.point.y = 0
goal.target.point.z = 0.4
goal.max_velocity = 0.1
goal.min_duration = rospy.Duration(2.0)
head_client.send_goal(goal)
head_client.wait_for_result()
def _look_at(self, 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(2.0)
goal.max_velocity = 0.85
self.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 tilt_head(self, down=True):
name_space = '/head_traj_controller/point_head_action'
head_client = SimpleActionClient(name_space, PointHeadAction)
head_client.wait_for_server()
head_goal = PointHeadGoal()
head_goal.target.header.frame_id = self.get_frame()
head_goal.min_duration = rospy.Duration(0.3)
if down:
head_goal.target.point = Point(1, 0, Head.speed * -0.1)
else:
head_goal.target.point = Point(1, 0, Head.speed * 0.1)
head_goal.max_velocity = 10.0
head_client.send_goal(head_goal)
head_client.wait_for_result(rospy.Duration(2.0))
if (head_client.get_state() != GoalStatus.SUCCEEDED):
rospy.logwarn('Head action unsuccessful.')
def lookatpoint(self, x, y, z, velocity=10.8):
head_client = actionlib.SimpleActionClient(
"/head_controller/absolute_point_head_action", PointHeadAction)
head_client.wait_for_server()
goal = PointHeadGoal()
goal.target.header.stamp = rospy.Time.now()
goal.target.header.frame_id = "/camera_color_optical_frame"
goal.pointing_axis.x = 0
goal.pointing_axis.y = 0
goal.pointing_axis.z = 1
goal.target.point.x = x
goal.target.point.y = y
goal.target.point.z = z
goal.max_velocity = velocity
goal.min_duration = rospy.Duration(1.0)
## motion start
if self.isMotion == False:
head_client.send_goal(goal)
self.isMotion = True
rospy.sleep(0.5)
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
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()
