def online_control_A_star():
start = np.array([0, 0, 0])
end = np.array([1, 1, 0])
vel = np.array([0, 0])
T = 30
delta_t = 0.1
x_vec = []
y_vec = []
counter = 0
plt.axis([0, 5, 0, 5])
plt.xlabel('X axis')
plt.ylabel('Y Coordinates')
plt.title('World Map - Real time online AMotion Control')
for i in np.arange(T / delta_t):
vel = controller(start, end, vel, delta_t)
start = motion_model(start, vel, delta_t)
x_vec.append(start[0])
y_vec.append(start[1])
plt.plot(x_vec, y_vec)
plt.pause(pause_time)
plt.show()
def control_interface(start,end,cell_size,add_noise):
"""
inputs:
1. start: 3x1 array (x,y,heading)
2. end: 3 x 1 array (x,y,heading), where heading is not that important
3. cell_size: float
outputs:
1. path from start to end within cell_size. 3 arrays each column is [x,y]
"""
start = np.array(start)
end = np.array(end)
vel = np.array([0,0])
delta_t = 0.1
x_vec = [start[0]]
y_vec = [start[1]]
theta_vec = [start[2]]
while np.linalg.norm(start[:-1]-end[:-1])>cell_size/3.0:
vel = controller(start,end,vel,delta_t,add_noise)
start = motion_model(start,vel,delta_t, add_noise)
x_vec.append(start[0])
y_vec.append(start[1])
theta_vec.append(start[2])
return x_vec, y_vec, theta_vec
def __init__(self,
dt=0.5,
fullscreen=False,
name='unnamed',
iters=1000,
magnify=1.):
pyglet.resource.path = [PNG_PATH]
self.visible_cars = []
self.magnify = magnify
self.camera_center = None
self.name = name
self.objects = []
self.event_loop = pyglet.app.EventLoop()
self.window = pyglet.window.Window(1800,
1800,
fullscreen=fullscreen,
caption=name)
self.grass = pyglet.resource.texture('grass.png')
self.window.on_draw = self.on_draw
self.lanes = []
self.auto_cars = []
self.human_cars = []
self.dt = dt
self.auto_anim_x = {}
self.human_anim_x = {}
self.mock_anim_x = []
self.obj_anim_x = {}
self.prev_x = {}
self.vel_list = []
self.main_car = None
self.sess = None
self.rounds = 0
self.turn_flag = 1
self.sec_turn_flag = 0
self.arrive_flag = 0
self.leader_list = []
self.SEKIRO = []
self.mock = []
self.timerounds = 0
self.finalcnnt = 0
self.controller = control.controller()
self.doublecheck = [0 for i in range(5)]
self.historical_mock = []
self.stop_flag = 0
def centered_image(filename):
img = pyglet.resource.image(filename)
img.anchor_x = img.width / 2.
img.anchor_y = img.height / 2.
return img
def car_sprite(color, scale=0.17 / 600.):
sprite = pyglet.sprite.Sprite(centered_image(
'{}.png'.format(color)),
subpixel=True)
sprite.scale = scale
return sprite
def object_sprite(name, scale=0.15 / 900.):
sprite = pyglet.sprite.Sprite(centered_image(
'{}.png'.format(name)),
subpixel=True)
sprite.scale = scale
return sprite
self.sprites = {
c:
car_sprite(c) if c != 'black' else car_sprite(c, scale=0.2 / 600.)
for c in ['red', 'white', 'gray', 'blue', 'black']
}
self.obj_sprites = {c: object_sprite(c) for c in ['tree', 'firetruck']}
self.keys = key.KeyStateHandler()
self.window.push_handlers(self.keys)
self.window.on_key_press = self.on_key_press
sys.path.append(str(pathlib.Path('../src/')))
import data_ref as dr
import connections
# my_data_true = dr.data_ref(db = 'worker', collection = 'data')
# true_insert_result = my_data_true.data_insert(data = 'sdas', connectionStr = None, mongoClient = connections.client)
# my_data_true.documentID = true_insert_result.inserted_id
# my_data_ref = dr.data_ref(db = 'worker', collection = 'test_worker1')
# ref_data_insert_result = my_data_ref.data_insert(data = my_data_true, connectionStr = None, mongoClient = connections.client)
# my_data_ref.documentID=ref_data_insert_result.inserted_id
# ref_delete_result = my_data_ref.delete_data(connectionStr = None, mongoClient = connections.client)
# my_data_true.documentID = true_insert_result.inserted_id
# true_delete_result = my_data_true.delete_data(connectionStr = None, mongoClient = connections.client)
# import worker_command
# kill_command =worker_command.worker_command('kill')
# my_data_ref = dr.data_ref(db = 'worker', collection = 'test_worker1')
# my_data_ref.data_insert(data = kill_command, connectionStr = None, mongoClient = connections.client)
from control import controller
commnader = controller()
commnader.name = 'commander'
commnader.get_worker_health(worker_name='test_worker0')
# controller.worker_reload('test_worker0')
# controller.kill_worker('test_worker0')
# controller.kill_worker('test_worker1')
# controller.kill_worker('test_worker2')
# controller.kill_worker('test_worker6')
from control import controller
run_controller = True
# Servo angle definition:
# Servos [0,2,4] = CW positive rotation with 0 down
# Servos [1,3,5] = CCW positive rotation with 0 down
# All values sent to Arduino should be **positive** values
init_angle = 135.0 # in degrees
servo_angles = init_angle * np.asarray([1, -1, 1, -1, 1, -1])
dead_zone = 0.2 # Angle in degrees
# Init controller
controller = controller(init_angle=init_angle,
version='v1.0',
bounds=(105, 170))
# Init Arduino serial connection
ardu_ser = serial.Serial('/dev/ttyUSB1', 19200)
print(ardu_ser)
buffer = 'A=' + str(
servo_angles[0]) + '&B=' + str(-servo_angles[1]) + '&C=' + str(
servo_angles[2]) + '&D=' + str(-servo_angles[3]) + '&E=' + str(
servo_angles[4]) + '&F=' + str(-servo_angles[5]) + '\n'
ardu_ser.write(buffer)
# Init IMU serial connection
imu_ser = serial.Serial('/dev/ttyUSB0', 57600)
print(imu_ser)
def go(clist): # pass [timestamp, line, goargs=clist] mytup = board, time.time(), line, clist controller(mytup)
import time
import math
import numpy as np
# Import our Controller
from control import controller
init_angle = 135.0 # Must be a float
controller = controller(init_angle=init_angle, version='v1.0')
orientation = np.asarray([10, 10, 0])
translation = np.asarray([80, 50, 10])
time.sleep(0.01)
t0 = time.time() # In Seconds
for i in range(10000):
controller = controller.step(orientation, translation, t0)
t1 = time.time()
print('Execution Time: ', (t1 - t0) / 10000.0)
print('Angles: ', controller.theta)
#主函数
import turtle
from maze import Maze
from tortoise import Tortoise
from control import controller
maze_list = [[1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 1, 1],
[1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1],
[1, 0, 1, 1, 1, 0, 1, 1, 1, 1, 1, 1, 1],
[1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1],
[1, 1, 1, 0, 1, 1, 1, 0, 1, 0, 1, 0, 1],
[1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 1, 0, 1],
[1, 0, 1, 1, 1, 1, 1, 1, 1, 0, 1, 0, 1],
[1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 1],
[1, 1, 1, 0, 1, 1, 1, 1, 1, 1, 1, 0, 1],
[1, 0, 0, 0, 1, 0, 1, 0, 0, 0, 1, 0, 1],
[1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1],
[1, 0, 1, 0, 0, 0, 1, 0, 1, 0, 0, 0, 1],
[1, 1, 1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1]]
Maze(maze_list)
tortoise = Tortoise(maze_list, 0, 5, 12, 7)
controller(tortoise.go_up, tortoise.go_down, tortoise.go_left,
tortoise.go_right)
turtle.done()
#!/usr/bin/env python
import rospy
import numpy as np
from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import Vector3
import environment as en
import control as con
from copy import copy
# Publisher for sending acceleration commands to flappy bird
pub_acc_cmd = rospy.Publisher('/flappy_acc', Vector3, queue_size=1)
estimator = en.openingEstimator()
controller = con.controller()
def initNode():
# Here we initialize our node running the automation code
rospy.init_node('flappy_automation_code', anonymous=True)
# Subscribe to topics for velocity and laser scan from Flappy Bird game
rospy.Subscriber("/flappy_vel", Vector3, velCallback)
rospy.Subscriber("/flappy_laser_scan", LaserScan, laserScanCallback)
# Ros spin to prevent program from exiting
rospy.spin()
def velCallback(msg):
# msg has the format of geometry_msgs::Vector3
# Example of publishing acceleration command on velocity velCallback
x = 0
y = 0
servo_max = 650 # Max pulse length out of 4096
# Set frequency to 60hz, good for servos.
pwm.set_pwm_freq(60)
init_angle = 20.0 # Must be a float
servo_angles = [init_angle, init_angle, init_angle, init_angle, init_angle, init_angle] # in degrees
# servo_angles = np.deg2rad(servo_angles) # in radians
ccw = [0,2,4]
cw = [1,3,5]
servo_pulse = np.zeros(6)
# Initialize Controller
controller = controller(init_angle=init_angle, version='v1.0', bounds=(-45, 45))
for i in ccw:
servo_angles[i] = 270-servo_angles[i]
for i in range(len(servo_angles)):
servo_pulse[i] = (servo_angles[i])/270*(servo_max-servo_min) + servo_min
pwm.set_pwm(i, 0, int(round(servo_pulse[i])))
##print('Moving servo on channel 0, press Ctrl-C to quit...')
while True:
t = int(round(time.time()*1000)) # milliseconds
## s1 = 135+45*math.sin(2*math.pi*1*t/1000)
## c1 = 135+45*math.sin(2*math.pi*0.1*t/1000)