def main():
game = Game()
while True:
try:
game.run(input().replace("\r", "").replace("\n", "").split(" "))
except Exception as error:
Debugger.error(str(error))
def __init__(self, options):
Subject.__init__(self)
self.log = logging.getLogger("mallorymain")
config.logsetup(self.log)
self.configured_protos = []
self.configured_plugin_managers = []
self.protoinstances = []
self.opts = options.options
self.dbname = self.opts.trafficdb
self.debugon = False
self.debugger = Debugger()
self.config_protocols = config_proto.ConfigProtocols()
self.config_rules = config_rule.ConfigRules()
self.rpcserver = rpc.RPCServer()
self.nftool = netfilter.NetfilterTool()
def __init__(self, forx, fory, Ka, da, Kb, db, delta_t, distance):
self.vision = Vision()
self.debugger = Debugger()
time.sleep(0.5)
#print(self.vision.yellow_robot[0].x)
self.Refresh_robo()
self.ox = forx
self.oy = fory
self.Ka = Ka
self.da = da
self.Kb = Kb
self.db = db
self.delta_t = delta_t
self.distance = distance
self.final_list = [[self.px, self.py]]
self.ax = 1
self.ay = 1
def debug(file: str, isAsm=False, memoryFile=None):
b = BUS()
if memoryFile is not None:
readToMemory(b.cu, memoryFile)
symtab = None
datatab = None
if file is not None:
if isAsm:
obj, symtab, datatab = assemble(file, asClass=True)
load(b.cu, None, objectCode=obj)
else:
load(b.cu, file)
d = Debugger(b.cu, symtab, datatab=datatab)
n = ''
while n.strip() != 'exit':
try:
n = input(':>')
if n.strip() != 'exit':
c = d.evaluate(n)
c.execute()
except BaseException as e:
print(e)
def __init__(self, options):
Subject.__init__(self)
self.log = logging.getLogger("mallorymain")
config.logsetup(self.log)
self.configured_protos = []
self.configured_plugin_managers = []
self.protoinstances = []
self.opts = options.options
self.dbname = self.opts.trafficdb
self.debugon = False
self.debugger = Debugger()
self.config_protocols = config_proto.ConfigProtocols()
self.config_rules = config_rule.ConfigRules()
self.rpcserver = rpc.RPCServer()
self.nftool = netfilter.NetfilterTool()
from APF_MOVE import APF
from action import Action
from vision import Vision
from debug import Debugger
import time
if __name__ == '__main__':
x=[-2400,2400,-2400,2400,-2400,2400,-2400,2400,-2400,2400]
y=[-1500,1500,-1500,1500,-1500,1500,-1500,1500,-1500,1500]
vision = Vision()
debugger = Debugger()
action = Action()
for i in range(10):
APF_Plan = APF(x[i], y[i], Ka=17, Kb=2 * 10 ** 12, da=400, db=800, delta_t=10, distance=200, dy=150, ## TODO db=725 Kb = 2*10**12 Ka = 15
Kt=10 * 10 ** 11,
distance_2=300, distance_3=100, cv=420,debugger=debugger,vision=vision,action=action) ## TODO distance3 = 400
max_num = 10000
for i in range(max_num):
APF_Plan.Control(1) # 得到风向标位置
if APF_Plan.checkDestination():
# APF_Plan.final_list.append([forx, fory])
print("此次避障成功")
APF_Plan.Draw()
break
else:
APF_Plan.Draw()
print("超过最大迭代次数！")
time.sleep(1)
del APF_Plan
class Mallory(Subject):
"""
The main Mallory class used to instantiate and start the proxy. Protocols
can be configured through methods in the main Mallory class. This is
where it all starts.
"""
def __init__(self, options):
Subject.__init__(self)
self.log = logging.getLogger("mallorymain")
config.logsetup(self.log)
self.configured_protos = []
self.configured_plugin_managers = []
self.protoinstances = []
self.opts = options.options
self.dbname = self.opts.trafficdb
self.debugon = False
self.debugger = Debugger()
self.config_protocols = config_proto.ConfigProtocols()
self.config_rules = config_rule.ConfigRules()
self.rpcserver = rpc.RPCServer()
self.nftool = netfilter.NetfilterTool()
def configure_protocol(self, protocol, action):
"""
Configure a protocol. Use this method to configure Mallory protocols.
@param protocol: The Mallory protocol, from the protocol module, to
be configured.
@param action: when the value is "add" the protocol will be added to the
protocol classes Mallory uses to decode protocols
@type action: string
@return: No return value
"""
if action == "add":
self.configured_protos.append(protocol)
def configure_protocols(self):
self.log.debug("Mallory:configure_protocols - Entry")
protocols = self.config_protocols.get_protocols()
self.configured_protos = []
for protocol in protocols:
self.configure_protocol(protocol, "add")
def add_plugin_manager(self, pluginManager):
"""
Add a new plugin manager to Mallory's configured plugin managers.
@param plugin_manager: The plugin manager to be added.
@type plugin_manager:
plugin_managers.plugin_managers_base.PluginManagerBase.
"""
self.configured_plugin_managers.append (pluginManager)
def configure_socket(self, mevt, **kwargs):
"""
Private method to configure a socket.
@param mevt: This is a mallory event from module malloryevt
@param kwargs: keyworded arguments. Currently expects one named
argument, protoinst. The protoinst must be a protocol instance.
@type kwargs: protoinst=L{Protocol<src.protocol.base.Protocol>}
"""
protoinst = kwargs["protoinst"]
if not protoinst:
return
for proto in self.configured_protos:
if mevt in proto.supports:
if proto.serverPort == protoinst.serverPort:
if mevt == malloryevt.CSACCEPT or mevt == malloryevt.CSAFTERSS:
protoinst.configure_client_socket()
elif mevt == malloryevt.SSCREATE:
protoinst.configure_server_socket()
def forward(self, protoinst, conndata):
"""
Internal method for setting up data pumps for sockets.
@param protoinst: A protocol instance to set up.
@type protoinst: L{Protocol <src.protocol.base.Protocol>}
"""
if malloryevt.STARTS2C in protoinst.supports and conndata.direction == "s2c":
protoinst.forward_s2c(conndata)
elif malloryevt.STARTC2S in protoinst.supports and conndata.direction == "c2s":
protoinst.forward_c2s(conndata)
else:
protoinst.forward_any(conndata)
def update(self, publisher, **kwargs):
if "action" not in kwargs:
return
if kwargs["action"] == "load_protos":
self.configure_protocols()
def cleanup_proto(self):
#self.log.info("Mallory.cleanup_proto()")
# List of proto instances that are done
done_list = []
for proto in self.protoinstances:
if proto.is_done():
done_list.append(proto)
#self.log.info("Mallory.cleanup_proto():%s" % (done_list))
for proto in done_list:
"""
TODO: Figure out why we still leak socket handles
Current behavior is to slowly leak a few handles per minute
under heavy load. Especially when the remote server
disappears.
This fixes up the majority of the issues for now
"""
proto.close()
self.protoinstances.remove(proto)
def main(self):
"""
Mallory's main method. When this is called the following activities
occur.
- A new traffic database is created
- A new thread for debugger RPC is created
- a listening socket for the proxy is created
- A UDP socket is created
- A new thread for processing the DB Queue is created
- The proxy begins listening for incoming connections
"""
dbConn = TrafficDb(self.dbname)
# Kick off a thread for the debugger
#thread.start_new_thread(self.debugger.rpcserver, ())
# Mallory needs to know if the protocol config changes
self.config_protocols.attach(self)
self.rpcserver.add_remote_obj(self.debugger, "debugger")
self.rpcserver.add_remote_obj(self.config_protocols, "config_proto")
self.rpcserver.add_remote_obj(self.config_rules, "config_rules")
self.configure_protocols()
thread.start_new_thread(self.rpcserver.start_server, ())
try:
# Create proxy and wait for connections
proxy = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
proxy.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
udpproxy = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
if self.opts.listen:
bindaddr = ('', int(self.opts.listen))
self.log.info("Binding mallory to: %s:%d" \
% (bindaddr[0], bindaddr[1]))
proxy.bind(bindaddr)
udpproxy.bind(bindaddr)
proxy.listen(5)
if config.debug == 1:
self.log.debug("main: Waiting for connection")
connCount = 0
# This thread puts data in the database.
thread.start_new_thread(dbConn.fillDB, ())
try:
# Not a standard part of mallory. For ssh ownage only.
sshshellpwn = ssh.SSHProtocol(None, None, None)
thread.start_new_thread(sshshellpwn.provideshell, (self, ))
except NameError:
self.log.warn("main: SSHProtocol not defined. sshshellpwn "
"unavailable")
# Handle UDP
udp = base.UdpProtocol(dbConn, udpproxy, self.configured_protos)
udp.setrules(self.config_rules.get_rules())
self.config_rules.attach(udp)
# Setup bi-directional pub/sub setup for debugger events. This
# is a very important step. Without it the debugging client and or
# mallory will not know when an event has occured
#self.debugger.attach(udp)
#udp.attach(self.debugger)
thread.start_new_thread(udp.forward_any, ())
# Handle TCP
while 1:
# Clean up first
self.cleanup_proto()
# Main accept
(csock, caddr) = proxy.accept()
if config.debug == 1:
self.log.info("main: got connection from: %s:%s" %
(caddr[0], caddr[1]))
try:
# TODO: Move this option into the netfilter class
# destination lookup methods
if self.opts.notransparent:
shost,sport = self.opts.notransparent.split(":")
sport = int(sport)
self.log.debug("Sending to:%s:%d" % (shost, sport))
else:
(shost,sport) = self.nftool.getrealdest(csock)
except:
traceback.print_exc()
self.log.warn("main: error getting real destination")
try:
# Create protocol instance using server port as guide
protoinst = None
for proto in self.configured_protos:
if sport == proto.serverPort:
protoinst = proto.__class__(dbConn, csock,
None)
if not protoinst:
protoinst = base.TcpProtocol(dbConn, csock, None)
self.log.debug("Mallory.main: created a %s class" % (protoinst.__class__))
protoinst.setrules(self.config_rules.get_rules())
# Set the proper debugging flag.
protoinst.debugon = self.debugger.debugon
## You always *pass* the object interested in updates
## see observer.py
# Protocols are interested in updates from mallory
self.attach(protoinst)
# Protocols are interested in updates from rule_config
self.config_rules.attach(protoinst)
# Protocols are interested in updates from the debugger
self.debugger.attach(protoinst)
# The debugger is interested in events from protos as well
protoinst.attach(self.debugger)
# Which Protocol manager to which protocol
for plugin_manager in self.configured_plugin_managers:
if sport == plugin_manager.server_port:
protoinst.attach_plugin_manager(plugin_manager)
# Subscribe updates between mallory and the proto instance
protoinst.attach(self)
self.protoinstances.append(protoinst)
# Create the server socket (victim's destination)
ssock = socket.socket(socket.AF_INET, \
socket.SOCK_STREAM)
# Set server sock data in protocol instance then config it
protoinst.destination = ssock
protoinst.serverPort = sport
self.configure_socket(malloryevt.SSCREATE,
protoinst=protoinst)
if self.opts.proxify:
shost,sport = self.opts.proxify.split(":")
sport = int(sport)
# Connect the server socket
protoinst.destination.connect((shost, int(sport)))
# Client socket configuration after server socket creation
protoinst.source = csock
# buf = ""
# hack = csock.recv(1024, socket.MSG_PEEK)
# print "Hack Bits: " + repr(hack)
self.configure_socket(malloryevt.CSAFTERSS,
protoinst=protoinst)
except KeyboardInterrupt:
self.log.warn("mallory: got keyboard interrupt in" \
" conn attempt")
sys.exit(0)
except:
# Force clean up soon
protoinst.set_done(True)
# Deal with the freaky folks
self.log.error("main: error connecting to remote")
traceback.print_exc()
print sys.exc_info()
continue
# Retrieve the connection data.
clientconn = ConnData({'clientip' : caddr[0], \
'clientport' : caddr[1], \
'serverip' : shost, 'serverport' : sport, \
'conncount' : connCount, 'direction' : 'c2s' })
# Store the connection data
dbConn.qConn.put((connCount, shost, sport, \
caddr[0], caddr[1]))
# Kick off the s2c and c2s data pumps
thread.start_new_thread(self.forward, (protoinst, clientconn))
serverconn = copy.deepcopy(clientconn)
serverconn.direction = 's2c'
thread.start_new_thread(self.forward, (protoinst, serverconn))
connCount = connCount + 1
except KeyboardInterrupt:
self.log.info("Mallory: Goodbye.")
proxy.close()
sys.exit(0)
except:
self.log.error("Mallory: Blanket exception handler got an " \
"exception")
traceback.print_exc()
print sys.exc_info()
proxy.close()
from vision import Vision
from action import Action
from debug import Debugger
import time
if __name__ == '__main__':
vision = Vision()
action = Action()
debugger = Debugger()
my_command1 = {"id": 0, "vx": 300, "vy": 0, "vw": 0}
my_command2 = {"id": 1, "vx": 0, "vy": 0, "vw": 1.25}
while True:
# Do something(path planning)
# action.sendCommand(id = my_command1["id"],vx=my_command1["vx"], vy=my_command1['vy'], vw=my_command1["vw"])
#debugger.draw_line(vision.robots_yellow[0].x, vision.robots_yellow[0].y, vision.my_ball.x, vision.my_ball.y)
for val in vision.robots_blue.keys():
#print(vision.robots_blue[val].y)
if val == 0:
#action.sendCommand(id = my_command1["id"],vx=my_command1["vx"], vy=my_command1['vy'], vw=my_command1["vw"])
#time.sleep(1)
#print(vision.robots_yellow.keys())
#debugger.robot_msg(vision.robots_blue[val].x, vision.robots_blue[val].y)
debugger.draw_circle(vision.robots_blue[val].x,
vision.robots_blue[val].y)
#print(val)
pass
if val == 1:
#action.sendCommand(id = my_command2["id"],vx=my_command1["vx"], vy=my_command2['vy'], vw=my_command2["vw"])
debugger.draw_line(vision.robots_blue[val].x,
class Mallory(Subject):
"""
The main Mallory class used to instantiate and start the proxy. Protocols
can be configured through methods in the main Mallory class. This is
where it all starts.
"""
def __init__(self, options):
Subject.__init__(self)
self.log = logging.getLogger("mallorymain")
config.logsetup(self.log)
self.configured_protos = []
self.configured_plugin_managers = []
self.protoinstances = []
self.opts = options.options
self.dbname = self.opts.trafficdb
self.debugon = False
self.debugger = Debugger()
self.config_protocols = config_proto.ConfigProtocols()
self.config_rules = config_rule.ConfigRules()
self.rpcserver = rpc.RPCServer()
self.nftool = netfilter.NetfilterTool()
def configure_protocol(self, protocol, action):
"""
Configure a protocol. Use this method to configure Mallory protocols.
@param protocol: The Mallory protocol, from the protocol module, to
be configured.
@param action: when the value is "add" the protocol will be added to the
protocol classes Mallory uses to decode protocols
@type action: string
@return: No return value
"""
if action == "add":
self.configured_protos.append(protocol)
def configure_protocols(self):
self.log.debug("Mallory:configure_protocols - Entry")
protocols = self.config_protocols.get_protocols()
self.configured_protos = []
for protocol in protocols:
self.configure_protocol(protocol, "add")
def add_plugin_manager(self, pluginManager):
"""
Add a new plugin manager to Mallory's configured plugin managers.
@param plugin_manager: The plugin manager to be added.
@type plugin_manager:
plugin_managers.plugin_managers_base.PluginManagerBase.
"""
self.configured_plugin_managers.append(pluginManager)
def configure_socket(self, mevt, **kwargs):
"""
Private method to configure a socket.
@param mevt: This is a mallory event from module malloryevt
@param kwargs: keyworded arguments. Currently expects one named
argument, protoinst. The protoinst must be a protocol instance.
@type kwargs: protoinst=L{Protocol<src.protocol.base.Protocol>}
"""
protoinst = kwargs["protoinst"]
if not protoinst:
return
for proto in self.configured_protos:
if mevt in proto.supports:
if proto.serverPort == protoinst.serverPort:
if mevt == malloryevt.CSACCEPT or mevt == malloryevt.CSAFTERSS:
protoinst.configure_client_socket()
elif mevt == malloryevt.SSCREATE:
protoinst.configure_server_socket()
def forward(self, protoinst, conndata):
"""
Internal method for setting up data pumps for sockets.
@param protoinst: A protocol instance to set up.
@type protoinst: L{Protocol <src.protocol.base.Protocol>}
"""
if malloryevt.STARTS2C in protoinst.supports and conndata.direction == "s2c":
protoinst.forward_s2c(conndata)
elif malloryevt.STARTC2S in protoinst.supports and conndata.direction == "c2s":
protoinst.forward_c2s(conndata)
else:
protoinst.forward_any(conndata)
def update(self, publisher, **kwargs):
if "action" not in kwargs:
return
if kwargs["action"] == "load_protos":
self.configure_protocols()
def cleanup_proto(self):
#self.log.info("Mallory.cleanup_proto()")
# List of proto instances that are done
done_list = []
for proto in self.protoinstances:
if proto.is_done():
done_list.append(proto)
#self.log.info("Mallory.cleanup_proto():%s" % (done_list))
for proto in done_list:
"""
TODO: Figure out why we still leak socket handles
Current behavior is to slowly leak a few handles per minute
under heavy load. Especially when the remote server
disappears.
This fixes up the majority of the issues for now
"""
proto.close()
self.protoinstances.remove(proto)
def main(self):
"""
Mallory's main method. When this is called the following activities
occur.
- A new traffic database is created
- A new thread for debugger RPC is created
- a listening socket for the proxy is created
- A UDP socket is created
- A new thread for processing the DB Queue is created
- The proxy begins listening for incoming connections
"""
dbConn = TrafficDb(self.dbname)
self.dbname = dbConn.getDbName() #get the trafficDb being used
self.debugger.setdatabase(self.dbname)
# Kick off a thread for the debugger
#thread.start_new_thread(self.debugger.rpcserver, ())
# Mallory needs to know if the protocol config changes
self.config_protocols.attach(self)
self.rpcserver.add_remote_obj(self.debugger, "debugger")
self.rpcserver.add_remote_obj(self.config_protocols, "config_proto")
self.rpcserver.add_remote_obj(self.config_rules, "config_rules")
self.configure_protocols()
thread.start_new_thread(self.rpcserver.start_server, ())
try:
# Create proxy and wait for connections
proxy = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
proxy.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
udpproxy = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
if self.opts.listen:
bindaddr = ('', int(self.opts.listen))
self.log.info("Binding mallory to: %s:%d" \
% (bindaddr[0], bindaddr[1]))
proxy.bind(bindaddr)
udpproxy.bind(bindaddr)
proxy.listen(5)
if config.debug == 1:
self.log.debug("main: Waiting for connection")
connCount = 0
# This thread puts data in the database.
thread.start_new_thread(dbConn.fillDB, ())
# What is this?
try:
# Not a standard part of mallory. For ssh ownage only.
sshshellpwn = ssh.SSHProtocol(None, None, None)
thread.start_new_thread(sshshellpwn.provideshell, (self, ))
except NameError:
self.log.warn("main: SSHProtocol not defined. sshshellpwn "
"unavailable")
# Handle UDP
udp = base.UdpProtocol(dbConn, udpproxy, self.configured_protos)
udp.setrules(self.config_rules.get_rules())
self.config_rules.attach(udp)
# Setup bi-directional pub/sub setup for debugger events. This
# is a very important step. Without it the debugging client and or
# mallory will not know when an event has occured
#self.debugger.attach(udp)
#udp.attach(self.debugger)
thread.start_new_thread(udp.forward_any, ())
# Handle TCP
while 1:
# Clean up first
self.cleanup_proto()
# Main accept
(csock, caddr) = proxy.accept()
if config.debug == 1:
self.log.info("main: got connection from: %s:%s" %
(caddr[0], caddr[1]))
try:
# TODO: Move this option into the netfilter class
# destination lookup methods
if self.opts.notransparent:
shost, sport = self.opts.notransparent.split(":")
sport = int(sport)
self.log.debug("Sending to:%s:%d" % (shost, sport))
else:
(shost, sport) = self.nftool.getrealdest(csock)
except:
traceback.print_exc()
self.log.warn("main: error getting real destination")
try:
# Create protocol instance using server port as guide
protoinst = None
for proto in self.configured_protos:
if sport == proto.serverPort:
protoinst = proto.__class__(dbConn, csock, None)
if not protoinst:
protoinst = base.TcpProtocol(dbConn, csock, None)
self.log.debug("Mallory.main: created a %s class" %
(protoinst.__class__))
protoinst.setrules(self.config_rules.get_rules())
# Set the proper debugging flag.
protoinst.debugon = self.debugger.debugon
## You always *pass* the object interested in updates
## see observer.py
# Protocols are interested in updates from mallory
self.attach(protoinst)
# Protocols are interested in updates from rule_config
self.config_rules.attach(protoinst)
# Protocols are interested in updates from the debugger
self.debugger.attach(protoinst)
# The debugger is interested in events from protos as well
protoinst.attach(self.debugger)
# Which Protocol manager to which protocol
for plugin_manager in self.configured_plugin_managers:
if sport == plugin_manager.server_port:
protoinst.attach_plugin_manager(plugin_manager)
# Subscribe updates between mallory and the proto instance
protoinst.attach(self)
self.protoinstances.append(protoinst)
# Create the server socket (victim's destination)
ssock = socket.socket(socket.AF_INET, \
socket.SOCK_STREAM)
# Set server sock data in protocol instance then config it
protoinst.destination = ssock
protoinst.serverPort = sport
self.configure_socket(malloryevt.SSCREATE,
protoinst=protoinst)
if self.opts.proxify:
shost, sport = self.opts.proxify.split(":")
sport = int(sport)
# Connect the server socket
protoinst.destination.connect((shost, int(sport)))
# Client socket configuration after server socket creation
protoinst.source = csock
# buf = ""
# hack = csock.recv(1024, socket.MSG_PEEK)
# print "Hack Bits: " + repr(hack)
self.configure_socket(malloryevt.CSAFTERSS,
protoinst=protoinst)
except KeyboardInterrupt:
self.log.warn("mallory: got keyboard interrupt in" \
" conn attempt")
sys.exit(0)
except:
# Force clean up soon
protoinst.set_done(True)
# Deal with the freaky folks
self.log.error("main: error connecting to remote")
traceback.print_exc()
print sys.exc_info()
continue
# Retrieve the connection data.
clientconn = ConnData({'clientip' : caddr[0], \
'clientport' : caddr[1], \
'serverip' : shost, 'serverport' : sport, \
'conncount' : connCount, 'direction' : 'c2s' })
# Store the connection data
dbConn.qConn.put((connCount, shost, sport, \
caddr[0], caddr[1]))
# Kick off the s2c and c2s data pumps
thread.start_new_thread(self.forward, (protoinst, clientconn))
serverconn = copy.deepcopy(clientconn)
serverconn.direction = 's2c'
thread.start_new_thread(self.forward, (protoinst, serverconn))
connCount = connCount + 1
except KeyboardInterrupt:
self.log.info("Mallory: Goodbye.")
proxy.close()
sys.exit(0)
except:
self.log.error("Mallory: Blanket exception handler got an " \
"exception")
traceback.print_exc()
print sys.exc_info()
proxy.close()
class APF():
# setx,y:start point's x,y
# forx,y:end point's x,y
# Ka:attraction's contant
# da:attraction's Threshold
# Kb:rejection's contant
# db:rejection's Threshold
# This Function is used to get new information from map
def Refresh_robo(self): # 重新读取小车实时位置
self.px = self.vision.blue_robot[0].x
self.py = self.vision.blue_robot[0].y
self.vx = 0
self.vy = 0
self.orientation = self.vision.blue_robot[0].orientation
def vdistance(self, id):
if id == -1:
return [self.px - self.ox, self.py - self.oy]
return [
self.px - self.vision.yellow_robot[id].x,
self.py - self.vision.yellow_robot[id].y
]
def Euclidean_distance(self, vec):
return sqrt(vec[0]**2 + vec[1]**2)
def __init__(self, forx, fory, Ka, da, Kb, db, delta_t, distance):
self.vision = Vision()
self.debugger = Debugger()
time.sleep(0.5)
#print(self.vision.yellow_robot[0].x)
self.Refresh_robo()
self.ox = forx
self.oy = fory
self.Ka = Ka
self.da = da
self.Kb = Kb
self.db = db
self.delta_t = delta_t
self.distance = distance
self.final_list = [[self.px, self.py]]
self.ax = 1
self.ay = 1
# TYPE = 0 is attraction while TYPE = 1 is rejection
def Force(self, TYPE, id):
if TYPE == 0:
distance = self.vdistance(-1)
s = np.array(distance)
Edistance = self.Euclidean_distance(distance)
if Edistance <= self.da:
return list((-2) * self.Ka * s)
if Edistance > self.da:
return list((-2) * self.Ka * self.da / Edistance * s)
elif TYPE == 1:
distance = self.vdistance(id)
s = np.array(distance)
Edistance = self.Euclidean_distance(distance)
if Edistance <= self.db:
f = list(self.Kb * (1 / Edistance - 1 / self.db) *
((1 / Edistance)**3) * s)
return f
if Edistance > self.db:
return [0, 0]
def Total_Force(self): # 计算当前x ,y 方向上的合力
[self.ax, self.ay] = [0, 0]
#print(self.Force(0, 0))
[self.ax, self.ay
] = [self.ax + self.Force(0, 0)[0], self.ay + self.Force(0, 0)[1]]
for i in range(16):
print(self.Force(1, i))
[self.ax, self.ay] = [
self.ax + self.Force(1, i)[0], self.ay + self.Force(1, i)[1]
]
print("adddddd")
print(self.ax, self.ay)
def Refresh_Position(self):
# self.Refresh_robo() 路径规划时不执行
self.Total_Force()
# print("AA")
# print(self.ax, self.ay)
# self.vx += self.delta_t * self.ax
# self.vy += self.delta_t * self.ay
# self.px += self.vx * self.delta_t + 0.5 * self.ax * self.delta_t ** 2
# self.py += self.vy * self.delta_t + 0.5 * self.ay * self.delta_t ** 2
# print(self.px,self.py)
# self.final_list.append([self.px, self.py])
# self.debugger.draw_line(self.final_list[-1][0],self.final_list[-1][1],self.final_list[-2][0],self.final_list[-2][1],"FORWARD")
# time.sleep(1)
theta = atan(self.ay / self.ax)
print("theta")
print(theta)
self.px += self.delta_t * (self.ax / sqrt(self.ax**2 + self.ay**2))
self.py += self.delta_t * (self.ay / sqrt(self.ax**2 + self.ay**2))
self.final_list.append([self.px, self.py])
self.debugger.draw_line(self.final_list[-1][0], self.final_list[-1][1],
self.final_list[-2][0], self.final_list[-2][1],
"FORWARD")
time.sleep(0.01)
# Action() 发送具体的运动指令
def checkDestination(self):
s1 = np.array([self.ox, self.oy])
s2 = np.array(self.final_list[-1])
print("dis")
dis = self.Euclidean_distance(list(s2 - s1))
print(dis)
if dis <= self.distance:
return True
else:
return False
def Draw(self):
self.debugger.draw_final_line(self.final_list, 0, 0)
import sys
import csv
from debug import Debugger
from board import Board
if len(sys.argv) > 1:
depth = int(sys.argv[1])
assert 0 < depth < 7
board = Board()
debugger = Debugger(board, quiet=True)
def parse_perftsuite():
with open('test/perftsuite.epd', 'rb') as csvfile:
reader = csv.reader(csvfile, delimiter=';')
for row in reader:
parsed = [c.strip() for c in row]
fen, results = parsed[0], [r.split() for r in parsed[1:]]
yield fen, {k: int(v) for k, v in results}
l = 0
for fen, results in parse_perftsuite():
l += 1
board.reset(fen)
if not 'D%i' % depth in results: continue
try:
assert debugger.divide(depth) == results['D%i' % depth]
from vision import Vision
from action import Action
from debug import Debugger
from astar2 import Astar2
from zero_blue_action import zero_blue_action
import time
import math
if __name__ == '__main__':
vision = Vision()
action = Action()
debugger = Debugger()
zero = zero_blue_action()
#astar = Astar()
start_x = 2400
start_y = 1500
goal_x = -2400
goal_y = -1500
while True:
time.sleep(1)
start_x, start_y = 2400, 1500
#start_x, start_y = vision.my_robot.x, vision.my_robot.y
goal_x, goal_y = -2400, -1500
# goal_x.append = [-2400]
# goal_y.append = [-1500]
astar2 = Astar2(start_x, start_y, goal_x, goal_y)
path_x, path_y = astar2.plan(vision)
path_x, path_y = zero.simplify_path(path_x=path_x, path_y=path_y)
from debug import Debugger
# Initializing main objects
# Configuration
conf = None
try:
conf = INIReader("config.ini")
except FileNotFoundError:
log.notice(
'A config.ini file was created. Fill it, then relaunch RocksmithSceneSwitcher.'
)
log.notice('Press any key to exit this program.')
input()
exit(0)
debug = Debugger(conf.get_value("Debugging", "debug", int),
conf.get_value("Debugging", "log_state_interval", int))
# OBS Web Socket Client / OBS controller abstraction
client = ObsController(conf.get_value("OBSWebSocket", "host"),
conf.get_value("OBSWebSocket", "port"),
conf.get_value("OBSWebSocket", "pass"))
sniffer = Rocksniffer(
conf.get_value("RockSniffer", "host"),
conf.get_value("RockSniffer", "port"),
)
error_wait_time = 1
def error(message):
"""
log Error with incremental wait time.