def _publish_status(self, info_str=''):
"""Publish current state of this container."""
# Construct messages
with self._status_pub_lock:
path = self._path
#print str(structure_msg)
# Construct status message
#print self._container.get_active_states()
# Guard against non-pickleable user data
data_dict = {}
for key, value in self._container.userdata._data.items():
try:
data = pickle.dumps({key: value}, 2)
data_dict[key] = value
except:
#rospy.logwarn('Unable to Pickle user data')
data_dict[key] = 'unknown'
data = pickle.dumps(data_dict, 2)
state_msg = SmachContainerStatus(
Header(stamp=rospy.Time.now()), path,
self._container.get_initial_states(),
self._container.get_active_states(),
pickle.dumps(data_dict, 2), info_str)
# Publish message
self._status_pub.publish(state_msg)
def publish_update(self, node):
nodeid = self._nodeid(node)
status = SmachContainerStatus()
status.header.stamp = rospy.Time.now()
status.path = self._pathprefix + '/' + nodeid
status.initial_states = []
status.active_states = [self._nodeid(node)]
start_states_dict = node.worldstate.get_state_name_dict()
start_states_dict['WORLDSTATE'] = str(node) # id?
status.local_data = pickle.dumps(start_states_dict, 2)
status.info = 'node state'
self._publisher_status.publish(status)
def publish_net(self, goal_node, start_node=None):
"""Publishes an RGOAP planning net, reconstructing it from the goal node.
The goal node will be set as the active state, as its userdata is displayed.
"""
def _add_nodes_recursively(node, structure):
"""node: beginning with the goal node"""
structure.children.append(self._nodeid(node))
if len(node.possible_prev_nodes) > 0: # goal or inner node
for prev_node in node.possible_prev_nodes:
structure.internal_outcomes.append('\n'.join(
str(e) for e in prev_node.action._effects))
structure.outcomes_from.append(self._nodeid(prev_node))
structure.outcomes_to.append(self._nodeid(node))
_add_nodes_recursively(prev_node, structure)
else: # start or dead-end node
pass
# if len(node.parent_nodes_path_list) == 0: # goal node
# structure.internal_outcomes.append('succeeded')
# structure.outcomes_from.append(self._nodeid(node))
# structure.outcomes_to.append('None')
# else:
# structure.internal_outcomes.append('aborted')
# structure.outcomes_from.append(self._nodeid(node))
# structure.outcomes_to.append('None')
structure = SmachContainerStructure()
structure.header.stamp = rospy.Time.now()
structure.path = self._pathprefix_net
# structure.container_outcomes = ['succeeded', 'aborted']
_add_nodes_recursively(goal_node, structure)
self._publisher_structure_net.publish(structure)
_logger.info("Introspector published net with ~%s nodes",
len(structure.children))
status = SmachContainerStatus()
status.header.stamp = rospy.Time.now()
status.path = self._pathprefix_net
status.initial_states = [
self._nodeid(start_node)
if start_node is not None else 'No plan found'
]
status.active_states = [self._nodeid(goal_node)]
goal_states_dict = goal_node.worldstate.get_state_name_dict()
goal_states_dict['WORLDSTATE'] = 'GOAL'
status.local_data = pickle.dumps(goal_states_dict, 2)
status.info = 'goal state'
self._publisher_status_net.publish(status)
rospy.sleep(5)
def _publish_status(self, info_str=''):
"""Publish current state of this container."""
# Construct messages
with self._status_pub_lock:
path = self._path
#print str(structure_msg)
# Construct status message
#print self._container.get_active_states()
state_msg = SmachContainerStatus(
Header(stamp=rospy.Time.now()), path,
self._container.get_initial_states(),
self._container.get_active_states(),
pickle.dumps(self._container.userdata._data, 2), info_str)
# Publish message
self._status_pub.publish(state_msg)
def publish_update(self, node):
nodeid = self._nodeid(node)
status = SmachContainerStatus()
status.header.stamp = rospy.Time.now()
status.path = self._pathprefix + '/' + nodeid
status.initial_states = []
status.active_states = [self._nodeid(node)]
start_states_dict = node.worldstate.get_state_name_dict()
start_states_dict['WORLDSTATE'] = str(node) # id?
status.local_data = pickle.dumps(start_states_dict, 2)
status.info = 'node state'
self._publisher_status.publish(status)
def publish(self, start_node, pathprefix=None):
"""Publishes a planned RGOAP plan
The start node will be set as the active state, as its userdata is displayed.
"""
def _add_nodes_recursively(node, structure):
"""node: beginning with the start node"""
structure.children.append(self._nodeid(node))
if not node.is_goal():
next_node = node.parent_node()
structure.internal_outcomes.append('\n'.join(
str(e) for e in node.action._effects))
structure.outcomes_from.append(self._nodeid(node))
structure.outcomes_to.append(self._nodeid(next_node))
#
# structure.internal_outcomes.append('aborted')
# structure.outcomes_from.append(self._nodeid(node))
# structure.outcomes_to.append('None')
_add_nodes_recursively(next_node, structure)
# else: # goal node
# structure.internal_outcomes.append('succeeded')
# structure.outcomes_from.append(self._nodeid(node))
# structure.outcomes_to.append('None')
structure = SmachContainerStructure()
structure.header.stamp = rospy.Time.now()
structure.path = self._pathprefix if pathprefix is None else pathprefix
# structure.container_outcomes = ['succeeded', 'aborted']
_add_nodes_recursively(start_node, structure)
self._publisher_structure.publish(structure)
_logger.info("Introspector published plan with ~%s nodes",
len(structure.children))
status = SmachContainerStatus()
status.header.stamp = rospy.Time.now()
status.path = self._pathprefix
status.initial_states = [self._nodeid(start_node)]
status.active_states = [self._nodeid(start_node)]
start_states_dict = start_node.worldstate.get_state_name_dict()
start_states_dict['WORLDSTATE'] = 'START'
status.local_data = pickle.dumps(start_states_dict, 2)
status.info = 'initial state'
self._publisher_status.publish(status)
def _publish_status(self, info_str=''):
"""Publish current state of this container."""
# Construct messages
with self._status_pub_lock:
path = self._path
#print str(structure_msg)
# Construct status message
#print self._container.get_active_states()
state_msg = SmachContainerStatus(
header=Header(stamp=ROSClock().now().to_msg()),
path=path,
initial_states=self._container.get_initial_states(),
active_states=self._container.get_active_states(),
local_data=bytearray(
pickle.dumps(self._container.userdata._data, 2)),
info=info_str)
# Publish message
self._status_pub.publish(state_msg)
def publish_net(self, goal_node, start_node=None):
"""Publishes an RGOAP planning net, reconstructing it from the goal node.
The goal node will be set as the active state, as its userdata is displayed.
"""
def _add_nodes_recursively(node, structure):
"""node: beginning with the goal node"""
structure.children.append(self._nodeid(node))
if len(node.possible_prev_nodes) > 0: # goal or inner node
for prev_node in node.possible_prev_nodes:
structure.internal_outcomes.append('\n'.join(str(e) for e in prev_node.action._effects))
structure.outcomes_from.append(self._nodeid(prev_node))
structure.outcomes_to.append(self._nodeid(node))
_add_nodes_recursively(prev_node, structure)
else: # start or dead-end node
pass
# if len(node.parent_nodes_path_list) == 0: # goal node
# structure.internal_outcomes.append('succeeded')
# structure.outcomes_from.append(self._nodeid(node))
# structure.outcomes_to.append('None')
# else:
# structure.internal_outcomes.append('aborted')
# structure.outcomes_from.append(self._nodeid(node))
# structure.outcomes_to.append('None')
structure = SmachContainerStructure()
structure.header.stamp = rospy.Time.now()
structure.path = self._pathprefix_net
# structure.container_outcomes = ['succeeded', 'aborted']
_add_nodes_recursively(goal_node, structure)
self._publisher_structure_net.publish(structure)
_logger.info("Introspector published net with ~%s nodes", len(structure.children))
status = SmachContainerStatus()
status.header.stamp = rospy.Time.now()
status.path = self._pathprefix_net
status.initial_states = [self._nodeid(start_node) if start_node is not None else 'No plan found']
status.active_states = [self._nodeid(goal_node)]
goal_states_dict = goal_node.worldstate.get_state_name_dict()
goal_states_dict['WORLDSTATE'] = 'GOAL'
status.local_data = pickle.dumps(goal_states_dict, 2)
status.info = 'goal state'
self._publisher_status_net.publish(status)
rospy.sleep(5)
def publish(self, start_node, pathprefix=None):
"""Publishes a planned RGOAP plan
The start node will be set as the active state, as its userdata is displayed.
"""
def _add_nodes_recursively(node, structure):
"""node: beginning with the start node"""
structure.children.append(self._nodeid(node))
if not node.is_goal():
next_node = node.parent_node()
structure.internal_outcomes.append('\n'.join(str(e) for e in node.action._effects))
structure.outcomes_from.append(self._nodeid(node))
structure.outcomes_to.append(self._nodeid(next_node))
#
# structure.internal_outcomes.append('aborted')
# structure.outcomes_from.append(self._nodeid(node))
# structure.outcomes_to.append('None')
_add_nodes_recursively(next_node, structure)
# else: # goal node
# structure.internal_outcomes.append('succeeded')
# structure.outcomes_from.append(self._nodeid(node))
# structure.outcomes_to.append('None')
structure = SmachContainerStructure()
structure.header.stamp = rospy.Time.now()
structure.path = self._pathprefix if pathprefix is None else pathprefix
# structure.container_outcomes = ['succeeded', 'aborted']
_add_nodes_recursively(start_node, structure)
self._publisher_structure.publish(structure)
_logger.info("Introspector published plan with ~%s nodes", len(structure.children))
status = SmachContainerStatus()
status.header.stamp = rospy.Time.now()
status.path = self._pathprefix
status.initial_states = [self._nodeid(start_node)]
status.active_states = [self._nodeid(start_node)]
start_states_dict = start_node.worldstate.get_state_name_dict()
start_states_dict['WORLDSTATE'] = 'START'
status.local_data = pickle.dumps(start_states_dict, 2)
status.info = 'initial state'
self._publisher_status.publish(status)
def set_initial_state(self,
server,
path,
initial_states,
initial_userdata=smach.UserData(),
timeout=None):
"""Set the initial state of a smach server.
@type server: string
@param server: The name of the introspection server to which this client
should connect.
@type path: string
@param path: The path to the target container in the state machine.
@type initial_states: list of string
@param inital_state: The state the target container should take when it
starts. This is as list of at least one state label.
@type initial_userdata: UserData
@param initial_userdata: The userdata to inject into the target container.
@type timeout: rospy.Duration
@param timeout: Timeout for this call. If this is set to None, it will not
block, and the initial state may not be set before the target state machine
goes active.
"""
# Construct initial state command
initial_status_msg = SmachContainerInitialStatusCmd(
path=path,
initial_states=initial_states,
local_data=pickle.dumps(initial_userdata._data, 2))
# A status message to receive confirmation that the state was set properly
msg_response = SmachContainerStatus()
# Define a local callback to just stuff a local message
def local_cb(msg, msg_response):
rospy.logdebug("Received status response: " + str(msg))
msg_response.path = msg.path
msg_response.initial_states = msg.initial_states
msg_response.local_data = msg.local_data
# Create a subscriber to verify the request went through
state_sub = rospy.Subscriber(server + STATUS_TOPIC,
SmachContainerStatus,
callback=local_cb,
callback_args=msg_response)
# Create a publisher to send the command
rospy.logdebug("Sending initial state command: " +
str(initial_status_msg.path) + " on topic '" + server +
INIT_TOPIC + "'")
init_pub = rospy.Publisher(server + INIT_TOPIC,
SmachContainerInitialStatusCmd,
queue_size=1)
init_pub.publish(initial_status_msg)
start_time = rospy.Time.now()
# Block until we get a new state back
if timeout is not None:
while rospy.Time.now() - start_time < timeout:
# Send the initial state command
init_pub.publish(initial_status_msg)
# Filter messages that are from other containers
if msg_response.path == path:
# Check if the heartbeat came back to match
state_match = all([
s in msg_response.initial_states
for s in initial_states
])
local_data = smach.UserData()
local_data._data = pickle.loads(msg_response.local_data)
ud_match = all([\
(key in local_data and local_data._data[key] == initial_userdata._data[key])\
for key in initial_userdata._data])
rospy.logdebug("STATE MATCH: " + str(state_match) +
", UD_MATCH: " + str(ud_match))
if state_match and ud_match:
return True
rospy.sleep(0.3)
return False