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))
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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 GetCurrentJointValuesListState(EventState): ''' Retrieves current values of specified joints. Joints are specified by parameter list. -- joint_names string[] List of desired joint names. -- timeout double Timeout value (optional) -- joint_states_topic string Optional name of joint states topic (default: /joint_states) #> joint_names string[] List of current joint names. #> joint_values float[] List of current joint values. <= retrieved Joint values are available. <= timeout Joint values are not available. ''' def __init__(self, joint_names, timeout=None, joint_states_topic='/joint_states'): ''' Constructor ''' super(GetCurrentJointValuesListState, self).__init__(outcomes=['retrieved', 'timeout'], output_keys=['joint_names', 'joint_values']) self.topic = joint_states_topic self.sub = ProxySubscriberCached({self.topic: JointState}) self.joint_names = joint_names self.joint_values = list() if (timeout is not None) and (timeout > 0.0): self.timeout_duration = rospy.Duration(timeout) else: self.timeout_duration = None def execute(self, userdata): if (self.return_code is not None): # Handle blocked transition or error during on_enter userdata.joint_names = self.joint_names userdata.joint_values = self.joint_values return self.return_code while self.sub.has_buffered(self.topic): msg = self.sub.get_from_buffer(self.topic) for i in range(len(msg.name)): if msg.name[i] in self.joint_names \ and self.joint_values[self.joint_names.index(msg.name[i])] is None: self.joint_values[self.joint_names.index( msg.name[i])] = msg.position[i] if all(v is not None for v in self.joint_values): userdata.joint_names = self.joint_names userdata.joint_values = self.joint_values self.return_code = 'retrieved' return 'retrieved' if (self.timeout_duration is not None and \ (rospy.Time.now()-self.start_time) > self.timeout_duration ): Logger.logerr( 'Timeout %s - found %s of %s' % (self.name, str(self.joint_values), str(self.joint_names))) self.return_code = 'timeout' return 'timeout' def on_enter(self, userdata): self.sub.enable_buffer(self.topic) self.sub.remove_last_msg(self.topic, True) self.joint_values = [None] * len(self.joint_names) self.return_code = None self.start_time = rospy.Time.now() userdata.joint_names = self.joint_names # Clear user data until we have valid data userdata.joint_values = None def on_exit(self, userdata): self.sub.disable_buffer(self.topic)
class hsr_SpaCoTyGetDatasetPlus(EventState): ''' This state is to detect bounding box and grasping points for objects in image. Output is detect result. -- output_topic string output_topic is "/bounding_box_2d_monitor" -- rgb_topic string rgb_topic is "/hsrb/head_rgbd_sensor/rgb/image_rect_color" -- depth_topic string depth_topic is "/hsrb/head_rgbd_sensor/depth_registered/image_rect_raw" -- camera_info_topic string camera_info_topic is "/hsrb/head_rgbd_sensor/rgb/camera_info" -- yolo_yaml string load the yaml file for getting labels(default:yolov3.yaml) -- resnet_model string select resnet model (default: resnet50) -- save_image bool option saving image(default:True) -- z_offset float offset about z coordinate (default:0.1 [m]) -- MEMO Please push "/spacoty/get_dataset"(False), if u wanna finished ># save_folder string save the dataset to save_folder directory(default:default) <= completed transition leading to completion <= failed transition leading to failure ''' def __init__( self, output_topic="/bounding_box_2d_monitor", rgb_topic="/hsrb/head_rgbd_sensor/rgb/image_rect_color", depth_topic="/hsrb/head_rgbd_sensor/depth_registered/image_rect_raw", camera_info_topic="/hsrb/head_rgbd_sensor/rgb/camera_info", yolo_yaml="yolov3.yaml", resnet_model="resnet50", save_image=True, z_offset=0.1, MEMO="/spacoty/get_dataset"): super(hsr_SpaCoTyGetDatasetPlus, self).__init__(outcomes=['completed', 'failed'], input_keys=['save_folder']) # Create the action client when building the behavior. # This will cause the behavior to wait for the client before starting execution # and will trigger a timeout error if it is not available. # Using the proxy client provides asynchronous access to the result and status # and makes sure only one client is used, no matter how often this state is used in a behavior. self._topic = { "img": "bounding_box_2d", "sw": "/spacoty/get_dataset", "wrd": "/spacoty/place_name" } self._output_topic = output_topic self._depth_topic = depth_topic self._rgb_topic = rgb_topic self._camera_info_topic = camera_info_topic self._save_folder = "/root/HSR/catkin_ws/src/em_spco_tidy_up/training_data/" self._save_image = save_image self.camera_info = None self.rgb_image = None self.depth_image = None self.z_offset = z_offset # get object labels # self._yolo_yaml_path = "/root/HSR/catkin_ws/src/hsr_launch/config/darknet_ros/"+yolo_yaml # obj_yaml = rosparam.load_file(self._yolo_yaml_path) # self.obj_class = obj_yaml[0][0]["yolo_model"]["detection_classes"]["names"] self._yolo_yaml_path = "/root/HSR/catkin_ws/src/cv_detect_object/scripts/" + yolo_yaml obj_yaml = rosparam.load_file(self._yolo_yaml_path) self.obj_class = obj_yaml[0][0].values() # select ResNet if resnet_model[-2:] == '50': self.resnet = chainer.links.ResNet50Layers() elif resnet_model[-3:] == '101': self.resnet = chainer.links.ResNet101Layers() elif resnet_model[-3:] == '152': self.resnet = chainer.links.ResNet152Layers() else: print( "[ERROR] You must choice \'ResNet-50\', \'ResNet-101\' or \'ResNet-152\'." ) exit(1) # pass required clients as dict (topic: type) self._client = ProxyActionClient( {self._topic["img"]: hsr_common.msg.BoundingBox2DAction}) self.bridge = cv_bridge.CvBridge() # Subscriber self.sub_sw = ProxySubscriberCached({self._topic["sw"]: Bool}) self.sub_word = ProxySubscriberCached({self._topic["wrd"]: String}) # define the dimension self._data_dim = {"pose": 3, "object": 2048, "word": 0} # It may happen that the action client fails to send the action goal. self._error = False self._get_image_error = False def execute(self, userdata): # While this state is active, check if the action has been finished and evaluate the result. if self.sub_sw.has_msg(self._topic["sw"]): Logger.loginfo('Get the Switcher.') self.finish_sw = self.sub_sw.get_last_msg(self._topic["sw"]) self.finish_sw = self.finish_sw.data self.sub_sw.remove_last_msg(self._topic["sw"]) # Get image #timeout = 1.0 rgb_msg = rospy.wait_for_message( self._rgb_topic, sensor_msgs.msg.Image #, timeout = timeout ) depth_msg = rospy.wait_for_message( self._depth_topic, sensor_msgs.msg.Image #, timeout = timeout ) camera_info_msg = rospy.wait_for_message( self._camera_info_topic, sensor_msgs.msg.CameraInfo #, timeout = timeout ) self.rgb_image = self.bridge.imgmsg_to_cv2(rgb_msg, "bgr8") self.depth_image = self.bridge.imgmsg_to_cv2(depth_msg, "16UC1") self.camera_info = camera_info_msg # Logger.loginfo("Got the image...") if self.finish_sw == True and self._ignore == False: # Create the goal. # Logger.loginfo("Waiting image...") # if self.rgb_image is not None: # Logger.loginfo("Get rgb image!") # sys.stdout.flush() # else: # Logger.logwarn( # 'Failed to get RGB image!') # self._get_image_error = True # if self.depth_image is not None: # Logger.loginfo("Get depth image!") # sys.stdout.flush() # else: # Logger.logwarn( # 'Failed to get Depth image!') # self._get_image_error = True # if self.camera_info is not None: # Logger.loginfo("Get camera info!") # sys.stdout.flush() # else: # Logger.logwarn( # 'Failed to get Camera info!') # self._get_image_error = True self.start = timer() goal = hsr_common.msg.BoundingBox2DGoal() goal.image = self.bridge.cv2_to_imgmsg(self.rgb_image, "bgr8") # Send the goal. # make sure to reset the error state since a previous state execution might have failed try: self._client.send_goal(self._topic["img"], goal) except Exception as e: # Since a state failure not necessarily causes a behavior failure, it is recommended to only print warnings, not errors. # Using a linebreak before appending the error log enables the operator to collapse details in the GUI. Logger.logwarn( 'Failed to send the BoundingBox2D command:\n%s' % str(e)) self._error = True # Check if the client failed to send the goal. if self._get_image_error: return 'failed' elif self._error: return 'failed' # Check if the action has been finished rospy.sleep(0.2) if self._client.has_result(self._topic["img"]): # Logger.loginfo('Success for getting the result of YOLO detecttion.') result = self._client.get_result(self._topic["img"]) self.end = timer() self.detect_sw = True else: self.detect_sw = False if self.detect_sw == True: # In this example, we also provide the amount of cleaned dishes as output key. # userdata.detection = result self.object_count = 0 rgb_image_init = copy(self.rgb_image) for n, region in enumerate(result.regions): hsv = cv2.cvtColor( np.uint8([[[120 * (1 - result.scores[n]), 255, 255]]]), cv2.COLOR_HSV2BGR)[0][0] x0 = region.x_offset y0 = region.y_offset x1 = region.x_offset + region.width - 1 y1 = region.y_offset + region.height - 1 # save the object features dst_im = rgb_image_init[int(y0):int(y0 + region.height), int(x0):int(x0 + region.width)] cv2.rectangle(self.rgb_image, (x0, y0), (x1, y1), (int(hsv[0]), int(hsv[1]), int(hsv[2])), 2) label_str = '%.2f: %s' % (result.scores[n], result.names[n]) text_config = { 'text': label_str, 'fontFace': cv2.FONT_HERSHEY_PLAIN, 'fontScale': 1, 'thickness': 1, } size, baseline = cv2.getTextSize(**text_config) cv2.rectangle(self.rgb_image, (x0, y0), (x0 + size[0], y0 + size[1]), (255, 255, 255), cv2.FILLED) cv2.putText(self.rgb_image, org=(x0, y0 + size[1]), color=(255, 0, 0), **text_config) combinations = [ (x, y) for x in range(int(x0 + region.width * 0.45 - 1), int(x0 + region.width * 0.55 - 1)) for y in range(int(y0 + region.height * 0.45 - 1), int(y0 + region.height * 0.55 - 1)) ] grasping_z = 0 pixel_count = 0 for x, y in combinations: if (self.depth_image.item(y, x) == 0) or (self.depth_image.item(y, x) == 66535): pass else: grasping_z = grasping_z + self.depth_image.item( y, x) pixel_count = pixel_count + 1 if pixel_count != 0: grasping_z = grasping_z / pixel_count cx, cy, cz = self.get_direction( self.camera_info, (x0 + x1) / 2, (y0 + y1) / 2) pose = Pose() pose.position.x = cx * grasping_z / 1000.0 pose.position.y = cy * grasping_z / 1000.0 pose.position.z = (cz * grasping_z / 1000.0) pose.orientation.x = 0.0 pose.orientation.y = 0.0 pose.orientation.z = 0.0 pose.orientation.w = 0.0 try: # transform and save the object poses trans_pose = self.tf2_trans(pose) trans_pose.position.z += self.z_offset self.pose_data = np.append(self.pose_data, [ trans_pose.position.x, trans_pose.position.y, trans_pose.position.z ]) # save the object features dst_im = cv2.resize(dst_im, (224, 224), interpolation=cv2.INTER_CUBIC) dst_im_conv = chainer.links.model.vision.resnet.prepare( dst_im) dst_im_conv = dst_im_conv[np.newaxis] resnet_vec = self.resnet.extract(dst_im_conv) self.object_data = np.append( self.object_data, resnet_vec['pool5'].data[0]) # count detection of object for word information self.object_count += 1 Logger.loginfo( "[Successfully]:Collect the information.") except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): Logger.loginfo( "[ERROR_tf2_time]:ignore the information. (obj:%s, id:%d)" % (result.names[n], len(self.pose_data))) self.tf_buffer = tf2_ros.Buffer() tf_listener = tf2_ros.TransformListener( self.tf_buffer) # Draw the region of using depth information cv2.rectangle(self.rgb_image, (int(x0 + region.width * 0.45 - 1), int(y0 + region.height * 0.45 - 1)), (int(x0 + region.width * 0.55 - 1), int(y0 + region.height * 0.55 - 1)), (grasping_z / 10000 * 255, grasping_z / 10000 * 255, grasping_z / 10000 * 255), cv2.FILLED) #cv2.imshow("Image window", cv_image) # cv2.waitKey(3) # Get the place names pre_word_data = [] if self.sub_word.has_msg(self._topic["wrd"]): msg = self.sub_word.get_last_msg(self._topic["wrd"]) self.sub_word.remove_last_msg(self._topic["wrd"]) pre_word_data = [0 for i in xrange(self._data_dim["word"])] pre_word_data[self.word_list.index(msg.data)] += 1 for m in xrange(self.object_count): self.word_data = np.append(self.word_data, pre_word_data) else: pre_word_data = np.zeros(self._data_dim["word"] * self.object_count) self.word_data = np.append(self.word_data, pre_word_data) try: self.image_pub.publish( self.bridge.cv2_to_imgmsg(self.rgb_image, "bgr8")) if self._save_image: img_name = self._save_folder + '/image/detect' + str( self.save_image_count) + '.png' cv2.imwrite(img_name, self.rgb_image) Logger.loginfo('Save the image: %s' % (img_name)) self.save_image_count += 1 except cv_bridge.CvBridgeError as e: Logger.logwarn(str(e)) # Based on the result, decide which outcome to trigger. if not result is None: Logger.loginfo('Finished detection. (%f [sec])' % (self.end - self.start, )) else: return 'failed' # If the action has not yet finished, no outcome will be returned and the state stays active. elif self.finish_sw == False and self._ignore == False: Logger.loginfo('Finished to get dataset.') re_pose = self.pose_data.reshape( len(self.pose_data) / self._data_dim["pose"], self._data_dim["pose"]) re_object = self.object_data.reshape( len(self.object_data) / self._data_dim["object"], self._data_dim["object"]) re_word = self.word_data.reshape( len(self.word_data) / self._data_dim["word"], self._data_dim["word"]) np.savetxt(self._save_folder + "/pose.csv", re_pose) np.savetxt(self._save_folder + "/object.csv", re_object) np.savetxt(self._save_folder + "/word.csv", re_word) Logger.loginfo("Save the dataset to %s" % (self._save_folder)) return 'completed' self._ignore = False def on_enter(self, userdata): # When entering this state, we send the action goal once to let the robot start its work. # As documented above, we get the specification of which dishwasher to use as input key. # This enables a previous state to make this decision during runtime and provide the ID as its own output key. self.image_pub = rospy.Publisher(self._output_topic, sensor_msgs.msg.Image, latch=True, queue_size=10) self._save_folder = self._save_folder + userdata.save_folder if os.path.exists(self._save_folder) == True: subprocess.Popen("rm -rf " + self._save_folder, shell=True) subprocess.Popen("mkdir -p " + self._save_folder + "/image", shell=True) subprocess.Popen("cp " + self._save_folder[:-len(userdata.save_folder)] + "word_list_st" + userdata.save_folder[-1:] + ".csv " + self._save_folder, shell=True) subprocess.Popen("mv " + self._save_folder + "/word_list_st" + userdata.save_folder[-1:] + ".csv " + self._save_folder + "/word_list.csv", shell=True) time.sleep(0.1) self.pose_data = np.array([]) self.word_data = np.array([]) self.object_data = np.array([]) self.word_list = np.loadtxt(self._save_folder + "/word_list.csv", dtype="unicode", delimiter="\n") self._data_dim.update({"word": len(self.word_list)}) self.save_image_count = 0 self.finish_sw = True self.detect_sw = False self._ignore = True self.tf_buffer = tf2_ros.Buffer() def on_exit(self, userdata): # Make sure that the action is not running when leaving this state. # A situation where the action would still be active is for example when the operator manually triggers an outcome. if not self._client.has_result(self._topic["img"]): self._client.cancel(self._topic["img"]) Logger.loginfo('Cancelled active action goal.') def tf2_trans(self, before_pose): # tf_buffer = tf2_ros.Buffer() trans = self.tf_buffer.lookup_transform( "map", "head_rgbd_sensor_rgb_frame", rospy.Time(0), rospy.Duration(1.0) ) #rospy.Time(0), rospy.Duration(1.0)) #rospy.Time.now()) rospy.get_rostime() pose = PoseStamped() pose.pose = before_pose transformed = tf2_geometry_msgs.do_transform_pose(pose, trans) return transformed.pose def get_direction(self, camera_info, px, py): camera_model = PinholeCameraModel() camera_model.fromCameraInfo(camera_info) cx, cy, cz = camera_model.projectPixelTo3dRay((px, py)) return cx, cy, cz
class GoFowardState(EventState): ''' Driving state for a Robco19. This state allows the robot to drive forward a certain distance at a specified velocity/ speed with colision avoicance. Laser scan for obstacle detectionon on /scan_filtered topic. -- speed float Speed at which to drive the robot -- travel_dist float How far to drive the robot before leaving this state -- obstacle_dist float Distance at which to determine blockage of robot path ># remaining_travel_dist_IN float Holds the remaining distance for the robot to travel #> remaining_travel_dist_OUT float Holds the current distance travelled. <= failed If behavior is unable to ready on time <= done Example for a failure outcome. ''' def __init__(self, speed, travel_dist, obstacle_dist): #add input and output keys super(GoFowardState, self).__init__(outcomes=['failed', 'done'], input_keys=['remaining_travel_dist_IN'], output_keys=['remaining_travel_dist_OUT']) self._start_time = None #make distance_travelled an Instance variable self.distance_travelled = 0.0 self.data = None self._speed = speed self._travel_dist = travel_dist self._obstacle_dist = obstacle_dist self.vel_topic = '/cmd_vel' self.scan_topic = '/scan_filtered' #create publisher passing it the vel_topic_name and msg_type self.pub = ProxyPublisher({self.vel_topic: Twist}) #create subscriber self.scan_sub = ProxySubscriberCached({self.scan_topic: LaserScan}) def execute(self, userdata): if not self.cmd_pub: return 'failed' #run obstacle checks [index 0: left, 360: middle, 719: right] if self.scan_sub.has_msg(self.scan_topic): self.data = self.scan_sub.get_last_msg(self.scan_topic) self.scan_sub.remove_last_msg(self.scan_topic) Logger.loginfo('FWD obstacle distance is: %s' % self.data.ranges[360]) if self.data.ranges[360] <= self._obstacle_dist: #self.scan_sub.remove_last_msg(self.scan_topic) self.data = None #set the output key before transitioning userdata.remaining_travel_dist_OUT = self._travel_dist - self.distance_travelled return 'failed' #measure distance travelled elapsed_time = (rospy.Time.now() - self._start_time).to_sec() self.distance_travelled = (elapsed_time) * self._speed Logger.loginfo("Distance Travelled: %s" % self.distance_travelled) if self.distance_travelled >= self._travel_dist: return 'done' #drive self.pub.publish(self.vel_topic, self.cmd_pub) def on_enter(self, userdata): Logger.loginfo("Drive FWD STARTED!") #set robot speed here self.cmd_pub = Twist() self.cmd_pub.linear.x = self._speed self.cmd_pub.angular.z = 0.0 #check if the input key is set if userdata.remaining_travel_dist_IN: self._travel_dist = userdata.remaining_travel_dist_IN Logger.loginfo("Remaining Distance to Travel: %sm" % self._travel_dist) #start the timer self._start_time = rospy.Time.now() def on_exit(self, userdata): self.cmd_pub.linear.x = 0.0 self.pub.publish(self.vel_topic, self.cmd_pub) #self.scan_sub.unsubscribe_topic(self.scan_topic) Logger.loginfo("Drive FWD ENDED!") def on_start(self): Logger.loginfo("Drive FWD READY!") self._start_time = rospy.Time.now() #bug detected! (move to on_enter) def on_stop(self): Logger.loginfo("Drive FWD STOPPED!") self.cmd_pub.linear.x = 0.0 self.pub.publish(self.vel_topic, self.cmd_pub) def scan_callback(self, data): self.data = data
class GetPositionToPlaceOnTable(EventState): """ Renvoie la position d'un endroit sur la table pour poser un objet. ### InputKey ># angle msg.ranges ### OutputKey <= done Angle de l'obstacle """ def __init__(self): ''' Constructor ''' super(GetPositionToPlaceOnTable, self).__init__(outcomes=['done', 'not_found'], input_keys=['distanceFromEdge'], output_keys=['position']) self.planeTopic = "/segment_table/plane" self.planeSub = ProxySubscriberCached({self.planeTopic: PointCloud2}) self.robotposeTopic = "/robot_pose" self.robotposeSub = ProxySubscriberCached({self.robotposeTopic: Pose}) self.plane = None def dist(self, point1, point2): x = point1.x - point2.x y = point1.y - point2.y return math.sqrt(x * x + y * y) def execute(self, userdata): ''' Execute this state ''' # Get the latest robot pose mypose = self.robotposeSub.get_last_msg(self.robotposeTopic) if self.planeSub.has_msg(self.planeTopic): Logger.loginfo('getting table point cloud') self.plane = self.planeSub.get_last_msg(self.planeTopic) self.planeSub.remove_last_msg(self.planeTopic) if self.plane is not None and mypose is not None: gen = point_cloud2.read_points(self.plane) closest_point = Point() closest_point.x = 0 closest_point.y = 0 closest_point.z = 0 min_dist = 99999 numberOfPoints = 0 sum_x = 0 sum_y = 0 sum_z = 0 # find the closest point and center point = Point() for p in gen: numberOfPoints = numberOfPoints + 1 point.x = p[0] point.y = p[1] point.z = p[2] sum_x += point.x sum_y += point.y sum_z += point.z if self.dist(mypose.position, point) < min_dist: min_dist = self.dist(mypose.position, point) closest_point = point center = Point() center.x = sum_x / numberOfPoints center.y = sum_y / numberOfPoints center.z = sum_z / numberOfPoints #find point to return distanceClosestPointCenter = self.dist(closest_point, center) if distanceClosestPointCenter < userdata.distanceFromEdge: userdata.position = center return "done" else: rapportProportionnel = userdata.distanceFromEdge / distanceClosestPointCenter pointToReturn = Point() pointToReturn.x = closest_point.x + ( center.x - closest_point.x) * rapportProportionnel pointToReturn.y = closest_point.y + ( center.y - closest_point.y) * rapportProportionnel pointToReturn.z = closest_point.z + ( center.z - closest_point.z) * rapportProportionnel userdata.position = pointToReturn return "done" else: if self.plane is None: Logger.loginfo('plane is none') if mypose is None: Logger.loginfo('pose is none')
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 TwistSubState(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(TwistSubState, self).__init__(outcomes=['received', 'unavailable'], output_keys=['message']) self._topic = topic self._blocking = blocking self._clear = clear self._connected = False self._value = Float32() (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._value = self._sub.get_last_msg(self._topic) self._sub.remove_last_msg(self._topic) if (int(value.data) == int(7777.0)): #ball not found in image return 'unavailable' 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 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 WaitForMsgState(EventState): ''' Check new messages on the given topic if they are satisfying the given function. Whenever the function returns True the outcome will be 'successed'. If the function have not been True during the specified time the outcome will be 'failed'. Only message that are received since this state is active are processed. -- topic string The topic on which should be listened. -- wait_time float Amount of time to wait for the function to be valid. -- function Function that check the comming msgs. The function should return bool value -- input_keys string[] State machine variables that will are in the input. -- output_keys string[] State machine variables that will be returned. -- output_function Function that is called whenever calling function with incoming message was successful. It should be used in the case you want to store output_key with message <= successed The function processed a msg with valid result. <= failed Timeout has been reached before the function was valid. ''' @staticmethod def default_output_function(message, userdata): pass def __init__(self, topic, wait_time, function, input_keys = [], output_keys = [], output_function = None): ''' Constructor ''' super(WaitForMsgState, self).__init__(outcomes=['successed', 'failed'], output_keys = output_keys, input_keys = input_keys) self._topic = rospy.resolve_name(topic) self._wait_time = wait_time self._function = function self._connected = False if output_function is None: self._output_function = WaitForMsgState.default_output_function else: self._output_function = output_function (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('[WaitForMsg]: 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 'failed' if self._sub.has_msg(self._topic): message = self._sub.get_last_msg(self._topic) self._sub.remove_last_msg(self._topic) try: if self._function(message): self._output_function(message, userdata) return 'successed' else: return except Exception as e: Logger.logerr('[WaitForMsg]: Failed to use function. Error is: %s' % str(e)) return 'failed' if self._wait_time >= 0: elapsed = rospy.get_rostime() - self._start_time; if (elapsed.to_sec() > self._wait_time): Logger.logerr('[WaitForMsg]: Condition was not satisfied in time limit %d s!'% self._wait_time) return 'failed' def on_enter(self, userdata): self._start_time = rospy.get_rostime() 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('[WaitForMsg]: Successfully subscribed to previously unavailable topic %s' % self._topic) else: Logger.logwarn('[WaitForMsg]: Topic %s still not available, giving up.' % self._topic) # removed last msg if self._connected and self._sub.has_msg(self._topic): Logger.loginfo('[WaitForMsg]: Waiting for msg from topic %s' % 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 list_entities_near_point(EventState): ''' Will list entities near a point, within a radius sorted by distance -- radius float distance used to screen out farther entities. #< name string name to compare entities with, can be used to screen found entities by name #< position Point position used to compare distance, can be Point or Pose #> found_entities object list of found entities <= found entities are found <= none_found no one is found ''' def __init__(self, radius): ''' Constructor ''' super(list_entities_near_point, self).__init__(outcomes=['found', 'none_found'], output_keys=['entity_list', 'number'], input_keys=['name', 'position']) self._sub = ProxySubscriberCached({'/entities': Entities}) self.radius = radius self.message = None self.position = None def execute(self, userdata): if self._sub.has_msg('/entities'): Logger.loginfo('getting detected entities') self.message = self._sub.get_last_msg('/entities') self._sub.remove_last_msg('/entities') if type(userdata.position) == Pose: self.position = userdata.position.position else: self.position = userdata.position if self.message is not None: found_entities = self.list(userdata.name) userdata.entity_list = found_entities userdata.number = len(found_entities) if len(found_entities) != 0: return 'found' else: return 'none_found' def list(self, name): found_entities = [] wraps = [] for entity in self.message.entities: x = entity.position.x - self.position.x y = entity.position.y - self.position.y z = entity.position.z - self.position.z dist = (x**2 + y**2 + z**2)**0.5 #Logger.loginfo('dist: '+str(dist)+" name: "+entity.name) if (name == "" or entity.name == name or entity.category == name) and dist < self.radius: wrap = wrapper() wrap.init(self.position, entity) wraps.append(wrap) wraps.sort(key=wrapper.key) for wrap in wraps: found_entities.append(wrap.entity) return found_entities
class SetGripperState(EventState): ''' Publishes a command for the gripper. >= width float desired gripper width in meter. >= effort float desired gripper effort. <= object_size float result width. <# object an object has been gripped ># no_object no object gripped ''' def __init__(self, width, effort): ''' Constructor ''' super(SetGripperState, self).__init__(outcomes=['object', 'no_object'], output_keys=['object_size']) self.width = width self.effort = effort self._connected = False self._topic = '/sara_gripper_action_controller/gripper_cmd/goal' self._topic_f = '/sara_gripper_action_controller/gripper_cmd/result' self._pub = ProxyPublisher({self._topic: GripperCommandActionGoal}) (msg_path, msg_topic, fn) = rostopic.get_topic_type(self._topic_f) if msg_topic == self._topic_f: msg_type = self._get_msg_from_path(msg_path) self._sub = ProxySubscriberCached({self._topic_f: msg_type}) self._connected = True Logger.loginfo('connecting marker state to '+self._topic_f) else: Logger.logwarn('Topic %s for state %s not yet available.\nFound: %s\nWill try again when entering the state...' % (self._topic_f, self.name, str(msg_topic))) def execute(self, userdata): if not self._connected: return 'no_object' if self._sub.has_msg(self._topic_f): message = self._sub.get_last_msg(self._topic_f) size = message.result.position self._sub.remove_last_msg(self._topic_f) userdata.object_size = size if abs( size-self.width) > 0.006: return 'object' else: return 'no_object' def on_enter(self, userdata): msg = GripperCommandActionGoal() msg.goal.command.position = self.width msg.goal.command.max_effort = self.effort self._pub.publish(self._topic, msg) if not self._connected: (msg_path, msg_topic, fn) = rostopic.get_topic_type(self._topic_f) if msg_topic == self._topic_f: msg_type = self._get_msg_from_path(msg_path) self._sub = ProxySubscriberCached({self._topic_f: msg_type}) self._connected = True Logger.loginfo('Successfully subscribed to previously unavailable topic %s' % self._topic_f) else: Logger.logwarn('Topic %s still not available, giving up.' % self._topic_f) self._sub.remove_last_msg(self._topic_f) 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 # claim priority to propagate pause event PriorityContainer.active_container = self._get_path() 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 and not PreemptableState.preempt: 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 and not PreemptableState.preempt \ 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 SaraSay(EventState): """ Make sara say something -- sentence string What to say. Can be a simple String or a lanmda function using input_keys. (e.g., lambda x: "I found the "x[0] + " on the " + x[1] + "!"). -- input_keys string[] List of available input keys. -- emotion int Set the facial expression. (0=no changes, 1=happy, 2=sad, 3=straight mouth, 4=angry, 5=surprise, 6=blink, 7=party) -- block Bool Should the robot finish it's sentence before continuing? ># input_keys object[] Input(s) to the sentence as a list of userdata. The individual inputs can be accessed as list elements (see lambda expression example). <= done what's said is said """ def __init__(self, sentence, input_keys=[], emotion=0, block=True): """Constructor""" super(SaraSay, self).__init__(outcomes=['done'], input_keys=input_keys) if not (isinstance(sentence, types.LambdaType) and sentence.__name__ == "<lambda>") and len(input_keys) is not 0: raise ValueError( "In state " + type(self).__name__ + " saying \"" + sentence + "\", you need to define a lambda function for sentence.") # Get parameters self.msg = say() self.sentence = sentence self.emotion = UInt8() self.emotion.data = emotion self.block = block # Set topics self.emotion_topic = "sara_face/Emotion" self.say_topic = "/say" self.sayServiceTopic = "/wm_say" self.emotionPub = ProxyPublisher({self.emotion_topic: UInt8}) # Prepare wm_tts if self.block: self.sayClient = ProxyServiceCaller( {self.sayServiceTopic: say_service}) else: self.sayPub = ProxyPublisher({self.say_topic: say}) # Initialise the face sub self.emotionSub = ProxySubscriberCached({self.emotion_topic: UInt8}) self.lastEmotion = None def execute(self, userdata): if self.block: if self.sayClient.is_available(self.sayServiceTopic): try: # Call the say service srvSay = say_serviceRequest() srvSay.say.sentence = self.msg.sentence srvSay.say.emotion = self.msg.emotion self._response = self.sayClient.call( self.sayServiceTopic, srvSay) except rospy.ServiceException as exc: Logger.logwarn("Sara say did not work: \n" + str(exc)) return 'done' else: Logger.logwarn( "wm_tts service is not available. Remaining trials: " + str(self.maxBlockCount)) # Count the number of trials before continuing on life self.maxBlockCount -= 1 if self.maxBlockCount <= 0: return 'done' else: return 'done' def on_enter(self, userdata): if self.emotion.data is not 0: # Save the previous emotion so we can place it back later if self.block and self.emotionSub.has_msg(self.emotion_topic): self.lastEmotion = self.emotionSub.get_last_msg( self.emotion_topic) self.emotionSub.remove_last_msg(self.emotion_topic) # Set the emotion self.emotionPub.publish(self.emotion_topic, self.emotion) # Set the message according to the lambda function if needed. if isinstance( self.sentence, types.LambdaType) and self.sentence.__name__ == "<lambda>": self.msg.sentence = self.sentence( map(lambda key: userdata[key], list(reversed(list(self._input_keys))))) else: self.msg.sentence = self.sentence Logger.loginfo('Sara is saying: "' + str(self.msg.sentence) + '".') # Say the thing if not blocking if not self.block: # publish self.sayPub.publish(self.say_topic, self.msg) return "done" # Set the maximum tries before continuing self.maxBlockCount = 30 def on_exit(self, userdata): if self.block and self.emotion.data is not 0: # Put the original emotion back if self.lastEmotion: self.emotionPub.publish(self.emotion_topic, self.lastEmotion)
class VigirBehaviorMirror(object): ''' classdocs ''' def __init__(self): ''' Constructor ''' self._sm = None smach.set_loggers ( rospy.logdebug, # hide SMACH transition log spamming rospy.logwarn, rospy.logdebug, rospy.logerr ) # set up proxys for sm <--> GUI communication # publish topics self._pub = ProxyPublisher({'flexbe/behavior_update': String, 'flexbe/request_mirror_structure': Int32}) self._running = False self._stopping = False self._active_id = 0 self._starting_path = None self._current_struct = None self._outcome_topic = 'flexbe/mirror/outcome' self._struct_buffer = list() # listen for mirror message self._sub = ProxySubscriberCached() self._sub.subscribe(self._outcome_topic, UInt8) self._sub.enable_buffer(self._outcome_topic) self._sub.subscribe('flexbe/mirror/structure', ContainerStructure, self._mirror_callback) self._sub.subscribe('flexbe/status', BEStatus, self._status_callback) self._sub.subscribe('flexbe/mirror/sync', BehaviorSync, self._sync_callback) self._sub.subscribe('flexbe/mirror/preempt', Empty, self._preempt_callback) def _mirror_callback(self, msg): rate = rospy.Rate(10) while self._stopping: rate.sleep() if self._running: rospy.logwarn('Received a new mirror structure while mirror is already running, adding to buffer (ID: %s).' % str(msg.behavior_id)) elif self._active_id != 0 and msg.behavior_id != self._active_id: rospy.logwarn('ID of received mirror structure (%s) does not match expected ID (%s), will ignore.' % (str(msg.behavior_id), str(self._active_id))) return else: rospy.loginfo('Received a new mirror structure for ID %s' % str(msg.behavior_id)) self._struct_buffer.append(msg) if self._active_id == msg.behavior_id: self._struct_buffer = list() self._mirror_state_machine(msg) rospy.loginfo('Mirror built.') self._execute_mirror() def _status_callback(self, msg): if msg.code == BEStatus.STARTED: thread = threading.Thread(target=self._start_mirror, args=[msg]) thread.daemon = True thread.start() else: thread = threading.Thread(target=self._stop_mirror, args=[msg]) thread.daemon = True thread.start() def _start_mirror(self, msg): rate = rospy.Rate(10) while self._stopping: rate.sleep() if self._running: rospy.logwarn('Tried to start mirror while it is already running, will ignore.') return if len(msg.args) > 0: self._starting_path = "/" + msg.args[0][1:].replace("/", "_mirror/") + "_mirror" self._active_id = msg.behavior_id while self._sm is None and len(self._struct_buffer) > 0: struct = self._struct_buffer[0] self._struct_buffer = self._struct_buffer[1:] if struct.behavior_id == self._active_id: self._mirror_state_machine(struct) rospy.loginfo('Mirror built.') else: rospy.logwarn('Discarded mismatching buffered structure for ID %s' % str(struct.behavior_id)) if self._sm is None: rospy.logwarn('Missing correct mirror structure, requesting...') rospy.sleep(0.2) # no clean way to wait for publisher to be ready... self._pub.publish('flexbe/request_mirror_structure', Int32(msg.behavior_id)) self._active_id = msg.behavior_id return self._execute_mirror() def _stop_mirror(self, msg): self._stopping = True if self._sm is not None and self._running: if msg is not None and msg.code == BEStatus.FINISHED: rospy.loginfo('Onboard behavior finished successfully.') self._pub.publish('flexbe/behavior_update', String()) elif msg is not None and msg.code == BEStatus.SWITCHING: self._starting_path = None rospy.loginfo('Onboard performing behavior switch.') elif msg is not None and msg.code == BEStatus.READY: rospy.loginfo('Onboard engine just started, stopping currently running mirror.') self._pub.publish('flexbe/behavior_update', String()) elif msg is not None: rospy.logwarn('Onboard behavior failed!') self._pub.publish('flexbe/behavior_update', String()) PreemptableState.preempt = True rate = rospy.Rate(10) while self._running: rate.sleep() else: rospy.loginfo('No onboard behavior is active.') self._active_id = 0 self._sm = None self._current_struct = None self._sub.remove_last_msg(self._outcome_topic, clear_buffer=True) if msg is not None and msg.code != BEStatus.SWITCHING: rospy.loginfo('\033[92m--- Behavior Mirror ready! ---\033[0m') self._stopping = False def _sync_callback(self, msg): if msg.behavior_id == self._active_id: thread = threading.Thread(target=self._restart_mirror, args=[msg]) thread.daemon = True thread.start() else: rospy.logerr('Cannot synchronize! Different behavior is running onboard, please stop execution!') thread = threading.Thread(target=self._stop_mirror, args=[None]) thread.daemon = True thread.start() def _restart_mirror(self, msg): rospy.loginfo('Restarting mirror for synchronization...') self._sub.remove_last_msg(self._outcome_topic, clear_buffer=True) if self._sm is not None and self._running: PreemptableState.preempt = True rate = rospy.Rate(10) while self._running: rate.sleep() self._sm = None if msg.current_state_checksum in self._state_checksums: current_state_path = self._state_checksums[msg.current_state_checksum] self._starting_path = "/" + current_state_path[1:].replace("/", "_mirror/") + "_mirror" rospy.loginfo('Current state: %s' % current_state_path) try: self._mirror_state_machine(self._current_struct) rospy.loginfo('Mirror built.') self._execute_mirror() except (AttributeError, smach.InvalidStateError): rospy.loginfo('Stopping synchronization because behavior has stopped.') def _execute_mirror(self): self._running = True rospy.loginfo("Executing mirror...") if self._starting_path is not None: LockableStateMachine.path_for_switch = self._starting_path rospy.loginfo("Starting mirror in state " + self._starting_path) self._starting_path = None result = 'preempted' try: result = self._sm.execute() except Exception as e: rospy.loginfo('Catched exception on preempt:\n%s' % str(e)) if self._sm is not None: self._sm.destroy() self._running = False rospy.loginfo('Mirror finished with result ' + result) def _mirror_state_machine(self, msg): self._current_struct = msg self._state_checksums = dict() root = None for con_msg in msg.containers: if con_msg.path.find('/') == -1: root = con_msg.path break self._add_node(msg, root) # calculate checksums of all states for con_msg in msg.containers: if con_msg.path.find('/') != -1: self._state_checksums[zlib.adler32(con_msg.path)] = con_msg.path def _add_node(self, msg, path): #rospy.loginfo('Add node: '+path) container = None for con_msg in msg.containers: if con_msg.path == path: container = con_msg break transitions = None if container.transitions is not None: transitions = {} for i in range(len(container.transitions)): transitions[container.outcomes[i]] = container.transitions[i] + '_mirror' path_frags = path.split('/') container_name = path_frags[len(path_frags)-1] if len(container.children) > 0: sm_outcomes = [] for outcome in container.outcomes: sm_outcomes.append(outcome + '_mirror') sm = JumpableStateMachine(outcomes=sm_outcomes) with sm: for child in container.children: self._add_node(msg, path+'/'+child) if len(transitions) > 0: container_transitions = {} for i in range(len(container.transitions)): container_transitions[sm_outcomes[i]] = transitions[container.outcomes[i]] JumpableStateMachine.add(container_name + '_mirror', sm, transitions=container_transitions) else: self._sm = sm else: JumpableStateMachine.add(container_name + '_mirror', MirrorState(container_name, path, container.outcomes, container.autonomy), transitions=transitions) def _jump_callback(self, msg): start_time = rospy.Time.now() current_elapsed = 0 jumpable = True while self._sm is None or not self._sm.is_running(): current_elapsed = rospy.Time.now() - start_time if current_elapsed > rospy.Duration.from_sec(10): jumpable = False break rospy.sleep(0.05) if jumpable: self._sm.jump_to(msg.stateName) else: rospy.logwarn('Could not jump in mirror: Mirror does not exist or is not running!') def _preempt_callback(self, msg): if self._sm is not None and self._sm.is_running(): rospy.logwarn('Explicit preempting is currently ignored, mirror should be preempted by onboard behavior.') else: rospy.logwarn('Could not preempt mirror because it seems not to be running!')
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 TestOnboard(unittest.TestCase): def __init__(self, name): super(TestOnboard, self).__init__(name) self.sub = ProxySubscriberCached({ 'flexbe/status': BEStatus, 'flexbe/log': BehaviorLog }) self.rate = rospy.Rate(100) # make sure that behaviors can be imported data_folder = os.path.dirname(os.path.realpath(__file__)) sys.path.insert(0, data_folder) # run onboard and add custom test behaviors to onboard lib self.onboard = FlexbeOnboard() self.lib = self.onboard._behavior_lib self.lib._add_behavior_manifests(data_folder) def assertStatus(self, expected, timeout): """ Assert that the expected onboard status is received before the timeout. """ for i in range(int(timeout * 100)): self.rate.sleep() if self.sub.has_msg('flexbe/status'): break else: raise AssertionError('Did not receive a status as required.') msg = self.sub.get_last_msg('flexbe/status') self.sub.remove_last_msg('flexbe/status') self.assertEqual(msg.code, expected) return msg def test_onboard_behaviors(self): behavior_pub = rospy.Publisher('flexbe/start_behavior', BehaviorSelection, queue_size=1) rospy.sleep(0.5) # wait for publisher # wait for the initial status message self.assertStatus(BEStatus.READY, 1) # send simple behavior request without checksum be_id, _ = self.lib.find_behavior("Test Behavior Log") request = BehaviorSelection() request.behavior_id = be_id request.autonomy_level = 255 behavior_pub.publish(request) self.assertStatus(BEStatus.ERROR, 2) # send valid simple behavior request with open(self.lib.get_sourcecode_filepath(be_id)) as f: request.behavior_checksum = zlib.adler32(f.read()) behavior_pub.publish(request) self.assertStatus(BEStatus.STARTED, 1) self.assertStatus(BEStatus.FINISHED, 3) self.assertIn('data', self.sub.get_last_msg('flexbe/log').text) # send valid complex behavior request be_id, _ = self.lib.find_behavior("Test Behavior Complex") request = BehaviorSelection() request.behavior_id = be_id request.autonomy_level = 255 request.arg_keys = ['param'] request.arg_values = ['value_2'] request.input_keys = ['data'] request.input_values = ['2'] with open(self.lib.get_sourcecode_filepath(be_id)) as f: content = f.read() modifications = [('flexbe_INVALID', 'flexbe_core'), ('raise ValueError("TODO: Remove!")', '')] for replace, by in modifications: index = content.index(replace) request.modifications.append( BehaviorModification(index, index + len(replace), by)) for replace, by in modifications: content = content.replace(replace, by) request.behavior_checksum = zlib.adler32(content) behavior_pub.publish(request) self.assertStatus(BEStatus.STARTED, 1) result = self.assertStatus(BEStatus.FINISHED, 3) self.assertEqual(result.args[0], 'finished') self.assertIn('value_2', self.sub.get_last_msg('flexbe/log').text) # send the same behavior with different parameters request.arg_keys = ['param', 'invalid'] request.arg_values = ['value_1', 'should be ignored'] request.input_keys = [] request.input_values = [] behavior_pub.publish(request) self.assertStatus(BEStatus.STARTED, 1) result = self.assertStatus(BEStatus.FINISHED, 3) self.assertEqual(result.args[0], 'failed') self.assertIn('value_1', self.sub.get_last_msg('flexbe/log').text)