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