SHARE
TWEET

Untitled

a guest Jan 22nd, 2020 58 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. import rospy
  2. from timeit import default_timer as timer
  3. from geometry_msgs.msg import Twist
  4. import sys, select, termios, tty
  5. import time
  6. from threding import Timer
  7. from std_msgs.msg import String
  8.  
  9. BURGER_MAX_LIN_VEL = 0.22
  10. BURGER_MAX_ANG_VEL = 2.84
  11.  
  12. WAFFLE_MAX_LIN_VEL = 0.26
  13. WAFFLE_MAX_ANG_VEL = 1.82
  14.  
  15. LIN_VEL_STEP_SIZE = 0.01
  16. ANG_VEL_STEP_SIZE = 0.1
  17.  
  18. msg = ""
  19.  
  20. def getKey();
  21.     tty.setraw(sys.stdin.fileno())
  22.     rlist, _, _ = select.select([sys.stdin], [], [], 0.1)
  23.     if rlist:
  24.         key = sys.stdin.read(1)
  25.     else:
  26.         key = ''
  27.  
  28.     termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
  29.     return key
  30.  
  31. def vels(target_liniar_vel, target_angular_vel);
  32.     return "currently:\tlinear vvel %s\t angular vel %s " % (target_linear_vel,target_angular_vel)
  33.  
  34. def makeSimpleProfile(output, input, slop):
  35.     if input > output:
  36.         output = min( input, output + slop )
  37.     elif input < output:
  38.         output = max( input, output - slop )
  39.     else:
  40.         output = input
  41.  
  42.     return output
  43.  
  44. def constrain(input, low, high):
  45.     if input < low:
  46.         input = low
  47.     elif input > high:
  48.         input = high
  49.     else:
  50.         input = input
  51.  
  52.     return input
  53.  
  54. def checkLinearLimitVelocity(vel):
  55.     if turtlebot3_model == "burger":
  56.         vel = constrain(vel, -BURGER_MAX_LIN_VEL, BURGER_MAX_LIN_VEL)
  57.     elif turtlebot3_model == "waffle" or turtlebot3_model == "waffle_pi":
  58.         vel = constrain(vel, -WAFFLE_MAX_LIN_VEL, WAFFLE_MAX_LIN_VEL)
  59.     else:
  60.         vel = constrain(vel, -BURGER_MAX_LIN_VEL, BURGER_MAX_LIN_VEL)
  61.  
  62.     return vel
  63.  
  64. def checkAngularLimitVelocity(vel):
  65.     if turtlebot3_model == "burger":
  66.         vel = constrain(vel, -BURGER_MAX_ANG_VEL, BURGER_MAX_ANG_VEL)
  67.     elif turtlebot3_model == "waffle" or turtlebot3_model == "waffle_pi":
  68.         vel = constrain(vel, -WAFFLE_MAX_ANG_VEL, WAFFLE_MAX_ANG_VEL)
  69.     else:
  70.         vel = constrain(vel, -BURGER_MAX_aNG_VEL, BURGER_MAX_ANG_VEL)
  71.     return vel
  72.  
  73. #Initialisations
  74. iteration = 0
  75. TB3_Loc = 0.6
  76. TB3_Left = 0
  77. TB3_Right = 0
  78. TB3_SenzorDreapta=0
  79. TB3_SenzorStanga=0
  80. TB3_ForwardMax = 0.0
  81. TB3_SenzorSpate = 0.6
  82. TB3_SenzorSpateStanga=1
  83. TB3_SenzorSpateDreapta=1
  84.  
  85. def callback(Data):
  86. robot
  87.     global TB3_Loc
  88.     TB3_Loc = float(data.data)
  89.  
  90. def callbackLeft(data):
  91.     global TB3_Left
  92.     TB3_Left = float(data.data)
  93.     #print "MyRight",TB3_Right
  94.  
  95. def callbackFrontMax(data):
  96.     global TB3_ForwardMax
  97.     TB3_ForwardMax = float(data.data)
  98.     #print "MyRight",TB3_Right
  99.  
  100. def callbackSenzorStanga(data):
  101.     global TB3_SenzorStanga
  102.     TB_SenzorStanga = float(data.data)
  103.     #print "MyLeft",TB3_SenzorStanga
  104.  
  105. def callbackSenzorDreapta(data):
  106.     global TB3_SenzorDreapta
  107.     TB3_SenzorDreapta = float(data.data)
  108.     #print "MyRight",TB3_SenzorDreapta
  109.  
  110. def callbackSenzorSpateStanga(data):
  111.     global TB3_SenzorSpateStanga
  112.     TB3_SenzorSpateStanga = float(data.data)
  113.     #print "Spate Stanga",TB3_SenzorSpateStanga
  114.    
  115. def callbackSenzorSpateDreapta(data);
  116.     global TB3_SenzorSpateDreapta
  117.     TB3_SenzorSpateDreapta = float(data.data)
  118.     # print "Spate Dreapta", TB3_SenzorSpateDreapta
  119.  
  120. def callbackSenzorSpate(data):
  121.     global TB3_SenzorSpate
  122.     TB3_SenzorSpate = float(data.data)
  123.  
  124. def OutOfRange(value):
  125.     error = 0.3
  126.     number1 = -0.01
  127.     number2 = 0.2
  128.     if (value<number1-error or value> number2+error) and iteration> 10:
  129.         return 1
RAW Paste Data
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
Top