def __init__(self, id_r, weight, color, xd=0, yd=0): self.id = id_r self.weight = weight self.color = color self.speed_v = 0 self.speed_w = 0 self.speed_pub = None self.pose_sub = None self.pose = None self.first_pose = None self.mass = 0 self.xd = xd self.yd = yd self.neighbors = {} self.weight_publisher = rospy.Publisher("/voronoi/robot_" + str(self.id) + "/weight", Float64, queue_size=1) self.weight_publisher_str = rospy.Publisher( "/voronoi/robot_" + str(self.id) + "/weight/str", String, queue_size=1) self.control = RobotControl()
def __init__(self): self._robot = RobotControl(robotName=ROBOT_NAME, dof=DOF) self._world = WorldControl() self._pin_hole_left_cam = Camera( rgbImageTopic=PIN_HOLE_LEFT_CAMERA_TOPIC) self._pin_hole_right_cam = Camera( rgbImageTopic=PIN_HOLE_RIGHT_CAMERA_TOPIC) self._inspected_pin_leads_in_cameras = None self._board_pose_mm_deg = None self._robot_init_pose_mm_deg = None self._left_pin_lead_pose_init_mm_deg = None self._right_pin_lead_pose_init_mm_deg = None
def executar_planejamento(planejamento): robot_control = RobotControl() while planejamento and robot_control.simulator.isConnected(): acao = planejamento.pop(0) if acao == 'straight': print("Seguindo em frente...") elif acao == 'left': print("Virando para a esquerda...") elif acao == 'right': print("Virando para a direita...") robot_control.run(acao) time.sleep(1) print("Planejamento executado") print() robot_control.simulator.close()
class VSGazeboEnv(object): def __init__(self): self._robot = RobotControl(robotName=ROBOT_NAME, dof=DOF) self._world = WorldControl() self._pin_hole_left_cam = Camera( rgbImageTopic=PIN_HOLE_LEFT_CAMERA_TOPIC) self._pin_hole_right_cam = Camera( rgbImageTopic=PIN_HOLE_RIGHT_CAMERA_TOPIC) self._inspected_pin_leads_in_cameras = None self._board_pose_mm_deg = None self._robot_init_pose_mm_deg = None self._left_pin_lead_pose_init_mm_deg = None self._right_pin_lead_pose_init_mm_deg = None # ----- get images ------ def getPinHoleLeftImage(self): return self._pin_hole_left_cam.getRGBImage() def getPinHoleRightImage(self): return self._pin_hole_right_cam.getRGBImage() # ----- robot manipulation ----- def getRobotPose(self): return self._robot.getRobotPos() def setRobotPose(self, pose_mm_deg, wait=True): self._robot.setRobotPos(pose_mm_deg) timeout = 0 while wait and self._robot.isMoving: time.sleep(0.01) timeout += 0.01 if timeout > ROBOT_TIMEOUT_SEC: raise VSGazeboEnvError('Robot moving timeout') def setRobotJointAngles(self, angles_deg, wait=True): self._robot.setJointAngle(angles_deg) timeout = 0 while wait and self._robot.isMoving: time.sleep(0.01) timeout += 0.01 if timeout > ROBOT_TIMEOUT_SEC: raise VSGazeboEnvError('Robot moving timeout') def resetRobotToHome(self): self.setRobotJointAngles([0] * 6) def goToLeadInspectionPose(self): self.setRobotPose(LEAD_INSPECTION_POSE_MM_DEG) def setRobotSpeed(self, speed_percentage): self._robot.setSpeedRate(speed_percentage) # ----- model manipulation ----- def _getModelPose_mm_deg(self, link_name): return self._world.getLinkPos(link_name) def _setModelPose(self, link_name, pose_mm_deg): self._world.setLinkPos(link_name, pose_mm_deg) # ----- board related actions ----- def getBoardPose_mm_deg(self): return self._getModelPose_mm_deg(BOARD_LINK) def setBoardPose(self, pose_mm_deg): self._setModelPose(BOARD_LINK, pose_mm_deg) def getHolePoses_mm_deg(self): board_pose_mm_deg = self.getBoardPose_mm_deg() t_board_to_ref = Pose2T(board_pose_mm_deg) t_left_hole_to_board = Pose2T(LEFT_HOLE2BOARD_MM_DEG) t_right_hole_to_board = Pose2T(RIGHT_HOLE2BOARD_MM_DEG) t_left_hole_to_ref = t_board_to_ref.dot(t_left_hole_to_board) t_right_hole_to_ref = t_board_to_ref.dot(t_right_hole_to_board) left_hole_pose_mm_deg = T2Pose(t_left_hole_to_ref) right_hole_pose_mm_deg = T2Pose(t_right_hole_to_ref) return left_hole_pose_mm_deg.flatten(), right_hole_pose_mm_deg.flatten( ) # ----- pin related operations ----- def getPinLink3PoseByCBase_mm_deg(self, pin_link3_to_c_base): c_base_pose_mm_deg = self._getModelPose_mm_deg(C_BASE_LINK) t_c_base_to_ref = Pose2T(c_base_pose_mm_deg) t_pin_link3_to_c_base = Pose2T(pin_link3_to_c_base) t_pin_link3_to_ref = t_c_base_to_ref.dot(t_pin_link3_to_c_base) pin_link3_pose_mm_deg = T2Pose(t_pin_link3_to_ref) return pin_link3_pose_mm_deg.flatten() def getLeftPinLink3Pose_mm_deg(self, by_c_base=False): return self.getPinLink3PoseByCBase_mm_deg(LEFT_PIN_LINK3_TO_CBASE_MM_DEG) if by_c_base else \ self._getModelPose_mm_deg(LEFT_PIN_LINK_3) def getRightPinLink3Pose_mm_deg(self, by_c_base=False): return self.getPinLink3PoseByCBase_mm_deg(RIGHT_PIN_LINK3_TO_CBASE_MM_DEG) if by_c_base else \ self._getModelPose_mm_deg(RIGHT_PIN_LINK_3) def getPinLeadPoses_mm_deg(self, by_c_base=False): left_pin_link3_pose_mm_deg = self.getLeftPinLink3Pose_mm_deg(by_c_base) right_pin_link3_pose_mm_deg = self.getRightPinLink3Pose_mm_deg( by_c_base) t_left_pin_link3_to_ref = Pose2T(left_pin_link3_pose_mm_deg) t_right_pin_link3_to_ref = Pose2T(right_pin_link3_pose_mm_deg) t_lead_to_link3 = Pose2T(PIN_LEAD2LINK3_MM_DEG) t_left_pin_lead_to_ref = t_left_pin_link3_to_ref.dot(t_lead_to_link3) t_right_pin_lead_to_ref = t_right_pin_link3_to_ref.dot(t_lead_to_link3) left_pin_lead_pose_mm_deg = T2Pose(t_left_pin_lead_to_ref) right_pin_lead_pose_mm_deg = T2Pose(t_right_pin_lead_to_ref) return left_pin_lead_pose_mm_deg.flatten( ), right_pin_lead_pose_mm_deg.flatten() def getPinLeadsHolesInImage(self, camera_link): f_width = IMAGE_WIDTH_PX / (np.tan(FOV / 2.0) * 2.0) cameraMatrix = np.array( [[f_width, 0, IMAGE_WIDTH_PX / 2.0], [0, f_width, IMAGE_HEIGHT_PX / 2.0], [0, 0, 1]], dtype=np.float32) camera_pose_mm_deg = self._getModelPose_mm_deg(camera_link) t_camera_to_ref = Pose2T(camera_pose_mm_deg) t_ref_to_camera = np.linalg.inv(t_camera_to_ref) # -----pin leads in image ----- left_pin_lead_pose_mm_deg, right_pin_lead_pose_mm_deg = self.getPinLeadPoses_mm_deg( ) left_pin_in_camera_px = projectPtsToImg(left_pin_lead_pose_mm_deg[:3], t_ref_to_camera, cameraMatrix, []) right_pin_in_camera_px = projectPtsToImg( right_pin_lead_pose_mm_deg[:3], t_ref_to_camera, cameraMatrix, []) # -----holes in image ----- left_hole_pose_mm_deg, right_hole_pose_mm_deg = self.getHolePoses_mm_deg( ) left_hole_in_camera_px = projectPtsToImg(left_hole_pose_mm_deg[:3], t_ref_to_camera, cameraMatrix, []) right_hole_in_camera_px = projectPtsToImg(right_hole_pose_mm_deg[:3], t_ref_to_camera, cameraMatrix, []) return left_pin_in_camera_px.flatten(), right_pin_in_camera_px.flatten(), \ left_hole_in_camera_px.flatten(), right_hole_in_camera_px.flatten() def getTcA2ref(self): camera_pose_mm_deg = self._getModelPose_mm_deg( PIN_HOLE_LEFT_CAMERA_LINK) t_camera_to_ref = Pose2T(camera_pose_mm_deg) return t_camera_to_ref def getTcB2ref(self): camera_pose_mm_deg = self._getModelPose_mm_deg( PIN_HOLE_RIGHT_CAMERA_LINK) t_camera_to_ref = Pose2T(camera_pose_mm_deg) return t_camera_to_ref def getPinLeadsHolesInLeftCamera(self): return self.getPinLeadsHolesInImage(PIN_HOLE_LEFT_CAMERA_LINK) def getPinLeadsHolesInRightCamera(self): return self.getPinLeadsHolesInImage(PIN_HOLE_RIGHT_CAMERA_LINK) def getPinLeadsHolesInCameras(self): # ----- pin leads and holes in left camera ----- left_pin_in_left_camera_px, right_pin_in_left_camera_px, \ left_hole_in_left_camera_px, right_hole_in_left_camera_px = self.getPinLeadsHolesInLeftCamera() # ----- pin leads and holes in right camera ----- left_pin_in_right_camera_px, right_pin_in_right_camera_px, \ left_hole_in_right_camera_px, right_hole_in_right_camera_px = self.getPinLeadsHolesInRightCamera() return np.r_[left_pin_in_left_camera_px, right_pin_in_left_camera_px, left_pin_in_right_camera_px, right_pin_in_right_camera_px], \ np.r_[left_hole_in_left_camera_px, right_hole_in_left_camera_px, left_hole_in_right_camera_px, right_hole_in_right_camera_px] # def setPinJointAngles(self, joint_angles_deg): def getTc2r(self, camera_link): camera_pose_mm_deg = self._getModelPose_mm_deg(camera_link) t_camera_to_ref = Pose2T(camera_pose_mm_deg) robot_base_pose = [0., 0., 600., 0., 0., 0.] t_robot_base_to_ref = Pose2T(robot_base_pose) t_ref_to_robot_base = np.linalg.inv(t_robot_base_to_ref) t_camera_to_robot_base = np.dot(t_ref_to_robot_base, t_camera_to_ref) return t_camera_to_robot_base def getTcA2r(self): return self.getTc2r(PIN_HOLE_LEFT_CAMERA_LINK) def getTcB2r(self): return self.getTc2r(PIN_HOLE_RIGHT_CAMERA_LINK) def getTcA2cB(self): TcA2r = self.getTcA2r() TcB2r = self.getTcB2r() Tr2cB = np.linalg.inv(TcB2r) TcA2cB = np.dot(Tr2cB, TcA2r) return TcA2cB def getTref2r(self): robot_base_pose = [0., 0., 600., 0., 0., 0.] t_robot_base_to_ref = Pose2T(robot_base_pose) t_ref_to_robot_base = np.linalg.inv(t_robot_base_to_ref) return t_ref_to_robot_base def setVirtualPointsInImg(self, delta_z, camera_link): f_width = IMAGE_WIDTH_PX / (np.tan(FOV / 2.0) * 2.0) cameraMatrix = np.array( [[f_width, 0, IMAGE_WIDTH_PX / 2.0], [0, f_width, IMAGE_HEIGHT_PX / 2.0], [0, 0, 1]], dtype=np.float32) camera_pose_mm_deg = self._getModelPose_mm_deg(camera_link) t_camera_to_ref = Pose2T(camera_pose_mm_deg) t_ref_to_camera = np.linalg.inv(t_camera_to_ref) # -----pin leads in image ----- left_pin_lead_pose_mm_deg, right_pin_lead_pose_mm_deg = self.getPinLeadPoses_mm_deg( ) left_pin_in_camera_px = projectPtsToImg(left_pin_lead_pose_mm_deg[:3], t_ref_to_camera, cameraMatrix, []) right_pin_in_camera_px = projectPtsToImg( right_pin_lead_pose_mm_deg[:3], t_ref_to_camera, cameraMatrix, []) # -----holes in image ----- left_hole_pose_mm_deg, right_hole_pose_mm_deg = self.getHolePoses_mm_deg( ) left_hole_pose_mm_deg[2] += delta_z right_hole_pose_mm_deg[2] += delta_z left_hole_in_camera_px = projectPtsToImg(left_hole_pose_mm_deg[:3], t_ref_to_camera, cameraMatrix, []) right_hole_in_camera_px = projectPtsToImg(right_hole_pose_mm_deg[:3], t_ref_to_camera, cameraMatrix, []) return left_pin_in_camera_px.flatten(), right_pin_in_camera_px.flatten(), \ left_hole_in_camera_px.flatten(), right_hole_in_camera_px.flatten() def getVirtualPointsInImg(self, delta_z): left_pin_in_left_camera, right_pin_in_left_camera, left_hole_in_left_camera, right_hole_in_left_camera = self.setVirtualPointsInImg( delta_z, PIN_HOLE_LEFT_CAMERA_LINK) left_pin_in_right_camera, right_pin_in_right_camera, left_hole_in_right_camera, right_hole_in_right_camera = self.setVirtualPointsInImg( delta_z, PIN_HOLE_RIGHT_CAMERA_LINK) virtualImgPtsA_hole = np.hstack( (left_hole_in_left_camera.reshape(2, -1), right_hole_in_left_camera.reshape(2, -1))) virtualImgPtsB_hole = np.hstack( (left_hole_in_right_camera.reshape(2, -1), right_hole_in_right_camera.reshape(2, -1))) return virtualImgPtsA_hole, virtualImgPtsB_hole def getRealImgPts_hole(self): _, _, left_hole_in_left_camera, right_hole_in_left_camera = self.getPinLeadsHolesInLeftCamera( ) _, _, left_hole_in_right_camera, right_hole_in_right_camera = self.getPinLeadsHolesInRightCamera( ) realImgPtsA_hole = np.hstack((left_hole_in_left_camera.reshape(2, -1), right_hole_in_left_camera.reshape(2, -1))) realImgPtsB_hole = np.hstack( (left_hole_in_right_camera.reshape(2, -1), right_hole_in_right_camera.reshape(2, -1))) return realImgPtsA_hole, realImgPtsB_hole def getRealImgPts_pin(self): left_pin_in_left_camera, right_pin_in_left_camera, _, _, = self.getPinLeadsHolesInLeftCamera( ) left_pin_in_right_camera, right_pin_in_right_camera, _, _, = self.getPinLeadsHolesInRightCamera( ) RealImgPtsA_pin = np.hstack((left_pin_in_left_camera.reshape(2, -1), right_pin_in_left_camera.reshape(2, -1))) RealImgPtsB_pin = np.hstack((left_pin_in_right_camera.reshape(2, -1), right_pin_in_right_camera.reshape(2, -1))) return RealImgPtsA_pin, RealImgPtsB_pin def getRealPinPtsInRob(self): left_pin_lead_pose_mm_deg, right_pin_lead_pose_mm_deg = self.getPinLeadPoses_mm_deg( ) t_ref_to_robot_base = self.getTref2r() RealPinPtsInRob_left = projectPts( left_pin_lead_pose_mm_deg[:3].reshape(3, -1), t_ref_to_robot_base) RealPinPtsInRob_right = projectPts( right_pin_lead_pose_mm_deg[:3].reshape(3, -1), t_ref_to_robot_base) RealPinPtsInRob = np.hstack((RealPinPtsInRob_left.reshape(3, -1), RealPinPtsInRob_right.reshape(3, -1))) return RealPinPtsInRob # ----- operations for RL algorithms ----- def reset(self, board_pose_mm_deg, robot_pose_mm_deg): self._board_pose_mm_deg = board_pose_mm_deg self._robot_init_pose_mm_deg = robot_pose_mm_deg self.setBoardPose(board_pose_mm_deg) self.goToLeadInspectionPose() self._inspected_pin_leads_in_cameras = self.getPinLeadsHolesInCameras() self.setRobotPose(robot_pose_mm_deg) self._left_pin_lead_pose_init_mm_deg, self._right_pin_lead_pose_init_mm_deg = \ self.getPinLeadPoses_mm_deg(GET_LINK3_POSE_BY_C_BASE) return True # return False if not self.straighten_pins() else True def getInspectedPinLeadsInCameras(self): return self._inspected_pin_leads_in_cameras
from RobotControl import RobotControl if __name__ == "__main__": RC = RobotControl() RC.link(fileName="shm_VS.bin") RC.setSpeed([10, 10, 10]) RC.setAcce([10, 10, 10, 10, 0, 0]) print '[350,153.8,121.0,51.5,0,0] isPoseOK?', RC.isPoseOK( [350, 153.8, 121.0, 51.5, 0, 0]) print RC.getCurPos() RC.sendRobotPos(robotPos=[350, 153.8, 121.0, 51.5, 0, 0], type="MOVE", error=0.5) print RC.getCurPos() raw_input('input anykey to next') RC.sendRobotPos(robotPos=[299, 154, 76, 82, 0, 0], type="GO") print RC.getCurPos()
class Robot: def __init__(self, id_r, weight, color, xd=0, yd=0): self.id = id_r self.weight = weight self.color = color self.speed_v = 0 self.speed_w = 0 self.speed_pub = None self.pose_sub = None self.pose = None self.first_pose = None self.mass = 0 self.xd = xd self.yd = yd self.neighbors = {} self.weight_publisher = rospy.Publisher("/voronoi/robot_" + str(self.id) + "/weight", Float64, queue_size=1) self.weight_publisher_str = rospy.Publisher("/voronoi/robot_" + str(self.id) + "/weight/str", String, queue_size=1) self.control = RobotControl() def get_yaw(self): (roll, pitch, yaw) = euler_from_quaternion(self.pose.orientation) return yaw def set_speed_publisher(self, topic): self.speed_pub = rospy.Publisher(topic, Twist, queue_size=5) self.init_robot_control() def init_robot_control(self): self.control.set_speed_publisher(self.speed_pub) self.control.set_pose_updater(self.get_pose) self.control.start() def set_pose_subscriber(self, topic): # type: (basestring) -> None self.pose_sub = rospy.Subscriber(topic, Odometry, self.pose_callback) def publish_speed(self, v, w): # type: (float, float) -> None if self.speed_pub is None: raise ValueError("geometry_msgs/Twist publisher for robot " + self.id + " not initialized.") else: self.speed_v = v self.speed_w = w speed = Twist() speed.linear.x = v speed.angular.w = w self.speed_pub.publish(speed) def pose_callback(self, msg): # type: (Odometry) -> None if self.pose is None: self.first_pose = msg.pose.pose self.pose = msg.pose.pose Util.publish_tf_transformation(self.first_pose, "robot_" + str(self.id) + "/odom", "/map") def get_pose_array(self): return [self.pose.position.x, self.pose.position.y] def get_pose(self): return self.pose def __repr__(self): rep = self.__dict__.copy() del rep["pose"] if rep["speed_pub"] is not None: rep["speed_pub"] = "value set" if rep["pose_sub"] is not None: rep["pose_sub"] = "value set" return json.dumps(rep) def __str__(self): return self.__repr__() def set_pose(self, arr): # type: (list) -> None if self.pose is None: self.pose = Pose() self.pose.position.x = arr[0] self.pose.position.y = arr[1] def clear(self): self.mass = 0 self.neighbors = {} def get_kdel(self): return np.matrix([[self.xd, self.yd], [self.xd, self.yd]])
def heardEnter(): i,o,e = select.select([sys.stdin],[],[],0.0001) for s in i: if s == sys.stdin: input = sys.stdin.readline() return True return False ####################################MAIN METHOD########################################## colorseg = ColorSegStereo(colorseg_debug) botcontrol = RobotControl() pathplan = PathPlanner() sensorcontrol = SensorControl() processQ = Queue.Queue(2) wrapper = Wrapper() #client = Client() tw = ThreadWrapper() #nw = NetworkWrapper() #sw = ServerWrapper() def signal_handler(signal,frame): print 'Stopping robot' botcontrol.stop_robot() #tw.stop() if androidFlag: nw.stop()
while openNodes: visitedNodes = visitedNodes + 1 current = getShortest(openNodes) if current.data == goal: print("*** {avaliados} estados avaliados".format(avaliados=visitedNodes)) Busca_Largura.imprimir_caminho(Busca_Largura.determinar_caminho(current)) planning = Busca_Largura.gerar_planejamento(current) return planning neighbors_data = Busca_Largura.transicoes_possiveis(current.data) for neighbor_data in neighbors_data: new_cost = cost_so_far[repr(current.data)] + 1 if repr(neighbor_data) not in cost_so_far or new_cost < cost_so_far[repr(neighbor_data)]: cost_so_far[repr(neighbor_data)] = new_cost heuristic_cost = cost_so_far[repr(current.data)] + heuristic(goal, neighbor_data) openNodes.append([Node(neighbor_data, current), heuristic_cost]) raise Exception('O estado {destino} não é alcançável a partir de {origem}'.format(origem=start, destino=goal)) #def execute(): print("Origem: {origem}".format(origem=estadoInicial)) print("Destino: {destino}".format(destino=estadoFinal)) print("") planejamento = A_Star_search(estadoInicial, estadoFinal) robotControl = RobotControl() robotControl.execute_commands(planejamento)
from RobotControl import RobotControl if __name__ == "__main__": RC = RobotControl() RC.link(fileName="shm_VS.bin") RC.setSpeed([10, 10, 10]) RC.setAcce([10, 10, 10, 10, 0, 0]) # zx, oi, begin #print '[350,153.8,121.0,51.5,0,0] isPoseOK?', RC.isPoseOK([350,153.8,121.0,51.5,0,0]) #print RC.getCurPos() RC.sendRobotPos(robotPos=[350, 153.8, 121.0, 51.5, 0, 0], type="MOVE", error=0.5) # zx, oi, end #RC.sendRobotPos(robotPos=[200.0, -226.0, 395.07, -37.89, 0.0, 89.92], type="MOVE", error=0.5) print RC.getCurPos() raw_input('input anykey to next') # zx, oi, begin #RC.sendRobotPos(robotPos=[299,154,60,82,0,0],type="GO") # zx, oi, end #RC.sendRobotPos(robotPos=[221.13211468, 144.40044912, 60, 82, 0, 0], type="GO") #RC.sendRobotPos(robotPos=[364.29494662, -52.58334555, 54.57142639, 0.66162154, 0.0, 0.0], type="GO") # p0 #RC.sendRobotPos(robotPos=[340.12691049, -55.48038278, 54.57142639, 0.66162154, 0.0, 0.0], type="GO") # p1 #RC.sendRobotPos(robotPos=[337.16612335, -30.80669559, 54.57142639, 0.66162154, 0.0, 0.0], type="GO") # p8 #RC.sendRobotPos(robotPos=[239.50279732, -42.36209227, 54.57142639, 0.66162154, 0.0, 0.0], type="GO") # p12 #RC.sendRobotPos(robotPos=[233.35125407, 7.32913015, 54.57142639, 0.66162154, 0.0, 0.0], type="GO") # p26 #RC.sendRobotPos(robotPos=[223.98607617, 82.37886273, 54.57142639, 0.66162154, 0.0, 0.0], type="GO") # p47
def heardEnter(): i,o,e = select.select([sys.stdin],[],[],0.0001) for s in i: if s == sys.stdin: input = sys.stdin.readline() return True return False ####################################MAIN METHOD########################################## colorseg = ColorSegStereo(colorseg_debug) botcontrol = RobotControl() pathplan = PathPlanner() sensorcontrol = SensorControl() processQ = Queue.Queue(2) wrapper = Wrapper() client = Client() tw = ThreadWrapper() nw = NetworkWrapper() sw = ServerWrapper() def signal_handler(signal,frame): print 'Stopping robot' botcontrol.stop_robot() tw.stop() if androidFlag: nw.stop() sys.exit(0)
from RobotControl import RobotControl robot_control = RobotControl() commands = { '8': 'straight', '4': 'left', '6': 'right', } while robot_control.simulator.isConnected(): print(''' Comandos: Frente: 8 Esquerda: 4 Direita: 6 ''') command = input() if command not in commands: print('Comando Invalido') continue robot_control.run(commands[command]) command = 0 while robot_control.isWalking(): continue print('Stopped') print(command)
final_position, population_size=20, mutation_probability=0.01, map_size=10, with_elitism=True) for i in range(maximum_generations): next_generation = genetic.generateNextGeneration() # print(genetic.best_score) # print(genetic.best_chromosome) print("Melhor cromossomo: {best_chromosome}".format( best_chromosome=genetic.best_chromosome)) print("Avaliação: {best_score}".format(best_score=genetic.best_score)) print() print("Primeira parte do caminho:") first_path_segment = Busca_Largura.search(initial_position, genetic.best_chromosome) print("Parte final do caminho:") final_path_segment = Busca_Largura.search(genetic.best_chromosome, final_position) full_path = first_path_segment + final_path_segment print("Caminho encontrado ({size} passos):".format(size=len(full_path))) print(full_path) RobotControl.execute_commands(full_path)