def draw_turn_Right(x,y,theta,v,direction,distance,radius):
d = 0
while(d <= distance):
(x2,y2,theta2,v2) = util.turn_Right((x,y,theta,v),direction,d,radius)
draw_point(x2,y2,theta2,'green')
d+=10
(x_temp,y_temp,theta_temp,v_temp) = util.turn_Right((x,y,theta,v),direction,distance,radius)
draw_point(x_temp,y_temp,theta_temp,'green')
return (x_temp,y_temp,theta_temp,v_temp)
def figureofeight((x1, y1, th1, v1)):
path = []
state = (x1, y1, th1, v1)
newstate = None
d = 0
while d <= 2 * math.pi * parameters.ROBOT_RADIUS_MIN:
newstate = util.turn_Left(state, 'f', 5, parameters.ROBOT_RADIUS_MIN)
path.append((newstate[0] / 100, newstate[1] / 100, newstate[2]))
state = newstate
d += 5
(x1, y1, th1, v1) = state
newstate = None
d = 0
while d <= 2 * math.pi * parameters.ROBOT_RADIUS_MIN:
newstate = util.turn_Right(state, 'f', 5, parameters.ROBOT_RADIUS_MIN)
path.append((newstate[0] / 100, newstate[1] / 100, newstate[2]))
state = newstate
d += 5
path_to_send = LatticePlannersim.Path()
for point in path:
pose = LatticePlannersim.Pose()
# plan uses center of car, so transform such that path has points to be traversed by rear axle center, # which is 17.41 cm away from the center
pose.x = point[0] - 17.41 * math.cos(point[2]) / 100.0
pose.y = point[1] - 17.41 * math.sin(point[2]) / 100.0
pose.theta = point[2]
pathelement = lattice_planner.PathElement()
pathelement.pose = pose
pathelement.type = 't'
pathelement.direction = 'f'
path_to_send.poses.append(pathelement)
lattice_planner.pub_path.publish(path_to_send)
print "path published"
def figureofeight((x1,y1,th1,v1)):
path = []
state = (x1,y1,th1,v1)
newstate = None
d = 0
while d <= 2*math.pi*parameters.ROBOT_RADIUS_MIN:
newstate = util.turn_Left(state,'f',5,parameters.ROBOT_RADIUS_MIN)
path.append((newstate[0]/100,newstate[1]/100,newstate[2]))
state = newstate
d += 5
(x1,y1,th1,v1) = state
newstate = None
d = 0
while d <= 2*math.pi*parameters.ROBOT_RADIUS_MIN:
newstate = util.turn_Right(state,'f',5,parameters.ROBOT_RADIUS_MIN)
path.append((newstate[0]/100,newstate[1]/100,newstate[2]))
state = newstate
d += 5
path_to_send = LatticePlannersim.Path()
for point in path:
pose = LatticePlannersim.Pose()
# plan uses center of car, so transform such that path has points to be traversed by rear axle center, # which is 17.41 cm away from the center
pose.x = point[0] - 17.41*math.cos(point[2])/100.0
pose.y = point[1] - 17.41*math.sin(point[2])/100.0
pose.theta = point[2]
pathelement = lattice_planner.PathElement()
pathelement.pose = pose
pathelement.type = 't'
pathelement.direction = 'f'
path_to_send.poses.append(pathelement)
lattice_planner.pub_path.publish(path_to_send)
print "path published"
def segment(point,action,canv):
"""
Given a state and an action, draw the segment corresponding to taking 'action' from 'state'
"""
(x,y,theta) = point
v = 0
# Set direction of motion
if action in ("R_f","L_f","F","F3","SR_f","SL_f","LS_f","RS_f","F_diag","F_diag3"):
direction = "f"
else:
direction = "b"
if action in ("R_f","R_b"): # turning right
d = 0
while(d <= util.ROBOT_RADIUS_MIN*math.pi/2):
(x2,y2,theta2,v2) = util.turn_Right((x,y,theta,v),direction,d,util.ROBOT_RADIUS_MIN)
car = draw_car(x2,y2,theta2,canv)
d+=10
elif action in ("L_f","L_b"): # turning left
d = 0
while(d <= util.ROBOT_RADIUS_MIN*math.pi/2):
(x2,y2,theta2,v2) = util.turn_Left((x,y,theta,v),direction,d,util.ROBOT_RADIUS_MIN)
draw_car(x2,y2,theta2,canv)
d +=10
elif action in ("F","B"):
d = 0
while(d <= util.CELL_SIZE):
(x2,y2,theta2,v2) = util.go_Straight((x,y,theta,v),direction,d)
draw_car(x2,y2,theta2,canv)
d +=10
elif action in ("F3"):
d = 0
while(d <= 3*util.CELL_SIZE):
(x2,y2,theta2,v2) = util.go_Straight((x,y,theta,v),direction,d)
draw_car(x2,y2,theta2,canv)
d +=10
elif action == "SL_f":
d = 0
while(d <= 20.5/70.0*util.ROBOT_RADIUS_MIN):
(x2,y2,theta2,v2) = util.go_Straight((x,y,theta,v),direction,d)
draw_car(x2,y2,theta2,canv)
d += 10
d = 0
(x_temp,y_temp,theta_temp,v_temp) = util.go_Straight((x,y,theta,v),direction,20.5/70.0*util.ROBOT_RADIUS_MIN)
while(d <= util.ROBOT_RADIUS_2*math.pi/4):
(x2,y2,theta2,v2) = util.turn_Left((x_temp,y_temp,theta_temp,v_temp),direction,d,util.ROBOT_RADIUS_2)
draw_car(x2,y2,theta2,canv)
d += 10
elif action == "SR_f":
d = 0
while(d <= 20.5/70.0*util.ROBOT_RADIUS_MIN):
(x2,y2,theta2,v2) = util.go_Straight((x,y,theta,v),direction,d)
draw_car(x2,y2,theta2,canv)
d += 10
d = 0
(x_temp,y_temp,theta_temp,v_temp) = util.go_Straight((x,y,theta,v),direction,20.5/70.0*util.ROBOT_RADIUS_MIN)
while(d <= util.ROBOT_RADIUS_2*math.pi/4):
(x2,y2,theta2,v2) = util.turn_Right((x_temp,y_temp,theta_temp,v_temp),direction,d,util.ROBOT_RADIUS_2)
draw_car(x2,y2,theta2,canv)
d += 10
elif action == "LS_f":
d = 0
while(d <= util.ROBOT_RADIUS_2*math.pi/4):
(x2,y2,theta2,v2) = util.turn_Left((x,y,theta,v),direction,d,util.ROBOT_RADIUS_2)
draw_car(x2,y2,theta2,canv)
d += 10
d = 0
(x_temp,y_temp,theta_temp,v_temp) = util.turn_Left((x,y,theta,v),direction,util.ROBOT_RADIUS_2*math.pi/4,util.ROBOT_RADIUS_2)
while(d <= 20.5/70.0*util.ROBOT_RADIUS_MIN):
(x2,y2,theta2,v2) = util.go_Straight((x_temp,y_temp,theta_temp,v_temp),direction,d)
draw_car(x2,y2,theta2,canv)
d += 10
elif action == "RS_f":
d = 0
while(d <= util.ROBOT_RADIUS_2*math.pi/4):
(x2,y2,theta2,v2) = util.turn_Right((x,y,theta,v),direction,d,util.ROBOT_RADIUS_2)
draw_car(x2,y2,theta2,canv)
d += 10
d = 0
(x_temp,y_temp,theta_temp,v_temp) = util.turn_Right((x,y,theta,v),direction,util.ROBOT_RADIUS_2*math.pi/4,util.ROBOT_RADIUS_2)
while(d <= 20.5/70.0*util.ROBOT_RADIUS_MIN):
(x2,y2,theta2,v2) = util.go_Straight((x_temp,y_temp,theta_temp,v_temp),direction,d)
draw_car(x2,y2,theta2,canv)
d += 10
elif action == "F_diag":
d = 0
while(d <= util.CELL_SIZE*math.sqrt(2)):
(x2,y2,theta2,v2) = util.go_Straight((x,y,theta,v),direction,d)
draw_car(x2,y2,theta2,canv)
d +=10
elif action == "F_diag3":
d = 0
while(d <= 3*util.CELL_SIZE*math.sqrt(2)):
(x2,y2,theta2,v2) = util.go_Straight((x,y,theta,v),direction,d)
draw_car(x2,y2,theta2,canv)
d +=10
elif action == "B_diag":
d = 0
while(d <= util.CELL_SIZE*math.sqrt(2)):
(x2,y2,theta2,v2) = util.go_Straight((x,y,theta,v),direction,d)
draw_car(x2,y2,theta2,canv)
d +=10
def draw_segment(state, action):
"""
Given a state and an action, draw the segment corresponding to taking 'action' from 'state'
"""
global canvas
(x, y, theta, v) = state.get_stateparams()
# Set direction of motion
if action in ("R_f", "L_f", "F", "F3", "SR_f", "SL_f", "LS_f", "RS_f",
"F_diag", "F_diag3"):
direction = "f"
else:
direction = "b"
if action in ("R_f", "R_b"): # turning right
d = 0
while (d <= util.ROBOT_RADIUS_MIN * math.pi / 2):
(x2, y2, theta2, v2) = util.turn_Right((x, y, theta, v), direction,
d, util.ROBOT_RADIUS_MIN)
draw_point(x2, y2, theta2, 'green')
d += 10
elif action in ("L_f", "L_b"): # turning left
d = 0
while (d <= util.ROBOT_RADIUS_MIN * math.pi / 2):
(x2, y2, theta2, v2) = util.turn_Left((x, y, theta, v), direction,
d, util.ROBOT_RADIUS_MIN)
draw_point(x2, y2, theta2, 'green')
d += 10
elif action in ("F", "B"):
d = 0
while (d <= util.CELL_SIZE):
(x2, y2, theta2, v2) = util.go_Straight((x, y, theta, v),
direction, d)
draw_point(x2, y2, theta2, 'green')
d += 10
elif action in ("F3"):
d = 0
while (d <= 3 * util.CELL_SIZE):
(x2, y2, theta2, v2) = util.go_Straight((x, y, theta, v),
direction, d)
draw_point(x2, y2, theta2, 'green')
d += 10
elif action == "SL_f":
d = 0
while (d <= 20.5 / 70.0 * util.ROBOT_RADIUS_MIN):
(x2, y2, theta2, v2) = util.go_Straight((x, y, theta, v),
direction, d)
draw_point(x2, y2, theta2, 'green')
d += 10
d = 0
(x_temp, y_temp, theta_temp, v_temp) = util.go_Straight(
(x, y, theta, v), direction, 20.5 / 70.0 * util.ROBOT_RADIUS_MIN)
while (d <= util.ROBOT_RADIUS_2 * math.pi / 4):
(x2, y2, theta2, v2) = util.turn_Left(
(x_temp, y_temp, theta_temp, v_temp), direction, d,
util.ROBOT_RADIUS_2)
draw_point(x2, y2, theta2, 'green')
d += 10
elif action == "SR_f":
d = 0
while (d <= 20.5 / 70.0 * util.ROBOT_RADIUS_MIN):
(x2, y2, theta2, v2) = util.go_Straight((x, y, theta, v),
direction, d)
draw_point(x2, y2, theta2, 'green')
d += 10
d = 0
(x_temp, y_temp, theta_temp, v_temp) = util.go_Straight(
(x, y, theta, v), direction, 20.5 / 70.0 * util.ROBOT_RADIUS_MIN)
while (d <= util.ROBOT_RADIUS_2 * math.pi / 4):
(x2, y2, theta2, v2) = util.turn_Right(
(x_temp, y_temp, theta_temp, v_temp), direction, d,
util.ROBOT_RADIUS_2)
draw_point(x2, y2, theta2, 'green')
d += 10
elif action == "LS_f":
d = 0
while (d <= util.ROBOT_RADIUS_2 * math.pi / 4):
(x2, y2, theta2, v2) = util.turn_Left((x, y, theta, v), direction,
d, util.ROBOT_RADIUS_2)
draw_point(x2, y2, theta2, 'green')
d += 10
d = 0
(x_temp, y_temp, theta_temp, v_temp) = util.turn_Left(
(x, y, theta, v), direction, util.ROBOT_RADIUS_2 * math.pi / 4,
util.ROBOT_RADIUS_2)
while (d <= 20.5 / 70.0 * util.ROBOT_RADIUS_MIN):
(x2, y2, theta2, v2) = util.go_Straight(
(x_temp, y_temp, theta_temp, v_temp), direction, d)
draw_point(x2, y2, theta2, 'green')
d += 10
elif action == "RS_f":
d = 0
while (d <= util.ROBOT_RADIUS_2 * math.pi / 4):
(x2, y2, theta2, v2) = util.turn_Right((x, y, theta, v), direction,
d, util.ROBOT_RADIUS_2)
draw_point(x2, y2, theta2, 'green')
d += 10
d = 0
(x_temp, y_temp, theta_temp, v_temp) = util.turn_Right(
(x, y, theta, v), direction, util.ROBOT_RADIUS_2 * math.pi / 4,
util.ROBOT_RADIUS_2)
while (d <= 20.5 / 70.0 * util.ROBOT_RADIUS_MIN):
(x2, y2, theta2, v2) = util.go_Straight(
(x_temp, y_temp, theta_temp, v_temp), direction, d)
draw_point(x2, y2, theta2, 'green')
d += 10
elif action == "F_diag":
d = 0
while (d <= util.CELL_SIZE * math.sqrt(2)):
(x2, y2, theta2, v2) = util.go_Straight((x, y, theta, v),
direction, d)
draw_point(x2, y2, theta2, 'green')
d += 10
elif action == "F_diag3":
d = 0
while (d <= 3 * util.CELL_SIZE * math.sqrt(2)):
(x2, y2, theta2, v2) = util.go_Straight((x, y, theta, v),
direction, d)
draw_point(x2, y2, theta2, 'green')
d += 10
elif action == "B_diag":
d = 0
while (d <= util.CELL_SIZE * math.sqrt(2)):
(x2, y2, theta2, v2) = util.go_Straight((x, y, theta, v),
direction, d)
draw_point(x2, y2, theta2, 'green')
d += 10
def odometry():
"""
Draw car, and update position
"""
global CAR, ROBOT_X, ROBOT_Y, ROBOT_TH, ROBOT_D
v = 0
t1 = 0
looptime = 0
while (True):
loopstart = time.time()
action = ROBOT_ACTION
d = ROBOT_SPEED * (looptime)
print d, ROBOT_SPEED, "steer", ROBOT_STEER
ROBOT_D += d
if action in ("R_f", "L_f", "F", "F3", "SR_f", "SL_f", "LS_f", "RS_f",
"F_diag", "F_diag3"):
direction = "f"
else:
direction = "b"
if action in ("R_f", "R_b"): # turning right
(ROBOT_X, ROBOT_Y, ROBOT_TH, v2) = util.turn_Right(
(ROBOT_X, ROBOT_Y, ROBOT_TH, v), direction, d,
util.ROBOT_RADIUS_MIN)
elif action in ("L_f", "L_b"): # turning left
(ROBOT_X, ROBOT_Y, ROBOT_TH, v2) = util.turn_Left(
(ROBOT_X, ROBOT_Y, ROBOT_TH, v), direction, d,
util.ROBOT_RADIUS_MIN)
elif action in ("F", "B"):
(ROBOT_X, ROBOT_Y, ROBOT_TH, v2) = util.go_Straight(
(ROBOT_X, ROBOT_Y, ROBOT_TH, v), direction, d)
elif action in ("F3"):
(ROBOT_X, ROBOT_Y, ROBOT_TH, v2) = util.go_Straight(
(ROBOT_X, ROBOT_Y, ROBOT_TH, v), direction, d)
elif action in ("SL_f", "LS_f"):
if ROBOT_STEER == 0:
(ROBOT_X, ROBOT_Y, ROBOT_TH, v2) = util.go_Straight(
(ROBOT_X, ROBOT_Y, ROBOT_TH, v), direction, d)
elif ROBOT_STEER == -70:
(ROBOT_X, ROBOT_Y, ROBOT_TH, v2) = util.turn_Left(
(ROBOT_X, ROBOT_Y, ROBOT_TH, v), direction, d,
util.ROBOT_RADIUS_2)
elif action == ("SR_f", "RS_f"):
if ROBOT_STEER == 0:
(ROBOT_X, ROBOT_Y, ROBOT_TH, v2) = util.go_Straight(
(ROBOT_X, ROBOT_Y, ROBOT_TH, v), direction, d)
elif ROBOT_STEER == 70:
(ROBOT_X, ROBOT_Y, ROBOT_TH, v2) = util.turn_Right(
(ROBOT_X, ROBOT_Y, ROBOT_TH, v), direction, d,
util.ROBOT_RADIUS_2)
elif action == "F_diag":
(ROBOT_X, ROBOT_Y, ROBOT_TH, v2) = util.go_Straight(
(ROBOT_X, ROBOT_Y, ROBOT_TH, v), direction, d)
elif action == "F_diag3":
(ROBOT_X, ROBOT_Y, ROBOT_TH, v2) = util.go_Straight(
(ROBOT_X, ROBOT_Y, ROBOT_TH, v), direction, d)
elif action == "B_diag":
(ROBOT_X, ROBOT_Y, ROBOT_TH, v2) = util.go_Straight(
(ROBOT_X, ROBOT_Y, theta, v), direction, d)
else:
return
#time.sleep(0.1)
if (CAR != None):
graphics.canvas.delete(CAR[0])
graphics.canvas.delete(CAR[1])
CAR = graphics.draw_car(ROBOT_X, ROBOT_Y, ROBOT_TH)
graphics.canvas.update_idletasks()
looptime = time.time() - loopstart
if (not thread_control.isAlive()):
print odomreturning
return
