def __init__(self, _num, _mapTopic, _algorithmName):
# Initialise fields
self.estimatedpose = PoseWithCovarianceStamped()
self.occupancy_map = OccupancyGrid()
self.particlecloud = PoseArray()
self.tf_message = tfMessage()
self.weights = []
self.maxWeight = 0
self.totalWeight = 0
self.num = _num
self.clusterTask = ClusterTask()
self.floorName = _mapTopic
self.exploded = False
# Initialise objects
self.cloud = UpdateParticleCloud()
self.estimate = EstimatePose()
self.init = InitialiseCloud()
self._weighted_particle_publisher = rospy.Publisher("/weightedParticles", WeightedParticles)
# 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()
print(_algorithmName)
# What algorithm do we use?
if _algorithmName == "async":
self.asynchronous = True
elif _algorithmName == "sync":
self.asynchronous = False
else:
print("Invalid argument 3: expected \"sync\" or \"async\"")
sys.exit(1)
# Free Point where Robot can be
self.listFreePoints = []
class PFLocaliserBase(object):
INIT_X = 10 # Initial x location of robot (metres)
INIT_Y = 5 # Initial y location of robot (metres)
INIT_Z = 0 # Initial z location of robot (metres)
INIT_HEADING = 0 # Initial orientation of robot (radians)
def __init__(self, _num, _mapTopic, _algorithmName):
# Initialise fields
self.estimatedpose = PoseWithCovarianceStamped()
self.occupancy_map = OccupancyGrid()
self.particlecloud = PoseArray()
self.tf_message = tfMessage()
self.weights = []
self.maxWeight = 0
self.totalWeight = 0
self.num = _num
self.clusterTask = ClusterTask()
self.floorName = _mapTopic
self.exploded = False
# Initialise objects
self.cloud = UpdateParticleCloud()
self.estimate = EstimatePose()
self.init = InitialiseCloud()
self._weighted_particle_publisher = rospy.Publisher("/weightedParticles", WeightedParticles)
# 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()
print(_algorithmName)
# What algorithm do we use?
if _algorithmName == "async":
self.asynchronous = True
elif _algorithmName == "sync":
self.asynchronous = False
else:
print("Invalid argument 3: expected \"sync\" or \"async\"")
sys.exit(1)
# Free Point where Robot can be
self.listFreePoints = []
def initialise_particle_cloud(self, initialpose):
"""
Called whenever an initialpose message is received (to change the
starting location of the robot), or a new occupancy_map is received.
:Args:
| initialpose: the initial pose estimate
:Return:
| (geometry_msgs.msg.PoseArray) poses of the particles
"""
raise NotImplementedError()
def update_filter(self, scan, map_topic, numParticles, lock):
"""
Called whenever there is a new LaserScan message.
This calls update methods (implemented by subclass) to do actual
particle filtering, given the map and the LaserScan, and then updates
Transform tf appropriately.
:Args:
| scan (sensor_msgs.msg.LaserScan) latest laser scan to resample
the particle filter based on
"""
if not self.sensor_model_initialised:
self.sensor_model.set_laser_scan_parameters(self.NUMBER_PREDICTED_READINGS, scan.range_max, len(scan.ranges), scan.angle_min, scan.angle_max)
self.sensor_model_initialised = True
with lock:
#print("CALLED________________________________")
t = time.time()
# Call user-implemented particle filter update method
#self.update_particle_cloud(scan)
#SERVER CODE
if self.asynchronous:
self.update_particle_cloud(scan)
else:
self.cloud.weight_particles(scan, self)
# Get particle cloud and weights and publish it
pWeights = WeightedParticles()
pWeights.poseArray.header.frame_id = map_topic
pWeights.poseArray.header.seq = 1
pWeights.poseArray.header.stamp = rospy.get_rostime()
pWeights.poseArray.poses = self.particlecloud.poses
pWeights.array = self.weights
pWeights.maxWeight = self.maxWeight
pWeights.totalWeight = self.totalWeight
pWeights.reinit = self.exploded
#rospy.loginfo("SENDING FROM: " + map_topic)
self._weighted_particle_publisher.publish(pWeights)
resampledParticles = []
reinit = None
loop = True
try:
while loop:
pArray = rospy.wait_for_message("/updatedCloud", Particles, 2)
#rospy.loginfo("MESSAGE FROM: " + pArray.particles.header.frame_id)
#rospy.loginfo("EXPECTED: " + map_topic)
if pArray.particles.header.frame_id == map_topic:
loop = False
resampledParticles = pArray.particles.poses
reinit = pArray.reinit
if reinit:
self.particlecloud = self.reinitialise_cloud(self.estimatedpose.pose.pose, 0, False)
else:
self.particlecloud = self.cloud.smudge_amcl(resampledParticles)
except:
rospy.loginfo("TIMED OUT WAITING FOR MESSAGE")
self.particlecloud.header.frame_id = map_topic
self.estimatedpose.pose.pose = self.estimate_pose()
# Publish the estimated pose
floorName = self.floorName
estimatedPose = self.estimatedpose.pose.pose
largestClusterSize = self.estimate.dbscan_largestclustersize()
self.clusterTask.publish(floorName, estimatedPose, largestClusterSize, len(self.particlecloud.poses))
currentTime = rospy.Time.now()
# Given new estimated pose, now work out the new transform
self.recalculate_transform(currentTime)
# Insert correct timestamp in particlecloud and estimatedpose,
# so extending subclasses don't need to worry about this, but can
# just concentrate on updating actual particle and pose locations
self.particlecloud.header.stamp = currentTime
self.estimatedpose.header.stamp = currentTime
return time.time()-t
def update_particle_cloud(self, scan):
"""
This should use the supplied laser scan to update the current
particle cloud. I.e. self.particlecloud should be updated.
:Args:
| scan (sensor_msgs.msg.LaserScan): laser scan to use for update
"""
raise NotImplementedError()
def reinitialise_cloud(self, initialpose, ratio, random):
raise NotImplementedError()
def estimate_pose(self):
"""
This should calculate and return an updated robot pose estimate based
on the particle cloud (self.particlecloud).
:Return:
| (geometry_msgs.msg.Pose) robot's estimated pose.
"""
raise NotImplementedError()
def recalculate_transform(self, currentTime):
"""
Creates updated transform from /odom to /map given recent odometry and
laser data.
:Args:
| currentTime (rospy.Time()): Time stamp for this update
"""
transform = Transform()
T_est = transformations.quaternion_matrix([self.estimatedpose.pose.pose.orientation.x,self.estimatedpose.pose.pose.orientation.y,self.estimatedpose.pose.pose.orientation.z,self.estimatedpose.pose.pose.orientation.w])
T_est[0, 3] = self.estimatedpose.pose.pose.position.x
T_est[1, 3] = self.estimatedpose.pose.pose.position.y
T_est[2, 3] = self.estimatedpose.pose.pose.position.z
T_odom = transformations.quaternion_matrix([self.last_odom_pose.pose.pose.orientation.x,self.last_odom_pose.pose.pose.orientation.y,self.last_odom_pose.pose.pose.orientation.z,self.last_odom_pose.pose.pose.orientation.w])
T_odom[0, 3] = self.last_odom_pose.pose.pose.position.x
T_odom[1, 3] = self.last_odom_pose.pose.pose.position.y
T_odom[2, 3] = self.last_odom_pose.pose.pose.position.z
T = np.dot(T_est, np.linalg.inv(T_odom))
q = transformations.quaternion_from_matrix(T) #[:3, :3])
transform.translation.x = T[0, 3]
transform.translation.y = T[1, 3]
transform.translation.z = T[2, 3]
transform.rotation.x = q[0]
transform.rotation.y = q[1]
transform.rotation.z = q[2]
transform.rotation.w = q[3]
# Insert new Transform into a TransformStamped object and add to the
# tf tree
new_tfstamped = TransformStamped()
new_tfstamped.child_frame_id = "/odom"
new_tfstamped.header.frame_id = "/map"
new_tfstamped.header.stamp = currentTime
new_tfstamped.transform = transform
# Add the transform to the list of all transforms
self.tf_message = tfMessage(transforms=[new_tfstamped])
def predict_from_odometry(self, odom, lock):
"""
Adds the estimated motion from odometry readings to each of the
particles in particlecloud.
:Args:
| odom (nav_msgs.msg.Odometry): Recent Odometry data
"""
with lock:
t = time.time()
x = odom.pose.pose.position.x
y = odom.pose.pose.position.y
new_heading = getHeading(odom.pose.pose.orientation)
# On our first run, the incoming translations may not be equal to
# zero, so set them appropriately
if not self.odom_initialised:
self.prev_odom_x = x
self.prev_odom_y = y
self.prev_odom_heading = new_heading
self.odom_initialised = True
# Find difference between current and previous translations
dif_x = x - self.prev_odom_x
dif_y = y - self.prev_odom_y
dif_heading = new_heading - self.prev_odom_heading
if dif_heading > math.pi:
dif_heading = (math.pi * 2) - dif_heading
if dif_heading < -math.pi:
dif_heading = (math.pi * 2) + dif_heading
# Update previous pure odometry location (i.e. excluding noise)
# with the new translation
self.prev_odom_x = x
self.prev_odom_y = y
self.prev_odom_heading = new_heading
self.last_odom_pose = odom
# Find robot's linear forward/backward motion, given the dif_x and
# dif_y changes and its orientation
distance_travelled = math.sqrt(dif_x*dif_x + dif_y*dif_y)
direction_travelled = math.atan2(dif_y, dif_x)
temp = abs(new_heading - direction_travelled)
if temp < -PI_OVER_TWO or temp > PI_OVER_TWO:
# We are going backwards
distance_travelled = distance_travelled * -1
# Update each particle with change in position (plus noise)
for p in self.particlecloud.poses:
rnd = random.normalvariate(0, 1)
# Rotate particle according to odometry rotation, plus noise
p.orientation = (rotateQuaternion(p.orientation,dif_heading + rnd * dif_heading * self.ODOM_ROTATION_NOISE))
# Get particle's new orientation
theta = getHeading(p.orientation)
# Find translation in the direction of particle's orientation
travel_x = distance_travelled * math.cos(theta)
travel_y = distance_travelled * math.sin(theta)
p.position.x = (p.position.x + travel_x + (rnd * travel_x * self.ODOM_TRANSLATION_NOISE))
p.position.y = (p.position.y + travel_y + (rnd * travel_y * self.ODOM_DRIFT_NOISE))
return time.time() - t
def set_initial_pose(self, pose):
""" Initialise filter with start pose """
self.estimatedpose.pose = pose.pose
# Estimated pose has been set, so we should now reinitialise the
# particle cloud around it
rospy.loginfo("Got pose. Calling initialise_particle_cloud().")
self.particlecloud = self.initialise_particle_cloud(self.estimatedpose)
self.particlecloud.header.frame_id = "/map"
def set_map(self, occupancy_map):
""" Set the map for localisation """
self.occupancy_map = occupancy_map
self.sensor_model.set_map(occupancy_map)
#Define Free Occupancy Points
sizeRealMap = len(self.occupancy_map.data)
sizeSupposedMap = self.occupancy_map.info.width * self.occupancy_map.info.height
rospy.loginfo("Real Map = %s"%sizeRealMap)
rospy.loginfo("Suppose Map = %s"%sizeSupposedMap)
i = 0
occupancyData = self.occupancy_map.data
for h in range(0, self.occupancy_map.info.height):
for w in range(0, self.occupancy_map.info.width):
if (occupancyData[i] >= 0.0) :
point = Point()
point.x = w
point.y = h
point.z = 0
self.listFreePoints.append(point)
i += 1
rospy.loginfo("Len listFreePoints= %s"%len(self.listFreePoints))
self.cloud.mapInfo = []
self.cloud.mapInfo.append((self.floorName, self.listFreePoints, self.occupancy_map.info.resolution))
# Map has changed, so we should reinitialise the particle cloud
rospy.loginfo("Particle filter got map. (Re)initialising.")
self.particlecloud = self.initialise_particle_cloud(self.estimatedpose)
self.particlecloud.header.frame_id = "/map"
