コード例 #1
0
    def __init__(self,
                 controller='motion/controller/joint_state_head',
                 tolerance=0.17,
                 timeout=10.0,
                 joint_topic="joint_states",
                 outcomes=['done', 'failed', 'timeout']):
        super(SetJointStateBase, self).__init__(outcomes=outcomes)

        # Store topic parameter for later use.
        self._controller = controller
        self._joint_topic = joint_topic
        self._tolerance = tolerance
        self._timeout = Duration.from_sec(timeout)

        # create proxies
        self._action_client = ProxyActionClient(
            {self._controller: SetOperationalAction})
        self._pose_publisher = ProxyPublisher(
            {self._controller + '/in_joints_ref': JointState})
        self._pose_subscriber = ProxySubscriberCached(
            {self._joint_topic: JointState})

        # timestamp
        self._timestamp = None
        # error in enter hook
        self._error = False
コード例 #2
0
	def __init__(self, topic):
		super(StorePointcloudState, self).__init__(outcomes = ['done'],
													output_keys = ['pointcloud'])

		self._sub = ProxySubscriberCached({topic: PointCloud2})

		self._pcl_topic = topic
コード例 #3
0
    def __init__(self):
        self._sm = None

        # 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._struct_buffer = list()
        self._sync_lock = threading.Lock()

        self._outcome_topic = 'flexbe/mirror/outcome'

        # 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)
コード例 #4
0
class PreemptableStateMachine(LockableStateMachine):
    """
    A state machine that can be preempted.
    If preempted, the state machine will return the outcome preempted.
    """

    _preempted_name = 'preempted'

    def __init__(self, *args, **kwargs):
        super(PreemptableStateMachine, self).__init__(*args, **kwargs)
        # always listen to preempt so that the behavior can be stopped even if unsupervised
        self._preempt_topic = 'flexbe/command/preempt'
        self._sub = ProxySubscriberCached({self._preempt_topic: Empty})
        self._sub.set_callback(self._preempt_topic, self._preempt_cb)

    def _preempt_cb(self, msg):
        if not self._is_controlled:
            PreemptableState.preempt = True

    @staticmethod
    def add(label, state, transitions=None, remapping=None):
        transitions[PreemptableState.
                    _preempted_name] = PreemptableStateMachine._preempted_name
        LockableStateMachine.add(label, state, transitions, remapping)

    @property
    def _valid_targets(self):
        return super(PreemptableStateMachine, self)._valid_targets + [
            PreemptableStateMachine._preempted_name
        ]
コード例 #5
0
    def __init__(self):
        '''Constructor'''
        super(AttachObjectState,
              self).__init__(outcomes=['done', 'failed'],
                             input_keys=['template_id', 'hand_side'],
                             output_keys=['template_pose'])

        # Set up service for attaching object template
        self._service_topic = "/stitch_object_template"
        self._srv = ProxyServiceCaller(
            {self._service_topic: SetAttachedObjectTemplate})

        # Set up subscribers for listening to the latest wrist poses
        self._wrist_pose_topics = dict()
        self._wrist_pose_topics['left'] = '/flor/l_arm_current_pose'
        self._wrist_pose_topics['right'] = '/flor/r_arm_current_pose'

        self._sub = ProxySubscriberCached({
            self._wrist_pose_topics['left']:
            PoseStamped,
            self._wrist_pose_topics['right']:
            PoseStamped
        })

        self._sub.make_persistant(self._wrist_pose_topics['left'])
        self._sub.make_persistant(self._wrist_pose_topics['right'])

        self._failed = False
コード例 #6
0
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'
コード例 #7
0
    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})
コード例 #8
0
    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.

        for i in [0, 1, 2, 3, 4, 5]:
            self._topic = "/ariac/logical_camera_" + str(i)

            self._start_time = rospy.Time.now()

            (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})
            elapsed = rospy.get_rostime() - self._start_time
            while (elapsed.to_sec() < self._wait):
                elapsed = rospy.get_rostime() - self._start_time
                if self._sub.has_msg(self._topic):
                    message = self._sub.get_last_msg(self._topic)
                    for model in message.models:
                        if model.type == userdata.part_type:
                            userdata.bin = "bingr" + str(i) + "PreGrasp"
                            userdata.camera_topic = "/ariac/logical_camera_" + str(
                                i)
                            userdata.camera_frame = "logical_camera_" + str(
                                i) + "_frame"
                            Logger.loginfo("bingr" + str(i) + "PreGrasp")
                            userdata.ref_frame = "torso_main"
                            return 'continue'
        Logger.loginfo("part_type not found")
        return 'failed'
コード例 #9
0
	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.

 		# One of the outcomes declared above.
	   	for i in [1,2,3,4,5,6]:
			self._topic = "/ariac/logical_camera_stock"+str(i)
			self._start_time = rospy.Time.now()
			(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})
			elapsed = rospy.get_rostime() - self._start_time;
			while (elapsed.to_sec() < self._wait):
				elapsed = rospy.get_rostime() - self._start_time;
				if self._sub.has_msg(self._topic):
					message = self._sub.get_last_msg(self._topic)
					for model in message.models:
						if model.type == userdata.part_type:
							if userdata.arm_id == "Right_Arm":
								userdata.gantry_pos = "Gantry_Part"+str(i)+"_R"
							elif userdata.arm_id == "Left_Arm":
								userdata.gantry_pos = "Gantry_Part"+str(i)+"_L"
							userdata.camera_topic = "/ariac/logical_camera_stock"+str(i)
							userdata.camera_frame = "logical_camera_stock"+str(i)+"_frame"
							Logger.loginfo("Gantry_Part"+str(i))
							if i < 5:
								return 'bin'
							else:
								return 'shelf'
		Logger.loginfo("part_type not found")
		return 'not_found'
コード例 #10
0
    def __init__(self, hand, force_threshold=1.5):
        '''Constructor'''
        super(WaitForForceState,
              self).__init__(outcomes=['success', 'failure'])
        self._action_name = rospy.get_name()
        self._prefix = "meka_roscontrol"
        self._client = {}
        self._movement_finished = {}
        self.force_variation = {}
        self.force_bias = {}
        self.previous_force = {}
        self._r = rospy.Rate(100)
        self._threshold = force_threshold

        # store latest planning scene
        self.current_planning_scene = None

        self.force_variation['left_arm'] = numpy.array([0.0, 0.0, 0.0])
        self.force_variation['right_arm'] = numpy.array([0.0, 0.0, 0.0])
        self.force_bias['left_arm'] = numpy.array([0.0, 0.0, 0.0])
        self.force_bias['right_arm'] = numpy.array([0.0, 0.0, 0.0])
        self.previous_force['left_arm'] = numpy.array([0.0, 0.0, 0.0])
        self.previous_force['right_arm'] = numpy.array([0.0, 0.0, 0.0])

        self._current_val = 0

        self._group = hand + '_arm'

        self.sub_left = ProxySubscriberCached(
            {'/meka_ros_pub/m3loadx6_ma30_l0/wrench': WrenchStamped})
        self.sub_right = ProxySubscriberCached(
            {'/meka_ros_pub/m3loadx6_ma29_l0/wrench': WrenchStamped})
コード例 #11
0
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))
コード例 #12
0
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()
コード例 #13
0
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"
コード例 #14
0
    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})
コード例 #15
0
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))
コード例 #16
0
    def __init__(self):
        """Constructor"""

        super(KeepLookingAt, self).__init__(outcomes=['failed'],
                                         input_keys=['ID'])

        self.entities_topic = "/entities"
        self._sub = ProxySubscriberCached({self.entities_topic: Entities})

        self.pubp = rospy.Publisher("/sara_head_pitch_controller/command", Float64, queue_size=1)
        self.puby = rospy.Publisher("/sara_head_yaw_controller/command", Float64, queue_size=1)

        # Reference from and reference to
        self.service = get_directionRequest()
        self.service.reference = "head_xtion_link"
        self.service.origine = "base_link"

        self.Entity = None

        self.serviceName = '/get_direction'
        Logger.loginfo('waiting for service '+str(self.serviceName))
        self.serv = rospy.ServiceProxy(self.serviceName, get_direction)

        self._active = True
        try:
            self.serv.wait_for_service(1)
        except:
            self._active = False
コード例 #17
0
class PreemptableStateMachine(LoopbackStateMachine):
    """
    A state machine that can be preempted.
    If preempted, the state machine will return the outcome preempted.
    """
    
    _preempted_name = 'preempted'
    
    def __init__(self, *args, **kwargs):
        # add preempted outcome
        if len(args) > 0:
            args[0].append(self._preempted_name)
        else:
            outcomes = kwargs.get('outcomes', [])
            outcomes.append(self._preempted_name)
            kwargs['outcomes'] = outcomes
            
        super(PreemptableStateMachine, self).__init__(*args, **kwargs)

        # always listen to preempt so that the behavior can be stopped even if unsupervised
        self._preempt_topic = 'flexbe/command/preempt'
        self._sub = ProxySubscriberCached({self._preempt_topic: Empty})
        self._sub.set_callback(self._preempt_topic, self._preempt_cb)


    def _preempt_cb(self, msg):
        if not self._is_controlled:
            PreemptableState.preempt = True
            rospy.loginfo("--> Preempted requested while unsupervised")
コード例 #18
0
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
コード例 #19
0
    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
コード例 #20
0
    def __init__(self,
                 target_time,
                 velocity,
                 rotation_rate,
                 cmd_topic='/create_node/cmd_vel',
                 sensor_topic='/create_node/sensor_state'):
        # Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
        super(CreateTimedTwistState,
              self).__init__(outcomes=['done', 'failed'])

        # Store state parameter for later use.
        self._target_time = rospy.Duration(target_time)
        self._twist = TwistStamped()
        self._twist.twist.linear.x = velocity
        self._twist.twist.angular.z = rotation_rate

        # The constructor is called when building the state machine, not when actually starting the behavior.
        # Thus, we cannot save the starting time now and will do so later.
        self._start_time = None

        self._done = None  # Track the outcome so we can detect if transition is blocked

        self._sensor_topic = sensor_topic
        self._cmd_topic = cmd_topic
        self._sensor_sub = ProxySubscriberCached(
            {self._sensor_topic: CreateSensorState})
        self._pub = ProxyPublisher({self._cmd_topic: TwistStamped})
コード例 #21
0
 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)
コード例 #22
0
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
コード例 #23
0
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))
コード例 #24
0
    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
コード例 #25
0
class MirrorState(EventState):
    '''
    This state will display its possible outcomes as buttons in the GUI
    and is designed in a way to be created dynamically.
    '''
    def __init__(self, target_name, target_path, given_outcomes,
                 outcome_autonomy):
        super(MirrorState, self).__init__(outcomes=given_outcomes)
        self.set_rate(100)
        self._target_name = target_name
        self._target_path = target_path

        self._outcome_topic = 'flexbe/mirror/outcome'

        self._pub = ProxyPublisher()
        self._sub = ProxySubscriberCached({self._outcome_topic: UInt8})

    def execute(self, userdata):
        if self._sub.has_buffered(self._outcome_topic):
            msg = self._sub.get_from_buffer(self._outcome_topic)
            if msg.data < len(self.outcomes):
                rospy.loginfo("State update: %s > %s", self._target_name,
                              self.outcomes[msg.data])
                return self.outcomes[msg.data]
        try:
            self.sleep()
        except ROSInterruptException:
            print('Interrupted mirror sleep.')

    def on_enter(self, userdata):
        self._pub.publish(
            'flexbe/behavior_update',
            String("/" + "/".join(self._target_path.split("/")[1:])))
コード例 #26
0
    def __init__(self, ref_frame, camera_topic, camera_frame):
        # Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
        super(DetectPartCameraState,
              self).__init__(outcomes=['continue', 'failed'],
                             output_keys=['pose'])
        self.ref_frame = ref_frame
        self._topic = camera_topic
        self._camera_frame = camera_frame
        self._connected = False
        self._failed = False

        # tf to transfor the object pose
        self._tf_buffer = tf2_ros.Buffer(
            rospy.Duration(10.0))  #tf buffer length
        self._tf_listener = tf2_ros.TransformListener(self._tf_buffer)

        # Subscribe to the topic for the logical camera
        (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)))
コード例 #27
0
 def __init__(self):
     """
     Constructor
     """
     super(WaitForOpenDoorState, self).__init__(outcomes=['done'])
     self._topic = '/laser/srd_front/scan'
     self._sub = ProxySubscriberCached({self._topic: LaserScan})
    def __init__(self, outcomes = ['unknown'], pose_ns = 'saved_msgs/joint_state', tolerance = 0.17, joint_topic = "joint_states", timeout = 1.0):
        # Process supplied pose list. Note it must be processed befor superclass constructor is called,
        # beacuse it changes 'outcomes' list due to side effect.

        # check if 'unknown' outcome is present
        if 'unknown' not in outcomes:
            raise RuntimeError, '"unknown" should presents in state outcomes'
        # remove 'unknown' while preserving items order
        poses_names = [ outcome for outcome in outcomes if outcome != 'unknown' ]
        # Load poses from paramere server. Pose is loaded in form dict(joint name -> position),
        # but they are stored as a list of tuples (pose_name, pose_dict) to preserve order.
        self._poses = [ (name, self.load_joint_state(pose_ns, name)) for name in poses_names ]

        # Call superclass constructor.
        super(CheckJointState, self).__init__(outcomes = outcomes)

        # Subscribe to the joint topic.
        self._joint_topic = joint_topic 
        self._pose_subscriber = ProxySubscriberCached({ self._joint_topic: JointState })
        
        # Store topic parameter for later use.
        self._tolerance = tolerance
        self._timeout = Duration.from_sec(timeout)

        # timestamp
        self._timestamp = None
コード例 #29
0
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
コード例 #30
0
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
コード例 #31
0
    def __init__(self, distance=1, ReplanPeriod=0.5):
        """Constructor"""

        super(SaraFollow, self).__init__(outcomes=['failed'],
                                         input_keys=['ID'])

        # Prepare the action client for move_base
        self._action_topic = "/move_base"
        self._client = ProxyActionClient({self._action_topic: MoveBaseAction})

        # Initialise the subscribers
        self.entities_topic = "/entities"
        self._pose_topic = "/robot_pose"
        self._sub = ProxySubscriberCached({
            self._pose_topic: Pose,
            self.entities_topic: Entities
        })

        # Get the parameters
        self.targetDistance = distance
        self.ReplanPeriod = ReplanPeriod

        # Initialise the status variables
        self.MyPose = None
        self.lastPlanTime = 0
コード例 #32
0
    def __init__(self,
                 leap_motion_topic,
                 exit_states,
                 pose_topic='',
                 parameters=[2.0, 0.02, 0.2]):
        super(LeapMotionMonitor, self).__init__(
            outcomes=['no_object', 'still_object', 'moving_object'],
            output_keys=['pose'])

        # store state parameter for later use.
        self._leap_topic = leap_motion_topic
        self._pose_topic = pose_topic
        self._exit_states = exit_states
        self._detecton_period = parameters[0]
        self._position_tolerance = parameters[1]
        self._orientation_tolerance = parameters[2]

        # setup proxies
        self._leap_subscriber = ProxySubscriberCached(
            {self._leap_topic: leapros})
        if self._pose_topic:
            self._pose_publisher = ProxyPublisher(
                {self._pose_topic: PoseStamped})
        else:
            self._pose_publisher = None

        # pose buffers
        self._pose = None
        self._pose_prev = None
        self._pose_averaged = None
        self._nonstillness_stamp = None
コード例 #33
0
        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})
コード例 #34
0
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'
コード例 #35
0
class PreemptableStateMachine(LoopbackStateMachine):
    """
    A state machine that can be preempted.
    If preempted, the state machine will return the outcome preempted.
    """

    _preempted_name = 'preempted'

    def __init__(self, *args, **kwargs):
        # add preempted outcome
        if len(args) > 0:
            args[0].append(self._preempted_name)
        else:
            outcomes = kwargs.get('outcomes', [])
            outcomes.append(self._preempted_name)
            kwargs['outcomes'] = outcomes

        super(PreemptableStateMachine, self).__init__(*args, **kwargs)

        # always listen to preempt so that the behavior can be stopped even if unsupervised
        self._preempt_topic = 'flexbe/command/preempt'
        self._sub = ProxySubscriberCached({self._preempt_topic: Empty})
        self._sub.set_callback(self._preempt_topic, self._preempt_cb)

    def _preempt_cb(self, msg):
        if not self._is_controlled:
            PreemptableState.preempt = True
コード例 #36
0
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'
コード例 #37
0
class GetWristPoseState(EventState):
	'''
	Retrieves the current wrist pose from the corresponding ROS topic.

	># hand_side 	string 		Wrist whose pose will be returned (left, right)

	#> wrist_pose 	PoseStamped	The current pose of the left or right wrist

	<= done 					Wrist pose has been retrieved.
	<= failed 					Failed to retrieve wrist pose.

	'''

	def __init__(self):
		'''Constructor'''
		super(GetWristPoseState, self).__init__(outcomes = ['done', 'failed'],
												input_keys = ['hand_side'],
												output_keys = ['wrist_pose'])

		# Set up subscribers for listening to the latest wrist poses
		self._wrist_pose_topics = dict()
		self._wrist_pose_topics['left']  = '/flor/l_arm_current_pose'
		self._wrist_pose_topics['right'] = '/flor/r_arm_current_pose'
		
		self._sub = ProxySubscriberCached({
						self._wrist_pose_topics['left']:  PoseStamped,
						self._wrist_pose_topics['right']: PoseStamped})
		
		self._sub.make_persistant(self._wrist_pose_topics['left'])
		self._sub.make_persistant(self._wrist_pose_topics['right'])

		self._failed = False


	def execute(self, userdata):

		if self._failed:
			return 'failed'
		else:
			return 'done'


	def on_enter(self, userdata):

		self._failed = False

		# Read the current wrist pose and write to userdata
		try:
			wrist_pose_topic = self._wrist_pose_topics[userdata.hand_side]
			wrist_pose = self._sub.get_last_msg(wrist_pose_topic)
			userdata.wrist_pose = wrist_pose
			rospy.loginfo('Retrieved %s wrist pose: \n%s',
						  userdata.hand_side, wrist_pose)
		except Exception as e:
			Logger.logwarn('Failed to get the latest wrist pose:\n%s' % str(e))
			self._failed = True
			return
コード例 #38
0
	def __init__(self):
		super(GetRobotPose, self).__init__(outcomes = ['succeeded'], output_keys = ['pose'])

		self._objectTopic = '/robot_pose'
		self._sub = ProxySubscriberCached({self._objectTopic: PoseStamped})

		self._succeeded = False
コード例 #39
0
    def __init__(self):
        """
        Constructor
        """
        self._sm = None

        smach.set_loggers(
            rospy.logdebug, rospy.logwarn, rospy.logdebug, rospy.logerr  # hide SMACH transition log spamming
        )

        # 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)
コード例 #40
0
	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})
コード例 #41
0
    def __init__(self):
        '''
        Constructor
        '''
        self._sm = None
        
        # 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)
コード例 #42
0
class MirrorState(EventState):
    '''
    This state will display its possible outcomes as buttons in the GUI and is designed in a way to be created dynamically.
    '''


    def __init__(self, target_name, target_path, given_outcomes, outcome_autonomy):
        '''
        Constructor
        '''
        super(MirrorState, self).__init__(outcomes=given_outcomes)
        self._rate = rospy.Rate(100)
        self._given_outcomes = given_outcomes
        self._outcome_autonomy = outcome_autonomy
        self._target_name = target_name
        self._target_path = target_path
        
        self._outcome_topic = 'flexbe/mirror/outcome'

        self._pub = ProxyPublisher() #{'flexbe/behavior_update': String}
        self._sub = ProxySubscriberCached({self._outcome_topic: UInt8})
        
        
    def execute(self, userdata):
        '''
        Execute this state
        '''
        if JumpableStateMachine.refresh:
            JumpableStateMachine.refresh = False
            self.on_enter(userdata)

        if self._sub.has_buffered(self._outcome_topic):
            msg = self._sub.get_from_buffer(self._outcome_topic)
            if msg.data < len(self._given_outcomes):
                rospy.loginfo("State update: %s > %s", self._target_name, self._given_outcomes[msg.data])
                return self._given_outcomes[msg.data]

        try:
            self._rate.sleep()
        except ROSInterruptException:
            print 'Interrupted mirror sleep.'
    
    
    def on_enter(self, userdata):
        #rospy.loginfo("Mirror entering %s", self._target_path)
        self._pub.publish('flexbe/behavior_update', String("/" + "/".join(self._target_path.split("/")[1:])))
コード例 #43
0
	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)
コード例 #44
0
    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
コード例 #45
0
    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})
コード例 #46
0
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)
コード例 #47
0
	def __init__(self):
		# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
		super(MarkPoint, self).__init__(outcomes = ['succeeded'], output_keys = ['pose'])

		
		#self._topic = '/move_base/simple_goal'
		#self._pub = ProxyPublisher({self._topic: PoseStamped})

		self._objectTopic = '/robot_pose'
		self._sub = ProxySubscriberCached({self._objectTopic: PoseStamped})
コード例 #48
0
	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
コード例 #49
0
class DetectObjectNew(EventState):
	'''
	Observe the update of objects in the worldmodel.
	># type	   string		object_type to be found
	># state   string		state the object should be in

	<= found 			Object was found

	#> pose    PoseStamped		Pose of the object, which was detected
	#> object_id  string		object_id of detected object

	'''

	def __init__(self):
		
		super(DetectObjectNew, self).__init__(outcomes = ['found'], input_keys =['type', 'state', 'pose'], output_keys = ['pose', 'object_id'])

		self._objectTopic = '/worldmodel/object'
		self._sub = ProxySubscriberCached({self._objectTopic: Object})


	def execute(self, userdata):
	

		current_obj = self._sub.get_last_msg(self._objectTopic)
		if current_obj:
			if current_obj.info.class_id == userdata.type and current_obj.state.state == userdata.state: 
				userdata.pose = PoseStamped()
				userdata.pose.pose = current_obj.pose.pose
				userdata.pose.header.stamp = Time.now()
				userdata.pose.header.frame_id = 'map'
				userdata.object_id = current_obj.info.object_id
				Logger.loginfo('detected %(x)s' % {
					'x': current_obj.info.object_id
				})
				return 'found'
			
		

	def on_enter(self, userdata):

		pass

	def on_exit(self, userdata):
	
		pass 


	def on_start(self):
		
		pass

	def on_stop(self):
	
		pass 
コード例 #50
0
class DetectObject(EventState):
	'''
	Observe the update of objects (victims) in the worldmodel. When its state is 'active', we return it.

	># pose	   PoseStamped		Pose of object

	<= found 			Victim was found

	#> pose    PoseStamped		Pose of the object, which was detected
	#> victim  string		object_id of detected victim

	'''

	def __init__(self):
		
		super(DetectObject, self).__init__(outcomes = [ 'found'], input_keys =['pose'], output_keys = ['pose', 'victim'])

		self._objectTopic = '/worldmodel/object'
		self._sub = ProxySubscriberCached({self._objectTopic: Object})


	def execute(self, userdata):
	

		current_obj = self._sub.get_last_msg(self._objectTopic)
		if current_obj:
			if current_obj.info.class_id == 'victim' and current_obj.state.state == 2: 
				userdata.pose = PoseStamped()
				userdata.pose.pose = current_obj.pose.pose
				userdata.pose.header.stamp = Time.now()
				userdata.pose.header.frame_id = 'map'
				userdata.victim = current_obj.info.object_id
				Logger.loginfo('detected %(x)s' % {
					'x': current_obj.info.object_id
				})
				return 'found'
			
		

	def on_enter(self, userdata):

		pass

	def on_exit(self, userdata):
	
		pass 


	def on_start(self):
		
		pass

	def on_stop(self):
	
		pass 
コード例 #51
0
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()
コード例 #52
0
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'
コード例 #53
0
	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)
コード例 #54
0
    def __init__(self, *args, **kwargs):
        super(OperatableStateMachine, self).__init__(*args, **kwargs)
        self._message = None

        self.id = None
        self.autonomy = None

        self._autonomy = {}
        self._ordered_states = []
        
        self._pub = ProxyPublisher()

        self._sub = ProxySubscriberCached()
コード例 #55
0
    def __init__(self, *args, **kwargs):
        super(ManuallyTransitionableState, self).__init__(*args, **kwargs)
        
        self._force_transition = False
        
        self.__execute = self.execute
        self.execute = self._manually_transitionable_execute

        self._feedback_topic = '/flexbe/command_feedback'
        self._transition_topic = '/flexbe/command/transition'

        self._pub = ProxyPublisher()
        self._sub = ProxySubscriberCached()
コード例 #56
0
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)
コード例 #57
0
    def __init__(self):
        '''
        Constructor
        '''
        self.be = None
        Logger.initialize()

        #ProxyPublisher._simulate_delay = True
        #ProxySubscriberCached._simulate_delay = True

        # prepare temp folder
        rp = rospkg.RosPack()
        self._tmp_folder = os.path.join(rp.get_path('flexbe_onboard'), 'tmp/')
        if not os.path.exists(self._tmp_folder):
            os.makedirs(self._tmp_folder)
        sys.path.append(self._tmp_folder)

        # prepare manifest folder access
        manifest_folder = os.path.join(rp.get_path('flexbe_behaviors'), 'behaviors/')
        rospy.loginfo("Parsing available behaviors...")
        file_entries = [os.path.join(manifest_folder, filename) for filename in os.listdir(manifest_folder) if not filename.startswith('#')]
        manifests = sorted([xmlpath for xmlpath in file_entries if not os.path.isdir(xmlpath)])
        self._behavior_lib = dict()
        for i in range(len(manifests)):
            m = ET.parse(manifests[i]).getroot()
            e = m.find("executable")
            self._behavior_lib[i] = {"name": m.get("name"), "package": e.get("package_path").split(".")[0], "file": e.get("package_path").split(".")[1], "class": e.get("class")}
#            rospy.loginfo("+++ " + self._behavior_lib[i]["name"])

        self._pub = ProxyPublisher()
        self._sub = ProxySubscriberCached()

        self.status_topic = '/flexbe/status'
        self.feedback_topic = '/flexbe/command_feedback'

        self._pub.createPublisher(self.status_topic, BEStatus, _latch = True)
        self._pub.createPublisher(self.feedback_topic, CommandFeedback)

        # listen for new behavior to start
        self._running = False
        self._switching = False
        self._sub.subscribe('/flexbe/start_behavior', BehaviorSelection, self._behavior_callback)

        # heartbeat
        self._pub.createPublisher('/flexbe/heartbeat', Empty)
        self._execute_heartbeat()

        rospy.sleep(0.5) # wait for publishers etc to really be set up
        self._pub.publish(self.status_topic, BEStatus(code=BEStatus.READY))
        rospy.loginfo('\033[92m--- Behavior Engine ready! ---\033[0m')