def test_motion_model(self):
fs = FastSLAM()
fpold = FilterParticle()
fpold.state.pose.pose.position.y = 2.0
twist = Twist()
twist.linear.x = 1
dt = rospy.Duration.from_sec(.1)
fpnew = fs.motion_model(fpold, twist, dt)
dy_expected = twist.linear.x * dt.secs
dy_measured = fpnew.state.pose.pose.position.y - fpold.state.pose.pose.position.y
error = abs(dy_measured - dy_expected)
self.assertTrue(error < .01)
twist.linear.x = 2
dt = rospy.Duration.from_sec(.1)
fpnew = fs.motion_model(fpold, twist, dt)
dy_expected = twist.linear.x * dt.secs
dy_measured = fpnew.state.pose.pose.position.y - fpold.state.pose.pose.position.y
error = abs(dy_measured - dy_expected)
self.assertTrue(error < .02)
def initialize_particle_filter(self, preset_features):
'''
Create an instance of FastSLAM algorithm
'''
self.core = FastSLAM(preset_features)
def test_initilization(self):
fs = FastSLAM()
self.assertIsInstance(fs.last_control, Twist)
self.assertIsInstance(fs.last_update, rospy.Time)
self.assertIsInstance(fs.particles, list)
self.assertIsInstance(fs.Qt, np.ndarray)
