class ManuallyTransitionableState(MonitoringState):
"""
A state for that a desired outcome can be declared.
If any outcome is declared, this outcome is forced.
"""
def __init__(self, *args, **kwargs):
super(ManuallyTransitionableState, self).__init__(*args, **kwargs)
self._force_transition = False
self.__execute = self.execute
self.execute = self._manually_transitionable_execute
self._feedback_topic = '/flexbe/command_feedback'
self._transition_topic = '/flexbe/command/transition'
self._pub = ProxyPublisher()
self._sub = ProxySubscriberCached()
def _manually_transitionable_execute(self, *args, **kwargs):
if self._is_controlled and self._sub.has_buffered(
self._transition_topic):
command_msg = self._sub.get_from_buffer(self._transition_topic)
self._pub.publish(
self._feedback_topic,
CommandFeedback(command="transition",
args=[command_msg.target, self.name]))
if command_msg.target != self.name:
rospy.logwarn("--> Requested outcome for state " +
command_msg.target + " but active state is " +
self.name)
else:
self._force_transition = True
outcome = self._outcome_list[command_msg.outcome]
rospy.loginfo("--> Manually triggered outcome " + outcome +
" of state " + self.name)
return outcome
# return the normal outcome
self._force_transition = False
return self.__execute(*args, **kwargs)
def _enable_ros_control(self):
super(ManuallyTransitionableState, self)._enable_ros_control()
self._pub.createPublisher(self._feedback_topic, CommandFeedback)
self._sub.subscribe(self._transition_topic, OutcomeRequest)
self._sub.enable_buffer(self._transition_topic)
def _disable_ros_control(self):
super(ManuallyTransitionableState, self)._disable_ros_control()
self._sub.unsubscribe_topic(self._transition_topic)
class GetJointValuesDynState(EventState):
'''
Retrieves current values of specified joints.
># joint_names string[] List of desired joint names.
#> joint_values float[] List of current joint values.
<= retrieved Joint values are available.
'''
def __init__(self):
'''
Constructor
'''
super(GetJointValuesDynState, self).__init__(
outcomes=['retrieved'],
output_keys=['joint_values'],
input_keys=['joint_names'])
self._topic = '/joint_states'
self._sub = ProxySubscriberCached({self._topic: JointState})
self._joints = None
self._joint_values = list()
def execute(self, userdata):
while self._sub.has_buffered(self._topic):
msg = self._sub.get_from_buffer(self._topic)
for i in range(len(msg.name)):
if msg.name[i] in self._joints \
and self._joint_values[self._joints.index(msg.name[i])] is None:
self._joint_values[self._joints.index(msg.name[i])] = msg.position[i]
if all(v is not None for v in self._joint_values):
userdata.joint_values = self._joint_values
return 'retrieved'
def on_enter(self, userdata):
self._sub.enable_buffer(self._topic)
self._joint_values = [None] * len(self._joints)
self._joints = userdata.joint_names
def on_exit(self, userdata):
self._sub.disable_buffer(self._topic)
class ManuallyTransitionableState(MonitoringState):
"""
A state for that a desired outcome can be declared.
If any outcome is declared, this outcome is forced.
"""
def __init__(self, *args, **kwargs):
super(ManuallyTransitionableState, self).__init__(*args, **kwargs)
self._force_transition = False
self.__execute = self.execute
self.execute = self._manually_transitionable_execute
self._feedback_topic = '/flexbe/command_feedback'
self._transition_topic = '/flexbe/command/transition'
self._pub = ProxyPublisher()
self._sub = ProxySubscriberCached()
def _manually_transitionable_execute(self, *args, **kwargs):
if self._is_controlled and self._sub.has_buffered(self._transition_topic):
command_msg = self._sub.get_from_buffer(self._transition_topic)
self._pub.publish(self._feedback_topic, CommandFeedback(command="transition", args=[command_msg.target, self.name]))
if command_msg.target != self.name:
rospy.logwarn("--> Requested outcome for state " + command_msg.target + " but active state is " + self.name)
else:
self._force_transition = True
outcome = self._outcome_list[command_msg.outcome]
rospy.loginfo("--> Manually triggered outcome " + outcome + " of state " + self.name)
return outcome
# return the normal outcome
self._force_transition = False
return self.__execute(*args, **kwargs)
def _enable_ros_control(self):
super(ManuallyTransitionableState, self)._enable_ros_control()
self._pub.createPublisher(self._feedback_topic, CommandFeedback)
self._sub.subscribe(self._transition_topic, OutcomeRequest)
self._sub.enable_buffer(self._transition_topic)
def _disable_ros_control(self):
super(ManuallyTransitionableState, self)._disable_ros_control()
self._sub.unsubscribe_topic(self._transition_topic)
class WaitForOpenDoorState(EventState):
"""
Implements a state that waits for the way to be clear.
<= done Indicates completion.
"""
LASERS_TO_IGNORE = 5
ERR_THRESHOLD = 0.15
old_values = None
def __init__(self):
"""
Constructor
"""
super(WaitForOpenDoorState, self).__init__(outcomes=['done'])
self._topic = '/laser/srd_front/scan'
self._sub = ProxySubscriberCached({self._topic: LaserScan})
def execute(self, userdata):
"""Execute this state"""
while self._sub.has_buffered(self._topic):
msg = self._sub.get_from_buffer(self._topic)
values = msg.ranges
if self.old_values is None:
self.old_values = values
else:
err = 0.0
for i in range(self.LASERS_TO_IGNORE, len(values)-1-self.LASERS_TO_IGNORE):
err += self.old_values[i] - values[i]
err /= len(values)
if err < 0 and abs(err) > self.ERR_THRESHOLD:
Logger.loginfo('THRESHOLD REACHED! (%.3f) - Door should be open!' % err)
return 'done'
self.old_values = values
def on_enter(self, userdata):
self._sub.enable_buffer(self._topic)
Logger.loginfo('Waiting for open door...')
def on_stop(self):
self._sub.unsubscribe_topic(self._topic)
def on_exit(self):
self._sub.unsubscribe_topic(self._topic)
class GetJointValuesDynState(EventState):
'''
Retrieves current values of specified joints.
># joint_names string[] List of desired joint names.
#> joint_values float[] List of current joint values.
<= retrieved Joint values are available.
'''
def __init__(self):
'''
Constructor
'''
super(GetJointValuesDynState,
self).__init__(outcomes=['retrieved'],
output_keys=['joint_values'],
input_keys=['joint_names'])
self._topic = '/joint_states'
self._sub = ProxySubscriberCached({self._topic: JointState})
self._joints = None
self._joint_values = list()
def execute(self, userdata):
while self._sub.has_buffered(self._topic):
msg = self._sub.get_from_buffer(self._topic)
for i in range(len(msg.name)):
if msg.name[i] in self._joints \
and self._joint_values[self._joints.index(msg.name[i])] is None:
self._joint_values[self._joints.index(
msg.name[i])] = msg.position[i]
if all(v is not None for v in self._joint_values):
userdata.joint_values = self._joint_values
return 'retrieved'
def on_enter(self, userdata):
self._sub.enable_buffer(self._topic)
self._joint_values = [None] * len(self._joints)
self._joints = userdata.joint_names
def on_exit(self, userdata):
self._sub.disable_buffer(self._topic)
def test_subscribe_buffer(self):
t1 = '/buffered_1'
pub = ProxyPublisher({t1: String})
sub = ProxySubscriberCached({t1: String})
sub.enable_buffer(t1)
self.assertTrue(pub.wait_for_any(t1))
pub.publish(t1, String('1'))
pub.publish(t1, String('2'))
rospy.sleep(.5) # make sure messages can be received
self.assertTrue(sub.has_msg(t1))
self.assertTrue(sub.has_buffered(t1))
self.assertEqual(sub.get_from_buffer(t1).data, '1')
pub.publish(t1, String('3'))
rospy.sleep(.5) # make sure messages can be received
self.assertEqual(sub.get_from_buffer(t1).data, '2')
self.assertEqual(sub.get_from_buffer(t1).data, '3')
self.assertIsNone(sub.get_from_buffer(t1))
self.assertFalse(sub.has_buffered(t1))
class GetCurrentJointValuesListState(EventState):
'''
Retrieves current values of specified joints.
Joints are specified by parameter list.
-- joint_names string[] List of desired joint names.
-- timeout double Timeout value (optional)
-- joint_states_topic string Optional name of joint states topic
(default: /joint_states)
#> joint_names string[] List of current joint names.
#> joint_values float[] List of current joint values.
<= retrieved Joint values are available.
<= timeout Joint values are not available.
'''
def __init__(self,
joint_names,
timeout=None,
joint_states_topic='/joint_states'):
'''
Constructor
'''
super(GetCurrentJointValuesListState,
self).__init__(outcomes=['retrieved', 'timeout'],
output_keys=['joint_names', 'joint_values'])
self.topic = joint_states_topic
self.sub = ProxySubscriberCached({self.topic: JointState})
self.joint_names = joint_names
self.joint_values = list()
if (timeout is not None) and (timeout > 0.0):
self.timeout_duration = rospy.Duration(timeout)
else:
self.timeout_duration = None
def execute(self, userdata):
if (self.return_code is not None):
# Handle blocked transition or error during on_enter
userdata.joint_names = self.joint_names
userdata.joint_values = self.joint_values
return self.return_code
while self.sub.has_buffered(self.topic):
msg = self.sub.get_from_buffer(self.topic)
for i in range(len(msg.name)):
if msg.name[i] in self.joint_names \
and self.joint_values[self.joint_names.index(msg.name[i])] is None:
self.joint_values[self.joint_names.index(
msg.name[i])] = msg.position[i]
if all(v is not None for v in self.joint_values):
userdata.joint_names = self.joint_names
userdata.joint_values = self.joint_values
self.return_code = 'retrieved'
return 'retrieved'
if (self.timeout_duration is not None and \
(rospy.Time.now()-self.start_time) > self.timeout_duration ):
Logger.logerr(
'Timeout %s - found %s of %s' %
(self.name, str(self.joint_values), str(self.joint_names)))
self.return_code = 'timeout'
return 'timeout'
def on_enter(self, userdata):
self.sub.enable_buffer(self.topic)
self.sub.remove_last_msg(self.topic, True)
self.joint_values = [None] * len(self.joint_names)
self.return_code = None
self.start_time = rospy.Time.now()
userdata.joint_names = self.joint_names
# Clear user data until we have valid data
userdata.joint_values = None
def on_exit(self, userdata):
self.sub.disable_buffer(self.topic)
class MonitoringState(smach.State):
"""
A state to monitor a custom set of conditions.
For each conditions, an outcome is added or mapped which will be returned if the condition is not met.
"""
def __init__(self, *args, **kwargs):
super(MonitoringState, self).__init__(*args, **kwargs)
outcomes = kwargs.get('outcomes', [])
self._outcome_list = list(outcomes)
self._outcome_list.remove('loopback')
self.name = None
self._parent = None
self._is_controlled = False
self._force_monitoring = False
self._monitored_keys = dict()
self._sent_keys = list()
self._current_status = None
self.__execute = self.execute
self.execute = self._monitoring_execute
self._diagnostics_topic = 'diagnostics_agg'
self._sub = ProxySubscriberCached()
def _monitoring_execute(self, *args, **kwargs):
new_status = None
had_warning = False
if (self._force_monitoring
or self._is_controlled) and self._sub.has_buffered(
self._diagnostics_topic):
new_status = ""
diag_msg = self._sub.get_from_buffer(self._diagnostics_topic)
for status in diag_msg.status:
if not status.name in self._monitored_keys.keys(): continue
if status.level == DiagnosticStatus.WARN:
had_warning = True
if not status.name + "_warn" in self._sent_keys:
self._sent_keys.append(status.name + "_warn")
Logger.logwarn("%s: %s" %
(status.name, status.message))
if status.level == DiagnosticStatus.ERROR:
if not status.name + "_err" in self._sent_keys:
self._sent_keys.append(status.name + "_err")
Logger.logerr("%s: %s" % (status.name, status.message))
new_status = status.name
if new_status == "":
self._current_status = None
new_status = None
if not had_warning:
self._sent_keys = list()
if new_status is None or self._current_status == new_status:
return self.__execute(*args, **kwargs)
self._current_status = new_status
return self._monitored_keys[self._current_status]
def monitor(self, key, outcome=None):
oc = outcome if not outcome is None else key
self._monitored_keys[key] = oc
if not oc in self._outcomes:
self.register_outcomes([oc])
self._outcome_list.append(oc)
def force_monitoring(self):
self._force_monitoring = True
if not self._is_controlled:
self._sub.subscribe(self._diagnostics_topic, DiagnosticArray)
self._sub.enable_buffer(self._diagnostics_topic)
def _enable_ros_control(self):
self._is_controlled = True
self._sub.subscribe(self._diagnostics_topic, DiagnosticArray)
self._sub.enable_buffer(self._diagnostics_topic)
def _disable_ros_control(self):
self._is_controlled = False
self._sub.unsubscribe_topic(self._diagnostics_topic)
def _get_path(self):
return self._parent._get_path() + "/" + self.name
class MonitoringState(smach.State):
"""
A state to monitor a custom set of conditions.
For each conditions, an outcome is added or mapped which will be returned if the condition is not met.
"""
def __init__(self, *args, **kwargs):
super(MonitoringState, self).__init__(*args, **kwargs)
outcomes = kwargs.get('outcomes', [])
self._outcome_list = list(outcomes)
self._outcome_list.remove('loopback')
self.name = None
self._parent = None
self._is_controlled = False
self._force_monitoring = False
self._monitored_keys = dict()
self._sent_keys = list()
self._current_status = None
self.__execute = self.execute
self.execute = self._monitoring_execute
self._diagnostics_topic = 'diagnostics_agg'
self._sub = ProxySubscriberCached()
def _monitoring_execute(self, *args, **kwargs):
new_status = None
had_warning = False
if (self._force_monitoring or self._is_controlled) and self._sub.has_buffered(self._diagnostics_topic):
new_status = ""
diag_msg = self._sub.get_from_buffer(self._diagnostics_topic)
for status in diag_msg.status:
if not status.name in self._monitored_keys.keys(): continue
if status.level == DiagnosticStatus.WARN:
had_warning = True
if not status.name + "_warn" in self._sent_keys:
self._sent_keys.append(status.name + "_warn")
Logger.logwarn("%s: %s" % (status.name, status.message))
if status.level == DiagnosticStatus.ERROR:
if not status.name + "_err" in self._sent_keys:
self._sent_keys.append(status.name + "_err")
Logger.logerr("%s: %s" % (status.name, status.message))
new_status = status.name
if new_status == "":
self._current_status = None
new_status = None
if not had_warning:
self._sent_keys = list()
if new_status is None or self._current_status == new_status:
return self.__execute(*args, **kwargs)
self._current_status = new_status
return self._monitored_keys[self._current_status]
def monitor(self, key, outcome = None):
oc = outcome if not outcome is None else key
self._monitored_keys[key] = oc
if not oc in self._outcomes:
self.register_outcomes([oc])
self._outcome_list.append(oc)
def force_monitoring(self):
self._force_monitoring = True
if not self._is_controlled:
self._sub.subscribe(self._diagnostics_topic, DiagnosticArray)
self._sub.enable_buffer(self._diagnostics_topic)
def _enable_ros_control(self):
self._is_controlled = True
self._sub.subscribe(self._diagnostics_topic, DiagnosticArray)
self._sub.enable_buffer(self._diagnostics_topic)
def _disable_ros_control(self):
self._is_controlled = False
self._sub.unsubscribe_topic(self._diagnostics_topic)
def _get_path(self):
return self._parent._get_path() + "/" + self.name
class ObjectDetectionMonitor(EventState):
'''
Receive list of detected objects as cob_object_detection_msgs/DetectionArray messages and check if there is an object
which matches given (label, type) pair. Based on present of this object classify situation in `no_detections` state,
`have_detections` (there is objects but none of them matches give (label, type)),
`detection_matches` (desired object found). In the last case its pose bublished on given topic.
-- detection_topic string DetectionArray message topic.
-- label string Object label. Use '*' to match all labals.
-- type string Object type. Use '*' to match all types.
-- detection_period float Parameteres: detection_period (s), position_tolerance (m), orientation_tolerance (rad).
-- exit_states string[] Stop monitoring and return outcome if detected situation in this list.
-- pose_topic string Topic for publishing matched object pose (may be Empty).
-- pose_frame_id string Frame in which pose should be published.
#> pose object Object pose before exit.
<= no_detections Nothing have been detected for detection_period.
<= have_detections Objects were detected, but not of them matches (label, type)
<= detecton_matches Object presents and it is moving. `pose` key contains last pose.
<= failure Runtime error.
'''
def __init__(self,
detection_topic='detection',
label='*',
type='*',
exit_states=['no_detections'],
pose_topic='',
pose_frame_id='odom_combined',
detection_period=1.0):
super(ObjectDetectionMonitor, self).__init__(outcomes=[
'no_detections', 'have_detections', 'detection_matches', 'failure'
],
output_keys=['pose'])
# store state parameter for later use.
self._detection_topic = detection_topic
self._label = label
self._type = type
self._pose_topic = pose_topic
self._frame_id = pose_frame_id
self._exit_states = exit_states
self._detection_period = detection_period
# setup proxies
self._detection_subscriber = ProxySubscriberCached(
{self._detection_topic: DetectionArray})
self._detection_subscriber.enable_buffer(self._detection_topic)
if self._pose_topic:
self._pose_publisher = ProxyPublisher(
{self._pose_topic: PoseStamped})
else:
self._pose_publisher = None
self._tf_listener = ProxyTransformListener().listener()
# check parameters
if not isinstance(
self._frame_id,
str): # or not self._tf_listener.frameExists(self._frame_id):
raise ValueError(
'ObjectDetectionMonitor: %s is not a valid frame' %
self._frame_id)
if not isinstance(self._label, str) or not isinstance(self._type, str):
raise ValueError(
'ObjectDetectionMonitor: label and type parameters must be string.'
)
if not isinstance(self._detection_period,
float) or self._detection_period < 0.0:
raise ValueError(
'ObjectDetectionMonitor: detection_period must be float.')
if not isinstance(self._exit_states, list) or not all([
state
in ['no_detections', 'have_detections', 'detection_matches']
for state in self._exit_states
]):
raise ValueError(
'ObjectDetectionMonitor: incorrect list of exit states.')
# pose buffers
self._pose = None
self._last_detection_stamp = None
self._last_match_stamp = None
self._last_match_id = None
def on_enter(self, userdata):
# reset pose buffers
self._pose = None
stamp = rospy.Time.now()
self._last_detection_stamp = stamp
self._last_match_stamp = stamp
self._last_match_id = None
Logger.loginfo('ObjectDetectionMonitor: start monitoring.')
def execute(self, userdata):
# list of found matches
have_detections = False
best_match = None
match = None
# check if new message is available
while self._detection_subscriber.has_buffered(self._detection_topic):
# get detections msg
detections_msg = self._detection_subscriber.get_from_buffer(
self._detection_topic)
# check if have detections
if len(detections_msg.detections) > 0:
have_detections = True
# process detections
for detection in detections_msg.detections:
# check match
is_match = True
if self._label != '*' and detection.label != self._label:
is_match = False
if self._type != '*' and detection.type != self._type:
is_match = False
# register match
if is_match:
match = detection
if match.id == self._last_match_id:
best_match = match
if best_match:
match = best_match
# result
stamp = rospy.Time.now()
if not have_detections:
# no object detected
# check if detection preiod exceeded
if (stamp - self._last_detection_stamp
).to_sec() > self._detection_period:
# no detections state
if 'no_detections' in self._exit_states:
Logger.loginfo('ObjectDetectionMonitor: no detections.')
return 'no_detections'
elif not match:
# object detected but it is not match
self._last_detection_stamp = stamp
# check if detection perod exceeded
if (stamp - self._last_detection_stamp
).to_sec() > self._detection_period:
# have_detections state
if 'have_detections' in self._exit_states:
Logger.loginfo('ObjectDetectionMonitor: detection.')
return 'have_detections'
else:
# appropriate match is found
self._last_match_stamp = stamp
self._last_match_id = match.id
# publish
if self._pose_publisher:
# transform to frame_id
try:
# transform
pose_stamped = PoseStamped(
pose=match.pose,
header=Header(frame_id=match.header.frame_id))
self._pose = self._tf_listener.transformPose(
self._frame_id, pose_stamped)
except tf.Exception as e:
Logger.logwarn(
'Unable to transform from %s to %s' %
(match.pose.header.frame_id, self._frame_id))
return 'failure'
# publish
self._pose_publisher.publish(self._pose_topic, self._pose)
# match_detection state
if 'detection_matches' in self._exit_states:
Logger.loginfo('ObjectDetectionMonitor: match.')
return 'detection_matches'
return None
def on_exit(self, userdata):
userdata.pose = self._pose
class VigirBehaviorMirror(object):
'''
classdocs
'''
def __init__(self):
'''
Constructor
'''
self._sm = None
smach.set_loggers(
rospy.logdebug, # hide SMACH transition log spamming
rospy.logwarn,
rospy.logdebug,
rospy.logerr)
# set up proxys for sm <--> GUI communication
# publish topics
self._pub = ProxyPublisher({
'flexbe/behavior_update': String,
'flexbe/request_mirror_structure': Int32
})
self._running = False
self._stopping = False
self._active_id = 0
self._starting_path = None
self._current_struct = None
self._sync_lock = threading.Lock()
self._outcome_topic = 'flexbe/mirror/outcome'
self._struct_buffer = list()
# listen for mirror message
self._sub = ProxySubscriberCached()
self._sub.subscribe(self._outcome_topic, UInt8)
self._sub.enable_buffer(self._outcome_topic)
self._sub.subscribe('flexbe/mirror/structure', ContainerStructure,
self._mirror_callback)
self._sub.subscribe('flexbe/status', BEStatus, self._status_callback)
self._sub.subscribe('flexbe/mirror/sync', BehaviorSync,
self._sync_callback)
self._sub.subscribe('flexbe/mirror/preempt', Empty,
self._preempt_callback)
def _mirror_callback(self, msg):
rate = rospy.Rate(10)
while self._stopping:
rate.sleep()
if self._running:
rospy.logwarn(
'Received a new mirror structure while mirror is already running, adding to buffer (checksum: %s).'
% str(msg.behavior_id))
elif self._active_id != 0 and msg.behavior_id != self._active_id:
rospy.logwarn(
'checksum of received mirror structure (%s) does not match expected checksum (%s), will ignore.'
% (str(msg.behavior_id), str(self._active_id)))
return
else:
rospy.loginfo('Received a new mirror structure for checksum %s' %
str(msg.behavior_id))
self._struct_buffer.append(msg)
if self._active_id == msg.behavior_id:
self._struct_buffer = list()
self._mirror_state_machine(msg)
rospy.loginfo('Mirror built.')
self._execute_mirror()
def _status_callback(self, msg):
if msg.code == BEStatus.STARTED:
thread = threading.Thread(target=self._start_mirror, args=[msg])
thread.daemon = True
thread.start()
else:
thread = threading.Thread(target=self._stop_mirror, args=[msg])
thread.daemon = True
thread.start()
def _start_mirror(self, msg):
with self._sync_lock:
rate = rospy.Rate(10)
while self._stopping:
rate.sleep()
if self._running:
rospy.logwarn(
'Tried to start mirror while it is already running, will ignore.'
)
return
if len(msg.args) > 0:
self._starting_path = "/" + msg.args[0][1:].replace(
"/", "_mirror/") + "_mirror"
self._active_id = msg.behavior_id
while self._sm is None and len(self._struct_buffer) > 0:
struct = self._struct_buffer[0]
self._struct_buffer = self._struct_buffer[1:]
if struct.behavior_id == self._active_id:
self._mirror_state_machine(struct)
rospy.loginfo('Mirror built.')
else:
rospy.logwarn(
'Discarded mismatching buffered structure for checksum %s'
% str(struct.behavior_id))
if self._sm is None:
rospy.logwarn(
'Missing correct mirror structure, requesting...')
rospy.sleep(
0.2
) # no clean wayacquire to wait for publisher to be ready...
self._pub.publish('flexbe/request_mirror_structure',
Int32(msg.behavior_id))
self._active_id = msg.behavior_id
return
self._execute_mirror()
def _stop_mirror(self, msg):
with self._sync_lock:
self._stopping = True
if self._sm is not None and self._running:
if msg is not None and msg.code == BEStatus.FINISHED:
rospy.loginfo('Onboard behavior finished successfully.')
self._pub.publish('flexbe/behavior_update', String())
elif msg is not None and msg.code == BEStatus.SWITCHING:
self._starting_path = None
rospy.loginfo('Onboard performing behavior switch.')
elif msg is not None and msg.code == BEStatus.READY:
rospy.loginfo(
'Onboard engine just started, stopping currently running mirror.'
)
self._pub.publish('flexbe/behavior_update', String())
elif msg is not None:
rospy.logwarn('Onboard behavior failed!')
self._pub.publish('flexbe/behavior_update', String())
PreemptableState.preempt = True
rate = rospy.Rate(10)
while self._running:
rate.sleep()
else:
rospy.loginfo('No onboard behavior is active.')
self._active_id = 0
self._sm = None
self._current_struct = None
self._sub.remove_last_msg(self._outcome_topic, clear_buffer=True)
if msg is not None and msg.code != BEStatus.SWITCHING:
rospy.loginfo('\033[92m--- Behavior Mirror ready! ---\033[0m')
self._stopping = False
def _sync_callback(self, msg):
if msg.behavior_id == self._active_id:
thread = threading.Thread(target=self._restart_mirror, args=[msg])
thread.daemon = True
thread.start()
else:
rospy.logerr(
'Cannot synchronize! Different behavior is running onboard, please stop execution!'
)
thread = threading.Thread(target=self._stop_mirror, args=[None])
thread.daemon = True
thread.start()
def _restart_mirror(self, msg):
with self._sync_lock:
rospy.loginfo('Restarting mirror for synchronization...')
self._sub.remove_last_msg(self._outcome_topic, clear_buffer=True)
if self._sm is not None and self._running:
PreemptableState.preempt = True
rate = rospy.Rate(10)
while self._running:
rate.sleep()
self._sm = None
if msg.current_state_checksum in self._state_checksums:
current_state_path = self._state_checksums[
msg.current_state_checksum]
self._starting_path = "/" + current_state_path[1:].replace(
"/", "_mirror/") + "_mirror"
rospy.loginfo('Current state: %s' % current_state_path)
try:
self._mirror_state_machine(self._current_struct)
rospy.loginfo('Mirror built.')
except (AttributeError, smach.InvalidStateError):
rospy.loginfo(
'Stopping synchronization because behavior has stopped.')
self._execute_mirror()
def _execute_mirror(self):
self._running = True
rospy.loginfo("Executing mirror...")
if self._starting_path is not None:
LockableStateMachine.path_for_switch = self._starting_path
rospy.loginfo("Starting mirror in state " + self._starting_path)
self._starting_path = None
result = 'preempted'
try:
result = self._sm.execute()
except Exception as e:
rospy.loginfo('Caught exception on preempt:\n%s' % str(e))
result = 'preempted'
if self._sm is not None:
self._sm.destroy()
self._running = False
rospy.loginfo('Mirror finished with result ' + result)
def _mirror_state_machine(self, msg):
self._current_struct = msg
self._state_checksums = dict()
root = None
for con_msg in msg.containers:
if con_msg.path.find('/') == -1:
root = con_msg.path
break
self._add_node(msg, root)
# calculate checksums of all states
for con_msg in msg.containers:
if con_msg.path.find('/') != -1:
self._state_checksums[zlib.adler32(
con_msg.path)] = con_msg.path
def _add_node(self, msg, path):
#rospy.loginfo('Add node: '+path)
container = None
for con_msg in msg.containers:
if con_msg.path == path:
container = con_msg
break
transitions = None
if container.transitions is not None:
transitions = {}
for i in range(len(container.transitions)):
transitions[container.
outcomes[i]] = container.transitions[i] + '_mirror'
path_frags = path.split('/')
container_name = path_frags[len(path_frags) - 1]
if len(container.children) > 0:
sm_outcomes = []
for outcome in container.outcomes:
sm_outcomes.append(outcome + '_mirror')
sm = JumpableStateMachine(outcomes=sm_outcomes)
with sm:
for child in container.children:
self._add_node(msg, path + '/' + child)
if len(transitions) > 0:
container_transitions = {}
for i in range(len(container.transitions)):
container_transitions[sm_outcomes[i]] = transitions[
container.outcomes[i]]
JumpableStateMachine.add(container_name + '_mirror',
sm,
transitions=container_transitions)
else:
self._sm = sm
else:
JumpableStateMachine.add(container_name + '_mirror',
MirrorState(container_name, path,
container.outcomes,
container.autonomy),
transitions=transitions)
def _jump_callback(self, msg):
start_time = rospy.Time.now()
current_elapsed = 0
jumpable = True
while self._sm is None or not self._sm.is_running():
current_elapsed = rospy.Time.now() - start_time
if current_elapsed > rospy.Duration.from_sec(10):
jumpable = False
break
rospy.sleep(0.05)
if jumpable:
self._sm.jump_to(msg.stateName)
else:
rospy.logwarn(
'Could not jump in mirror: Mirror does not exist or is not running!'
)
def _preempt_callback(self, msg):
if self._sm is not None and self._sm.is_running():
rospy.logwarn(
'Explicit preempting is currently ignored, mirror should be preempted by onboard behavior.'
)
else:
rospy.logwarn(
'Could not preempt mirror because it seems not to be running!')
class VigirBehaviorMirror(object):
'''
classdocs
'''
def __init__(self):
'''
Constructor
'''
self._sm = None
smach.set_loggers (
rospy.logdebug, # hide SMACH transition log spamming
rospy.logwarn,
rospy.logdebug,
rospy.logerr
)
# set up proxys for sm <--> GUI communication
# publish topics
self._pub = ProxyPublisher({'flexbe/behavior_update': String,
'flexbe/request_mirror_structure': Int32})
self._running = False
self._stopping = False
self._active_id = 0
self._starting_path = None
self._current_struct = None
self._outcome_topic = 'flexbe/mirror/outcome'
self._struct_buffer = list()
# listen for mirror message
self._sub = ProxySubscriberCached()
self._sub.subscribe(self._outcome_topic, UInt8)
self._sub.enable_buffer(self._outcome_topic)
self._sub.subscribe('flexbe/mirror/structure', ContainerStructure, self._mirror_callback)
self._sub.subscribe('flexbe/status', BEStatus, self._status_callback)
self._sub.subscribe('flexbe/mirror/sync', BehaviorSync, self._sync_callback)
self._sub.subscribe('flexbe/mirror/preempt', Empty, self._preempt_callback)
def _mirror_callback(self, msg):
rate = rospy.Rate(10)
while self._stopping:
rate.sleep()
if self._running:
rospy.logwarn('Received a new mirror structure while mirror is already running, adding to buffer (ID: %s).' % str(msg.behavior_id))
elif self._active_id != 0 and msg.behavior_id != self._active_id:
rospy.logwarn('ID of received mirror structure (%s) does not match expected ID (%s), will ignore.' % (str(msg.behavior_id), str(self._active_id)))
return
else:
rospy.loginfo('Received a new mirror structure for ID %s' % str(msg.behavior_id))
self._struct_buffer.append(msg)
if self._active_id == msg.behavior_id:
self._struct_buffer = list()
self._mirror_state_machine(msg)
rospy.loginfo('Mirror built.')
self._execute_mirror()
def _status_callback(self, msg):
if msg.code == BEStatus.STARTED:
thread = threading.Thread(target=self._start_mirror, args=[msg])
thread.daemon = True
thread.start()
else:
thread = threading.Thread(target=self._stop_mirror, args=[msg])
thread.daemon = True
thread.start()
def _start_mirror(self, msg):
rate = rospy.Rate(10)
while self._stopping:
rate.sleep()
if self._running:
rospy.logwarn('Tried to start mirror while it is already running, will ignore.')
return
if len(msg.args) > 0:
self._starting_path = "/" + msg.args[0][1:].replace("/", "_mirror/") + "_mirror"
self._active_id = msg.behavior_id
while self._sm is None and len(self._struct_buffer) > 0:
struct = self._struct_buffer[0]
self._struct_buffer = self._struct_buffer[1:]
if struct.behavior_id == self._active_id:
self._mirror_state_machine(struct)
rospy.loginfo('Mirror built.')
else:
rospy.logwarn('Discarded mismatching buffered structure for ID %s' % str(struct.behavior_id))
if self._sm is None:
rospy.logwarn('Missing correct mirror structure, requesting...')
rospy.sleep(0.2) # no clean way to wait for publisher to be ready...
self._pub.publish('flexbe/request_mirror_structure', Int32(msg.behavior_id))
self._active_id = msg.behavior_id
return
self._execute_mirror()
def _stop_mirror(self, msg):
self._stopping = True
if self._sm is not None and self._running:
if msg is not None and msg.code == BEStatus.FINISHED:
rospy.loginfo('Onboard behavior finished successfully.')
self._pub.publish('flexbe/behavior_update', String())
elif msg is not None and msg.code == BEStatus.SWITCHING:
self._starting_path = None
rospy.loginfo('Onboard performing behavior switch.')
elif msg is not None and msg.code == BEStatus.READY:
rospy.loginfo('Onboard engine just started, stopping currently running mirror.')
self._pub.publish('flexbe/behavior_update', String())
elif msg is not None:
rospy.logwarn('Onboard behavior failed!')
self._pub.publish('flexbe/behavior_update', String())
PreemptableState.preempt = True
rate = rospy.Rate(10)
while self._running:
rate.sleep()
else:
rospy.loginfo('No onboard behavior is active.')
self._active_id = 0
self._sm = None
self._current_struct = None
self._sub.remove_last_msg(self._outcome_topic, clear_buffer=True)
if msg is not None and msg.code != BEStatus.SWITCHING:
rospy.loginfo('\033[92m--- Behavior Mirror ready! ---\033[0m')
self._stopping = False
def _sync_callback(self, msg):
if msg.behavior_id == self._active_id:
thread = threading.Thread(target=self._restart_mirror, args=[msg])
thread.daemon = True
thread.start()
else:
rospy.logerr('Cannot synchronize! Different behavior is running onboard, please stop execution!')
thread = threading.Thread(target=self._stop_mirror, args=[None])
thread.daemon = True
thread.start()
def _restart_mirror(self, msg):
rospy.loginfo('Restarting mirror for synchronization...')
self._sub.remove_last_msg(self._outcome_topic, clear_buffer=True)
if self._sm is not None and self._running:
PreemptableState.preempt = True
rate = rospy.Rate(10)
while self._running:
rate.sleep()
self._sm = None
if msg.current_state_checksum in self._state_checksums:
current_state_path = self._state_checksums[msg.current_state_checksum]
self._starting_path = "/" + current_state_path[1:].replace("/", "_mirror/") + "_mirror"
rospy.loginfo('Current state: %s' % current_state_path)
try:
self._mirror_state_machine(self._current_struct)
rospy.loginfo('Mirror built.')
self._execute_mirror()
except (AttributeError, smach.InvalidStateError):
rospy.loginfo('Stopping synchronization because behavior has stopped.')
def _execute_mirror(self):
self._running = True
rospy.loginfo("Executing mirror...")
if self._starting_path is not None:
LockableStateMachine.path_for_switch = self._starting_path
rospy.loginfo("Starting mirror in state " + self._starting_path)
self._starting_path = None
result = 'preempted'
try:
result = self._sm.execute()
except Exception as e:
rospy.loginfo('Catched exception on preempt:\n%s' % str(e))
if self._sm is not None:
self._sm.destroy()
self._running = False
rospy.loginfo('Mirror finished with result ' + result)
def _mirror_state_machine(self, msg):
self._current_struct = msg
self._state_checksums = dict()
root = None
for con_msg in msg.containers:
if con_msg.path.find('/') == -1:
root = con_msg.path
break
self._add_node(msg, root)
# calculate checksums of all states
for con_msg in msg.containers:
if con_msg.path.find('/') != -1:
self._state_checksums[zlib.adler32(con_msg.path)] = con_msg.path
def _add_node(self, msg, path):
#rospy.loginfo('Add node: '+path)
container = None
for con_msg in msg.containers:
if con_msg.path == path:
container = con_msg
break
transitions = None
if container.transitions is not None:
transitions = {}
for i in range(len(container.transitions)):
transitions[container.outcomes[i]] = container.transitions[i] + '_mirror'
path_frags = path.split('/')
container_name = path_frags[len(path_frags)-1]
if len(container.children) > 0:
sm_outcomes = []
for outcome in container.outcomes: sm_outcomes.append(outcome + '_mirror')
sm = JumpableStateMachine(outcomes=sm_outcomes)
with sm:
for child in container.children:
self._add_node(msg, path+'/'+child)
if len(transitions) > 0:
container_transitions = {}
for i in range(len(container.transitions)): container_transitions[sm_outcomes[i]] = transitions[container.outcomes[i]]
JumpableStateMachine.add(container_name + '_mirror', sm, transitions=container_transitions)
else:
self._sm = sm
else:
JumpableStateMachine.add(container_name + '_mirror', MirrorState(container_name, path, container.outcomes, container.autonomy), transitions=transitions)
def _jump_callback(self, msg):
start_time = rospy.Time.now()
current_elapsed = 0
jumpable = True
while self._sm is None or not self._sm.is_running():
current_elapsed = rospy.Time.now() - start_time
if current_elapsed > rospy.Duration.from_sec(10):
jumpable = False
break
rospy.sleep(0.05)
if jumpable:
self._sm.jump_to(msg.stateName)
else:
rospy.logwarn('Could not jump in mirror: Mirror does not exist or is not running!')
def _preempt_callback(self, msg):
if self._sm is not None and self._sm.is_running():
rospy.logwarn('Explicit preempting is currently ignored, mirror should be preempted by onboard behavior.')
else:
rospy.logwarn('Could not preempt mirror because it seems not to be running!')
class TestOnboard(unittest.TestCase):
def __init__(self, name):
super(TestOnboard, self).__init__(name)
self.sub = ProxySubscriberCached({
'flexbe/status': BEStatus,
'flexbe/log': BehaviorLog
})
self.rate = rospy.Rate(100)
# make sure that behaviors can be imported
data_folder = os.path.dirname(os.path.realpath(__file__))
sys.path.insert(0, data_folder)
# run onboard and add custom test behaviors to onboard lib
self.onboard = FlexbeOnboard()
self.lib = self.onboard._behavior_lib
self.lib._add_behavior_manifests(data_folder)
def assertStatus(self, expected, timeout):
""" Assert that the expected onboard status is received before the timeout. """
for i in range(int(timeout * 100)):
self.rate.sleep()
if self.sub.has_msg('flexbe/status'):
break
else:
raise AssertionError('Did not receive a status as required.')
msg = self.sub.get_last_msg('flexbe/status')
self.sub.remove_last_msg('flexbe/status')
self.assertEqual(msg.code, expected)
return msg
def test_onboard_behaviors(self):
behavior_pub = rospy.Publisher('flexbe/start_behavior',
BehaviorSelection,
queue_size=1)
rospy.sleep(0.5) # wait for publisher
# wait for the initial status message
self.assertStatus(BEStatus.READY, 1)
# send simple behavior request without checksum
be_id, _ = self.lib.find_behavior("Test Behavior Log")
request = BehaviorSelection()
request.behavior_id = be_id
request.autonomy_level = 255
behavior_pub.publish(request)
self.assertStatus(BEStatus.ERROR, 2)
# send valid simple behavior request
with open(self.lib.get_sourcecode_filepath(be_id)) as f:
request.behavior_checksum = zlib.adler32(
f.read().encode()) & 0x7fffffff
self.sub.enable_buffer('flexbe/log')
behavior_pub.publish(request)
self.assertStatus(BEStatus.STARTED, 1)
self.assertStatus(BEStatus.FINISHED, 3)
behavior_logs = []
while self.sub.has_buffered('flexbe/log'):
behavior_logs.append(self.sub.get_from_buffer('flexbe/log').text)
self.assertIn('Test data', behavior_logs)
# send valid complex behavior request
be_id, _ = self.lib.find_behavior("Test Behavior Complex")
request = BehaviorSelection()
request.behavior_id = be_id
request.autonomy_level = 255
request.arg_keys = ['param']
request.arg_values = ['value_2']
request.input_keys = ['data']
request.input_values = ['2']
with open(self.lib.get_sourcecode_filepath(be_id)) as f:
content = f.read()
modifications = [('flexbe_INVALID', 'flexbe_core'),
('raise ValueError("TODO: Remove!")', '')]
for replace, by in modifications:
index = content.index(replace)
request.modifications.append(
BehaviorModification(index, index + len(replace), by))
for replace, by in modifications:
content = content.replace(replace, by)
request.behavior_checksum = zlib.adler32(content.encode()) & 0x7fffffff
behavior_pub.publish(request)
self.assertStatus(BEStatus.STARTED, 1)
result = self.assertStatus(BEStatus.FINISHED, 3)
self.assertEqual(result.args[0], 'finished')
behavior_logs = []
while self.sub.has_buffered('flexbe/log'):
behavior_logs.append(self.sub.get_from_buffer('flexbe/log').text)
self.assertIn('value_2', behavior_logs)
# send the same behavior with different parameters
request.arg_keys = ['param', 'invalid']
request.arg_values = ['value_1', 'should be ignored']
request.input_keys = []
request.input_values = []
behavior_pub.publish(request)
self.assertStatus(BEStatus.STARTED, 1)
result = self.assertStatus(BEStatus.FINISHED, 3)
self.assertEqual(result.args[0], 'failed')
behavior_logs = []
while self.sub.has_buffered('flexbe/log'):
behavior_logs.append(self.sub.get_from_buffer('flexbe/log').text)
self.assertIn('value_1', behavior_logs)