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