diff --git a/HardwareArduino/Hardware/Hardware.ino b/HardwareArduino/Hardware/Hardware.ino index 0d5f7d7..99d05fd 100644 --- a/HardwareArduino/Hardware/Hardware.ino +++ b/HardwareArduino/Hardware/Hardware.ino @@ -10,7 +10,7 @@ byakugan::RefletanciaMsg dataRefletancia; ros::Publisher pubRefletancia("refletancia", &dataRefletancia); byakugan::BotoesMsg dataBtns; -ros::Publisher pubBtns("botoes", &dataBtns); +ros::Publisher pubBtns("botoes_init", &dataBtns); byakugan::SensoresDistanciaMsg dataDist; ros::Publisher pubDist("distancia", &dataDist); diff --git a/byakugan/launch/sala3.launch b/byakugan/launch/sala3.launch index c9ae804..bc614a3 100644 --- a/byakugan/launch/sala3.launch +++ b/byakugan/launch/sala3.launch @@ -1,5 +1,5 @@ - + diff --git a/byakugan/scripts/cmdGarras.py b/byakugan/scripts/cmdGarras.py index 0641279..bc64649 100644 --- a/byakugan/scripts/cmdGarras.py +++ b/byakugan/scripts/cmdGarras.py @@ -11,7 +11,7 @@ def __init__(self, pub): def resgatar(self): self.dataGarras.mao.data = 0 - self.dataGarras.braco.data = 80 + self.dataGarras.braco.data = 100 self.pub.publish(self.dataGarras) self.abrirMao() for i in range (0, 2): diff --git a/byakugan/scripts/findRectangle.py b/byakugan/scripts/findRectangle.py index 71c9f69..d3ef4ac 100755 --- a/byakugan/scripts/findRectangle.py +++ b/byakugan/scripts/findRectangle.py @@ -80,6 +80,12 @@ def find(self): contours = self.cookImg() # cozinha a img novamente separando possiveis contours colados na img inicial indexCntImg = len(contours) - 1 + + if indexCntImg == 0: + areaBool = BoolStamped() + areaBool.existe.data = False + self.pub.publish(areaBool) + for i in range(0, indexCntImg): # -1 impede de draw contour da propria img cnt = contours[i] @@ -113,7 +119,7 @@ def find(self): self.pub.publish(areaBool) except: # ??? - print 'error in img' + rospy.loginfo('error in img') def callback(self, compressedImg): np_arr = np.fromstring(compressedImg.data, np.uint8) diff --git a/byakugan/scripts/sala3.py b/byakugan/scripts/sala3.py index d96b967..a0807eb 100755 --- a/byakugan/scripts/sala3.py +++ b/byakugan/scripts/sala3.py @@ -8,7 +8,7 @@ import cmdGarras from geometry_msgs.msg import Vector3Stamped from std_msgs.msg import Int32MultiArray -from byakugan.msg import BoolStamped, SensoresDistanciaMsg, BotoesMsg, CtrlMotores +from byakugan.msg import BoolStamped, SensoresDistanciaMsg, BotoesMsg, CtrlMotores, BoolGarras class Sala3(): def __init__(self): @@ -22,6 +22,7 @@ def __init__(self): self.encontrouArea = False self.pegou = False self.resgatou = False + self.chegueiPerto = False self.entrouNaSala = False @@ -42,6 +43,7 @@ def __init__(self): ts.registerCallback(self.callback) def callback(self, btns, coordinates, circle, dist, centroid): + if btns.botao2.data: self.podeExecutar = True elif btns.botao3.data: @@ -57,79 +59,81 @@ def callback(self, btns, coordinates, circle, dist, centroid): self.resgatar(centroid, dist) elif self.encontrou and self.pegou and self.resgatou: self.executou = True - + elif self.podeExecutar and self.executou: + self.cmdMotores.roboParaTras(1) + self.cmdMotores.roboDir(1) + self.cmdMotores.roboParar(1) + self.reboot() + + def reboot(self): + self.encontrou = False + self.encontrouArea = False + self.pegou = False + self.resgatou = False + self.chegueiPerto = False + self.entrouNaSala = False + def procurar(self, coordinates, circle, dist): x, y, r = coordinates.vector.x, coordinates.vector.y, coordinates.vector.z - if(self.entrouNaSala): - if circle.existe.data: - self.encontrou = True + if circle.existe.data: + self.encontrou = True + + if self.encontrou: + self.cmdMotores.roboAcionarMotores(0, 0) + + rospy.loginfo("achei a tete") + self.cmdMotores.roboParar(1) - if self.encontrou: - self.cmd.roboAcionarMotores(0, 0) - - rospy.loginfo("achei a tete") - - ''' - if x in numpy.arange(200, 280, 1): - self.cmd.roboAcionarMotores(0, 0) - self.pegarVitima() - ''' - else: ''' - if not self.qntAchou < 0: - self.qntAchou = self.qntAchou - 1 - else: - self.qntAchou = 0 - - rospy.loginfo("cade a tete") + if x in numpy.arange(200, 280, 1): + self.cmd.roboAcionarMotores(0, 0) + self.pegarVitima() ''' - self.cmd.roboAcionarMotores(-30, 30) + else: + ''' + if not self.qntAchou < 0: + self.qntAchou = self.qntAchou - 1 + else: + self.qntAchou = 0 + ''' + rospy.loginfo("sumiu?!") + + self.cmdMotores.roboAcionarMotores(-30, 30) - def pegar(self, coordinates): + def pegar(self, coordenadas): x, y, r = coordenadas.vector.x, coordenadas.vector.y, coordenadas.vector.z - if(r < 48 and self.encontrei == False ): + if(r < 48 and self.chegueiPerto == False): rospy.loginfo("Estou longe") self.cmdMotores.roboAcionarMotores(30, 34) else: self.cmdMotores.roboAcionarMotores(0,0) rospy.loginfo("Estou perto") + self.chegueiPerto = True self.cmdGarras.abrirMao() self.cmdGarras.abaixarBraco() self.cmdGarras.fecharMao() self.cmdGarras.subirBraco() - time.sleep(2) self.pegou = True + self.cmdMotores.roboParar(3) - def resgatar(self): + def resgatar(self, areaBool, dist): if self.encontrouArea and not self.resgatou: - if sonar.sensoresDistancia[0] < 6: + if dist.sensoresDistancia[0] < 6: self.resgatou = True - self.cmd.roboParar(0.8) + self.cmdMotores.roboParar(0.8) self.cmdGarras.resgatar() else: - self.cmd.roboAcionarMotores(30, 34) + self.cmdMotores.roboAcionarMotores(30, 34) else: if areaBool.existe.data == False: rospy.loginfo("cade a tete?") - self.cmd.roboAcionarMotores(25, -25) + self.cmdMotores.roboAcionarMotores(25, -25) else: - if areaBool.centroid.data > 30: # area na esq - #self.publishLeds(1, 0, 0) - rospy.loginfo("area na esq") - self.cmd.roboAcionarMotores(-25, 25) - #pass - elif areaBool.centroid.data < -50: # area na dir - #self.publishLeds(0, 0, 1) - rospy.loginfo("area na dir") - self.cmd.roboAcionarMotores(25, -25) - #pass - elif not self.encontrou: - rospy.loginfo("achei a tete!!!") - self.encontrouArea = True - self.cmd.roboParar(1) + self.cmdMotores.roboAcionarMotores(0, 0) + self.encontrouArea = True if __name__ == "__main__": diff --git a/byakugan/scripts/showDeArea.py b/byakugan/scripts/showDeArea.py index 1e75e55..d7f9822 100755 --- a/byakugan/scripts/showDeArea.py +++ b/byakugan/scripts/showDeArea.py @@ -86,6 +86,7 @@ def find(self): contours = self.cookImg() # cozinha a img novamente separando possiveis contours colados na img inicial indexCntImg = len(contours) - 1 + rospy.loginfo('oi') for i in range(0, indexCntImg): # -1 impede de draw contour da propria img cnt = contours[i] @@ -93,7 +94,8 @@ def find(self): approx = cv2.approxPolyDP(cnt, .03 * cv2.arcLength(cnt, True), True) cX, cY = self.getCentroid(cnt, self.img) # pega centro do contorno - #print cX, cY + + print cX area = self.getArea(cnt) @@ -111,6 +113,7 @@ def find(self): print 'error in img' def callback(self, compressedImg): + rospy.loginfo('oi') np_arr = np.fromstring(compressedImg.data, np.uint8) self.img = cv2.imdecode(np_arr, cv2.IMREAD_COLOR) self.img = cv2.flip(self.img, 2)