def process_position(self, odom):
'''
ros callback for the position that publishes new sensor data for that
position
'''
vs = VizScan()
vs.header = odom.header
vs.observes = []
o_x = odom.pose.pose.position.x
o_y = odom.pose.pose.position.y
o_h = quaternion_to_heading(odom.pose.pose.orientation)
# rospy.loginfo('odom: %f %f %f' % (o_x, o_y, o_h,))
# odom.header.frame_id = '/map'
for feature in self.feature_list:
# rospy.loginfo('feat: %f %f' % (feature.x, feature.y,))
compass = math.atan2(feature.y-o_y, feature.x-o_x)
bearing = compass - o_h
# rospy.loginfo('compass: %f heading: %f bearing: %f' % (compass, o_h, bearing))
bob = Blob()
bob.bearing = bearing
bob.color.r = feature.red
bob.color.g = feature.green
bob.color.b = feature.blue
vs.observes.append(bob)
rospy.loginfo('viz_core publish sensor data')
self.feature_pub.publish(vs)
self.rateLimit.sleep()
def test_cross_readings(self):
particle = FilterParticle()
old_reading = (
Odometry(),
Blob(),
)
new_odom = Odometry()
new_odom.pose.pose.orientation = heading_to_quaternion(math.pi / 2)
new_reading = (
new_odom,
Blob(),
)
result = particle.cross_readings(old_reading, new_reading)
self.assertIsNotNone(result)
self.assertEqual(result[0], 0.0)
self.assertEqual(result[1], 0.0)
new_odom = Odometry()
new_odom.pose.pose.orientation = heading_to_quaternion(0.0)
new_odom.pose.pose.position.y = -1.0
new_reading = (
new_odom,
Blob(),
)
result = particle.cross_readings(old_reading, new_reading)
self.assertIsNone(result)
def blob_to_msg(img, blob):
from viz_feature_sim.msg import Blob
b = Blob()
b.size = blob.size
b.bearing = pix_x_to_bearing(img, blob.pt[0])
pixel = img[int(blob.pt[1])][int(blob.pt[0])]
b.color.b = pixel[0]
b.color.g = pixel[1]
b.color.r = pixel[2]
return b
def core_to_rosmsg(reading_list):
temp = []
for reading in reading_list:
blob = Blob()
blob.bearing = reading.bearing
blob.size = reading.size
blob.color.r = reading.r
blob.color.g = reading.g
blob.color.b = reading.b
temp.append(blob)
return temp
def easy_observation(odom, feature):
'''
helper method to create Blobs to use in navigation calculations
'''
blob = Blob()
heading = math.atan2(feature.y-odom.pose.pose.position.y,
feature.x-odom.pose.pose.position.x)
blob.bearing = heading - quaternion_to_heading(odom.pose.pose.orientation)
rospy.loginfo('h | b || %f | %f' % (heading, quaternion_to_heading(odom.pose.pose.orientation),))
blob.size = feature.red
blob.color.r = feature.red
blob.color.g = feature.green
blob.color.b = feature.blue
return blob
def test_probability_of_match_bearing(self):
particle = FilterParticle()
state = Odometry()
# two 0 cases: colors far apart, _bearing_ far off
blob_bearing = Blob()
blob_bearing.bearing = math.pi
feature = Feature(mean=np.array([1, 0, 0, 0, 0]))
self.assertIsInstance(feature.mean, np.ndarray)
result_bearing = particle.probability_of_match(state, blob_bearing,
feature)
self.assertEqual(result_bearing, 0.0)
def test_prob_color_match(self):
# Note to self: check and see if the 0 is a problem for high covariance
particle = FilterParticle()
f_mean = np.array([0, 0, 255, 0, 0])
f_covar = list([[0, 0, 0, 0, 0], [0, 0, 0, 0, 0], [0, 0, 5, 0, 0],
[0, 0, 0, 5, 0], [0, 0, 0, 0, 5]])
f_covar = np.array(f_covar)
blob = Blob()
blob.color.r = 255
blob.color.g = 0
blob.color.b = 0
result1 = particle.prob_color_match(f_mean, f_covar, blob)
self.assertTrue(result1 > 0.005)
blob.color.r = 250
result2 = particle.prob_color_match(f_mean, f_covar, blob)
self.assertTrue(result2 < result1)
blob.color.b = 5
result3 = particle.prob_color_match(f_mean, f_covar, blob)
self.assertTrue(result3 < result2)
blob.color.r = 200
result4 = particle.prob_color_match(f_mean, f_covar, blob)
self.assertTrue(result4 < result2)
def generate_measurement(self, featureid):
'''
Generate an expected measurement for the given featureid
'''
state = self.state
s_x = state.pose.pose.position.x
s_y = state.pose.pose.position.y
feature = self.get_feature_by_id(featureid)
f_x = feature.mean[0]
f_y = feature.mean[1]
bobby = Blob()
bobby.bearing = math.atan2(f_y-s_y, f_x-s_x)
# bobby.size = 1/math.sqrt(math.pow(f_x-s_x, 2)+math.pow(f_y-s_y, 2))
bobby.color.r = feature.mean[2]
bobby.color.g = feature.mean[3]
bobby.color.b = feature.mean[4]
return bobby
def generate_measurement(self, featureid):
'''
Generate an expected measurement for the given featureid
'''
state = self.state
s_x = state.pose.pose.position.x
s_y = state.pose.pose.position.y
feature = self.get_feature_by_id(featureid)
f_x = feature.mean[0]
f_y = feature.mean[1]
bobby = Blob()
bobby.bearing = math.atan2(f_y - s_y, f_x - s_x)
# bobby.size = 1/math.sqrt(math.pow(f_x-s_x, 2)+math.pow(f_y-s_y, 2))
bobby.color.r = feature.mean[2]
bobby.color.g = feature.mean[3]
bobby.color.b = feature.mean[4]
return bobby
def test_add_orphaned_reading(self):
particle = FilterParticle()
original_size = len(particle.hypothesis_set)
particle.add_orphaned_reading(Odometry(), Blob())
new_size = len(particle.hypothesis_set)
self.assertTrue(original_size < new_size)
def test_reading_distance_function(self):
particle = FilterParticle()
state1 = Odometry()
blob1 = Blob()
# lines are parallel, should have no intersection
state2 = Odometry()
state2.pose.pose.position.y = 1
blob2 = Blob()
result2 = particle.reading_distance_function(state1, blob1, state2,
blob2)
self.assertEqual(result2, float('inf'))
# lines don't intersect should have no intersection
state3 = Odometry()
state3.pose.pose.position.y = 1
blob3 = Blob()
blob3.bearing = 1.0
result3 = particle.reading_distance_function(state1, blob1, state3,
blob3)
self.assertEqual(result3, float('inf'))
# lines intersect, color is the same, distance is 0
state4 = Odometry()
state4.pose.pose.position.y = 1
blob4 = Blob()
blob4.bearing = -1.0
result4 = particle.reading_distance_function(state1, blob1, state4,
blob4)
self.assertEqual(result4, 0.0)
# lines intersect, color is close, distance is small
state5 = Odometry()
state5.pose.pose.position.y = 1
blob5 = Blob()
blob5.bearing = -1.0
blob5.color.r = 5
result5 = particle.reading_distance_function(state1, blob1, state5,
blob5)
self.assertTrue(result5 > 0.0)
def test_find_nearest_reading(self):
particle = FilterParticle()
state1 = Odometry() # 0,0
blob1 = Blob()
blob1.bearing = .1
state2 = Odometry() # 0,1
state2.pose.pose.position.y = 1
blob2 = Blob()
blob2.bearing = -.1
particle.potential_features[-1] = (state1, blob1)
min_dist_id = particle.find_nearest_reading(state2, blob2)
self.assertEqual(min_dist_id, -1)
# parallel to state2 option, should not match
state3 = Odometry()
state3.pose.pose.position.y = 1
blob3 = Blob()
blob3.bearing = -.1
particle.potential_features[-3] = (state3, blob3)
min_dist_id = particle.find_nearest_reading(state2, blob2)
self.assertEqual(min_dist_id, -1)
# wrong color option, should not match
state4 = Odometry() # 0,0
blob4 = Blob()
blob4.bearing = .1
blob4.color.r = 255
blob4.color.g = 255
blob4.color.b = 255
particle.potential_features[-4] = (state4, blob4)
min_dist_id = particle.find_nearest_reading(state2, blob2)
self.assertEqual(min_dist_id, -1)
# doesn't intersect state 2, should not match
state5 = Odometry() # 0,0
state5.pose.pose.position.y = 100
blob5 = Blob()
blob5.bearing = -.1
particle.potential_features[-5] = (state4, blob4)
min_dist_id = particle.find_nearest_reading(state2, blob2)
self.assertEqual(min_dist_id, -1)
