def __init__(self, num_particles, gaussian_models):
self.sensor_model = sm.SensorModel(gaussian_models)
self.number_of_particles = num_particles #100
self.particle_location = np.zeros((self.number_of_particles,3))
self.particle_observation_likelihood = np.zeros((self.number_of_particles))
self.best_particle = None
self.centroid = None
self.motion_model_variance = [0.02, 0.02, 0.005] #0.05,0.05,0.01
self.raw_location = [0,0,0]
def __init__(self):
#POSE RECORDE
self.pre_st_x = 0
self.pre_st_y = 0
self.pre_st_w = 0
self._pre_pose = Pose()
self._pre_pose.position.x = 0
self._pre_pose.position.y = 0
self._pre_pose.position.z = 0
self._pre_pose.orientation.x = 0
self._pre_pose.orientation.y = 0
self._pre_pose.orientation.z = 0
self._pre_pose.orientation.w = 0
# Initialise fields
self.estimatedpose = PoseWithCovarianceStamped()
self.occupancy_map = OccupancyGrid()
self.particlecloud = PoseArray()
self.tf_message = tfMessage()
self._update_lock = Lock()
# Parameters
self.ODOM_ROTATION_NOISE = 0 # Odometry model rotation noise
self.ODOM_TRANSLATION_NOISE = 0 # Odometry x axis (forward) noise
self.ODOM_DRIFT_NOISE = 0 # Odometry y axis (side-side) noise
self.NUMBER_PREDICTED_READINGS = 20
# Set 'previous' translation to origin
# All Transforms are given relative to 0,0,0, not in absolute coords.
self.prev_odom_x = 0.0 # Previous odometry translation from origin
self.prev_odom_y = 0.0 # Previous odometry translation from origin
self.prev_odom_heading = 0.0 # Previous heading from odometry data
self.last_odom_pose = None
# Request robot's initial odometry values to be recorded in prev_odom
self.odom_initialised = False
self.sensor_model_initialised = False
# Set default initial pose to initial position and orientation.
self.estimatedpose.pose.pose.position.x = self.INIT_X
self.estimatedpose.pose.pose.position.y = self.INIT_Y
self.estimatedpose.pose.pose.position.z = self.INIT_Z
self.estimatedpose.pose.pose.orientation = rotateQuaternion(
Quaternion(w=1.0), self.INIT_HEADING)
# NOTE: Currently not making use of covariance matrix
self.estimatedpose.header.frame_id = "/map"
self.particlecloud.header.frame_id = "/map"
# Sensor model
self.sensor_model = sensor_model.SensorModel()