def received_odometry(data):
global Ballbot_x,Ballbot_y,Ballbot_theta,Ballbot_Tkobjects
# Coordinate frame conversion from localization frame to planner frame
Ballbot_x = (data.y + 3.658)
Ballbot_y = (30.17 - data.x)
Ballbot_theta = (data.theta - math.pi/2)%(2*math.pi)
#print (Ballbot_x,Ballbot_y,Ballbot_theta)
"""
delete old car
"""
if(Ballbot_Tkobjects!=None):
graphics.canvas.delete(Ballbot_Tkobjects[0])
#graphics.canvas.delete(Ballbot_Tkobjects[1])
"""
redraw car at new position
"""
#rospy.loginfo("drawing car at %f,%f,%f",Ballbot_x,Ballbot_y,Ballbot_theta)
Ballbot_Tkobjects = graphics.draw_car(Ballbot_x*100.0 + 17.41*math.cos(Ballbot_theta) ,Ballbot_y*100.0+ 17.41*math.sin(Ballbot_theta),Ballbot_theta)
def received_odometry(data):
global Ballbot_x, Ballbot_y, Ballbot_theta, Ballbot_Tkobjects
# Coordinate frame conversion from localization frame to planner frame
Ballbot_x = (data.y + 3.658)
Ballbot_y = (30.17 - data.x)
Ballbot_theta = (data.theta - math.pi / 2) % (2 * math.pi)
#print (Ballbot_x,Ballbot_y,Ballbot_theta)
"""
delete old car
"""
if (Ballbot_Tkobjects != None):
graphics.canvas.delete(Ballbot_Tkobjects[0])
#graphics.canvas.delete(Ballbot_Tkobjects[1])
"""
redraw car at new position
"""
#rospy.loginfo("drawing car at %f,%f,%f",Ballbot_x,Ballbot_y,Ballbot_theta)
Ballbot_Tkobjects = graphics.draw_car(
Ballbot_x * 100.0 + 17.41 * math.cos(Ballbot_theta),
Ballbot_y * 100.0 + 17.41 * math.sin(Ballbot_theta), Ballbot_theta)
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
