def adjust_torso(self):
     # move torso up
     tgoal = SingleJointPositionGoal()
     tgoal.position = 0.238  # all the way up is 0.300
     tgoal.min_duration = rospy.Duration( 2.0 )
     tgoal.max_velocity = 1.0
     self.torso_sac.send_goal_and_wait(tgoal)
    def execute(self, userdata):
        goal = SingleJointPositionGoal()
        goal.position = userdata.position
        rospy.loginfo("Sending torso goal with position %0.3f and waiting for result"%userdata.position) 

        # send the goal
        self.torso_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.torso_timeout):
                rospy.loginfo("torso timed out!")
                self.torso_client.cancel_goal()
                return 'failed'
            if self.preempt_requested():
                rospy.loginfo("torso goal preempted!")
                self.torso_client.cancel_goal()
                self.service_preempt()
                return 'preempted'
            state = self.torso_client.get_state()
            if state == GoalStatus.SUCCEEDED:
                break
            r.sleep()
        rospy.loginfo("move torso succeeded")
        return 'succeeded'
Example #3
0
 def set_torso_state(self, jval, wait=False):
     """ Sets goal for torso using provided value"""
     
     if not (type(jval) is list):
         jval = [jval] 
     
     if len(jval) == 0:
         rospy.logwarn("No torso target given!")
         self.torso_done = True
         return
     
     #HACK: it looks like with a value outside this range the actionserver doesn't return..
     if jval[0] < 0.012:
         jval[0] = 0.2
     if jval[0] > 0.3:
         jval[0] = 0.3
     self.torso_done = False
     
     goal = SingleJointPositionGoal()
     goal.position = jval[0]
     goal.min_duration = rospy.Duration(self.time_to_reach)
     
     self.torso_client.send_goal(goal, done_cb=self.__torso_done_cb())
     if wait:
         self.torso_client.wait_for_result()
Example #4
0
 def move_torso(self, pos):
     rospy.loginfo("Moving Torso to reach arm goal")
     goal_out = SingleJointPositionGoal()
     goal_out.position = pos
     
     finished_within_time = False
     self.torso_client.send_goal(goal_out)
     finished_within_time = self.torso_client.wait_for_result(rospy.Duration(45))
     if not (finished_within_time):
         self.torso_client.cancel_goal()
         self.wt_log_out.publish(data="Timed out moving torso")
         rospy.loginfo("Timed out moving torso")
         return False
     else:
         state = self.torso_client.get_state()
         result = self.torso_client.get_result()
         if (state == 3): #3 == SUCCEEDED
             rospy.loginfo("Torso Action Succeeded")
             self.wt_log_out.publish(data="Move Torso Succeeded")
             return True
         else:
             rospy.loginfo("Move Torso Failed")
             rospy.loginfo("Failure Result: %s" %result)
             self.wt_log_out.publish(data="Move Torso Failed")
             return False
Example #5
0
    def pickUpObject(self, actuatorVal, initial=False):
        """
        Pick Up Object with the Gripper and sensor on pr2
        """
        if initial:
            #rospy.init_node('pick_and_place_demo', anonymous=False)
            use_slip_controller = rospy.get_param('/reactive_grasp_node_right/use_slip_controller', 0)
            use_slip_detection = rospy.get_param('/reactive_grasp_node_right/use_slip_detection', 0)
            self.pick_and_place_demo = PickAndPlaceDemo(use_slip_controller = use_slip_controller, \
                                                       use_slip_detection = use_slip_detection)
        else:
            if int(actuatorVal)==1:
                #subprocess.Popen(['rosrun', 'pr2_pick_and_place_demos', 'IROS_2013_pick.py'],stdout=subprocess.PIPE)
                torso_action_client = actionlib.SimpleActionClient('torso_controller/position_joint_action', SingleJointPositionAction);
                rospy.loginfo("run_pick_and_place_demo: waiting for torso action server")
                torso_action_client.wait_for_server()
                rospy.loginfo("run_pick_and_place_demo: torso action server found")
                goal = SingleJointPositionGoal()
                goal.position = 0.295
                rospy.loginfo("sending command to lift torso")
                torso_action_client.send_goal(goal)
                torso_action_client.wait_for_result(rospy.Duration(30))

                #start the pick and place demo
                rospy.loginfo("starting pick and place demo")
                self.pick_and_place_demo.run_pick_up()
                print "PICK UP COMPLETED"
                #self.pick_and_place_demo.start_autonomous_thread(True)
                #pose = self.pose_handler.getPose()
                #self.drive_handler.setVelocity(self.Velocity[0,0], self.Velocity[1,0],pose[2])
                self.loco.sendCommand([-1,0,0])
                time.sleep(3)
                self.loco.sendCommand([0,0,0])
Example #6
0
    def set_torso_state(self, jval, wait=False):
        """ Sets goal for torso using provided value"""

        if not (type(jval) is list):
            jval = [jval]

        if len(jval) == 0:
            rospy.logwarn("No torso target given!")
            self.torso_done = True
            return

        #HACK: it looks like with a value outside this range the actionserver doesn't return..
        if jval[0] < 0.012:
            jval[0] = 0.2
        if jval[0] > 0.3:
            jval[0] = 0.3
        self.torso_done = False

        goal = SingleJointPositionGoal()
        goal.position = jval[0]
        goal.min_duration = rospy.Duration(self.time_to_reach)

        self.torso_client.send_goal(goal, done_cb=self.__torso_done_cb())
        if wait:
            self.torso_client.wait_for_result()
Example #7
0
    def move_torso(self, pos):
        rospy.loginfo("Moving Torso to reach arm goal")
        goal_out = SingleJointPositionGoal()
        goal_out.position = pos

        finished_within_time = False
        self.torso_client.send_goal(goal_out)
        finished_within_time = self.torso_client.wait_for_result(
            rospy.Duration(45))
        if not (finished_within_time):
            self.torso_client.cancel_goal()
            self.wt_log_out.publish(data="Timed out moving torso")
            rospy.loginfo("Timed out moving torso")
            return False
        else:
            state = self.torso_client.get_state()
            result = self.torso_client.get_result()
            if (state == 3):  #3 == SUCCEEDED
                rospy.loginfo("Torso Action Succeeded")
                self.wt_log_out.publish(data="Move Torso Succeeded")
                return True
            else:
                rospy.loginfo("Move Torso Failed")
                rospy.loginfo("Failure Result: %s" % result)
                self.wt_log_out.publish(data="Move Torso Failed")
                return False
    def get_sm(self):
        sm = smach.StateMachine(outcomes=['succeeded', 'preempted', 'aborted'])

        with sm:
            smach.StateMachine.add('NAV_APPROACH',
                                   self.sm_nav_approach.get_sm(),
                                   transitions={
                                       'succeeded': 'TORSO_SETUP',
                                       'shutdown': 'aborted'
                                   })

            # move torso up
            tgoal = SingleJointPositionGoal()
            tgoal.position = 0.300  # all the way up is 0.300
            tgoal.min_duration = rospy.Duration(2.0)
            tgoal.max_velocity = 1.0
            smach.StateMachine.add(
                'TORSO_SETUP',
                SimpleActionState('torso_controller/position_joint_action',
                                  SingleJointPositionAction,
                                  goal=tgoal),
                transitions={'succeeded': 'R_UNTUCK'})

            # Untucks the right arm
            smach.StateMachine.add(
                'R_UNTUCK',
                ServiceState('traj_playback/untuck_r_arm',
                             TrajPlaybackSrv,
                             request=TrajPlaybackSrvRequest(
                                 False)),  # if True, reverse trajectory
                transitions={'succeeded': 'R_ADJUST_MIRROR'})

            # Adjusts the mirror
            smach.StateMachine.add(
                'R_ADJUST_MIRROR',
                ServiceState('traj_playback/r_adjust_mirror',
                             TrajPlaybackSrv,
                             request=TrajPlaybackSrvRequest(
                                 False)),  # if True, reverse trajectory
                transitions={'succeeded': 'L_UNTUCK'})

            # Untucks the left arm
            smach.StateMachine.add(
                'L_UNTUCK',
                ServiceState('traj_playback/untuck_l_arm',
                             TrajPlaybackSrv,
                             request=TrajPlaybackSrvRequest(
                                 False)),  # if True, reverse trajectory
                transitions={'succeeded': 'HEAD_REG_ALL'})

            smach.StateMachine.add(
                'HEAD_REG_ALL',
                self.sm_ell_reg.get_sm(),
                transitions={'succeeded': 'SETUP_TASK_CONTROLLER'})

            smach.StateMachine.add('SETUP_TASK_CONTROLLER',
                                   SetupTaskController())

        return sm
Example #9
0
def move_body_torso(position):
    service = 'torso_controller/position_joint_action'
    goal = SingleJointPositionGoal()
    goal.position = position
    goal.min_duration = roslib.rostime.Duration(2.0)
    goal.max_velocity = 1.0
     
    client = actionlib.SimpleActionClient(service, SingleJointPositionAction)
    client.send_goal(goal, done_cb=task_completed)
    client.wait_for_result()
Example #10
0
def move_body_torso(position):
    service = 'torso_controller/position_joint_action'
    goal = SingleJointPositionGoal()
    goal.position = position
    goal.min_duration = roslib.rostime.Duration(2.0)
    goal.max_velocity = 1.0

    client = actionlib.SimpleActionClient(service, SingleJointPositionAction)
    client.send_goal(goal, done_cb=task_completed)
    client.wait_for_result()
    def get_sm(self):
        sm = smach.StateMachine(outcomes=['succeeded','preempted','aborted'])
        
        with sm:
            smach.StateMachine.add('NAV_APPROACH',
                    self.sm_nav_approach.get_sm(),
                    transitions={'succeeded' : 'TORSO_SETUP',
                                 'shutdown' : 'aborted'})

            # move torso up
            tgoal = SingleJointPositionGoal()
            tgoal.position = 0.300  # all the way up is 0.300
            tgoal.min_duration = rospy.Duration( 2.0 )
            tgoal.max_velocity = 1.0
            smach.StateMachine.add(
                'TORSO_SETUP',
                SimpleActionState( 'torso_controller/position_joint_action',
                                   SingleJointPositionAction,
                                   goal = tgoal),
                transitions = { 'succeeded': 'R_UNTUCK' })

            # Untucks the right arm
            smach.StateMachine.add(
                'R_UNTUCK',
                ServiceState( 'traj_playback/untuck_r_arm',
                              TrajPlaybackSrv,
                              request = TrajPlaybackSrvRequest( False )), # if True, reverse trajectory
                transitions = { 'succeeded' : 'R_ADJUST_MIRROR' })

            # Adjusts the mirror
            smach.StateMachine.add(
                'R_ADJUST_MIRROR',
                ServiceState( 'traj_playback/r_adjust_mirror',
                              TrajPlaybackSrv,
                              request = TrajPlaybackSrvRequest( False )), # if True, reverse trajectory
                transitions = { 'succeeded' : 'L_UNTUCK' })

            # Untucks the left arm
            smach.StateMachine.add(
                'L_UNTUCK',
                ServiceState( 'traj_playback/untuck_l_arm',
                              TrajPlaybackSrv,
                              request = TrajPlaybackSrvRequest( False )), # if True, reverse trajectory
                transitions = { 'succeeded' : 'HEAD_REG_ALL' })

            smach.StateMachine.add('HEAD_REG_ALL',
                    self.sm_ell_reg.get_sm(),
                    transitions = { 'succeeded' : 'SETUP_TASK_CONTROLLER' })

            smach.StateMachine.add(
                'SETUP_TASK_CONTROLLER',
                SetupTaskController())

        return sm
Example #12
0
    def move(self, position):
        assert (position >= 0.0 and position <= 0.2)
        self.position = position

        up = SingleJointPositionGoal()
        up.position = self.position
        up.min_duration = rospy.Duration(2.0)
        up.max_velocity = 1.0

        self.torso_client.send_goal(up)
        self.torso_client.wait_for_result()
Example #13
0
    def move(self, position):
        assert(position >= 0.0 and position <= 0.2)
        self.position = position
        
        up = SingleJointPositionGoal()
        up.position = self.position
        up.min_duration = rospy.Duration(2.0)
        up.max_velocity = 1.0

        self.torso_client.send_goal(up)
        self.torso_client.wait_for_result()
Example #14
0
 def command_torso(self, pose, timeout, blocking=True):
     goal = SingleJointPositionGoal(position=pose,
                                    min_duration=rospy.Duration(timeout),
                                    max_velocity=1.0)
     return self._send_command('torso',
                               goal,
                               blocking=blocking,
                               timeout=timeout)
Example #15
0
    def pickUpObject(self, actuatorVal, initial=False):
        """
        Pick Up Object with the Gripper and sensor on pr2
        """
        if initial:
            #rospy.init_node('pick_and_place_demo', anonymous=False)
            use_slip_controller = rospy.get_param(
                '/reactive_grasp_node_right/use_slip_controller', 0)
            use_slip_detection = rospy.get_param(
                '/reactive_grasp_node_right/use_slip_detection', 0)
            self.pick_and_place_demo = PickAndPlaceDemo(use_slip_controller = use_slip_controller, \
                                                       use_slip_detection = use_slip_detection)
        else:
            if int(actuatorVal) == 1:
                #subprocess.Popen(['rosrun', 'pr2_pick_and_place_demos', 'IROS_2013_pick.py'],stdout=subprocess.PIPE)
                torso_action_client = actionlib.SimpleActionClient(
                    'torso_controller/position_joint_action',
                    SingleJointPositionAction)
                rospy.loginfo(
                    "run_pick_and_place_demo: waiting for torso action server")
                torso_action_client.wait_for_server()
                rospy.loginfo(
                    "run_pick_and_place_demo: torso action server found")
                goal = SingleJointPositionGoal()
                goal.position = 0.295
                rospy.loginfo("sending command to lift torso")
                torso_action_client.send_goal(goal)
                torso_action_client.wait_for_result(rospy.Duration(30))

                #start the pick and place demo
                rospy.loginfo("starting pick and place demo")
                self.pick_and_place_demo.run_pick_up()
                print "PICK UP COMPLETED"
                #self.pick_and_place_demo.start_autonomous_thread(True)
                #pose = self.pose_handler.getPose()
                #self.drive_handler.setVelocity(self.Velocity[0,0], self.Velocity[1,0],pose[2])
                self.loco.sendCommand([-1, 0, 0])
                time.sleep(3)
                self.loco.sendCommand([0, 0, 0])
            return abs(msg.position[i] - HEIGHT) < TOLERANCE
    return False
            
if __name__ == '__main__':
    rospy.init_node('torso_lifter_client')

    _mutex = threading.Lock()

    last_state = LastMessage('joint_states', JointState)

    client = actionlib.SimpleActionClient('torso_controller/position_joint_action',
                                          SingleJointPositionAction)
    rospy.loginfo('Waiting for torso action client')
    client.wait_for_server()

    goal = SingleJointPositionGoal()
    goal.position = HEIGHT
    
    client.send_goal(goal)
    
    # Wait for result, or wait 60 seconds to allow full travel
    client.wait_for_result(rospy.Duration.from_sec(60))

    # Check that torso is mostly up
    ok = False
    msg = None
    with _mutex:
        msg = last_state.last()
        if msg:
            ok = check_torso_up(msg)
            
Example #17
0
def sm_grasp():
    # Create a SMACH state machine
    sm = smach.StateMachine(outcomes=['succeeded','aborted','preempted'])

    with sm:
        # Setup arm pose (out of way for perception)
        tgoal = SingleJointPositionGoal()
        tgoal.position = 0.190  # all the way up is 0.200
        tgoal.min_duration = rospy.Duration( 2.0 )
        tgoal.max_velocity = 1.0
        smach.StateMachine.add(
            'TORSO_SETUP',
            SimpleActionState( 'torso_controller/position_joint_action',
                               SingleJointPositionAction,
                               goal = tgoal),
            transitions = { 'succeeded': 'THREE_TRIES' })

        # We will run the grasper at most 3 times.
        smach.StateMachine.add(
            'THREE_TRIES',
            NTries( 3 ),
            transitions = {'succeeded':'PERCEIVE_SETUP',
                           'aborted':'aborted'})

        # get hand out of face
        smach.StateMachine.add(
            'PERCEIVE_SETUP',
            ServiceState( '/rfid_handoff/grasp', HandoffSrv ),
            transitions = { 'succeeded' : 'PERCEIVE_OBJECT' })
            

        # Setment objects
        smach.StateMachine.add(
            'PERCEIVE_OBJECT',
            ServiceState( '/obj_segment_inst',
                          DetectTableInst,
                          request = DetectTableInstRequest( 1.0 ),
                          response_slots = ['grasp_points']), # PoseArray
            transitions = {'succeeded':'GRASP_SETUP'},
            remapping = {'grasp_points':'object_poses'}) #output

        # Setup arm pose (out of way for perception)
        smach.StateMachine.add(
            'GRASP_SETUP',
            SimpleActionState( 'overhead_grasp_setup',
                               OverheadGraspSetupAction,
                               goal = OverheadGraspSetupGoal( True )), # disable new look
            transitions = { 'succeeded': 'GRASP' })

        # Actually perform grasp of some object in front of robot on table
        def grasp_goal_cb( userdata, goal ):
            # grasp_poses is PoseArray in base_link
            mgp = userdata.grasp_poses.poses[0]
            
            ggoal = OverheadGraspGoal()
            ggoal.is_grasp = True
            ggoal.grasp_type = OverheadGraspGoal.MANUAL_GRASP
            
            ggoal.x = mgp.position.x + 0.05 # Converts base_link -> torso_lift_link (only for x,y)
            ggoal.y = mgp.position.y

            o = mgp.orientation
            r,p,y = tft.euler_from_quaternion(( o.x, o.y, o.z, o.w ))
            ggoal.rot = y
            
            return ggoal

        smach.StateMachine.add(
            'GRASP',
            SimpleActionState( 'overhead_grasp',
                               OverheadGraspAction,
                               goal_cb = grasp_goal_cb,
                               input_keys = ['grasp_poses']),
            remapping = {'grasp_poses':'object_poses'},
            transitions = { 'succeeded': 'succeeded',
                            'aborted':'THREE_TRIES' })
            
    return sm
Example #18
0
#!/usr/bin/python
import roslib
roslib.load_manifest('pr2lite_moveit_config')

import rospy
import actionlib
from pr2_controllers_msgs.msg import SingleJointPositionAction, SingleJointPositionGoal 
rospy.init_node('torso_down')
torso_action_client = actionlib.SimpleActionClient('torso_controller/position_joint_action', SingleJointPositionAction);
rospy.loginfo("torso_down: waiting for torso action server")
torso_action_client.wait_for_server()
rospy.loginfo("torso_down: torso action server found")
goal = SingleJointPositionGoal()
goal.position = 0.0
rospy.loginfo("torso_down: sending command to lift torso")
torso_action_client.send_goal(goal)
torso_action_client.wait_for_result(rospy.Duration(30))
Example #19
0
 def move_torso(self, pos):
     rospy.loginfo("Moving Torso to reach arm goal")
     goal_out = SingleJointPositionGoal()
     goal_out.position = pos
     self.torso_client.send_goal(goal_out)
     return True
Example #20
0
def sm_grasp():
    if len(sys.argv) < 2 or sys.argv[1] not in ['r', 'l']:
        print "First arg should be 'r' or 'l'"
        return None
    arm = sys.argv[1]
    if arm == 'r':
        arm_mult = 1
    else:
        arm_mult = -1

    # Create a SMACH state machine
    sm = smach.StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])

    with sm:
        # Setup arm pose (out of way for perception)
        tgoal = SingleJointPositionGoal()
        #tgoal.position = 0.190  # all the way up is 0.200
        tgoal.position = 0.210  # all the way up is 0.200
        tgoal.min_duration = rospy.Duration(2.0)
        tgoal.max_velocity = 1.0
        smach.StateMachine.add('TORSO_SETUP',
                               SimpleActionState(
                                   'torso_controller/position_joint_action',
                                   SingleJointPositionAction,
                                   goal=tgoal),
                               transitions={'succeeded': 'ARM_UNTUCK'})

        smach.StateMachine.add('ARM_UNTUCK',
                               ServiceState(
                                   'traj_playback/' + arm + '_arm_untuck',
                                   TrajPlaybackSrv),
                               transitions={'succeeded': 'GRASP_BEGIN_SETUP'})

        # Setup arm pose (out of way for perception)
        smach.StateMachine.add('GRASP_BEGIN_SETUP',
                               SimpleActionState(
                                   arm + '_overhead_grasp_setup',
                                   OverheadGraspSetupAction,
                                   goal=OverheadGraspSetupGoal()),
                               transitions={'succeeded': 'DEMO_START'})

        @smach.cb_interface(outcomes=['succeeded'])
        def wait_for_enter(ud):
            raw_input("Press enter to begin cleanup demo.")
            return 'succeeded'

        smach.StateMachine.add('DEMO_START',
                               smach.CBState(wait_for_enter),
                               transitions={'succeeded': 'THREE_OBJECTS'})

        # We will pick up 3 objects.
        smach.StateMachine.add('THREE_OBJECTS',
                               NTries(3),
                               transitions={
                                   'succeeded': 'THREE_TRIES',
                                   'aborted': 'RESET_ARMS'
                               },
                               remapping={'ntries_counter': 'object_number'})

        # We will run the grasper at most 3 times.
        grasp_tries = NTries(3)
        smach.StateMachine.add('THREE_TRIES',
                               grasp_tries,
                               transitions={
                                   'succeeded': 'GRASP_SETUP',
                                   'aborted': 'aborted'
                               })

        # Setup arm pose (out of way for perception)
        smach.StateMachine.add('GRASP_SETUP',
                               SimpleActionState(
                                   arm + '_overhead_grasp_setup',
                                   OverheadGraspSetupAction,
                                   goal=OverheadGraspSetupGoal()),
                               transitions={'succeeded': 'GRASP'})

        def grasp_goal_cb(userdata, goal):
            ############################################################
            # Creating grasp goal
            grasp_goal = OverheadGraspGoal()
            grasp_goal.is_grasp = True
            grasp_goal.disable_head = False
            grasp_goal.disable_coll = False
            grasp_goal.grasp_type = OverheadGraspGoal.VISION_GRASP
            grasp_goal.x = GRASP_LOCATION[0]
            grasp_goal.y = arm_mult * GRASP_LOCATION[1]
            grasp_goal.behavior_name = "overhead_grasp"
            grasp_goal.sig_level = 0.999
            ############################################################
            return grasp_goal

        smach.StateMachine.add('GRASP',
                               SimpleActionState(arm + '_overhead_grasp',
                                                 OverheadGraspAction,
                                                 goal_cb=grasp_goal_cb),
                               transitions={
                                   'succeeded': 'PLACE',
                                   'aborted': 'THREE_TRIES'
                               })

        def place_goal_cb(userdata, goal):
            print "object Number", userdata.object_number
            ############################################################
            # Creating place place_goal
            place_goal = OverheadGraspGoal()
            place_goal.is_grasp = False
            place_goal.disable_head = False
            place_goal.disable_coll = False
            place_goal.grasp_type = OverheadGraspGoal.MANUAL_GRASP
            place_goal.x = PLACE_LOCATIONS[userdata.object_number - 1][0]
            place_goal.y = arm_mult * PLACE_LOCATIONS[userdata.object_number -
                                                      1][1]
            place_goal.roll = PLACE_LOCATIONS[userdata.object_number - 1][2]
            place_goal.behavior_name = "overhead_grasp"
            place_goal.sig_level = 0.999
            ############################################################
            return place_goal

        def clear_grasp_tries(userdata, status, result):
            grasp_tries.counter = 0

        smach.StateMachine.add('PLACE',
                               SimpleActionState(arm + '_overhead_grasp',
                                                 OverheadGraspAction,
                                                 goal_cb=place_goal_cb,
                                                 result_cb=clear_grasp_tries,
                                                 input_keys=['object_number']),
                               transitions={
                                   'succeeded': 'THREE_OBJECTS',
                                   'aborted': 'THREE_OBJECTS'
                               })

        # Setup arm pose (out of way for perception)
        smach.StateMachine.add('RESET_ARMS',
                               SimpleActionState(
                                   arm + '_overhead_grasp_setup',
                                   OverheadGraspSetupAction,
                                   goal=OverheadGraspSetupGoal()),
                               transitions={'succeeded': 'ARM_TUCK'})

        smach.StateMachine.add('ARM_TUCK',
                               ServiceState(
                                   'traj_playback/' + arm + '_arm_untuck',
                                   TrajPlaybackSrv,
                                   request=TrajPlaybackSrvRequest(True)),
                               transitions={'succeeded': 'succeeded'})

    return sm
Example #21
0
def execute_cb(goal):
    rospy.loginfo("Received a goal")

    # check for network cable
    if network_connected:
        server.set_aborted(None, "Dangerous to navigate with network cable connected.")
        return

    # check for power cable
    if ac_power_connected:
        server.set_aborted(None, "Dangerous to navigate with AC power cable connected.")
        return

    # check for power cable
    # TODO

    # start the arms tucking
    tuck_arms_client = actionlib.SimpleActionClient('tuck_arms', TuckArmsAction)
    tuck_arms_client.wait_for_server()
    tuck_goal = TuckArmsGoal()
    tuck_goal.tuck_left=True
    tuck_goal.tuck_right=True
    tuck_arms_client.send_goal(tuck_goal)

    # start the torso lowering
    torso_client = actionlib.SimpleActionClient('torso_controller/position_joint_action', SingleJointPositionAction)
    torso_client.wait_for_server()
    torso_down_goal = SingleJointPositionGoal()
    torso_down_goal.position = 0
    torso_client.send_goal(torso_down_goal)

    # configure the tilting laser
    if not set_tilt_profile([1.05,  -.7, 1.05], [0.0, 1.8, 2.0125 + .3]):
        server.set_aborted(MoveBaseResult(), "Couldn't set the profile on the laser")
        return

    configure_laser()
    configure_head()

    # wait for tuck_arms to finish. (Don't wait for torso to lower, that is a Nice To Have)
    while not tuck_arms_client.wait_for_result(rospy.Duration(0.1)):
        if server.is_preempt_requested():
            if not server.is_new_goal_available():
                tuck_arms_client.cancel_goal()

    tuck_state = tuck_arms_client.get_state()
    if tuck_state != GoalStatus.SUCCEEDED:
        if tuck_state == GoalStatus.PREEMPTED:
            server.set_preempted(MoveBaseResult(), "Tuck-arms preempted")
        elif tuck_state == GoalStatus.ABORTED:
            server.set_aborted(MoveBaseResult(), "Tuck-arms aborted")
        return

    # Now everything should be ready so send the navigation goal.
    move_base_client.send_goal(goal, None, None, feedback_cb)

    while not move_base_client.wait_for_result(rospy.Duration(0.1)):
        #in the unlikely event of network cable being plugged in while moving, stop moving.
        if network_connected:
            move_base_client.cancel_goal()
            server.set_aborted(None, "Dangerous to navigate with network cable connected.")
            return

        #in the unlikely event of ac power cable being plugged in while moving, stop moving.
        if ac_power_connected:
            move_base_client.cancel_goal()
            server.set_aborted(None, "Dangerous to navigate with ac power cable connected.")
            return

        if server.is_preempt_requested():
            if not server.is_new_goal_available():
                rospy.loginfo("Preempt requested without new goal, cancelling move_base goal.")
                move_base_client.cancel_goal()

            server.set_preempted(MoveBaseResult(), "Got preempted by a new goal")
            return


    terminal_state = move_base_client.get_state()
    result = move_base_client.get_result()
    if terminal_state == GoalStatus.PREEMPTED:
        server.set_preempted(result)
    elif terminal_state == GoalStatus.SUCCEEDED:
        server.set_succeeded(result)
    elif terminal_state == GoalStatus.ABORTED:
        server.set_aborted(result)
    else:
        server.set_aborted(result, "Unknown result from move_base")
Example #22
0
def execute_cb(goal):
    rospy.loginfo("Received a goal")

    # check for network cable
    if network_connected:
        server.set_aborted(
            None, "Dangerous to navigate with network cable connected.")
        return

    # check for power cable
    if ac_power_connected:
        server.set_aborted(
            None, "Dangerous to navigate with AC power cable connected.")
        return

    # check for power cable
    # TODO

    # start the arms tucking
    tuck_arms_client = actionlib.SimpleActionClient('tuck_arms',
                                                    TuckArmsAction)
    tuck_arms_client.wait_for_server()
    tuck_goal = TuckArmsGoal()
    tuck_goal.tuck_left = True
    tuck_goal.tuck_right = True
    tuck_arms_client.send_goal(tuck_goal)

    # start the torso lowering
    torso_client = actionlib.SimpleActionClient(
        'torso_controller/position_joint_action', SingleJointPositionAction)
    torso_client.wait_for_server()
    torso_down_goal = SingleJointPositionGoal()
    torso_down_goal.position = 0
    torso_client.send_goal(torso_down_goal)

    # configure the tilting laser
    #if not set_tilt_profile([1.05,  -.7, 1.05], [0.0, 1.8, 2.0125 + .3]):
    if not set_tilt_profile([0.1, 0.1, 0.1], [0.0, 2, 4]):
        server.set_aborted(MoveBaseResult(),
                           "Couldn't set the profile on the laser")
        return

    configure_laser()
    configure_head()

    # wait for tuck_arms to finish. (Don't wait for torso to lower, that is a Nice To Have)
    while not tuck_arms_client.wait_for_result(rospy.Duration(0.1)):
        if server.is_preempt_requested():
            if not server.is_new_goal_available():
                tuck_arms_client.cancel_goal()

    tuck_state = tuck_arms_client.get_state()
    if tuck_state != GoalStatus.SUCCEEDED:
        if tuck_state == GoalStatus.PREEMPTED:
            server.set_preempted(MoveBaseResult(), "Tuck-arms preempted")
        elif tuck_state == GoalStatus.ABORTED:
            server.set_aborted(MoveBaseResult(), "Tuck-arms aborted")
        return

    # Now everything should be ready so send the navigation goal.
    move_base_client.send_goal(goal, None, None, feedback_cb)

    while not move_base_client.wait_for_result(rospy.Duration(0.1)):
        #in the unlikely event of network cable being plugged in while moving, stop moving.
        if network_connected:
            move_base_client.cancel_goal()
            server.set_aborted(
                None, "Dangerous to navigate with network cable connected.")
            return

        #in the unlikely event of ac power cable being plugged in while moving, stop moving.
        if ac_power_connected:
            move_base_client.cancel_goal()
            server.set_aborted(
                None, "Dangerous to navigate with ac power cable connected.")
            return

        if server.is_preempt_requested():
            if not server.is_new_goal_available():
                rospy.loginfo(
                    "Preempt requested without new goal, cancelling move_base goal."
                )
                move_base_client.cancel_goal()

            server.set_preempted(MoveBaseResult(),
                                 "Got preempted by a new goal")
            return

    terminal_state = move_base_client.get_state()
    result = move_base_client.get_result()
    if terminal_state == GoalStatus.PREEMPTED:
        server.set_preempted(result)
    elif terminal_state == GoalStatus.SUCCEEDED:
        server.set_succeeded(result)
    elif terminal_state == GoalStatus.ABORTED:
        server.set_aborted(result)
    else:
        server.set_aborted(result, "Unknown result from move_base")
Example #23
0
#!/usr/bin/python
import roslib
roslib.load_manifest('pr2lite_arm_navigation')

import rospy
import actionlib
from pr2_controllers_msgs.msg import SingleJointPositionAction, SingleJointPositionGoal 
rospy.init_node('torso_up')
torso_action_client = actionlib.SimpleActionClient('torso_controller/position_joint_action', SingleJointPositionAction);
rospy.loginfo("torso_up: waiting for torso action server")
torso_action_client.wait_for_server()
rospy.loginfo("torso_up: torso action server found")
goal = SingleJointPositionGoal()
goal.position = 0.295
rospy.loginfo("torso_up: sending command to lift torso")
torso_action_client.send_goal(goal)
torso_action_client.wait_for_result(rospy.Duration(30))
Example #24
0
def sm_grasp():
    # Create a SMACH state machine
    sm = smach.StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])

    with sm:
        # Setup arm pose (out of way for perception)
        tgoal = SingleJointPositionGoal()
        tgoal.position = 0.190  # all the way up is 0.200
        tgoal.min_duration = rospy.Duration(2.0)
        tgoal.max_velocity = 1.0
        smach.StateMachine.add('TORSO_SETUP',
                               SimpleActionState(
                                   'torso_controller/position_joint_action',
                                   SingleJointPositionAction,
                                   goal=tgoal),
                               transitions={'succeeded': 'THREE_TRIES'})

        # We will run the grasper at most 3 times.
        smach.StateMachine.add('THREE_TRIES',
                               NTries(3),
                               transitions={
                                   'succeeded': 'PERCEIVE_SETUP',
                                   'aborted': 'aborted'
                               })

        # get hand out of face
        smach.StateMachine.add('PERCEIVE_SETUP',
                               ServiceState('/rfid_handoff/grasp', HandoffSrv),
                               transitions={'succeeded': 'PERCEIVE_OBJECT'})

        # Setment objects
        smach.StateMachine.add(
            'PERCEIVE_OBJECT',
            ServiceState('/obj_segment_inst',
                         DetectTableInst,
                         request=DetectTableInstRequest(1.0),
                         response_slots=['grasp_points']),  # PoseArray
            transitions={'succeeded': 'GRASP_SETUP'},
            remapping={'grasp_points': 'object_poses'})  #output

        # Setup arm pose (out of way for perception)
        smach.StateMachine.add(
            'GRASP_SETUP',
            SimpleActionState(
                'overhead_grasp_setup',
                OverheadGraspSetupAction,
                goal=OverheadGraspSetupGoal(True)),  # disable new look
            transitions={'succeeded': 'GRASP'})

        # Actually perform grasp of some object in front of robot on table
        def grasp_goal_cb(userdata, goal):
            # grasp_poses is PoseArray in base_link
            mgp = userdata.grasp_poses.poses[0]

            ggoal = OverheadGraspGoal()
            ggoal.is_grasp = True
            ggoal.grasp_type = OverheadGraspGoal.MANUAL_GRASP

            ggoal.x = mgp.position.x + 0.05  # Converts base_link -> torso_lift_link (only for x,y)
            ggoal.y = mgp.position.y

            o = mgp.orientation
            r, p, y = tft.euler_from_quaternion((o.x, o.y, o.z, o.w))
            ggoal.rot = y

            return ggoal

        smach.StateMachine.add('GRASP',
                               SimpleActionState('overhead_grasp',
                                                 OverheadGraspAction,
                                                 goal_cb=grasp_goal_cb,
                                                 input_keys=['grasp_poses']),
                               remapping={'grasp_poses': 'object_poses'},
                               transitions={
                                   'succeeded': 'succeeded',
                                   'aborted': 'THREE_TRIES'
                               })

    return sm
Example #25
0
def cousins_demo():
    # Create a SMACH state machine
    sm = smach.StateMachine(outcomes=['succeeded','aborted','preempted'],
                            input_keys = [ 'person_id' ])

    with sm:

        # Just a precation
        tgoal = SingleJointPositionGoal()
        tgoal.position = 0.040  # all the way up is 0.200, mostly down is 0.040
        tgoal.min_duration = rospy.Duration( 2.0 )
        tgoal.max_velocity = 1.0
        smach.StateMachine.add(
            'TORSO_SETUP',
            SimpleActionState( 'torso_controller/position_joint_action',
                               SingleJointPositionAction,
                               goal = tgoal),
            transitions = { 'succeeded': 'INIT_ARM_POSE' })

        
        smach.StateMachine.add(
            'INIT_ARM_POSE',
            ServiceState( '/traj_playback/hfa_untuck',
                          TrajPlaybackSrv,
                          request = TrajPlaybackSrvRequest( False )), 
            transitions = { 'succeeded':'MOVE_GET_OBJ' })
        

        go = MoveBaseGoal()
        
        go.target_pose.header.frame_id = '/map'
        go.target_pose.header.stamp = rospy.Time(0)
        go.target_pose.pose.position.x = -5.07
        go.target_pose.pose.position.y = 8.725
        go.target_pose.pose.orientation.z = 0.926
        go.target_pose.pose.orientation.w = 0.377

        smach.StateMachine.add(
            'MOVE_GET_OBJ',
            SimpleActionState( '/move_base',
                               MoveBaseAction,
                               goal = go ),
            transitions = {'succeeded':'READY_HANDOFF_INIT'})


        smach.StateMachine.add(
            'READY_HANDOFF_INIT',
            PrintStr('Hand me an object [ENTER]'), 
            transitions = { 'succeeded':'HANDOFF_INIT' })

        smach.StateMachine.add(
            'HANDOFF_INIT',
            ServiceState( 'rfid_handoff/initialize',
                          HandoffSrv,
                          request = HandoffSrvRequest()), 
            transitions = { 'succeeded':'CONFIRM_HANDOFF' })

        smach.StateMachine.add(
            'CONFIRM_HANDOFF',
            ConfirmObj('Does the robot have the object [\'yes\' to proceed]?'), 
            transitions = { 'succeeded':'ROTATE_AFTER_HANDOFF',
                            'aborted':'READY_HANDOFF_INIT'} )
        
        smach.StateMachine.add(
            'ROTATE_AFTER_HANDOFF',
            ServiceState( '/rotate_backup',
                          RotateBackupSrv,
                          request = RotateBackupSrvRequest( 3.14, 0.0)),  # Full 180-deg spin.
            transitions = { 'succeeded':'MOVE_DELIVERY' })

        gd = MoveBaseGoal()
        gd.target_pose.header.frame_id = '/map'
        gd.target_pose.header.stamp = rospy.Time(0)
        gd.target_pose.pose.position.x = 2.956
        gd.target_pose.pose.position.y = 3.047
        gd.target_pose.pose.orientation.z = 0.349
        gd.target_pose.pose.orientation.w = 0.937

        smach.StateMachine.add(
            'MOVE_DELIVERY',
            SimpleActionState( '/move_base',
                               MoveBaseAction,
                               goal = gd ),
            transitions = {'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': 'TUCKL_SUCC',
                            'aborted': 'TUCKL_ABOR'})

        smach.StateMachine.add(
            'TUCKL_SUCC',
            ServiceState( '/robotis/servo_left_pan_moveangle',
                          MoveAng,
                          request = MoveAngRequest( 1.350, 0.3, 0)), 
            transitions = { 'succeeded':'TUCKR_SUCC' })
        smach.StateMachine.add(
            'TUCKR_SUCC',
            ServiceState( '/robotis/servo_right_pan_moveangle',
                          MoveAng,
                          request = MoveAngRequest( -1.350, 0.3, 0)), 
            transitions = { 'succeeded':'BACKUP_AFTER_DELIVERY' })

        smach.StateMachine.add(
            'TUCKL_ABOR',
            ServiceState( '/robotis/servo_left_pan_moveangle',
                          MoveAng,
                          request = MoveAngRequest( 1.350, 0.3, 0)), 
            transitions = { 'succeeded':'TUCKR_ABOR' })
        smach.StateMachine.add(
            'TUCKR_ABOR',
            ServiceState( '/robotis/servo_right_pan_moveangle',
                          MoveAng,
                          request = MoveAngRequest( -1.350, 0.3, 0)), 
            transitions = { 'succeeded':'aborted' })


        smach.StateMachine.add(
            'BACKUP_AFTER_DELIVERY',
            ServiceState( '/rotate_backup',
                          RotateBackupSrv,
                          request = RotateBackupSrvRequest(0.0, -0.50)), 
            transitions = { 'succeeded':'ROTATE_AFTER_DELIVERY' })

        smach.StateMachine.add(
            'ROTATE_AFTER_DELIVERY',
            ServiceState( '/rotate_backup',
                          RotateBackupSrv,
                          request = RotateBackupSrvRequest( 3.14, 0.0)), 
            transitions = { 'succeeded':'MOVE_FINAL' })

        # Kitchen
        # gk = MoveBaseGoal()
        # gk.target_pose.header.frame_id = '/map'
        # gk.target_pose.header.stamp = rospy.Time(0)
        # gk.target_pose.pose.position.x = -1.61
        # gk.target_pose.pose.position.y =  0.88
        # gk.target_pose.pose.orientation.z = 0.91
        # gk.target_pose.pose.orientation.w = 0.40

        smach.StateMachine.add(
            'MOVE_FINAL',
            SimpleActionState( '/move_base',
                               MoveBaseAction,
                               goal = go ),  # Back down the hallway
            transitions = {'succeeded':'succeeded'})

        

            
    return sm
    def get_nav_prep_sm(self):
        nav_prep_sm = smach.StateMachine(outcomes=['succeeded','preempted','shutdown', 'aborted'])
        with nav_prep_sm:

            # make sure the robot is clear of obstacles
            # make sure the arms are tucked with
            # rosrun pr2_tuckarm tuck_arms.py r t l t
            # wait for the user to click on the head so the robot can approach
            smach.StateMachine.add(
                'WAIT_FOR_HEAD_CLICK',
                ClickMonitor(),
                transitions={'click' : 'PROCESS_NAV_POSE',
                             'shutdown' : 'shutdown'},
                remapping={'click_pose' : 'head_click_pose_global'}) # output (PoseStamped)

            # prepare the navigation pose for move_base
            # gets a point aligned with the normal and a distance away (nav_approach_dist)
            smach.StateMachine.add(
                'PROCESS_NAV_POSE',
                self.get_nav_approach_pose(),
                transitions={'succeeded' : 'MOVE_BASE', 
                             'tf_failure' : 'WAIT_FOR_HEAD_CLICK'},
                remapping={'head_click_pose' : 'head_click_pose_global', # input (PoseStamped)
                           'nav_pose_ps' : 'nav_pose_ps_global'}) # output (PoseStamped)

            # moves the base using nav stack to the appropirate location for moving forward
            smach.StateMachine.add(
                'MOVE_BASE',
                SimpleActionState('/move_base',
                                  MoveBaseAction,
                                  goal_slots=['target_pose'], # PoseStamped
                                  outcomes=['succeeded','aborted','preempted']),
                transitions = { 'succeeded' : 'CHECK_HEADING',
                                'aborted' : 'WAIT_FOR_HEAD_CLICK' },
                remapping = {'target_pose':'nav_pose_ps_global'}) # input (PoseStamped)

            # checks the current angle and returns the error
            smach.StateMachine.add(
                'CHECK_HEADING',
                CheckHeading( listener = self.tf_listener ),
                transitions = { 'succeeded':'ADJUST_HEADING' },
                remapping = { 'target_pose':'nav_pose_ps_global', # input (PoseStamped)
                              'angular_error':'angular_error' }) # output (float)

            # corrects for the error in angle
            smach.StateMachine.add(
                'ADJUST_HEADING',
                ServiceState( '/rotate_backup',
                              RotateBackupSrv,
                              request_slots = ['rotate']), # float (displace = 0.0)
                transitions = { 'succeeded':'MOVE_FORWARD_DIST' },
                remapping = {'rotate':'angular_error'})
            
            # approaches the touching position by moving forward nav_approach_dist meters
            # checks for collisions and will return shutdown if it detects a collsion
            smach.StateMachine.add(
                'MOVE_FORWARD_DIST',
                self.get_nav_approach(),
                transitions = {'succeeded' : 'TORSO_SETUP',
                               'shutdown' : 'shutdown'},
                remapping={'nav_dist' : 'nav_dist_global'})

            # move torso up
            tgoal = SingleJointPositionGoal()
            tgoal.position = 0.300  # all the way up is 0.300
            tgoal.min_duration = rospy.Duration( 2.0 )
            tgoal.max_velocity = 1.0
            smach.StateMachine.add(
                'TORSO_SETUP',
                SimpleActionState( 'torso_controller/position_joint_action',
                                   SingleJointPositionAction,
                                   goal = tgoal),
                transitions = { 'succeeded': 'UNFOLD' })

            # Unfolds the arms
            smach.StateMachine.add(
                'UNFOLD',
                ServiceState( 'traj_playback/unfold',
                              TrajPlaybackSrv,
                              request = TrajPlaybackSrvRequest( False )), # if True, reverse trajectory
                transitions = { 'succeeded':'succeeded' })

        return nav_prep_sm
        #     'MOVE_HALLWAY',
        #     SimpleActionState( '/move_base',
        #                        MoveBaseAction,
        #                        goal = go ),  # Back down the hallway
        #     transitions = {'succeeded':'succeeded'})

    return sm


if False:
    rospy.init_node('localization_trial')

    sm = smach.StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
    with sm:
        # Just a precation
        tgoal = SingleJointPositionGoal()
        tgoal.position = 0.040  # all the way up is 0.200
        tgoal.min_duration = rospy.Duration(2.0)
        tgoal.max_velocity = 1.0
        smach.StateMachine.add('TORSO_SETUP',
                               SimpleActionState(
                                   'torso_controller/position_joint_action',
                                   SingleJointPositionAction,
                                   goal=tgoal),
                               transitions={'succeeded': 'succeeded'})

    sm.execute()

if __name__ == '__main__':
    # if False:
    rospy.init_node('smach_aware_home')
Example #28
0
def set_torso(pos, wait=True):
    msg = SingleJointPositionGoal()
    msg.position = pos
    torso_client.send_goal(msg)
    if wait:
        torso_client.wait_for_result()
Example #29
0
        #                        MoveBaseAction,
        #                        goal = go ),  # Back down the hallway
        #     transitions = {'succeeded':'succeeded'})

        

            
    return sm

if False:
    rospy.init_node('localization_trial')

    sm = smach.StateMachine( outcomes=['succeeded','aborted','preempted'] )
    with sm:
        # Just a precation
        tgoal = SingleJointPositionGoal()
        tgoal.position = 0.040  # all the way up is 0.200
        tgoal.min_duration = rospy.Duration( 2.0 )
        tgoal.max_velocity = 1.0
        smach.StateMachine.add(
            'TORSO_SETUP',
            SimpleActionState( 'torso_controller/position_joint_action',
                               SingleJointPositionAction,
                               goal = tgoal),
            transitions = { 'succeeded': 'succeeded' })

    sm.execute()
    

if __name__ == '__main__':
# if False:
Example #30
0
#!/usr/bin/python
import roslib
roslib.load_manifest('pr2lite_arm_navigation')

import rospy
import actionlib
from pr2_controllers_msgs.msg import SingleJointPositionAction, SingleJointPositionGoal
rospy.init_node('torso_up')
torso_action_client = actionlib.SimpleActionClient(
    'torso_controller/position_joint_action', SingleJointPositionAction)
rospy.loginfo("torso_up: waiting for torso action server")
torso_action_client.wait_for_server()
rospy.loginfo("torso_up: torso action server found")
goal = SingleJointPositionGoal()
goal.position = 0.0
rospy.loginfo("torso_up: sending command to lift torso")
torso_action_client.send_goal(goal)
torso_action_client.wait_for_result(rospy.Duration(30))
Example #31
0
 def move_torso(self, pos):
     rospy.loginfo("Moving Torso to reach arm goal")
     goal_out = SingleJointPositionGoal()
     goal_out.position = pos
     self.torso_client.send_goal(goal_out)
     return True