def __init__(self, confidence_thres, maxSpeedAtDist, maxSpeedAtAngle,
minDriveSpeed, minTurningSpeed):
ControlState.__init__(self, maxSpeedAtDist, maxSpeedAtAngle,
minDriveSpeed, minTurningSpeed)
SearchState.__init__(confidence_thres)
self.goalTracker = goalTracker
self.ball_pose = None
def __init__(self, maxSpeedAtDist, maxSpeedAtAngle, minDriveSpeed,
minTurningSpeed):
ControlState.__init__(self, maxSpeedAtDist, maxSpeedAtAngle,
minDriveSpeed, minTurningSpeed)
self.received_map = False
self.parent = None
np.set_printoptions(linewidth=100)
def attach(self):
ControlState.attach(self)
self.goalPose = None
self.path = None
self.currentPose = None
self.goalReached = True
self.receivedPath = False
self.path_sub = rospy.Subscriber("trajectory", Path, self.setPath)
self.odom_sub = rospy.Subscriber("/fusion/local_fusion/filtered",
Odometry, self.update)
self.pub = rospy.Publisher("/motor_ctl", MotorCMD, queue_size=10)
def __init__(self, maxSpeedAtDist, maxSpeedAtAngle, minDriveSpeed,
minTurningSpeed):
ControlState.__init__(self, maxSpeedAtDist, maxSpeedAtAngle,
minDriveSpeed, minTurningSpeed)
self.state = "idle" # "aiming" "moving" "finetuning"
self.goalPose = None
self.path = None
self.parent = None
self.currentPose = None
self.goalReached = True
self.receivedPath = False
def attach(self):
self.parent.resetModes()
self.parent.blobSearch = True
ControlState.attach(self)
#self.odom_sub = rospy.Subscriber("/fusion/local_fusion/filtered", Odometry, self.update)
self.odom_sub = rospy.Subscriber("/initialpose",
PoseWithCovarianceStamped,
self.update)
self.map_sub = rospy.Subscriber("/occupancy_grid", OccupancyGrid,
self.mapCallback)
self.path_pub = rospy.Publisher("/path", Path, queue_size=2)
self.goal_pub = rospy.Publisher("/move_base_simple/goal",
Path,
queue_size=2)
def __init__(self, confidence_thres, maxSpeedAtDist, maxSpeedAtAngle,
minDriveSpeed, minTurningSpeed):
ControlState.__init__(self, maxSpeedAtDist, maxSpeedAtAngle,
minDriveSpeed, minTurningSpeed)
SearchState.__init__(self, confidence_thres)
self.odom_sub = None
def detach(self):
ControlState.detach(self)
SearchState.detach(self)
self.odom_sub.unregister()
def attach(self):
ControlState.attach(self)
SearchState.attach(self)
self.startAngle = None
self.odom_sub = rospy.Subscriber("/fusion/local_fusion/filtered",
Odometry, self.update)
from ControlState import ControlState cs = ControlState() cs.initTimer(10000) cs.updateTimer() cs.showState()
from ControlState import ControlState
cs = ControlState()
cs.executePrograma("FRT;FRT;ESQ;ESQ;FRT;FRT")
cs.updateExecution()
cs.showState()
cs.initTimer(10000)
cs.updateTimer()
class ControlStateMachine(StateMachine):
# Define states
state_unconfigured = ControlState(b'UNCONFIGURED')
state_configured = ControlState(b'CONFIGURED')
state_running = ControlState(b'RUNNING')
state_enabled = ControlState(b'ENABLED')
def __init__(self, pv_base):
# initialize PVs
self.pvMsgEnable = PV(pv_base + ':MsgEnable', initialize=True)
logging.debug("Create PV: %s" % self.pvMsgEnable.name)
self.pvMsgDisable = PV(pv_base + ':MsgDisable', initialize=True)
logging.debug("Create PV: %s" % self.pvMsgDisable.name)
self.pvMsgConfig = PV(pv_base + ':MsgConfig', initialize=True)
logging.debug("Create PV: %s" % self.pvMsgConfig.name)
self.pvMsgUnconfig = PV(pv_base + ':MsgUnconfig', initialize=True)
logging.debug("Create PV: %s" % self.pvMsgUnconfig.name)
self.pvRun = PV(pv_base + ':Run', initialize=True)
logging.debug("Create PV: %s" % self.pvRun.name)
# register callbacks for each valid state+transition combination
unconfigured_dict = {
Transition.configure: (self.configfunc, self.state_configured)
}
configured_dict = {
Transition.unconfigure:
(self.unconfigfunc, self.state_unconfigured),
Transition.beginrun: (self.beginrunfunc, self.state_running)
}
running_dict = {
Transition.endrun: (self.endrunfunc, self.state_configured),
Transition.enable: (self.enablefunc, self.state_enabled)
}
enabled_dict = {
Transition.disable: (self.disablefunc, self.state_running)
}
self.state_unconfigured.register(unconfigured_dict)
self.state_configured.register(configured_dict)
self.state_running.register(running_dict)
self.state_enabled.register(enabled_dict)
# Start with a default state.
self._state = self.state_unconfigured
@staticmethod
def pv_put(pv, val):
retval = False
if not pv.isinitialized:
logging.error("PV not initialized: %s" % pv.name)
elif not pv.isconnected:
logging.error("PV not connected: %s" % pv.name)
else:
try:
pv.put(val)
except pyca.pyexc:
logging.error("PV put(%d) timeout: %s" % (val, pv.name))
else:
retval = True
logging.debug("PV put(%d): %s" % (val, pv.name))
return retval
def configfunc(self):
logging.debug("configfunc()")
# PV MsgConfig=0
# PV MsgConfig=1
# PV MsgConfig=0
# PV Run=0
return (self.pv_put(self.pvMsgConfig, 0)
and self.pv_put(self.pvMsgConfig, 1)
and self.pv_put(self.pvMsgConfig, 0)
and self.pv_put(self.pvRun, 0))
def unconfigfunc(self):
logging.debug("unconfigfunc()")
# PV MsgUnconfig=0
# PV MsgUnconfig=1
# PV MsgUnconfig=0
# PV Run=0
return (self.pv_put(self.pvMsgUnconfig, 0)
and self.pv_put(self.pvMsgUnconfig, 1)
and self.pv_put(self.pvMsgUnconfig, 0)
and self.pv_put(self.pvRun, 0))
def beginrunfunc(self):
logging.debug("beginrunfunc()")
# PV Run=1
return self.pv_put(self.pvRun, 1)
def endrunfunc(self):
logging.debug("endrunfunc()")
# PV Run=0
return self.pv_put(self.pvRun, 0)
def enablefunc(self):
logging.debug("enablefunc()")
# PV MsgEnable=0
# PV MsgEnable=1
# PV MsgEnable=0
# PV Run=1
return (self.pv_put(self.pvMsgEnable, 0)
and self.pv_put(self.pvMsgEnable, 1)
and self.pv_put(self.pvMsgEnable, 0)
and self.pv_put(self.pvRun, 1))
def disablefunc(self):
logging.debug("disablefunc()")
# PV MsgDisable=0
# PV MsgDisable=1
# PV MsgDisable=0
# PV Run=1
return (self.pv_put(self.pvMsgDisable, 0)
and self.pv_put(self.pvMsgDisable, 1)
and self.pv_put(self.pvMsgDisable, 0)
and self.pv_put(self.pvRun, 1))
def __init__(self, maxSpeedAtDist, maxSpeedAtAngle, minDriveSpeed, minTurningSpeed):
ControlState.__init__(self, maxSpeedAtDist,
maxSpeedAtAngle, minDriveSpeed, minTurningSpeed)
def detach(self):
ControlState.detach(self)
self.odom_sub.unregister()
self.map_sub.unregister()
self.received_map = False
import time
from ControlState import ControlState
cs = ControlState()
counter = 0
message_interval = 1
last_message = 0
cs.executePrograma("FRT;FRT;ESQ;ESQ;FRT;FRT")
while counter < 20:
try:
if (time.time() - last_message) > message_interval:
counter += 1
print(counter)
cs.updateExecution()
cs.showState()
last_message = time.time()
except OSError as e:
print(e)
def attach(self):
ControlState.attach(self)
SearchState.attach(self)
self.odom_sub = rospy.Subscriber("/fusion/local_fusion/filtered",
Odometry, self.odomCallback)
def detach(self):
ControlState.detach(self)
self.path_sub.unregister()
self.odom_sub.unregister()