def test_publish_subscribe(self): t1 = '/pubsub_1' t2 = '/pubsub_2' pub = ProxyPublisher({t1: String}) pub = ProxyPublisher({t2: String}, _latch=True) sub = ProxySubscriberCached({t1: String}) self.assertTrue(pub.is_available(t1)) self.assertTrue(pub.wait_for_any(t1)) self.assertFalse(pub.wait_for_any(t2)) pub.publish(t1, String('1')) pub.publish(t2, String('2')) rospy.sleep( .5) # make sure latched message is sent before subscriber is added sub = ProxySubscriberCached({t2: String}) rospy.sleep( .5) # make sure latched message can be received before checking self.assertTrue(sub.has_msg(t1)) self.assertEqual(sub.get_last_msg(t1).data, '1') sub.remove_last_msg(t1) self.assertFalse(sub.has_msg(t1)) self.assertIsNone(sub.get_last_msg(t1)) self.assertTrue(sub.has_msg(t2)) self.assertEqual(sub.get_last_msg(t2).data, '2')
class GetClosestEntity(EventState): ''' return the closest entity from the list in parameter. If no list in parameter, return the closest entity from the topic /entities ># entityList object[] List of entities. #> entityFound object The found entity. <= done Indicates completion of the extraction. <= not_found Indicates completion of the extraction. ''' def __init__(self): '''Constructor''' super(GetClosestEntity, self).__init__(outcomes=['done', 'not_found'], input_keys=['entityList'], output_keys=['output_entity']) self._sub = ProxySubscriberCached({'/entities': Entities}) self._subpos = ProxySubscriberCached({'/robot_pose': Pose}) self.mypose = None def dist(self, entity1, entity2): x = entity1.position.x - entity2.position.x y = entity1.position.y - entity2.position.y return math.sqrt(x * x + y * y) def execute(self, userdata): '''Execute this state''' if self._subpos.has_msg('/robot_pose'): self.mypose = self._subpos.get_last_msg('/robot_pose') if len(userdata.entityList) == 0: Logger.loginfo('va chercher la liste entities') if self._sub.has_msg('/entities'): myList = self._sub.get_last_msg('/entities') self._sub.remove_last_msg('/entities') myList = myList.entities Logger.loginfo('a la liste entities') else: myList = userdata.entityList Logger.loginfo('a la liste en parametre') if len(myList) == 0: Logger.loginfo('liste vide') return "not_found" min_dist = 99999 closestEntity = myList[0] for entity in myList: if self.dist(self.mypose, entity) < min_dist: min_dist = self.dist(self.mypose, entity) closestEntity = entity userdata.output_entity = closestEntity return 'done'
class ReadTorque(EventState): ''' Reads torque of a joint. <= done torque increase has been detected. ''' def __init__(self, watchdog, Joint, Threshold, min_time): super(ReadTorque, self).__init__(outcomes=['threshold', 'watchdog', 'fail'], output_keys=['torque']) self.watchdog = watchdog self.Threshold = Threshold self.Joint = Joint self.min_time = min_time self.timer = 0 self._topic = "/joint_states" self.watchdogTime = None self._sub = ProxySubscriberCached({self._topic: JointState}) self.initialTorque = None def execute(self, userdata): if self.initialTorque is None: self.watchdogTime = get_time() + self.watchdog if self._sub.has_msg(self._topic): message = self._sub.get_last_msg(self._topic) for i in range(len(message.name)): if message.name[i] == self.Joint: self.Joint = i break self.initialTorque = message.effort[self.Joint] print("Initial torque is:" + str(self.initialTorque)) else: if self._sub.has_msg(self._topic): torque = self._sub.get_last_msg(self._topic).effort[self.Joint] if abs(self.initialTorque - torque) >= self.Threshold: Logger.loginfo('Reading torque :' + str(abs(self.initialTorque - torque))) if self.timer < get_time(): userdata.torque = torque return 'threshold' else: self.timer = get_time() + self.min_time if (self.watchdogTime - get_time() <= 0): return "watchdog" def on_enter(self, userdata): self.initialTorque = None
class GetLaserscanState(EventState): ''' Grabs the most recent laserscan data. #> laserscan LaserScan The current laser scan. <= done Scanning data is available. ''' def __init__(self): '''Constructor''' super(GetLaserscanState, self).__init__(outcomes=['done'], output_keys=['laserscan']) self._scan_topic = "/multisense/lidar_scan" self._sub = ProxySubscriberCached({self._scan_topic: LaserScan}) def execute(self, userdata): if self._sub.has_msg(self._scan_topic): userdata.laserscan = self._sub.get_last_msg(self._scan_topic) return 'done' def on_enter(self, userdata): pass
class DetectPersonState(EventState): """ Detects the nearest person and provides their pose. -- wait_timeout float Time (seconds) to wait for a person before giving up. #> person_pose PoseStamped Pose of the nearest person if one is detected, else None. <= detected Detected a person. <= not_detected No person detected, but time ran out. """ def __init__(self, wait_timeout): super(DetectPersonState, self).__init__(outcomes=["detected", "not_detected"], output_keys=["person_pose"]) self._wait_timeout = rospy.Duration(wait_timeout) self._topic = "/people_tracker/pose" self._sub = ProxySubscriberCached({self._topic: PoseStamped}) self._start_waiting_time = None def execute(self, userdata): if rospy.Time.now() > self._start_waiting_time + self._wait_timeout: userdata.person_pose = None return "not_detected" if self._sub.has_msg(self._topic): userdata.person_pose = self._sub.get_last_msg(self._topic) return "detected" def on_enter(self, userdata): self._sub.remove_last_msg(self._topic) self._start_waiting_time = rospy.Time.now()
class StorePointcloudState(EventState): ''' Stores the latest pointcloud of the given topic. -- topic string The topic on which to listen for the pointcloud. #> pointcloud PointCloud2 The received pointcloud. <= done Pointcloud has been received and stored. ''' def __init__(self, topic): super(StorePointcloudState, self).__init__(outcomes = ['done'], output_keys = ['pointcloud']) self._sub = ProxySubscriberCached({topic: PointCloud2}) self._pcl_topic = topic def execute(self, userdata): if self._sub.has_msg(self._pcl_topic): userdata.pointcloud = self._sub.get_last_msg(self._pcl_topic) return 'done'
class ShowPictureWebinterfaceState(EventState): ''' Displays the picture in a web interface ># Image Image The received Image <= done Displaying the Picture ''' def __init__(self): super(ShowPictureWebinterfaceState, self).__init__(outcomes = ['tweet', 'forget'], input_keys = ['image_name']) self._pub_topic = '/webinterface/display_picture' self._pub = ProxyPublisher({self._pub_topic: String}) self._sub_topic = '/webinterface/dialog_feedback' self._sub = ProxySubscriberCached({self._sub_topic: String}) def execute(self, userdata): if self._sub.has_msg(self._sub_topic): msg = self._sub.get_last_msg(self._sub_topic) if msg.data == 'yes': print 'show_picture_webinterface_state, returning tweet' return 'tweet' else: print 'show_picture_webinterface_state, returning forget' return 'forget' def on_enter(self,userdata): self._sub.remove_last_msg(self._sub_topic) self._pub.publish(self._pub_topic, String(userdata.image_name))
class GetCameraImageState(EventState): ''' Grabs the most recent camera image. #> camera_img Image The current color image of the left camera. <= done Image data is available. ''' def __init__(self): '''Constructor''' super(GetCameraImageState, self).__init__(outcomes = ['done'], output_keys = ['camera_img']) self._img_topic = "/multisense/camera/left/image_rect_color" self._sub = ProxySubscriberCached({self._img_topic: Image}) def execute(self, userdata): if self._sub.has_msg(self._img_topic): userdata.camera_img = self._sub.get_last_msg(self._img_topic) return 'done' def on_enter(self, userdata): pass
class ContinueButton(EventState): ''' Reads on a topic to see for the continue button. <= done Continue if button pressed. ''' def __init__(self): ''' Constructor ''' super(ContinueButton, self).__init__(outcomes=['true', 'false']) self._topic = "/continue_button" self._connected = False self._sub = ProxySubscriberCached({self._topic: Bool}) def execute(self, userdata): Logger.loginfo('Waiting for the continue button') if self._sub.has_msg(self._topic): message = self._sub.get_last_msg(self._topic) self._sub.remove_last_msg(self._topic) if message.data: return 'true' else: return 'false'
class CheckCurrentControlModeState(EventState): ''' Implements a state where the robot checks its current control mode. -- target_mode enum The control mode to check for being the current one (e.g. 3 for stand, 6 for manipulate). The state's class variables can also be used (e.g. CheckCurrentControlModeState.STAND). -- wait bool Whether to check once and return (False), or wait for the control mode to change (True). #> control_mode enum The current control mode when the state returned. <= correct Indicates that the current control mode is the target/expected one. <= incorrect Indicates that the current control mode is not the target/expected one. ''' NONE = 0 FREEZE = 1 STAND_PREP = 2 STAND = 3 STAND_MANIPULATE = 3 WALK = 4 STEP = 5 MANIPULATE = 6 USER = 7 CALIBRATE = 8 SOFT_STOP = 9 def __init__(self, target_mode, wait=False): ''' Constructor ''' super(CheckCurrentControlModeState, self).__init__(outcomes=['correct', 'incorrect'], output_keys=['control_mode']) self._status_topic = '/flor/controller/mode' self._sub = ProxySubscriberCached( {self._status_topic: VigirAtlasControlMode}) self._sub.make_persistant(self._status_topic) self._target_mode = target_mode self._wait = wait def execute(self, userdata): ''' Execute this state ''' if self._sub.has_msg(self._status_topic): msg = self._sub.get_last_msg(self._status_topic) userdata.control_mode = msg.bdi_current_behavior if msg.bdi_current_behavior == self._target_mode: return 'correct' elif not self._wait: return 'incorrect' def on_enter(self, userdata): if self._wait: Logger.loghint("Waiting for %s" % str(self._target_mode))
class CheckCurrentControlModeState(EventState): ''' Implements a state where the robot checks its current control mode. -- target_mode enum The control mode to check for being the current one (e.g. 3 for stand, 6 for manipulate). The state's class variables can also be used (e.g. CheckCurrentControlModeState.STAND). -- wait bool Whether to check once and return (False), or wait for the control mode to change (True). #> control_mode enum The current control mode when the state returned. <= correct Indicates that the current control mode is the target/expected one. <= incorrect Indicates that the current control mode is not the target/expected one. ''' NONE = 0 FREEZE = 1 STAND_PREP = 2 STAND = 3 STAND_MANIPULATE = 3 WALK = 4 STEP = 5 MANIPULATE = 6 USER = 7 CALIBRATE = 8 SOFT_STOP = 9 def __init__(self, target_mode, wait = False): ''' Constructor ''' super(CheckCurrentControlModeState, self).__init__(outcomes=['correct', 'incorrect'], output_keys=['control_mode']) self._status_topic = '/flor/controller/mode' self._sub = ProxySubscriberCached({self._status_topic: VigirAtlasControlMode}) self._sub.make_persistant(self._status_topic) self._target_mode = target_mode self._wait = wait def execute(self, userdata): ''' Execute this state ''' if self._sub.has_msg(self._status_topic): msg = self._sub.get_last_msg(self._status_topic) userdata.control_mode = msg.bdi_current_behavior if msg.bdi_current_behavior == self._target_mode: return 'correct' elif not self._wait: return 'incorrect' def on_enter(self, userdata): if self._wait: Logger.loghint("Waiting for %s" % str(self._target_mode))
class GetCameraImageState(EventState): ''' Grabs the most recent camera image. #> camera_img Image The current color image of the left camera. <= done Image data is available. ''' def __init__(self): '''Constructor''' super(GetCameraImageState, self).__init__(outcomes=['done'], output_keys=['camera_img']) self._img_topic = "/multisense/camera/left/image_rect_color" self._sub = ProxySubscriberCached({self._img_topic: Image}) def execute(self, userdata): if self._sub.has_msg(self._img_topic): userdata.camera_img = self._sub.get_last_msg(self._img_topic) return 'done' def on_enter(self, userdata): pass
class GenericSub(EventState): ''' GenericPubSub state subscribes to a given topic (Int32 message only) and will not move forward until a response is recieved. This is meant to be a template to be modified to your needs it is not super useful in its current state. Intended mostly for communicating with arduino ''' def __init__(self, subtopic): # Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments. super(GenericSub, self).__init__(outcomes=["completed", "failed"]) # Store state parameters for later use. self._subtopic = subtopic self._sub = ProxySubscriberCached({self._subtopic: Int32}) #rospy.init_node('reset_control', anonymous=True) def execute(self, userdata): #publish to arduino #while(1): if self._sub.has_msg(self._subtopic): msg = self._sub.get_last_msg(self._subtopic) print(msg) # in case you want to make sure the same message is not processed twice: self._sub.remove_last_msg(self._subtopic) return "completed"
class GetLaserscanState(EventState): """ Grabs the most recent laserscan data. #> laserscan LaserScan The current laser scan. <= done Scanning data is available. """ def __init__(self): """Constructor""" super(GetLaserscanState, self).__init__(outcomes=["done"], output_keys=["laserscan"]) self._scan_topic = "/multisense/lidar_scan" self._sub = ProxySubscriberCached({self._scan_topic: LaserScan}) def execute(self, userdata): if self._sub.has_msg(self._scan_topic): userdata.laserscan = self._sub.get_last_msg(self._scan_topic) return "done" def on_enter(self, userdata): pass
class PreemptableState(LoopbackState): """ A state that can be preempted. If preempted, the state will not be executed anymore and return the outcome preempted. """ _preempted_name = "preempted" preempt = False switching = False def __init__(self, *args, **kwargs): # add preempted outcome if len(args) > 0 and type(args[0]) is list: # need this ugly check for list type, because first argument in CBState is the callback args[0].append(self._preempted_name) else: outcomes = kwargs.get("outcomes", []) outcomes.append(self._preempted_name) kwargs["outcomes"] = outcomes super(PreemptableState, self).__init__(*args, **kwargs) self.__execute = self.execute self.execute = self._preemptable_execute self._feedback_topic = "/flexbe/command_feedback" self._preempt_topic = "/flexbe/command/preempt" self._pub = ProxyPublisher() self._sub = ProxySubscriberCached() def _preemptable_execute(self, *args, **kwargs): preempting = False if self._is_controlled and self._sub.has_msg(self._preempt_topic): self._sub.remove_last_msg(self._preempt_topic) self._pub.publish(self._feedback_topic, CommandFeedback(command="preempt")) preempting = True rospy.loginfo("--> Behavior will be preempted") if PreemptableState.preempt: PreemptableState.preempt = False preempting = True rospy.loginfo("Behavior will be preempted") if preempting: self.service_preempt() self._force_transition = True return self._preempted_name return self.__execute(*args, **kwargs) def _enable_ros_control(self): super(PreemptableState, self)._enable_ros_control() self._pub.createPublisher(self._feedback_topic, CommandFeedback) self._sub.subscribe(self._preempt_topic, Empty) PreemptableState.preempt = False def _disable_ros_control(self): super(PreemptableState, self)._disable_ros_control() self._sub.unsubscribe_topic(self._preempt_topic)
class TurtlebotStatusState(EventState): """ The state monitors the Kobuki turtlebot bumper and cliff sensors and will return an outcome if a one is activated. -- bumper_topic string The topic that the bumper events are published -- cliff_topic string The topic that the cliff events are published <= bumper A bumper was activated <= cliff The cliff sensor """ def __init__(self, bumper_topic='mobile_base/events/bumper', cliff_topic='mobile_base/events/cliff'): super(TurtlebotStatusState, self).__init__(outcomes=['bumper', 'cliff']) self._bumper_topic = bumper_topic self._cliff_topic = cliff_topic self._bumper_sub = ProxySubscriberCached( {self._bumper_topic: BumperEvent}) self._cliff_sub = ProxySubscriberCached( {self._cliff_topic: CliffEvent}) self._return = None # Handle return in case outcome is blocked by low autonomy def execute(self, userdata): if self._bumper_sub.has_msg(self._bumper_topic): sensor = self._bumper_sub.get_last_msg(self._bumper_topic) self._bumper_sub.remove_last_msg(self._bumper_topic) if sensor.state > 0: Logger.logwarn('Bumper %d contact' % (sensor.bumper)) self._return = 'bumper' if self._cliff_sub.has_msg(self._cliff_topic): sensor = self._cliff_sub.get_last_msg(self._cliff_topic) self._cliff_sub.remove_last_msg(self._cliff_topic) if sensor.state > 0: Logger.logwarn('Cliff alert') self._return = 'cliff' return self._return def on_enter(self, userdata): self._return = None # Clear the completion flag
class MonitorPerceptState(EventState): ''' Monitors the hector worldmodel for percepts and triggers a detection event if a percept has a high-enough support. -- required_support float Support threshold which needs to be reached before an event is triggered. -- percept_class string Class name of percepts to be monitored. -- track_percepts bool Defines if this state should track previously detected percepts. Recommended if it is not desired to react multiple times on the same percept. #> object_id string Identifier of the detected object. #> object_pose PoseStamped Pose of the detected object. #> object_data float[] Data associated with this object. <= detected Percept is detected. ''' def __init__(self, required_support, percept_class, track_percepts = True): ''' Constructor ''' super(MonitorPerceptState, self).__init__(outcomes=['detected'], output_keys=['object_id', 'object_pose', 'object_data']) self._topic = '/worldmodel/objects' self._sub = ProxySubscriberCached({self._topic: ObjectModel}) self._required_support = required_support self._percept_class = percept_class self._track_percepts = track_percepts self._percepts = list() def execute(self, userdata): if self._sub.has_msg(self._topic): msg = self._sub.get_last_msg(self._topic) objects = filter(lambda obj: #obj.state.state == ObjectState.ACTIVE \ obj.info.class_id == self._percept_class \ and obj.info.support >= self._required_support \ and obj.info.object_id not in self._percepts, msg.objects ) for obj in objects: if self._track_percepts: self._percepts.append(obj.info.object_id) (x,y,z) = (obj.pose.pose.position.x, obj.pose.pose.position.y, obj.pose.pose.position.z) Logger.loginfo('Detected %s percept with id: %s\nPosition: (%.1f, %.1f, %.1f)' % (self._percept_class, obj.info.object_id, x, y, z)) userdata.object_id = obj.info.object_id userdata.object_pose = PoseStamped(header=obj.header, pose=obj.pose.pose) userdata.object_data = obj.info.data return 'detected'
class SpeakState(EventState): """ Genie TTS request """ def __init__(self): super(SpeakState, self).__init__(outcomes=['done'], input_keys=['tts_text']) self.tts_topic = '/genie_tts' self.state_topic = '/genie_state' self.tts_pub = ProxyPublisher({self.tts_topic: String}) self.state_sub = ProxySubscriberCached({self.state_topic: UInt8}) def execute(self, userdata): if self.state_sub.has_msg(self.state_topic): # get genie state state = self.state_sub.get_last_msg(self.state_topic) self.state_sub.remove_last_msg(self.state_topic) # non-speaking state if state.data == 0: return 'done' def on_enter(self, userdata): tts_text = String() tts_text.data = userdata.tts_text check_rate = rospy.Rate(10) while self.state_sub.has_msg(self.state_topic): # get genie state state = self.state_sub.get_last_msg(self.state_topic) self.state_sub.remove_last_msg(self.state_topic) # non-speaking state if state.data == 0: break check_rate.sleep() # send tts request self.tts_pub.publish(self.tts_topic, tts_text)
class CupboardDoorDetector(EventState): ''' Detect if door is open REF : https://github.com/WalkingMachine/wm_door_detector -- timeout limit time before conclusion <= done Finish job. <= failed Job as failed. ''' def __init__(self, timeout): # See example_state.py for basic explanations. super(CupboardDoorDetector, self).__init__(outcomes=['open', 'closed']) self.distances = [] self._topic = "/scan" self._sub = ProxySubscriberCached({self._topic: LaserScan}) self.time = 0 self.timeout = timeout def avg(self, lst): return sum(lst) / max(len(lst), 1) def interval(self, lst): return max(lst) - min(lst) def process_scan(self, scan_msg): # Extract the interval in a restricted range to determine if the is a closed door middle_index = len(scan_msg.ranges) / 2 ranges_at_center = scan_msg.ranges[middle_index - 40:middle_index + 40] interval_of_door = self.interval(ranges_at_center) print("dist = " + str(interval_of_door)) if interval_of_door < 0.1 or interval_of_door > 0.5 or max( ranges_at_center) > 0.85: rospy.loginfo("Distance to door is more than a meter") return "open" if (self.time - get_time() <= 0): Logger.loginfo('no speech detected') return 'closed' def execute(self, userdata): if self._sub.has_msg(self._topic): message = self._sub.get_last_msg(self._topic) return self.process_scan(message) def on_enter(self, userdata): rospy.loginfo("Waiting for door...") self.time = get_time() + self.timeout def on_exit(self, userdata): rospy.loginfo("The door is now open")
class hsr_GetWrench(EventState): ''' Example for a state to demonstrate which functionality is available for state implementation. This example lets the behavior wait until the given target_time has passed since the behavior has been started. -- threshold float StartBottun(20.0>0), PutControl(-15.0<0), Ignore(0.0) <= completed Completed <= failed Failed ''' def __init__(self, threshold=-15.0): super(hsr_GetWrench, self).__init__(outcomes=['completed', 'failed']) self.threshold = threshold self._topic = '/hsrb/wrist_wrench/raw' self.sub_wrench = ProxySubscriberCached({self._topic: WrenchStamped}) def execute(self, userdata): if self.sub_wrench.has_msg(self._topic): msg = self.sub_wrench.get_last_msg(self._topic) self.sub_wrench.remove_last_msg(self._topic) if self.threshold < 0.0 and msg.wrench.force.x < self.threshold: print "Move on Gripper Open Phase." return "completed" if self.threshold > 0.0 and msg.wrench.force.x > self.threshold: print "Start the Task." return "completed" if self.threshold == 0.0: print "Ignore the this state" return "completed" else: rospy.loginfo("Cannot subscribe the topic: %s", self._topic) return "failed" def on_enter(self, userdata): pass #self._sub_wrench = ProxySubscriberCached({self._topic: WrenchStamped}) def on_exit(self, userdata): pass def on_start(self): pass def on_stop(self): pass
class Reset_Control_State_GR(EventState): ''' Reset control takes in the trial information from Test control and on a succesful completion starts the Data Control. If all trials are completed then it will loop back to Test control. Direction is used for determining a successful and unsuccseful outcome for testing purposed but will need to be replaced with a different measure of success once it becomes more fleshed out. TODO: More complex information for trials, update info -- direction int TEMPORARY: Determines a succesful (1) or unsuccesful (0) outcome for testing purposes ># number_of_trials Trial information (currently just an int) <= continue All actions completed <= failed Trial control failed to initialize or call something TODO: Proper error checking <= completed All Trials have been succesfully completed, go back to Test control ''' def __init__(self): # Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments. super(Reset_Control_State_GR, self).__init__(outcomes = ["continue", "failed"], input_keys=["rotation"]) # Store state parameters for later use. self._rotation = None self._pub = ProxyPublisher({"reset_start": Int32}) self._sub = ProxySubscriberCached({"reset_complete": Int32}) #rospy.init_node('reset_control', anonymous=True) def execute(self, userdata): #publish to arduino #while(1): if self._sub.has_msg("reset_complete"): msg = self._sub.get_last_msg("reset_complete") print(msg) # in case you want to make sure the same message is not processed twice: self._sub.remove_last_msg("reset_complete") return "continue" def on_enter(self, userdata): #self._rotation = 5 #Initializes class variable from userdata, has to be done outside of constructor if(self._rotation is None and userdata.rotation is not None): self._rotation = userdata.rotation self._pub.publish("reset_start", self._rotation)
class GetPoseState(EventState): """ Grabs the most recent published PoseStamped. -- topic String The topic to subscribe to #> goal PointStamped The goal. <= done Goal PoseStamped is available. """ def __init__(self, topic = 'move_base_simple/goal'): """Constructor""" super(GetPoseState, self).__init__(outcomes=['done'], output_keys=['goal']) self._topic = topic self._sub = ProxySubscriberCached({self._topic: PoseStamped}) self._goal_pose = None def execute(self, userdata): if self._sub.has_msg(self._topic): Logger.loginfo('%s Received new goal' % (self.name)) self._goal_pose = self._sub.get_last_msg(self._topic) self._sub.remove_last_msg(self._topic) if self._goal_pose is not None: userdata.goal = self._goal_pose return 'done' def on_enter(self, userdata): userdata.goal = None self._goal_pose = None if self._sub.has_msg(self._topic): Logger.loginfo('%s Clearing prior goal' % (self.name)) self._sub.remove_last_msg(self._topic) Logger.loghint('%s Input new 2D Nav goal (e.g. via RViz) ' % (self.name))
class hsr_SpeechRecognitionByHeadMic(EventState): ''' Example for a state to demonstrate which functionality is available for state implementation. This example lets the behavior wait until the given target_time has passed since the behavior has been started. #> recognition String speech recognition <= recognition recognition ''' def __init__(self): super(hsr_SpeechRecognitionByHeadMic,self).__init__(outcomes=['recognition'],output_keys=['recognition']) self._topic = "/hsrb/voice/text" self.Speech2Text_CB = ProxySubscriberCached({self._topic: RecognitionResult}) #self.led_cli = actionlib.SimpleActionClient('em_led_action', ledAction) # LED self.goal = ledGoal() p = subprocess.Popen("rosservice call /hsrb/voice/stop_recognition '{}'", shell=True) def execute(self, userdata): if self.Speech2Text_CB.has_msg(self._topic): text = self.Speech2Text_CB.get_last_msg(self._topic) result = text.sentences rospy.loginfo('[Subscribe %s] "%s"', self._topic, result[0]) print result[0] userdata.recognition = result[0] self.Speech2Text_CB.remove_last_msg(self._topic) return 'recognition' def on_enter(self, userdata): #self.led_cli.wait_for_server() self.goal.mode = 2 self.goal.color = ColorRGBA(r=0.0, g=1.0, b=0.0, a=0.0) self.goal.flash_interval = 5 #self.led_cli.send_goal(self.goal) p = subprocess.Popen("rosservice call /hsrb/voice/start_recognition '{}'", shell=True) def on_exit(self, userdata): self.goal.mode = 1 self.goal.color = ColorRGBA(r=0.0, g=0.0, b=1.0, a=0.0) self.goal.flash_interval = 0 #self.led_cli.send_goal(self.goal) p = subprocess.Popen("rosservice call /hsrb/voice/stop_recognition '{}'", shell=True) def on_start(self): pass def on_stop(self): pass
class hsr_SpeechRecognition(EventState): ''' Example for a state to demonstrate which functionality is available for state implementation. This example lets the behavior wait until the given target_time has passed since the behavior has been started. -- topic string topic #> recognition String speech recognition <= recognition recognition ''' def __init__(self, topic): super(hsr_SpeechRecognition, self).__init__(outcomes=['recognition'], output_keys=['recognition']) self._topic = topic self.Speech2Text_CB = ProxySubscriberCached({self._topic: String}) # self.led_cli = actionlib.SimpleActionClient('em_led_action', ledAction) # self.goal = ledGoal() def execute(self, userdata): if self.Speech2Text_CB.has_msg(self._topic): text = self.Speech2Text_CB.get_last_msg(self._topic) rospy.loginfo('[Subscribe %s] "%s"', self._topic, text.data) userdata.recognition = text.data self.Speech2Text_CB.remove_last_msg(self._topic) return 'recognition' def on_enter(self, userdata): pass # self.led_cli.wait_for_server() # self.goal.mode = 2 # self.goal.color = ColorRGBA(r=0.0, g=1.0, b=0.0, a=0.0) # self.goal.flash_interval = 5 # self.led_cli.send_goal(self.goal) def on_exit(self, userdata): pass def on_start(self): pass def on_stop(self): pass
class AutonomousState(EventState): ''' Project11 state where backseat driver is in control with mission manager executing missions and commands. ''' def __init__(self): # Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments. super(AutonomousState, self).__init__(outcomes=['manual', 'standby']) self.subscribers = ProxySubscriberCached({'/joy': Joy}) def execute(self, userdata): # This method is called periodically while the state is active. # Main purpose is to check state conditions and trigger a corresponding outcome. # If no outcome is returned, the state will stay active. if self.subscribers.has_msg('/joy'): msg = self.subscribers.get_last_msg('/joy') print msg if msg.buttons[0]: return 'manual' if msg.buttons[2]: return 'standby' def on_enter(self, userdata): # This method is called when the state becomes active, i.e. a transition from another state to this one is taken. # It is primarily used to start actions which are associated with this state. pass def on_exit(self, userdata): # This method is called when an outcome is returned and another state gets active. # It can be used to stop possibly running processes started by on_enter. pass # Nothing to do in this example. def on_start(self): # This method is called when the behavior is started. # If possible, it is generally better to initialize used resources in the constructor # because if anything failed, the behavior would not even be started. pass def on_stop(self): # This method is called whenever the behavior stops execution, also if it is cancelled. # Use this event to clean up things like claimed resources. pass # Nothing to do in this example.
class Project11StateBase(): ''' Base class for Project11 states. Handles operations, such as interpreting jostick buttons, that are common to many states. ''' def __init__(self): self.base_subscribers = ProxySubscriberCached({'/joy': Joy}) def checkJoystick(self): if self.base_subscribers.has_msg('/joy'): msg = self.base_subscribers.get_last_msg('/joy') state_request = None if msg.buttons[0]: state_request = 'manual' if msg.buttons[1]: state_request = 'autonomous' if msg.buttons[2]: state_request = 'standby' return msg, state_request
class TakePictureState(EventState): ''' Stores the picture of the given topic. #> Image Image The received pointcloud. <= done The picture has been received and stored. ''' def __init__(self): super(TakePictureState, self).__init__(outcomes=['done'], output_keys=['Image']) self._topic = '/head_xtion/rgb/image_rect_color' self._sub = ProxySubscriberCached({self._topic: Image}) def execute(self, userdata): if self._sub.has_msg(self._topic): userdata.Image = self._sub.get_last_msg(self._topic) return 'done'
class DetectPersonState(EventState): ''' Detects the nearest person and provides their pose. -- wait_timeout float Time (seconds) to wait for a person before giving up. #> person_pose PoseStamped Pose of the nearest person if one is detected, else None. <= detected Detected a person. <= not_detected No person detected, but time ran out. ''' def __init__(self, wait_timeout): super(DetectPersonState, self).__init__(outcomes = ['detected', 'not_detected'], output_keys = ['person_pose']) self._wait_timeout = rospy.Duration(wait_timeout) self._topic = '/people_tracker/pose' self._sub = ProxySubscriberCached({self._topic: PoseStamped}) self._start_waiting_time = None def execute(self, userdata): if rospy.Time.now() > self._start_waiting_time + self._wait_timeout: userdata.person_pose = None print 'detect_person_state: did not detect any person' return 'not_detected' if self._sub.has_msg(self._topic): userdata.person_pose = self._sub.get_last_msg(self._topic) print 'detect_person_state: detected a person' return 'detected' def on_enter(self, userdata): print 'detect_person_state: trying to detect a person' self._sub.remove_last_msg(self._topic) self._start_waiting_time = rospy.Time.now()
class TakePictureState(EventState): ''' Stores the picture of the given topic. #> Image Image The received pointcloud. <= done The picture has been received and stored. ''' def __init__(self): super(TakePictureState, self).__init__(outcomes = ['done'], output_keys = ['Image']) self._topic = '/head_xtion/rgb/image_rect_color' self._sub = ProxySubscriberCached({self._topic:Image}) def execute(self, userdata): if self._sub.has_msg(self._topic): userdata.Image = self._sub.get_last_msg(self._topic) return 'done'
class CreateStatusState(EventState): """ The state monitors the iRobot Create bumper status, and stops and returns a failed outcome if a bumper is activated. <= failed A bumper was activated prior to completion """ def __init__(self): super(CreateStatusState, self).__init__(outcomes = ['failed']) self._sensor_topic = '/create_node/sensor_state' self._sensor_sub = ProxySubscriberCached({self._sensor_topic: CreateSensorState}) def execute(self, userdata): if (self._sensor_sub.has_msg(self._sensor_topic)): sensors = self._sub.get_last_msg(self._sensor_topic) if ((sensors.bumps_wheeldrops > 0) or sensors.cliff_left or sensors.cliff_front_left or sensors.cliff_front_right or sensors.cliff_right): Logger.logwarn('Bumper contact = %d cliff: left=%d %d %d %d = right ' % (sensors.bumps_wheeldrops, sensors.cliff_left, sensors.cliff_front_left,sensors.cliff_front_right, sensors.cliff_right)) return 'failed'
class GetRobcoCommand(EventState): ''' Listens to /robco/command and gets the incomming command. #> robco_command_OUT String Last received robco command <= failed If behavior is unable to send the TTS message <= done TTS message sent succesfully ''' def __init__(self): super(GetRobcoCommand, self).__init__(outcomes=['failed', 'done'], output_keys=['robco_command_OUT']) self._command_topic = '/robco/command' self.command = None #create subscriber self.sub = ProxySubscriberCached({self._command_topic: String}) def execute(self, userdata): if self.sub.has_msg(self._command_topic): command = self._sub.get_last_msg(self._command_topic) userdata.robco_command_OUT = command.data Logger.loginfo('Incomming command on /robco/command topic: %s' % command) # in case you want to make sure the same message is not processed twice: self._sub.remove_last_msg(self._command_topic) return 'done' def on_exit(self, userdata): #TODO unsubscribe the subscriber proxy from that topic? # self.sub.???? pass def on_stop(self): pass
class DetectPersonState(EventState): ''' Detects the nearest person and provides their pose. -- wait_timeout float Time (seconds) to wait for a person before giving up. #> person_pose PoseStamped Pose of the nearest person if one is detected, else None. <= detected Detected a person. <= not_detected No person detected, but time ran out. ''' def __init__(self, wait_timeout): super(DetectPersonState, self).__init__(outcomes = ['detected', 'not_detected'], output_keys = ['person_pose']) self._wait_timeout = rospy.Duration(wait_timeout) self._topic = '/people_tracker/pose' self._sub = ProxySubscriberCached({self._topic: PoseStamped}) self._start_waiting_time = None def execute(self, userdata): if rospy.Time.now() > self._start_waiting_time + self._wait_timeout: userdata.person_pose = None return 'not_detected' if self._sub.has_msg(self._topic): userdata.person_pose = self._sub.get_last_msg(self._topic) return 'detected' def on_enter(self, userdata): self._sub.remove_last_msg(self._topic) self._start_waiting_time = rospy.Time.now()
def test_subscribe_buffer(self): t1 = '/buffered_1' pub = ProxyPublisher({t1: String}) sub = ProxySubscriberCached({t1: String}) sub.enable_buffer(t1) self.assertTrue(pub.wait_for_any(t1)) pub.publish(t1, String('1')) pub.publish(t1, String('2')) rospy.sleep(.5) # make sure messages can be received self.assertTrue(sub.has_msg(t1)) self.assertTrue(sub.has_buffered(t1)) self.assertEqual(sub.get_from_buffer(t1).data, '1') pub.publish(t1, String('3')) rospy.sleep(.5) # make sure messages can be received self.assertEqual(sub.get_from_buffer(t1).data, '2') self.assertEqual(sub.get_from_buffer(t1).data, '3') self.assertIsNone(sub.get_from_buffer(t1)) self.assertFalse(sub.has_buffered(t1))
class Arm_Control_State_GR(EventState): ''' Arm control takes in the trial information from Test control and on a succesful completion starts the reset. TODO: More complex information and better ways to call things also better description. -- direction int TEMPORARY: Determines a succesful (1) or unsuccesful (0) outcome for testing purposes ># number_of_trials Trial information (currently just an int) <= continue All actions completed <= failed Trial control failed to initialize or call something TODO: Proper error checking <= completed All Trials have been succesfully completed, go back to Test control ''' def __init__(self): # Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments. super(Arm_Control_State_GR, self).__init__(outcomes = ["continue", "failed"]) self._pub = ProxyPublisher({"sim2real/reset_status": Int32}) self._sub = ProxySubscriberCached({"sim2real/reset": Int32}) #rospy.init_node('reset_control', anonymous=True) def execute(self, userdata): if self._sub.has_msg("sim2real/reset"): msg = self._sub.get_last_msg("sim2real/reset") self._sub.remove_last_msg("sim2real/reset") if(msg.data == 1): return "continue" def on_enter(self, userdata): self._pub.publish("sim2real/reset_status", 1)
class GetPointcloudState(EventState): ''' Retrieves the latest pointcloud on the given topic. -- topic string The topic on which to listen for the pointcloud. -- timeout float Optional timeout in seconds of waiting for a pointclod. Set to 0 in order to wait forever. #> pointcloud PointCloud2 The received pointcloud. <= done Pointcloud has been received and is now available in userdata. <= timeout No pointcloud has been received, but maximum waiting time has passed. ''' def __init__(self, topic, timeout = 0): super(GetPointcloudState, self).__init__(outcomes = ['done', 'timeout'], output_keys = ['pointcloud']) self._sub = ProxySubscriberCached({topic: PointCloud2}) self._pcl_topic = topic self._timeout = timeout self._timeout_time = None def execute(self, userdata): if self._sub.has_msg(self._pcl_topic): userdata.pointcloud = self._sub.get_last_msg(self._pcl_topic) return 'done' if self._timeout_time < rospy.Time.now(): return 'timeout' def on_enter(self, userdata): self._timeout_time = rospy.Time.now() + rospy.Duration(self._timeout)
class TestSubState(EventState): def __init__(self, topic): '''Constructor''' super(TestSubState, self).__init__(outcomes=['received', 'unavailable'], output_keys=['result']) self._topic = topic self._sub = ProxySubscriberCached({self._topic: String}) self._msg_counter = 0 self._timeout = rospy.Time.now() + rospy.Duration(1.5) def execute(self, userdata): if self._msg_counter == 0 and rospy.Time.now() > self._timeout: return 'unavailable' if self._sub.has_msg(self._topic): msg = self._sub.get_last_msg(self._topic) self._sub.remove_last_msg(self._topic) userdata.result = msg.data self._msg_counter += 1 if self._msg_counter == 3: return 'received'
class GetEntityByID(EventState): ''' Search for an entity by it's ID number ># ID int The ID of the entity #> Entity object The entity found <= found the entity has been found <= not_found noting has been found ''' def __init__(self): ''' Constructor ''' super(GetEntityByID, self).__init__(outcomes=['found', 'not_found'], input_keys=['ID'], output_keys=['Entity']) self._sub = ProxySubscriberCached({'/entities': Entities}) self.message = None def execute(self, userdata): if self._sub.has_msg('/entities'): Logger.loginfo('getting list of entities') self.message = self._sub.get_last_msg('/entities') self._sub.remove_last_msg('/entities') if self.message is not None: for entity in self.message.entities: if entity.ID == userdata.ID: userdata.Entity = entity return 'found' return 'not_found'
class GenericPubSub(EventState): ''' GenericPubSub state publishes an integer (Int32) once to the topic given. It also subscribes to a given topic (Int32 message only) and will not move forward until a response is recieved. This is meant to be a template to be modified to your needs it is not super useful in its current state. ''' def __init__(self, pubtopic, subtopic, pubint): # Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments. super(GenericPubSub, self).__init__(outcomes=["completed", "failed"]) # Store state parameters for later use. self._rotation = None self._pubtopic = pubtopic self._subtopic = subtopic self._pubint = pubint self._pub = ProxyPublisher({self._pubtopic: Int32}) self._sub = ProxySubscriberCached({self._subtopic: Int32}) #rospy.init_node('reset_control', anonymous=True) def execute(self, userdata): #publish to arduino #while(1): if self._sub.has_msg(self._subtopic): msg = self._sub.get_last_msg(self._subtopic) print(msg) # in case you want to make sure the same message is not processed twice: self._sub.remove_last_msg(self._subtopic) return "completed" def on_enter(self, userdata): #self._rotation = 5 #Initializes class variable from userdata, has to be done outside of constructor self._pub.publish(self._pubtopic, self._pubint)
class TakeImageCamera(EventState): ''' Return the image from the topic. >= topic String the topic <= image Image the image <= received the image is receivec <= unavailable can't subscribe ''' def __init__(self, topic): ''' Constructor ''' super(TakeImageCamera, self).__init__(outcomes=['received', 'unavailable'], output_keys=['image']) self._sub = ProxySubscriberCached({topic: Image}) self.topic = topic self.message = None def execute(self, userdata): if self._sub.has_msg(self.topic): Logger.loginfo('Has an image') self.message = self._sub.get_last_msg(self.topic) self._sub.remove_last_msg(self.topic) if self.message is not None: userdata.image = self.message return 'received' return 'unavailable'
class LockableState(ManuallyTransitionableState): """ A state that can be locked. When locked, no transition can be done regardless of the resulting outcome. However, if any outcome would be triggered, the outcome will be stored and the state won't be executed anymore until it is unlocked and the stored outcome is set. """ def __init__(self, *args, **kwargs): super(LockableState, self).__init__(*args, **kwargs) self._locked = False self._stored_outcome = None self.__execute = self.execute self.execute = self._lockable_execute self._feedback_topic = 'flexbe/command_feedback' self._lock_topic = 'flexbe/command/lock' self._unlock_topic = 'flexbe/command/unlock' self._pub = ProxyPublisher() self._sub = ProxySubscriberCached() def _lockable_execute(self, *args, **kwargs): if self._is_controlled and self._sub.has_msg(self._lock_topic): msg = self._sub.get_last_msg(self._lock_topic) self._sub.remove_last_msg(self._lock_topic) self._execute_lock(msg.data) if self._is_controlled and self._sub.has_msg(self._unlock_topic): msg = self._sub.get_last_msg(self._unlock_topic) self._sub.remove_last_msg(self._unlock_topic) self._execute_unlock(msg.data) if self._locked: if self._stored_outcome is None or self._stored_outcome == 'None': self._stored_outcome = self.__execute(*args, **kwargs) return None if not self._locked and not self._stored_outcome is None and not self._stored_outcome == 'None': if self._parent.transition_allowed(self.name, self._stored_outcome): outcome = self._stored_outcome self._stored_outcome = None return outcome else: return None outcome = self.__execute(*args, **kwargs) if outcome is None or outcome == 'None': return None if not self._parent is None and not self._parent.transition_allowed(self.name, outcome): self._stored_outcome = outcome return None return outcome def _execute_lock(self, target): found_target = False if target == self._get_path(): found_target = True self._locked = True else: found_target = self._parent.lock(target) self._pub.publish(self._feedback_topic, CommandFeedback(command="lock", args=[target, target if found_target else ""])) if not found_target: rospy.logwarn("--> Wanted to lock %s, but could not find it in current path %s.", target, self._get_path()) else: rospy.loginfo("--> Locking in state %s", target) def _execute_unlock(self, target): found_target = False if target == self._get_path(): found_target = True self._locked = False else: found_target = self._parent.unlock(target) self._pub.publish(self._feedback_topic, CommandFeedback(command="unlock", args=[target, target if found_target else ""])) if not found_target: rospy.logwarn("--> Wanted to unlock %s, but could not find it in current path %s.", target, self._get_path()) else: rospy.loginfo("--> Unlocking in state %s", target) def _enable_ros_control(self): super(LockableState, self)._enable_ros_control() self._pub.createPublisher(self._feedback_topic, CommandFeedback) self._sub.subscribe(self._lock_topic, String) self._sub.subscribe(self._unlock_topic, String) def _disable_ros_control(self): super(LockableState, self)._disable_ros_control() self._sub.unsubscribe_topic(self._lock_topic) self._sub.unsubscribe_topic(self._unlock_topic) def is_locked(self): return self._locked
class SubscriberState(EventState): ''' Gets the latest message on the given topic and stores it to userdata. -- topic string The topic on which should be listened. -- blocking bool Blocks until a message is received. -- clear bool Drops last message on this topic on enter in order to only handle message received since this state is active. #> message object Latest message on the given topic of the respective type. <= received Message has been received and stored in userdata or state is not blocking. <= unavailable The topic is not available when this state becomes actives. ''' def __init__(self, topic, blocking = True, clear = False): ''' Constructor ''' super(SubscriberState, self).__init__(outcomes=['received', 'unavailable'], output_keys=['message']) self._topic = topic self._blocking = blocking self._clear = clear self._connected = False (msg_path, msg_topic, fn) = rostopic.get_topic_type(self._topic) if msg_topic == self._topic: msg_type = self._get_msg_from_path(msg_path) self._sub = ProxySubscriberCached({self._topic: msg_type}) self._connected = True else: Logger.logwarn('Topic %s for state %s not yet available.\nFound: %s\nWill try again when entering the state...' % (self._topic, self.name, str(msg_topic))) def execute(self, userdata): ''' Execute this state ''' if not self._connected: return 'unavailable' if self._sub.has_msg(self._topic) or not self._blocking: userdata.message = self._sub.get_last_msg(self._topic) self._sub.remove_last_msg(self._topic) return 'received' def on_enter(self, userdata): if not self._connected: (msg_path, msg_topic, fn) = rostopic.get_topic_type(self._topic) if msg_topic == self._topic: msg_type = self._get_msg_from_path(msg_path) self._sub = ProxySubscriberCached({self._topic: msg_type}) self._connected = True Logger.loginfo('Successfully subscribed to previously unavailable topic %s' % self._topic) else: Logger.logwarn('Topic %s still not available, giving up.' % self._topic) if self._connected and self._clear and self._sub.has_msg(self._topic): self._sub.remove_last_msg(self._topic) def _get_msg_from_path(self, msg_path): msg_import = msg_path.split('/') msg_module = '%s.msg' % (msg_import[0]) package = __import__(msg_module, fromlist=[msg_module]) clsmembers = inspect.getmembers(package, lambda member: inspect.isclass(member) and member.__module__.endswith(msg_import[1])) return clsmembers[0][1]
class EventState(OperatableState): """ A state that allows implementing certain events. """ def __init__(self, *args, **kwargs): super(EventState, self).__init__(*args, **kwargs) self._entering = True self._skipped = False self.__execute = self.execute self.execute = self._event_execute self._paused = False self._last_active_container = None self._feedback_topic = "flexbe/command_feedback" self._repeat_topic = "flexbe/command/repeat" self._pause_topic = "flexbe/command/pause" self._pub = ProxyPublisher() self._sub = ProxySubscriberCached() def _event_execute(self, *args, **kwargs): if self._is_controlled and self._sub.has_msg(self._pause_topic): msg = self._sub.get_last_msg(self._pause_topic) if msg.data: self._sub.remove_last_msg(self._pause_topic) rospy.loginfo("--> Pausing in state %s", self.name) self._pub.publish(self._feedback_topic, CommandFeedback(command="pause")) self._last_active_container = PriorityContainer.active_container PriorityContainer.active_container = "" self._paused = True if self._paused and not PreemptableState.preempt: self._notify_skipped() return self._loopback_name if self._entering: self._entering = False self.on_enter(*args, **kwargs) if self._skipped: self._skipped = False self.on_resume(*args, **kwargs) execute_outcome = self.__execute(*args, **kwargs) repeat = False if self._is_controlled and self._sub.has_msg(self._repeat_topic): rospy.loginfo("--> Repeating state %s", self.name) self._sub.remove_last_msg(self._repeat_topic) self._pub.publish(self._feedback_topic, CommandFeedback(command="repeat")) repeat = True if execute_outcome != self._loopback_name and not PreemptableState.switching or repeat: self._entering = True self.on_exit(*args, **kwargs) return execute_outcome def _notify_skipped(self): if not self._skipped: self.on_pause() self._skipped = True if self._is_controlled and self._sub.has_msg(self._pause_topic): msg = self._sub.get_last_msg(self._pause_topic) if not msg.data: self._sub.remove_last_msg(self._pause_topic) rospy.loginfo("--> Resuming in state %s", self.name) self._pub.publish(self._feedback_topic, CommandFeedback(command="resume")) PriorityContainer.active_container = self._last_active_container self._last_active_container = None self._paused = False super(EventState, self)._notify_skipped() def _enable_ros_control(self): super(EventState, self)._enable_ros_control() self._pub.createPublisher(self._feedback_topic, CommandFeedback) self._sub.subscribe(self._repeat_topic, Empty) self._sub.subscribe(self._pause_topic, Bool) def _disable_ros_control(self): super(EventState, self)._disable_ros_control() self._sub.unsubscribe_topic(self._repeat_topic) self._sub.unsubscribe_topic(self._pause_topic) self._last_active_container = None if self._paused: PriorityContainer.active_container = None # Events # (just implement the ones you need) def on_start(self): """ Will be executed once when the behavior starts. """ pass def on_stop(self): """ Will be executed once when the behavior stops or is preempted. """ pass def on_pause(self): """ Will be executed each time this state is paused. """ pass def on_resume(self, userdata): """ Will be executed each time this state is resumed. """ pass def on_enter(self, userdata): """ Will be executed each time the state is entered from any other state (but not from itself). """ pass def on_exit(self, userdata): """ Will be executed each time the state will be left to any other state (but not to itself). """ pass