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)
