def print_pd_dfs(dfs, names):
import pandas as pd
import datetime
folder_name = u'debug_matrices/{}'.format(datetime.datetime.now().strftime('%Y-%m-%d %H:%M:%S'))
for df, name in zip(dfs, names):
path = u'{}/{}.debug'.format(folder_name, name)
create_path(path)
with pd.option_context('display.max_rows', None, 'display.max_columns', None):
with open(path, 'w') as f:
f.write(df.to_csv())
def grow_tree():
action_server_name = u'giskardpy/command'
god_map = initialize_god_map()
# ----------------------------------------------
wait_for_goal = Sequence(u'wait for goal')
wait_for_goal.add_child(TFPlugin(u'tf'))
wait_for_goal.add_child(ConfigurationPlugin(u'js'))
wait_for_goal.add_child(WorldUpdatePlugin(u'pybullet updater'))
wait_for_goal.add_child(GoalReceived(u'has goal', action_server_name, MoveAction))
wait_for_goal.add_child(ConfigurationPlugin(u'js'))
# ----------------------------------------------
planning_3 = PluginBehavior(u'planning III', sleep=0)
planning_3.add_plugin(CollisionChecker(u'coll'))
planning_3.add_plugin(ControllerPlugin(u'controller'))
planning_3.add_plugin(KinSimPlugin(u'kin sim'))
planning_3.add_plugin(LogTrajPlugin(u'log'))
planning_3.add_plugin(GoalReachedPlugin(u'goal reached'))
planning_3.add_plugin(WiggleCancel(u'wiggle'))
planning_3.add_plugin(TimePlugin(u'time'))
# ----------------------------------------------
publish_result = failure_is_success(Selector)(u'monitor execution')
publish_result.add_child(GoalCanceled(u'goal canceled', action_server_name))
publish_result.add_child(SendTrajectory(u'send traj'))
# ----------------------------------------------
# ----------------------------------------------
planning_2 = failure_is_success(Selector)(u'planning II')
planning_2.add_child(GoalCanceled(u'goal canceled', action_server_name))
# planning.add_child(CollisionCancel(u'in collision', collision_time_threshold))
if god_map.safe_get_data(identifier.marker_visualization):
planning_2.add_child(success_is_failure(VisualizationBehavior)(u'visualization'))
if god_map.safe_get_data(identifier.enable_collision_marker):
planning_2.add_child(success_is_failure(CPIMarker)(u'cpi marker'))
planning_2.add_child(planning_3)
# ----------------------------------------------
move_robot = failure_is_success(Sequence)(u'move robot')
move_robot.add_child(IF(u'execute?', identifier.execute))
move_robot.add_child(publish_result)
# ----------------------------------------------
# ----------------------------------------------
planning_1 = success_is_failure(Sequence)(u'planning I')
planning_1.add_child(GoalToConstraints(u'update constraints', action_server_name))
planning_1.add_child(planning_2)
# ----------------------------------------------
# ----------------------------------------------
process_move_goal = failure_is_success(Selector)(u'process move goal')
process_move_goal.add_child(planning_1)
process_move_goal.add_child(SetCmd(u'set move goal', action_server_name))
# ----------------------------------------------
# ----------------------------------------------
root = Sequence(u'root')
root.add_child(wait_for_goal)
root.add_child(CleanUp(u'cleanup'))
root.add_child(process_move_goal)
if god_map.safe_get_data(identifier.plot_trajectory):
root.add_child(PlotTrajectory(u'plot trajectory'))
root.add_child(move_robot)
root.add_child(SendResult(u'send result', action_server_name, MoveAction))
tree = BehaviourTree(root)
if god_map.safe_get_data(identifier.debug):
def post_tick(snapshot_visitor, behaviour_tree):
logging.logdebug(u'\n' + py_trees.display.ascii_tree(behaviour_tree.root,
snapshot_information=snapshot_visitor))
snapshot_visitor = py_trees_ros.visitors.SnapshotVisitor()
tree.add_post_tick_handler(functools.partial(post_tick, snapshot_visitor))
tree.visitors.append(snapshot_visitor)
path = god_map.safe_get_data(identifier.data_folder) + u'tree'
create_path(path)
render_dot_tree(root, name=path)
tree.setup(30)
return tree
def safe_compiled_function(f, file_name):
create_path(file_name)
with open(file_name, 'w') as file:
pickle.dump(f, file)
logging.loginfo(u'saved {}'.format(file_name))
def grow_tree():
action_server_name = u'~command'
god_map = initialize_god_map()
# ----------------------------------------------
wait_for_goal = Sequence(u'wait for goal')
wait_for_goal.add_child(TFPlugin(u'tf'))
wait_for_goal.add_child(ConfigurationPlugin(u'js1'))
wait_for_goal.add_child(WorldUpdatePlugin(u'pybullet updater'))
wait_for_goal.add_child(GoalReceived(u'has goal', action_server_name, MoveAction))
wait_for_goal.add_child(ConfigurationPlugin(u'js2'))
# ----------------------------------------------
planning_3 = PluginBehavior(u'planning III', sleep=0)
planning_3.add_plugin(CollisionChecker(u'coll'))
# if god_map.safe_get_data(identifier.enable_collision_marker):
# planning_3.add_plugin(success_is_running(CPIMarker)(u'cpi marker'))
planning_3.add_plugin(ControllerPlugin(u'controller'))
planning_3.add_plugin(KinSimPlugin(u'kin sim'))
planning_3.add_plugin(LogTrajPlugin(u'log'))
planning_3.add_plugin(WiggleCancel(u'wiggle'))
planning_3.add_plugin(LoopDetector(u'loop detector'))
planning_3.add_plugin(GoalReachedPlugin(u'goal reached'))
planning_3.add_plugin(TimePlugin(u'time'))
# planning_3.add_plugin(MaxTrajLength(u'traj length check'))
# ----------------------------------------------
publish_result = failure_is_success(Selector)(u'monitor execution')
publish_result.add_child(GoalCanceled(u'goal canceled', action_server_name))
publish_result.add_child(SendTrajectory(u'send traj'))
# ----------------------------------------------
# ----------------------------------------------
planning_2 = failure_is_success(Selector)(u'planning II')
planning_2.add_child(GoalCanceled(u'goal canceled', action_server_name))
if god_map.get_data(identifier.enable_VisualizationBehavior):
planning_2.add_child(success_is_failure(VisualizationBehavior)(u'visualization'))
if god_map.get_data(identifier.enable_WorldVisualizationBehavior):
planning_2.add_child(success_is_failure(WorldVisualizationBehavior)(u'world_visualization'))
if god_map.get_data(identifier.enable_CPIMarker):
planning_2.add_child(success_is_failure(CollisionMarker)(u'cpi marker'))
planning_2.add_child(planning_3)
# ----------------------------------------------
move_robot = failure_is_success(Sequence)(u'move robot')
move_robot.add_child(IF(u'execute?', identifier.execute))
move_robot.add_child(publish_result)
# ----------------------------------------------
# ----------------------------------------------
planning_1 = Sequence(u'planning I')
planning_1.add_child(GoalToConstraints(u'update constraints', action_server_name))
planning_1.add_child(planning_2)
planning_1.add_child(running_is_success(TimePlugin)(u'time for zero velocity'))
planning_1.add_child(AppendZeroVelocity(u'append zero velocity'))
planning_1.add_child(running_is_success(LogTrajPlugin)(u'log zero velocity'))
# planning_1.add_child(running_is_success(TimePlugin)(u'time for zero velocity'))
# planning_1.add_child(AppendZeroVelocity(u'append zero velocity'))
# planning_1.add_child(running_is_success(LogTrajPlugin)(u'log zero velocity'))
if god_map.get_data(identifier.enable_VisualizationBehavior):
planning_1.add_child(VisualizationBehavior(u'visualization', ensure_publish=True))
if god_map.get_data(identifier.enable_WorldVisualizationBehavior):
planning_1.add_child(WorldVisualizationBehavior(u'world_visualization', ensure_publish=True))
if god_map.get_data(identifier.enable_CPIMarker):
planning_1.add_child(CollisionMarker(u'cpi marker'))
# ----------------------------------------------
post_processing = failure_is_success(Sequence)(u'post planning')
# post_processing.add_child(WiggleCancel(u'final wiggle detection', final_detection=True))
if god_map.get_data(identifier.enable_PlotTrajectory):
post_processing.add_child(PlotTrajectory(u'plot trajectory', order=3))
post_processing.add_child(PostProcessing(u'evaluate result'))
# post_processing.add_child(PostProcessing(u'check reachability'))
# ----------------------------------------------
planning = success_is_failure(Sequence)(u'planning')
planning.add_child(IF(u'goal_set?', identifier.next_move_goal))
planning.add_child(planning_1)
planning.add_child(post_processing)
process_move_goal = failure_is_success(Selector)(u'process move goal')
process_move_goal.add_child(planning)
# process_move_goal.add_child(planning_1)
# process_move_goal.add_child(post_processing)
process_move_goal.add_child(SetCmd(u'set move goal', action_server_name))
# ----------------------------------------------
#
# post_processing = failure_is_success(Sequence)(u'post processing')
# post_processing.add_child(PostProcessing(u'post_processing'))
# ----------------------------------------------
# ----------------------------------------------
root = Sequence(u'root')
root.add_child(wait_for_goal)
root.add_child(CleanUp(u'cleanup'))
root.add_child(process_move_goal)
root.add_child(move_robot)
root.add_child(SendResult(u'send result', action_server_name, MoveAction))
tree = BehaviourTree(root)
if god_map.get_data(identifier.debug):
def post_tick(snapshot_visitor, behaviour_tree):
logging.logdebug(u'\n' + py_trees.display.ascii_tree(behaviour_tree.root,
snapshot_information=snapshot_visitor))
snapshot_visitor = py_trees_ros.visitors.SnapshotVisitor()
tree.add_post_tick_handler(functools.partial(post_tick, snapshot_visitor))
tree.visitors.append(snapshot_visitor)
path = god_map.get_data(identifier.data_folder) + u'tree'
create_path(path)
render_dot_tree(root, name=path)
tree.setup(30)
tree_m = TreeManager(tree)
god_map.set_data(identifier.tree_manager, tree_m)
return tree