def odometryPlot(self, deltaT):
tetai = 1
xi = 0
yi = 0
t = 0
# while self.getStateWheel()==1:
while True:
speedR = self.get_speed_right_wheels()
speedL = self.get_speed_left_wheels()
l = odom.direct_kinematics(speedL[0], speedR[0])
vLinear = l[0]
vAngular = l[1]
dx_dy_dteta = odom.odomWorld(vLinear, vAngular, deltaT, tetai)
L = odom.tick_odom(xi, yi, tetai, dx_dy_dteta)
xi = L[0]
yi = L[1]
tetai = L[2]
time.sleep(deltaT)
return (xi, yi)
def _handle_odom_timer():
global _ctrl_on, _velocities, _set_speed, _xhat, _yhat, _thetahat
if _odom_on:
odom = Odometry.update(_odom_timer_period, _velocityhat)
print "{}\r".format(odom)
_xhat = odom[0]
_yhat = odom[1]
_thetahat = odom[2]
if _ctrl_on:
x_c, y_c, theta_c = Controller.get_commanded_position()
if _close(_xhat, x_c) and _close(_yhat, y_c) and \
_close(_thetahat, theta_c, tolerance=8):
_ctrl_on = False
print("\r\n*** Reached Set Point within Tolerances ***\r\n")
_motion_timer.stop()
motion.stop()
time.sleep(0.5)
_velocities = (0, 0, 0)
_set_speed = True
_motion_timer.start()
def main():
rospy.init_node('odometry', anonymous=False)
rospy.Subscriber('encoder_estimates', EncoderEstimates, _handle_encoder_estimates)
pub = rospy.Publisher('estimated_position', Pose2D, queue_size=10)
rate = rospy.Rate(int(1/_odom_period))
while not rospy.is_shutdown():
(xhat, yhat, thetahat) = Odometry.update(_odom_period, _velocities)
msg = Pose2D()
msg.x = xhat
msg.y = yhat
msg.theta = thetahat
pub.publish(msg)
rate.sleep()
# spin() simply keeps python from exiting until this node is stopped
rospy.spin()
def main():
rospy.init_node('odometry', anonymous=False)
rospy.Subscriber('encoder_estimates', EncoderEstimates,
_handle_encoder_estimates)
pub = rospy.Publisher('estimated_position', Pose2D, queue_size=10)
rate = rospy.Rate(int(1 / _odom_period))
while not rospy.is_shutdown():
(xhat, yhat, thetahat) = Odometry.update(_odom_period, _velocities)
msg = Pose2D()
msg.x = xhat
msg.y = yhat
msg.theta = thetahat
pub.publish(msg)
rate.sleep()
# spin() simply keeps python from exiting until this node is stopped
rospy.spin()
def main():
global _motion_timer
_motion_timer = RepeatedTimer(_motion_timer_period, _handle_motion_timer)
_motion_timer.start()
global _odom_timer
_odom_timer = RepeatedTimer(_odom_timer_period, _handle_odom_timer)
_odom_timer.start()
global _ctrl_timer
_ctrl_timer = RepeatedTimer(_ctrl_timer_period, _handle_ctrl_timer)
_ctrl_timer.start()
use_rcv3 = os.environ['USE_RCV3'] == 'true'
w.init(use_rcv3=use_rcv3)
# TODO: Okay, so Controller.init(None) will automagically load in some PID
# values for each of the x, y, theta position controllers. However, since
# `teleop` is run in real life on different robots, really it should read
# the correct YAML robot config file and load in the PID constants and
# pass them into Controller.init(gains) as in the controller_node.py.
# Two ways to do this: (1) do like os.environ['USE_RCV3'], where the odroid_setup.bash
# script loads up the PID constants as environment variables (this seems messy).
# (2) find a python YAML reading library and just read the file directly from here
# based on which robot this is (you can use os.environ['ROBOT'])
Controller.init()
print 'Started.'
global _velocities, _set_speed, _smooth, _odom_on, _previous_action, _ctrl_on
while(1):
action = get_action()
if action == 'UP':
_set_speed = True
_velocities = (0, _vy, 0)
elif action == 'DOWN':
_set_speed = True
_velocities = (0, -_vy, 0)
elif action == 'RIGHT':
_set_speed = True
_velocities = (_vx, 0, 0)
elif action == 'LEFT':
_set_speed = True
_velocities = (-_vx, 0, 0)
elif action == 'SPIN_CW':
_set_speed = True
_velocities = (0, 0, _w)
elif action == 'SPIN_CCW':
_set_speed = True
_velocities = (0, 0, -_w)
elif action == 'SET_HOME':
Odometry.init()
elif action == 'GO_HOME':
_motion_timer.stop()
motion.stop()
time.sleep(1)
_go_home()
time.sleep(1)
_set_speed = True
_velocities = (0, 0, 0)
_motion_timer.start()
elif action == 'GO_TO_POINT':
_toggle_timers(False)
motion.stop()
time.sleep(1)
_ask_for_point()
time.sleep(1)
_ctrl_on = True
_odom_on = True
_toggle_timers(True)
elif action == 'TOGGLE_CNTRL':
_ctrl_on = not _ctrl_on
print("Controller: {}".format(_ctrl_on))
elif action == 'TOGGLE_SMOOTH':
_smooth = not _smooth
print("Smooth: {}".format(_smooth))
elif action == 'TOGGLE_ODOM':
_odom_on = not _odom_on
print("Odom: {}".format(_odom_on))
elif action == 'BATTERY':
print("\n\r*** Battery: {} ***\n\r".format(get_battery()))
elif action == 'BREAKPOINT':
_toggle_timers(False)
import ipdb; ipdb.set_trace()
_toggle_timers(True)
elif action == 'CALIBRATION_MODE':
_toggle_timers(False)
_deal_with_calibration()
time.sleep(1)
_toggle_timers(True)
elif action == 'DIE':
_toggle_timers(False)
motion.stop()
w.kill()
return sys.exit(0)
else:
_set_speed = True
_velocities = (0, 0, 0)
# handle stopping before changing directions
if _action_requires_stop(action):
_set_speed = False
motion.stop()
time.sleep(0.4)
_set_speed = True
_previous_action = action
print "{}\r".format(_velocities)
def __init__(self, serial):
self.serial = serial
self.odometry = Odometry.Odometry(self)
class Test:
prevOdoStep = 0
prevWifiDistance = 0
time.sleep(2)
PLd0 = -25.333
n = -1.7844
d0 = 1
#n= -2.3714
# d0 = 1 #values updated 03/04/19 FOR OFFICE
#PLd0 = -18.667
odoCounter = 0
nextStop = 17
#desiredAddress = ['72:3A:CB:C0:43:E4', '50:C7:BF:96:D4:AE', '70:3A:CB:C0:43:EA']
#desiredAddress = ['30:FD:38:F0:DA:3B', '30:FD:38:F0:99:E8', '30:FD:38:F0:7F:2E'] # setup3ADB0,setup99E80,setup7F2E0
desiredAddress = ['70:3A:CB:C0:43:E6', '70:3A:CB:D4:C2:15', 'CC:40:D0:17:FB:DA'] #CASA: Viger Studio, Viger living room, neighbor Netgear10
## wifiLocationX = [0, 142, 142]
## wifiLocationY = [0, -61, 61] # relative to first point
wifiLocationX = [0, 80, 437]
wifiLocationY = [0, 80, 127] #relative to first point
odometryOn = 1
wifiOn = 1
odoDistance = 30
## try:
if wifiOn == 1:
done = 0
while done == 0:
try:
startPosition = WifiData.WifiPosition(desiredAddress, wifiLocationX, wifiLocationY, n, PLd0, d0)
done = 1
print('Start Position', startPosition)
except:
#print('A wifi is not available on Start Position:',datetime.datetime.now())
print('.',end='')
MotorControl.Motor_Forward() # This is the init to start the wheels
while(True):
#ODOMETRY
if odometryOn == 1:
odoStep = Odometry.stepCounter()
#odoDistance =(Odometry.step2cm(odoStep))
#odoDistance+=30
if odoStep != prevOdoStep : # reporting step
#print("ODO_DISTANCE: ",odoDistance, odoStep)
prevOdoStep= odoStep
#COMBINATION
if Odometry.step2cm(odoStep) >= nextStop:
MotorControl.Motor_Stop() # stop wheel
odoDistance +=30
print("ODO_DISTANCE: ",odoDistance, "STEP", odoStep)
#CHECK WIFI
if wifiOn == 1:
done = 0
while done == 0:
try:
WifiCurrentPosition = WifiData.WifiPosition(desiredAddress, wifiLocationX, wifiLocationY, n, PLd0, d0)
wifiDistance = WifiData.getDist(startPosition, WifiCurrentPosition)
#print('WifiDistance:', wifiDistance)
print('WifiCurentPosition:', WifiCurrentPosition)
done = 1
except:
#print('Wifi error occured, try again:',datetime.datetime.now())
print('.',end='')
#COMBINED
odoCounter += 1
outliers = DynamicWeightAllocation.checkOutliers(odoDistance, WifiCurrentPosition, 0.1)
print("OUTLIERS", outliers)
finalPosition = DynamicWeightAllocation.getPosition(odoDistance, WifiCurrentPosition, outliers)
if outliers == 1:
odoCounter = 0
print("odoCounter: ",odoCounter)
print("FINAL DISTANCE: ", finalPosition)
odoDistance = finalPosition
input("PRESS ENTER TO CONTINUE :D -----------------------------------------------------------------------------------------------------")
MotorControl.Motor_Forward()
nextStop+=17
