def __init__(self, x0=0.8,y0=0.3,theta0=-0.03, odom_lin_sigma=0.025, odom_ang_sigma=np.deg2rad(2),
meas_rng_noise=0.2, meas_ang_noise=np.deg2rad(10)):
'''
Initializes publishers, subscribers and the particle filter.
'''
# 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
# Particle filter
self.ekf = EKF(get_map(), x0, y0, theta0, odom_lin_sigma, odom_ang_sigma, meas_rng_noise, meas_ang_noise)
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)):
'''
Initializes publishers, subscribers and the particle filter.
'''
# Publishers
self.pub_lines = rospy.Publisher("lines", Marker)
self.pub_particles = rospy.Publisher("particles", PoseArray)
self.pub_big_particle = rospy.Publisher("mean_particle", PoseStamped)
self.pub_odom = rospy.Publisher("mean_particle_odom", Odometry)
# 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
# Particle filter
self.part_filter = ParticleFilter(get_map(), 500, odom_lin_sigma,
odom_ang_sigma, meas_rng_noise,
meas_ang_noise)
def publish_results(self):
'''
Publishes all results from the filter.
'''
# Map of the room
map_lines = get_map()
publish_lines(map_lines, self.pub_lines, frame='/world', ns='map', color=(0,1,0))
msg_odom, msg_ellipse, trans, rot = get_ekf_msgs(self.ekf)
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')
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 publish_results(self):
'''
Publishes all results from the filter.
'''
# Map of the room
map_lines = get_map()
publish_lines(map_lines,
self.pub_lines,
frame='/world',
ns='map',
color=(0, 1, 0))
# Particles and biggest weighted particle
msg, msg_mean, msg_odom, trans, rot = get_particle_msgs(
self.part_filter)
self.pub_particles.publish(msg)
self.pub_big_particle.publish(msg_mean)
self.pub_odom.publish(msg_odom)
self.tfBroad.sendTransform(translation=trans,
rotation=rot,
time=self.time,
child='/mean_particle',
parent='/world')
