Esempio n. 1
0
    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))
Esempio n. 2
0
            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
Esempio n. 3
0
    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))
Esempio n. 4
0
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)
Esempio n. 5
0
        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
Esempio n. 6
0
    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))
Esempio n. 7
0
 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)
Esempio n. 8
0
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
Esempio n. 9
0
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")
Esempio n. 10
0
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()
Esempio n. 11
0
    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()
Esempio n. 12
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)
Esempio n. 13
0
 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
Esempio n. 14
0
 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
Esempio n. 15
0
  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)
Esempio n. 16
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))
Esempio n. 17
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")
Esempio n. 19
0
    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)
Esempio n. 20
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)
Esempio n. 21
0
            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'
Esempio n. 23
0
        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
Esempio n. 24
0
        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
Esempio n. 25
0
    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'
Esempio n. 31
0
    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
Esempio n. 33
0
    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)
Esempio n. 35
0
    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)
Esempio n. 36
0
    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()
Esempio n. 37
0
 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)
Esempio n. 38
0
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)
Esempio n. 41
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
Esempio n. 42
0
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
Esempio n. 43
0
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