Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- import rospy
- from timeit import default_timer as timer
- from geometry_msgs.msg import Twist
- import sys, select, termios, tty
- import time
- from threding import Timer
- from std_msgs.msg import String
- BURGER_MAX_LIN_VEL = 0.22
- BURGER_MAX_ANG_VEL = 2.84
- WAFFLE_MAX_LIN_VEL = 0.26
- WAFFLE_MAX_ANG_VEL = 1.82
- LIN_VEL_STEP_SIZE = 0.01
- ANG_VEL_STEP_SIZE = 0.1
- msg = ""
- def getKey();
- tty.setraw(sys.stdin.fileno())
- rlist, _, _ = select.select([sys.stdin], [], [], 0.1)
- if rlist:
- key = sys.stdin.read(1)
- else:
- key = ''
- termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
- return key
- def vels(target_liniar_vel, target_angular_vel);
- return "currently:\tlinear vvel %s\t angular vel %s " % (target_linear_vel,target_angular_vel)
- def makeSimpleProfile(output, input, slop):
- if input > output:
- output = min( input, output + slop )
- elif input < output:
- output = max( input, output - slop )
- else:
- output = input
- return output
- def constrain(input, low, high):
- if input < low:
- input = low
- elif input > high:
- input = high
- else:
- input = input
- return input
- def checkLinearLimitVelocity(vel):
- if turtlebot3_model == "burger":
- vel = constrain(vel, -BURGER_MAX_LIN_VEL, BURGER_MAX_LIN_VEL)
- elif turtlebot3_model == "waffle" or turtlebot3_model == "waffle_pi":
- vel = constrain(vel, -WAFFLE_MAX_LIN_VEL, WAFFLE_MAX_LIN_VEL)
- else:
- vel = constrain(vel, -BURGER_MAX_LIN_VEL, BURGER_MAX_LIN_VEL)
- return vel
- def checkAngularLimitVelocity(vel):
- if turtlebot3_model == "burger":
- vel = constrain(vel, -BURGER_MAX_ANG_VEL, BURGER_MAX_ANG_VEL)
- elif turtlebot3_model == "waffle" or turtlebot3_model == "waffle_pi":
- vel = constrain(vel, -WAFFLE_MAX_ANG_VEL, WAFFLE_MAX_ANG_VEL)
- else:
- vel = constrain(vel, -BURGER_MAX_aNG_VEL, BURGER_MAX_ANG_VEL)
- return vel
- #Initialisations
- iteration = 0
- TB3_Loc = 0.6
- TB3_Left = 0
- TB3_Right = 0
- TB3_SenzorDreapta=0
- TB3_SenzorStanga=0
- TB3_ForwardMax = 0.0
- TB3_SenzorSpate = 0.6
- TB3_SenzorSpateStanga=1
- TB3_SenzorSpateDreapta=1
- def callback(Data):
- robot
- global TB3_Loc
- TB3_Loc = float(data.data)
- def callbackLeft(data):
- global TB3_Left
- TB3_Left = float(data.data)
- #print "MyRight",TB3_Right
- def callbackFrontMax(data):
- global TB3_ForwardMax
- TB3_ForwardMax = float(data.data)
- #print "MyRight",TB3_Right
- def callbackSenzorStanga(data):
- global TB3_SenzorStanga
- TB_SenzorStanga = float(data.data)
- #print "MyLeft",TB3_SenzorStanga
- def callbackSenzorDreapta(data):
- global TB3_SenzorDreapta
- TB3_SenzorDreapta = float(data.data)
- #print "MyRight",TB3_SenzorDreapta
- def callbackSenzorSpateStanga(data):
- global TB3_SenzorSpateStanga
- TB3_SenzorSpateStanga = float(data.data)
- #print "Spate Stanga",TB3_SenzorSpateStanga
- def callbackSenzorSpateDreapta(data);
- global TB3_SenzorSpateDreapta
- TB3_SenzorSpateDreapta = float(data.data)
- # print "Spate Dreapta", TB3_SenzorSpateDreapta
- def callbackSenzorSpate(data):
- global TB3_SenzorSpate
- TB3_SenzorSpate = float(data.data)
- def OutOfRange(value):
- error = 0.3
- number1 = -0.01
- number2 = 0.2
- if (value<number1-error or value> number2+error) and iteration> 10:
- return 1
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement