def __init__(self, omap_path, sparki):
self.omap = ObstacleMap(omap_path, noise_range=1)
self.rangefinder = Rangefinder(cone_width=deg2rad(15.),
obstacle_width=1.)
FrontEnd.__init__(self, self.omap.width, self.omap.height)
self.sparki = sparki
self.robot = Robot()
# center robot
self.robot.x = self.omap.width * 0.5
self.robot.y = self.omap.height * 0.5
# create particle filter
alpha = [0.5, 0.5, 0.5, 0.5]
self.particle_filter = ParticleFilter(num_particles=50,
alpha=alpha,
robot=self.robot,
omap=self.omap)
# set message callback
self.sparki.message_callback = self.message_received
# last timestamp received
self.last_timestamp = 0
def __init__(self, init_x, init_y, init_z, init_theta, init_v, init_w, planner_key_list, env_info):
"""
Initialilze an AUV class given the initial auv position and other configurations
Parameters:
init_x - initial x position
init_y - initial y position
init_z - initial z position
init_theta - initial theta position
init_v - nonzero, initial linear velocity
Warning: currently, no way to change the linear velocity
init_w - inital angular velocity
planner_key_list - Python list, indicate which planner(s) to use in this auv
TODO: check with MotionPlanner code - is this format ok?
env_info - Python dictionary, contain information neccessary for the planner
(e.g. boundary, obstacles, habitats)
TODO: check with MotionPlanner code - is this format ok?
"""
self.state = MotionPlanState(init_x, init_y, init_z, init_theta, init_v, init_w)
self.particle_filter = ParticleFilter()
self.motion_planner = MotionPlanner(planner_key_list, env_info)
self.curr_traj_pt_index = 0
self.TRAJ_LOOK_AHEAD_TIME = 0.5
# for plotting
self.x_list_plot = [init_x]
self.y_list_plot = [init_y]
self.z_list_plot = [init_z]
def run(self):
"""spawn soda in each date directory"""
os.system('ulimit -s unlimited')
for dd in self.options.dates:
os.chdir(dd)
if self.options.todo != 'pfpython':
os.system('./soda.exe')
else:
# BC April 2020
# nice but not maintaned and deprecated class
# which allowed to test and develop locally python version of the PF
# before implementing it into fortran.
plot = True
if plot:
plt.figure()
self.pf = ParticleFilter(self.xpiddir, self.options, dd)
_, resample = self.pf.localize(k=self.options.pgd.npts,
errobs_factor=self.options.fact,
plot=plot)
_, gresample = self.pf.globalize(
errobs_factor=self.options.fact, plot=plot)
print(('gres', gresample))
self.pf.pag = self.pf.analyze(gresample)
self.pf.pal = self.pf.analyze(resample)
if plot:
plt.show()
os.chdir('..')
def __init__(self, x, y, yaw, side, button=True):
b_time = utime.ticks_ms()
print(b_time)
side_loop = 0
side = False
while (side_loop == 0):
if (pin9.value() == 0): # нажатие на кнопку на голове
side_loop = 1
side = True
print("I will attack blue goal")
break
if (utime.ticks_ms() - b_time) > 1500:
print("I will attack yellow goal")
break
b_time = utime.ticks_ms()
side_loop = 0
s_coord = 0
while (side_loop == 0):
if (pin3.value() == 0):
side_loop
s_coord = 2
print("right")
break
if (pin2.value() == 0):
side_loop
s_coord = 1
print("left")
break
if (utime.ticks_ms() - b_time) > 1500:
print("centr")
break
with open("localization/start_coord.json", "r") as f:
start_coord = json.loads(f.read())
self.robot_position = tuple(start_coord[str(s_coord)])
print(self.robot_position[2])
self.ballPosSelf = None
self.ball_position = None
self.localized = False
self.seeBall = False
self.side = False
with open("localization/landmarks.json", "r") as f:
landmarks = json.loads(f.read())
# choice side
if side:
colors = []
for goal_color in landmarks:
colors.append(goal_color)
neutral = landmarks[colors[0]]
landmarks[colors[0]] = landmarks[colors[1]]
landmarks[colors[1]] = neutral
if button:
x, y, yaw = self.robot_position
self.pf = ParticleFilter(Robot(x, y, yaw),
Field("localization/parfield.json"),
landmarks)
def main():
particles_num = 40
img_path = r'./test-videos1/Dog1/img'
out_path = r'./output'
PF = ParticleFilter(particles_num, img_path, out_path)
#print(PF.imgs)
while PF.img_index < len(PF.imgs):
PF.select()
PF.propagate()
PF.observe()
PF.estimate()
def __init__ (self, world, width=1000, height=800):
self.width = width
self.height = height
self.master = Tk()
self.canvas = Canvas(self.master, width=width, height=height)
canvas = self.canvas
canvas.pack()
self.world = world
world.display(canvas)
self.localizer = ParticleFilter(N=3000, width=width, height=height)
localizer = self.localizer
localizer.display(canvas)
def __init__(self,omap_path,sparki):
self.omap = ObstacleMap(omap_path,noise_range=0)
self.rangefinder = Rangefinder(cone_width=deg2rad(15.),obstacle_width=1.)
FrontEnd.__init__(self,self.omap.width,self.omap.height)
self.sparki = sparki
self.robot = Robot()
# center robot
self.robot.x = self.omap.width*0.5
self.robot.y = self.omap.height*0.5
# create particle filter
alpha=[0.5,0.5,0.5,0.5,0.5,0.5]
self.particle_filter = ParticleFilter(num_particles=100,alpha=alpha,robot=self.robot,omap=self.omap)
self.sonar_step = deg2rad(1.)
def init_particle_filter():
""" Initialize the particle filter. """
num_particles = 500
num_states = 2
dynamics_matrix = [[1, 0], [1, 1]]
lower_bounds = [0, 0]
upper_bounds = [200, 200]
noise_type = 'gaussian'
noise_param1 = num_states * [0.0]
noise_param2 = num_states * [5.0]
final_state_decision_method = 'weighted_average'
pf = ParticleFilter(num_particles, num_states, dynamics_matrix,
lower_bounds, upper_bounds, noise_type, noise_param1,
noise_param2, final_state_decision_method)
particle_init_method = 'uniform'
pf.init_particles(particle_init_method, lower_bounds, upper_bounds)
return pf
def __init__ (self, world, width=1000, height=800):
self.master = Tk()
self.canvas = Canvas(self.master, width=width, height=height)
canvas = self.canvas
canvas.pack()
self.world = world
world.display(canvas)
self.localizer = ParticleFilter(N=3000, width=width, height=height)
localizer = self.localizer
localizer.display(canvas)
def __init__(self,
group=None,
target=None,
name=None,
args=(),
kwargs={},
daemon=None,
courseMap=None,
sensorConversionThread=None):
'''
Initializes the control planner
@param Refer to the Python Thread class for documentation on all thread specific parameters
@param courseMap - the course map
@param sensorConversionThread - the sensor conversion thread
'''
Thread.__init__(self,
group=group,
target=target,
name=name,
daemon=daemon)
Publisher.__init__(self)
self.args = args
self.kwargs = kwargs
self.courseMap = courseMap
self.particleFilter = ParticleFilter(Constants.PARTICLE_NUMBER,
Constants.MAP_START_BOX,
Constants.MAP_HEADING_RANGE,
self.courseMap,
sensorConversionThread)
self.shutDown = False
self.estVehicleX = 0.0
self.estVehicleY = 0.0
self.estVehicleHeading = 0.0
self.covarVehicle = None
self.waypoint = 0
self.waypointCheck = 0
self.steeringAngleGoal = 0.0
self.velocityGoal = 0.0
def __init__(self, tmap_path, sparki):
self.tmap = TilesMap(tmap_path)
FrontEnd.__init__(self, self.tmap.width, self.tmap.height)
self.sparki = sparki
self.robot = Robot()
# center robot
self.robot.x = self.tmap.width * 0.5
self.robot.y = self.tmap.height * 0.5
# create particle filter
alpha = [0.5, 0.5, 0.5, 0.5]
self.particle_filter = ParticleFilter(num_particles=200,
alpha=alpha,
robot=self.robot,
tmap=self.tmap)
# set message callback
self.sparki.message_callback = self.message_received
# last timestamp received
self.last_timestamp = 0
def __init__(self, generatedMap, master = None):
tk.Frame.__init__(self, master)
self.moveStep = 10
self.map = generatedMap
self.nearestBuildingIndexes = []
self.quadrocopter = Quadrocopter(generatedMap)
self.particleFilter = ParticleFilter(generatedMap)
self.particleFilter.createParticles(300)
self.master.bind("<Right>", self.moveRight)
self.master.bind("<Left>", self.moveLeft)
self.master.bind("<Up>", self.moveUp)
self.master.bind("<Down>", self.moveDown)
self.master.title("Quadrocopter World")
self.canvas = tk.Canvas(self.master, width = self.map.rows * 160, height = self.map.cols * 160)
self.canvas.pack()
def __init__(self, num_particles, xmin, xmax, ymin, ymax):
# Initialize node
rospy.init_node("MonteCarloLocalization")
# Get map from map server
print("Wait for static_map from map server...")
rospy.wait_for_service('static_map')
map = rospy.ServiceProxy("static_map", GetMap)
resp1 = map()
self.grid_map = resp1.map
print("Map resolution: " + str(self.grid_map.info.resolution))
print("Map loaded.")
self.particle_filter = ParticleFilter(num_particles, self.grid_map,
xmin,xmax,ymin,ymax,
0,0,0,0,
0.25, # translation
0.1, # orientation
0.3) # measurement
self.particle_filter.init_particles()
self.last_scan = None
self.mutex = Lock()
self.particles_pub = rospy.Publisher('/particle_filter/particles', MarkerArray, queue_size=1)
self.mcl_estimate_pose_pub = rospy.Publisher('/mcl_estimate_pose', PoseStamped, queue_size=1)
self.publish_fake_scan = rospy.Publisher('/fake_scan', LaserScan, queue_size=1)
rospy.Subscriber('/base_pose_ground_truth', Odometry, self.odometry_callback, queue_size=1)
rospy.Subscriber('/scan', LaserScan, self.laser_callback, queue_size=1)
self.first_laser_flag = True
self.odom_initialized = False
self.laser_initialized = False
self.mutex = Lock()
def __init__(self):
self.node = rospy.init_node('see_ego_motion')
self.odom_pub = rospy.Publisher("see_ego_motion/odom_filtered",
see_ego_motion_interface,
queue_size=10)
self.rviz_particle_pub = rospy.Publisher(
"odometry_visualizer/particles", Marker, queue_size=1)
self.filter = ParticleFilter.ParticleFilter(ctrvModel, numParticles=20)
self.filter.initParticles(particleMean, particleCov)
self.sensorTimeLast = rospy.Time.now()
self.publishTimeLast = rospy.Time.now()
self.lastOdom = see_ego_motion_interface() #for visualization hack
self.odom_pub = rospy.Publisher("see_ego_motion/odom_filtered",
see_ego_motion_interface,
queue_size=10)
self.rviz_particle_pub = rospy.Publisher(
"see_ego_motion_test/odometry_visualization/particles",
Marker,
queue_size=1)
self.rviz_odom_pub = rospy.Publisher(
"see_ego_motion_test/odometry_filtered", Marker, queue_size=1)
self.rviz_gps_pub = rospy.Publisher("see_ego_motion_test/gps",
Marker,
queue_size=1)
if rosbag:
rospy.Subscriber("gps", NavSatFix, self.sensor_gps_callback)
rospy.Subscriber("optical_speed_sensor", TwistStamped,
self.sensor_odom_callback)
else:
#rospy.Subscriber("see_localization/vehicle_pose", vehicle_pose, self.slam_pose_callback)
rospy.Subscriber("vectornav/GPS", NavSatFix,
self.sensor_gps_callback)
rospy.Subscriber("vectornav/Odom", Odometry,
self.sensor_odom_callback)
#rospy.Subscriber("see_localization/vehicle_pose", vehicle_pose, self.slam_pose_callback)
rospy.Subscriber("vectornav/GPS", NavSatFix, self.sensor_gps_callback)
rospy.Subscriber("vectornav/Odom", Odometry, self.sensor_odom_callback)
self.utm_origin = None
import sys
import csv
import cv2
import glob
import numpy as np
from ParticleFilter import ParticleFilter
if __name__ == "__main__":
cv2.namedWindow('Lane Markers')
imgs = glob.glob("images/*.png")
intercepts = []
xl_int_pf = ParticleFilter(N=1000,
x_range=(0, 1500),
sensor_err=1,
par_std=100)
xl_phs_pf = ParticleFilter(N=1000,
x_range=(15, 90),
sensor_err=0.3,
par_std=1)
xr_int_pf = ParticleFilter(N=1000,
x_range=(100, 1800),
sensor_err=1,
par_std=100)
xr_phs_pf = ParticleFilter(N=1000,
x_range=(15, 90),
sensor_err=0.3,
par_std=1)
#tracking queues
class MyFrontEnd(FrontEnd):
def __init__(self,omap_path,sparki):
self.omap = ObstacleMap(omap_path,noise_range=0)
self.rangefinder = Rangefinder(cone_width=deg2rad(15.),obstacle_width=1.)
FrontEnd.__init__(self,self.omap.width,self.omap.height)
self.sparki = sparki
self.robot = Robot()
# center robot
self.robot.x = self.omap.width*0.5
self.robot.y = self.omap.height*0.5
# create particle filter
alpha=[0.5,0.5,0.5,0.5,0.5,0.5]
self.particle_filter = ParticleFilter(num_particles=100,alpha=alpha,robot=self.robot,omap=self.omap)
self.sonar_step = deg2rad(1.)
def keydown(self,key):
# update velocities based on key pressed
if key == pygame.K_UP: # set positive linear velocity
self.robot.lin_vel = 20.0
elif key == pygame.K_DOWN: # set negative linear velocity
self.robot.lin_vel = -20.0
elif key == pygame.K_LEFT: # set positive angular velocity
self.robot.ang_vel = 100.*math.pi/180.
elif key == pygame.K_RIGHT: # set negative angular velocity
self.robot.ang_vel = -100.*math.pi/180.
elif key == pygame.K_k: # set positive servo angle
self.robot.sonar_angle = 45.*math.pi/180.
elif key == pygame.K_l: # set negative servo angle
self.robot.sonar_angle = -45.*math.pi/180.
def keyup(self,key):
# update velocities based on key released
if key == pygame.K_UP or key == pygame.K_DOWN: # set zero linear velocity
self.robot.lin_vel = 0
elif key == pygame.K_LEFT or key == pygame.K_RIGHT: # set zero angular velocity
self.robot.ang_vel = 0
elif key == pygame.K_k or key == pygame.K_l: # set zero servo angle
self.robot.sonar_angle = 0
def draw(self,surface):
# draw obstacle map
self.omap.draw(surface)
# draw robot
self.robot.draw(surface)
# draw particles
self.particle_filter.draw(surface)
def update(self,time_delta):
# Get map/sonar transformation matrices
T_map_sonar = self.robot.get_robot_sonar_transform()*self.robot.get_map_robot_transform()
T_sonar_map = self.robot.get_robot_map_transform()*self.robot.get_sonar_robot_transform()
if self.sparki.port == '':
# calculate sonar distance from map
self.robot.sonar_distance = self.omap.get_first_hit(T_sonar_map)
else:
# get current rangefinder reading
self.robot.sonar_distance = self.sparki.dist
# update particles
self.particle_filter.generate(time_delta)
self.particle_filter.update()
self.particle_filter.sample()
# calculate servo setting
servo = int(self.robot.sonar_angle*180./math.pi)
# calculate motor settings
left_speed, left_dir, right_speed, right_dir = self.robot.compute_motors()
# send command
self.sparki.send_command(left_speed,left_dir,right_speed,right_dir,servo)
# update robot position
self.robot.update(time_delta)
def main():
env = Environment()
env.SetViewer('qtcoin')
collisionChecker = RaveCreateCollisionChecker(env, 'ode')
env.SetCollisionChecker(collisionChecker)
env.Reset()
env.Load('path_gen/pr2test2.env.xml')
time.sleep(0.1)
robot0 = env.GetRobots()[0]
tuckarms(env, robot0)
handles = []
kf_traj = None
pf_traj = None
kf_ground_truth = None
kf_actual_path = None
kf_in_collision = None
pf_ground_truth = None
pf_actual_path = None
pf_in_collision = None
filename = "data/env2_circle_back.pickle"
with env:
# the active DOF are translation in X and Y and rotation about the Z axis of the base of the robot.
robot0.SetActiveDOFs([],DOFAffine.X|DOFAffine.Y|DOFAffine.RotationAxis,[0,0,1])
# ******* Printout expected time to run ******
print "Estimated time to run: 10~12 minutes"
print "Ground truth: black points"
print "Estimation of Particle Filter: blue points/ red if in-collision"
print "Estimation of Kalman Filter: green points/ purple if in-collision"
# ******* PARTICLE FILTER *******
PF = ParticleFilter(2000, [[-4.0, 4.0], [-1.5, 4.0]], 0.1, 'multivariate_normal')
pf_sim = Simulator(env, robot0, filename, PF.filter)
print "****** Particle Filter ******"
start = time.clock()
pf_ground_truth, pf_actual_path, pf_in_collision = pf_sim.simulate()
end = time.clock()
print "PF Test for ", filename
print "Execution Time: ", end-start
if True in pf_in_collision:
print "Estimated Path is in Collision"
# Plotting the Data
for pt in pf_ground_truth:
handles.append(env.plot3(points=(pt[0], pt[1], 0.3), pointsize=3.0, colors=(((0,0,0)))))
for pt,collision in zip(pf_actual_path, pf_in_collision):
if collision:
handles.append(env.plot3(points=(pt[0], pt[1], 0.3), pointsize=5.0, colors=(((1,0,0)))))
else:
handles.append(env.plot3(points=(pt[0], pt[1], 0.3), pointsize=5.0, colors=(((0,0,1)))))
pf_traj = ConvertPathToTrajectory(env, robot0, pf_actual_path)
# ******* KALMAN FILTER *******
kf_sim = Simulator(env, robot0, filename, KalmanFilter)
print "****** Kalman Filter ******"
start = time.clock()
kf_ground_truth, kf_actual_path, kf_in_collision = kf_sim.simulate()
end = time.clock()
print "KF Test for ", filename
print "Execution Time: ", end-start
if True in kf_in_collision:
print "Estimated Path is in Collision"
# Plotting the Data
for pt,collision in zip(kf_actual_path, kf_in_collision):
if collision:
handles.append(env.plot3(points=(pt[0], pt[1], 0.6), pointsize=5.0, colors=(((90.0/255.0, 0/255.0, 160.0/255.0)))))
else:
handles.append(env.plot3(points=(pt[0], pt[1], 0.6), pointsize=5.0, colors=(((0,1,0)))))
kf_traj = ConvertPathToTrajectory(env, robot0, kf_actual_path)
plot_report(kf_ground_truth, kf_actual_path, kf_in_collision, pf_ground_truth, pf_actual_path, pf_in_collision)
raw_input("Press to play particle path")
if pf_traj:
robot0.GetController().SetPath(pf_traj)
waitrobot(robot0)
raw_input("Press to play kalman path")
if kf_traj:
robot0.GetController().SetPath(kf_traj)
waitrobot(robot0)
raw_input("Press enter to exit...")
return val
else:
red_degree = 2 * g - r - b
# return 1.0 / (np.sqrt(2 * np.pi) * sigma) * np.exp(-red_degree / 2 * sigma * sigma)
return max(red_degree + 10, 0) + 1
def next_state(p):
next_vec = np.random.randn(2) * [10, 10]
next_y = (p[0] + int(next_vec[0]) + 2 * HEIGHT + 1) % HEIGHT - 1
next_x = (p[1] + int(next_vec[1]) + 2 * WIDTH + 1) % WIDTH - 1
return [next_y, next_x]
def initial_state(_):
return np.random.rand(2) * [HEIGHT, WIDTH]
pf = ParticleFilter(size = 100, evaluate = evaluate, next_state = next_state, initial_state = initial_state)
pf.initialize()
### end PF
### Hit Detection ###
def hit(past_point_list):
average_move = 0
for i in range(1, len(past_point_list)):
average_move += np.linalg.norm(past_point_list[i-1] - past_point_list[i]) / len(past_point_list)
print "Average_move", average_move
#return average_move > 3
return average_move > 100
def camera_check():
time.sleep(0.1)
# 1) get the 1st robot that is inside the loaded scene
# 2) assign it to the variable named 'robot'
robot = env.GetRobots()[0]
# tuck in the PR2's arms for driving
tuckarms(env,robot);
handles = []
with env:
# the active DOF are translation in X and Y and rotation about the Z axis of the base of the robot.
robot.SetActiveDOFs([],DOFAffine.X|DOFAffine.Y|DOFAffine.RotationAxis,[0,0,1])
#### YOUR CODE HERE ####
PF = ParticleFilter(3000, [[-4.,4.],[-1.5,4.]],0.1,"multivariate_normal")
sim = Simulator(env, robot, "data/env2_hwk3.pickle", PF.filter)
# PF = ParticleFilter(10000,[[-4.,4.],[-1.5,4.]],0.1,'skewnorm', Q=[-10,0,0.5])
# sim = Simulator(env, robot, "data/env2_skewnorm.pickle", PF.filter)
# sim = Simulator(env, robot, "data/env2_hwk3.pickle", KalmanFilter)
start = time.clock()
ground_truth, actual_path, in_collision = sim.simulate()
end = time.clock()
print "Execution time: ", end-start
ar = np.array(actual_path)
plt.plot(ar[:,0],ar[:,1],label='estimation')
gd = np.array(ground_truth)
class Auv:
def __init__(self, init_x, init_y, init_z, init_theta, init_v, init_w, planner_key_list, env_info):
"""
Initialilze an AUV class given the initial auv position and other configurations
Parameters:
init_x - initial x position
init_y - initial y position
init_z - initial z position
init_theta - initial theta position
init_v - nonzero, initial linear velocity
Warning: currently, no way to change the linear velocity
init_w - inital angular velocity
planner_key_list - Python list, indicate which planner(s) to use in this auv
TODO: check with MotionPlanner code - is this format ok?
env_info - Python dictionary, contain information neccessary for the planner
(e.g. boundary, obstacles, habitats)
TODO: check with MotionPlanner code - is this format ok?
"""
self.state = MotionPlanState(init_x, init_y, init_z, init_theta, init_v, init_w)
self.particle_filter = ParticleFilter()
self.motion_planner = MotionPlanner(planner_key_list, env_info)
self.curr_traj_pt_index = 0
self.TRAJ_LOOK_AHEAD_TIME = 0.5
# for plotting
self.x_list_plot = [init_x]
self.y_list_plot = [init_y]
self.z_list_plot = [init_z]
def run_auv_control_loop(self, shark_measurements, auv_comm_msgs, curr_time, delta_t):
"""
Run one iteration of the auv control loop
(Called by the worldSim class)
Parameters:
shark_measurements - Python list of MotionPlanState, represent the shark measurements
TODO: check with worldSim - what format?
auv_comm_msgs - Python list of dictionaries, representing the information sent by other auvs
TODO: check with MotionPlanner - is this format ok?
curr_time - in seconds, current time in the worldSim
delta_t - in seconds, time step
Return:
dictionary, representing the communication message sent from the auv
"""
shark_state_list = self.calc_shark_state(shark_measurements)
shark_state_estimates, shark_estate_estimation_err = self.particle_filter.estimate_shark_state(
self.state,
shark_state_list,
delta_t)
auv_trajectory, new_trajectory = self.motion_planner.plan_trajectory(self, shark_state_estimates, auv_comm_msgs)
auv_control_signals = self.control_based_on_traj(auv_trajectory, new_trajectory, curr_time)
self.update_auv_state(auv_control_signals, delta_t)
return self.create_auv_msg(shark_state_estimates, auv_trajectory)
def calc_shark_state(self, shark_measurements):
"""
Based on the shark_measurements, calculate additional shark information (related to the auv position)
which would get used in particle filter
Parameter:
shark_measurements - Python list of MotionPlanState, represent the shark measurements
TODO: check with worldSim - what format?
Return:
Python list of lists, each element is the measurement for a shark,
each element's format: [x_shark, y_shark, z_shark_range, z_shark_bearing, shark_id]
"""
shark_state_list = []
for shark in shark_measurements:
delta_x = shark.x - self.state.x
delta_y = shark.y - self.state.y
# added with Gaussian noise with 0 mean and standard deviation of 5
z_shark_range = math.sqrt(delta_x**2 + delta_y**2) + np.random.normal(0, 5)
# added with Gaussian noise with 0 mean and standard deviation of 0.5
z_shark_bearing = angle_wrap(math.atan2(delta_y, delta_x) - self.state.theta + np.random.normal(0, 0.5))
shark_state_list.append([shark.x, shark.y, z_shark_range, z_shark_bearing, shark.id])
return shark_state_list
def control_based_on_traj(self, auv_trajectory, new_trajectory, curr_time):
"""
Using the auv_trajectory from the motion planner to determine the linear and angular
velocity that the auv should have
Parameters:
trajectory - a list of trajectory points, where each element is
new_trajectory - boolean, True, auv_trajectory is a new trajectory (reset counter)
False, auv_trajectory is an old trajectory
curr_time - time in second, the current time in worldSim
Return:
linear velocity, angular velocity
the velocities that the auv should have to head towards the waypoint
"""
# control constant
K_P = 0.5
waypoint = self.find_waypoint_to_track(auv_trajectory, new_trajectory, curr_time)
angle_to_traj_point = math.atan2(self.state.x - self.state.y, waypoint.x - self.state.x)
self.state.w = K_P * angle_wrap(angle_to_traj_point - self.state.theta) #proportional control
return [self.state.v, self.state.w]
def find_waypoint_to_track(self, auv_trajectory, new_trajectory, curr_time):
"""
Return an MotionPlanState object representing the trajectory point TRAJ_LOOK_AHEAD_TIME sec ahead
of current time
Parameters:
trajectory - a list of trajectory points, where each element is
new_trajectory - boolean, True, auv_trajectory is a new trajectory (reset counter)
False, auv_trajectory is an old trajectory
curr_time - time in second, the current time in worldSim
Return:
a MotionPlanState object, which represents the waypoint to track
"""
# reset the trajectory index if it's a new trajectory
if new_trajectory == True:
self.curr_traj_pt_index = 0
# only increment the index if it hasn't reached the end of the trajectory list
while (self.curr_traj_pt_index < len(auv_trajectory)-1) and\
(curr_time + self.TRAJ_LOOK_AHEAD_TIME) > auv_trajectory[self.curr_traj_pt_index].traj_time_stamp:
self.curr_traj_pt_index += 1
return auv_trajectory[self.curr_traj_pt_index]
def update_auv_state(self, auv_control_signals, delta_t):
"""
Update the auv state (x, y, theta) based on the control signals
Parameters:
auv_control_signals - Python list, [linear_velocity, angular_velocity]
delta - time step (sec)
"""
v, w = auv_control_signals
self.state.x = self.state.x + v * math.cos(self.state.theta) * delta_t
self.state.y = self.state.y + v * math.sin(self.state.theta) * delta_t
self.state.theta = angle_wrap(self.state.theta + w * delta_t)
# add the new position for plotting
self.x_list_plot.append(self.state.x)
self.y_list_plot.append(self.state.y)
self.z_list_plot.append(self.state.z)
def create_auv_msg(self, shark_state_estimates, auv_trajectory):
"""
Create communication message that contain information about this auv
in the current iteration of control loop
Parameter:
shark_state_estimates - Python list, [x_estimated_shark, y_estimated_shark]
auv_trajectory - Python list, list of trajectory points from the auv planner
Return:
dictionary, contains
"auv_state": MotionPlanState object, which represents the current state of the auv
"shark_est": Python list, [x_estimated_shark, y_estimated_shark]
"auv_trajectory": Python list, list of trajectory points from the auv planner
TODO:
add type of planner
what the planner is planning for
"""
msg = {
"auv_state": self.state,
"shark_est": shark_state_estimates,
"auv_trajectory": auv_trajectory
}
return msg
def createFilter(self):
xHat, yHat = self.gps.read()
orientationHat = self.compass.read()
phiHat = self.wheelAngle.read()
vHat = self.speedometer.read()
self.particleFilter = ParticleFilter(Bike, xHat, yHat, orientationHat, phiHat, vHat, self.length, 500)
class QuadrocopterWorld(tk.Frame):
def __init__(self, generatedMap, master = None):
tk.Frame.__init__(self, master)
self.moveStep = 10
self.map = generatedMap
self.nearestBuildingIndexes = []
self.quadrocopter = Quadrocopter(generatedMap)
self.particleFilter = ParticleFilter(generatedMap)
self.particleFilter.createParticles(300)
self.master.bind("<Right>", self.moveRight)
self.master.bind("<Left>", self.moveLeft)
self.master.bind("<Up>", self.moveUp)
self.master.bind("<Down>", self.moveDown)
self.master.title("Quadrocopter World")
self.canvas = tk.Canvas(self.master, width = self.map.rows * 160, height = self.map.cols * 160)
self.canvas.pack()
def moveRight(self, event):
previousX = self.quadrocopter.x
self.quadrocopter.move(self.moveStep, True)
self.canvas.move(self.quadrocopterGraphics, math.fabs(self.quadrocopter.x - previousX), 0)
for i in range(len(self.particleFilter.particles)):
previousX = self.particleFilter.particles[i].x
self.particleFilter.particles[i].move(self.moveStep, True)
self.canvas.move(self.particlesGraphics[i], math.fabs(self.particleFilter.particles[i].x - previousX), 0)
self.sense()
def moveLeft(self, event):
previousX = self.quadrocopter.x
self.quadrocopter.move(-self.moveStep, True)
self.canvas.move(self.quadrocopterGraphics, -math.fabs(self.quadrocopter.x - previousX), 0)
for i in range(len(self.particleFilter.particles)):
previousX = self.particleFilter.particles[i].x
self.particleFilter.particles[i].move(-self.moveStep, True)
self.canvas.move(self.particlesGraphics[i], -math.fabs(self.particleFilter.particles[i].x - previousX), 0)
self.sense()
def moveUp(self, event):
previousY = self.quadrocopter.y
self.quadrocopter.move(-self.moveStep, False)
self.canvas.move(self.quadrocopterGraphics, 0, -math.fabs(self.quadrocopter.y - previousY))
for i in range(len(self.particleFilter.particles)):
previousY = self.particleFilter.particles[i].y
self.particleFilter.particles[i].move(-self.moveStep, False)
self.canvas.move(self.particlesGraphics[i], 0, -math.fabs(self.particleFilter.particles[i].y - previousY))
self.sense()
def moveDown(self, event):
previousY = self.quadrocopter.y
self.quadrocopter.move(self.moveStep, False)
self.canvas.move(self.quadrocopterGraphics, 0, math.fabs(self.quadrocopter.y - previousY))
for i in range(len(self.particleFilter.particles)):
previousY = self.particleFilter.particles[i].y
self.particleFilter.particles[i].move(self.moveStep, False)
self.canvas.move(self.particlesGraphics[i], 0, math.fabs(self.particleFilter.particles[i].y - previousY))
self.sense()
def sense(self):
# print 'quadro: ' + str(self.quadrocopter)
# print self.particleFilter.particles
for i in range(len(self.nearestBuildingIndexes)):
self.canvas.itemconfig(self.buildingsGraphics[self.nearestBuildingIndexes[i]], fill = self.previousColors[i])
self.previousColors = []
self.nearestBuildingIndexes = []
self.nearestBuildingIndexes, measurement = self.quadrocopter.sense()
for i in range(len(self.nearestBuildingIndexes)):
self.previousColors.append(self.canvas.itemcget(self.buildingsGraphics[self.nearestBuildingIndexes[i]], "fill"))
self.canvas.itemconfig(self.buildingsGraphics[self.nearestBuildingIndexes[i]], fill = "blue")
self.particleFilter.resampleParticles(measurement)
self.updateParticles()
def drawMap(self):
colors = ["#FF201A", "#FF301C", "#FF4820", "#FF6126", "#FF792C", "#FF8F32", "#FFA539", "#FFBB3F", "#FFCF46", "#FFE34C"]
self.buildingsGraphics = []
for i in range(self.map.num_of_bldgs):
self.buildingsGraphics.append(self.canvas.create_rectangle(self.map.x[i] * 8, self.map.y[i] * 8, self.map.x[i] * 8 + self.map.length[i] * 8, self.map.y[i] * 8 + self.map.width[i] * 8, fill = colors[9 - int(math.floor(self.map.height[i]))], tags = ('map')))
def drawParticles(self):
self.particlesGraphics = []
for i in range(len(self.particleFilter.particles)):
self.particlesGraphics.append(self.canvas.create_oval(self.particleFilter.particles[i].x, self.particleFilter.particles[i].y, self.particleFilter.particles[i].x + self.particleFilter.particles[i].iconSize, self.particleFilter.particles[i].y + self.particleFilter.particles[i].iconSize, fill = 'gray40', tags = ('particle')))
def updateParticles(self):
for i in range(len(self.particlesGraphics)):
self.canvas.coords(self.particlesGraphics[i], (self.particleFilter.particles[i].x, self.particleFilter.particles[i].y, self.particleFilter.particles[i].x + self.particleFilter.particles[i].iconSize, self.particleFilter.particles[i].y + self.particleFilter.particles[i].iconSize))
def drawQuadrocopter(self):
self.quadrocopterGraphics = self.canvas.create_oval(self.quadrocopter.x, self.quadrocopter.y, self.quadrocopter.x + self.quadrocopter.iconSize, self.quadrocopter.y + self.quadrocopter.iconSize, outline = 'black', fill = '#2D8B30', tags = ('quadrocopter'))
class ControlPlanner(Thread, Publisher):
'''
Determines what action to take to get the vehicle to where we want to go.
'''
#-------------------------------------------------------------------------------
def __init__(self,
group=None,
target=None,
name=None,
args=(),
kwargs={},
daemon=None,
courseMap=None,
sensorConversionThread=None):
'''
Initializes the control planner
@param Refer to the Python Thread class for documentation on all thread specific parameters
@param courseMap - the course map
@param sensorConversionThread - the sensor conversion thread
'''
Thread.__init__(self,
group=group,
target=target,
name=name,
daemon=daemon)
Publisher.__init__(self)
self.args = args
self.kwargs = kwargs
self.courseMap = courseMap
self.particleFilter = ParticleFilter(Constants.PARTICLE_NUMBER,
Constants.MAP_START_BOX,
Constants.MAP_HEADING_RANGE,
self.courseMap,
sensorConversionThread)
self.shutDown = False
self.estVehicleX = 0.0
self.estVehicleY = 0.0
self.estVehicleHeading = 0.0
self.covarVehicle = None
self.waypoint = 0
self.waypointCheck = 0
self.steeringAngleGoal = 0.0
self.velocityGoal = 0.0
#-------------------------------------------------------------------------------
def run(self):
'''
Runs the planner to set goals for where the vehicle is going to go
'''
while not self.shutDown:
self.estVehicleX, self.estVehicleY, self.estVehicleHeading, self.covarVehicle = self.particleFilter.getEstiamtedVehicleLocation(
)
self._checkWaypoint()
self._control()
self.particleFilter.prevTime = self.particleFilter.currentTime
sleepTime = (1.0 / Constants.CONTROL_UPDATE_RATE) - (
time.time() - self.particleFilter.currentTime)
if sleepTime > Constants.CONTROL_SLEEP_THRESHOLD:
time.sleep(sleepTime)
# TODO: Modify number of particles
#-------------------------------------------------------------------------------
def shutdown(self):
'''
Shutdown the thread
'''
self.shutDown = True
#-------------------------------------------------------------------------------
def _control(self):
'''
Send control to vehicle
'''
xErr = self.courseMap.waypoints[self.waypoint][
Constants.X] - self.estVehicleX
yErr = self.courseMap.waypoints[self.waypoint][
Constants.Y] - self.estVehicleY
xErr, yErr = rotate(xErr, yErr, -self.estVehicleHeading)
self.steeringAngleGoal = math.atan(Constants.VEHICLE_AXLE_LEN * (
(2.0 * yErr) / (math.pow(xErr, 2) + math.pow(yErr, 2))
)) * Constants.CONTROL_STEERING_AGRESSION
self.velocityGoal = max(
Constants.MIN_VEHICLE_VELOCITY, Constants.MAX_VEHICLE_VELOCITY -
math.atan(self.steeringAngleGoal) *
Constants.VELOCITY_SCALE_FACTOR)
# Publish goals
self.publish(self.velocityGoal, self.steeringAngleGoal)
#-------------------------------------------------------------------------------
def _checkWaypoint(self):
'''
Checks if the estimated vehicle location has passed the current waypoint goal.
If the vehicle has then increment to the next waypoint
'''
#print("DBG: waypoint = {0}".format(self.waypoint))
#print("DBG: len(courseMap.waypoints) = {0}".format(len(self.courseMap.waypoints)))
#print("DBG: courseMap.waypoints:")
#for wp in self.courseMap.waypoints:
# print("DBG: wp = {0}".format(wp))
rWpX, rWpY = rotate(
self.courseMap.waypoints[self.waypoint][Constants.X],
self.courseMap.waypoints[self.waypoint][Constants.Y],
-self.courseMap.waypoints[self.waypoint][Constants.HEADING])
rEstX, rEstY = rotate(
self.estVehicleX, self.estVehicleY,
-self.courseMap.waypoints[self.waypoint][Constants.HEADING])
if rEstX > (rWpX - Constants.WAYPOINT_CHECK_DIST):
self.waypointCheck += 1
else:
self.waypointCheck = 0
if self.waypointCheck >= Constants.WAYPOINT_MAX_CHECKS:
self.waypointCheck = 0
self.waypoint += 1
if self.waypoint > len(self.courseMap.waypoints):
self.waypoint = 0
# TODO: Add in switch cases for special points (NERF, stop, etc...). IDEA: Add a call back to the waypoint list, if it has a callback then call it otherwise no special function
#-------------------------------------------------------------------------------
def _debugDescription(self):
'''
Generates debugging information about the control planner
@return string describing debug information
'''
desc = "ControlPlanner:\n"
# TODO: add in setTabs for particle filter and course map
desc += "\tshutDown = {0}\n".format(self.shutDown)
desc += "\tparticleFilter = {0}\n".format(self.particleFilter)
desc += "\tcourseMap = {0}\n".format(self.courseMap)
desc += "\testVehicleX = {0}\n".format(self.estVehicleX)
desc += "\testVehicleY = {0}\n".format(self.estVehicleY)
desc += "\testVehicleHeading = {0}\n".format(self.estVehicleHeading)
desc += "\tcovarVehicle = {0}\n".format(self.covarVehicle)
desc += "\twaypoint = [{0}: {1}]\n".format(
self.waypoint, self.courseMap.waypoints[self.waypoint])
desc += "\twaypointCheck = {0}\n".format(self.waypointCheck)
desc += "\tvelocityGoal = {0}\n".format(self.velocityGoal)
desc += "\tsteeringAngleGoal = {0}\n".format(self.steeringAngleGoal)
return desc
#-------------------------------------------------------------------------------
def __repr__(self):
'''
Gets the string representation of the class
@return string representation of the class
'''
return self._debugDescription()
#-------------------------------------------------------------------------------
def __str__(self):
'''
Gets the string representation of the class
@return string representation of the class
'''
return self._debugDescription()
#-------------------------------------------------------------------------------
def __del__(self):
'''
Destructor
'''
self.join(timeout=5)
class MyFrontEnd(FrontEnd):
def __init__(self, omap_path, sparki):
self.omap = ObstacleMap(omap_path, noise_range=1)
self.rangefinder = Rangefinder(cone_width=deg2rad(15.),
obstacle_width=1.)
FrontEnd.__init__(self, self.omap.width, self.omap.height)
self.sparki = sparki
self.robot = Robot()
# center robot
self.robot.x = self.omap.width * 0.5
self.robot.y = self.omap.height * 0.5
# create particle filter
alpha = [0.5, 0.5, 0.5, 0.5]
self.particle_filter = ParticleFilter(num_particles=50,
alpha=alpha,
robot=self.robot,
omap=self.omap)
# set message callback
self.sparki.message_callback = self.message_received
# last timestamp received
self.last_timestamp = 0
def keydown(self, key):
# update velocities based on key pressed
if key == pygame.K_UP: # set positive linear velocity
self.robot.requested_lin_vel = 2.0
elif key == pygame.K_DOWN: # set negative linear velocity
self.robot.requested_lin_vel = -2.0
elif key == pygame.K_LEFT: # set positive angular velocity
self.robot.requested_ang_vel = 10. * math.pi / 180.
elif key == pygame.K_RIGHT: # set negative angular velocity
self.robot.requested_ang_vel = -10. * math.pi / 180.
elif key == pygame.K_k: # set positive servo angle
self.robot.requested_sonar_angle = 45. * math.pi / 180.
elif key == pygame.K_l: # set negative servo angle
self.robot.requested_sonar_angle = -45. * math.pi / 180.
def keyup(self, key):
# update velocities based on key released
if key == pygame.K_UP or key == pygame.K_DOWN: # set zero linear velocity
self.robot.requested_lin_vel = 0
elif key == pygame.K_LEFT or key == pygame.K_RIGHT: # set zero angular velocity
self.robot.requested_ang_vel = 0
elif key == pygame.K_k or key == pygame.K_l: # set zero servo angle
self.robot.requested_sonar_angle = 0
def draw(self, surface):
# draw obstacle map
self.omap.draw(surface)
# draw robot
self.robot.draw(surface)
# draw particles
self.particle_filter.draw(surface)
def update(self):
""" Sends command to robot, if not in simulator mode. """
if self.sparki.port is not '':
# calculate servo setting
servo = int(self.robot.requested_sonar_angle * 180. / math.pi)
# calculate motor settings
left_speed, left_dir, right_speed, right_dir = self.robot.compute_motors(
)
# send command
self.sparki.send_command(left_speed, left_dir, right_speed,
right_dir, servo)
def message_received(self, message):
""" Callback for when a message is received from the robot or the simulator.
Arguments:
message: dictionary with status values
"""
# check if there is no previous timestamp
if self.last_timestamp == 0:
self.last_timestamp = message['timestamp']
return
# calculate time_delta based on the current and previous timestamps
time_delta = (message['timestamp'] - self.last_timestamp) / 1000.
self.last_timestamp = message['timestamp']
# print(time_delta)
# update linear and angular velocities
if 'left_motor_speed' in message.keys():
# calculate linear and angular velocities from reported motor settings
left_speed = message['left_motor_speed']
left_dir = message['left_motor_dir']
right_speed = message['right_motor_speed']
right_dir = message['right_motor_dir']
# print(left_speed,left_dir,right_speed,right_dir)
self.robot.compute_velocities(left_speed, left_dir, right_speed,
right_dir)
else:
# copy requested velocities to actual velocities
self.robot.lin_vel = self.robot.requested_lin_vel
self.robot.ang_vel = self.robot.requested_ang_vel
# update robot position using forward kinematics
self.robot.update(time_delta)
# update sonar ping distance
if 'rangefinder' in message.keys():
# convert to cm
self.robot.sonar_distance = message['rangefinder']
else:
# simulate rangefinder reading
T_sonar_map = self.robot.get_robot_map_transform(
) * self.robot.get_sonar_robot_transform()
self.robot.sonar_distance = self.omap.get_first_hit(T_sonar_map)
# update particles
self.particle_filter.generate(time_delta)
self.particle_filter.update()
self.particle_filter.sample()
FPS = 25 # Choose what filter(s) to display USE_PARTICLE_FILTER = False USE_MATHEW_CUSTOM_FILTER = False USE_MATHEW_PARTICLE_FILTER = True TRUNCATE_INITIAL_DATA = False # The ball objects that are updated and plotted old_filter_ball = Ball(0, 0) particle_ball = Ball(0, 0) mathew_custom_ball = Ball(0, 0) mathew_particle_ball = Ball(0, 0) pFilter = ParticleFilter(FIELD_LENGTH, FIELD_WIDTH) mathewCustomFilter = MFilter(FIELD_LENGTH, FIELD_WIDTH) mathewParticleFilter = MathewParticleFilter(FIELD_LENGTH, FIELD_WIDTH) # The simulator # ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~# # variables to hold computed/read data from logfile all_balls = [] # the list of balls detected from vision (each tick) all_balls_data = [] old_filter_ball_data = [] particle_ball_data = [] mathew_custom_ball_data = [] mathew_particle_ball_data = [] friendly_1_data = [] friendly_2_data = []
odometryData = OdometryData(float(odometry[0]), float(odometry[1]), float(odometry[2]))
particleFilter.updateParticlesWithOdometry(odometryData)
shouldUpdateParticleWeights = True
elif string == "ANGLE":
rawSize = conn.recv(4)
size = struct.unpack("i", rawSize)[0]
data = conn.recv(size)
currentAngle = pickle.loads(data)
particleFilter.robotAngle = float(currentAngle)
shouldUpdateParticleWeights = True
HOST = 'localhost'
PORT = 50040
s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
s.bind((HOST, PORT))
s.listen(2)
particleFilter = ParticleFilter()
gps = []
currentAngle = 0
while 1:
conn, addr = s.accept()
print 'Connected by', addr
thread.start_new_thread(handleConnection,(conn,addr))
conn.close()
# plt.imshow(outer, cmap='gray')
class Localization:
def __init__(self, x, y, yaw, side, button=True):
b_time = utime.ticks_ms()
print(b_time)
side_loop = 0
side = False
while (side_loop == 0):
if (pin9.value() == 0): # нажатие на кнопку на голове
side_loop = 1
side = True
print("I will attack blue goal")
break
if (utime.ticks_ms() - b_time) > 1500:
print("I will attack yellow goal")
break
b_time = utime.ticks_ms()
side_loop = 0
s_coord = 0
while (side_loop == 0):
if (pin3.value() == 0):
side_loop
s_coord = 2
print("right")
break
if (pin2.value() == 0):
side_loop
s_coord = 1
print("left")
break
if (utime.ticks_ms() - b_time) > 1500:
print("centr")
break
with open("localization/start_coord.json", "r") as f:
start_coord = json.loads(f.read())
self.robot_position = tuple(start_coord[str(s_coord)])
print(self.robot_position[2])
self.ballPosSelf = None
self.ball_position = None
self.localized = False
self.seeBall = False
self.side = False
with open("localization/landmarks.json", "r") as f:
landmarks = json.loads(f.read())
# choice side
if side:
colors = []
for goal_color in landmarks:
colors.append(goal_color)
neutral = landmarks[colors[0]]
landmarks[colors[0]] = landmarks[colors[1]]
landmarks[colors[1]] = neutral
if button:
x, y, yaw = self.robot_position
self.pf = ParticleFilter(Robot(x, y, yaw),
Field("localization/parfield.json"),
landmarks)
def update(self, data):
self.robot_position = updatePF(self.pf, data)
if self.pf.consistency > 0.5:
self.localized = True
else:
self.localized = False
def update_ball(self, data):
if len(data['ball']) != 0:
self.seeBall = True
self.ballPosSelf = median(data["ball"])
#min(data["ball"], key=lambda x: x[0]**2 + x[1]**2)
#print("ballPosSelf = ", self.ballPosSelf)
bx = self.ballPosSelf[0]
by = self.ballPosSelf[1]
x_ball = self.pf.myrobot.x + bx * \
math.cos(self.pf.myrobot.yaw) - by * \
math.sin(self.pf.myrobot.yaw)
y_ball = self.pf.myrobot.y + bx * \
math.sin(self.pf.myrobot.yaw) + by * \
math.cos(self.pf.myrobot.yaw)
self.ball_position = (x_ball, y_ball)
#self.ball_position = ()
print("eto ball", self.ball_position)
else:
self.seeBall = False
def move(self, odometry):
self.pf.particles_move(odometry)
showImages = True
vrep_connection = VrepConnection("127.0.0.1",
25000,
force_finish_comm=False)
image_getter = ImageGetter(vrep_connection, "NAO_vision1")
depth_getter = ImageGetter(vrep_connection, "NAO_vision3")
pose_getter = PoseGetter(vrep_connection, "NAO")
obstacle_interpreter = ObstacleInterpreter(number_classes=number_classes,
correct_depth=False)
particle_map = ParticleMap(objects=np.array([[900, 430, 2, -1],
[900, 170, 2, 1],
[900, 300, 2, 0],
[900, 250, 1, 0]]),
number_particles=2000)
particle_filter = ParticleFilter(particle_map,
number_classes=number_classes)
kinetic_control = KineticControl()
robot = Robot(vrep_connection,
image_getter,
depth_getter,
pose_getter,
obstacle_interpreter,
particle_filter,
model,
kinetic_control,
show_images=True)
for i in range(10):
robot.kinetic_control.do_movement()
time.sleep(0.2)
class Simulator:
robot = None
def __init__ (self, world, width=1000, height=800):
self.width = width
self.height = height
self.master = Tk()
self.canvas = Canvas(self.master, width=width, height=height)
canvas = self.canvas
canvas.pack()
self.world = world
world.display(canvas)
self.localizer = ParticleFilter(N=3000, width=width, height=height)
localizer = self.localizer
localizer.display(canvas)
def interactive (self):
"""Start interactive mode (doesn't return)"""
print "Click anywhere on the canvas to place the robot"
def callback (event):
print "@", event.x, ",", event.y
self.place_robot(event.x, event.y)
self.canvas.bind("<Button-1>", callback)
mainloop()
def explore (self, x, y, moves, delay = 1/2):
self.place_robot(x, y)
for (x, y) in moves:
self.move_robot(x, y)
time.sleep(delay)
def measurement_probabilty (self, particle, Z):
loss = self.world.binary_loss(particle, Z)
if loss:
particle.color = "blue"
else:
particle. color = "gray"
return loss
def move_robot(self, rotation, distance):
robot = self.robot
canvas = self.canvas
localizer = self.localizer
if not robot:
raise ValueError, "Need to place robot in simulator before moving it"
original_x = robot.x
original_y = robot.y
robot.move(rotation, distance)
if robot.color and not robot.color == "None":
canvas.create_line(original_x, original_y, robot.x, robot.y)
Z = self.world.surface (robot.x, robot.y)
self.localizer.erase(canvas)
localizer.update(rotation, distance, lambda particle: self.measurement_probabilty(particle, Z))
localizer.display(canvas)
robot.display(canvas)
self.master.update()
print "Sense:", Z
def place_robot(self, x = None, y = None, bearing=None, color = "green"):
"""Move the robot to the given position on the canvas"""
if not self.robot:
land = self.world.terrain[0]
if not x: x = random.randint(land[0], land[2])
if not y: y = random.randint(land[1], land[3])
self.robot = Robot (x, y)
self.robot.display_noise = 0.0
self.robot.color = color
self.robot.size = 5
robot = self.robot
if not bearing:
bearing = atan2((y - robot.y), (x - robot.x))
rotation = bearing - robot.orientation
distance = sqrt((robot.x - x) ** 2 + (robot.y - y) ** 2)
self.move_robot(rotation, distance)
return self.robot
import numpy as np
import matplotlib.pyplot as plt
import datetime
from math import log10, pow, sin, cos, radians
from Calculator import Calculator
from Database import Database
from Plotter import Plotter
from ParticleFilter import ParticleFilter
MACADD = "18:64:72:56:84:b3"
DB = Database()
CA = Calculator()
PL = Plotter()
PF = ParticleFilter()
PF.compareDistancesWithSignalStrength()
"""
(X, Y, Z) = CA.generateCircle(RSSI=-70, x=1, y=2, z=1)
plt.scatter(X, Y, s=Z)
plt.show()
"""
def iteratePlotting():
# List out all the MAC addresses
SUTD_Staff_MACs = DB.viewMACs()
# Read all wifi scan results from database
(result_set1, result_set2, result_set3) = DB.generateResultSets()
# Process three result sets
for address in SUTD_Staff_MACs:
class Vehicle(Body):
'''
The overal Vehicle class pull together all other components
'''
def __init__(self, length, fig, ax):
self.engine = Actuator()
self.steering = Actuator()
self.speedometer = Sensor()
self.wheelAngle = Sensor()
self.gps = Sensor()
self.compass = Sensor()
self.laneTracker = Sensor()
self.lidar = Sensor()
self.lidarArray = []
self.pilot = Pilot()
self.planner = Planner()
self.vizualizer = Vizualizer(length, fig, ax)
self.length = length
self.maxLonAcc = 10
self.particleFilter = None
def getPos(self):
# return self.particleFilter.getPosition()
x, y = self.gps.read()
return x[0], y[0]
def getOrientation(self):
return self.compass.read()
def getVelocity(self):
return self.speedometer.read()
def getAcc(self):
return self.engine.getValue()
def getOmega(self):
return self.steering.getValue()
def headingTracker(self, dt):
'''
Wrapper for the heading controller
'''
x, y = self.gps.read()
# x, y = self.particleFilter.getPosition()
orientation = self.compass.read()
phi = self.wheelAngle.read()
v = self.speedometer.read()
# x_road, y_road, angle_road, v_d = self.planner.getGoal(x, y, v, dt)
x_road, y_road, angle_road, v_d = self.planner.getPath(x, y)
x_road, y_road, angle_road, v_d = self.planner.getPath(x[0], y[0])
x_road += cos(angle_road)*v_d*dt*10
y_road += sin(angle_road)*v_d*dt*10
return self.pilot.headingTracker(x, y, orientation,
phi, v, self.length,
x_road, y_road, angle_road, v_d,
dt)
def lqrTracker(self, dt):
'''
Wrapper for the heading controller
'''
x, y = self.gps.read()
#
# x, y = self.particleFilter.getPosition()
orientation = self.compass.read()
phi = self.wheelAngle.read()
v = self.speedometer.read()
x_road, y_road, angle_road, v_d = self.planner.getPath(x[0], y[0])
# x_road, y_road, angle_road, v_d = self.planner.getGoal(x, y, v, dt)
return self.pilot.lqrTracker(x[0], y[0], orientation,
phi, v, self.length, self.planner,
x_road, y_road, angle_road, v_d,
dt)
def phiController(self, dt):
'''
Wrapper for the phi controller
'''
x, y = self.gps.read()
# x, y = self.particleFilter.getPosition()
orientation = self.compass.read()
phi = self.wheelAngle.read()
v = self.speedometer.read()
x_road, y_road, angle_road, v_d = self.planner.getGoal(x, y, v, dt)
return self.pilot.phiController(x, y, orientation, phi,
x_road, y_road, angle_road,
dt)
def cteController(self, dt):
'''
Wrapper for the Cross Track Error controller
'''
x, y = self.gps.read()
# x, y = self.particleFilter.getPosition()
return self.pilot.crossTrackErrorController(x, y, self.planner, dt)
def velocityController(self, dt):
'''
Wrapper for the velocity controller
'''
x, y = self.gps.read()
# x, y = self.particleFilter.getPosition()
v = self.speedometer.read()
# _, _, _, v_ref = self.planner.getGoal(x, y, v, dt)
x_road, y_road, angle_road, v_d = self.planner.getPath(x[0], y[0])
return self.pilot.velocityController(v, v_d, self.maxLonAcc, histeresis = 0.1)
# def addLIDAR(self, lidar):
# self.lidarArray.append(lidar)
def connectToEngine(self, function):
'''
Adds a connection to the engine
'''
self.engine.connect(function)
def connectToSteering(self, function):
'''
Adds a connection to the steering wheel
'''
self.steering.connect(function)
def connectToSpeedometer(self, function):
'''
Adds a connection to the velocimeter
'''
self.speedometer.connect(function)
def connectToWheelAngle(self, function):
'''
Adds a connection to system to measure the steering wheel angle
'''
self.wheelAngle.connect(function)
def connectToGPS(self, function):
'''
Adds a connection to system that captures the vehicle position
'''
self.gps.connect(function)
def connectToCompass(self, function):
'''
Adds a connection to system that captures the vehicle orientation
'''
self.compass.connect(function)
def connectToLidar(self, function):
'''
Adds a connection to system that captures the enviroment
'''
self.lidar.connect(function)
def connectToLaneTracker(self, function):
'''
Adds a connection to system that captures the vehicle orientation
'''
self.laneTracker.connect(function)
def connectToSimulation(self, simulationVehicle, laneTracker, simulationEnviroment = None):
'''
Wrapper to connect all system to simulation model
'''
self.connectToEngine(simulationVehicle.setAcceleration)
self.connectToSteering(simulationVehicle.setOmega)
self.connectToCompass(simulationVehicle.getOrientation)
self.connectToSpeedometer(simulationVehicle.getVelocity)
self.connectToGPS(simulationVehicle.getPosition)
self.connectToWheelAngle(simulationVehicle.getSteering)
if(simulationEnviroment is not None):
self.connectToLidar(simulationEnviroment.getPointMap)
else:
self.connectToLidar(lambda : 0)
self.connectToLaneTracker(laneTracker)
def scan(self):
'''
Do a scan round through all of the sensors
'''
self.speedometer.scan()
self.compass.scan()
self.gps.scan()
self.wheelAngle.scan()
self.laneTracker.scan()
self.lidar.scan()
def createFilter(self):
xHat, yHat = self.gps.read()
orientationHat = self.compass.read()
phiHat = self.wheelAngle.read()
vHat = self.speedometer.read()
self.particleFilter = ParticleFilter(Bike, xHat, yHat, orientationHat, phiHat, vHat, self.length, 500)
def updateFilter(self, dt):
xHat, yHat = self.gps.read()
xLane, yLane = self.laneTracker.read()
orientationHat = self.compass.read()
phiHat = self.wheelAngle.read()
vHat = self.speedometer.read()
acc = self.engine.getValue()
omega = self.steering.getValue()
measurements = [ xHat, yHat, orientationHat, phiHat, vHat]
V = [self.gps.getUncertanty(),
self.gps.getUncertanty(),
self.compass.getUncertanty(),
self.wheelAngle.getUncertanty(),
self.speedometer.getUncertanty()]
# measurements = [ xHat, yHat]
# V = [self.gps.getUncertanty(), self.gps.getUncertanty()]
self.particleFilter.updateParticles(acc, omega, dt)
self.particleFilter.weightParticles(measurements, V)
self.particleFilter.resampleParticles()
# measurements = [xLane, yLane]
# V = [self.laneTracker.getUncertanty(), self.laneTracker.getUncertanty()]
# self.particleFilter.weightParticles(measurements, V)
# self.particleFilter.resampleParticles()
def plot(self, draw = True):
'''
Wrapper for the vizualizer to update the plot
'''
x, y = self.gps.read()
orientation = self.compass.read()
# x_goal, y_goal = self.planner.nextX, self.planner.nextY
self.vizualizer.plot(x, y, orientation, self.length, draw)
def plot3d(self):
'''
Wrapper for the 3d plotting of the vizualizer. Not yet implement
'''
x, y = self.gps.read()
orientation = self.compass.read()
self.vizualizer.plot3d(x, y, orientation)
class CrocoPf(CrocO):
'''
class meant to perform LOCAL runs of the pf
'''
def __init__(self, options, setup=True):
CrocO.__init__(self, options)
# for the local soda PF, it is safer if mblist is a continous range (no removal of the synth mb but one mb less.
# handling of the synth member is done in prepare_soda_env
self.mblist = list(range(1, options.nmembers + 1))
# setup all dirs
if setup is True:
self.setup()
def setup(self):
if not os.path.exists(self.crocOdir):
os.mkdir(self.crocOdir)
os.chdir(self.crocOdir)
if self.options.todo != 'parallel':
saverep = self.options.saverep
if not os.path.exists(saverep):
os.mkdir(saverep)
else:
saverep = ''
os.chdir(self.crocOdir + '/' + saverep)
for dd in self.options.dates:
if self.options.todo == 'parallel':
if self.options.pf != 'ol':
path = self.crocOdir + '/' + saverep + '/' + dd + '/workSODA'
else: # no need to create workSODA in openloop case
path = self.crocOdir + '/' + saverep + '/' + dd + '/'
else:
path = self.crocOdir + '/' + saverep + '/' + dd
if dd in self.options.dates:
if os.path.exists(path):
# bc slight change for local tests where it is painful to have the rep deleted each time. (pwd error)
for dirpath, _, filenames in os.walk(path):
# Remove regular files, ignore directories
for filename in filenames:
os.remove(os.path.join(dirpath, filename))
else:
os.makedirs(path)
if self.options.pf != 'ol':
self.prepare_sodaenv(path, dd)
else:
print(('prescribed date ' + dd +
'does not exist in the experiment, remove it.'))
self.options.dates.remove(dd)
def prepare_sodaenv(self, path, date):
"""
set soda environment for each date (=path): -PGD, links to preps, namelist, ecoclimap etc.
"""
cwd = os.getcwd()
os.chdir(path)
if not os.path.exists('PGD.nc'):
os.symlink(self.options.pathPgd, 'PGD.nc')
self.prepare_namelist()
# in the parallel case, the namelist is checked by CrocOparallel class
if self.options.todo != 'parallel':
check_namelist_soda(self.options)
else:
os.rename('OPTIONS_base.nam', 'OPTIONS.nam')
if 'sxcen' not in self.machine:
# prepare ecoclimap binaries
if not os.path.exists('ecoclimapI_covers_param.bin'):
os.symlink(
self.exesurfex +
'/../MY_RUN/ECOCLIMAP/ecoclimapI_covers_param.bin',
'ecoclimapI_covers_param.bin')
os.symlink(
self.exesurfex +
'/../MY_RUN/ECOCLIMAP/ecoclimapII_eu_covers_param.bin',
'ecoclimapII_eu_covers_param.bin')
# flanner stuff
os.symlink(
self.exesurfex +
'/../MY_RUN//DATA/CROCUS/drdt_bst_fit_60.nc',
'drdt_bst_fit_60.nc')
if not os.path.exists('soda.exe'):
os.symlink(self.exesurfex + '/SODA', 'soda.exe')
# prepare (get or fake) the obs
self.prepare_obs(date)
# prepare the pgd and prep files.
# in local parallel sequence, must be done only once the corresponding escroc run has produced the files
if self.options.todo != 'parallel':
self.prepare_preps(date)
os.chdir(cwd)
def prepare_preps(self, date):
'''
Prepare the PREP files for SODA.
'''
dateAssSoda = convertdate(date).strftime('%y%m%dH%H')
for mb in self.mblist:
if self.options.synth is not None:
if mb < self.options.synth:
self.build_link(mb, mb, date, dateAssSoda)
else:
self.build_link(mb + 1, mb, date, dateAssSoda)
else:
self.build_link(mb, mb, date, dateAssSoda)
# a link fro PREP...1.nc to PREP.nc is also necessary for SODA
if not os.path.exists('PREP.nc'):
os.symlink('PREP_' + dateAssSoda + '_PF_ENS1.nc', 'PREP.nc')
def build_link(self, mb, imb, date, dateAssSoda):
if self.options.todo != 'parallel':
try:
os.symlink(
self.xpiddir + 'mb{0:04d}'.format(mb) + '/bg/PREP_' +
date + '.nc',
'PREP_' + dateAssSoda + '_PF_ENS' + str(imb) + '.nc')
except Exception:
os.remove('PREP_' + date + '_PF_ENS' + str(imb + 1) + '.nc')
os.symlink(
self.xpiddir + 'mb{0:04d}'.format(mb) + '/bg/PREP_' +
date + '.nc',
'PREP_' + dateAssSoda + '_PF_ENS' + str(imb) + '.nc')
else: # in parallel mode, bg and an reps do not exist.
try:
os.symlink(
self.xpiddir + date + '/mb{0:04d}'.format(mb) +
'/SURFOUT.nc',
'PREP_' + dateAssSoda + '_PF_ENS' + str(imb) + '.nc')
except Exception:
os.remove('PREP_' + date + '_PF_ENS' + str(imb + 1) + '.nc')
os.symlink(
self.xpiddir + date + '/mb{0:04d}'.format(mb) +
'/SURFOUT.nc',
'PREP_' + dateAssSoda + '_PF_ENS' + str(imb) + '.nc')
def run(self):
"""spawn soda in each date directory"""
os.system('ulimit -s unlimited')
for dd in self.options.dates:
os.chdir(dd)
if self.options.todo != 'pfpython':
os.system('./soda.exe')
else:
# BC April 2020
# nice but not maintaned and deprecated class
# which allowed to test and develop locally python version of the PF
# before implementing it into fortran.
plot = True
if plot:
plt.figure()
self.pf = ParticleFilter(self.xpiddir, self.options, dd)
_, resample = self.pf.localize(k=self.options.pgd.npts,
errobs_factor=self.options.fact,
plot=plot)
_, gresample = self.pf.globalize(
errobs_factor=self.options.fact, plot=plot)
print(('gres', gresample))
self.pf.pag = self.pf.analyze(gresample)
self.pf.pal = self.pf.analyze(resample)
if plot:
plt.show()
os.chdir('..')
def run_parallel(self, date):
"""
This method allows to run soda inside a parallelized assimilation sequence.
/!\ soda is not parallelized (NOMPI mode).
"""
os.chdir(self.xpiddir + date + '/workSODA')
self.prepare_preps(date)
# print('launching SODA on ', date)
with open('soda.out', 'w') as f:
p = subprocess.call('./soda.exe', stdout=f)
if p != 0:
raise RuntimeError('SODA crashed, check ', os.getcwd() + f)
os.chdir('..')
import os
import sys
import csv
import cv2
import glob
import numpy as np
from ParticleFilter import ParticleFilter
if __name__ == "__main__":
cv2.namedWindow('Lane Markers')
imgs = glob.glob("images/*.png")
intercepts = []
xl_int_pf=ParticleFilter(N=1000,x_range=(0,1500),sensor_err=1,par_std=100)
xl_phs_pf=ParticleFilter(N=1000,x_range=(15,90),sensor_err=0.3,par_std=1)
xr_int_pf=ParticleFilter(N=1000,x_range=(100,1800),sensor_err=1,par_std=100)
xr_phs_pf=ParticleFilter(N=1000,x_range=(15,90),sensor_err=0.3,par_std=1)
#tracking queues
xl_int_q = [0]*15
xl_phs_q = [0]*15
count = 0
# imgs = ['images/mono_0000002062.png']
for fname in imgs:
# Load image and prepare output image
orig_img = cv2.imread(fname)
if __name__ == '__main__':
# Initialize ***
# coordinates in the middle of a room
initx = 170
inity = 150
the_map = TrueMap(floorplan='BinaryMaps/MD_MINI_binary.png')
# the_map = TrueMap()
viz = Visualization(the_map, start_x=initx, start_y=inity)
sensor = Sensor(the_map)
odom = Odometry(the_map, start_x=initx, start_y=inity)
part_filt = ParticleFilter(the_map, sensor, odom, numParticles=100)
part_filt.initializeParticles()
scale = 7 # number of pixels to move each time a movement is given
# assuming a move command represents a 1 pixel move
moveCommandDeltas = {'a':(0, -scale), 'w':(-scale, 0), 's':(scale, 0),'d':(0, scale), 'aw':(-scale, -scale), 'wa':(-scale,-scale),
'wd':(-scale, scale) , 'dw':(-scale, scale) , 'sd':(scale,scale), 'ds':(scale,scale),
'as':(scale, -scale), 'sa':(scale, -scale)}
# Do some stuff ***
viz.replot_particles(part_filt.getParticleLocations(), numtoplot=100)
viz.display_plot()
step_count = 0
while True:
class MonteCarloLocalization:
def __init__(self, num_particles, xmin, xmax, ymin, ymax):
# Initialize node
rospy.init_node("MonteCarloLocalization")
# Get map from map server
print("Wait for static_map from map server...")
rospy.wait_for_service('static_map')
map = rospy.ServiceProxy("static_map", GetMap)
resp1 = map()
self.grid_map = resp1.map
print("Map resolution: " + str(self.grid_map.info.resolution))
print("Map loaded.")
self.particle_filter = ParticleFilter(num_particles, self.grid_map,
xmin,xmax,ymin,ymax,
0,0,0,0,
0.25, # translation
0.1, # orientation
0.3) # measurement
self.particle_filter.init_particles()
self.last_scan = None
self.mutex = Lock()
self.particles_pub = rospy.Publisher('/particle_filter/particles', MarkerArray, queue_size=1)
self.mcl_estimate_pose_pub = rospy.Publisher('/mcl_estimate_pose', PoseStamped, queue_size=1)
self.publish_fake_scan = rospy.Publisher('/fake_scan', LaserScan, queue_size=1)
rospy.Subscriber('/base_pose_ground_truth', Odometry, self.odometry_callback, queue_size=1)
rospy.Subscriber('/scan', LaserScan, self.laser_callback, queue_size=1)
self.first_laser_flag = True
self.odom_initialized = False
self.laser_initialized = False
self.mutex = Lock()
#********** Callbacks **********#
def odometry_callback(self, msg):
self.particle_filter.robot_odom = msg
if not self.odom_initialized: self.odom_initialized = True
def laser_callback(self, msg):
if not self.laser_initialized:
print("Got first laser callback.")
self.particle_filter.laser_min_angle = msg.angle_min
self.particle_filter.laser_max_angle = msg.angle_max
self.particle_filter.laser_min_range = msg.range_min
self.particle_filter.laser_max_range = msg.range_max
self.laser_initialized = True
self.laser_data = msg
# -------------------------------- #
def publish_mcl_estimate_pose(self):
estimate_pose = PoseStamped()
# Populate
estimate_pose.header.stamp = rospy.Time.now()
estimate_pose.header.frame_id = "map"
estimate_pose.pose.position.x = self.particle_filter.mean_estimate_particle.x
estimate_pose.pose.position.y = self.particle_filter.mean_estimate_particle.y
quat_data = tr.quaternion_from_euler(0,0, self.particle_filter.mean_estimate_particle.theta)
estimate_pose.pose.orientation.z = quat_data[2]
estimate_pose.pose.orientation.w = quat_data[3]
# Publish
self.mcl_estimate_pose_pub.publish(estimate_pose)
#********** Publishers ************#
def publish_particle_markers(self):
marker_array = MarkerArray()
timestamp = rospy.Time.now()
for i in range(len(self.particle_filter.particles)):
marker_array.markers.append(self.get_rviz_particle_marker(timestamp, self.particle_filter.particles[i], i))
self.particles_pub.publish(marker_array)
#----------------------------------#
def get_rviz_particle_marker(self, timestamp, particle, marker_id):
"""Returns an rviz marker that visualizes a single particle"""
msg = Marker()
msg.header.stamp = timestamp
msg.header.frame_id = 'map'
msg.ns = 'particles'
msg.id = marker_id
msg.type = 0
msg.action = 0
msg.lifetime = rospy.Duration(1)
yaw_in_map = particle.theta
vx = cos(yaw_in_map)
vy = sin(yaw_in_map)
msg.color = ColorRGBA(0, 1.0, 0, 1.0)
msg.points.append(Point(particle.x, particle.y, 0.2))
msg.points.append(Point(particle.x + 0.3*vx, particle.y + 0.3*vy, 0.2))
msg.scale.x = 0.05
msg.scale.y = 0.05
msg.scale.z = 0.1
return msg
def run(self):
rate = rospy.Rate(5)
while not rospy.is_shutdown():
self.publish_particle_markers()
self.run_mcl()
rate.sleep()
def run_mcl(self):
if self.odom_initialized and self.laser_initialized:
odom = copy.deepcopy(self.particle_filter.robot_odom)
laser = copy.deepcopy(self.laser_data)
# print "old samples"
self.particle_filter.sample_motion_model_odometry(odom)
self.particle_filter.handle_observation(laser)
# Re-sample particles based on weight
particle_indices = np.arange(self.particle_filter.num_particles)
new_particles_indices = np.random.choice(particle_indices, size=self.particle_filter.num_particles,
p=self.particle_filter.weights)
current_particles = copy.deepcopy(self.particle_filter.particles)
new_particles = [copy.deepcopy(current_particles[i]) for i in new_particles_indices]
self.particle_filter.particles = copy.deepcopy(new_particles)
self.particle_filter.calculate_mean_particle()
self.publish_scan()
self.publish_mcl_estimate_pose()
def publish_scan(self):
ls = LaserScan()
ls.header.stamp = rospy.Time.now()
ls.header.frame_id = "base_laser_link"
ls.angle_min = np.min(self.particle_filter.ls_angles)
ls.angle_max = np.max(self.particle_filter.ls_angles)
ls.angle_increment = np.abs(self.particle_filter.ls_angles[0] - self.particle_filter.ls_angles[1])
ls.range_min = 0.0
ls.range_max = np.max(self.particle_filter.ls_ranges)
ls.ranges = self.particle_filter.ls_ranges
self.publish_fake_scan.publish(ls)
