def __init__(self, xinit, odom_lin_sigma, odom_ang_sigma, meas_rng_noise,
meas_ang_noise, rob2sensor):
"""Initialize publishers, subscribers and the filter."""
# Publishers
self.pub_laser = rospy.Publisher("ekf_laser", LaserScan, queue_size=2)
self.pub_lines = rospy.Publisher("linesekf", Marker, queue_size=2)
self.pub_odom = rospy.Publisher("predicted_odom",
Odometry,
queue_size=2)
self.pub_uncertainity = rospy.Publisher("uncertainity",
Marker,
queue_size=2)
# Subscribers
self.sub_laser = rospy.Subscriber("scan", LaserScan, self.cbk_laser)
self.sub_scan = rospy.Subscriber("lines", Marker, self.cbk_lines)
self.sub_odom = rospy.Subscriber("odom", Odometry, self.cbk_odom)
# TF
self.tfBroad = tf.TransformBroadcaster()
# Incremental odometry
self.last_odom = None
self.odom = None
# Times
self.time = rospy.Time(0)
self.odomtime = rospy.Time(0)
self.linestime = rospy.Time(0)
# Flags
self.uk = None
self.lines = None
self.new_odom = False
self.new_laser = False
self.pub = False
# Filter
self.ekf = EKF(xinit, odom_lin_sigma, odom_ang_sigma, meas_rng_noise,
meas_ang_noise)
# Transformation from robot to sensor
self.robot2sensor = np.array(rob2sensor)
print self.robot2sensor
