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 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.