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