def __init__(self,
controller='motion/controller/joint_state_head',
tolerance=0.17,
timeout=10.0,
joint_topic="joint_states",
outcomes=['done', 'failed', 'timeout']):
super(SetJointStateBase, self).__init__(outcomes=outcomes)
# Store topic parameter for later use.
self._controller = controller
self._joint_topic = joint_topic
self._tolerance = tolerance
self._timeout = Duration.from_sec(timeout)
# create proxies
self._action_client = ProxyActionClient(
{self._controller: SetOperationalAction})
self._pose_publisher = ProxyPublisher(
{self._controller + '/in_joints_ref': JointState})
self._pose_subscriber = ProxySubscriberCached(
{self._joint_topic: JointState})
# timestamp
self._timestamp = None
# error in enter hook
self._error = False
def __init__(self, topic):
super(StorePointcloudState, self).__init__(outcomes = ['done'],
output_keys = ['pointcloud'])
self._sub = ProxySubscriberCached({topic: PointCloud2})
self._pcl_topic = topic
def __init__(self):
self._sm = None
# 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._struct_buffer = list()
self._sync_lock = threading.Lock()
self._outcome_topic = 'flexbe/mirror/outcome'
# 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)
class PreemptableStateMachine(LockableStateMachine):
"""
A state machine that can be preempted.
If preempted, the state machine will return the outcome preempted.
"""
_preempted_name = 'preempted'
def __init__(self, *args, **kwargs):
super(PreemptableStateMachine, self).__init__(*args, **kwargs)
# always listen to preempt so that the behavior can be stopped even if unsupervised
self._preempt_topic = 'flexbe/command/preempt'
self._sub = ProxySubscriberCached({self._preempt_topic: Empty})
self._sub.set_callback(self._preempt_topic, self._preempt_cb)
def _preempt_cb(self, msg):
if not self._is_controlled:
PreemptableState.preempt = True
@staticmethod
def add(label, state, transitions=None, remapping=None):
transitions[PreemptableState.
_preempted_name] = PreemptableStateMachine._preempted_name
LockableStateMachine.add(label, state, transitions, remapping)
@property
def _valid_targets(self):
return super(PreemptableStateMachine, self)._valid_targets + [
PreemptableStateMachine._preempted_name
]
def __init__(self):
'''Constructor'''
super(AttachObjectState,
self).__init__(outcomes=['done', 'failed'],
input_keys=['template_id', 'hand_side'],
output_keys=['template_pose'])
# Set up service for attaching object template
self._service_topic = "/stitch_object_template"
self._srv = ProxyServiceCaller(
{self._service_topic: SetAttachedObjectTemplate})
# Set up subscribers for listening to the latest wrist poses
self._wrist_pose_topics = dict()
self._wrist_pose_topics['left'] = '/flor/l_arm_current_pose'
self._wrist_pose_topics['right'] = '/flor/r_arm_current_pose'
self._sub = ProxySubscriberCached({
self._wrist_pose_topics['left']:
PoseStamped,
self._wrist_pose_topics['right']:
PoseStamped
})
self._sub.make_persistant(self._wrist_pose_topics['left'])
self._sub.make_persistant(self._wrist_pose_topics['right'])
self._failed = False
class StorePointcloudState(EventState):
'''
Stores the latest pointcloud of the given topic.
-- topic string The topic on which to listen for the pointcloud.
#> pointcloud PointCloud2 The received pointcloud.
<= done Pointcloud has been received and stored.
'''
def __init__(self, topic):
super(StorePointcloudState, self).__init__(outcomes = ['done'],
output_keys = ['pointcloud'])
self._sub = ProxySubscriberCached({topic: PointCloud2})
self._pcl_topic = topic
def execute(self, userdata):
if self._sub.has_msg(self._pcl_topic):
userdata.pointcloud = self._sub.get_last_msg(self._pcl_topic)
return 'done'
def __init__(self):
'''Constructor'''
super(GetCameraImageState, self).__init__(outcomes=['done'],
output_keys=['camera_img'])
self._img_topic = "/multisense/camera/left/image_rect_color"
self._sub = ProxySubscriberCached({self._img_topic: Image})
def execute(self, userdata):
# This method is called periodically while the state is active.
# Main purpose is to check state conditions and trigger a corresponding outcome.
# If no outcome is returned, the state will stay active.
for i in [0, 1, 2, 3, 4, 5]:
self._topic = "/ariac/logical_camera_" + str(i)
self._start_time = rospy.Time.now()
(msg_path, msg_topic, fn) = rostopic.get_topic_type(self._topic)
if msg_topic == self._topic:
msg_type = self._get_msg_from_path(msg_path)
self._sub = ProxySubscriberCached({self._topic: msg_type})
elapsed = rospy.get_rostime() - self._start_time
while (elapsed.to_sec() < self._wait):
elapsed = rospy.get_rostime() - self._start_time
if self._sub.has_msg(self._topic):
message = self._sub.get_last_msg(self._topic)
for model in message.models:
if model.type == userdata.part_type:
userdata.bin = "bingr" + str(i) + "PreGrasp"
userdata.camera_topic = "/ariac/logical_camera_" + str(
i)
userdata.camera_frame = "logical_camera_" + str(
i) + "_frame"
Logger.loginfo("bingr" + str(i) + "PreGrasp")
userdata.ref_frame = "torso_main"
return 'continue'
Logger.loginfo("part_type not found")
return 'failed'
def execute(self, userdata):
# This method is called periodically while the state is active.
# Main purpose is to check state conditions and trigger a corresponding outcome.
# If no outcome is returned, the state will stay active.
# One of the outcomes declared above.
for i in [1,2,3,4,5,6]:
self._topic = "/ariac/logical_camera_stock"+str(i)
self._start_time = rospy.Time.now()
(msg_path, msg_topic, fn) = rostopic.get_topic_type(self._topic)
if msg_topic == self._topic:
msg_type = self._get_msg_from_path(msg_path)
self._sub = ProxySubscriberCached({self._topic: msg_type})
elapsed = rospy.get_rostime() - self._start_time;
while (elapsed.to_sec() < self._wait):
elapsed = rospy.get_rostime() - self._start_time;
if self._sub.has_msg(self._topic):
message = self._sub.get_last_msg(self._topic)
for model in message.models:
if model.type == userdata.part_type:
if userdata.arm_id == "Right_Arm":
userdata.gantry_pos = "Gantry_Part"+str(i)+"_R"
elif userdata.arm_id == "Left_Arm":
userdata.gantry_pos = "Gantry_Part"+str(i)+"_L"
userdata.camera_topic = "/ariac/logical_camera_stock"+str(i)
userdata.camera_frame = "logical_camera_stock"+str(i)+"_frame"
Logger.loginfo("Gantry_Part"+str(i))
if i < 5:
return 'bin'
else:
return 'shelf'
Logger.loginfo("part_type not found")
return 'not_found'
def __init__(self, hand, force_threshold=1.5):
'''Constructor'''
super(WaitForForceState,
self).__init__(outcomes=['success', 'failure'])
self._action_name = rospy.get_name()
self._prefix = "meka_roscontrol"
self._client = {}
self._movement_finished = {}
self.force_variation = {}
self.force_bias = {}
self.previous_force = {}
self._r = rospy.Rate(100)
self._threshold = force_threshold
# store latest planning scene
self.current_planning_scene = None
self.force_variation['left_arm'] = numpy.array([0.0, 0.0, 0.0])
self.force_variation['right_arm'] = numpy.array([0.0, 0.0, 0.0])
self.force_bias['left_arm'] = numpy.array([0.0, 0.0, 0.0])
self.force_bias['right_arm'] = numpy.array([0.0, 0.0, 0.0])
self.previous_force['left_arm'] = numpy.array([0.0, 0.0, 0.0])
self.previous_force['right_arm'] = numpy.array([0.0, 0.0, 0.0])
self._current_val = 0
self._group = hand + '_arm'
self.sub_left = ProxySubscriberCached(
{'/meka_ros_pub/m3loadx6_ma30_l0/wrench': WrenchStamped})
self.sub_right = ProxySubscriberCached(
{'/meka_ros_pub/m3loadx6_ma29_l0/wrench': WrenchStamped})
class ShowPictureWebinterfaceState(EventState):
'''
Displays the picture in a web interface
># Image Image The received Image
<= done Displaying the Picture
'''
def __init__(self):
super(ShowPictureWebinterfaceState, self).__init__(outcomes = ['tweet', 'forget'],
input_keys = ['image_name'])
self._pub_topic = '/webinterface/display_picture'
self._pub = ProxyPublisher({self._pub_topic: String})
self._sub_topic = '/webinterface/dialog_feedback'
self._sub = ProxySubscriberCached({self._sub_topic: String})
def execute(self, userdata):
if self._sub.has_msg(self._sub_topic):
msg = self._sub.get_last_msg(self._sub_topic)
if msg.data == 'yes':
print 'show_picture_webinterface_state, returning tweet'
return 'tweet'
else:
print 'show_picture_webinterface_state, returning forget'
return 'forget'
def on_enter(self,userdata):
self._sub.remove_last_msg(self._sub_topic)
self._pub.publish(self._pub_topic, String(userdata.image_name))
class DetectPersonState(EventState):
"""
Detects the nearest person and provides their pose.
-- wait_timeout float Time (seconds) to wait for a person before giving up.
#> person_pose PoseStamped Pose of the nearest person if one is detected, else None.
<= detected Detected a person.
<= not_detected No person detected, but time ran out.
"""
def __init__(self, wait_timeout):
super(DetectPersonState, self).__init__(outcomes=["detected", "not_detected"], output_keys=["person_pose"])
self._wait_timeout = rospy.Duration(wait_timeout)
self._topic = "/people_tracker/pose"
self._sub = ProxySubscriberCached({self._topic: PoseStamped})
self._start_waiting_time = None
def execute(self, userdata):
if rospy.Time.now() > self._start_waiting_time + self._wait_timeout:
userdata.person_pose = None
return "not_detected"
if self._sub.has_msg(self._topic):
userdata.person_pose = self._sub.get_last_msg(self._topic)
return "detected"
def on_enter(self, userdata):
self._sub.remove_last_msg(self._topic)
self._start_waiting_time = rospy.Time.now()
class GenericSub(EventState):
'''
GenericPubSub state subscribes to a given topic (Int32 message only) and will not move forward until a response is recieved.
This is meant to be a template to be modified to your needs it is not super useful in its current
state. Intended mostly for communicating with arduino
'''
def __init__(self, subtopic):
# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
super(GenericSub, self).__init__(outcomes=["completed", "failed"])
# Store state parameters for later use.
self._subtopic = subtopic
self._sub = ProxySubscriberCached({self._subtopic: Int32})
#rospy.init_node('reset_control', anonymous=True)
def execute(self, userdata):
#publish to arduino
#while(1):
if self._sub.has_msg(self._subtopic):
msg = self._sub.get_last_msg(self._subtopic)
print(msg)
# in case you want to make sure the same message is not processed twice:
self._sub.remove_last_msg(self._subtopic)
return "completed"
def __init__(self):
'''Constructor'''
super(GetLaserscanState, self).__init__(outcomes=['done'],
output_keys=['laserscan'])
self._scan_topic = "/multisense/lidar_scan"
self._sub = ProxySubscriberCached({self._scan_topic: LaserScan})
class CheckCurrentControlModeState(EventState):
'''
Implements a state where the robot checks its current control mode.
-- target_mode enum The control mode to check for being the current one (e.g. 3 for stand, 6 for manipulate).
The state's class variables can also be used (e.g. CheckCurrentControlModeState.STAND).
-- wait bool Whether to check once and return (False), or wait for the control mode to change (True).
#> control_mode enum The current control mode when the state returned.
<= correct Indicates that the current control mode is the target/expected one.
<= incorrect Indicates that the current control mode is not the target/expected one.
'''
NONE = 0
FREEZE = 1
STAND_PREP = 2
STAND = 3
STAND_MANIPULATE = 3
WALK = 4
STEP = 5
MANIPULATE = 6
USER = 7
CALIBRATE = 8
SOFT_STOP = 9
def __init__(self, target_mode, wait = False):
'''
Constructor
'''
super(CheckCurrentControlModeState, self).__init__(outcomes=['correct', 'incorrect'],
output_keys=['control_mode'])
self._status_topic = '/flor/controller/mode'
self._sub = ProxySubscriberCached({self._status_topic: VigirAtlasControlMode})
self._sub.make_persistant(self._status_topic)
self._target_mode = target_mode
self._wait = wait
def execute(self, userdata):
'''
Execute this state
'''
if self._sub.has_msg(self._status_topic):
msg = self._sub.get_last_msg(self._status_topic)
userdata.control_mode = msg.bdi_current_behavior
if msg.bdi_current_behavior == self._target_mode:
return 'correct'
elif not self._wait:
return 'incorrect'
def on_enter(self, userdata):
if self._wait:
Logger.loghint("Waiting for %s" % str(self._target_mode))
def __init__(self):
"""Constructor"""
super(KeepLookingAt, self).__init__(outcomes=['failed'],
input_keys=['ID'])
self.entities_topic = "/entities"
self._sub = ProxySubscriberCached({self.entities_topic: Entities})
self.pubp = rospy.Publisher("/sara_head_pitch_controller/command", Float64, queue_size=1)
self.puby = rospy.Publisher("/sara_head_yaw_controller/command", Float64, queue_size=1)
# Reference from and reference to
self.service = get_directionRequest()
self.service.reference = "head_xtion_link"
self.service.origine = "base_link"
self.Entity = None
self.serviceName = '/get_direction'
Logger.loginfo('waiting for service '+str(self.serviceName))
self.serv = rospy.ServiceProxy(self.serviceName, get_direction)
self._active = True
try:
self.serv.wait_for_service(1)
except:
self._active = False
class PreemptableStateMachine(LoopbackStateMachine):
"""
A state machine that can be preempted.
If preempted, the state machine will return the outcome preempted.
"""
_preempted_name = 'preempted'
def __init__(self, *args, **kwargs):
# add preempted outcome
if len(args) > 0:
args[0].append(self._preempted_name)
else:
outcomes = kwargs.get('outcomes', [])
outcomes.append(self._preempted_name)
kwargs['outcomes'] = outcomes
super(PreemptableStateMachine, self).__init__(*args, **kwargs)
# always listen to preempt so that the behavior can be stopped even if unsupervised
self._preempt_topic = 'flexbe/command/preempt'
self._sub = ProxySubscriberCached({self._preempt_topic: Empty})
self._sub.set_callback(self._preempt_topic, self._preempt_cb)
def _preempt_cb(self, msg):
if not self._is_controlled:
PreemptableState.preempt = True
rospy.loginfo("--> Preempted requested while unsupervised")
class GetLaserscanState(EventState):
"""
Grabs the most recent laserscan data.
#> laserscan LaserScan The current laser scan.
<= done Scanning data is available.
"""
def __init__(self):
"""Constructor"""
super(GetLaserscanState, self).__init__(outcomes=["done"], output_keys=["laserscan"])
self._scan_topic = "/multisense/lidar_scan"
self._sub = ProxySubscriberCached({self._scan_topic: LaserScan})
def execute(self, userdata):
if self._sub.has_msg(self._scan_topic):
userdata.laserscan = self._sub.get_last_msg(self._scan_topic)
return "done"
def on_enter(self, userdata):
pass
def __init__(self, topic = 'move_base_simple/goal'):
"""Constructor"""
super(GetPoseState, self).__init__(outcomes=['done'], output_keys=['goal'])
self._topic = topic
self._sub = ProxySubscriberCached({self._topic: PoseStamped})
self._goal_pose = None
def __init__(self,
target_time,
velocity,
rotation_rate,
cmd_topic='/create_node/cmd_vel',
sensor_topic='/create_node/sensor_state'):
# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
super(CreateTimedTwistState,
self).__init__(outcomes=['done', 'failed'])
# Store state parameter for later use.
self._target_time = rospy.Duration(target_time)
self._twist = TwistStamped()
self._twist.twist.linear.x = velocity
self._twist.twist.angular.z = rotation_rate
# The constructor is called when building the state machine, not when actually starting the behavior.
# Thus, we cannot save the starting time now and will do so later.
self._start_time = None
self._done = None # Track the outcome so we can detect if transition is blocked
self._sensor_topic = sensor_topic
self._cmd_topic = cmd_topic
self._sensor_sub = ProxySubscriberCached(
{self._sensor_topic: CreateSensorState})
self._pub = ProxyPublisher({self._cmd_topic: TwistStamped})
def __init__(self):
super(hsr_SpeechRecognitionByHeadMic,self).__init__(outcomes=['recognition'],output_keys=['recognition'])
self._topic = "/hsrb/voice/text"
self.Speech2Text_CB = ProxySubscriberCached({self._topic: RecognitionResult})
#self.led_cli = actionlib.SimpleActionClient('em_led_action', ledAction) # LED
self.goal = ledGoal()
p = subprocess.Popen("rosservice call /hsrb/voice/stop_recognition '{}'", shell=True)
class GetCameraImageState(EventState):
'''
Grabs the most recent camera image.
#> camera_img Image The current color image of the left camera.
<= done Image data is available.
'''
def __init__(self):
'''Constructor'''
super(GetCameraImageState, self).__init__(outcomes=['done'],
output_keys=['camera_img'])
self._img_topic = "/multisense/camera/left/image_rect_color"
self._sub = ProxySubscriberCached({self._img_topic: Image})
def execute(self, userdata):
if self._sub.has_msg(self._img_topic):
userdata.camera_img = self._sub.get_last_msg(self._img_topic)
return 'done'
def on_enter(self, userdata):
pass
class CheckCurrentControlModeState(EventState):
'''
Implements a state where the robot checks its current control mode.
-- target_mode enum The control mode to check for being the current one (e.g. 3 for stand, 6 for manipulate).
The state's class variables can also be used (e.g. CheckCurrentControlModeState.STAND).
-- wait bool Whether to check once and return (False), or wait for the control mode to change (True).
#> control_mode enum The current control mode when the state returned.
<= correct Indicates that the current control mode is the target/expected one.
<= incorrect Indicates that the current control mode is not the target/expected one.
'''
NONE = 0
FREEZE = 1
STAND_PREP = 2
STAND = 3
STAND_MANIPULATE = 3
WALK = 4
STEP = 5
MANIPULATE = 6
USER = 7
CALIBRATE = 8
SOFT_STOP = 9
def __init__(self, target_mode, wait=False):
'''
Constructor
'''
super(CheckCurrentControlModeState,
self).__init__(outcomes=['correct', 'incorrect'],
output_keys=['control_mode'])
self._status_topic = '/flor/controller/mode'
self._sub = ProxySubscriberCached(
{self._status_topic: VigirAtlasControlMode})
self._sub.make_persistant(self._status_topic)
self._target_mode = target_mode
self._wait = wait
def execute(self, userdata):
'''
Execute this state
'''
if self._sub.has_msg(self._status_topic):
msg = self._sub.get_last_msg(self._status_topic)
userdata.control_mode = msg.bdi_current_behavior
if msg.bdi_current_behavior == self._target_mode:
return 'correct'
elif not self._wait:
return 'incorrect'
def on_enter(self, userdata):
if self._wait:
Logger.loghint("Waiting for %s" % str(self._target_mode))
def __init__(self, sentence, input_keys=[], emotion=0, block=True):
"""Constructor"""
super(SaraSay, self).__init__(outcomes=['done'], input_keys=input_keys)
if not (isinstance(sentence, types.LambdaType) and sentence.__name__
== "<lambda>") and len(input_keys) is not 0:
raise ValueError(
"In state " + type(self).__name__ + " saying \"" + sentence +
"\", you need to define a lambda function for sentence.")
# Get parameters
self.msg = say()
self.sentence = sentence
self.emotion = UInt8()
self.emotion.data = emotion
self.block = block
# Set topics
self.emotion_topic = "sara_face/Emotion"
self.say_topic = "/say"
self.sayServiceTopic = "/wm_say"
self.emotionPub = ProxyPublisher({self.emotion_topic: UInt8})
# Prepare wm_tts
if self.block:
self.sayClient = ProxyServiceCaller(
{self.sayServiceTopic: say_service})
else:
self.sayPub = ProxyPublisher({self.say_topic: say})
# Initialise the face sub
self.emotionSub = ProxySubscriberCached({self.emotion_topic: UInt8})
self.lastEmotion = None
class MirrorState(EventState):
'''
This state will display its possible outcomes as buttons in the GUI
and is designed in a way to be created dynamically.
'''
def __init__(self, target_name, target_path, given_outcomes,
outcome_autonomy):
super(MirrorState, self).__init__(outcomes=given_outcomes)
self.set_rate(100)
self._target_name = target_name
self._target_path = target_path
self._outcome_topic = 'flexbe/mirror/outcome'
self._pub = ProxyPublisher()
self._sub = ProxySubscriberCached({self._outcome_topic: UInt8})
def execute(self, userdata):
if self._sub.has_buffered(self._outcome_topic):
msg = self._sub.get_from_buffer(self._outcome_topic)
if msg.data < len(self.outcomes):
rospy.loginfo("State update: %s > %s", self._target_name,
self.outcomes[msg.data])
return self.outcomes[msg.data]
try:
self.sleep()
except ROSInterruptException:
print('Interrupted mirror sleep.')
def on_enter(self, userdata):
self._pub.publish(
'flexbe/behavior_update',
String("/" + "/".join(self._target_path.split("/")[1:])))
def __init__(self, ref_frame, camera_topic, camera_frame):
# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
super(DetectPartCameraState,
self).__init__(outcomes=['continue', 'failed'],
output_keys=['pose'])
self.ref_frame = ref_frame
self._topic = camera_topic
self._camera_frame = camera_frame
self._connected = False
self._failed = False
# tf to transfor the object pose
self._tf_buffer = tf2_ros.Buffer(
rospy.Duration(10.0)) #tf buffer length
self._tf_listener = tf2_ros.TransformListener(self._tf_buffer)
# Subscribe to the topic for the logical camera
(msg_path, msg_topic, fn) = rostopic.get_topic_type(self._topic)
if msg_topic == self._topic:
msg_type = self._get_msg_from_path(msg_path)
self._sub = ProxySubscriberCached({self._topic: msg_type})
self._connected = True
else:
Logger.logwarn(
'Topic %s for state %s not yet available.\nFound: %s\nWill try again when entering the state...'
% (self._topic, self.name, str(msg_topic)))
def __init__(self):
"""
Constructor
"""
super(WaitForOpenDoorState, self).__init__(outcomes=['done'])
self._topic = '/laser/srd_front/scan'
self._sub = ProxySubscriberCached({self._topic: LaserScan})
def __init__(self, outcomes = ['unknown'], pose_ns = 'saved_msgs/joint_state', tolerance = 0.17, joint_topic = "joint_states", timeout = 1.0):
# Process supplied pose list. Note it must be processed befor superclass constructor is called,
# beacuse it changes 'outcomes' list due to side effect.
# check if 'unknown' outcome is present
if 'unknown' not in outcomes:
raise RuntimeError, '"unknown" should presents in state outcomes'
# remove 'unknown' while preserving items order
poses_names = [ outcome for outcome in outcomes if outcome != 'unknown' ]
# Load poses from paramere server. Pose is loaded in form dict(joint name -> position),
# but they are stored as a list of tuples (pose_name, pose_dict) to preserve order.
self._poses = [ (name, self.load_joint_state(pose_ns, name)) for name in poses_names ]
# Call superclass constructor.
super(CheckJointState, self).__init__(outcomes = outcomes)
# Subscribe to the joint topic.
self._joint_topic = joint_topic
self._pose_subscriber = ProxySubscriberCached({ self._joint_topic: JointState })
# Store topic parameter for later use.
self._tolerance = tolerance
self._timeout = Duration.from_sec(timeout)
# timestamp
self._timestamp = None
class GetLaserscanState(EventState):
'''
Grabs the most recent laserscan data.
#> laserscan LaserScan The current laser scan.
<= done Scanning data is available.
'''
def __init__(self):
'''Constructor'''
super(GetLaserscanState, self).__init__(outcomes=['done'],
output_keys=['laserscan'])
self._scan_topic = "/multisense/lidar_scan"
self._sub = ProxySubscriberCached({self._scan_topic: LaserScan})
def execute(self, userdata):
if self._sub.has_msg(self._scan_topic):
userdata.laserscan = self._sub.get_last_msg(self._scan_topic)
return 'done'
def on_enter(self, userdata):
pass
class GetCameraImageState(EventState):
'''
Grabs the most recent camera image.
#> camera_img Image The current color image of the left camera.
<= done Image data is available.
'''
def __init__(self):
'''Constructor'''
super(GetCameraImageState, self).__init__(outcomes = ['done'],
output_keys = ['camera_img'])
self._img_topic = "/multisense/camera/left/image_rect_color"
self._sub = ProxySubscriberCached({self._img_topic: Image})
def execute(self, userdata):
if self._sub.has_msg(self._img_topic):
userdata.camera_img = self._sub.get_last_msg(self._img_topic)
return 'done'
def on_enter(self, userdata):
pass
def __init__(self, distance=1, ReplanPeriod=0.5):
"""Constructor"""
super(SaraFollow, self).__init__(outcomes=['failed'],
input_keys=['ID'])
# Prepare the action client for move_base
self._action_topic = "/move_base"
self._client = ProxyActionClient({self._action_topic: MoveBaseAction})
# Initialise the subscribers
self.entities_topic = "/entities"
self._pose_topic = "/robot_pose"
self._sub = ProxySubscriberCached({
self._pose_topic: Pose,
self.entities_topic: Entities
})
# Get the parameters
self.targetDistance = distance
self.ReplanPeriod = ReplanPeriod
# Initialise the status variables
self.MyPose = None
self.lastPlanTime = 0
def __init__(self,
leap_motion_topic,
exit_states,
pose_topic='',
parameters=[2.0, 0.02, 0.2]):
super(LeapMotionMonitor, self).__init__(
outcomes=['no_object', 'still_object', 'moving_object'],
output_keys=['pose'])
# store state parameter for later use.
self._leap_topic = leap_motion_topic
self._pose_topic = pose_topic
self._exit_states = exit_states
self._detecton_period = parameters[0]
self._position_tolerance = parameters[1]
self._orientation_tolerance = parameters[2]
# setup proxies
self._leap_subscriber = ProxySubscriberCached(
{self._leap_topic: leapros})
if self._pose_topic:
self._pose_publisher = ProxyPublisher(
{self._pose_topic: PoseStamped})
else:
self._pose_publisher = None
# pose buffers
self._pose = None
self._pose_prev = None
self._pose_averaged = None
self._nonstillness_stamp = None
def __init__(self):
# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
super(Arm_Control_State_GR, self).__init__(outcomes = ["continue", "failed"])
self._pub = ProxyPublisher({"sim2real/reset_status": Int32})
self._sub = ProxySubscriberCached({"sim2real/reset": Int32})
class ContinueButton(EventState):
'''
Reads on a topic to see for the continue button.
<= done Continue if button pressed.
'''
def __init__(self):
'''
Constructor
'''
super(ContinueButton, self).__init__(outcomes=['true', 'false'])
self._topic = "/continue_button"
self._connected = False
self._sub = ProxySubscriberCached({self._topic: Bool})
def execute(self, userdata):
Logger.loginfo('Waiting for the continue button')
if self._sub.has_msg(self._topic):
message = self._sub.get_last_msg(self._topic)
self._sub.remove_last_msg(self._topic)
if message.data:
return 'true'
else:
return 'false'
class PreemptableStateMachine(LoopbackStateMachine):
"""
A state machine that can be preempted.
If preempted, the state machine will return the outcome preempted.
"""
_preempted_name = 'preempted'
def __init__(self, *args, **kwargs):
# add preempted outcome
if len(args) > 0:
args[0].append(self._preempted_name)
else:
outcomes = kwargs.get('outcomes', [])
outcomes.append(self._preempted_name)
kwargs['outcomes'] = outcomes
super(PreemptableStateMachine, self).__init__(*args, **kwargs)
# always listen to preempt so that the behavior can be stopped even if unsupervised
self._preempt_topic = 'flexbe/command/preempt'
self._sub = ProxySubscriberCached({self._preempt_topic: Empty})
self._sub.set_callback(self._preempt_topic, self._preempt_cb)
def _preempt_cb(self, msg):
if not self._is_controlled:
PreemptableState.preempt = True
class MonitorPerceptState(EventState):
'''
Monitors the hector worldmodel for percepts
and triggers a detection event if a percept has a high-enough support.
-- required_support float Support threshold which needs to be reached before an event is triggered.
-- percept_class string Class name of percepts to be monitored.
-- track_percepts bool Defines if this state should track previously detected percepts.
Recommended if it is not desired to react multiple times on the same percept.
#> object_id string Identifier of the detected object.
#> object_pose PoseStamped Pose of the detected object.
#> object_data float[] Data associated with this object.
<= detected Percept is detected.
'''
def __init__(self, required_support, percept_class, track_percepts = True):
'''
Constructor
'''
super(MonitorPerceptState, self).__init__(outcomes=['detected'],
output_keys=['object_id', 'object_pose', 'object_data'])
self._topic = '/worldmodel/objects'
self._sub = ProxySubscriberCached({self._topic: ObjectModel})
self._required_support = required_support
self._percept_class = percept_class
self._track_percepts = track_percepts
self._percepts = list()
def execute(self, userdata):
if self._sub.has_msg(self._topic):
msg = self._sub.get_last_msg(self._topic)
objects = filter(lambda obj:
#obj.state.state == ObjectState.ACTIVE \
obj.info.class_id == self._percept_class \
and obj.info.support >= self._required_support \
and obj.info.object_id not in self._percepts,
msg.objects
)
for obj in objects:
if self._track_percepts:
self._percepts.append(obj.info.object_id)
(x,y,z) = (obj.pose.pose.position.x, obj.pose.pose.position.y, obj.pose.pose.position.z)
Logger.loginfo('Detected %s percept with id: %s\nPosition: (%.1f, %.1f, %.1f)' % (self._percept_class, obj.info.object_id, x, y, z))
userdata.object_id = obj.info.object_id
userdata.object_pose = PoseStamped(header=obj.header, pose=obj.pose.pose)
userdata.object_data = obj.info.data
return 'detected'
class GetWristPoseState(EventState):
'''
Retrieves the current wrist pose from the corresponding ROS topic.
># hand_side string Wrist whose pose will be returned (left, right)
#> wrist_pose PoseStamped The current pose of the left or right wrist
<= done Wrist pose has been retrieved.
<= failed Failed to retrieve wrist pose.
'''
def __init__(self):
'''Constructor'''
super(GetWristPoseState, self).__init__(outcomes = ['done', 'failed'],
input_keys = ['hand_side'],
output_keys = ['wrist_pose'])
# Set up subscribers for listening to the latest wrist poses
self._wrist_pose_topics = dict()
self._wrist_pose_topics['left'] = '/flor/l_arm_current_pose'
self._wrist_pose_topics['right'] = '/flor/r_arm_current_pose'
self._sub = ProxySubscriberCached({
self._wrist_pose_topics['left']: PoseStamped,
self._wrist_pose_topics['right']: PoseStamped})
self._sub.make_persistant(self._wrist_pose_topics['left'])
self._sub.make_persistant(self._wrist_pose_topics['right'])
self._failed = False
def execute(self, userdata):
if self._failed:
return 'failed'
else:
return 'done'
def on_enter(self, userdata):
self._failed = False
# Read the current wrist pose and write to userdata
try:
wrist_pose_topic = self._wrist_pose_topics[userdata.hand_side]
wrist_pose = self._sub.get_last_msg(wrist_pose_topic)
userdata.wrist_pose = wrist_pose
rospy.loginfo('Retrieved %s wrist pose: \n%s',
userdata.hand_side, wrist_pose)
except Exception as e:
Logger.logwarn('Failed to get the latest wrist pose:\n%s' % str(e))
self._failed = True
return
def __init__(self):
super(GetRobotPose, self).__init__(outcomes = ['succeeded'], output_keys = ['pose'])
self._objectTopic = '/robot_pose'
self._sub = ProxySubscriberCached({self._objectTopic: PoseStamped})
self._succeeded = False
def __init__(self):
"""
Constructor
"""
self._sm = None
smach.set_loggers(
rospy.logdebug, rospy.logwarn, rospy.logdebug, rospy.logerr # hide SMACH transition log spamming
)
# 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 __init__(self):
'''Constructor'''
super(GetCameraImageState, self).__init__(outcomes = ['done'],
output_keys = ['camera_img'])
self._img_topic = "/multisense/camera/left/image_rect_color"
self._sub = ProxySubscriberCached({self._img_topic: Image})
def __init__(self):
'''
Constructor
'''
self._sm = None
# 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)
class MirrorState(EventState):
'''
This state will display its possible outcomes as buttons in the GUI and is designed in a way to be created dynamically.
'''
def __init__(self, target_name, target_path, given_outcomes, outcome_autonomy):
'''
Constructor
'''
super(MirrorState, self).__init__(outcomes=given_outcomes)
self._rate = rospy.Rate(100)
self._given_outcomes = given_outcomes
self._outcome_autonomy = outcome_autonomy
self._target_name = target_name
self._target_path = target_path
self._outcome_topic = 'flexbe/mirror/outcome'
self._pub = ProxyPublisher() #{'flexbe/behavior_update': String}
self._sub = ProxySubscriberCached({self._outcome_topic: UInt8})
def execute(self, userdata):
'''
Execute this state
'''
if JumpableStateMachine.refresh:
JumpableStateMachine.refresh = False
self.on_enter(userdata)
if self._sub.has_buffered(self._outcome_topic):
msg = self._sub.get_from_buffer(self._outcome_topic)
if msg.data < len(self._given_outcomes):
rospy.loginfo("State update: %s > %s", self._target_name, self._given_outcomes[msg.data])
return self._given_outcomes[msg.data]
try:
self._rate.sleep()
except ROSInterruptException:
print 'Interrupted mirror sleep.'
def on_enter(self, userdata):
#rospy.loginfo("Mirror entering %s", self._target_path)
self._pub.publish('flexbe/behavior_update', String("/" + "/".join(self._target_path.split("/")[1:])))
def __init__(self, topic):
'''Constructor'''
super(TestSubState, self).__init__(outcomes=['received', 'unavailable'],
output_keys=['result'])
self._topic = topic
self._sub = ProxySubscriberCached({self._topic: String})
self._msg_counter = 0
self._timeout = rospy.Time.now() + rospy.Duration(1.5)
def __init__(self, wait_timeout):
super(DetectPersonState, self).__init__(outcomes=["detected", "not_detected"], output_keys=["person_pose"])
self._wait_timeout = rospy.Duration(wait_timeout)
self._topic = "/people_tracker/pose"
self._sub = ProxySubscriberCached({self._topic: PoseStamped})
self._start_waiting_time = None
def __init__(self):
super(ShowPictureWebinterfaceState, self).__init__(outcomes = ['tweet', 'forget'],
input_keys = ['image_name'])
self._pub_topic = '/webinterface/display_picture'
self._pub = ProxyPublisher({self._pub_topic: String})
self._sub_topic = '/webinterface/dialog_feedback'
self._sub = ProxySubscriberCached({self._sub_topic: String})
class PreemptableState(LoopbackState):
"""
A state that can be preempted.
If preempted, the state will not be executed anymore and return the outcome preempted.
"""
_preempted_name = "preempted"
preempt = False
switching = False
def __init__(self, *args, **kwargs):
# add preempted outcome
if len(args) > 0 and type(args[0]) is list:
# need this ugly check for list type, because first argument in CBState is the callback
args[0].append(self._preempted_name)
else:
outcomes = kwargs.get("outcomes", [])
outcomes.append(self._preempted_name)
kwargs["outcomes"] = outcomes
super(PreemptableState, self).__init__(*args, **kwargs)
self.__execute = self.execute
self.execute = self._preemptable_execute
self._feedback_topic = "/flexbe/command_feedback"
self._preempt_topic = "/flexbe/command/preempt"
self._pub = ProxyPublisher()
self._sub = ProxySubscriberCached()
def _preemptable_execute(self, *args, **kwargs):
preempting = False
if self._is_controlled and self._sub.has_msg(self._preempt_topic):
self._sub.remove_last_msg(self._preempt_topic)
self._pub.publish(self._feedback_topic, CommandFeedback(command="preempt"))
preempting = True
rospy.loginfo("--> Behavior will be preempted")
if PreemptableState.preempt:
PreemptableState.preempt = False
preempting = True
rospy.loginfo("Behavior will be preempted")
if preempting:
self.service_preempt()
self._force_transition = True
return self._preempted_name
return self.__execute(*args, **kwargs)
def _enable_ros_control(self):
super(PreemptableState, self)._enable_ros_control()
self._pub.createPublisher(self._feedback_topic, CommandFeedback)
self._sub.subscribe(self._preempt_topic, Empty)
PreemptableState.preempt = False
def _disable_ros_control(self):
super(PreemptableState, self)._disable_ros_control()
self._sub.unsubscribe_topic(self._preempt_topic)
def __init__(self):
# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
super(MarkPoint, self).__init__(outcomes = ['succeeded'], output_keys = ['pose'])
#self._topic = '/move_base/simple_goal'
#self._pub = ProxyPublisher({self._topic: PoseStamped})
self._objectTopic = '/robot_pose'
self._sub = ProxySubscriberCached({self._objectTopic: PoseStamped})
def __init__(self, topic, timeout = 0):
super(GetPointcloudState, self).__init__(outcomes = ['done', 'timeout'],
output_keys = ['pointcloud'])
self._sub = ProxySubscriberCached({topic: PointCloud2})
self._pcl_topic = topic
self._timeout = timeout
self._timeout_time = None
class DetectObjectNew(EventState):
'''
Observe the update of objects in the worldmodel.
># type string object_type to be found
># state string state the object should be in
<= found Object was found
#> pose PoseStamped Pose of the object, which was detected
#> object_id string object_id of detected object
'''
def __init__(self):
super(DetectObjectNew, self).__init__(outcomes = ['found'], input_keys =['type', 'state', 'pose'], output_keys = ['pose', 'object_id'])
self._objectTopic = '/worldmodel/object'
self._sub = ProxySubscriberCached({self._objectTopic: Object})
def execute(self, userdata):
current_obj = self._sub.get_last_msg(self._objectTopic)
if current_obj:
if current_obj.info.class_id == userdata.type and current_obj.state.state == userdata.state:
userdata.pose = PoseStamped()
userdata.pose.pose = current_obj.pose.pose
userdata.pose.header.stamp = Time.now()
userdata.pose.header.frame_id = 'map'
userdata.object_id = current_obj.info.object_id
Logger.loginfo('detected %(x)s' % {
'x': current_obj.info.object_id
})
return 'found'
def on_enter(self, userdata):
pass
def on_exit(self, userdata):
pass
def on_start(self):
pass
def on_stop(self):
pass
class DetectObject(EventState):
'''
Observe the update of objects (victims) in the worldmodel. When its state is 'active', we return it.
># pose PoseStamped Pose of object
<= found Victim was found
#> pose PoseStamped Pose of the object, which was detected
#> victim string object_id of detected victim
'''
def __init__(self):
super(DetectObject, self).__init__(outcomes = [ 'found'], input_keys =['pose'], output_keys = ['pose', 'victim'])
self._objectTopic = '/worldmodel/object'
self._sub = ProxySubscriberCached({self._objectTopic: Object})
def execute(self, userdata):
current_obj = self._sub.get_last_msg(self._objectTopic)
if current_obj:
if current_obj.info.class_id == 'victim' and current_obj.state.state == 2:
userdata.pose = PoseStamped()
userdata.pose.pose = current_obj.pose.pose
userdata.pose.header.stamp = Time.now()
userdata.pose.header.frame_id = 'map'
userdata.victim = current_obj.info.object_id
Logger.loginfo('detected %(x)s' % {
'x': current_obj.info.object_id
})
return 'found'
def on_enter(self, userdata):
pass
def on_exit(self, userdata):
pass
def on_start(self):
pass
def on_stop(self):
pass
class DetectPersonState(EventState):
'''
Detects the nearest person and provides their pose.
-- wait_timeout float Time (seconds) to wait for a person before giving up.
#> person_pose PoseStamped Pose of the nearest person if one is detected, else None.
<= detected Detected a person.
<= not_detected No person detected, but time ran out.
'''
def __init__(self, wait_timeout):
super(DetectPersonState, self).__init__(outcomes = ['detected', 'not_detected'],
output_keys = ['person_pose'])
self._wait_timeout = rospy.Duration(wait_timeout)
self._topic = '/people_tracker/pose'
self._sub = ProxySubscriberCached({self._topic: PoseStamped})
self._start_waiting_time = None
def execute(self, userdata):
if rospy.Time.now() > self._start_waiting_time + self._wait_timeout:
userdata.person_pose = None
print 'detect_person_state: did not detect any person'
return 'not_detected'
if self._sub.has_msg(self._topic):
userdata.person_pose = self._sub.get_last_msg(self._topic)
print 'detect_person_state: detected a person'
return 'detected'
def on_enter(self, userdata):
print 'detect_person_state: trying to detect a person'
self._sub.remove_last_msg(self._topic)
self._start_waiting_time = rospy.Time.now()
class TakePictureState(EventState):
'''
Stores the picture of the given topic.
#> Image Image The received pointcloud.
<= done The picture has been received and stored.
'''
def __init__(self):
super(TakePictureState, self).__init__(outcomes = ['done'], output_keys = ['Image'])
self._topic = '/head_xtion/rgb/image_rect_color'
self._sub = ProxySubscriberCached({self._topic:Image})
def execute(self, userdata):
if self._sub.has_msg(self._topic):
userdata.Image = self._sub.get_last_msg(self._topic)
return 'done'
def on_enter(self, userdata):
if not self._connected:
(msg_path, msg_topic, fn) = rostopic.get_topic_type(self._topic)
if msg_topic == self._topic:
msg_type = self._get_msg_from_path(msg_path)
self._sub = ProxySubscriberCached({self._topic: msg_type})
self._connected = True
Logger.loginfo('Successfully subscribed to previously unavailable topic %s' % self._topic)
else:
Logger.logwarn('Topic %s still not available, giving up.' % self._topic)
if self._connected and self._clear and self._sub.has_msg(self._topic):
self._sub.remove_last_msg(self._topic)
def __init__(self, *args, **kwargs):
super(OperatableStateMachine, self).__init__(*args, **kwargs)
self._message = None
self.id = None
self.autonomy = None
self._autonomy = {}
self._ordered_states = []
self._pub = ProxyPublisher()
self._sub = ProxySubscriberCached()
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()
class GetPointcloudState(EventState):
'''
Retrieves the latest pointcloud on the given topic.
-- topic string The topic on which to listen for the pointcloud.
-- timeout float Optional timeout in seconds of waiting for a pointclod.
Set to 0 in order to wait forever.
#> pointcloud PointCloud2 The received pointcloud.
<= done Pointcloud has been received and is now available in userdata.
<= timeout No pointcloud has been received, but maximum waiting time has passed.
'''
def __init__(self, topic, timeout = 0):
super(GetPointcloudState, self).__init__(outcomes = ['done', 'timeout'],
output_keys = ['pointcloud'])
self._sub = ProxySubscriberCached({topic: PointCloud2})
self._pcl_topic = topic
self._timeout = timeout
self._timeout_time = None
def execute(self, userdata):
if self._sub.has_msg(self._pcl_topic):
userdata.pointcloud = self._sub.get_last_msg(self._pcl_topic)
return 'done'
if self._timeout_time < rospy.Time.now():
return 'timeout'
def on_enter(self, userdata):
self._timeout_time = rospy.Time.now() + rospy.Duration(self._timeout)
def __init__(self):
'''
Constructor
'''
self.be = None
Logger.initialize()
#ProxyPublisher._simulate_delay = True
#ProxySubscriberCached._simulate_delay = True
# prepare temp folder
rp = rospkg.RosPack()
self._tmp_folder = os.path.join(rp.get_path('flexbe_onboard'), 'tmp/')
if not os.path.exists(self._tmp_folder):
os.makedirs(self._tmp_folder)
sys.path.append(self._tmp_folder)
# prepare manifest folder access
manifest_folder = os.path.join(rp.get_path('flexbe_behaviors'), 'behaviors/')
rospy.loginfo("Parsing available behaviors...")
file_entries = [os.path.join(manifest_folder, filename) for filename in os.listdir(manifest_folder) if not filename.startswith('#')]
manifests = sorted([xmlpath for xmlpath in file_entries if not os.path.isdir(xmlpath)])
self._behavior_lib = dict()
for i in range(len(manifests)):
m = ET.parse(manifests[i]).getroot()
e = m.find("executable")
self._behavior_lib[i] = {"name": m.get("name"), "package": e.get("package_path").split(".")[0], "file": e.get("package_path").split(".")[1], "class": e.get("class")}
# rospy.loginfo("+++ " + self._behavior_lib[i]["name"])
self._pub = ProxyPublisher()
self._sub = ProxySubscriberCached()
self.status_topic = '/flexbe/status'
self.feedback_topic = '/flexbe/command_feedback'
self._pub.createPublisher(self.status_topic, BEStatus, _latch = True)
self._pub.createPublisher(self.feedback_topic, CommandFeedback)
# listen for new behavior to start
self._running = False
self._switching = False
self._sub.subscribe('/flexbe/start_behavior', BehaviorSelection, self._behavior_callback)
# heartbeat
self._pub.createPublisher('/flexbe/heartbeat', Empty)
self._execute_heartbeat()
rospy.sleep(0.5) # wait for publishers etc to really be set up
self._pub.publish(self.status_topic, BEStatus(code=BEStatus.READY))
rospy.loginfo('\033[92m--- Behavior Engine ready! ---\033[0m')