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