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