API tools faq
paste
Advertisement
Guest User

Untitled

a guest
Jan 18th, 2018
66
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
Python 33.92 KB | None | 0 0
raw download clone embed print report
  1. #!/usr/bin/env python
  2. import rospy
  3. from geometry_msgs.msg import Pose2D, Pose
  4. from math import *
  5. import time
  6. import socket
  7. #import readchar
  8. import sys
  9. from struct import *
  10. import math
  11.  
  12.  
  13. class HexaGuide():
  14.    
  15.     def __init__(self):
  16.         self.pose1      = Pose2D()
  17.         self.pose2      = Pose2D()
  18.         self.marker     = Pose2D()
  19.         self.marker2    = Pose2D()
  20.  
  21.         self.IP1        = '192.250.9.9'
  22.         self.IP2        = '192.168.4.1'
  23.         #self.IP2        = '192.155.5.5' #if Lein hex then ovaj IP
  24.         self.Port       = 80
  25.  
  26.         self.power1     = 0
  27.         self.angle1     = 0
  28.         self.rotation1  = 0
  29.         self.staticTilt = 0
  30.         self.movingTilt = 0
  31.         self.onoff1     = 1
  32.         self.accX       = 0
  33.         self.accY       = 0
  34.         self.sliders1   = 50
  35.         self.sliders2   = 25
  36.         self.sliders3   = 0
  37.         self.sliders4   = 0
  38.         self.sliders5   = 0
  39.         self.sliders6   = 50
  40.         self.sliders7   = 0
  41.         self.sliders8   = 0
  42.         self.sliders9   = 0
  43.         self.duration   = 20
  44.        
  45.         self.power2     = 0
  46.         self.angle2     = 0
  47.         self.rotation2  = 0
  48.         self.onoff2     = 1
  49.         self.switch0    = False
  50.         self.switch1    = False
  51.         self.switch2    = False
  52.         self.switch3    = False
  53.         self.switch4    = False
  54.         self.switch5    = False
  55.         rospy.Subscriber("/pose_node/pose_id1", Pose2D, self.pose_callback1)
  56.         rospy.Subscriber("/pose_node/pose_id2", Pose2D, self.pose_callback2)
  57.         rospy.Subscriber("/pose_node/pose_id15", Pose2D, self.pose_callback15)
  58.         rospy.Subscriber("/pose_node/pose_id4", Pose2D, self.pose_callback4)
  59.         self.pub1 = rospy.Publisher("/algoritam/naredbe1",ByteMultiArray, queue_size=10)
  60.         self.pub2 = rospy.Publisher("/algoritam/naredbe2",ByteMultiArray, queue_size=10)
  61.        
  62.     def pose_callback1(self, data):                        
  63.         self.marker.x = data.x
  64.         self.marker.y = data.y
  65.         self.marker.theta = data.theta
  66.  
  67.     def pose_callback2(self, data):                        
  68.         self.pose1 = data
  69.         self.pose1.x = data.x
  70.         self.pose1.y = data.y
  71.         self.pose1.theta = data.theta
  72.  
  73.     def pose_callback15(self, data):
  74.         self.pose2.x = data.x
  75.         self.pose2.y = data.y
  76.         self.pose2.theta = data.theta
  77.  
  78.     def pose_callback4(self, data):
  79.         self.marker2.x = data.x
  80.         self.marker2.y = data.y
  81.         self.marker2.theta = data.theta
  82.    
  83.     def odmicanje(self,s1,s2):
  84.         if abs(self.pose1.theta) > 90 and self.pose1.y < (self.marker2.y+0.50) and abs(self.pose1.x-(self.marker.x+self.marker2.x)/2) < 0.50: #iznad tunela
  85.             self.angle1 = 180
  86.             self.angle2 = 180
  87.             self.power1 = 50
  88.             self.power2 = 50
  89.             self.rotation1 = 0
  90.             self.rotation2 = 0
  91.            
  92.             a1 = ByteMultiArray(data=[self.power1, self.angle1, self.rotation1])
  93.             a2 = ByteMultiArray(data=[self.power2, self.angle2,self.rotation2])
  94.            
  95.             self.pub1.publish(a1)
  96.             self.pub2.publish(a2)
  97.            
  98.             time.sleep(0.2)
  99.            
  100.             self.power1 = 0
  101.             self.power2 = 0
  102.             self.rotation1 = 0
  103.             self.rotation2 = 0
  104.            
  105.             a1 = ByteMultiArray(data=[self.power1, self.angle1, self.rotation1])
  106.             a2 = ByteMultiArray(data=[self.power2, self.angle2,self.rotation2])
  107.            
  108.             self.pub1.publish(a1)
  109.             self.pub2.publish(a2)
  110.            
  111.             time.sleep(0.2)
  112.            
  113.  
  114.         if abs(self.pose1.theta) <= 90 and self.pose2.y < self.marker2.y+0.35 and abs(self.pose2.x-(self.marker.x+self.marker2.x)/2) < 0.35:
  115.             self.angle1 = 0
  116.             self.angle2 = 0
  117.             self.power1 = 50
  118.             self.power2 = 50
  119.             self.rotation1 = 0
  120.             self.rotation2 = 0
  121.            
  122.             a1 = ByteMultiArray(data=[self.power1, self.angle1, self.rotation1])
  123.             a2 = ByteMultiArray(data=[self.power2, self.angle2,self.rotation2])
  124.            
  125.             self.pub1.publish(a1)
  126.             self.pub2.publish(a2)
  127.            
  128.             time.sleep(0.2)
  129.             self.power1 = 0
  130.             self.power2 = 0
  131.             self.rotation1 = 0
  132.             self.rotation2 = 0
  133.            
  134.             a1 = ByteMultiArray(data=[self.power1, self.angle1, self.rotation1])
  135.             a2 = ByteMultiArray(data=[self.power2, self.angle2,self.rotation2])
  136.            
  137.             self.pub1.publish(a1)
  138.             self.pub2.publish(a2)
  139.            
  140.             time.sleep(0.2)
  141.        
  142.     def regulacija(self,sign1,sign2,sign_rot):
  143.         x = self.pose1.x + 45*sin(self.pose1.theta*0.01745329)            
  144.         y = self.pose1.y - 45*cos(self.pose1.theta*0.01745329)
  145.         deltax2 = x - self.pose2.x
  146.         deltay2 = y - self.pose2.y
  147.         self.dist = sqrt(deltax2*deltax2 + deltay2*deltay2)
  148.         an2 = -atan2(deltax2, deltay2)/0.01745329
  149.        
  150.         if self.pose2.theta < 0 :
  151.             an1 = 360 + self.pose2.theta              
  152.         else:
  153.             an1 = self.pose2.theta
  154.        
  155.         if  an2<0:
  156.             an2 = 360 + an2
  157.                
  158.         self.angle2 = an2-an1          
  159.                
  160.         if abs(self.angle2) >180:
  161.            self.angle2 = copysign( 360-abs(self.angle2), (-1)*self.angle2)        
  162.  
  163.         if self.dist < 0.5:
  164.             self.power2 = 45
  165.                
  166.         elif self.dist < 0.3:
  167.             self.power2 = 30
  168.  
  169.         else:
  170.             self.power2 = 55
  171.              
  172.         if sign_rot==-1:
  173.        
  174.             if self.pose2.theta-self.pose1.theta>5 or  :
  175.                 print('-----situacija 1')
  176.                 self.rotation2=sign_rot*27
  177.                 self.rotation1=sign_rot*22
  178.                 self.power2=sign2*37
  179.                 self.power1=sign1*37
  180.  
  181.             elif self.pose1.theta-self.pose2.theta>5 or :
  182.                 print('-----situacija 2')
  183.                 self.rotation2=sign_rot*17
  184.                 self.rotation1=sign_rot*23
  185.                 self.power2=sign2*32
  186.                 self.power1=sign1*32
  187.  
  188.         if sign_rot==1:
  189.  
  190.             if abs(self.pose2.theta-self.pose1.theta)>5:
  191.                 print('-----situacija 3')
  192.                 self.rotation2=sign_rot*27
  193.                 self.rotation1=sign_rot*22
  194.                 self.power2=sign2*37
  195.                 self.power1=sign1*37
  196.  
  197.             elif abs(self.pose1.theta-self.pose2.theta)>5:
  198.                 print('-----situacija 4')
  199.                 self.rotation2=sign_rot*23
  200.                 self.rotation1=sign_rot*17
  201.                 self.power2=sign2*30
  202.                 self.power1=sign1*27
  203.                                
  204.        
  205.     def run(self):
  206.         s1 = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
  207.         s2 = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
  208.  
  209.         s1.connect((self.IP1, self.Port))
  210.         s2.connect((self.IP2, self.Port))
  211.        
  212.         'odmicanje od tunela kad je bocno'
  213.         if (((self.pose2.x < self.marker.x + 0.35) or (self.pose2.x < self.marker2.x - 0.35)) and self.pose2.y < self.marker2.y and self.pose2.y > (self.marker.y - 35) and abs(self.pose1.theta) > 90):
  214.             self.power1 = 50
  215.             self.power2 = 50
  216.             self.angle1 = 0
  217.             self.angle2 = 0
  218.             self.rotation1 = 0
  219.             self.rotation2 = 0
  220.            
  221.             a1 = ByteMultiArray(data=[self.power1, self.angle1, self.rotation1])
  222.             a2 = ByteMultiArray(data=[self.power2, self.angle2,self.rotation2])
  223.            
  224.             self.pub1.publish(a1)
  225.             self.pub2.publish(a2)
  226.            
  227.             time.sleep(0.2)
  228.  
  229.             self.power1 = 0
  230.             self.power2 = 0
  231.             self.rotation1 = 0
  232.             self.rotation2 = 0
  233.            
  234.             a1 = ByteMultiArray(data=[self.power1, self.angle1, self.rotation1])
  235.             a2 = ByteMultiArray(data=[self.power2, self.angle2,self.rotation2])
  236.            
  237.             self.pub1.publish(a1)
  238.             self.pub2.publish(a2)
  239.            
  240.             time.sleep(0.2)
  241.  
  242.         if (((self.pose1.x < self.marker.x + 0.35) or (self.pose1.x < self.marker2.x - 0.35)) and self.pose1.y < self.marker2.y and self.pose1.y > (self.marker.y - 35) and abs(self.pose1.theta) > 90):
  243.             self.power1 = 50
  244.             self.power2 = 50
  245.             self.angle1 = 180
  246.             self.angle2 = 180
  247.             self.rotation1 = 0
  248.             self.rotation2 = 0
  249.            
  250.             a1 = ByteMultiArray(data=[self.power1, self.angle1, self.rotation1])
  251.             a2 = ByteMultiArray(data=[self.power2, self.angle2,self.rotation2])
  252.            
  253.             self.pub1.publish(a1)
  254.             self.pub2.publish(a2)
  255.            
  256.             time.sleep(0.2)
  257.  
  258.             self.power1 = 0
  259.             self.power2 = 0
  260.             self.rotation1 = 0
  261.             self.rotation2 = 0
  262.            
  263.             a1 = ByteMultiArray(data=[self.power1, self.angle1, self.rotation1])
  264.             a2 = ByteMultiArray(data=[self.power2, self.angle2,self.rotation2])
  265.            
  266.             self.pub1.publish(a1)
  267.             self.pub2.publish(a2)
  268.            
  269.             time.sleep(0.2)
  270.         'odmicanje od tunela kad je iznad'
  271.         self.odmicanje(s1,s2)
  272.         'rotacija'
  273.         if self.pose1.x < self.pose2.x:
  274.             predznak_1 = -1
  275.             predznak_2 = 1
  276.             predznak_rot = -1
  277.             angle1 = -90
  278.             angle2 = 90
  279.            
  280.         else:
  281.             predznak_1 = 1
  282.             predznak_2 = -1
  283.             predznak_rot = 1
  284.             angle1 = 90
  285.             angle2 = -90
  286.  
  287.         print(self.pose1.x)
  288.         while not(rospy.is_shutdown()) and self.switch4 == False:
  289.             while abs(self.pose1.theta) > 10:
  290.                 print('tu sam')
  291.                 self.rotation1 = predznak_rot*20
  292.                 self.rotation2 = predznak_rot*20
  293.                 self.power1    = predznak_1*24
  294.                 self.power2    = predznak_2*35
  295.                 self.angle1    = angle1
  296.                 self.angle2    = angle2
  297.                 self.regulacija(predznak_1,predznak_2,predznak_rot)
  298.                 #print('theta 1.- ' + str(self.pose1.theta))
  299.                 #print('theta 2.- ' + str(self.pose2.theta))
  300.                 #print('pozicija 1.- ' + str(self.pose1.x))
  301.                 #print('pozicija 2.- ' + str(self.pose2.x))
  302.                 a1 = ByteMultiArray(data=[self.power1, self.angle1, self.rotation1])
  303.                 a2 = ByteMultiArray(data=[self.power2, self.angle2,self.rotation2])
  304.            
  305.                 self.pub1.publish(a1)
  306.                 self.pub2.publish(a2)
  307.            
  308.                 time.sleep(0.2)
  309.             print('nakon 1. petlje')
  310.             self.power1     = 0
  311.             self.power2     = 0
  312.             self.angle1     = 0
  313.             self.angle2     = 0
  314.             self.rotation1  = 0
  315.             self.rotation2  = 0
  316.             a1 = ByteMultiArray(data=[self.power1, self.angle1, self.rotation1])
  317.             a2 = ByteMultiArray(data=[self.power2, self.angle2,self.rotation2])
  318.            
  319.             self.pub1.publish(a1)
  320.             self.pub2.publish(a2)
  321.            
  322.             time.sleep(0.2)
  323.             self.switch4 = True
  324.             print('ispred breaka')          
  325.  
  326.  
  327.  
  328.         if self.pose1.x < self.pose2.x:
  329.             predznak_1      = -1
  330.             predznak_2      = 1
  331.             predznak_rot    = -1
  332.         else:
  333.             predznak_1      = 1
  334.             predznak_2      = -1
  335.             predznak_rot    = 1
  336.  
  337.         while not(rospy.is_shutdown()) and self.switch0 == False:
  338.             while abs(self.pose1.theta) > 10:
  339.                 pow1 = 30
  340.                 pow2 = 24
  341.                 rot1 = predznak_rot * 20
  342.                 rot2 = predznak_rot * 20
  343.                 ang1 = predznak_1 * 90
  344.                 ang2 = predznak_2 * 90
  345.  
  346.                 self.niz1()
  347.                 self.niz2()
  348.             self.switch0=True
  349.            
  350.         'izlazi iz tunela'
  351.         while not(rospy.is_shutdown()) and self.switch5 == False:
  352.             while (abs(self.pose1.theta) > 3) or ( abs(self.pose2.theta - self.pose1.theta) > 3 ):
  353.                 self.power1 = 0
  354.                 self.power2 = 0
  355.                 self.rotation1 = -int(self.pose1.theta*(100./180))
  356.                 self.rotation2 = -int(self.pose2.theta*(100./180))
  357.                                
  358.                 a1 = ByteMultiArray(data=[self.power1, self.angle1, self.rotation1])
  359.                 a2 = ByteMultiArray(data=[self.power2, self.angle2,self.rotation2])
  360.                
  361.                 self.pub1.publish(a1)
  362.                 self.pub2.publish(a2)
  363.                
  364.                 time.sleep(0.2)
  365.             while ( (self.pose1.y - self.pose2.y) > 0.49 or  (self.pose1.y - self.pose2.y) < 0.41 ):
  366.                 print "poravnava po y"
  367.                 self.power1 = 0
  368.                 self.power2 = 0
  369.                 self.angle1 = 0
  370.                 self.angle2 = 0
  371.                 self.rotation1 = 0
  372.                 self.rotation2 = 0
  373.                
  374.                 a1 = ByteMultiArray(data=[self.power1, self.angle1, self.rotation1])
  375.                 a2 = ByteMultiArray(data=[self.power2, self.angle2,self.rotation2])
  376.                
  377.                 self.pub1.publish(a1)
  378.                 self.pub2.publish(a2)
  379.                
  380.                 time.sleep(0.2)
  381.                 if ( (self.pose1.y - self.pose2.y) > 0.49 ):
  382.                     self.angle2 = 0
  383.                 else:
  384.                     self.angle2 = 180
  385.                 self.power2 = 20
  386.                 self.rotation1 = 0
  387.                 self.rotation2 = 0
  388.                
  389.                 a1 = ByteMultiArray(data=[self.power1, self.angle1, self.rotation1])
  390.                 a2 = ByteMultiArray(data=[self.power2, self.angle2,self.rotation2])
  391.                
  392.                 self.pub1.publish(a1)
  393.                 self.pub2.publish(a2)
  394.                
  395.                 time.sleep(0.2)
  396.             while (abs(self.pose1.x - self.pose2.x) > 0.02):
  397.                 print "ravna po x"
  398.                 self.angle2 = copysign( 90 , self.pose2.x - self.pose1.x)
  399.                 self.power1 = 0
  400.                 self.power2 = 20
  401.                 self.rotation1 = 0
  402.                 self.rotation2 = 0
  403.                
  404.                 a1 = ByteMultiArray(data=[self.power1, self.angle1, self.rotation1])
  405.                 a2 = ByteMultiArray(data=[self.power2, self.angle2,self.rotation2])
  406.                
  407.                 self.pub1.publish(a1)
  408.                 self.pub2.publish(a2)
  409.                
  410.                 time.sleep(0.2)
  411.                
  412.             if (self.marker.x > self.marker2.x):                   #ako ulaz u tunel ima vecu x koordinatu od izlaza
  413.                 while (self.pose1.x < (self.marker.x + 0.1) and self.pose1.x > (self.marker.x + self.marker2.x)/2) or (self.pose1.x < (self.marker.x + self.marker2.x)/2 and self.pose1.x > self.marker2.x - 0.1):                                   #ponasanje ako je hexapod u ravnini tunela
  414.                     self.power1 = 35
  415.                     self.power2 = 35
  416.                     if (self.pose1.x < (self.marker.x + 0.1) and self.pose1.x > (self.marker.x + self.marker2.x)/2):
  417.                         self.angle1 = -90
  418.                         self.angle2 = -90
  419.                     else:
  420.                         self.angle1 = 90
  421.                         self.angle2 = 90
  422.                     self.rotation1 = 0
  423.                     self.rotation2 = 0
  424.                    
  425.                     a1 = ByteMultiArray(data=[self.power1, self.angle1, self.rotation1])
  426.                     a2 = ByteMultiArray(data=[self.power2, self.angle2,self.rotation2])
  427.                    
  428.                     self.pub1.publish(a1)
  429.                     self.pub2.publish(a2)
  430.                    
  431.                     time.sleep(0.2)
  432.             else:                                                 #ako izlaz iz tunela ima vecu x koordinatu od ulaza
  433.                 while (self.pose1.x < (self.marker2.x + 0.1) and self.pose1.x > (self.marker.x + self.marker2.x)/2) or (self.pose1.x < (self.marker.x + self.marker2.x)/2 and self.pose1.x > self.marker.x - 0.1):
  434.                     self.power1 = 35
  435.                     self.power2 = 35
  436.                     if (self.pose1.x < (self.marker2.x + 0.1) and self.pose1.x > (self.marker.x + self.marker2.x)/2):
  437.                         self.angle1 = -90
  438.                         self.angle2 = -90
  439.                     else:
  440.                         self.angle1 = 90
  441.                         self.angle2 = 90
  442.                     self.rotation1 = 0
  443.                     self.rotation2 = 0
  444.                    
  445.                     a1 = ByteMultiArray(data=[self.power1, self.angle1, self.rotation1])
  446.                     a2 = ByteMultiArray(data=[self.power2, self.angle2,self.rotation2])
  447.                    
  448.                     self.pub1.publish(a1)
  449.                     self.pub2.publish(a2)
  450.                    
  451.                     time.sleep(0.2)
  452.  
  453.         'spusta po y'
  454.         while not(rospy.is_shutdown()) and self.switch1 == False:
  455.             while (abs(self.pose1.theta) > 5) or ( abs(self.pose2.theta - self.pose1.theta) > 5 ):
  456.                 print "ispravljam kut"
  457.                 self.power1 = 0
  458.                 self.power2 = 0
  459.                 self.rotation1 = -int(self.pose1.theta*(100./180))
  460.                 self.rotation2 = -int(self.pose2.theta*(100./180))
  461.                
  462.                 print "theta1"
  463.                 print self.pose1.theta
  464.                 print "rot1"
  465.                 print self.rotation1
  466.              
  467.                 a1 = ByteMultiArray(data=[self.power1, self.angle1, self.rotation1])
  468.                 a2 = ByteMultiArray(data=[self.power2, self.angle2,self.rotation2])
  469.                
  470.                 self.pub1.publish(a1)
  471.                 self.pub2.publish(a2)
  472.                
  473.                 time.sleep(0.2)                          
  474.             while (abs(self.pose1.x - self.pose2.x) > 0.02):
  475.                 print "ravna po x"
  476.                 self.angle2 = copysign( 90 , self.pose2.x - self.pose1.x)
  477.                 self.power1 = 0
  478.                 self.power2 = 20
  479.                 self.rotation1 = 0
  480.                 self.rotation2 = 0
  481.                
  482.                 a1 = ByteMultiArray(data=[self.power1, self.angle1, self.rotation1])
  483.                 a2 = ByteMultiArray(data=[self.power2, self.angle2,self.rotation2])
  484.                
  485.                 self.pub1.publish(a1)
  486.                 self.pub2.publish(a2)
  487.                
  488.                 time.sleep(0.2)
  489.             while ( (self.pose1.y - self.pose2.y) > 0.49 or  (self.pose1.y - self.pose2.y) < 0.41 ):
  490.                 print "poravnava po y"
  491.                 if ( (self.pose1.y - self.pose2.y) > 0.49 ):
  492.                     self.angle2 = 0
  493.                 else:
  494.                     self.angle2 = 180
  495.                 self.power1 = 0
  496.                 self.power2 = 10
  497.                 self.rotation1 = 0
  498.                 self.rotation2 = 0
  499.                
  500.                 a1 = ByteMultiArray(data=[self.power1, self.angle1, self.rotation1])
  501.                 a2 = ByteMultiArray(data=[self.power2, self.angle2,self.rotation2])
  502.                
  503.                 self.pub1.publish(a1)
  504.                 self.pub2.publish(a2)
  505.                
  506.                 time.sleep(0.2)
  507.          
  508.             deltay = self.marker.y - self.pose1.y
  509.             if (deltay > 0.25):
  510.                 print "stoj "
  511.                 self.power1 = 0
  512.                 self.power2 =  0
  513.                 self.switch1 = True
  514.             else:
  515.                 print "hodaj"
  516.                 self.power1 = 35
  517.                 self.power2 = 35
  518.             self.angle1 = 180
  519.             self.angle2 = 180
  520.             self.rotation1 = 0
  521.             self.rotation2 = 0
  522.            
  523.             a1 = ByteMultiArray(data=[self.power1, self.angle1, self.rotation1])
  524.             a2 = ByteMultiArray(data=[self.power2, self.angle2,self.rotation2])
  525.            
  526.             self.pub1.publish(a1)
  527.             self.pub2.publish(a2)
  528.            
  529.             time.sleep(0.2)
  530.        
  531.         self.rotation1 = 0
  532.         self.rotation2 = 0
  533.         self.power1 = 0
  534.         self.power2 = 0
  535.            
  536.         a1 = ByteMultiArray(data=[self.power1, self.angle1, self.rotation1])
  537.         a2 = ByteMultiArray(data=[self.power2, self.angle2,self.rotation2])
  538.            
  539.         self.pub1.publish(a1)
  540.         self.pub2.publish(a2)
  541.            
  542.         time.sleep(0.2)
  543.        
  544.         ' prva tocka '
  545.         print "prva tocka"
  546.        
  547.         while (abs(self.pose1.theta) > 5) or ( abs(self.pose2.theta - self.pose1.theta) > 5 ):
  548.                 print "popravljam dvicu"
  549.                 self.power1 = 0
  550.                 self.power2 = 0
  551.                 self.rotation1 = -int(self.pose1.theta*(100./180))
  552.                 self.rotation2 = -int(self.pose2.theta*(100./180))
  553.                                
  554.                 a1 = ByteMultiArray(data=[self.power1, self.angle1, self.rotation1])
  555.                 a2 = ByteMultiArray(data=[self.power2, self.angle2,self.rotation2])
  556.                
  557.                 self.pub1.publish(a1)
  558.                 self.pub2.publish(a2)
  559.                
  560.                 time.sleep(0.2)
  561.         while ( (self.pose1.y - self.pose2.y) > 0.49 or  (self.pose1.y - self.pose2.y) < 0.41 ):
  562.                 print "poravnava po y"
  563.                 if ( (self.pose1.y - self.pose2.y) > 0.49 ):
  564.                     self.angle2 = 0
  565.                 else:
  566.                     self.angle2 = 180
  567.                 self.power1 = 10
  568.                 self.power2 = 20
  569.                 self.rotation1 = 0
  570.                 self.rotation2 = 0
  571.                
  572.                 a1 = ByteMultiArray(data=[self.power1, self.angle1, self.rotation1])
  573.                 a2 = ByteMultiArray(data=[self.power2, self.angle2,self.rotation2])
  574.                
  575.                 self.pub1.publish(a1)
  576.                 self.pub2.publish(a2)
  577.                
  578.                 time.sleep(0.2)
  579.         while (abs(self.pose1.x - self.pose2.x) > 0.02):
  580.                 print "ravna po x"
  581.  
  582.                 self.angle2 = copysign( 90 , self.pose2.x - self.pose1.x)
  583.                 self.power1 = 10
  584.                 self.power2 = 20
  585.                 self.rotation1 = 0
  586.                 self.rotation2 = 0
  587.                
  588.                 a1 = ByteMultiArray(data=[self.power1, self.angle1, self.rotation1])
  589.                 a2 = ByteMultiArray(data=[self.power2, self.angle2,self.rotation2])
  590.                
  591.                 self.pub1.publish(a1)
  592.                 self.pub2.publish(a2)
  593.                
  594.                 time.sleep(0.2)
  595.        
  596.         'micanje po x'
  597.         while not(rospy.is_shutdown()) and self.switch2 == False:
  598.             while (abs(self.pose1.theta) > 3) or ( abs(self.pose2.theta - self.pose1.theta) > 3 ):
  599.                 self.power1 = 0
  600.                 self.power2 = 0
  601.                 self.rotation1 = -int(self.pose1.theta*(100./180))
  602.                 self.rotation2 = -int(self.pose2.theta*(100./180))
  603.                
  604.                 a1 = ByteMultiArray(data=[self.power1, self.angle1, self.rotation1])
  605.                 a2 = ByteMultiArray(data=[self.power2, self.angle2,self.rotation2])
  606.                
  607.                 self.pub1.publish(a1)
  608.                 self.pub2.publish(a2)
  609.                
  610.                 time.sleep(0.2)
  611.             while ( (self.pose1.y - self.pose2.y) > 0.49 or  (self.pose1.y - self.pose2.y) < 0.41 ):
  612.                 print "poravnava po y(ou suck)"
  613.                 if ( (self.pose1.y - self.pose2.y) > 0.49 ):
  614.                     self.angle2 = 0
  615.                 else:
  616.                     self.angle2 = 180
  617.                 self.power1 = 0
  618.                 self.power2 = 20
  619.                 self.rotation1 = 0
  620.                 self.rotation2 = 0
  621.                
  622.                 a1 = ByteMultiArray(data=[self.power1, self.angle1, self.rotation1])
  623.                 a2 = ByteMultiArray(data=[self.power2, self.angle2,self.rotation2])
  624.                
  625.                 self.pub1.publish(a1)
  626.                 self.pub2.publish(a2)
  627.                
  628.                 time.sleep(0.2)
  629.                
  630.             while (abs(self.pose1.x - self.pose2.x) > 0.03):
  631.                 print "ravna po x"
  632.  
  633.                 self.angle2 = copysign( 90 , self.pose2.x - self.pose1.x)
  634.                 self.power1 = 0
  635.                 self.power2 = 20
  636.                 self.rotation1 = 0
  637.                 self.rotation2 = 0
  638.                
  639.                 a1 = ByteMultiArray(data=[self.power1, self.angle1, self.rotation1])
  640.                 a2 = ByteMultiArray(data=[self.power2, self.angle2,self.rotation2])
  641.                
  642.                 self.pub1.publish(a1)
  643.                 self.pub2.publish(a2)
  644.                
  645.                 time.sleep(0.2)  
  646.            
  647.             self.power1 = 0
  648.             self.power2 = 0
  649.             self.rotation1 = 0
  650.             self.rotation2 = 0
  651.            
  652.             a1 = ByteMultiArray(data=[self.power1, self.angle1, self.rotation1])
  653.             a2 = ByteMultiArray(data=[self.power2, self.angle2,self.rotation2])
  654.            
  655.             self.pub1.publish(a1)
  656.             self.pub2.publish(a2)
  657.            
  658.             time.sleep(0.2)
  659.            
  660.                  
  661.             deltax = abs(((self.marker2.x + self.marker.x)/2) - self.pose1.x)
  662.             print "deltax: "
  663.             print deltax
  664.             razlika = self.pose1.x - self.pose2.x
  665.             print "razlika: "
  666.             print razlika
  667.             #print "lajna po iksici"
  668.             if (((self.marker2.x + self.marker.x)/2) > self.pose1.x):
  669.                 self.angle1 = -90
  670.                 self.angle2 = -90
  671.             else:
  672.                 self.angle1 = 90
  673.                 self.angle2 = 90
  674.             if (deltax < 0.05):
  675.                 self.power1  = 0
  676.                 self.power2  =  0
  677.                 self.switch2 = True  
  678.             else:
  679.                 self.power1 = 35
  680.                 self.power2 = 35
  681.             self.rotation1 = 0
  682.             self.rotation2 = 0
  683.            
  684.             a1 = ByteMultiArray(data=[self.power1, self.angle1, self.rotation1])
  685.             a2 = ByteMultiArray(data=[self.power2, self.angle2,self.rotation2])
  686.            
  687.             self.pub1.publish(a1)
  688.             self.pub2.publish(a2)
  689.            
  690.             time.sleep(0.2)
  691.        
  692.        
  693.        
  694.         self.rotation1 = 0
  695.         self.rotation2 = 0
  696.         self.power1 = 0
  697.         self.power2 = 0
  698.        
  699.         a1 = ByteMultiArray(data=[self.power1, self.angle1, self.rotation1])
  700.         a2 = ByteMultiArray(data=[self.power2, self.angle2,self.rotation2])
  701.            
  702.         self.pub1.publish(a1)
  703.         self.pub2.publish(a2)
  704.            
  705.         time.sleep(0.2)
  706.        
  707.        
  708.        
  709.         ' druga tocka'
  710.        
  711.        
  712.         while (abs(self.pose1.theta) > 5) or ( abs(self.pose2.theta - self.pose1.theta) > 5 ):
  713.                 self.power1 = 0
  714.                 self.power2 = 0
  715.                 self.rotation1 = -int(self.pose1.theta*(100./180))
  716.                 self.rotation2 = -int(self.pose2.theta*(100./180))
  717.                
  718.                 a1 = ByteMultiArray(data=[self.power1, self.angle1, self.rotation1])
  719.                 a2 = ByteMultiArray(data=[self.power2, self.angle2,self.rotation2])
  720.                
  721.                 self.pub1.publish(a1)
  722.                 self.pub2.publish(a2)
  723.                
  724.                 time.sleep(0.2)  
  725.         while ( (self.pose1.y - self.pose2.y) > 0.49 or  (self.pose1.y - self.pose2.y) < 0.41 ):
  726.                 print "poravnava po y"
  727.                 if ( (self.pose1.y - self.pose2.y) > 0.49 ):
  728.                     self.angle2 = 0
  729.                 else:
  730.                     self.angle2 = 180
  731.                 self.power1 = 10
  732.                 self.power2 = 20
  733.                 self.rotation1 = 0
  734.                 self.rotation2 = 0
  735.                
  736.                 a1 = ByteMultiArray(data=[self.power1, self.angle1, self.rotation1])
  737.                 a2 = ByteMultiArray(data=[self.power2, self.angle2,self.rotation2])
  738.                
  739.                 self.pub1.publish(a1)
  740.                 self.pub2.publish(a2)
  741.                
  742.                 time.sleep(0.2)  
  743.        
  744.         while (abs(self.pose1.x - self.pose2.x) > 0.02):
  745.                 print "ravna po x"
  746.                 self.angle2 = copysign( 90 , self.pose2.x - self.pose1.x)
  747.                 self.power1 = 10
  748.                 self.power2 = 20
  749.                 self.rotation1 = 0
  750.                 self.rotation2 = 0
  751.                
  752.                 a1 = ByteMultiArray(data=[self.power1, self.angle1, self.rotation1])
  753.                 a2 = ByteMultiArray(data=[self.power2, self.angle2,self.rotation2])
  754.                
  755.                 self.pub1.publish(a1)
  756.                 self.pub2.publish(a2)
  757.                
  758.                 time.sleep(0.2)
  759.        
  760.                
  761.         ' prolazi kroz tunel '
  762.         while not(rospy.is_shutdown()) and self.switch3 == False:
  763.             while (abs(self.pose1.theta) > 5) or ( abs(self.pose2.theta - self.pose1.theta) > 5 ):
  764.                 self.power1 = 0
  765.                 self.power2 = 0
  766.                 self.rotation1 = -int(self.pose1.theta*(100./180))
  767.                 self.rotation2 = -int(self.pose2.theta*(100./180))
  768.                
  769.                 a1 = ByteMultiArray(data=[self.power1, self.angle1, self.rotation1])
  770.                 a2 = ByteMultiArray(data=[self.power2, self.angle2,self.rotation2])
  771.                
  772.                 self.pub1.publish(a1)
  773.                 self.pub2.publish(a2)
  774.                
  775.                 time.sleep(0.2)
  776.                
  777.             while (abs(self.pose1.x - self.pose2.x) > 0.02):
  778.                 print "ravna po x"
  779.                 self.power1 = 0
  780.                 self.angle2 = copysign( 90 , self.pose2.x - self.pose1.x)
  781.                 self.power2 = 15
  782.                 self.rotation1 = 0
  783.                 self.rotation2 = 0
  784.                
  785.                 a1 = ByteMultiArray(data=[self.power1, self.angle1, self.rotation1])
  786.                 a2 = ByteMultiArray(data=[self.power2, self.angle2,self.rotation2])
  787.                
  788.                 self.pub1.publish(a1)
  789.                 self.pub2.publish(a2)
  790.                
  791.                 time.sleep(0.2)
  792.             while ( (self.pose1.y - self.pose2.y) > 0.49 or  (self.pose1.y - self.pose2.y) < 0.41 ):
  793.                 print "poravnava po y"
  794.                 if ( (self.pose1.y - self.pose2.y) > 0.49 ):
  795.                     self.angle2 = 0
  796.                 else:
  797.                     self.angle2 = 180
  798.                 self.power1 = 0
  799.                 self.power2 = 10
  800.                 self.rotation1 = 0
  801.                 self.rotation2 = 0
  802.                
  803.                 a1 = ByteMultiArray(data=[self.power1, self.angle1, self.rotation1])
  804.                 a2 = ByteMultiArray(data=[self.power2, self.angle2,self.rotation2])
  805.                
  806.                 self.pub1.publish(a1)
  807.                 self.pub2.publish(a2)
  808.                
  809.                 time.sleep(0.2)        
  810.            
  811.            
  812.             print "samo gore misko"
  813.             if (self.marker2.y + 0.2 < self.pose2.y):
  814.                 self.power1  = 0
  815.                 self.power2  =  0
  816.                 self.switch3 = True
  817.             else:
  818.                 self.power1 = 35
  819.                 self.power2 = 35
  820.             self.angle1 = 0
  821.             self.angle2 = 0
  822.             self.rotation1 = 0
  823.             self.rotation2 = 0
  824.            
  825.             a1 = ByteMultiArray(data=[self.power1, self.angle1, self.rotation1])
  826.             a2 = ByteMultiArray(data=[self.power2, self.angle2,self.rotation2])
  827.            
  828.             self.pub1.publish(a1)
  829.             self.pub2.publish(a2)
  830.            
  831.             time.sleep(0.2)
  832.            
  833.         print "stoooooooooooooj"
  834.         self.power1 = 0
  835.         self.power2 = 0
  836.         self.rotation1 = 0
  837.         self.rotation2 = 0
  838.            
  839.         a1 = ByteMultiArray(data=[self.power1, self.angle1, self.rotation1])
  840.         a2 = ByteMultiArray(data=[self.power2, self.angle2,self.rotation2])
  841.            
  842.         self.pub1.publish(a1)
  843.         self.pub2.publish(a2)
  844.            
  845.         time.sleep(0.2)
  846.         sys.exit()
  847.        
  848.        
  849.     def niz1(self):
  850.  
  851.         self.paket1=pack('cccbbbbbbbbbbbbbbbbbBB','P','K','T', self.power1,(self.angle1/2), self.rotation1, self.staticTilt, self.movingTilt, self.onoff1, self.accX, self.accY, self.sliders1, self.sliders2, self.sliders3, self.sliders4, self.sliders5, self.sliders6, self.sliders7, self.sliders8, self.sliders9, (self.duration/256), (self.duration%256))
  852.  
  853.     def niz2(self):
  854.  
  855.         self.paket2=pack('cccbbbbbbbbbbbbbbbbbBB','P','K','T', self.power2,(self.angle2/2), self.rotation2, self.staticTilt, self.movingTilt, self.onoff2, self.accX, self.accY, self.sliders1, self.sliders2, self.sliders3, self.sliders4, self.sliders5, self.sliders6, self.sliders7, self.sliders8, self.sliders9, (self.duration/256), (self.duration%256))
  856.    
  857.    
  858.     def nula(self):
  859.         s1 = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
  860.         s2 = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
  861.         s1.connect((self.IP1, self.Port))
  862.         s2.connect((self.IP2, self.Port))
  863.         self.power1     = 0
  864.         self.angle1     = 0
  865.         self.power2     = 0
  866.         self.angle2     = 0
  867.         self.rotation1  = 0
  868.         self.rotation2  = 0
  869.        
  870.         a1 = ByteMultiArray(data=[self.power1, self.angle1, self.rotation1])
  871.         a2 = ByteMultiArray(data=[self.power2, self.angle2,self.rotation2])
  872.            
  873.         self.pub1.publish(a1)
  874.         self.pub2.publish(a2)
  875.            
  876.         time.sleep(0.2)
  877.        
  878. if __name__ == '__main__':
  879.    
  880.     try:
  881.         rospy.init_node('Control', anonymous = True)
  882.         Hex = HexaGuide()
  883.         Hex.nula()
  884.         Hex.run()
  885.         rospy.spin()
  886.     except (rospy.ROSInterruptException,KeyboardInterrupt):
  887.         Hex.nula()
  888.         s1.close()
  889.         s2.close()
  890.         sys.exit()
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!