def __init__(self, xinit, odom_lin_sigma, odom_ang_sigma, meas_rng_noise,
meas_ang_noise):
"""
Initialize the EKF filter.
Input:
room_map : a nx4 array of lines in the form [x1 y1 x2 y2]
xinit : initial position
odom_lin_sigma: odometry linear noise
odom_ang_sigma: odometry angular noise
meas_rng_noise: measurement linear noise
meas_ang_noise: measurement angular noise
"""
# Map with initial displacement
self.map = funcs.get_dataset3_map(xinit[0], xinit[1], xinit[2])
# Prediction noise
self.Qk = np.array([[odom_lin_sigma**2, 0,
0], [0, odom_lin_sigma**2, 0],
[0, 0, odom_ang_sigma**2]])
# Measurement noise
self.Rk = np.array([[meas_rng_noise**2, 0], [0, meas_ang_noise**2]])
# Pose initialization
self.xk = np.zeros(3)
self.Pk = 0.2 * 0.2 * np.eye(3) # initial uncertainty of robot state
def publish_results(self, lines):
'''
Publishes all results from the filter.
'''
if self.time is not None:
time = rospy.Time.now()
# Map of the room
map_lines = get_dataset3_map()
publish_lines(map_lines, self.pub_map, frame='/world', ns='map',
color=(0,1,0))
# Particles and biggest weighted particle
msg, msg_mean, msg_odom, trans, rot, msg_wei = get_particle_msgs(self.part_filter,
time)
self.pub_particles.publish(msg)
self.pub_big_particle.publish(msg_mean)
self.pub_odom.publish(msg_odom)
self.pub_wei.publish(msg_wei)
self.tfBroad.sendTransform(translation = trans,
rotation = rot,
time = time,
parent = 'world',
child = 'mean_particle')
# Publish scanned lines
if lines is not None:
publish_lines(lines, self.pub_lines_mean, frame='mean_particle',
time=time, ns='scan_lines_mean', color=(0,0,1))
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 __init__(self, xinit, odom_lin_sigma, odom_ang_sigma, meas_rng_noise,
meas_ang_noise):
"""
Initialize the EKF filter.
Input:
room_map : a nx4 array of lines in the form [x1 y1 x2 y2]
xinit : initial position
odom_lin_sigma: odometry linear noise
odom_ang_sigma: odometry angular noise
meas_rng_noise: measurement linear noise
meas_ang_noise: measurement angular noise
"""
# Map with initial displacement
self.map = funcs.get_dataset3_map(xinit[0], xinit[1], xinit[2])
# Prediction noise
self.Qk = np.array([[odom_lin_sigma**2, 0, 0],
[0, odom_lin_sigma**2, 0],
[0, 0, odom_ang_sigma**2]])
# Measurement noise
self.Rk = np.array([[meas_rng_noise**2, 0],
[0, meas_ang_noise**2]])
# Pose initialization
self.xk = np.zeros(3)
self.Pk = 0.2*0.2*np.eye(3) # initial uncertainty of robot state
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,
rob2sensor = [-0.087, 0.013, np.deg2rad(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_scan = rospy.Subscriber("scan", LaserScan, self.laser_callback)
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 __init__(self,
odom_lin_sigma=0.025,
odom_ang_sigma=np.deg2rad(2),
meas_rng_noise=0.2,
meas_ang_noise=np.deg2rad(10),
x_init=0,
y_init=0,
theta_init=0):
'''
Initializes publishers, subscribers and the particle filter.
'''
# Threads
self.lock = threading.RLock()
# Publishers
self.pub_map = rospy.Publisher("map", Marker, queue_size=2)
self.pub_lines = rospy.Publisher("lines", Marker, queue_size=2)
self.pub_lines_mean = rospy.Publisher("lines_mean",
Marker,
queue_size=2)
self.pub_particles = rospy.Publisher("particles",
PoseArray,
queue_size=2)
self.pub_big_particle = rospy.Publisher("mean_particle",
PoseStamped,
queue_size=2)
self.pub_odom = rospy.Publisher("mean_particle_odom",
Odometry,
queue_size=2)
self.pub_wei = rospy.Publisher("weights", MarkerArray, queue_size=2)
# Subscribers
self.sub_scan = rospy.Subscriber("lines", Marker, 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
self.time = None
self.lines = None
# Particle filter
self.part_filter = ParticleFilter(get_dataset3_map(), 500,
odom_lin_sigma, odom_ang_sigma,
meas_rng_noise, meas_ang_noise,
x_init, y_init, theta_init)
def __init__(self, odom_lin_sigma=0.025, odom_ang_sigma=np.deg2rad(2), meas_rng_noise=0.2, meas_ang_noise=np.deg2rad(10), x_init=0, y_init=0, theta_init=0):
'''
Initializes publishers, subscribers and the particle filter.
'''
# Threads
self.lock = threading.RLock()
# Publishers
self.pub_map = rospy.Publisher("map", Marker, queue_size = 2)
self.pub_lines = rospy.Publisher("lines", Marker, queue_size = 2)
self.pub_lines_mean = rospy.Publisher("lines_mean", Marker, queue_size = 2)
self.pub_particles = rospy.Publisher("particles", PoseArray, queue_size = 2)
self.pub_big_particle = rospy.Publisher("mean_particle", PoseStamped, queue_size = 2)
self.pub_odom = rospy.Publisher("mean_particle_odom", Odometry, queue_size = 2)
self.pub_wei = rospy.Publisher("weights", MarkerArray, queue_size = 2)
# Subscribers
self.sub_scan = rospy.Subscriber("lines", Marker, 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
self.time = None
self.lines = None
self.i = 0
# Particle filter
self.part_filter = ParticleFilter(get_dataset3_map(), 500, odom_lin_sigma, odom_ang_sigma, meas_rng_noise, meas_ang_noise, x_init, y_init, theta_init)
def publish_results(self, lines):
'''
Publishes all results from the filter.
'''
if self.time is not None:
time = rospy.Time.now()
# Map of the room
map_lines = get_dataset3_map()
publish_lines(map_lines,
self.pub_map,
frame='/world',
ns='map',
color=(0, 1, 0))
# Particles and biggest weighted particle
msg, msg_mean, msg_odom, trans, rot, msg_wei = get_particle_msgs(
self.part_filter, time)
self.pub_particles.publish(msg)
self.pub_big_particle.publish(msg_mean)
self.pub_odom.publish(msg_odom)
self.pub_wei.publish(msg_wei)
self.tfBroad.sendTransform(translation=trans,
rotation=rot,
time=time,
parent='world',
child='mean_particle')
# Publish scanned lines
if lines is not None:
publish_lines(lines,
self.pub_lines_mean,
frame='mean_particle',
time=time,
ns='scan_lines_mean',
color=(0, 0, 1))