class LocalizationNode(object):
'''
Class to hold all ROS related transactions to use split and merge algorithm.
'''
#===========================================================================
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 odom_callback(self, msg):
'''
Publishes a tf based on the odometry of the robot and calculates the incremental odometry as seen from the vehicle frame.
'''
# Save time
self.time = msg.header.stamp
# Translation
trans = (msg.pose.pose.position.x, msg.pose.pose.position.y,
msg.pose.pose.position.z)
# Rotation
rot = (msg.pose.pose.orientation.x, msg.pose.pose.orientation.y,
msg.pose.pose.orientation.z, msg.pose.pose.orientation.w)
# Publish transform
self.tfBroad.sendTransform(translation=self.robot2sensor,
rotation=(0, 0, 0, 1),
time=msg.header.stamp,
child='/sensor',
parent='/robot')
self.tfBroad.sendTransform(translation=self.robot2sensor,
rotation=(0, 0, 0, 1),
time=msg.header.stamp,
child='/camera_depth_frame',
parent='/base_footprint')
self.tfBroad.sendTransform(translation=trans,
rotation=rot,
time=msg.header.stamp,
child='/base_footprint',
parent='/odom')
self.tfBroad.sendTransform(
translation=(self.x0[0], self.x0[1], 0),
rotation=tf.transformations.quaternion_from_euler(
0, 0, self.x0[2]),
time=msg.header.stamp,
child='/odom',
parent='/world')
# Incremental odometry
if self.last_odom is not None and not self.new_odom:
# Increment computation
delta_x = msg.pose.pose.position.x - self.last_odom.pose.pose.position.x
delta_y = msg.pose.pose.position.y - self.last_odom.pose.pose.position.y
yaw = yaw_from_quaternion(msg.pose.pose.orientation)
lyaw = yaw_from_quaternion(self.last_odom.pose.pose.orientation)
# Odometry seen from vehicle frame
self.uk = np.array([
delta_x * np.cos(lyaw) + delta_y * np.sin(lyaw),
-delta_x * np.sin(lyaw) + delta_y * np.cos(lyaw),
angle_wrap(yaw - lyaw)
])
# Flag
self.new_odom = True
# For next loop
self.last_odom = msg
if self.last_odom is None:
self.last_odom = msg
#===========================================================================
def laser_callback(self, msg):
'''
Republishes laser scan in the EKF solution frame.
'''
msg.header.frame_id = '/sensor'
self.pub_laser.publish(msg)
#===========================================================================
def lines_callback(self, msg):
'''
Function called each time a LaserScan message with topic "scan" arrives.
'''
# Save time
self.linestime = msg.header.stamp
# Get the lines
if len(msg.points) > 0:
# Structure for the lines
self.lines = np.zeros((len(msg.points) / 2, 4))
for i in range(0, len(msg.points) / 2):
# Get start and end points
pt1 = msg.points[2 * i]
pt2 = msg.points[2 * i + 1]
# Transform to robot frame
pt1R = comp(self.robot2sensor, [pt1.x, pt1.y, 0.0])
pt2R = comp(self.robot2sensor, [pt2.x, pt2.y, 0.0])
# Append to line list
self.lines[i, :2] = pt1R[:2]
self.lines[i, 2:] = pt2R[:2]
# Flag
self.new_laser = True
# Publish
publish_lines(self.lines,
self.pub_lines,
frame='/robot',
time=msg.header.stamp,
ns='lines_robot',
color=(0, 0, 1))
#===========================================================================
def iterate(self):
'''
Main loop of the filter.
'''
# Prediction
if self.new_odom:
self.ekf.predict(self.uk.copy())
self.new_odom = False
self.pub = True
# Weightimg and resampling
if self.new_laser:
Innovk_List, H_k_List, Rk_List, idx_not_associated = self.ekf.data_association(
self.lines.copy())
self.ekf.update_position(Innovk_List, H_k_List, Rk_List)
self.ekf.state_augmentation(self.lines.copy(), idx_not_associated)
self.new_laser = False
self.pub = True
# Publish results
if self.pub:
self.publish_results()
self.pub = False
#===========================================================================
def publish_results(self):
'''
Publishes all results from the filter.
'''
# Ground turth of the map of the room
publish_lines(self.map,
self.pub_map_gt,
frame='/world',
ns='gt_map',
color=(0.3, 0.3, 0.3))
odom, ellipse, trans, rot, room_map = get_ekf_msgs(self.ekf)
publish_lines(room_map,
self.pub_map,
frame='/world',
ns='map',
color=(0, 1, 0))
self.pub_odom.publish(odom)
self.pub_uncertainity.publish(ellipse)
self.tfBroad.sendTransform(translation=trans,
rotation=rot,
time=self.time,
child='/robot',
parent='/world')
class LocalizationNode(object):
'''
Class to hold all ROS related transactions to use split and merge algorithm.
'''
#===========================================================================
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 odom_callback(self, msg):
'''
Publishes a tf based on the odometry of the robot and calculates the incremental odometry as seen from the vehicle frame.
'''
# Save time
self.time = msg.header.stamp
# Translation
trans = (msg.pose.pose.position.x,
msg.pose.pose.position.y,
msg.pose.pose.position.z)
# Rotation
rot = (msg.pose.pose.orientation.x,
msg.pose.pose.orientation.y,
msg.pose.pose.orientation.z,
msg.pose.pose.orientation.w)
# Publish transform
self.tfBroad.sendTransform(translation = self.robot2sensor,
rotation = (0, 0, 0, 1),
time = msg.header.stamp,
child = '/sensor',
parent = '/robot')
self.tfBroad.sendTransform(translation = self.robot2sensor,
rotation = (0, 0, 0, 1),
time = msg.header.stamp,
child = '/camera_depth_frame',
parent = '/base_footprint')
self.tfBroad.sendTransform(translation = trans,
rotation = rot,
time = msg.header.stamp,
child = '/base_footprint',
parent = '/odom')
self.tfBroad.sendTransform(translation = (self.x0[0],self.x0[1],0),
rotation = tf.transformations.quaternion_from_euler(0,0,self.x0[2]),
time = msg.header.stamp,
child = '/odom',
parent = '/world')
# Incremental odometry
if self.last_odom is not None and not self.new_odom:
# Increment computation
delta_x = msg.pose.pose.position.x - self.last_odom.pose.pose.position.x
delta_y = msg.pose.pose.position.y - self.last_odom.pose.pose.position.y
yaw = yaw_from_quaternion(msg.pose.pose.orientation)
lyaw = yaw_from_quaternion(self.last_odom.pose.pose.orientation)
# Odometry seen from vehicle frame
self.uk = np.array([delta_x * np.cos(lyaw) + delta_y * np.sin(lyaw),
- delta_x * np.sin(lyaw) + delta_y * np.cos(lyaw),
angle_wrap(yaw - lyaw)])
# Flag
self.new_odom = True
# For next loop
self.last_odom = msg
if self.last_odom is None:
self.last_odom = msg
#===========================================================================
def laser_callback(self, msg):
'''
Republishes laser scan in the EKF solution frame.
'''
msg.header.frame_id = '/sensor'
self.pub_laser.publish(msg)
#===========================================================================
def lines_callback(self, msg):
'''
Function called each time a LaserScan message with topic "scan" arrives.
'''
# Save time
self.linestime = msg.header.stamp
# Get the lines
if len(msg.points) > 0:
# Structure for the lines
self.lines = np.zeros((len(msg.points) / 2, 4))
for i in range(0, len(msg.points)/2):
# Get start and end points
pt1 = msg.points[2*i]
pt2 = msg.points[2*i+1]
# Transform to robot frame
pt1R = comp(self.robot2sensor, [pt1.x, pt1.y, 0.0])
pt2R = comp(self.robot2sensor, [pt2.x, pt2.y, 0.0])
# Append to line list
self.lines[i, :2] = pt1R[:2]
self.lines[i, 2:] = pt2R[:2]
# Flag
self.new_laser = True
# Publish
publish_lines(self.lines, self.pub_lines,
frame = '/robot',
time = msg.header.stamp,
ns = 'lines_robot', color = (0,0,1))
#===========================================================================
def iterate(self):
'''
Main loop of the filter.
'''
# Prediction
if self.new_odom:
self.ekf.predict(self.uk.copy())
self.new_odom = False
self.pub = True
# Weightimg and resampling
if self.new_laser:
Innovk_List, H_k_List, Rk_List,idx_not_associated = self.ekf.data_association(self.lines.copy())
self.ekf.update_position(Innovk_List, H_k_List, Rk_List)
self.ekf.state_augmentation(self.lines.copy(),idx_not_associated)
self.new_laser = False
self.pub = True
# Publish results
if self.pub:
self.publish_results()
self.pub = False
#===========================================================================
def publish_results(self):
'''
Publishes all results from the filter.
'''
# Ground turth of the map of the room
publish_lines(self.map, self.pub_map_gt, frame='/world', ns='gt_map', color=(0.3,0.3,0.3))
odom, ellipse, trans, rot, room_map = get_ekf_msgs(self.ekf)
publish_lines(room_map, self.pub_map, frame='/world', ns='map', color=(0,1,0))
self.pub_odom.publish(odom)
self.pub_uncertainity.publish(ellipse)
self.tfBroad.sendTransform(translation = trans,
rotation = rot,
time = self.time,
child = '/robot',
parent = '/world')
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))
class LocalizationNode(object):
'''
Class to hold all ROS related transactions to use split and merge algorithm.
'''
#===========================================================================
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)
#===========================================================================
def odom_callback(self, msg):
'''
Publishes a tf based on the odometry of the robot and calculates the incremental odometry as seen from the vehicle frame.
'''
# Save time
self.time = msg.header.stamp
# Translation
trans = (msg.pose.pose.position.x,
msg.pose.pose.position.y,
msg.pose.pose.position.z)
# Rotation
rot = (msg.pose.pose.orientation.x,
msg.pose.pose.orientation.y,
msg.pose.pose.orientation.z,
msg.pose.pose.orientation.w)
# Publish transform
self.tfBroad.sendTransform(translation = (self.robotToSensor[0],self.robotToSensor[1],0),
rotation = tf.transformations.quaternion_from_euler(0,0,self.robotToSensor[2]),
time = msg.header.stamp,
child = '/openni_depth_frame',
parent = '/robot')
self.tfBroad.sendTransform(translation = trans,
rotation = rot,
time = msg.header.stamp,
child = '/robot_original',
parent = '/odom')
self.tfBroad.sendTransform(translation = (self.x0[0]-0.1908,self.x0[1]+0.08481,self.x0[2]),
rotation = tf.transformations.quaternion_from_euler(0,0,-0.034128),
# rotation = (0,0,0,1),
time = msg.header.stamp,
child = '/odom',
parent = '/world')
# Incremental odometry
if self.last_odom is not None and not self.new_odom:
# Increment computation
delta_x = msg.pose.pose.position.x - self.last_odom.pose.pose.position.x
delta_y = msg.pose.pose.position.y - self.last_odom.pose.pose.position.y
yaw = yaw_from_quaternion(msg.pose.pose.orientation)
lyaw = yaw_from_quaternion(self.last_odom.pose.pose.orientation)
# Odometry seen from vehicle frame
self.uk = np.array([delta_x * np.cos(lyaw) + delta_y * np.sin(lyaw),
- delta_x * np.sin(lyaw) + delta_y * np.cos(lyaw),
angle_wrap(yaw - lyaw)])
# Flag
self.new_odom = True
# For next loop
self.last_odom = msg
if self.last_odom is None:
self.last_odom = msg
#===========================================================================
def laser_callback(self, msg):
'''
Function called each time a LaserScan message with topic "scan" arrives.
'''
# Save time
self.time = msg.header.stamp
# Project LaserScan to points in space
rng = np.array(msg.ranges)
ang = np.linspace(msg.angle_min, msg.angle_max, len(msg.ranges))
points = np.vstack((rng * np.cos(ang),
rng * np.sin(ang)))
# Filter long ranges
points = points[:, rng < msg.range_max]
# Use split and merge to obtain lines and publish
self.lines = splitandmerge(points, split_thres=0.1,
inter_thres=0.3,
min_points=6,
dist_thres=0.12,
ang_thres=np.deg2rad(10))
# Have valid points
if self.lines is not None:
# Transform line to robot frame
for i in range(0,self.lines.shape[0]):
point1S = np.append(self.lines[i,0:2], 0)
point2S = np.append(self.lines[i,2:4], 0)
point1R = comp(self.robotToSensor, point1S)
point2R = comp(self.robotToSensor, point2S)
self.lines[i,:] = np.append(point1R[0:2],point2R[0:2])
# Publish results
publish_lines(self.lines, self.pub_lines, frame='/robot',
time=msg.header.stamp, ns='scan_lines_robot', color=(0,0,1))
# Flag
self.new_laser = True
#===========================================================================
def iterate(self):
'''
Main loop of the filter.
'''
# Prediction
if self.new_odom:
self.ekf.predict(self.uk.copy())
self.new_odom = False
self.pub = True
# Weightimg and resampling
if self.new_laser:
Innovk_List, H_k_List, S_f_List, Rk_List,idx_not_associated = self.ekf.data_association(self.lines.copy())
self.ekf.update_position(Innovk_List, H_k_List, S_f_List, Rk_List)
self.ekf.state_augmentation(self.lines.copy(),idx_not_associated)
self.new_laser = False
self.pub = True
# Publish results
if self.pub:
self.publish_results()
self.pub = False
#===========================================================================
def publish_results(self):
'''
Publishes all results from the filter.
'''
# Ground turth of the map of the room
publish_lines(self.map, self.pub_lines, frame='/world', ns='gt_map', color=(0.3,0.3,0.3))
msg_odom, msg_ellipse, trans, rot, room_map = get_ekf_msgs(self.ekf)
publish_lines(room_map, self.pub_lines, frame='/world', ns='map', color=(0,1,0))
self.pub_odom.publish(msg_odom)
self.pub_uncertainity.publish(msg_ellipse)
self.tfBroad.sendTransform(translation = trans,
rotation = rot,
time = self.time,
child = '/robot',
parent = '/world')
