def test_publish_subscribe(self):
t1 = '/pubsub_1'
t2 = '/pubsub_2'
pub = ProxyPublisher({t1: String})
pub = ProxyPublisher({t2: String}, _latch=True)
sub = ProxySubscriberCached({t1: String})
self.assertTrue(pub.is_available(t1))
self.assertTrue(pub.wait_for_any(t1))
self.assertFalse(pub.wait_for_any(t2))
pub.publish(t1, String('1'))
pub.publish(t2, String('2'))
rospy.sleep(
.5) # make sure latched message is sent before subscriber is added
sub = ProxySubscriberCached({t2: String})
rospy.sleep(
.5) # make sure latched message can be received before checking
self.assertTrue(sub.has_msg(t1))
self.assertEqual(sub.get_last_msg(t1).data, '1')
sub.remove_last_msg(t1)
self.assertFalse(sub.has_msg(t1))
self.assertIsNone(sub.get_last_msg(t1))
self.assertTrue(sub.has_msg(t2))
self.assertEqual(sub.get_last_msg(t2).data, '2')
class GetClosestEntity(EventState):
'''
return the closest entity from the list in parameter. If no list in parameter, return the closest entity from the topic /entities
># entityList object[] List of entities.
#> entityFound object The found entity.
<= done Indicates completion of the extraction.
<= not_found Indicates completion of the extraction.
'''
def __init__(self):
'''Constructor'''
super(GetClosestEntity, self).__init__(outcomes=['done', 'not_found'],
input_keys=['entityList'],
output_keys=['output_entity'])
self._sub = ProxySubscriberCached({'/entities': Entities})
self._subpos = ProxySubscriberCached({'/robot_pose': Pose})
self.mypose = None
def dist(self, entity1, entity2):
x = entity1.position.x - entity2.position.x
y = entity1.position.y - entity2.position.y
return math.sqrt(x * x + y * y)
def execute(self, userdata):
'''Execute this state'''
if self._subpos.has_msg('/robot_pose'):
self.mypose = self._subpos.get_last_msg('/robot_pose')
if len(userdata.entityList) == 0:
Logger.loginfo('va chercher la liste entities')
if self._sub.has_msg('/entities'):
myList = self._sub.get_last_msg('/entities')
self._sub.remove_last_msg('/entities')
myList = myList.entities
Logger.loginfo('a la liste entities')
else:
myList = userdata.entityList
Logger.loginfo('a la liste en parametre')
if len(myList) == 0:
Logger.loginfo('liste vide')
return "not_found"
min_dist = 99999
closestEntity = myList[0]
for entity in myList:
if self.dist(self.mypose, entity) < min_dist:
min_dist = self.dist(self.mypose, entity)
closestEntity = entity
userdata.output_entity = closestEntity
return 'done'
class ReadTorque(EventState):
'''
Reads torque of a joint.
<= done torque increase has been detected.
'''
def __init__(self, watchdog, Joint, Threshold, min_time):
super(ReadTorque,
self).__init__(outcomes=['threshold', 'watchdog', 'fail'],
output_keys=['torque'])
self.watchdog = watchdog
self.Threshold = Threshold
self.Joint = Joint
self.min_time = min_time
self.timer = 0
self._topic = "/joint_states"
self.watchdogTime = None
self._sub = ProxySubscriberCached({self._topic: JointState})
self.initialTorque = None
def execute(self, userdata):
if self.initialTorque is None:
self.watchdogTime = get_time() + self.watchdog
if self._sub.has_msg(self._topic):
message = self._sub.get_last_msg(self._topic)
for i in range(len(message.name)):
if message.name[i] == self.Joint:
self.Joint = i
break
self.initialTorque = message.effort[self.Joint]
print("Initial torque is:" + str(self.initialTorque))
else:
if self._sub.has_msg(self._topic):
torque = self._sub.get_last_msg(self._topic).effort[self.Joint]
if abs(self.initialTorque - torque) >= self.Threshold:
Logger.loginfo('Reading torque :' +
str(abs(self.initialTorque - torque)))
if self.timer < get_time():
userdata.torque = torque
return 'threshold'
else:
self.timer = get_time() + self.min_time
if (self.watchdogTime - get_time() <= 0):
return "watchdog"
def on_enter(self, userdata):
self.initialTorque = 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 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 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'
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 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 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 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))
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))
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 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"
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 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)
class TurtlebotStatusState(EventState):
"""
The state monitors the Kobuki turtlebot bumper and cliff sensors and will
return an outcome if a one is activated.
-- bumper_topic string The topic that the bumper events are published
-- cliff_topic string The topic that the cliff events are published
<= bumper A bumper was activated
<= cliff The cliff sensor
"""
def __init__(self,
bumper_topic='mobile_base/events/bumper',
cliff_topic='mobile_base/events/cliff'):
super(TurtlebotStatusState,
self).__init__(outcomes=['bumper', 'cliff'])
self._bumper_topic = bumper_topic
self._cliff_topic = cliff_topic
self._bumper_sub = ProxySubscriberCached(
{self._bumper_topic: BumperEvent})
self._cliff_sub = ProxySubscriberCached(
{self._cliff_topic: CliffEvent})
self._return = None # Handle return in case outcome is blocked by low autonomy
def execute(self, userdata):
if self._bumper_sub.has_msg(self._bumper_topic):
sensor = self._bumper_sub.get_last_msg(self._bumper_topic)
self._bumper_sub.remove_last_msg(self._bumper_topic)
if sensor.state > 0:
Logger.logwarn('Bumper %d contact' % (sensor.bumper))
self._return = 'bumper'
if self._cliff_sub.has_msg(self._cliff_topic):
sensor = self._cliff_sub.get_last_msg(self._cliff_topic)
self._cliff_sub.remove_last_msg(self._cliff_topic)
if sensor.state > 0:
Logger.logwarn('Cliff alert')
self._return = 'cliff'
return self._return
def on_enter(self, userdata):
self._return = None # Clear the completion flag
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 SpeakState(EventState):
"""
Genie TTS request
"""
def __init__(self):
super(SpeakState, self).__init__(outcomes=['done'],
input_keys=['tts_text'])
self.tts_topic = '/genie_tts'
self.state_topic = '/genie_state'
self.tts_pub = ProxyPublisher({self.tts_topic: String})
self.state_sub = ProxySubscriberCached({self.state_topic: UInt8})
def execute(self, userdata):
if self.state_sub.has_msg(self.state_topic):
# get genie state
state = self.state_sub.get_last_msg(self.state_topic)
self.state_sub.remove_last_msg(self.state_topic)
# non-speaking state
if state.data == 0:
return 'done'
def on_enter(self, userdata):
tts_text = String()
tts_text.data = userdata.tts_text
check_rate = rospy.Rate(10)
while self.state_sub.has_msg(self.state_topic):
# get genie state
state = self.state_sub.get_last_msg(self.state_topic)
self.state_sub.remove_last_msg(self.state_topic)
# non-speaking state
if state.data == 0:
break
check_rate.sleep()
# send tts request
self.tts_pub.publish(self.tts_topic, tts_text)
class CupboardDoorDetector(EventState):
'''
Detect if door is open
REF : https://github.com/WalkingMachine/wm_door_detector
-- timeout limit time before conclusion
<= done Finish job.
<= failed Job as failed.
'''
def __init__(self, timeout):
# See example_state.py for basic explanations.
super(CupboardDoorDetector, self).__init__(outcomes=['open', 'closed'])
self.distances = []
self._topic = "/scan"
self._sub = ProxySubscriberCached({self._topic: LaserScan})
self.time = 0
self.timeout = timeout
def avg(self, lst):
return sum(lst) / max(len(lst), 1)
def interval(self, lst):
return max(lst) - min(lst)
def process_scan(self, scan_msg):
# Extract the interval in a restricted range to determine if the is a closed door
middle_index = len(scan_msg.ranges) / 2
ranges_at_center = scan_msg.ranges[middle_index - 40:middle_index + 40]
interval_of_door = self.interval(ranges_at_center)
print("dist = " + str(interval_of_door))
if interval_of_door < 0.1 or interval_of_door > 0.5 or max(
ranges_at_center) > 0.85:
rospy.loginfo("Distance to door is more than a meter")
return "open"
if (self.time - get_time() <= 0):
Logger.loginfo('no speech detected')
return 'closed'
def execute(self, userdata):
if self._sub.has_msg(self._topic):
message = self._sub.get_last_msg(self._topic)
return self.process_scan(message)
def on_enter(self, userdata):
rospy.loginfo("Waiting for door...")
self.time = get_time() + self.timeout
def on_exit(self, userdata):
rospy.loginfo("The door is now open")
class hsr_GetWrench(EventState):
'''
Example for a state to demonstrate which functionality is available for state implementation.
This example lets the behavior wait until the given target_time has passed since the behavior has been started.
-- threshold float StartBottun(20.0>0), PutControl(-15.0<0), Ignore(0.0)
<= completed Completed
<= failed Failed
'''
def __init__(self, threshold=-15.0):
super(hsr_GetWrench, self).__init__(outcomes=['completed', 'failed'])
self.threshold = threshold
self._topic = '/hsrb/wrist_wrench/raw'
self.sub_wrench = ProxySubscriberCached({self._topic: WrenchStamped})
def execute(self, userdata):
if self.sub_wrench.has_msg(self._topic):
msg = self.sub_wrench.get_last_msg(self._topic)
self.sub_wrench.remove_last_msg(self._topic)
if self.threshold < 0.0 and msg.wrench.force.x < self.threshold:
print "Move on Gripper Open Phase."
return "completed"
if self.threshold > 0.0 and msg.wrench.force.x > self.threshold:
print "Start the Task."
return "completed"
if self.threshold == 0.0:
print "Ignore the this state"
return "completed"
else:
rospy.loginfo("Cannot subscribe the topic: %s", self._topic)
return "failed"
def on_enter(self, userdata):
pass
#self._sub_wrench = ProxySubscriberCached({self._topic: WrenchStamped})
def on_exit(self, userdata):
pass
def on_start(self):
pass
def on_stop(self):
pass
class Reset_Control_State_GR(EventState):
'''
Reset control takes in the trial information from Test control and on a succesful completion starts
the Data Control. If all trials are completed then it will loop back to Test control. Direction is
used for determining a successful and unsuccseful outcome for testing purposed but will need to be
replaced with a different measure of success once it becomes more fleshed out.
TODO: More complex information for trials, update info
-- direction int TEMPORARY: Determines a succesful (1) or unsuccesful (0) outcome for testing purposes
># number_of_trials Trial information (currently just an int)
<= continue All actions completed
<= failed Trial control failed to initialize or call something TODO: Proper error checking
<= completed All Trials have been succesfully completed, go back to Test control
'''
def __init__(self):
# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
super(Reset_Control_State_GR, self).__init__(outcomes = ["continue", "failed"], input_keys=["rotation"])
# Store state parameters for later use.
self._rotation = None
self._pub = ProxyPublisher({"reset_start": Int32})
self._sub = ProxySubscriberCached({"reset_complete": Int32})
#rospy.init_node('reset_control', anonymous=True)
def execute(self, userdata):
#publish to arduino
#while(1):
if self._sub.has_msg("reset_complete"):
msg = self._sub.get_last_msg("reset_complete")
print(msg)
# in case you want to make sure the same message is not processed twice:
self._sub.remove_last_msg("reset_complete")
return "continue"
def on_enter(self, userdata):
#self._rotation = 5
#Initializes class variable from userdata, has to be done outside of constructor
if(self._rotation is None and userdata.rotation is not None):
self._rotation = userdata.rotation
self._pub.publish("reset_start", self._rotation)
class GetPoseState(EventState):
"""
Grabs the most recent published PoseStamped.
-- topic String The topic to subscribe to
#> goal PointStamped The goal.
<= done Goal PoseStamped is available.
"""
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 execute(self, userdata):
if self._sub.has_msg(self._topic):
Logger.loginfo('%s Received new goal' % (self.name))
self._goal_pose = self._sub.get_last_msg(self._topic)
self._sub.remove_last_msg(self._topic)
if self._goal_pose is not None:
userdata.goal = self._goal_pose
return 'done'
def on_enter(self, userdata):
userdata.goal = None
self._goal_pose = None
if self._sub.has_msg(self._topic):
Logger.loginfo('%s Clearing prior goal' % (self.name))
self._sub.remove_last_msg(self._topic)
Logger.loghint('%s Input new 2D Nav goal (e.g. via RViz) ' % (self.name))
class hsr_SpeechRecognitionByHeadMic(EventState):
'''
Example for a state to demonstrate which functionality is available for state implementation.
This example lets the behavior wait until the given target_time has passed since the behavior has been started.
#> recognition String speech recognition
<= recognition recognition
'''
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)
def execute(self, userdata):
if self.Speech2Text_CB.has_msg(self._topic):
text = self.Speech2Text_CB.get_last_msg(self._topic)
result = text.sentences
rospy.loginfo('[Subscribe %s] "%s"', self._topic, result[0])
print result[0]
userdata.recognition = result[0]
self.Speech2Text_CB.remove_last_msg(self._topic)
return 'recognition'
def on_enter(self, userdata):
#self.led_cli.wait_for_server()
self.goal.mode = 2
self.goal.color = ColorRGBA(r=0.0, g=1.0, b=0.0, a=0.0)
self.goal.flash_interval = 5
#self.led_cli.send_goal(self.goal)
p = subprocess.Popen("rosservice call /hsrb/voice/start_recognition '{}'", shell=True)
def on_exit(self, userdata):
self.goal.mode = 1
self.goal.color = ColorRGBA(r=0.0, g=0.0, b=1.0, a=0.0)
self.goal.flash_interval = 0
#self.led_cli.send_goal(self.goal)
p = subprocess.Popen("rosservice call /hsrb/voice/stop_recognition '{}'", shell=True)
def on_start(self):
pass
def on_stop(self):
pass
class hsr_SpeechRecognition(EventState):
'''
Example for a state to demonstrate which functionality is available for state implementation.
This example lets the behavior wait until the given target_time has passed since the behavior has been started.
-- topic string topic
#> recognition String speech recognition
<= recognition recognition
'''
def __init__(self, topic):
super(hsr_SpeechRecognition,
self).__init__(outcomes=['recognition'],
output_keys=['recognition'])
self._topic = topic
self.Speech2Text_CB = ProxySubscriberCached({self._topic: String})
# self.led_cli = actionlib.SimpleActionClient('em_led_action', ledAction)
# self.goal = ledGoal()
def execute(self, userdata):
if self.Speech2Text_CB.has_msg(self._topic):
text = self.Speech2Text_CB.get_last_msg(self._topic)
rospy.loginfo('[Subscribe %s] "%s"', self._topic, text.data)
userdata.recognition = text.data
self.Speech2Text_CB.remove_last_msg(self._topic)
return 'recognition'
def on_enter(self, userdata):
pass
# self.led_cli.wait_for_server()
# self.goal.mode = 2
# self.goal.color = ColorRGBA(r=0.0, g=1.0, b=0.0, a=0.0)
# self.goal.flash_interval = 5
# self.led_cli.send_goal(self.goal)
def on_exit(self, userdata):
pass
def on_start(self):
pass
def on_stop(self):
pass
class AutonomousState(EventState):
'''
Project11 state where backseat driver is in control with mission manager executing missions and commands.
'''
def __init__(self):
# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
super(AutonomousState, self).__init__(outcomes=['manual', 'standby'])
self.subscribers = ProxySubscriberCached({'/joy': Joy})
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.
if self.subscribers.has_msg('/joy'):
msg = self.subscribers.get_last_msg('/joy')
print msg
if msg.buttons[0]:
return 'manual'
if msg.buttons[2]:
return 'standby'
def on_enter(self, userdata):
# This method is called when the state becomes active, i.e. a transition from another state to this one is taken.
# It is primarily used to start actions which are associated with this state.
pass
def on_exit(self, userdata):
# This method is called when an outcome is returned and another state gets active.
# It can be used to stop possibly running processes started by on_enter.
pass # Nothing to do in this example.
def on_start(self):
# This method is called when the behavior is started.
# If possible, it is generally better to initialize used resources in the constructor
# because if anything failed, the behavior would not even be started.
pass
def on_stop(self):
# This method is called whenever the behavior stops execution, also if it is cancelled.
# Use this event to clean up things like claimed resources.
pass # Nothing to do in this example.
class Project11StateBase():
'''
Base class for Project11 states.
Handles operations, such as interpreting jostick buttons, that are common to many states.
'''
def __init__(self):
self.base_subscribers = ProxySubscriberCached({'/joy': Joy})
def checkJoystick(self):
if self.base_subscribers.has_msg('/joy'):
msg = self.base_subscribers.get_last_msg('/joy')
state_request = None
if msg.buttons[0]:
state_request = 'manual'
if msg.buttons[1]:
state_request = 'autonomous'
if msg.buttons[2]:
state_request = 'standby'
return msg, state_request
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'
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'
class CreateStatusState(EventState):
"""
The state monitors the iRobot Create bumper status, and stops and returns a
failed outcome if a bumper is activated.
<= failed A bumper was activated prior to completion
"""
def __init__(self):
super(CreateStatusState, self).__init__(outcomes = ['failed'])
self._sensor_topic = '/create_node/sensor_state'
self._sensor_sub = ProxySubscriberCached({self._sensor_topic: CreateSensorState})
def execute(self, userdata):
if (self._sensor_sub.has_msg(self._sensor_topic)):
sensors = self._sub.get_last_msg(self._sensor_topic)
if ((sensors.bumps_wheeldrops > 0) or sensors.cliff_left or sensors.cliff_front_left or sensors.cliff_front_right or sensors.cliff_right):
Logger.logwarn('Bumper contact = %d cliff: left=%d %d %d %d = right ' %
(sensors.bumps_wheeldrops, sensors.cliff_left, sensors.cliff_front_left,sensors.cliff_front_right, sensors.cliff_right))
return 'failed'
class GetRobcoCommand(EventState):
'''
Listens to /robco/command and gets the incomming command.
#> robco_command_OUT String Last received robco command
<= failed If behavior is unable to send the TTS message
<= done TTS message sent succesfully
'''
def __init__(self):
super(GetRobcoCommand,
self).__init__(outcomes=['failed', 'done'],
output_keys=['robco_command_OUT'])
self._command_topic = '/robco/command'
self.command = None
#create subscriber
self.sub = ProxySubscriberCached({self._command_topic: String})
def execute(self, userdata):
if self.sub.has_msg(self._command_topic):
command = self._sub.get_last_msg(self._command_topic)
userdata.robco_command_OUT = command.data
Logger.loginfo('Incomming command on /robco/command topic: %s' %
command)
# in case you want to make sure the same message is not processed twice:
self._sub.remove_last_msg(self._command_topic)
return 'done'
def on_exit(self, userdata):
#TODO unsubscribe the subscriber proxy from that topic?
# self.sub.????
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
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()
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 Arm_Control_State_GR(EventState):
'''
Arm control takes in the trial information from Test control and on a succesful completion starts
the reset.
TODO: More complex information and better ways to call things also better description.
-- direction int TEMPORARY: Determines a succesful (1) or unsuccesful (0) outcome for testing purposes
># number_of_trials Trial information (currently just an int)
<= continue All actions completed
<= failed Trial control failed to initialize or call something TODO: Proper error checking
<= completed All Trials have been succesfully completed, go back to Test control
'''
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})
#rospy.init_node('reset_control', anonymous=True)
def execute(self, userdata):
if self._sub.has_msg("sim2real/reset"):
msg = self._sub.get_last_msg("sim2real/reset")
self._sub.remove_last_msg("sim2real/reset")
if(msg.data == 1):
return "continue"
def on_enter(self, userdata):
self._pub.publish("sim2real/reset_status", 1)
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)
class TestSubState(EventState):
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 execute(self, userdata):
if self._msg_counter == 0 and rospy.Time.now() > self._timeout:
return 'unavailable'
if self._sub.has_msg(self._topic):
msg = self._sub.get_last_msg(self._topic)
self._sub.remove_last_msg(self._topic)
userdata.result = msg.data
self._msg_counter += 1
if self._msg_counter == 3:
return 'received'
class GetEntityByID(EventState):
'''
Search for an entity by it's ID number
># ID int The ID of the entity
#> Entity object The entity found
<= found the entity has been found
<= not_found noting has been found
'''
def __init__(self):
'''
Constructor
'''
super(GetEntityByID, self).__init__(outcomes=['found', 'not_found'],
input_keys=['ID'],
output_keys=['Entity'])
self._sub = ProxySubscriberCached({'/entities': Entities})
self.message = None
def execute(self, userdata):
if self._sub.has_msg('/entities'):
Logger.loginfo('getting list of entities')
self.message = self._sub.get_last_msg('/entities')
self._sub.remove_last_msg('/entities')
if self.message is not None:
for entity in self.message.entities:
if entity.ID == userdata.ID:
userdata.Entity = entity
return 'found'
return 'not_found'
class GenericPubSub(EventState):
'''
GenericPubSub state publishes an integer (Int32) once to the topic given. It also 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.
'''
def __init__(self, pubtopic, subtopic, pubint):
# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
super(GenericPubSub, self).__init__(outcomes=["completed", "failed"])
# Store state parameters for later use.
self._rotation = None
self._pubtopic = pubtopic
self._subtopic = subtopic
self._pubint = pubint
self._pub = ProxyPublisher({self._pubtopic: Int32})
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 on_enter(self, userdata):
#self._rotation = 5
#Initializes class variable from userdata, has to be done outside of constructor
self._pub.publish(self._pubtopic, self._pubint)
class TestSubState(EventState):
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 execute(self, userdata):
if self._msg_counter == 0 and rospy.Time.now() > self._timeout:
return 'unavailable'
if self._sub.has_msg(self._topic):
msg = self._sub.get_last_msg(self._topic)
self._sub.remove_last_msg(self._topic)
userdata.result = msg.data
self._msg_counter += 1
if self._msg_counter == 3:
return 'received'
class TakeImageCamera(EventState):
'''
Return the image from the topic.
>= topic String the topic
<= image Image the image
<= received the image is receivec
<= unavailable can't subscribe
'''
def __init__(self, topic):
'''
Constructor
'''
super(TakeImageCamera,
self).__init__(outcomes=['received', 'unavailable'],
output_keys=['image'])
self._sub = ProxySubscriberCached({topic: Image})
self.topic = topic
self.message = None
def execute(self, userdata):
if self._sub.has_msg(self.topic):
Logger.loginfo('Has an image')
self.message = self._sub.get_last_msg(self.topic)
self._sub.remove_last_msg(self.topic)
if self.message is not None:
userdata.image = self.message
return 'received'
return 'unavailable'
class LockableState(ManuallyTransitionableState):
"""
A state that can be locked.
When locked, no transition can be done regardless of the resulting outcome.
However, if any outcome would be triggered, the outcome will be stored
and the state won't be executed anymore until it is unlocked and the stored outcome is set.
"""
def __init__(self, *args, **kwargs):
super(LockableState, self).__init__(*args, **kwargs)
self._locked = False
self._stored_outcome = None
self.__execute = self.execute
self.execute = self._lockable_execute
self._feedback_topic = 'flexbe/command_feedback'
self._lock_topic = 'flexbe/command/lock'
self._unlock_topic = 'flexbe/command/unlock'
self._pub = ProxyPublisher()
self._sub = ProxySubscriberCached()
def _lockable_execute(self, *args, **kwargs):
if self._is_controlled and self._sub.has_msg(self._lock_topic):
msg = self._sub.get_last_msg(self._lock_topic)
self._sub.remove_last_msg(self._lock_topic)
self._execute_lock(msg.data)
if self._is_controlled and self._sub.has_msg(self._unlock_topic):
msg = self._sub.get_last_msg(self._unlock_topic)
self._sub.remove_last_msg(self._unlock_topic)
self._execute_unlock(msg.data)
if self._locked:
if self._stored_outcome is None or self._stored_outcome == 'None':
self._stored_outcome = self.__execute(*args, **kwargs)
return None
if not self._locked and not self._stored_outcome is None and not self._stored_outcome == 'None':
if self._parent.transition_allowed(self.name, self._stored_outcome):
outcome = self._stored_outcome
self._stored_outcome = None
return outcome
else:
return None
outcome = self.__execute(*args, **kwargs)
if outcome is None or outcome == 'None':
return None
if not self._parent is None and not self._parent.transition_allowed(self.name, outcome):
self._stored_outcome = outcome
return None
return outcome
def _execute_lock(self, target):
found_target = False
if target == self._get_path():
found_target = True
self._locked = True
else:
found_target = self._parent.lock(target)
self._pub.publish(self._feedback_topic, CommandFeedback(command="lock", args=[target, target if found_target else ""]))
if not found_target:
rospy.logwarn("--> Wanted to lock %s, but could not find it in current path %s.", target, self._get_path())
else:
rospy.loginfo("--> Locking in state %s", target)
def _execute_unlock(self, target):
found_target = False
if target == self._get_path():
found_target = True
self._locked = False
else:
found_target = self._parent.unlock(target)
self._pub.publish(self._feedback_topic, CommandFeedback(command="unlock", args=[target, target if found_target else ""]))
if not found_target:
rospy.logwarn("--> Wanted to unlock %s, but could not find it in current path %s.", target, self._get_path())
else:
rospy.loginfo("--> Unlocking in state %s", target)
def _enable_ros_control(self):
super(LockableState, self)._enable_ros_control()
self._pub.createPublisher(self._feedback_topic, CommandFeedback)
self._sub.subscribe(self._lock_topic, String)
self._sub.subscribe(self._unlock_topic, String)
def _disable_ros_control(self):
super(LockableState, self)._disable_ros_control()
self._sub.unsubscribe_topic(self._lock_topic)
self._sub.unsubscribe_topic(self._unlock_topic)
def is_locked(self):
return self._locked
class SubscriberState(EventState):
'''
Gets the latest message on the given topic and stores it to userdata.
-- topic string The topic on which should be listened.
-- blocking bool Blocks until a message is received.
-- clear bool Drops last message on this topic on enter
in order to only handle message received since this state is active.
#> message object Latest message on the given topic of the respective type.
<= received Message has been received and stored in userdata or state is not blocking.
<= unavailable The topic is not available when this state becomes actives.
'''
def __init__(self, topic, blocking = True, clear = False):
'''
Constructor
'''
super(SubscriberState, self).__init__(outcomes=['received', 'unavailable'],
output_keys=['message'])
self._topic = topic
self._blocking = blocking
self._clear = clear
self._connected = False
(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 execute(self, userdata):
'''
Execute this state
'''
if not self._connected:
return 'unavailable'
if self._sub.has_msg(self._topic) or not self._blocking:
userdata.message = self._sub.get_last_msg(self._topic)
self._sub.remove_last_msg(self._topic)
return 'received'
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 _get_msg_from_path(self, msg_path):
msg_import = msg_path.split('/')
msg_module = '%s.msg' % (msg_import[0])
package = __import__(msg_module, fromlist=[msg_module])
clsmembers = inspect.getmembers(package, lambda member: inspect.isclass(member) and member.__module__.endswith(msg_import[1]))
return clsmembers[0][1]
class EventState(OperatableState):
"""
A state that allows implementing certain events.
"""
def __init__(self, *args, **kwargs):
super(EventState, self).__init__(*args, **kwargs)
self._entering = True
self._skipped = False
self.__execute = self.execute
self.execute = self._event_execute
self._paused = False
self._last_active_container = None
self._feedback_topic = "flexbe/command_feedback"
self._repeat_topic = "flexbe/command/repeat"
self._pause_topic = "flexbe/command/pause"
self._pub = ProxyPublisher()
self._sub = ProxySubscriberCached()
def _event_execute(self, *args, **kwargs):
if self._is_controlled and self._sub.has_msg(self._pause_topic):
msg = self._sub.get_last_msg(self._pause_topic)
if msg.data:
self._sub.remove_last_msg(self._pause_topic)
rospy.loginfo("--> Pausing in state %s", self.name)
self._pub.publish(self._feedback_topic, CommandFeedback(command="pause"))
self._last_active_container = PriorityContainer.active_container
PriorityContainer.active_container = ""
self._paused = True
if self._paused and not PreemptableState.preempt:
self._notify_skipped()
return self._loopback_name
if self._entering:
self._entering = False
self.on_enter(*args, **kwargs)
if self._skipped:
self._skipped = False
self.on_resume(*args, **kwargs)
execute_outcome = self.__execute(*args, **kwargs)
repeat = False
if self._is_controlled and self._sub.has_msg(self._repeat_topic):
rospy.loginfo("--> Repeating state %s", self.name)
self._sub.remove_last_msg(self._repeat_topic)
self._pub.publish(self._feedback_topic, CommandFeedback(command="repeat"))
repeat = True
if execute_outcome != self._loopback_name and not PreemptableState.switching or repeat:
self._entering = True
self.on_exit(*args, **kwargs)
return execute_outcome
def _notify_skipped(self):
if not self._skipped:
self.on_pause()
self._skipped = True
if self._is_controlled and self._sub.has_msg(self._pause_topic):
msg = self._sub.get_last_msg(self._pause_topic)
if not msg.data:
self._sub.remove_last_msg(self._pause_topic)
rospy.loginfo("--> Resuming in state %s", self.name)
self._pub.publish(self._feedback_topic, CommandFeedback(command="resume"))
PriorityContainer.active_container = self._last_active_container
self._last_active_container = None
self._paused = False
super(EventState, self)._notify_skipped()
def _enable_ros_control(self):
super(EventState, self)._enable_ros_control()
self._pub.createPublisher(self._feedback_topic, CommandFeedback)
self._sub.subscribe(self._repeat_topic, Empty)
self._sub.subscribe(self._pause_topic, Bool)
def _disable_ros_control(self):
super(EventState, self)._disable_ros_control()
self._sub.unsubscribe_topic(self._repeat_topic)
self._sub.unsubscribe_topic(self._pause_topic)
self._last_active_container = None
if self._paused:
PriorityContainer.active_container = None
# Events
# (just implement the ones you need)
def on_start(self):
"""
Will be executed once when the behavior starts.
"""
pass
def on_stop(self):
"""
Will be executed once when the behavior stops or is preempted.
"""
pass
def on_pause(self):
"""
Will be executed each time this state is paused.
"""
pass
def on_resume(self, userdata):
"""
Will be executed each time this state is resumed.
"""
pass
def on_enter(self, userdata):
"""
Will be executed each time the state is entered from any other state (but not from itself).
"""
pass
def on_exit(self, userdata):
"""
Will be executed each time the state will be left to any other state (but not to itself).
"""
pass
