def _test_WaitForGoalState():
rospy.init_node('smach')
wfg = WaitForGoalState()
print 'execute #1'
wfg.execute(smach.UserData())
print 'execute #2'
wfg.execute(smach.UserData())
print 'execute #3'
wfg.execute(smach.UserData())
def _convert_userdata(self, state, userdata):
inner_userdata = smach.UserData()
for key in list(self._input_keys):
if key.startswith(state.name + "_"):
inner_userdata[key.replace(state.name + "_",
"")] = userdata[key]
return inner_userdata
def execute(self, parent_ud=smach.UserData()):
"""Run the state machine on entry to this state.
This will set the "closed" flag and spin up the execute thread. Once
this flag has been set, it will prevent more states from being added to
the state machine.
"""
##Create a the log file if ssm_enable_log is True
if (rospy.get_param("ssm_enable_log", False) == True):
date_str = datetime.datetime.now().strftime("%Y_%m_%d_%H_%M_")
self.userdata.logfile = rospy.get_param(
"ssm_log_path", default="/tmp/") + date_str + "log.txt"
try:
file = open(self.userdata.logfile, "a")
file.write(StrTimeStamped() + "SMART STATE MACHINE Started \n")
file.close()
except:
rospy.logerr("Log Fail !")
return 'preempt'
container_outcome = super(ssmMainStateMachine, self).execute(parent_ud)
if (rospy.get_param("ssm_enable_log", False) == True):
file = open(self.userdata.logfile, "a")
file.write(StrTimeStamped() + "SMART STATE MACHINE Finished \n")
file.close()
return container_outcome
def search_for_walls(self, robot_id):
robot_path = {}
#TODO Increase to 1.1 altitude for waypoints in real drone !!!
points_path = [
Point(x=-22, y=17, z=7),
Point(x=-21, y=17, z=1.2),
Point(x=-0.5, y=17, z=1.2),
Point(x=-21, y=17, z=1.2),
Point(x=-0.5, y=17, z=1.2)
]
robot_path = []
for point in points_path:
waypoint = PoseStamped()
waypoint.header.frame_id = 'arena'
waypoint.pose.position = point
robot_path.append(waypoint)
print('sending goal to search_objects server {}'.format(robot_id))
userdata = smach.UserData()
userdata.path = robot_path
self.task_manager.start_task(robot_id, FollowPath(), userdata)
while not rospy.is_shutdown() and not self.task_manager.is_idle(
robot_id) and not uav_walls_are_found(self.wall_segment_ids):
self.cluster_wall_segments()
rospy.logwarn('len(self.wall_segment_ids) = {}'.format(
len(self.wall_segment_ids)))
rospy.sleep(1.0)
if not self.task_manager.is_idle(robot_id):
rospy.logwarn('preempting {}'.format(robot_id))
self.task_manager.preempt_task(robot_id)
self.task_manager.wait_for([robot_id])
def main():
from bender_skills import robot_factory
rospy.init_node("crowd_information")
robot = robot_factory.build(
[
"facial_features",
# "AI",
"person_detector",
# "knowledge",
"tts"
],
core=False)
robot.check()
sm = getInstance(robot, True)
ud = smach.UserData()
ud.features = ['gender', 'age']
# introspection server
sis = IntrospectionServer('crowd_information', sm, '/CROWD_SM')
sis.start()
outcome = sm.execute(ud)
sis.stop()
def look_for_objects(self):
robot_paths = {}
point_paths = generate_uav_paths(len(self.available_robots),
self.field_width, self.field_height,
self.column_count)
for i, robot_id in enumerate(self.available_robots):
robot_path = []
flight_level = self.flight_levels[robot_id]
point_path = set_z(point_paths[i], flight_level)
for point in point_path:
waypoint = PoseStamped()
waypoint.header.frame_id = 'arena'
waypoint.pose.position = point
robot_path.append(waypoint)
robot_paths[robot_id] = robot_path
for robot_id in self.available_robots:
print('sending goal to search_objects server {}'.format(robot_id))
userdata = smach.UserData()
userdata.path = robot_paths[robot_id]
self.task_manager.start_task(robot_id, FollowPath(), userdata)
#TODO: what if objects never found?
while not all_piles_are_found(
self.uav_piles, self.ugv_piles) and not all_walls_are_found(
self.uav_walls, self.ugv_wall) and not rospy.is_shutdown():
rospy.logwarn('len(self.uav_piles) = {}'.format(len(
self.uav_piles)))
rospy.logwarn('len(self.ugv_piles) = {}'.format(len(
self.ugv_piles)))
rospy.sleep(1.0)
for robot_id in self.available_robots:
rospy.logwarn('preempting {}'.format(robot_id))
self.task_manager.preempt_task(robot_id)
def execute(self, parent_ud=smach.UserData()):
# First time in this state machine
if self._current_state is None:
# Spew some info
smach.loginfo(
"State machine starting in initial state '%s' with userdata: \n\t%s"
% (self._current_label, list(self.userdata.keys())))
# Set initial state
self._set_current_state(self._initial_state_label)
# Initialize preempt state
self._preempted_label = None
self._preempted_state = None
# Call start callbacks
self.call_start_cbs()
# Copy input keys
self._copy_input_keys(parent_ud, self.userdata)
container_outcome = self._loopback_name
if self.id is not None:
# only top-level statemachine should loop for outcome
while container_outcome == self._loopback_name and not smach.is_shutdown(
):
# Update the state machine
container_outcome = self._async_execute(parent_ud)
else:
container_outcome = self._async_execute(parent_ud)
if smach.is_shutdown():
container_outcome = 'preempted'
return container_outcome
def extinguish_ground_fire(self):
userdata = smach.UserData()
userdata.path = self.path
userdata.color = 'fire'
self.task_manager.start_task(self.robot_id, ExtinguishGroundFire(),
userdata)
self.task_manager.wait_for([self.robot_id])
def _init_cmd_cb(self, msg):
"""Initialize a tree's state and userdata."""
initial_states = msg.initial_states
local_data = msg.local_data
# Check if this init message is directed at this path
rospy.logdebug('Received init message for path: ' + msg.path + ' to ' +
str(initial_states))
if msg.path == self._path:
if all(s in self._container.get_children()
for s in initial_states):
ud = smach.UserData()
ud._data = pickle.loads(msg.local_data)
rospy.logdebug("Setting initial state in smach path: '" +
self._path + "' to '" + str(initial_states) +
"' with userdata: " + str(ud._data))
# Set the initial state
self._container.set_initial_state(initial_states, ud)
# Publish initial state
self._publish_status("REMOTE_INIT")
else:
rospy.logerr("Attempting to set initial state in container '" +
self._path + "' to '" + str(initial_states) +
"', but this container only has states: " +
str(self._container.get_children()))
def _test_SleepState():
rospy.init_node('smach')
ud = smach.UserData()
ud.duration = None
def exec_state(state):
sm = simple_state_wrapper(state)
return execute_smach_container(sm, userdata=ud)
rospy.loginfo("Testing ud:None/arg:1")
state = SleepState(1)
outcome = exec_state(state)
rospy.loginfo("outcome: %s" % outcome)
rospy.loginfo("Testing ud:2/arg:None")
ud.duration = 2
state = SleepState()
outcome = exec_state(state)
rospy.loginfo("outcome: %s" % outcome)
rospy.loginfo("Testing ud:3/arg:4")
ud.duration = 3
state = SleepState(4)
outcome = exec_state(state)
rospy.loginfo("outcome: %s" % outcome)
def __init__(self, hand_type, configuration):
'''Constructor'''
super(FingerConfigurationState, self).__init__(outcomes=['done', 'failed'],
input_keys=['hand_side'])
self._joint_names = dict()
self._joint_names['left'] = dict()
self._joint_names['left']['robotiq'] = ['left_f0_j1', 'left_f1_j0', 'left_f1_j1', 'left_f2_j1']
self._joint_names['left']['vt_hand'] = ['l_f0_j0', 'l_f1_j0']
self._joint_names['right'] = dict()
self._joint_names['right']['robotiq'] = ['right_f0_j1', 'right_f1_j0', 'right_f1_j1', 'right_f2_j1']
self._joint_names['right']['vt_hand'] = ['r_f0_j0', 'r_f1_j0']
self._joint_values = dict()
self._joint_values['robotiq'] = map(lambda x: x * configuration, [1.22, 0.08, 1.13, 1.13]) # 2nd value is the scissor joint
self._joint_values['vt_hand'] = map(lambda x: x * configuration, [2.685, 2.685])
self._execution_state = HandTrajectoryState(hand_type)
self._internal_userdata = smach.UserData()
if not hand_type in self._joint_names['left'].keys():
raise Exception("Unspecified hand type %s, only have %s" % (hand_type, str(self._joint_names['left'].keys())))
self._hand_type = hand_type
self._configuration = self._joint_values[hand_type]
self._print = True
self._failed = False
def execute(self, parent_ud=smach.UserData()):
with self._condition:
if self._root is None:
raise smach.InvalidTransitionError("BehaviorTreeContainer has no root task")
self._result = None
self._current_exec_root = self._initial_task
# Copy input keys
self._copy_input_keys(parent_ud, self.userdata)
result = self._current_exec_root.try_immediate(self.userdata)
if result == Task.DEFERRED:
self._current_exec_root.execute_deferred(self._root_termination_cb, self.userdata)
while self._current_exec_root is not None:
# Releases condition mutex during wait
self._condition.wait(0.1)
result = self._result
else:
self._current_exec_root = None
# Copy output keys
self._copy_output_keys(self.userdata, parent_ud)
return self._outcome_from_result(result)
def _merge_userdata_back(self, state, inner_userdata, userdata):
output_userdata = smach.UserData()
for key in list(self._output_keys):
if key.startswith(state.name + "_"):
output_userdata[key] = inner_userdata[key.replace(
state.name + "_", "")]
userdata.update(output_userdata)
def _async_execute(self, parent_ud=smach.UserData()):
"""Run the state machine on entry to this state.
This will set the "closed" flag and spin up the execute thread. Once
this flag has been set, it will prevent more states from being added to
the state machine.
"""
# This will prevent preempts from getting propagated to non-existent children
with self._state_transitioning_lock:
# Check state consistency
try:
self.check_consistency()
except (smach.InvalidStateError, smach.InvalidTransitionError):
smach.logerr("Container consistency check failed.")
return None
# Set running flag
self._is_running = True
# Initialize container outcome
container_outcome = None
# Step through state machine
if self._is_running and not smach.is_shutdown():
# Update the state machine
container_outcome = self._update_once()
if self._do_rate_sleep and self._current_state is not None and not isinstance(
self._current_state, OperatableStateMachine):
try:
# sleep with the rate of the state and update container rate accordingly
self._rate = self._current_state._rate
self._rate.sleep()
except ROSInterruptException:
rospy.logwarn('Interrupted rate sleep.')
if container_outcome is not None and container_outcome != self._loopback_name:
# Copy output keys
self._copy_output_keys(self.userdata, parent_ud)
else:
container_outcome = self._loopback_name
# provide explicit sync as back-up functionality
# should be used only if there is no other choice
# since it requires additional 8 byte + header update bandwith and time to restart mirror
if self._inner_sync_request and self._get_deep_state() is not None:
self._inner_sync_request = False
if self.id is None:
self._parent._inner_sync_request = True
else:
msg = BehaviorSync()
msg.behavior_id = self.id
msg.current_state_checksum = zlib.adler32(
self._get_deep_state()._get_path())
self._pub.publish('flexbe/mirror/sync', msg)
# We're no longer running
self._is_running = False
return container_outcome
def take_off(self, robot_id):
userdata = smach.UserData()
#userdata.height = self.flight_levels[robot_id]
userdata.height = 7.0 #TODO: take off altitude fixed
self.task_manager.start_task(robot_id, TakeOff(), userdata)
self.task_manager.wait_for([robot_id
]) # Sequential takeoff for safety reasons
def execute(self, userdata=smach.UserData()):
outcome = smach.StateMachine.execute(self, userdata)
if outcome == 'succeeded':
self.get_logger().log_waypoint_success(userdata.goal_pose)
elif outcome == 'failure':
self.get_logger().log_waypoint_fail(userdata.goal_pose)
return outcome
def execute(self, parent_ud = smach.UserData()):
"""Run the state machine on entry to this state.
This will set the "closed" flag and spin up the execute thread. Once
this flag has been set, it will prevent more states from being added to
the state machine.
"""
# This will prevent preempts from getting propagated to non-existent children
with self._state_transitioning_lock:
# Check state consistency
try:
self.check_consistency()
except (smach.InvalidStateError, smach.InvalidTransitionError):
smach.logerr("Container consistency check failed.")
return None
# Set running flag
self._is_running = True
# Initialize preempt state
self._preempted_label = None
self._preempted_state = None
# Set initial state
self._set_current_state(self._initial_state_label)
# Copy input keys
self._copy_input_keys(parent_ud, self.userdata)
# Spew some info
smach.loginfo("State machine starting in initial state '%s' with userdata: \n\t%s" %
(self._current_label, list(self.userdata.keys())))
if self._preempt_requested:
smach.logwarn("Preempt on State machine requested before even executing initial state. This could be a bug. Did last execution not service preemption?")
# Call start callbacks
self.call_start_cbs()
# Initialize container outcome
container_outcome = None
# Step through state machine
while container_outcome is None and self._is_running and not smach.is_shutdown():
# Update the state machine
container_outcome = self._update_once()
# Copy output keys
self._copy_output_keys(self.userdata, parent_ud)
# We're no longer running
self._is_running = False
if self._preempt_requested:
self.service_preempt()
return container_outcome
def execute(self, userdata):
for waypoint in userdata.path:
if self.preempt_requested():
self.service_preempt()
return 'preempted'
child_userdata = smach.UserData()
child_userdata.waypoint = waypoint
self.go_to_task.execute(child_userdata)
return 'succeeded'
def take_off(self):
for robot_id in self.available_robots:
# TODO: clear all regions?
userdata = smach.UserData()
userdata.height = self.flight_levels[
robot_id] # TODO: Why not directly inside tasks?
self.task_manager.start_task(robot_id, TakeOff(), userdata)
self.task_manager.wait_for(
[robot_id]) # Sequential takeoff for safety reasons
def ltm_execute(_self, ud=smach.UserData()):
# Raise any caught exception, but always execute the callbacks.
# The default UserData is required, as nodes won't always get a 'ud' arg.
fn_start(self)
try:
outcome = fn_execute(ud)
finally:
fn_end(self)
return outcome
def execute(self, parent_ud=smach.UserData()):
try:
l = parent_ud.transition_label
except KeyError:
l = 'init'
s = self._initial_state_dict[l]
self.set_initial_state([s])
out = smach.StateMachine.execute(self, parent_ud)
return out
def go_home(self):
# Go to start pose at 6 meters -> Now go to safe pose
userdata_goto = smach.UserData()
userdata_goto.waypoint = PoseStamped()
#userdata_goto.waypoint = self.path[0]
userdata_goto.waypoint.pose.position.x = -6.0
userdata_goto.waypoint.pose.position.y = 3.0
userdata_goto.waypoint.pose.position.z = 6.0
self.task_manager.start_task(self.robot_id, GoTo(), userdata_goto)
self.task_manager.wait_for([self.robot_id])
def execute_deferred(self, parent_cb, userdata=None):
if userdata is None:
userdata = smach.UserData()
# With lock
with self._lock:
#log("ParallelTask.execute_deferred()")
assert (self._runstate == Task.READY)
self._runstate = Task.EXECUTING
self._parent_cb = parent_cb
self._start_children(userdata)
def set_initial_state(self, initial_states, userdata=smach.UserData()):
smach.logdebug("Setting initial state to "+str(initial_states))
if len(initial_states) > 1:
smach.logwarn("Attempting to set initial state to include more than one state, but the StateMachine container can only have one initial state.")
# Set the initial state label
if len(initial_states) > 0:
self._initial_state_label = initial_states[0]
# Set local userdata
self.userdata.update(userdata)
def main(sm):
try:
# setup machine
ltm.setup(sm)
# execute machine
ud = smach.UserData()
sm.execute(ud)
except rospy.ROSInterruptException:
pass
def _test_smach_execute():
rospy.init_node('smach')
ud = smach.UserData()
# state = SleepState(12)
state = CheckSmachEnabledState()
rospy.loginfo("Executing test...")
outcome = state.execute(ud)
rospy.loginfo("outcome: %s" % outcome)
def execute_deferred(self, parent_cb, userdata=None):
if userdata is None:
userdata = smach.UserData()
with self._lock:
assert (self._runstate == Task.READY)
self._runstate = Task.EXECUTING
self._parent_cb = parent_cb
self._execute_deferred_child(self._active_task_idx,
self._make_cb(self._active_task_idx),
userdata)
def execute(self, parent_ud=smach.UserData()):
outcome = super(DoOnExit, self).execute(parent_ud)
# run all finally states associated to the parent's outcome, and pass the outcome through
for label, state in self._exit_states[outcome] + self._exit_states[
'__ANY__']:
state.execute(
smach.Remapper(self.userdata,
state.get_registered_input_keys(),
state.get_registered_output_keys(),
self._remappings[label]))
return outcome
def __init__(self, outcomes=[], input_keys=[], output_keys=[]):
"""Initializes callback lists as empty lists."""
smach.state.State.__init__(self, outcomes, input_keys, output_keys)
self.userdata = smach.UserData()
"""Userdata to be passed to child states."""
# Callback lists
self._start_cbs = []
self._transition_cbs = []
self._termination_cbs = []
def execute(self, parent_ud=smach.UserData()):
"""Run this function on entry of this state, check if a start gait is passed to the state."""
self._initial_state_label = self.default_start_state
if 'transition' in self._outcomes:
start_state = parent_ud.start_state
if start_state is not None:
self._initial_state_label = start_state
parent_ud.start_state = None
return super(WalkStateMachine, self).execute(parent_ud)
