Ejemplo n.º 1
0
 def __init__(self, pose, frame, usability):
     if frame != '/map' and frame != 'map':
         if MoveToPoseGoal.tl is None:
             MoveToPoseGoal.tl = tf.TransformListener()
             rospy.sleep(1)
         pose_stamped = PoseStamped()
         pose_stamped.header.frame_id = frame
         pose_stamped.pose = pose
         #tf.Transformer().waitForTransform('/map', frame)
         pose_stmpd_in_map = MoveToPoseGoal.tl.transformPose('/map', pose_stamped)
         pose = pose_stmpd_in_map.pose
     Goal.__init__(self, [Precondition(Condition.get('robot.pose'), pose)], usability)
Ejemplo n.º 2
0
 def __init__(self, pose, frame, usability):
     if frame != '/map' and frame != 'map':
         if MoveToPoseGoal.tl is None:
             MoveToPoseGoal.tl = tf.TransformListener()
             rospy.sleep(1)
         pose_stamped = PoseStamped()
         pose_stamped.header.frame_id = frame
         pose_stamped.pose = pose
         #tf.Transformer().waitForTransform('/map', frame)
         pose_stmpd_in_map = MoveToPoseGoal.tl.transformPose(
             '/map', pose_stamped)
         pose = pose_stmpd_in_map.pose
     Goal.__init__(self, [Precondition(Condition.get('robot.pose'), pose)],
                   usability)
Ejemplo n.º 3
0
    def testRunner(self):
        memory = self.runner.memory
        Condition.add(MemoryCondition(memory, 'awareness', 0))
        Condition.add(MemoryCondition(memory, 'arm_can_move', True))

        self.runner.actions.add(LookAroundAction())
        print self.runner.actions

        goal = Goal([Precondition(Condition.get('awareness'), 2)])
        self.runner.update_and_plan_and_execute(goal)
Ejemplo n.º 4
0
    runner.memory.set_value('memory.reminded_myself', 333)

    rospy.loginfo("Waiting to let conditions represent reality...")
    rospy.loginfo(
        "Remember to start topic publishers so conditions make sense instead of None!"
    )
    rospy.sleep(2)
    Condition.initialize_worldstate(runner.worldstate)
    rospy.loginfo("worldstate now is: %s", runner.worldstate)

    runner.actions.add(
        MemoryChangeVarAction(runner.memory, 'memory.reminded_myself', 333,
                              555))

    goal = Goal([
        Precondition(Condition.get('robot.pose'),
                     position_tuple_to_pose(1, 0, 0)),
        Precondition(Condition.get('memory.reminded_myself'), 555)
    ])

    start_node = runner.update_and_plan(goal, tries=2, introspection=True)

    rospy.loginfo("start_node: %s", start_node)

    if start_node is None:
        rospy.logwarn("No plan found! Check your ROS graph!")
    else:
        runner.execute_as_smach(start_node, introspection=True)

    rospy.sleep(20)
Ejemplo n.º 5
0
    def __init__(self):
        Action.__init__(self,
                        [Precondition(Condition.get('robot.bumpered'), True)],
                        [Effect(Condition.get('robot.bumpered'), False)])

    def run(self, next_worldstate):
        print '%s: resetting bumper..' % self.__class__


if __name__ == "__main__":

    runner = Runner()

    Condition.add(MemoryCondition(runner.memory, 'robot.bumpered', True))

    runner.actions.add(SymmetricAction())

    Condition.initialize_worldstate(runner.worldstate)
    print 'worldstate now is: ', runner.worldstate

    goal = Goal([Precondition(Condition.get('robot.bumpered'), False)])

    start_node = runner.update_and_plan(goal, introspection=True)

    print 'start_node: ', start_node

    if start_node is None:
        print 'No plan found! Check your ROS graph!'
    else:
        runner.execute(start_node)
Ejemplo n.º 6
0
    def testStateWrapperAction(self):
        # The in-value is translated into the SMACHStateWrapperAction
        # and checked inside the wrapped state to check data translation
        # to the wrapped state.
        NUMBER_IN = 123
        # The out-value is translated into the SMACHStateWrapperAction
        # and out of it again to check data translation from the wrapped
        # state and also the next_worldstate parameter.
        NUMBER_OUT = 456

        Condition.add(
            MemoryCondition(self.runner.memory, 'memory.in', NUMBER_IN))
        Condition.add(MemoryCondition(self.runner.memory, 'memory.out', -1))

        class ValueLooperState(State):
            def __init__(self):
                State.__init__(self, ['succeeded', 'aborted'],
                               input_keys=['i', 'to_loop'],
                               output_keys=['o'])

            def execute(self, userdata):
                print "%s found 'i' in userdata to be %s.." % (
                    self.__class__.__name__, userdata.i)
                if userdata.i == NUMBER_IN:
                    print "..which is correct"
                    userdata.o = userdata.to_loop
                    #return 'succeeded' # will only work when using rgoap_ros.SMACHRunner
                else:
                    print "..which is not correct!"
                    #return 'aborted' # see above

        class TranslateAction(SMACHStateWrapperAction):
            def __init__(self):
                SMACHStateWrapperAction.__init__(self, ValueLooperState(), [
                    Precondition(Condition.get('memory.in'), NUMBER_IN),
                    Precondition(Condition.get('memory.out'), -1)
                ], [Effect(Condition.get('memory.out'), NUMBER_OUT)])

            def translate_worldstate_to_userdata(self, next_worldstate,
                                                 userdata):
                userdata.i = next_worldstate.get_condition_value(
                    Condition.get('memory.in'))
                userdata.to_loop = next_worldstate.get_condition_value(
                    Condition.get('memory.out'))

            def translate_userdata_to_worldstate(self, userdata,
                                                 next_worldstate):
                print "FIXME: translation from userdata does not work"
                # FIXME: translation from userdata does not work


#                # memory.set_value('memory.out', userdata.o)
#                next_worldstate.set_condition_value(Condition.get('memory.out'), userdata.o)

        self.runner.actions.add(TranslateAction())

        goal = Goal([
            Precondition(Condition.get('memory.out'), NUMBER_OUT),
            # memory.in is added to goal to be available in goal/next_worldstate
            Precondition(Condition.get('memory.in'), NUMBER_IN)
        ])

        plan = self.runner.update_and_plan(goal)
        print "plan:", plan
        self.assertIsNotNone(plan, "plan should not be None")

        success = self.runner.execute(plan)
        print "success: ", success
        self.assertTrue(success, "executing plan was not successful")
        # self.assertEqual(outcome, 'succeeded', "executing plan did not return succeeded")

        mem_out = self.runner.memory.get_value('memory.out')
        print "memory.out = %s" % mem_out

        self.assertEqual(mem_out, NUMBER_OUT,
                         "out variable in memory is not changed successfully")
Ejemplo n.º 7
0
 def __init__(self):
     Goal.__init__(self, [Precondition(Condition.get('awareness'), 1)], 0.6)
Ejemplo n.º 8
0
 def __init__(self):
     Goal.__init__(self, [Precondition(Condition.get('awareness'), 1)], 0.6)