def get_goals(self):
"""Uses a simple metric to let the farthest possible goals have a
usability near 1 and nearer goals a smaller one"""
distance_max = 2
random_pose_tuples = [calc_random_pose_tuple(1, distance_max=distance_max)
for _ in xrange(10)]
# # debug usability:
# for x, y, yaw in random_pose_tuples:
# distance = math.sqrt(x * x + y * y)
# usability = distance / distance_max
# print "distance=%s, usability=%s" % (distance, usability)
goals = [MoveToPoseGoal(position_tuple_to_pose(x, y, yaw),
'/base_link',
usability=math.sqrt(x * x + y * y) / distance_max)
for x, y, yaw in random_pose_tuples]
return goals
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)
def _build_goal(self, userdata):
pose = position_tuple_to_pose(userdata.x, userdata.y, userdata.yaw)
return Goal([Precondition(Condition.get('robot.pose'), pose)]) # TODO: prepared goal available: MoveToPoseGoal
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 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)
