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