introspection.py
Go to the documentation of this file.
00001 # Copyright (c) 2013, Felix Kolbe
00002 # All rights reserved. BSD License
00003 #
00004 # Redistribution and use in source and binary forms, with or without
00005 # modification, are permitted provided that the following conditions
00006 # are met:
00007 #
00008 # * Redistributions of source code must retain the above copyright
00009 #   notice, this list of conditions and the following disclaimer.
00010 #
00011 # * Redistributions in binary form must reproduce the above copyright
00012 #   notice, this list of conditions and the following disclaimer in the
00013 #   documentation and/or other materials provided with the distribution.
00014 #
00015 # * Neither the name of the {organization} nor the names of its
00016 #   contributors may be used to endorse or promote products derived
00017 #   from this software without specific prior written permission.
00018 #
00019 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00020 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00021 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
00022 # A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
00023 # HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
00024 # SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
00025 # LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
00026 # DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
00027 # THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00028 # (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
00029 # OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00030 
00031 
00032 import roslib; roslib.load_manifest('rgoap_ros')
00033 import rospy
00034 import pickle
00035 
00036 from smach_msgs.msg import SmachContainerStatus, SmachContainerStructure
00037 from smach_ros.introspection import STATUS_TOPIC, STRUCTURE_TOPIC
00038 
00039 
00040 import logging
00041 _logger = logging.getLogger('rgoap.ros.introspection')
00042 
00043 
00044 
00045 class Introspector(object):
00046     """Gives insight to a RGOAP planner's plan and planning graph by
00047     publishing prepared information to a smach_viewer.
00048     """
00049     def __init__(self):
00050         self._pathprefix = '/RGOAP_PLAN'
00051         self._pathprefix_net = '/RGOAP_NET'
00052         self._publisher_structure = rospy.Publisher(self._pathprefix + STRUCTURE_TOPIC, SmachContainerStructure, latch=True)
00053         self._publisher_status = rospy.Publisher(self._pathprefix + STATUS_TOPIC, SmachContainerStatus, latch=True)
00054         self._publisher_structure_net = rospy.Publisher(self._pathprefix_net + STRUCTURE_TOPIC, SmachContainerStructure, latch=True)
00055         self._publisher_status_net = rospy.Publisher(self._pathprefix_net + STATUS_TOPIC, SmachContainerStatus, latch=True)
00056 
00057 
00058     def publish_net(self, goal_node, start_node=None):
00059         """Publishes an RGOAP planning net, reconstructing it from the goal node.
00060 
00061         The goal node will be set as the active state, as its userdata is displayed.
00062         """
00063         def _add_nodes_recursively(node, structure):
00064             """node: beginning with the goal node"""
00065             structure.children.append(self._nodeid(node))
00066 
00067             if len(node.possible_prev_nodes) > 0: # goal or inner node
00068                 for prev_node in node.possible_prev_nodes:
00069                     structure.internal_outcomes.append('\n'.join(str(e) for e in prev_node.action._effects))
00070                     structure.outcomes_from.append(self._nodeid(prev_node))
00071                     structure.outcomes_to.append(self._nodeid(node))
00072                     _add_nodes_recursively(prev_node, structure)
00073             else: # start or dead-end node
00074                 pass
00075 
00076 #            if len(node.parent_nodes_path_list) == 0: # goal node
00077 #                structure.internal_outcomes.append('succeeded')
00078 #                structure.outcomes_from.append(self._nodeid(node))
00079 #                structure.outcomes_to.append('None')
00080 #            else:
00081 #                structure.internal_outcomes.append('aborted')
00082 #                structure.outcomes_from.append(self._nodeid(node))
00083 #                structure.outcomes_to.append('None')
00084 
00085         structure = SmachContainerStructure()
00086         structure.header.stamp = rospy.Time.now()
00087         structure.path = self._pathprefix_net
00088 #        structure.container_outcomes = ['succeeded', 'aborted']
00089         _add_nodes_recursively(goal_node, structure)
00090         self._publisher_structure_net.publish(structure)
00091         _logger.info("Introspector published net with ~%s nodes", len(structure.children))
00092 
00093         status = SmachContainerStatus()
00094         status.header.stamp = rospy.Time.now()
00095         status.path = self._pathprefix_net
00096         status.initial_states = [self._nodeid(start_node) if start_node is not None else 'No plan found']
00097         status.active_states = [self._nodeid(goal_node)]
00098         goal_states_dict = goal_node.worldstate.get_state_name_dict()
00099         goal_states_dict['WORLDSTATE'] = 'GOAL'
00100         status.local_data = pickle.dumps(goal_states_dict, 2)
00101         status.info = 'goal state'
00102         self._publisher_status_net.publish(status)
00103 
00104         rospy.sleep(5)
00105 
00106     def publish(self, start_node, pathprefix=None):
00107         """Publishes a planned RGOAP plan
00108 
00109         The start node will be set as the active state, as its userdata is displayed.
00110         """
00111         def _add_nodes_recursively(node, structure):
00112             """node: beginning with the start node"""
00113             structure.children.append(self._nodeid(node))
00114 
00115             if not node.is_goal():
00116                 next_node = node.parent_node()
00117 
00118                 structure.internal_outcomes.append('\n'.join(str(e) for e in node.action._effects))
00119                 structure.outcomes_from.append(self._nodeid(node))
00120                 structure.outcomes_to.append(self._nodeid(next_node))
00121 #
00122 #                structure.internal_outcomes.append('aborted')
00123 #                structure.outcomes_from.append(self._nodeid(node))
00124 #                structure.outcomes_to.append('None')
00125 
00126                 _add_nodes_recursively(next_node, structure)
00127 #            else: # goal node
00128 #                structure.internal_outcomes.append('succeeded')
00129 #                structure.outcomes_from.append(self._nodeid(node))
00130 #                structure.outcomes_to.append('None')
00131 
00132         structure = SmachContainerStructure()
00133         structure.header.stamp = rospy.Time.now()
00134         structure.path = self._pathprefix if pathprefix is None else pathprefix
00135 #        structure.container_outcomes = ['succeeded', 'aborted']
00136         _add_nodes_recursively(start_node, structure)
00137         self._publisher_structure.publish(structure)
00138         _logger.info("Introspector published plan with ~%s nodes", len(structure.children))
00139 
00140         status = SmachContainerStatus()
00141         status.header.stamp = rospy.Time.now()
00142         status.path = self._pathprefix
00143         status.initial_states = [self._nodeid(start_node)]
00144         status.active_states = [self._nodeid(start_node)]
00145         start_states_dict = start_node.worldstate.get_state_name_dict()
00146         start_states_dict['WORLDSTATE'] = 'START'
00147         status.local_data = pickle.dumps(start_states_dict, 2)
00148         status.info = 'initial state'
00149         self._publisher_status.publish(status)
00150 
00151 #        for node in start_node.parent_nodes_path_list:
00152 #            _logger.info("Introspector published update for node: %s", node)
00153 #            self.publish_update(node)
00154 
00155     def publish_update(self, node):
00156         nodeid = self._nodeid(node)
00157         status = SmachContainerStatus()
00158         status.header.stamp = rospy.Time.now()
00159         status.path = self._pathprefix + '/' + nodeid
00160         status.initial_states = []
00161         status.active_states = [self._nodeid(node)]
00162         start_states_dict = node.worldstate.get_state_name_dict()
00163         start_states_dict['WORLDSTATE'] = str(node) # id?
00164         status.local_data = pickle.dumps(start_states_dict, 2)
00165         status.info = 'node state'
00166         self._publisher_status.publish(status)
00167 
00168 
00169     def _nodeid(self, node):
00170         if node.is_goal():
00171             node_action_name = 'GOAL'
00172         else:
00173             node_action_name = str(node.action) #.__class__.__name__
00174         return (node_action_name + ' %X' % id(node) +
00175                 ' n%s' % node.cost() +
00176                 ' p%s' % node.path_cost() +
00177                 ' h%s' % node.heuristic_distance +
00178                 ' t%s' % node.total_cost())


rgoap_ros
Author(s): Felix Kolbe
autogenerated on Sun Oct 5 2014 23:53:07