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 Get_Robot_Pose(EventState):
'''
Gets the current pose of the robot.
#> pose geometry_msgs.Pose Current pose of the robot.
<= done The pose is received.
'''
def __init__(self):
'''
Constructor
'''
super(Get_Robot_Pose, self).__init__(outcomes=['done'],
output_keys=['pose'])
self._topic = "/robot_pose"
self._sub = ProxySubscriberCached({self._topic: Pose})
def execute(self, userdata):
'''
Execute this state
'''
mypose = userdata.pose = self._sub.get_last_msg(self._topic)
if mypose:
Logger.loginfo('my pose is:' + str(mypose))
userdata.pose = mypose
else:
userdata.pose = Pose()
return 'done'
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
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 MarkPoint(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.
-- target_time float Time which needs to have passed since the behavior started.
<= continue Given time has passed.
<= failed Example for a failure outcome.
'''
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 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 rospy.Time.now() - self._start_time < self._target_time:
return 'succeeded' # One of the outcomes declared above.
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.
# The following code is just for illustrating how the behavior logger works.
# Text logged by the behavior logger is sent to the operator and displayed in the GUI.
userdata.pose = self._sub.get_last_msg(self._objectTopic)
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.
# In this example, we use this event to set the correct start time.
##self._start_time = rospy.Time.now()
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 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 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 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 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 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 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 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
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 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 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 WaitForPersonState(EventState):
'''
Implements a state that returns true if there is a face published on the given topic.
-- target_sub_topic String The topic for the coordinates where to gaze.
<= done There is a person.
<= failure Something bad happend.
'''
def __init__(self, target_sub_topic='/hace/people'):
'''Constructor'''
super(WaitForPersonState, self).__init__(outcomes=['done', 'failure'])
self._target_sub_topic = target_sub_topic
self._persons = None
self._start_sec = 0
self._start_nsec = 0
self._sub = ProxySubscriberCached(
{self._target_sub_topic: MinimalHumans})
def execute(self, userdata):
'''Execute this state'''
self._persons = self._sub.get_last_msg(self._target_sub_topic)
if (self._persons != None and len(self._persons.humans) > 0):
if self._persons.humans[0].header.stamp.secs > self._start_sec or \
(self._persons.humans[0].header.stamp.secs == self._start_sec and self._persons.humans[0].header.stamp.nsecs > self._start_nsec) :
return 'done'
def on_enter(self, userdata):
'''Upon entering the state, save the start time and calculate the duration.'''
self._persons = self._sub.get_last_msg(self._target_sub_topic)
if (self._persons != None and len(self._persons.humans) > 0):
self._start_sec = self._persons.humans[0].header.stamp.secs
self._start_nsec = self._persons.humans[0].header.stamp.nsecs
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 GetClosestObstacle(EventState):
"""
Renvoie l'angle de l'obstacle le plus proche.
### InputKey
># angle msg.ranges
### OutputKey
<= done Angle de l'obstacle
"""
def __init__(self, topic="/scan", maximumDistance=2):
'''
Constructor
'''
super(GetClosestObstacle, self).__init__(outcomes=['done'], output_keys=['Angle', 'distance', 'position'])
self._topic = topic
self._sub = ProxySubscriberCached({self._topic: LaserScan})
self.maximumDistance = maximumDistance
def execute(self, userdata):
'''
Execute this state
'''
Angle = 0
previousMinimum = self.maximumDistance
i = 0
msg = self._sub.get_last_msg(self._topic)
# print(str(msg))
if msg:
for range in msg.ranges:
ang = msg.angle_min + i * msg.angle_increment
if range < previousMinimum and (range > 0.4 or -1.8 < ang < 1.8) and range > 0.05:
previousMinimum = range
Angle = ang
i += 1
dist = previousMinimum
userdata.distance = dist
pos = Point()
pos.x = cos(Angle)*dist
pos.y = sin(Angle)*dist
userdata.position = pos
Logger.loginfo("closest obstacle is at "+str(Angle)+" and "+str(dist))
userdata.Angle = max(min(Angle, 1.8), -1.8)
return "done"
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 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 GetRobotPose(EventState):
'''
Retrieves the robots current pose and returns it
#> pose PoseStamped Position and Pose which has been marked
<= succeeded Point has been marked successfully.
'''
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 execute(self, userdata):
if self._succeeded == True:
return 'succeeded'
def on_enter(self, userdata):
userdata.pose = self._sub.get_last_msg(self._objectTopic)
self._succeeded = True
def on_exit(self, userdata):
pass
def on_start(self):
pass
def on_stop(self):
pass
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 ForceMonitor(EventState):
'''
Monitors Force (derivative).
'''
def __init__(self,
force_threshold=0.5,
force_topic='/force_helper/result'):
'''Constructor'''
super(ForceMonitor, self).__init__(outcomes=['success'],
input_keys=['carrying'])
self._threshold = force_threshold
self._force_topic = force_topic
self._sub = ProxySubscriberCached({self._force_topic: Float32})
def execute(self, d):
'''Execute this state'''
force_msg = self._sub.get_last_msg(self._force_topic)
if force_msg is None:
return
#current_force = np.array([np.clip(force_msg.wrench.force.x, -99, 0), np.clip(force_msg.wrench.force.y, 0, 99), force_msg.wrench.force.z*0.5])
force_norm = force_msg.data
current_threshold = self._threshold # * (0.5+current_acc)
if d.carrying:
current_threshold *= 1.5
#y contains gravity here
#current_force = force_msg.wrench.force.y
#force_norm = np.linalg.norm(current_force)
#Logger.loginfo('time: %r got force: force_norm %r _threshold %r adapted thresh %r' % (rospy.Time.now().nsecs/1000000, force_norm, self._threshold, current_threshold))
if force_norm > current_threshold:
Logger.loghint(
'got force: force_norm %r _threshold %r adapted thresh %r' %
(force_norm, self._threshold, current_threshold))
return 'success'
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 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 WaitForDoldButton(EventState):
'''
Implements a state that returns done if the dold button is pressed.
'''
def __init__(self, dold_button_topic='/dold_driver/state'):
super(WaitForDoldButton, self).__init__(outcomes=['done'])
self._dold_button_topic = dold_button_topic
self._sub = ProxySubscriberCached(
{self._dold_button_topic: DoldStates})
def on_enter(self, userdata):
Logger.loginfo('waiting for right upper dold button ...')
def execute(self, d):
msg = self._sub.get_last_msg(self._dold_button_topic)
if (msg != None):
for event in msg.inputs:
if event.type == DoldState.BUTTON and event.state == DoldState.PRESSED and event.name == 'B0':
Logger.loginfo('detected button press!')
return 'done'
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 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
class ATLAScheckoutSM(Behavior):
'''
A behavior meant to be run before high voltage is provided to ATLAS. It guides the robot through BDI's calibration, initial control mode changes, Praying Mantis calibration, and visual calibration check.
'''
def __init__(self):
super(ATLAScheckoutSM, self).__init__()
self.name = 'ATLAS checkout'
# parameters of this behavior
# references to used behaviors
self.add_behavior(PrayingMantisCalibrationSM, 'Praying Mantis Calibration')
# Additional initialization code can be added inside the following tags
# [MANUAL_INIT]
self._status_topic = '/flor/controller/robot_status'
self._sub = ProxySubscriberCached({self._status_topic: FlorRobotStatus})
self._sub.make_persistant(self._status_topic)
# [/MANUAL_INIT]
# Behavior comments:
def create(self):
# x:1125 y:169, x:430 y:273
_state_machine = OperatableStateMachine(outcomes=['finished', 'failed'])
_state_machine.userdata.left = 'left'
_state_machine.userdata.right = 'right'
_state_machine.userdata.none = None
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
# x:728 y:81, x:318 y:310
_sm_confirm_calibration_0 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['left', 'right'])
with _sm_confirm_calibration_0:
# x:68 y:71
OperatableStateMachine.add('Go_to_MANIPULATE',
ChangeControlModeActionState(target_mode=ChangeControlModeActionState.MANIPULATE),
transitions={'changed': 'Look_Slightly_Down', 'failed': 'failed'},
autonomy={'changed': Autonomy.Low, 'failed': Autonomy.Low})
# x:75 y:301
OperatableStateMachine.add('Check_Left_Arm',
MoveitPredefinedPoseState(target_pose=MoveitPredefinedPoseState.CALIBRATE_ARMS, vel_scaling=0.2, ignore_collisions=False, link_paddings={}, is_cartesian=False),
transitions={'done': 'Bring_Left_Arm_Down', 'failed': 'failed'},
autonomy={'done': Autonomy.Full, 'failed': Autonomy.Low},
remapping={'side': 'left'})
# x:76 y:394
OperatableStateMachine.add('Bring_Left_Arm_Down',
MoveitPredefinedPoseState(target_pose=MoveitPredefinedPoseState.SINGLE_ARM_STAND, vel_scaling=0.2, ignore_collisions=False, link_paddings={}, is_cartesian=False),
transitions={'done': 'Check_Right_Arm', 'failed': 'failed'},
autonomy={'done': Autonomy.High, 'failed': Autonomy.Low},
remapping={'side': 'left'})
# x:426 y:393
OperatableStateMachine.add('Check_Right_Arm',
MoveitPredefinedPoseState(target_pose=MoveitPredefinedPoseState.CALIBRATE_ARMS, vel_scaling=0.2, ignore_collisions=False, link_paddings={}, is_cartesian=False),
transitions={'done': 'Bring_Right_Arm_Down', 'failed': 'failed'},
autonomy={'done': Autonomy.Full, 'failed': Autonomy.Low},
remapping={'side': 'right'})
# x:426 y:301
OperatableStateMachine.add('Bring_Right_Arm_Down',
MoveitPredefinedPoseState(target_pose=MoveitPredefinedPoseState.SINGLE_ARM_STAND, vel_scaling=0.2, ignore_collisions=False, link_paddings={}, is_cartesian=False),
transitions={'done': 'Look_Straight', 'failed': 'failed'},
autonomy={'done': Autonomy.High, 'failed': Autonomy.Low},
remapping={'side': 'right'})
# x:415 y:75
OperatableStateMachine.add('Go_to_STAND',
ChangeControlModeActionState(target_mode=ChangeControlModeActionState.STAND),
transitions={'changed': 'finished', 'failed': 'failed'},
autonomy={'changed': Autonomy.Off, 'failed': Autonomy.Low})
# x:81 y:185
OperatableStateMachine.add('Look_Slightly_Down',
TiltHeadState(desired_tilt=TiltHeadState.DOWN_30),
transitions={'done': 'Check_Left_Arm', 'failed': 'failed'},
autonomy={'done': Autonomy.Off, 'failed': Autonomy.Low})
# x:449 y:178
OperatableStateMachine.add('Look_Straight',
TiltHeadState(desired_tilt=TiltHeadState.STRAIGHT),
transitions={'done': 'Go_to_STAND', 'failed': 'failed'},
autonomy={'done': Autonomy.Off, 'failed': Autonomy.Low})
# x:748 y:57, x:306 y:173
_sm_checks_and_bdi_calibration_1 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['none'])
with _sm_checks_and_bdi_calibration_1:
# x:54 y:28
OperatableStateMachine.add('Get_Air_Pressure',
CalculationState(calculation=self.get_air_sump_pressure),
transitions={'done': 'Check_Air_Pressure'},
autonomy={'done': Autonomy.Off},
remapping={'input_value': 'none', 'output_value': 'air_pressure'})
# x:46 y:411
OperatableStateMachine.add('Check_Hands_Initialized',
OperatorDecisionState(outcomes=['ready', 'again'], hint='Did both hands initialize?', suggestion='ready'),
transitions={'ready': 'Turn_Pump_ON', 'again': 'Request_Robot_Power'},
autonomy={'ready': Autonomy.High, 'again': Autonomy.Full})
# x:267 y:29
OperatableStateMachine.add('Air_Pressure_Warning',
LogState(text='Check the air pressure! Has to be 120 +/- 10 PSI.', severity=Logger.REPORT_WARN),
transitions={'done': 'failed'},
autonomy={'done': Autonomy.Full})
# x:50 y:149
OperatableStateMachine.add('Check_Air_Pressure',
DecisionState(outcomes=['ok', 'alert'], conditions=lambda p: 'ok' if p > 110.0 and p <= 130.0 else 'alert'),
transitions={'ok': 'Request_Robot_Power', 'alert': 'Air_Pressure_Warning'},
autonomy={'ok': Autonomy.Full, 'alert': Autonomy.Low},
remapping={'input_value': 'air_pressure'})
# x:500 y:286
OperatableStateMachine.add('Calibrate_ARMS',
RobotStateCommandState(command=RobotStateCommandState.CALIBRATE_ARMS),
transitions={'done': 'FREEZE_in_Between', 'failed': 'failed'},
autonomy={'done': Autonomy.Full, 'failed': Autonomy.Low})
# x:499 y:52
OperatableStateMachine.add('Calibrate_BIASES',
RobotStateCommandState(command=RobotStateCommandState.CALIBRATE),
transitions={'done': 'finished', 'failed': 'failed'},
autonomy={'done': Autonomy.Full, 'failed': Autonomy.Low})
# x:488 y:167
OperatableStateMachine.add('FREEZE_in_Between',
ChangeControlModeActionState(target_mode=ChangeControlModeActionState.FREEZE),
transitions={'changed': 'Calibrate_BIASES', 'failed': 'failed'},
autonomy={'changed': Autonomy.Off, 'failed': Autonomy.High})
# x:500 y:412
OperatableStateMachine.add('Turn_Pump_ON',
RobotStateCommandState(command=RobotStateCommandState.START_HYDRAULIC_PRESSURE_ON),
transitions={'done': 'Calibrate_ARMS', 'failed': 'failed'},
autonomy={'done': Autonomy.Full, 'failed': Autonomy.High})
# x:47 y:276
OperatableStateMachine.add('Request_Robot_Power',
LogState(text='Request robot power from field team.', severity=Logger.REPORT_HINT),
transitions={'done': 'Check_Hands_Initialized'},
autonomy={'done': Autonomy.Full})
with _state_machine:
# x:74 y:88
OperatableStateMachine.add('Checks_and_BDI_Calibration',
_sm_checks_and_bdi_calibration_1,
transitions={'finished': 'Go_to_STAND_PREP', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'none': 'none'})
# x:79 y:264
OperatableStateMachine.add('Go_to_STAND_PREP',
ChangeControlModeActionState(target_mode=ChangeControlModeActionState.STAND_PREP),
transitions={'changed': 'Go_to_STAND', 'failed': 'failed'},
autonomy={'changed': Autonomy.Full, 'failed': Autonomy.Low})
# x:78 y:393
OperatableStateMachine.add('Go_to_STAND',
ChangeControlModeActionState(target_mode=ChangeControlModeActionState.STAND),
transitions={'changed': 'Praying Mantis Calibration', 'failed': 'failed'},
autonomy={'changed': Autonomy.Full, 'failed': Autonomy.Low})
# x:611 y:388
OperatableStateMachine.add('Praying Mantis Calibration',
self.use_behavior(PrayingMantisCalibrationSM, 'Praying Mantis Calibration'),
transitions={'finished': 'Decide_Calibration_Check', 'failed': 'Decide_Retry'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit})
# x:624 y:249
OperatableStateMachine.add('Decide_Calibration_Check',
OperatorDecisionState(outcomes=['check', 'skip'], hint='Do you want to confirm the arm calibration visually?', suggestion='skip'),
transitions={'check': 'Confirm_Calibration', 'skip': 'Logging_Reminder'},
autonomy={'check': Autonomy.Full, 'skip': Autonomy.High})
# x:627 y:93
OperatableStateMachine.add('Confirm_Calibration',
_sm_confirm_calibration_0,
transitions={'finished': 'Logging_Reminder', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'left': 'left', 'right': 'right'})
# x:389 y:340
OperatableStateMachine.add('Decide_Retry',
OperatorDecisionState(outcomes=['retry', 'fail'], hint='Try running Praying Mantis Calibration again?', suggestion='retry'),
transitions={'retry': 'Praying Mantis Calibration', 'fail': 'failed'},
autonomy={'retry': Autonomy.High, 'fail': Autonomy.Full})
# x:907 y:164
OperatableStateMachine.add('Logging_Reminder',
LogState(text='Start video logging', severity=Logger.REPORT_HINT),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Full})
return _state_machine
# Private functions can be added inside the following tags
# [MANUAL_FUNC]
def get_air_sump_pressure(self, input):
'''Returns the current air sump pressure.'''
robot_status = self._sub.get_last_msg(self._status_topic)
air_pressure = robot_status.air_sump_pressure
Logger.loginfo('Air Sump Pressure: %.2f' % air_pressure)
return air_pressure
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 MarkPoint(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.
-- target_time float Time which needs to have passed since the behavior started.
<= continue Given time has passed.
<= failed Example for a failure outcome.
'''
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 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 rospy.Time.now() - self._start_time < self._target_time:
return 'succeeded' # One of the outcomes declared above.
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.
# The following code is just for illustrating how the behavior logger works.
# Text logged by the behavior logger is sent to the operator and displayed in the GUI.
userdata.pose = self._sub.get_last_msg(self._objectTopic)
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.
# In this example, we use this event to set the correct start time.
##self._start_time = rospy.Time.now()
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 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 AttachObjectState(EventState):
'''
Requests a grasp for a template from the template server.
># template_id int ID of the template to attach.
># hand_side string Hand side to which the template
should be attached {left, right}
#> template_pose PoseStamped Pose of the attached template in the
reference frame of the wrist (r/l_hand)
<= done Template has been attached.
<= failed Failed to attach template.
'''
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
def execute(self, userdata):
if self._failed:
return 'failed'
return 'done'
def on_enter(self, userdata):
self._failed = False
# The frame ID to which the object template will be attached to
if userdata.hand_side == 'left':
frame_id = "l_hand"
elif userdata.hand_side == 'right':
frame_id = "r_hand"
else:
Logger.logwarn('Unexpected value of hand side: %s Expected {left, right}' % str(userdata.hand_side))
self._failed = True
return
# Get the current wrist pose, which is required by the stitching service
try:
wrist_pose_topic = self._wrist_pose_topics[userdata.hand_side]
current_wrist_pose = self._sub.get_last_msg(wrist_pose_topic)
# Set the PoseStamped message's frame ID to the one
# that the object template should be attached to:
current_wrist_pose.header.frame_id = frame_id
except Exception as e:
Logger.logwarn('Failed to get the latest hand pose:\n%s' % str(e))
self._failed = True
return
# Send the stiching request and write response to userdata
try:
request = SetAttachedObjectTemplateRequest(template_id = userdata.template_id,
pose = current_wrist_pose)
self._srv_result = self._srv.call(self._service_topic, request)
# The result contains the template's pose, mass, and center of mass
userdata.template_pose = self._srv_result.template_pose
except Exception as e:
Logger.logwarn('Failed to send service call:\n%s' % str(e))
self._failed = True
return
class Object_Detection(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.
-- target_time float Time which needs to have passed since the behavior started.
<= continue Given time has passed.
<= failed Example for a failure outcome.
'''
def __init__(self):
# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
super(Object_Detection, self).__init__(outcomes = ['continue', 'found'], input_keys = ['pose'], output_keys = ['pose'])
self._objectTopic = '/worldmodel/object'
self._sub = ProxySubscriberCached({self._objectTopic: Object})
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.
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 and current_obj.info.object_id != 'victim_0':
userdata.pose = PoseStamped()
userdata.pose.pose = current_obj.pose.pose
userdata.pose.header.stamp = Time.now()
userdata.pose.header.frame_id = 'map'
Logger.loginfo(current_obj.info.object_id)
return 'found'
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.
# The following code is just for illustrating how the behavior logger works.
# Text logged by the behavior logger is sent to the operator and displayed in the GUI.
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.
# In this example, we use this event to set the correct start time.
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 Wait_DriveTo_new(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.
-- target_time float Time which needs to have passed since the behavior started.
<= continue Given time has passed.
<= failed Example for a failure outcome.
'''
def __init__(self):
# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
super(Wait_DriveTo_new, self).__init__(outcomes=['succeeded', 'aborted', 'waiting'])
# ???
self._abortStatus = [GoalStatus.ABORTED, GoalStatus.REJECTED, GoalStatus.PREEMPTED]
self._resultTopic = '/move_base/result' #if useMoveBase else 'controller/result'
self._sub = ProxySubscriberCached({self._resultTopic: MoveBaseActionResult})
# don't define a callback, simply request last message in execute
self._succeeded = False
self._aborted = False
self._waiting = False
self._result = GoalStatus.PENDING
# Store state parameter for later use.
## self._target_time = rospy.Duration(target_time)
# 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
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.
Logger.loginfo(' [hector_behavior] in Drive_to waiting.... ')
tmp = self._sub.get_last_msg(self._resultTopic)
self._result = GoalStatus.PENDING
if tmp is None:
Logger.loginfo(' [hector_behavior] drive to waiting because no published result')
# ???
rospy.sleep(0.5)
self_waiting = True
return 'waiting'
#if userdata.goalId == tmp.status.goal_id.id:
#self._result = tmp.status.status
if self._result == GoalStatus.SUCCEEDED:
self_succeeded = True
return 'succeeded'
elif self._result in self._abortStatus:
self_aborted = True
return 'aborted'
else:
Logger.loginfo(' [hector_behavior] drive to waiting because no matching goal id')
rospy.sleep(0.5)
self_waiting = True
return 'waiting'
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.
# In this example, we use this event to set the correct start time.
## self._start_time = rospy.Time.now()
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.
