def __init__(self, x0=0.0, y0=0.0, theta0=0.0, odom_lin_sigma=0.025,
odom_ang_sigma=np.deg2rad(2), meas_rng_noise=0.2,
meas_ang_noise=np.deg2rad(10), xs=0.0, ys=0.0, thetas=0.0,
rob2sensor=[-0.087, 0.013, np.deg2rad(0.0)]):
"""
Initializes publishers, subscribers and the particle filter.
"""
# Transformation from robot to sensor
self.robotToSensor = np.array([xs, ys, thetas])
# Publishers
self.pub_lines = rospy.Publisher("ekf_lines", Marker, queue_size=0)
self.pub_map = rospy.Publisher("map", Marker, queue_size=0)
self.pub_map_gt = rospy.Publisher("map_gt", Marker, queue_size=0)
self.pub_odom = rospy.Publisher(
"predicted_odom", Odometry, queue_size=0)
self.pub_uncertainity = rospy.Publisher(
"uncertainity", Marker, queue_size=0)
self.pub_laser = rospy.Publisher("ekf_laser", LaserScan, queue_size=0)
# Subscribers
self.sub_odom = rospy.Subscriber("odom", Odometry, self.odom_callback)
self.sub_scan = rospy.Subscriber("lines", Marker, self.lines_callback)
# TF
self.tfBroad = tf.TransformBroadcaster()
# Incremental odometry
self.last_odom = None
self.odom = None
# Flags
self.new_odom = False
self.new_laser = False
self.pub = False
# Ground truth map
dataset = rospy.get_param("~dataset", None)
if dataset == 1:
self.map = get_map()
x0 = 0.8 - 0.1908
y0 = 0.3 + 0.08481
theta0 = -0.034128
elif dataset == 2:
self.map = np.array([])
elif dataset == 3:
self.map = get_dataset3_map()
else:
self.map = np.array([])
# Initial state
self.x0 = np.array([x0, y0, theta0])
# Particle filter
self.ekf = EKF_SLAM(x0, y0, theta0, odom_lin_sigma,
odom_ang_sigma, meas_rng_noise, meas_ang_noise)
# Transformation from robot to sensor
self.robot2sensor = np.array(rob2sensor)
def getStates(self, timestep, use_slam=False):
delT, X, Y, xdot, ydot, psi, psidot = super().getStates(timestep)
# Initialize the EKF SLAM estimation
if self.counter == 0:
# Load the map
minX, maxX, minY, maxY = -120., 450., -350., 50.
#minX, maxX, minY, maxY = -120., 450., -500., 50.
map_x = np.linspace(minX, maxX, 7)
map_y = np.linspace(minY, maxY, 7)
map_X, map_Y = np.meshgrid(map_x, map_y)
map_X = map_X.reshape(-1,1)
map_Y = map_Y.reshape(-1,1)
self.map = np.hstack((map_X, map_Y)).reshape((-1))
# Parameters for EKF SLAM
self.n = int(len(self.map)/2)
X_est = X + 0.5
Y_est = Y - 0.5
psi_est = psi - 0.02
mu_est = np.zeros(3+2*self.n)
mu_est[0:3] = np.array([X_est, Y_est, psi_est])
mu_est[3:] = np.array(self.map)
init_P = 1*np.eye(3+2*self.n)
W = np.zeros((3+2*self.n, 3+2*self.n))
W[0:3, 0:3] = delT**2 * 0.1 * np.eye(3)
V = 0.1*np.eye(2*self.n)
V[self.n:, self.n:] = 0.01*np.eye(self.n)
# V[self.n:] = 0.01
print(V)
# Create a SLAM
self.slam = EKF_SLAM(mu_est, init_P, delT, W, V, self.n)
self.counter += 1
else:
mu = np.zeros(3+2*self.n)
mu[0:3] = np.array([X,
Y,
psi])
mu[3:] = self.map
y = self._compute_measurements(X, Y, psi)
mu_est, _ = self.slam.predict_and_correct(y, self.previous_u)
self.previous_u = np.array([xdot, ydot, psidot])
print("True X, Y, psi:", X, Y, psi)
print("Estimated X, Y, psi:", mu_est[0], mu_est[1], mu_est[2])
print("-------------------------------------------------------")
if use_slam == True:
return delT, mu_est[0], mu_est[1], xdot, ydot, mu_est[2], psidot
else:
return delT, X, Y, xdot, ydot, psi, psidot
def __init__(self, x0=0,y0=0,theta0=0, odom_lin_sigma=0.025, odom_ang_sigma=np.deg2rad(2),
meas_rng_noise=0.2, meas_ang_noise=np.deg2rad(10),xs = 0, ys = 0, thetas = 0):
'''
Initializes publishers, subscribers and the particle filter.
'''
# Transformation from robot to sensor
self.robotToSensor = np.array([xs,ys,thetas])
# Publishers
self.pub_lines = rospy.Publisher("lines", Marker)
self.pub_odom = rospy.Publisher("predicted_odom", Odometry)
self.pub_uncertainity = rospy.Publisher("uncertainity", Marker)
# Subscribers
self.sub_scan = rospy.Subscriber("scan", LaserScan, self.laser_callback)
self.sub_odom = rospy.Subscriber("odom", Odometry, self.odom_callback)
# TF
self.tfBroad = tf.TransformBroadcaster()
# Incremental odometry
self.last_odom = None
self.odom = None
# Flags
self.new_odom = False
self.new_laser = False
self.pub = False
# Ground truth map
# self.map = get_map(0.8,0.3,-0.03)
self.map = get_map()
# Initial state
self.x0 = np.array([x0,y0,theta0])
# Particle filter
self.ekf = EKF_SLAM(x0, y0, theta0, odom_lin_sigma, odom_ang_sigma, meas_rng_noise, meas_ang_noise)
#!/usr/bin/env python
import rospy
import sys
import os
import numpy as np
from std_msgs.msg import String
from nav_msgs.msg import Odometry
from marker_msgs.msg import MarkerDetection
from tf.transformations import euler_from_quaternion
sys.path.insert(1, '/home/ros/catkin_ws/src/moro_g12/src/')
from ekf_slam import EKF_SLAM
from csv_writer import csv_writer
previous_timestamp = None
ekf = EKF_SLAM(5, 3, 2, 2)
def get_rotation(quaternion):
global roll, pitch, yaw
orientation_list = [quaternion.x, quaternion.y, quaternion.z, quaternion.w]
(roll, pitch, yaw) = euler_from_quaternion(orientation_list)
return wrap_to_pi(yaw)
def odom_callback(msg):
timestamp = msg.header.stamp
x = msg.pose.pose.position.x
y = msg.pose.pose.position.y
theta = get_rotation(msg.pose.pose.orientation)
v = msg.twist.twist.linear.x
from importlib import reload
# import sys
# sys.path.append("../")
import params as P
from dynamics import Dynamics
from animation import Animation
from plotData import plotData
from ekf_slam import EKF_SLAM
from copy import deepcopy
dynamics = Dynamics()
dataPlot = plotData()
ekf_slam = EKF_SLAM()
animation = Animation(ekf_slam)
estimate = np.array([])
actual = np.array([])
t = P.t_start
while t < P.t_end:
t_next_plot = t + P.t_plot
while t < t_next_plot:
t = t + P.Ts
vc = 1.25 + 0.5 * np.cos(2 * np.pi * (0.2) * t)
omegac = -0.5 + 0.2 * np.cos(2 * np.pi * (0.6) * t)
noise_v = vc + np.random.normal(
0, np.sqrt(P.alpha1 * vc**2 + P.alpha2 * omegac**2))
noise_omega = omegac + np.random.normal(
def main():
args = get_cli_args()
validate_cli_args(args)
alphas = np.array(args.alphas)
beta = np.array(args.beta)
mean_prior = np.array([180., 50., 0.])
Sigma_prior = 1e-12 * np.eye(3, 3)
initial_state = Gaussian(mean_prior, Sigma_prior)
if args.input_data_file:
data = load_data(args.input_data_file)
elif args.num_steps:
# Generate data, assuming `--num-steps` was present in the CL args.
data = generate_input_data(initial_state.mu.T, args.num_steps,
args.num_landmarks_per_side,
args.max_obs_per_time_step, alphas, beta,
args.dt)
else:
raise RuntimeError('')
store_sim_data = True if args.output_dir else False
should_show_plots = True if args.animate else False
should_write_movie = True if args.movie_file else False
should_update_plots = True if should_show_plots or should_write_movie else False
field_map = FieldMap(args.num_landmarks_per_side)
fig = get_plots_figure(should_show_plots, should_write_movie)
movie_writer = get_movie_writer(should_write_movie, 'Simulation SLAM',
args.movie_fps, args.plot_pause_len)
progress_bar = FillingCirclesBar('Simulation Progress', max=data.num_steps)
if store_sim_data:
if not os.path.exists(args.output_dir):
os.makedirs(args.output_dir)
save_input_data(data, os.path.join(args.output_dir, 'input_data.npy'))
# slam object initialization
slam = EKF_SLAM('ekf', 'known', 'batch', args, initial_state)
mu_traj = mean_prior
sigma_traj = []
theta = []
with movie_writer.saving(
fig, args.movie_file,
data.num_steps) if should_write_movie else get_dummy_context_mgr():
for t in range(data.num_steps):
# Used as means to include the t-th time-step while plotting.
tp1 = t + 1
# Control at the current step.
u = data.filter.motion_commands[t]
# Observation at the current step.
z = data.filter.observations[t]
# TODO SLAM predict(u)
mu, Sigma = slam.predict(u)
# TODO SLAM update
mu, Sigma = slam.update(z)
mu_traj = np.vstack((mu_traj, mu[:3]))
sigma_traj.append(Sigma[:3, :3])
theta.append(mu[2])
progress_bar.next()
if not should_update_plots:
continue
plt.cla()
plot_field(field_map, z)
plot_robot(data.debug.real_robot_path[t])
plot_observations(data.debug.real_robot_path[t],
data.debug.noise_free_observations[t],
data.filter.observations[t])
plt.plot(data.debug.real_robot_path[1:tp1, 0],
data.debug.real_robot_path[1:tp1, 1], 'm')
plt.plot(data.debug.noise_free_robot_path[1:tp1, 0],
data.debug.noise_free_robot_path[1:tp1, 1], 'g')
plt.plot([data.debug.real_robot_path[t, 0]],
[data.debug.real_robot_path[t, 1]], '*r')
plt.plot([data.debug.noise_free_robot_path[t, 0]],
[data.debug.noise_free_robot_path[t, 1]], '*g')
# TODO plot SLAM solution
# robot filtered trajectory and covariance
plt.plot(mu_traj[:, 0], mu_traj[:, 1], 'blue')
plot2dcov(mu[:2], Sigma[:2, :2], color='b', nSigma=3, legend=None)
# landmarks covariances and expected poses
Sm = slam.Sigma[slam.iR:slam.iR + slam.iM,
slam.iR:slam.iR + slam.iM]
mu_M = slam.mu[slam.iR:]
for c in range(0, slam.iM, 2):
Sigma_lm = Sm[c:c + 2, c:c + 2]
mu_lm = mu_M[c:c + 2]
plt.plot(mu_lm[0], mu_lm[1], 'ro')
plot2dcov(mu_lm, Sigma_lm, color='k', nSigma=3, legend=None)
if should_show_plots:
# Draw all the plots and pause to create an animation effect.
plt.draw()
plt.pause(args.plot_pause_len)
if should_write_movie:
movie_writer.grab_frame()
progress_bar.finish()
# plt.figure(2)
# plt.plot(theta)
plt.show(block=True)
if store_sim_data:
file_path = os.path.join(args.output_dir, 'output_data.npy')
with open(file_path, 'wb') as data_file:
np.savez(data_file,
mean_trajectory=mu_traj,
covariance_trajectory=np.array(sigma_traj))
scan_angle.set_theta1((twr.x_new[2, 0] - twr.fov / 2) * 180 / np.pi)
scan_angle.set_theta2((twr.x_new[2, 0] + twr.fov / 2) * 180 / np.pi)
fig.canvas.draw()
plt.pause(0.01)
if __name__ == "__main__":
plot_live = True
# Two-Wheeled Robot Init
twr = TWR() # TWR model object
# EKF SLAM filter Init
ekf_slam = EKF_SLAM(twr)
body_radius = 0.3
fig, lines, lines_est, mlandmarks, robot_body, robot_head, scan_angle = InitPlot(
twr, body_radius)
mu = ekf_slam.mu[0:3].reshape([3, 1])
mu_landmarks = ekf_slam.mu[3:].reshape([len(ekf_slam.mu[3:]), 1])
w_landmarks = np.zeros((twr.nl, 1))
h_landmarks = np.zeros((twr.nl, 1))
ang_landmarks = np.zeros((twr.nl, 1))
K = ekf_slam.K
two_sig_x = np.array([[2 * np.sqrt(ekf_slam.cov.item((0, 0)))],
[-2 * np.sqrt(ekf_slam.cov.item((0, 0)))]])
two_sig_y = np.array([[2 * np.sqrt(ekf_slam.cov.item((1, 1)))],
[-2 * np.sqrt(ekf_slam.cov.item((1, 1)))]])
two_sig_theta = np.array([[2 * np.sqrt(ekf_slam.cov.item((2, 2)))],
