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")
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
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
        ]
class GoForwardState(EventState):
    def __init__(self, speed, travel_dist, obstacle_dist):
        super(GoForwardState, self).__init__(outcomes=['failed', 'done'])
        self._start_time = None
        self.data = None
        self._speed = speed
        self._travel_dist = travel_dist
        self._obstacle_dist = obstacle_dist

        self.vel_topic = 'diff_velocity_controller/cmd_vel'
        self.scan_topic = '/scan'

        #create publisher passing it the vel_topic_name and msg_type
        self.pub = ProxyPublisher({self.vel_topic: Twist})
        #create subsciber
        self.scan_sub = ProxySubscriberCached({self.scan_topic: LaserScan})
        self.scan_sub.set_callback(self.scan_topic, self.scan_callback)

    def execute(self, userdata):
        if not self.cmd_pub:
            return 'failed'
        #run obstacle checks [index 0: Left, 360: middle, 719:right]
        if (self.data is not None):
            Logger.loginfo('FWD obstacle distance is: %s' %
                           self.data.ranges[360])
            if self.data.ranges[360] <= self._obstacle_dist:
                return 'failed'

            #measure distance traveled
            elapsed_time = (rospy.Time.now() - self._start_time).to_sec()
            distance_traveled = elapsed_time * self._speed

            if distance_traveled >= 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!')
        self._start_time = rospy.Time.now()
        #set robot speed here
        self.cmd_pub = Twist()
        self.cmd_pub.linear.x = self._speed
        self.cmd_pub.angular.z = 0.0

    def on_exit(self, userdata):
        self.cmd_pub.linear.x = 0.0
        self.pub.publish(self.vel_topic, self.cmd_pub)
        Logger.loginfo('Drive FWD ENDED!')

    def on_start(self):
        Logger.loginfo('Drive FWD READY!')

    def on_stop(self):
        Logger.loginfo('Drive FWD STOPPED!')

    def scan_callback(self, data):
        self.data = data
class GoFowardState(EventState):
    '''
    Driving state for a ground robot. This state allows the robot to drive forward a certain distance
    at a specified velocity/ speed.

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

    <= failed 			    If behavior is unable to ready on time
    <= done 				Example for a failure outcome.

	'''
    def __init__(self, speed, travel_dist, obstacle_dist, proportional_turning_constant, angle_diff_thresh):
        super(GoFowardState, self).__init__(outcomes=['failed', 'done'])
        self._start_time = None
        self.depth = None
        self._speed = speed
        self._travel_dist = travel_dist
        self._obstacle_dist = obstacle_dist
        self._proportional_turning_constant = proportional_turning_constant
        self._angle_diff_thresh = angle_diff_thresh

        self.vel_topic = '/cmd_vel'
	self.scan_topic = '/depth_data'

        #create publisher passing it the vel_topic_name and msg_type
        self.pub = ProxyPublisher({self.vel_topic: Twist_float})

        #create subscriber
        self.scan_sub = ProxySubscriberCached({self.scan_topic: Depth})
        self.scan_sub.set_callback(self.scan_topic, self.scan_callback)

    def execute(self, userdata):
        if not self.cmd_pub: # check if  message in self.cmd_pub to publish to /cmd_vel else we exit
            return 'failed'
        #run obstacle checks [index 0: left, 360: middle, 719: right]
        if(self.depth is not None):
            Logger.loginfo('FWD obstacle distance is: %s and dist thres is : %s'\
		 % (self.depth.center, self._obstacle_dist))
            if self.depth.center <= self._obstacle_dist:
                self.cmd_pub.vel = 0.0
                self.cmd_pub.angle = 0.0
                self.pub.publish(self.vel_topic, self.cmd_pub)
                return 'done'

            Logger.loginfo('45 deg distance left: %s, right: %s' % (self.depth.left,self.depth.right))
            #angle_diff = self.image.data[0] - self.image.data[2]

	    angle_diff = self.depth.left - self.depth.right

            if angle_diff >= self._angle_diff_thresh:
                Logger.loginfo('Reached angle diff threshold')
                # return 'failed'

            # Proportional controller
            self.cmd_pub.angle =  angle_diff * self._proportional_turning_constant
	    self.cmd_pub.angle = max(-0.3 , min(0.3 , self.cmd_pub.angle))
            Logger.loginfo('Turning Angle is: %s' % self.cmd_pub.angle)

            #measure distance travelled
            elapsed_time = (rospy.Time.now() - self._start_time).to_sec()
            distance_travelled = elapsed_time * self._speed

            if distance_travelled >= self._travel_dist:
                Logger.loginfo('Traveled over threshold')
                return 'failed'

        #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_float()
        self.cmd_pub.vel = self._speed
        self.cmd_pub.angle = 0.0

    def on_exit(self, userdata):
        self.cmd_pub.vel = 0.0
        self.cmd_pub.angle = 0.0
        self.pub.publish(self.vel_topic, self.cmd_pub)
        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!")

    def scan_callback(self, data):
        self.depth = data
Beispiel #6
0
class GoBlindState(EventState):
    '''
	Driving state for a ground robot. This state allows the robot to drive forward a certain time
        at a specified velocity/ speed.

	-- speed 	float 	Speed at which to drive the robot
        -- time         float   Time at which to drive the robot

	<= failed 			    If behavior is unable to ready on time
	<= done 				Example for a failure outcome.

	'''
    def __init__(self, speed, time):
        super(GoBlindState, self).__init__(outcomes=['failed', 'done'])
        self._start_time = None
        self.data = None
        self._speed = speed
        self._time = time

        self.vel_topic = '/cmd_vel'
        self.scan_topic = '/scan'

        #create publisher passing it the vel_topic_name and msg_type
        self.pub = ProxyPublisher({self.vel_topic: Twist_float})
        #create subscriber
        self.scan_sub = ProxySubscriberCached({self.scan_topic: LaserScan})
        self.scan_sub.set_callback(self.scan_topic, self.scan_callback)

    def execute(self, userdata):
        if not self.cmd_pub:  # check if  message in self.cmd_pub to publish to /cmd_vel else we exit
            return 'failed'

        #measure distance travelled
        elapsed_time = (rospy.Time.now() - self._start_time).to_sec()

        if elapsed_time >= self._time:
            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_float()
        self.cmd_pub.vel = self._speed
        self.cmd_pub.angle = 0.0
        self._start_time = rospy.Time.now()

    def on_exit(self, userdata):
        self.cmd_pub.vel = 0.0
        self.cmd_pub.angle = 0.0
        self.pub.publish(self.vel_topic, self.cmd_pub)
        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!")

    def scan_callback(self, data):
        self.data = data
Beispiel #7
0
class TurnState(EventState):
    '''
	Turn state for a ground robot. This state allows the robot to turn at a certain angle
    at a specified velocity/ speed.

        -- t_speed 	float 	Speed at which to turn the robot
        -- turn_angle   float   The angle that the robot should make
        -- forward_dist float   free distance in front of robot

	<= failed 			    If behavior is unable to succeed on time
	<= done 				forward distance becomes sufficantly large

	'''
    def __init__(self, t_speed, turn_angle, forward_dist, timeout):
        super(TurnState, self).__init__(outcomes=['failed', 'done'])
        self._t_speed = t_speed
        self._turn_angle = turn_angle
        self._forward_dist = forward_dist
        self._timeout = timeout

        self._start_time = None
        self.depth = None

        self.vel_topic = '/cmd_vel'
        self.scan_topic = '/depth_data'

        #create publisher passing it the vel_topic_name and msg_type
        self.pub = ProxyPublisher({self.vel_topic: Twist_float})
        #create subscriber
        self.scan_sub = ProxySubscriberCached({self.scan_topic: Depth})
        self.scan_sub.set_callback(self.scan_topic, self.scan_callback)

    def execute(self, userdata):
        if not self.cmd_pub:  # check if  message in self.cmd_pub to publish to /cmd_vel else we exit
            Logger.loginfo('messesage does not exist')
            return 'failed'
        #run obstacle checks [index 0: left, 360: middle, 719: right]
        if (self.depth is not None):
            Logger.loginfo('FWD free distance is: %s' % self.depth.center)
            Logger.loginfo('Turn angle is : %s' % self._turn_angle)
            if self.depth.center >= self._forward_dist:
                return 'done'

            #measure distance travelled
            elapsed_time = (rospy.Time.now() - self._start_time).to_sec()

            if elapsed_time >= self._timeout:
                Logger.loginfo('Reached timeout')
                return 'failed'

        #drive
        self.pub.publish(self.vel_topic, self.cmd_pub)

    def on_enter(self, userdata):
        Logger.loginfo("Turn RIGHT STARTED!")
        #set robot speed here
        self.cmd_pub = Twist_float()
        self.cmd_pub.vel = self._t_speed
        self.cmd_pub.angle = self._turn_angle
        self._start_time = rospy.Time.now()

    def on_exit(self, userdata):
        self.cmd_pub.vel = 0.0
        self.cmd_pub.angle = 0.0
        self.pub.publish(self.vel_topic, self.cmd_pub)
        Logger.loginfo("Turn RIGHT 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("Turn RIGHT STOPPED!")

    def scan_callback(self, data):
        self.depth = data