Skip to content

Commit 1dd55de

Browse files
lukovicaleksalukovicaleksa
authored
Add files via upload
1 parent 9a3d20b commit 1dd55de

14 files changed

+1094
-0
lines changed

‎CMakeLists.txt

+21
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,21 @@
1+
cmake_minimum_required(VERSION 3.0.2)
2+
project(pf_localization)
3+
4+
## Find catkin and any catkin packages
5+
find_package(catkin REQUIRED COMPONENTS rospy std_msgs )
6+
7+
## Declare a catkin package
8+
catkin_package()
9+
10+
# Install python scripts
11+
catkin_install_python(PROGRAMS scripts/pf_algo.py
12+
scripts/pf_classes.py
13+
scripts/pf_functions.py
14+
scripts/pf_collect_data_node.py
15+
scripts/plot_map_features.py
16+
scripts/scan_node.py
17+
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
18+
)
19+
20+
## Build talker and listener
21+
include_directories(include ${catkin_INCLUDE_DIRS})

‎Maps/stage_1/Wall_0.csv

+4
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,4 @@
1+
-1.850000000000000089e+00
2+
1.850000000000000089e+00
3+
1.850000000000000089e+00
4+
1.850000000000000089e+00

‎Maps/stage_1/Wall_2.csv

+4
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,4 @@
1+
-1.849993204571064753e+00
2+
-1.850006795429947060e+00
3+
1.849999999987519406e+00
4+
-1.849999999987519406e+00

‎Maps/stage_1/Wall_3.csv

+4
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,4 @@
1+
-1.850000000000000089e+00
2+
1.850000000000000089e+00
3+
-1.850000000000000089e+00
4+
-1.850000000000000089e+00

‎Maps/stage_1/Wall_4.csv

+4
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,4 @@
1+
1.850006795429947060e+00
2+
1.849993204571064753e+00
3+
-1.849999999987519406e+00
4+
1.849999999987519406e+00

‎package.xml

+65
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,65 @@
1+
<?xml version="1.0"?>
2+
<package format="2">
3+
<name>pf_localization</name>
4+
<version>0.0.0</version>
5+
<description>The pf_localization package</description>
6+
7+
<!-- One maintainer tag required, multiple allowed, one person per tag -->
8+
<!-- Example: -->
9+
<!-- <maintainer email="[email protected]">Jane Doe</maintainer> -->
10+
<maintainer email="[email protected]">maestro</maintainer>
11+
12+
13+
<!-- One license tag required, multiple allowed, one license per tag -->
14+
<!-- Commonly used license strings: -->
15+
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
16+
<license>TODO</license>
17+
18+
19+
<!-- Url tags are optional, but multiple are allowed, one per tag -->
20+
<!-- Optional attribute type can be: website, bugtracker, or repository -->
21+
<!-- Example: -->
22+
<!-- <url type="website">http://wiki.ros.org/pf_localization</url> -->
23+
24+
25+
<!-- Author tags are optional, multiple are allowed, one per tag -->
26+
<!-- Authors do not have to be maintainers, but could be -->
27+
<!-- Example: -->
28+
<!-- <author email="[email protected]">Jane Doe</author> -->
29+
30+
31+
<!-- The *depend tags are used to specify dependencies -->
32+
<!-- Dependencies can be catkin packages or system dependencies -->
33+
<!-- Examples: -->
34+
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
35+
<!-- <depend>roscpp</depend> -->
36+
<!-- Note that this is equivalent to the following: -->
37+
<!-- <build_depend>roscpp</build_depend> -->
38+
<!-- <exec_depend>roscpp</exec_depend> -->
39+
<!-- Use build_depend for packages you need at compile time: -->
40+
<!-- <build_depend>message_generation</build_depend> -->
41+
<!-- Use build_export_depend for packages you need in order to build against this package: -->
42+
<!-- <build_export_depend>message_generation</build_export_depend> -->
43+
<!-- Use buildtool_depend for build tool packages: -->
44+
<!-- <buildtool_depend>catkin</buildtool_depend> -->
45+
<!-- Use exec_depend for packages you need at runtime: -->
46+
<!-- <exec_depend>message_runtime</exec_depend> -->
47+
<!-- Use test_depend for packages you need only for testing: -->
48+
<!-- <test_depend>gtest</test_depend> -->
49+
<!-- Use doc_depend for packages you need only for building documentation: -->
50+
<!-- <doc_depend>doxygen</doc_depend> -->
51+
<buildtool_depend>catkin</buildtool_depend>
52+
<build_depend>rospy</build_depend>
53+
<build_depend>std_msgs</build_depend>
54+
<build_export_depend>rospy</build_export_depend>
55+
<build_export_depend>std_msgs</build_export_depend>
56+
<exec_depend>rospy</exec_depend>
57+
<exec_depend>std_msgs</exec_depend>
58+
59+
60+
<!-- The export tag contains other, unspecified, tags -->
61+
<export>
62+
<!-- Other tools can request additional information be placed here -->
63+
64+
</export>
65+
</package>

‎scripts/__pycache__/pf_classes.cpython-38.pyc

3.66 KB
Binary file not shown.

‎scripts/__pycache__/pf_functions.cpython-38.pyc

7.21 KB
Binary file not shown.

‎scripts/pf_algo.py

+256
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,256 @@
1+
#! /usr/bin/env python
2+
3+
import rospy
4+
import os
5+
6+
from time import perf_counter
7+
from time import sleep
8+
from datetime import datetime
9+
import matplotlib.pyplot as plt
10+
11+
import sys
12+
DATA_PATH = '/home/maestro/catkin_ws/src/pf_localization/Data'
13+
MODULES_PATH = '/home/maestro/catkin_ws/src/pf_localization/scripts'
14+
sys.path.insert(0, MODULES_PATH)
15+
16+
from pf_functions import *
17+
18+
DATA_DIR = DATA_PATH + '/stage_1'
19+
MAP_NAME = 'stage_1'
20+
21+
LOAD_EST_DATA = True # True - load data from DATA_DIR/Estimation
22+
23+
if __name__ == '__main__':
24+
try:
25+
# Map configuration and object extraction
26+
map = Map(MAP_NAME)
27+
map.load_objects()
28+
29+
# Load data
30+
DATA_X_odom = np.genfromtxt(DATA_DIR + '/X_odom.csv', delimiter = ' , ')
31+
DATA_Y_odom = np.genfromtxt(DATA_DIR + '/Y_odom.csv', delimiter = ' , ')
32+
DATA_THETA_odom = np.genfromtxt(DATA_DIR + '/THETA_odom.csv', delimiter = ' , ')
33+
DATA_DISTANCES_lidar = np.genfromtxt(DATA_DIR + '/DISTANCES_lidar.csv', delimiter = ' , ')
34+
DATA_ANGLES_lidar = np.genfromtxt(DATA_DIR + '/ANGLES_lidar.csv', delimiter = ' , ')
35+
36+
print('\r\nLoaded data:')
37+
print('X_odom: ' + str(DATA_X_odom.shape) + ' ' + str(type(DATA_X_odom)))
38+
print('Y_odom: ' + str(DATA_Y_odom.shape) + ' ' + str(type(DATA_Y_odom)))
39+
print('THETA_odom: ' + str(DATA_THETA_odom.shape) + ' ' + str(type(DATA_THETA_odom)))
40+
print('DISTANCES_lidar: ' + str(DATA_DISTANCES_lidar.shape) + ' ' + str(type(DATA_DISTANCES_lidar)))
41+
print('ANGLES_lidar: ' + str(DATA_ANGLES_lidar.shape) + ' ' + str(type(DATA_ANGLES_lidar)))
42+
43+
# Init storing data
44+
DATA_PARTICLES = []
45+
46+
# Remembering init values
47+
x_odom_prev = DATA_X_odom[0]
48+
y_odom_prev = DATA_Y_odom[0]
49+
theta_odom_prev = DATA_THETA_odom[0]
50+
distances = DATA_DISTANCES_lidar[0]
51+
angles = DATA_ANGLES_lidar[0]
52+
53+
# Init time
54+
t_start = perf_counter()
55+
56+
if not LOAD_EST_DATA:
57+
#######################################
58+
### PARTICLE FILTER INITIALIZATION ###
59+
#####################################
60+
61+
N = 200 # Number of particles
62+
63+
# Particle filter algorithm - Create N uniformly distributed particles
64+
particles = generate_uniform_particles(map, N)
65+
66+
for p in particles:
67+
# Calculate particle weights: w_t = p(z_t|x_t)
68+
p = calculate_particle_weight(p, angles, distances, map)
69+
70+
# Normalize particle weight
71+
particles = normalize_particle_weights(particles)
72+
73+
print('\r\nInitial particles: ')
74+
for (i, p) in enumerate(particles):
75+
print(str(i + 1) + '. (x,y,theta) = (%.2f,%.2f,%.2f), weight = %.5f' % (p.X[0], p.X[1], p.X[2], p.w))
76+
77+
##################################
78+
### PARTICLE FILTER MAIN LOOP ###
79+
################################
80+
for i in range(1, len(DATA_X_odom)):
81+
# Evaluate Particle Filter algorithm on DATA_DIR
82+
83+
print('\r\nStep {} calculating...\r\n'.format(i))
84+
85+
# Calculate step time in [s]
86+
step_time = perf_counter()
87+
step_duration = step_time - t_start
88+
t_start = step_time
89+
90+
# LIDAR measurements
91+
distances = DATA_DISTANCES_lidar[i] # distances in [m]
92+
angles = DATA_ANGLES_lidar[i] # angles in [radians]
93+
94+
# Odometry measurements
95+
x_odom = DATA_X_odom[i] # x coordinate in [m]
96+
y_odom = DATA_Y_odom[i] # y coordinate in [m]
97+
theta_odom = DATA_THETA_odom[i] # theta coordinate in [radians]
98+
99+
X_odom = np.array([x_odom, y_odom, theta_odom])
100+
X_odom_prev = np.array([x_odom_prev, y_odom_prev, theta_odom_prev])
101+
dX = X_odom - X_odom_prev
102+
103+
# Resample particles from distribution based on weights w_t
104+
particles = resample_particles(particles, map)
105+
106+
for p in particles:
107+
# State transition model: p(x_t|x_t_1, u_t)
108+
p = apply_state_transision(p, dX)
109+
110+
# Calculate particle weights: w_t = p(z_t|x_t)
111+
p = calculate_particle_weight(p, angles, distances, map)
112+
113+
# Normalize particle weight
114+
particles = normalize_particle_weights(particles)
115+
116+
# Sorting particles by weights
117+
particles.sort(key = Particle.get_weight, reverse = True)
118+
# Top particle - maximum likelihood estimate
119+
p_mle = particles[0]
120+
121+
print('\r\nStep {}, step time: {} [s]'.format(i, step_duration))
122+
123+
print('Particles: ')
124+
for (i, p) in enumerate(particles):
125+
print(str(i + 1) + '. (x,y,theta) = (%.2f,%.2f,%.2f), weight = %.5f' % (p.X[0], p.X[1], p.X[2], p.w))
126+
127+
x_odom_prev = x_odom
128+
y_odom_prev = y_odom
129+
theta_odom_prev = theta_odom
130+
131+
# DATA_PARTICLES.append(particles) overwrites all existing elements in list
132+
DATA_PARTICLES.append(copy_particles(particles))
133+
else:
134+
"""
135+
Load estimated data from file
136+
"""
137+
DATA_PARTICLES = []
138+
estimations = os.listdir(DATA_DIR + '/Estimation')
139+
estimations.sort(key = get_estimation_step, reverse = False)
140+
141+
for est in estimations:
142+
data_particles = np.genfromtxt(DATA_DIR + '/Estimation/' + est, delimiter = ' , ')
143+
particles = []
144+
145+
for dp in data_particles:
146+
p_X = [dp[0], dp[1], dp[2]]
147+
p_w = dp[3]
148+
particles.append(Particle(p_X, p_w))
149+
150+
DATA_PARTICLES.append(particles)
151+
152+
153+
algo_time_s = perf_counter()
154+
algo_time_h = algo_time_s // 3600
155+
algo_time_m = (algo_time_s - algo_time_h * 3600) // 60
156+
algo_time_s = algo_time_s - algo_time_h * 3600 - algo_time_m * 60
157+
158+
print('\r\nAlgorithm duration: %i:%i:%.2f [h:m:s]\r\n' % (algo_time_h, algo_time_m, algo_time_s))
159+
160+
# Log the data
161+
if not LOAD_EST_DATA:
162+
for (i, particles) in enumerate(DATA_PARTICLES):
163+
164+
PARTICLES = np.empty([len(particles), 4])
165+
file_name = 'PARTICLES_step_{}.csv'.format(i)
166+
167+
for (j,p) in enumerate(particles):
168+
PARTICLES[j,0] = p.X[0]
169+
PARTICLES[j,1] = p.X[1]
170+
PARTICLES[j,2] = p.X[2]
171+
PARTICLES[j,3] = p.w
172+
173+
np.savetxt(DATA_DIR + '/Estimation/' + file_name, PARTICLES, delimiter = ' , ')
174+
175+
# Init figure - simulated real time
176+
plt.style.use('seaborn-ticks')
177+
fig = plt.figure(1)
178+
ax = fig.add_subplot(1,1,1)
179+
box = ax.get_position()
180+
ax.set_position([box.x0, box.y0, box.width * 0.8, box.height])
181+
182+
print('Plotting\r\n')
183+
184+
for (i, particles) in enumerate(DATA_PARTICLES):
185+
print('Plotting step {}'.format(i))
186+
187+
# LIDAR measurements
188+
distances = DATA_DISTANCES_lidar[i] # distances in [m]
189+
angles = DATA_ANGLES_lidar[i] # angles in [radians]
190+
191+
# Sorting particles by weights
192+
particles.sort(key = Particle.get_weight, reverse = True)
193+
# Top particle - maximum likelihood estimate
194+
p_mle = particles[0]
195+
196+
# Lidar measurements of MLE in x-y plane
197+
lidar_p_mle_x = np.array([])
198+
lidar_p_mle_y = np.array([])
199+
200+
for (dist, ang) in zip(distances, angles):
201+
lidar_p_mle_x = np.append(lidar_p_mle_x, p_mle.X[0] + dist * np.cos(ang + p_mle.X[2]))
202+
lidar_p_mle_y = np.append(lidar_p_mle_y, p_mle.X[1] + dist * np.sin(ang + p_mle.X[2]))
203+
204+
# Plot
205+
ax.clear()
206+
207+
# objects
208+
for w in map.walls:
209+
ax.plot(w.x, w.y, color = 'black', lw = 4)
210+
211+
# particles
212+
for (j,p) in enumerate(particles):
213+
214+
dir_p_x = np.array([p.X[0], p.X[0] + 0.1 * np.cos(p.X[2])])
215+
dir_p_y = np.array([p.X[1], p.X[1] + 0.1 * np.sin(p.X[2])])
216+
217+
if j == 0:
218+
# MLE
219+
ax.scatter(p.X[0], p.X[1], color = 'green', marker = 'o', lw = 7, label = 'MLE')
220+
ax.plot(dir_p_x, dir_p_y, color = 'green', lw = 4)
221+
elif j == 1:
222+
ax.scatter(p.X[0], p.X[1], color = 'blue', marker = 'o', lw = 1, label = 'particles')
223+
ax.plot(dir_p_x, dir_p_y, color = 'blue', lw = 0.75)
224+
else:
225+
ax.scatter(p.X[0], p.X[1], color = 'blue', marker = 'o', lw = 1)
226+
ax.plot(dir_p_x, dir_p_y, color = 'blue', lw = 0.75)
227+
228+
# LIDAR measurements of MLE
229+
ax.plot(lidar_p_mle_x, lidar_p_mle_y, 'r.', markersize = 4, label = 'lidar')
230+
231+
# plotting MLE again to get in front
232+
dir_p_mle_x = np.array([p_mle.X[0], p_mle.X[0] + 0.12 * np.cos(p_mle.X[2])])
233+
dir_p_mle_y = np.array([p_mle.X[1], p_mle.X[1] + 0.12 * np.sin(p_mle.X[2])])
234+
235+
ax.scatter(p_mle.X[0], p_mle.X[1], color = 'green', marker = 'o', lw = 7)
236+
ax.plot(dir_p_mle_x, dir_p_mle_y, color = 'green', lw = 4)
237+
238+
plt.xlabel('x[m]')
239+
plt.ylabel('y[m]')
240+
plt.xlim((-2.2, 2.2))
241+
plt.ylim((-2.5, 2.2))
242+
plt.title('Particle filter algorithm')
243+
ax.legend(loc = 'center left', bbox_to_anchor = (1, 0.5))
244+
plt.draw()
245+
plt.pause(0.0001)
246+
247+
if i == 0:
248+
sleep(3)
249+
250+
if i == (len(DATA_PARTICLES) - 1):
251+
print('End of plotting')
252+
sleep(10)
253+
254+
except rospy.ROSInterruptException:
255+
print('Simulation terminated!')
256+
pass

0 commit comments

Comments
 (0)