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