rospy.init_node('rgoap_bumper_test', log_level=rospy.INFO)
runner = SMACHRunner(rgoap_subclasses)
Condition.add(MemoryCondition(runner.memory, 'memory.reminded_myself'))
runner.memory.set_value('awareness', 0)
runner.memory.set_value('arm_can_move', True)
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 __name__ == "__main__":
rospy.init_node("rgoap_bumper_test", log_level=rospy.INFO)
runner = SMACHRunner(rgoap_subclasses)
Condition.add(MemoryCondition(runner.memory, "memory.reminded_myself"))
runner.memory.set_value("awareness", 0)
runner.memory.set_value("arm_can_move", True)
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)
