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))