def __init__(self, robotType, configData, workspace, shouldSavePlots,
baseSaveFName):
# calling the superclass contructor to inherit its properties
Robot.__init__(self,
robotType=robotType,
configData=configData,
workspace=workspace,
shouldSavePlots=shouldSavePlots,
baseSaveFName=baseSaveFName)
# need to maintain a separate history for c states and workspace states
# however, for a point robot, they are the same
self.cStateHistory = []
self.startCState = self.startState
self.goalCState = self.goalState
self.currentState = self.startState
self.updateRobotState(self.startState)
linearDiscretizationDensity = configData['linearDiscretizationDensity']
makeSquareCSpace = configData['makeSquareCSpace']
# have the factory make the PointRobot's C-space
self.cSpace = activeSpaces.get(robotSpaceType='POINTROBOTCSPACE',
robot=self,
N=linearDiscretizationDensity,
makeSquare=makeSquareCSpace,
shouldSavePlots=shouldSavePlots,
baseSaveFName=baseSaveFName)
def __init__(self, robotType, configData, workspace, shouldSavePlots,
baseSaveFName):
# calling the superclass contructor to inherit its properties
Robot.__init__(self,
robotType=robotType,
configData=configData,
workspace=workspace,
shouldSavePlots=shouldSavePlots,
baseSaveFName=baseSaveFName)
self.currentState = self.startState
(relativeShapeVerts, robotOriginIdx) = self.setRobotShape(configData)
self.workspaceVertCoords = []
# this is how the robot's vertices are the robot's verices in its
# local coordinate system
self.relativeShapeVerts = relativeShapeVerts
# need to store which of the robot's vertices is its origin
self.robotOriginIdx = robotOriginIdx
# have the factory get make the PolygonalRobot's C-space
self.cSpace = activeSpaces.get(robotSpaceType='POLYGONALROBOTCSPACE',
robot=self,
shouldSavePlots=shouldSavePlots,
baseSaveFName=baseSaveFName)
def __init__(self, robotType, configData, workspace, shouldSavePlots,
baseSaveFName):
# calling the superclass contructor to inherit its properties
Robot.__init__(self,
robotType=robotType,
configData=configData,
workspace=workspace,
shouldSavePlots=shouldSavePlots,
baseSaveFName=baseSaveFName)
self.configData = configData
self.shouldSavePlots = shouldSavePlots
self.baseSaveFName = baseSaveFName
# configure manipulator's links
linkLengths = configData['linkLengths']
jointOffset = configData['jointOffset']
links = []
for linkLength in linkLengths:
link = Link(origin=[0, 0],
linkLength=linkLength,
jointOffset=jointOffset)
links.append(link)
self.links = links
self.numLinks = len(links)
# need to save history of link updates for plotting motion of robot
self.linkHistory = []
# tolerancing for inverse kinematics
self.IK_TOL = 1e-6
# need to compute its start / goal cspace coordinates given the
# starting workspace state
self.cStateHistory = []
(self.startCState, self.currentState,
self.links) = self.setEffectorToDesired(self.startState)
(self.goalCState, _, _) = self.setEffectorToDesired(self.goalState)
# now "state" is the workspace end effector position after inverse
# kinematics
self.startState = self.currentState
# now, call the state update routine to make sure everything is set
# correctly after initialization
self.updateRobotState(self.startCState)
linearDiscretizationDensity = configData['linearDiscretizationDensity']
# have the factory make the ManipulatorRobot's C-space
self.cSpace = activeSpaces.get(robotSpaceType='MANIPULATORROBOTCSPACE',
robot=self,
N=linearDiscretizationDensity,
shouldSavePlots=shouldSavePlots,
baseSaveFName=baseSaveFName)
def __init__(self, name, **kargs):
# Initialisation du logger
self.logger = getLogger("ia")
f=open(name+".ini")
fileConfig(f)
f.close()
self.logger.info("Starting %s robot" % name)
assert(name in ["petit", "gros"])
module = __import__("robots."+name)
self.robot = getattr(getattr(module, name), name.capitalize()+"Robot")()
Robot.copy_from(self.robot)
# On crase les attributs du robot par ceux passs ici en argument, utiles pour le testing
for argument in kargs:
setattr(self.robot, argument, kargs[argument])
self.can_sock = socket.socket()
self.ui_sock = socket.socket()
self.inter = socket.socket()
self.logger.debug("Trying to connect to the CAN")
self.can_sock.connect((self.robot.can_ip, self.robot.can_port))
self.logger.debug("Trying to connect to the UI")
self.ui_sock.connect((self.robot.ui_ip, self.robot.ui_port))
self.logger.debug("Trying to connect to the INTERCOMM")
self.inter.connect((self.robot.inter_ip, self.robot.inter_port))
self.can = Can(self.can_sock)
self.ui = UI(self.ui_sock)
self.inter = InterCom(self.inter)
self.dispatcher = Dispatcher(self.robot, self.can, self.ui)
self.can.dispatcher = self.dispatcher
self.ui.dispatcher = self.dispatcher
self.inter.dispatcher = self.dispatcher
self.dispatcher.start() # Mieux si demarre avant can et ui
self.can.start()
self.ui.start()
self.inter.start()
self.logger.info("IA initialized")
self.ui.join()
self.logger.info("IA stopped")
def process_event(self, e):
if self.state == 1:
if e.name == "start":
self.state += 1
self.move.speed(-self.robot.pos_speed)
elif self.state == 2:
if e.name == "bump" and e.state == "close":
self.state += 1
self.create_timer(self.robot.pos_timer)
elif self.state == 3:
if e.name == "timer":
self.state += 0.5
self.move.stop(self)
elif self.state == 3.5:
if e.name == "move" and e.type == "done":
self.state += 0.5
self.can.send("asserv off")
self.odo.set(self, **{"y": 10000 - self.robot.dimensions["back"], "rot": 27000 + Robot.vrille() })
elif self.state == 4:
if e.name == "odo" and e.type == "done":
self.state += 1
self.can.send("asserv on")
self.move.forward(self, 1000)
elif self.state == 5:
if e.name == "move" and e.type == "done":
self.state += 1
self.move.rotate(self, 9000)
elif self.state == 6:
if e.name == "move" and e.type == "done":
self.state += 1
self.move.speed(-self.robot.pos_speed)
elif self.state == 7:
if e.name == "bump" and e.state == "close":
self.state += 0.5
self.create_timer(self.robot.pos_timer)
elif self.state == 7.5:
if e.name == "timer":
self.state += 0.5
self.move.stop(self)
elif self.state == 8:
if e.name == "move" and e.type == "done":
self.state += 1
self.can.send("asserv off")
self.odo.set(self, **{"x": self.robot.dimensions["back"] - 15000, "rot": Robot.vrille()})
elif self.state == 9:
if e.name == "odo" and e.type == "done":
self.state += 1
self.can.send("asserv on")
self.move.forward(self, 500)
elif self.state == 10:
if e.name == "move" and e.type == "done":
self.state = 0
self.logger.info("Gros en position !")
self.send_event(Event("positioning", "done"))
import logging
from robots.robot import Robot
from robots.maps import PheMap, BarrierMap
from robots.setting import BOT_NUMS
logging.basicConfig(
level=logging.INFO,
format='%(asctime)s - %(name)s - %(levelname)s - %(message)s')
logger = logging.getLogger(__name__)
phe_map = PheMap()
barrier_map = BarrierMap()
robots_list = []
robots_init_loc = []
robots_await_nodes = {bot_id: [] for bot_id in range(BOT_NUMS)}
for i in range(BOT_NUMS):
new_bot_node = barrier_map.get_random_node()
while new_bot_node.loc() in robots_init_loc:
new_bot_node = barrier_map.get_random_node()
robots_init_loc.append((new_bot_node.x, new_bot_node.y))
robots_list.append(Robot(new_bot_node, bot_id=i))
robots_await_nodes[i] = []
final_map = []
for i in range(BOT_NUMS):
logger.debug(f'bot #{i} is inited at {robots_list[i].loc()}')
def process_event(self, event):
if self.state == 0:
if event.name == "start":
self.state += 1
self.move.forward(self, 5500) # on sort du depart
elif self.state == 1:
if event.name == "move" and event.type == "done":
self.state += 1
self.move.rotate(self, 27000, True) # on tourne vers les bouteilles
elif self.state == 2:
if event.name == "move" and event.type == "done":
self.state += 1
#self.move.forward(self, 4700) # on avance vers le totem
if Robot.side == 'violet':
self.move.reach_y(self, 3150) # NIM: c’était 2950 33/240
else:
self.move.reach_y(self, 2900) # NIM: c’était 2950
elif self.state == 3:
if event.name == "move" and event.type == "done":
self.state += 1
self.move.rotate(self, -Robot.vrille(), True) # on tourne vers le totem
elif self.state == 4:
if event.name == "move" and event.type == "done":
self.state += 1
self.can.send("ax 2 angle set 508")
self.missions["threshold"].activate(2, False)
if not self.odo.brd:
self.can.send("odo unmute")
self.move.speed(35)
elif self.state == 5:
if event.name == "odo" and event.type == "pos":
if event.pos.x > -1500:
self.state += 1
self.missions["threshold"].activate(2, True)
self.move.stop(self)
elif self.state == 6:
if event.name == "move" and event.type == "done":
self.state += 1
self.can.send("ax 2 angle set 0")
self.create_timer(200)
elif self.state == 7:
if event.name == "timer":
self.state += 1
self.missions["speedrotate"].start(0, 50)
elif self.state == 8:
if event.name == "odo" and event.type == "pos":
if event.rot > 17000 and event.rot < 32000:
self.state += 1
self.missions["speedrotate"].stop(self)
elif self.state == 9:
if event.name == "speedrotate" and event.type == "done":
if not self.odo.brd:
self.state += 1
self.can.send("odo mute")
self.logger.info("Bad orientation (%d), adjusting ..."
%self.odo.rot)
self.move.rotate(self, 20100, True)
elif self.state == 10:
if event.name == "move" and event.type == "done":
self.state += 1
self.move.reach_x(self, -12000)
elif self.state == 11:
if event.name == "move" and event.type == "done":
self.state += 0.5
self.can.send("ax 2 angle set 1023")
self.create_timer(2500)
elif self.state == 11.5:
if event.name == "timer":
self.state += 0.5
self.move.forward(self, -4000)
elif self.state == 12:
if event.name == "move" and event.type == "done":
self.state += 1
self.can.send("ax 2 angle set 0")
self.move.rotate(self, 27000, True)
elif self.state == 13:
if event.name == "move" and event.type == "done":
self.state = 13.05
self.move.reach_y(self, 8000)
elif self.state == 13.05:
if event.name == "move" and event.type == "done":
self.state = 13.1
self.move.speed(-15)
elif self.state == 13.1:
self.logger.info("event", event)
if event.name == "bump" and event.state == "close":
self.state += 0.1
self.create_timer(self.robot.pos_timer)
elif self.state == 13.2:
if event.name == "timer":
self.state += 0.1
self.move.stop(self)
elif self.state == 13.3:
if e.name == "move" and e.type == "done":
self.state = 0.1
self.can.send("asserv off")
self.odo.set(self, **{"y": 10000 - self.robot.dimensions["back"], "rot": 27000 + Robot.vrille()})
elif self.state == 13.4:
if e.name == "odo" and e.type == "done":
self.state = 14
self.can.send("asserv on")
self.move.forward(self, 1300)
elif self.state == 14:
if event.name == "move" and event.type == "done":
self.state += 1
self.move.rotate(self, 0, True)
elif self.state == 15:
if event.name == "move" and event.type == "done":
self.state += 0.5
self.move.reach_x(self, -13000)
elif self.state == 15.5:
if event.name == "move" and event.type == "done":
self.state += 0.5
self.move.speed(-15)
elif self.state == 16:
if event.name == "bump" and event.state == "close":
self.state += 1
self.create_timer(self.robot.pos_timer)
elif self.state == 17:
if event.name == "timer":
self.state += 1
self.move.stop(self)
elif self.state == 18:
if event.name == "move" and event.type == "done":
self.state += 1
self.can.send("asserv off")
self.odo.set(self, **{"x": self.robot.dimensions["back"] - 15000, "rot": Robot.vrille() })
elif self.state == 19:
if event.name == "odo" and event.type == "done":
self.can.send("asserv on")
self.send_event(Event("totem", "done"))
